From a6511c8f45b1d7beab4b3185d0ee012242423ee6 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:34 +0200
Subject: [PATCH 001/118] (feature) Adds path interpolation as optional in
 Follower

---
 .../kompass_cpp/include/controllers/follower.h   |  2 +-
 .../kompass_cpp/src/controllers/follower.cpp     | 16 ++++++++++------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
index e99f6607..7c483475 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
@@ -94,7 +94,7 @@ class Follower : public Controller {
    *
    * @param path Global path to be followed
    */
-  void setCurrentPath(const Path::Path &path);
+  void setCurrentPath(const Path::Path &path, const bool interpolate = true);
 
   void clearCurrentPath();
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index d8fc8ba3..a5cacbb8 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -83,7 +83,7 @@ void Follower::clearCurrentPath() {
   return;
 }
 
-void Follower::setCurrentPath(const Path::Path &path) {
+void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
   // Delete old reference and current path before setting new values
   if (refPath) {
     delete refPath;
@@ -98,13 +98,17 @@ void Follower::setCurrentPath(const Path::Path &path) {
   currentPath->setMaxLength(
       this->config.getParameter<double>("max_path_length"));
 
-  currentPath->interpolate(maxDist, interpolationType);
+  if (interpolate) {
+    currentPath->interpolate(maxDist, interpolationType);
 
-  // Segment path
-  currentPath->segment(path_segment_length);
+    // Segment path
+    currentPath->segment(path_segment_length);
 
-  // Get max number of segments in the path
-  max_segment_index_ = currentPath->getMaxNumSegments();
+    // Get max number of segments in the path
+    max_segment_index_ = currentPath->getMaxNumSegments();
+  } else {
+    max_segment_index_ = 0;
+  }
 
   path_processing_ = true;
   current_segment_index_ = 0;

From b8d8c2f7f39289fa09bdb12166df92116e4298d1 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:57 +0200
Subject: [PATCH 002/118] (feature) Adds tracked pose type

---
 .../kompass_cpp/include/datatypes/control.h   | 141 ++++++++++++++++++
 .../kompass_cpp/include/datatypes/path.h      |  11 +-
 2 files changed, 151 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 9c2a52c4..417fb019 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <cmath>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 // Namespace for Control Types
@@ -12,6 +15,103 @@ namespace Control {
 // Enumeration for control modes
 enum class ControlType { ACKERMANN = 0, DIFFERENTIAL_DRIVE = 1, OMNI = 2 };
 
+class Pose3D {
+
+public:
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation_.toRotationMatrix()){};
+
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation.toRotationMatrix()){};
+
+  /**
+   * @brief Construct a new Pose3D object using 2D pose information
+   *
+   * @param pose_x
+   * @param pose_y
+   * @param pose_yaw
+   */
+  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw)
+      {update(pose_x, pose_y, pose_yaw);};
+
+  void setFrame(const std::string &frame_id) { frame_id_ = frame_id; }
+
+  std::string getFrame() const { return frame_id_; }
+
+  bool inFrame(const std::string &frame_id) const {
+    return frame_id == frame_id_;
+  }
+
+  float norm() const { return position_.norm(); };
+
+  /**
+   * @brief Extract x coordinates
+   *
+   * @return float
+   */
+  float x() const { return position_(0); };
+
+  /**
+   * @brief Extract y coordinates
+   *
+   * @return float
+   */
+  float y() const { return position_(1); };
+
+  /**
+   * @brief Extract z coordinates
+   *
+   * @return float
+   */
+  float z() const { return position_(2); };
+
+  /**
+   * @brief Extract pitch (y-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float pitch() const { return std::asin(rotation_matrix_(2, 0)); };
+
+  /**
+   * @brief Extract roll (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float roll() const {
+    return std::atan2(rotation_matrix_(2, 1), rotation_matrix_(2, 2));
+  };
+
+  /**
+   * @brief Extract yaw (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float yaw() const {
+    return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0));
+  };
+
+  void update(const float &pose_x, const float &pose_y, const float &pose_yaw){
+    position_ = {pose_x, pose_y, 0.0};
+    setRotation(0.0, 0.0, pose_yaw);
+  }
+
+  void setRotation(const float pitch, const float roll, const float yaw){
+    Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());
+    Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());
+    orientation_ = rotZ * rotY * rotX;
+    rotation_matrix_ = orientation_.toRotationMatrix();
+  }
+
+  protected :
+  Eigen::Vector3f position_;
+  Eigen::Quaternionf orientation_;
+  Eigen::Matrix3f rotation_matrix_;
+  std::string frame_id_;
+};
+
 class Velocity2D {
 public:
   // Default constructor
@@ -37,6 +137,47 @@ class Velocity2D {
   Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0};
 };
 
+class TrackedPose2D : public Pose3D {
+
+public:
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Vector4f &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Quaternionf &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const Velocity2D &vel)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const float &vx, const float &vy, const float &omega)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega){};
+
+  float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); };
+
+  float omega() const { return vel_.omega(); };
+
+  void update(const float timeStep) {
+    position_(0) +=
+        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * timeStep;
+    position_(1) +=
+        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * timeStep;
+    float yaw = this->yaw() + vel_.omega() * timeStep;
+    setRotation(0.0, 0.0, yaw);
+  }
+
+  float distance(const float x, const float y, const float z = 0.0) const{
+    return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
+                pow(position_.z() - z, 2));
+  }
+
+protected:
+  Velocity2D vel_;
+};
+
 struct Velocities {
   std::vector<float> vx; // Speed on x-asix (m/s)
   std::vector<float> vy;
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index dcec89d7..3af7603a 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "datatypes/control.h"
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
@@ -20,7 +21,15 @@ struct State {
 
   State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0,
         double speedValue = 0.0)
-      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {}
+      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue){};
+
+  void update(const Kompass::Control::Velocity2D &vel, const float timeStep) {
+    this->x +=
+        (vel.vx() * cos(this->yaw) - vel.vy() * sin(this->yaw)) * timeStep;
+    this->y +=
+        (vel.vx() * sin(this->yaw) + vel.vy() * cos(this->yaw)) * timeStep;
+    this->yaw += vel.omega() * timeStep;
+  };
 };
 
 // Point in 3D space

From ae3b8bc29ccbf5ed1f0de03f4f450c9134c95564 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:54:17 +0200
Subject: [PATCH 003/118] (feature) Adds sampling utilities in trajectory
 sampler

---
 .../include/utils/trajectory_sampler.h        | 15 ++-
 .../src/utils/trajectory_sampler.cpp          | 97 +++++++++++++++----
 2 files changed, 94 insertions(+), 18 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 7e240de7..0444859c 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -7,6 +7,8 @@
 #include "datatypes/trajectory.h"
 #include <array>
 #include <cmath>
+#include <functional>
+#include <memory>
 #include <vector>
 
 #ifndef MIN_VEL
@@ -95,6 +97,9 @@ class TrajectorySampler {
    */
   ~TrajectorySampler();
 
+  void updateState(const Path::State &current_state);
+
+
   void setSampleDroppingMode(const bool drop_samples);
 
   /**
@@ -124,6 +129,14 @@ class TrajectorySampler {
    */
   void resetOctreeResolution(const double resolution);
 
+  Trajectory2D
+  generateSingleSampleFromVel(const Velocity2D &vel,
+                              const Path::State &pose = Path::State());
+
+  template <typename T>
+  bool checkStatesFeasibility(const std::vector<Path::State> &states,
+                                const T &sensor_points);
+
   size_t numTrajectories;
   size_t numPointsPerTrajectory;
 
@@ -131,7 +144,7 @@ class TrajectorySampler {
   // Protected member variables
   ControlType ctrType;
   ControlLimitsParams ctrlimits;
-  CollisionChecker *collChecker;
+  std::unique_ptr<CollisionChecker> collChecker;
   int maxNumThreads;
 
 private:
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 9a090f3d..912cffe3 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -1,9 +1,11 @@
 #include <array>
 #include <cmath>
 #include <cstdlib>
+#include <memory>
 #include <mutex>
 #include <vector>
 
+#include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
 #include "utils/logger.h"
@@ -29,7 +31,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const double octreeRes,
     const int maxNumThreads) {
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -64,7 +66,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const int maxNumThreads) {
   double octreeRes = config.getParameter<double>("octree_map_resolution");
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -89,7 +91,7 @@ TrajectorySampler::TrajectorySampler(
   this->drop_samples_ = config.getParameter<bool>("drop_samples");
 }
 
-TrajectorySampler::~TrajectorySampler() { delete collChecker; }
+TrajectorySampler::~TrajectorySampler() {}
 
 void TrajectorySampler::resetOctreeResolution(const double resolution) {
   collChecker->resetOctreeResolution(resolution);
@@ -125,14 +127,7 @@ void TrajectorySampler::getAdmissibleTrajsFromVel(
   size_t last_free_index{numPointsPerTrajectory - 1};
 
   for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
-
-    simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                         vel.vy() * sin(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                         vel.vy() * cos(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.yaw += vel.omega() * time_step_;
+    simulated_pose.update(vel, time_step_);
 
     // Update the position of the robot in the collision checker (updates the
     // robot collision object) No need to update the Octree (laserscan)
@@ -201,13 +196,8 @@ void TrajectorySampler::getAdmissibleTrajsFromVelDiffDrive(
       simulated_pose.yaw += vel.omega() * time_step_;
       temp_vel = Velocity2D(0.0, 0.0, vel.omega(), vel.steer_ang());
     } else {
-      simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                           vel.vy() * sin(simulated_pose.yaw)) *
-                          time_step_;
-      simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                           vel.vy() * cos(simulated_pose.yaw)) *
-                          time_step_;
       temp_vel = Velocity2D(vel.vx(), vel.vy(), 0.0, 0.0);
+      simulated_pose.update(temp_vel, time_step_);
     }
 
     // Update the position of the robot in the collision checker (updates the
@@ -443,6 +433,10 @@ TrajectorySampler::getNewTrajectories(const Velocity2D &current_vel,
   }
 }
 
+void TrajectorySampler::updateState(const Path::State &current_state){
+  collChecker->updateState(current_state);
+}
+
 TrajectorySamples2D
 TrajectorySampler::generateTrajectories(const Velocity2D &current_vel,
                                         const Path::State &current_pose,
@@ -503,5 +497,74 @@ void TrajectorySampler::UpdateReachableVelocityRange(
       std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001);
 }
 
+template <>
+bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
+    const std::vector<Path::State> &states, const LaserScan &scan) {
+  // collChecker->updateState(states[0]);
+  collChecker->updateScan(scan.ranges, scan.angles);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if(collChecker->checkCollisions()){
+      return true;
+    }
+  }
+  return false;
+}
+
+template <>
+bool TrajectorySampler::checkStatesFeasibility<std::vector<Path::Point>>(
+    const std::vector<Path::State> &states,
+    const std::vector<Path::Point> &cloud) {
+  // collChecker->updateState(states[0]);
+  collChecker->updatePointCloud(cloud);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if (collChecker->checkCollisions()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+Trajectory2D
+TrajectorySampler::generateSingleSampleFromVel(const Velocity2D &vel,
+                                               const Path::State &pose) {
+  Path::State simulated_pose = pose;
+  Trajectory2D trajectory(numPointsPerTrajectory);
+  trajectory.path.add(0, simulated_pose.x, simulated_pose.y);
+  if (ctrType == ControlType::DIFFERENTIAL_DRIVE) {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      if (std::abs(vel.vx()) > MIN_VEL && std::abs(vel.omega()) > MIN_VEL) {
+        // Rotate then move
+        Velocity2D tempVel = vel;
+        tempVel.setVx(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+
+        tempVel.setVx(vel.vx());
+        tempVel.setOmega(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+        i++;
+      }
+      // Else apply directly
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  } else {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  }
+  return trajectory;
+}
+
 }; // namespace Control
 } // namespace Kompass

From 23764749c6eb37ab5d3273eceff152e193248ba0 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:12 +0200
Subject: [PATCH 004/118] (fix) Uses smart pointers in controller

---
 .../kompass_cpp/include/controllers/dwa.h     | 29 +++++++-----
 .../kompass_cpp/src/controllers/dwa.cpp       | 46 +++++++++----------
 2 files changed, 40 insertions(+), 35 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 4e82d926..fc36e70f 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -8,6 +8,7 @@
 #include "utils/cost_evaluator.h"
 #include "utils/trajectory_sampler.h"
 #include <cstddef>
+#include <memory>
 #include <vector>
 
 namespace Kompass {
@@ -71,6 +72,8 @@ class DWA : public Follower {
 
   void resetOctreeResolution(const double octreeRes);
 
+  void setCurrentState(const Path::State &position);
+
   /**
    * @brief Adds a new custom cost to be used in the trajectory evaluation
    *
@@ -123,18 +126,9 @@ class DWA : public Follower {
     debuggingSamples_ = new TrajectorySamples2D(samples_);
   };
 
-private:
-  TrajectorySampler *trajSampler;
-  CostEvaluator *trajCostEvaluator;
-  double max_forward_distance_ = 0.0;
-  int maxNumThreads;
-  TrajectorySamples2D *debuggingSamples_ = nullptr;
-
-  /**
-   * @brief get maximum reference path length
-   */
-  // size_t getMaxPathLength();
-  size_t getMaxPathLength();
+protected:
+  std::unique_ptr<TrajectorySampler> trajSampler;
+  std::unique_ptr<CostEvaluator> trajCostEvaluator;
 
   /**
    * @brief Given the current position and velocity of the robot, find the
@@ -149,6 +143,17 @@ class DWA : public Follower {
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
                                 const T &scan_points);
 
+private:
+  double max_forward_distance_ = 0.0;
+  int maxNumThreads;
+  TrajectorySamples2D *debuggingSamples_ = nullptr;
+
+  /**
+   * @brief get maximum reference path length
+   */
+  // size_t getMaxPathLength();
+  size_t getMaxPathLength();
+
   Path::Path findTrackedPathSegment();
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 0f0cd6e0..5c33370f 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -7,7 +7,7 @@
 #include <array>
 #include <cmath>
 #include <cstddef>
-#include <ostream>
+#include <memory>
 #include <stdexcept>
 #include <tuple>
 
@@ -25,13 +25,13 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
 
   trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
+      std::make_unique<CostEvaluator>(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
                         trajSampler->numPointsPerTrajectory, getMaxPathLength());
 
@@ -56,14 +56,14 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
-                        controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+  trajCostEvaluator = std::make_unique<CostEvaluator>(
+      costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
+      trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory,
+      getMaxPathLength());
 
   // Update the max forward distance the robot can make
   double timeHorizon = config.getParameter<double>("control_horizon");
@@ -78,8 +78,6 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
 }
 
 DWA::~DWA() {
-  delete trajSampler;
-  delete trajCostEvaluator;
   delete debuggingSamples_;
 }
 
@@ -94,17 +92,16 @@ void DWA::reconfigure(ControlLimitsParams controlLimits,
                       const double octreeRes,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
+
+  trajSampler.reset(new TrajectorySampler(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
+      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads));
 
-  delete trajCostEvaluator;
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -117,17 +114,15 @@ void DWA::reconfigure(TrajectorySampler::TrajectorySamplerParameters config,
                       const std::array<float, 4> &sensor_rotation_body,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
-      config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  delete trajCostEvaluator;
+  trajSampler.reset(new TrajectorySampler(
+      config, controlLimits, controlType, robotShapeType, robotDimensions,
+      sensor_position_body, sensor_rotation_body, maxNumThreads));
 
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -145,6 +140,11 @@ void DWA::addCustomCost(
   trajCostEvaluator->addCustomCost(weight, custom_cost_function);
 }
 
+void DWA::setCurrentState(const Path::State &position) {
+  this->currentState = position;
+  this->trajSampler->updateState(position);
+}
+
 Path::Path DWA::findTrackedPathSegment() {
   Path::Path trackedPathSegment;
   size_t segment_index{current_segment_index_ + 1};

From 7bc9b3790d5a7d4f3735542c0e2cd1c1e9246ae5 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:31 +0200
Subject: [PATCH 005/118] (feature) Adds DWA-based Vision Follower

---
 .../include/controllers/vision_dwa.h          | 150 +++++++++++++++++
 .../src/controllers/vision_dwa.cpp            | 151 ++++++++++++++++++
 2 files changed, 301 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
new file mode 100644
index 00000000..3b5ef86e
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -0,0 +1,150 @@
+#pragma once
+
+#include "datatypes/control.h"
+#include "datatypes/parameter.h"
+#include "dwa.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+class VisionDWA : public DWA {
+public:
+  struct TrackingImgData {
+    std::array<double, 2> size_xy; // width and height of the bounding box
+    int img_width;
+    int img_height;
+    std::array<double, 2>
+        center_xy; // x, y coordinates of the object center in image frame
+    double depth;  // -1 is equivalent to none
+  };
+
+  class VisionDWAConfig : public ControllerParameters {
+  public:
+    VisionDWAConfig() : ControllerParameters() {
+      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
+      addParameter("control_horizon", Parameter(2, 1, 1000));
+      addParameter("prediction_horizon", Parameter(10, 1, 1000));
+      addParameter("tolerance", Parameter(0.01, 1e-6, 1e3));
+      addParameter("target_distance",
+                   Parameter(0.1, -1.0, 1e9)); // Use -1 for None
+      addParameter(
+          "target_orientation",
+          Parameter(0.0, -M_PI, M_PI)); // target bearing angle with the target
+      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
+      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
+      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("enable_search", Parameter(false));
+    }
+    bool enable_search() const { return getParameter<bool>("enable_search"); }
+    double control_time_step() const {
+      return getParameter<double>("control_time_step");
+    }
+    double target_search_timeout() const {
+      return getParameter<double>("target_search_timeout");
+    }
+    double target_wait_timeout() const {
+      return getParameter<double>("target_wait_timeout");
+    }
+    double target_search_radius() const {
+      return getParameter<double>("target_search_radius");
+    }
+    double search_pause() const {
+      return getParameter<double>("target_search_pause");
+    }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
+    int prediction_horizon() const {
+      return getParameter<int>("prediction_horizon");
+    }
+    double tolerance() const { return getParameter<double>("tolerance"); }
+    double target_distance() const {
+      double val = getParameter<double>("target_distance");
+      return val < 0 ? -1.0 : val; // Return -1 for None
+    }
+    double target_orientation() const {
+      return getParameter<double>("target_orientation");
+    }
+    void set_target_distance(double value) {
+      setParameter("target_distance", value);
+    }
+    double K_omega() const { return getParameter<double>("rotation_gain"); }
+    double K_v() const { return getParameter<double>("speed_gain"); }
+    double min_vel() const { return getParameter<double>("min_vel"); }
+  };
+
+  VisionDWA(const ControlType robotCtrlType,
+            const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+            int maxAngularSamples,
+            const CollisionChecker::ShapeType robotShapeType,
+            const std::vector<float> robotDimensions,
+            const std::array<float, 3> &sensor_position_body,
+            const std::array<float, 4> &sensor_rotation_body,
+            const double octreeRes,
+            CostEvaluator::TrajectoryCostsWeights costWeights,
+            const int maxNumThreads = 1,
+            const VisionDWAConfig config = VisionDWAConfig());
+
+  // Default Destructor
+  ~VisionDWA() = default;
+
+  /**
+   * @brief Get the Pure Tracking Control Command
+   *
+   * @param tracking_pose
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
+
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA sampling
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                            const Velocity2D &current_vel,
+                                            const T &sensor_points);
+
+private:
+  ControlType _ctrlType;
+  ControlLimitsParams _ctrl_limits;
+  VisionDWAConfig _config;
+
+  bool _rotate_in_place;
+  Velocities _out_vel;
+  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
+  std::queue<std::array<double, 3>> _search_commands_queue;
+  std::array<double, 3> _search_command;
+  std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
+
+  /**
+   * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param sensor_points
+   * @return std::tuple<Trajectory2D, bool>
+   */
+  template <typename T>
+  std::tuple<Trajectory2D, bool>
+  getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                              const T &sensor_points);
+  //
+};
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
new file mode 100644
index 00000000..90866583
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -0,0 +1,151 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "utils/angles.h"
+#include "utils/logger.h"
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+VisionDWA::VisionDWA(const ControlType robotCtrlType,
+                     const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+                     int maxAngularSamples,
+                     const CollisionChecker::ShapeType robotShapeType,
+                     const std::vector<float> robotDimensions,
+                     const std::array<float, 3> &sensor_position_body,
+                     const std::array<float, 4> &sensor_rotation_body,
+                     const double octreeRes,
+                     CostEvaluator::TrajectoryCostsWeights costWeights,
+                     const int maxNumThreads, const VisionDWAConfig config)
+    : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
+          config.prediction_horizon(), config.control_horizon(),
+          maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
+          sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
+          maxNumThreads) {
+  _ctrlType = robotCtrlType;
+  _ctrl_limits = ctrlLimits;
+  _config = config;
+  // Initialize time steps
+  int num_steps = _config.control_horizon();
+  // Initialize control vectors
+  _out_vel = Velocities(num_steps);
+  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
+}
+
+Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0);
+  float gamma =
+      Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
+  float psi = Angle::normalizeToMinusPiPlusPi(
+      std::atan2(tracking_pose.y() - currentState.y,
+                 tracking_pose.x() - currentState.x) -
+      currentState.yaw);
+
+  float distance_error = _config.target_distance() - distance;
+  float angle_error =
+      Angle::normalizeToMinusPiPlusPi(_config.target_orientation() - psi);
+
+  float distance_tolerance = _config.tolerance() * _config.target_distance();
+  float angle_tolerance = std::max(0.001, _config.tolerance() * _config.target_orientation());
+
+  // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
+  //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
+
+  Velocity2D followingVel;
+  if (abs(distance_error) > distance_tolerance or
+      abs(angle_error) > angle_tolerance) {
+    float v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    followingVel.setVx(v);
+    float omega = -tracking_pose.omega() +
+                      2 * (v * sin(psi) / distance +
+                           tracking_pose.v() * sin(gamma - psi) / distance -
+                           _config.K_omega() * tanh(angle_error));
+    followingVel.setOmega(omega);
+  }
+  return followingVel;
+}
+
+template <typename T>
+std::tuple<Trajectory2D, bool>
+VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                                       const T &sensor_points) {
+  int step = 0;
+
+  Trajectory2D ref_traj(_config.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
+  while (step < _config.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    simulated_state.update(cmd, _config.control_time_step());
+    simulated_track.update(_config.control_time_step());
+    if(step < _config.prediction_horizon() -1){
+      ref_traj.velocities.add(step,cmd);
+    }
+
+    step++;
+  }
+  this->setCurrentState(original_state);
+
+  bool has_collisions = trajSampler->checkStatesFeasibility(states, sensor_points);
+
+  LOG_INFO("Found reference traj with collisions: ", has_collisions);
+
+  return std::make_tuple(ref_traj, has_collisions);
+}
+
+template <typename T>
+TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                          const Velocity2D &current_vel,
+                                          const T &sensor_points) {
+  Trajectory2D ref_traj;
+  bool has_collisions;
+  std::tie(ref_traj, has_collisions) =
+      this->getTrackingReferenceSegment(tracking_pose, sensor_points);
+  if(!has_collisions){
+    // The tracking sample is collision free -> No need to explore other samples
+    TrajSearchResult result;
+    result.isTrajFound = true;
+    result.trajCost = 0.0;
+    result.trajectory = ref_traj;
+    latest_velocity_command_ = ref_traj.velocities.getFront();
+    return result;
+  }
+  else{
+    LOG_INFO("USING DWA SAMPLING");
+    // The tracking sample has collisions -> use DWA-like sampling and control
+    Path::Path ref_tracking_path;
+    for (int idx=0; idx < _config.prediction_horizon(); ++idx){
+      ref_tracking_path.points.push_back(ref_traj.path.getIndex(idx));
+    }
+    // Set the tracking segment as the reference path
+    this->setCurrentPath(ref_tracking_path);
+    return this->computeVelocityCommandsSet(current_vel, sensor_points);
+  }
+}
+
+// Explicit instantiation for LaserScan
+template Control::TrajSearchResult VisionDWA::getTrackingCtrl<LaserScan>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const LaserScan &sensor_points);
+
+// Explicit instantiation for PointCloud
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const std::vector<Path::Point> &sensor_points);
+
+} // namespace Control
+} // namespace Kompass

From 6ad61f54587ca174ae78534a8b7a528f1e3b0036 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:01:05 +0200
Subject: [PATCH 006/118] (feature) Adds VisionDWA test

---
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 src/kompass_cpp/tests/json_export.cpp         |  21 +-
 .../tests/trajectory_sampler_plt.py           |  23 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 210 ++++++++++++++++++
 4 files changed, 247 insertions(+), 12 deletions(-)
 create mode 100644 src/kompass_cpp/tests/vision_dwa_test.cpp

diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index c331a8de..d27db2dc 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -40,6 +40,11 @@ add_executable(collisions_test collisions_test.cpp)
 target_link_libraries(collisions_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(collisions_tests collisions_test)
 
+# Vision DWA test
+add_executable(vision_dwa_test vision_dwa_test.cpp)
+target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
+add_test(vision_dwa_tests vision_dwa_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/json_export.cpp b/src/kompass_cpp/tests/json_export.cpp
index c3ea3747..7e4d053d 100644
--- a/src/kompass_cpp/tests/json_export.cpp
+++ b/src/kompass_cpp/tests/json_export.cpp
@@ -92,9 +92,8 @@ void to_json(json &j, const Control::TrajectorySamples2D &samples,
 }
 
 // Save trajectories to a JSON file
-void saveTrajectoriesToJson(
-    const Control::TrajectorySamples2D &trajectories,
-    const std::string &filename) {
+void saveTrajectoriesToJson(const Control::TrajectorySamples2D &trajectories,
+                            const std::string &filename) {
   json j;
   to_json(j, trajectories);
   std::ofstream file(filename);
@@ -106,6 +105,22 @@ void saveTrajectoriesToJson(
   }
 }
 
+void saveTrajectoryToJson(const Control::Trajectory2D &trajectory,
+                          const std::string &filename) {
+  json j;
+  j["paths"] = json::array(); // Initialize as a JSON array
+  json j_p;
+  to_json(j_p, trajectory.path);
+  j["paths"].push_back(j_p); // Serialize each Point
+  std::ofstream file(filename);
+  if (file.is_open()) {
+    file << j.dump(4); // Pretty print with 4 spaces indentation
+    file.close();
+  } else {
+    std::cerr << "Unable to open file: " << filename << std::endl;
+  }
+}
+
 // Save one path to a JSON file
 void savePathToJson(const Path::Path &path, const std::string &filename) {
   json j;
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index a80027a6..3a73acb0 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -29,12 +29,13 @@ def read_path_from_json(filename: str):
         print(f"Read file error: {e}")
 
 
-def plot_samples(trajectories, reference, figure_name):
-    # Plot reference
-    ref_path = reference["points"]
-    x_coords = [point["x"] for point in ref_path]
-    y_coords = [point["y"] for point in ref_path]
-    plt.plot(x_coords, y_coords, "--b")
+def plot_samples(figure_name, trajectories, reference=None):
+    if reference:
+        # Plot reference
+        ref_path = reference["points"]
+        x_coords = [point["x"] for point in ref_path]
+        y_coords = [point["y"] for point in ref_path]
+        plt.plot(x_coords, y_coords, "--b", linewidth=7.0, label="reference")
 
     for traj in trajectories:
         path = traj["points"]
@@ -63,7 +64,7 @@ def main():
     parser.add_argument(
         "--reference",
         type=str,
-        required=True,
+        required=False,
         help="Reference path JSON file name in the current directory",
     )
 
@@ -75,9 +76,13 @@ def main():
     reference_file = args.reference
 
     trajectories = read_trajectories_from_json(f"{samples_file}.json")
-    reference = read_path_from_json(f"{reference_file}.json")
+
+    reference = (
+        read_path_from_json(f"{reference_file}.json") if reference_file else None
+    )
+
     # print(trajectories)
-    plot_samples(trajectories, reference, samples_file)
+    plot_samples(samples_file, trajectories, reference)
 
 
 if __name__ == "__main__":
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
new file mode 100644
index 00000000..ce2771fd
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -0,0 +1,210 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "test.h"
+#include "utils/cost_evaluator.h"
+#include "utils/logger.h"
+#include <memory>
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include "json_export.cpp"
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <cmath>
+#include <vector>
+
+using namespace Kompass;
+
+struct VisionDWATestConfig {
+  // Sampling configuration
+  double timeStep;
+  double predictionHorizon;
+  double controlHorizon;
+  int maxLinearSamples;
+  int maxAngularSamples;
+  int maxNumThreads;
+
+  // Octomap resolution
+  double octreeRes;
+
+  // Cost weights
+  Control::CostEvaluator::TrajectoryCostsWeights costWeights;
+
+  // Robot configuration
+  Control::LinearVelocityControlParams x_params;
+  Control::LinearVelocityControlParams y_params;
+  Control::AngularVelocityControlParams angular_params;
+  Control::ControlLimitsParams controlLimits;
+  Control::ControlType controlType;
+  Kompass::CollisionChecker::ShapeType robotShapeType;
+  std::vector<float> robotDimensions;
+  const std::array<float, 3> sensorPositionWRTbody;
+  const std::array<float, 4> sensorRotationWRTbody;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud;
+
+  // Robot start state (pose)
+  Path::State robotState;
+
+  // Tracked target with respect to the robot
+  Control::Velocity2D tracked_vel;
+  Control::TrackedPose2D tracked_pose;
+  // VisionDWA configuration object
+  std::unique_ptr<Control::VisionDWA> controller;
+
+  // Constructor to initialize the struct
+  VisionDWATestConfig(const float timeStep, const float predictionHorizon,
+                      const float controlHorizon, const int maxLinearSamples,
+                      const int maxAngularSamples,
+                      const std::vector<Path::Point> sensor_points,
+                      const float maxVel = 1.0, const float maxOmega = 2.0,
+                      const int maxNumThreads = 1,
+                      const double reference_path_distance_weight = 1.0,
+                      const double goal_distance_weight = 0.5,
+                      const double obstacles_distance_weight = 0.0)
+      : timeStep(timeStep), predictionHorizon(predictionHorizon),
+        controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
+        maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
+        octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5),
+        angular_params(3.14, maxOmega, 3.0, 3.0),
+        controlLimits(x_params, y_params, angular_params),
+        controlType(Control::ControlType::ACKERMANN),
+        robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER),
+        robotDimensions{0.1, 0.4}, sensorPositionWRTbody{0.0, 0.0, 0.0},
+        sensorRotationWRTbody{0, 0, 0, 1}, cloud(sensor_points),
+        robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3),
+        tracked_pose(0.0, 0.0, 0.0, tracked_vel) {
+    // Initialize cost weights
+    costWeights.setParameter("reference_path_distance_weight",
+                             reference_path_distance_weight);
+    costWeights.setParameter("goal_distance_weight", goal_distance_weight);
+    costWeights.setParameter("obstacles_distance_weight",
+                             obstacles_distance_weight);
+    costWeights.setParameter("smoothness_weight", 0.0);
+    costWeights.setParameter("jerk_weight", 0.0);
+    controller = std::make_unique<Control::VisionDWA>(
+        controlType, controlLimits, maxLinearSamples, maxAngularSamples,
+        robotShapeType, robotDimensions, sensorPositionWRTbody,
+        sensorRotationWRTbody, octreeRes, costWeights);
+  }
+
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName) {
+    Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
+    Control::TrajectoryVelocities2D simulated_velocities(
+        numPointsPerTrajectory);
+    Control::TrajectoryPath robot_path(numPointsPerTrajectory),
+        tracked_path(numPointsPerTrajectory);
+    Control::Velocity2D cmd;
+
+    int step = 0;
+    while (step < numPointsPerTrajectory) {
+      Path::Point point(robotState.x, robotState.y, 0.0);
+      robot_path.add(step, point);
+      tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
+      controller->setCurrentState(robotState);
+
+      Control::TrajSearchResult result =
+          controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+      // cmd = controller.getPureTrackingCtrl(tracked_pose);
+      // robotState.update(cmd, timeStep);
+
+      if (result.isTrajFound) {
+        LOG_INFO(FMAG("STEP: "), step,
+                 FMAG(", Found best trajectory with cost: "), result.trajCost);
+
+        cmd.setVx(controller->getLinearVelocityCmdX());
+        cmd.setVy(controller->getLinearVelocityCmdY());
+        cmd.setOmega(controller->getAngularVelocityCmd());
+
+        LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
+                  BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
+                  BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
+                  BOLD(FBLU("}")));
+
+        LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, cmd.vx(), RST,
+                  BOLD(FGRN(", Vy: ")), KGRN, cmd.vy(), RST,
+                  BOLD(FGRN(", Omega: ")), KGRN, cmd.omega(), RST,
+                  BOLD(FGRN("}")));
+
+        robotState.update(cmd, timeStep);
+      } else {
+        LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED,
+                  robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
+                  RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST,
+                  BOLD(FRED("}")));
+        return false;
+      }
+
+      tracked_pose.update(timeStep);
+      step++;
+    }
+    samples.push_back(simulated_velocities, robot_path);
+    samples.push_back(simulated_velocities, tracked_path);
+
+
+    // Plot the trajectories (Save to json then run python script for plotting)
+    boost::filesystem::path executablePath = boost::dll::program_location();
+    std::string file_location = executablePath.parent_path().string();
+
+    std::string trajectories_filename = file_location + "/" + pltFileName;
+
+    saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+    // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+    std::string command = "python3 " + file_location +
+                          "/trajectory_sampler_plt --samples \"" +
+                          trajectories_filename + "\"";
+
+    // Execute the Python script
+    int res = system(command.c_str());
+    return true;
+  }
+};
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon =0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(numPointsPerTrajectory, std::string("vision_follower_obstacle_free"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon = 0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}

From c1b150467a581550e67b4833c3343044ff8963a1 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:34 +0200
Subject: [PATCH 007/118] (feature) Adds path interpolation as optional in
 Follower

---
 .../kompass_cpp/include/controllers/follower.h   |  2 +-
 .../kompass_cpp/src/controllers/follower.cpp     | 16 ++++++++++------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
index e99f6607..7c483475 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
@@ -94,7 +94,7 @@ class Follower : public Controller {
    *
    * @param path Global path to be followed
    */
-  void setCurrentPath(const Path::Path &path);
+  void setCurrentPath(const Path::Path &path, const bool interpolate = true);
 
   void clearCurrentPath();
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index d8fc8ba3..a5cacbb8 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -83,7 +83,7 @@ void Follower::clearCurrentPath() {
   return;
 }
 
-void Follower::setCurrentPath(const Path::Path &path) {
+void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
   // Delete old reference and current path before setting new values
   if (refPath) {
     delete refPath;
@@ -98,13 +98,17 @@ void Follower::setCurrentPath(const Path::Path &path) {
   currentPath->setMaxLength(
       this->config.getParameter<double>("max_path_length"));
 
-  currentPath->interpolate(maxDist, interpolationType);
+  if (interpolate) {
+    currentPath->interpolate(maxDist, interpolationType);
 
-  // Segment path
-  currentPath->segment(path_segment_length);
+    // Segment path
+    currentPath->segment(path_segment_length);
 
-  // Get max number of segments in the path
-  max_segment_index_ = currentPath->getMaxNumSegments();
+    // Get max number of segments in the path
+    max_segment_index_ = currentPath->getMaxNumSegments();
+  } else {
+    max_segment_index_ = 0;
+  }
 
   path_processing_ = true;
   current_segment_index_ = 0;

From 6748b16609d9265e9828431f63c2d19aadb334ff Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:57 +0200
Subject: [PATCH 008/118] (feature) Adds tracked pose type

---
 .../kompass_cpp/include/datatypes/control.h   | 141 ++++++++++++++++++
 .../kompass_cpp/include/datatypes/path.h      |  11 +-
 2 files changed, 151 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 9c2a52c4..417fb019 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <cmath>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 // Namespace for Control Types
@@ -12,6 +15,103 @@ namespace Control {
 // Enumeration for control modes
 enum class ControlType { ACKERMANN = 0, DIFFERENTIAL_DRIVE = 1, OMNI = 2 };
 
+class Pose3D {
+
+public:
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation_.toRotationMatrix()){};
+
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation.toRotationMatrix()){};
+
+  /**
+   * @brief Construct a new Pose3D object using 2D pose information
+   *
+   * @param pose_x
+   * @param pose_y
+   * @param pose_yaw
+   */
+  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw)
+      {update(pose_x, pose_y, pose_yaw);};
+
+  void setFrame(const std::string &frame_id) { frame_id_ = frame_id; }
+
+  std::string getFrame() const { return frame_id_; }
+
+  bool inFrame(const std::string &frame_id) const {
+    return frame_id == frame_id_;
+  }
+
+  float norm() const { return position_.norm(); };
+
+  /**
+   * @brief Extract x coordinates
+   *
+   * @return float
+   */
+  float x() const { return position_(0); };
+
+  /**
+   * @brief Extract y coordinates
+   *
+   * @return float
+   */
+  float y() const { return position_(1); };
+
+  /**
+   * @brief Extract z coordinates
+   *
+   * @return float
+   */
+  float z() const { return position_(2); };
+
+  /**
+   * @brief Extract pitch (y-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float pitch() const { return std::asin(rotation_matrix_(2, 0)); };
+
+  /**
+   * @brief Extract roll (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float roll() const {
+    return std::atan2(rotation_matrix_(2, 1), rotation_matrix_(2, 2));
+  };
+
+  /**
+   * @brief Extract yaw (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float yaw() const {
+    return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0));
+  };
+
+  void update(const float &pose_x, const float &pose_y, const float &pose_yaw){
+    position_ = {pose_x, pose_y, 0.0};
+    setRotation(0.0, 0.0, pose_yaw);
+  }
+
+  void setRotation(const float pitch, const float roll, const float yaw){
+    Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());
+    Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());
+    orientation_ = rotZ * rotY * rotX;
+    rotation_matrix_ = orientation_.toRotationMatrix();
+  }
+
+  protected :
+  Eigen::Vector3f position_;
+  Eigen::Quaternionf orientation_;
+  Eigen::Matrix3f rotation_matrix_;
+  std::string frame_id_;
+};
+
 class Velocity2D {
 public:
   // Default constructor
@@ -37,6 +137,47 @@ class Velocity2D {
   Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0};
 };
 
+class TrackedPose2D : public Pose3D {
+
+public:
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Vector4f &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Quaternionf &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const Velocity2D &vel)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const float &vx, const float &vy, const float &omega)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega){};
+
+  float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); };
+
+  float omega() const { return vel_.omega(); };
+
+  void update(const float timeStep) {
+    position_(0) +=
+        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * timeStep;
+    position_(1) +=
+        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * timeStep;
+    float yaw = this->yaw() + vel_.omega() * timeStep;
+    setRotation(0.0, 0.0, yaw);
+  }
+
+  float distance(const float x, const float y, const float z = 0.0) const{
+    return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
+                pow(position_.z() - z, 2));
+  }
+
+protected:
+  Velocity2D vel_;
+};
+
 struct Velocities {
   std::vector<float> vx; // Speed on x-asix (m/s)
   std::vector<float> vy;
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index dcec89d7..3af7603a 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "datatypes/control.h"
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
@@ -20,7 +21,15 @@ struct State {
 
   State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0,
         double speedValue = 0.0)
-      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {}
+      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue){};
+
+  void update(const Kompass::Control::Velocity2D &vel, const float timeStep) {
+    this->x +=
+        (vel.vx() * cos(this->yaw) - vel.vy() * sin(this->yaw)) * timeStep;
+    this->y +=
+        (vel.vx() * sin(this->yaw) + vel.vy() * cos(this->yaw)) * timeStep;
+    this->yaw += vel.omega() * timeStep;
+  };
 };
 
 // Point in 3D space

From fd608a09fa64cc071a195c00d21314eeb7063623 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:54:17 +0200
Subject: [PATCH 009/118] (feature) Adds sampling utilities in trajectory
 sampler

---
 .../include/utils/trajectory_sampler.h        | 15 ++-
 .../src/utils/trajectory_sampler.cpp          | 97 +++++++++++++++----
 2 files changed, 94 insertions(+), 18 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 7e240de7..0444859c 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -7,6 +7,8 @@
 #include "datatypes/trajectory.h"
 #include <array>
 #include <cmath>
+#include <functional>
+#include <memory>
 #include <vector>
 
 #ifndef MIN_VEL
@@ -95,6 +97,9 @@ class TrajectorySampler {
    */
   ~TrajectorySampler();
 
+  void updateState(const Path::State &current_state);
+
+
   void setSampleDroppingMode(const bool drop_samples);
 
   /**
@@ -124,6 +129,14 @@ class TrajectorySampler {
    */
   void resetOctreeResolution(const double resolution);
 
+  Trajectory2D
+  generateSingleSampleFromVel(const Velocity2D &vel,
+                              const Path::State &pose = Path::State());
+
+  template <typename T>
+  bool checkStatesFeasibility(const std::vector<Path::State> &states,
+                                const T &sensor_points);
+
   size_t numTrajectories;
   size_t numPointsPerTrajectory;
 
@@ -131,7 +144,7 @@ class TrajectorySampler {
   // Protected member variables
   ControlType ctrType;
   ControlLimitsParams ctrlimits;
-  CollisionChecker *collChecker;
+  std::unique_ptr<CollisionChecker> collChecker;
   int maxNumThreads;
 
 private:
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 9a090f3d..912cffe3 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -1,9 +1,11 @@
 #include <array>
 #include <cmath>
 #include <cstdlib>
+#include <memory>
 #include <mutex>
 #include <vector>
 
+#include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
 #include "utils/logger.h"
@@ -29,7 +31,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const double octreeRes,
     const int maxNumThreads) {
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -64,7 +66,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const int maxNumThreads) {
   double octreeRes = config.getParameter<double>("octree_map_resolution");
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -89,7 +91,7 @@ TrajectorySampler::TrajectorySampler(
   this->drop_samples_ = config.getParameter<bool>("drop_samples");
 }
 
-TrajectorySampler::~TrajectorySampler() { delete collChecker; }
+TrajectorySampler::~TrajectorySampler() {}
 
 void TrajectorySampler::resetOctreeResolution(const double resolution) {
   collChecker->resetOctreeResolution(resolution);
@@ -125,14 +127,7 @@ void TrajectorySampler::getAdmissibleTrajsFromVel(
   size_t last_free_index{numPointsPerTrajectory - 1};
 
   for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
-
-    simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                         vel.vy() * sin(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                         vel.vy() * cos(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.yaw += vel.omega() * time_step_;
+    simulated_pose.update(vel, time_step_);
 
     // Update the position of the robot in the collision checker (updates the
     // robot collision object) No need to update the Octree (laserscan)
@@ -201,13 +196,8 @@ void TrajectorySampler::getAdmissibleTrajsFromVelDiffDrive(
       simulated_pose.yaw += vel.omega() * time_step_;
       temp_vel = Velocity2D(0.0, 0.0, vel.omega(), vel.steer_ang());
     } else {
-      simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                           vel.vy() * sin(simulated_pose.yaw)) *
-                          time_step_;
-      simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                           vel.vy() * cos(simulated_pose.yaw)) *
-                          time_step_;
       temp_vel = Velocity2D(vel.vx(), vel.vy(), 0.0, 0.0);
+      simulated_pose.update(temp_vel, time_step_);
     }
 
     // Update the position of the robot in the collision checker (updates the
@@ -443,6 +433,10 @@ TrajectorySampler::getNewTrajectories(const Velocity2D &current_vel,
   }
 }
 
+void TrajectorySampler::updateState(const Path::State &current_state){
+  collChecker->updateState(current_state);
+}
+
 TrajectorySamples2D
 TrajectorySampler::generateTrajectories(const Velocity2D &current_vel,
                                         const Path::State &current_pose,
@@ -503,5 +497,74 @@ void TrajectorySampler::UpdateReachableVelocityRange(
       std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001);
 }
 
+template <>
+bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
+    const std::vector<Path::State> &states, const LaserScan &scan) {
+  // collChecker->updateState(states[0]);
+  collChecker->updateScan(scan.ranges, scan.angles);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if(collChecker->checkCollisions()){
+      return true;
+    }
+  }
+  return false;
+}
+
+template <>
+bool TrajectorySampler::checkStatesFeasibility<std::vector<Path::Point>>(
+    const std::vector<Path::State> &states,
+    const std::vector<Path::Point> &cloud) {
+  // collChecker->updateState(states[0]);
+  collChecker->updatePointCloud(cloud);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if (collChecker->checkCollisions()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+Trajectory2D
+TrajectorySampler::generateSingleSampleFromVel(const Velocity2D &vel,
+                                               const Path::State &pose) {
+  Path::State simulated_pose = pose;
+  Trajectory2D trajectory(numPointsPerTrajectory);
+  trajectory.path.add(0, simulated_pose.x, simulated_pose.y);
+  if (ctrType == ControlType::DIFFERENTIAL_DRIVE) {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      if (std::abs(vel.vx()) > MIN_VEL && std::abs(vel.omega()) > MIN_VEL) {
+        // Rotate then move
+        Velocity2D tempVel = vel;
+        tempVel.setVx(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+
+        tempVel.setVx(vel.vx());
+        tempVel.setOmega(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+        i++;
+      }
+      // Else apply directly
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  } else {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  }
+  return trajectory;
+}
+
 }; // namespace Control
 } // namespace Kompass

From 9fd49a9e8c535d861a8521e8502c967ec04f920a Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:12 +0200
Subject: [PATCH 010/118] (fix) Uses smart pointers in controller

---
 .../kompass_cpp/include/controllers/dwa.h     | 29 +++++++-----
 .../kompass_cpp/src/controllers/dwa.cpp       | 46 +++++++++----------
 2 files changed, 40 insertions(+), 35 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 4e82d926..fc36e70f 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -8,6 +8,7 @@
 #include "utils/cost_evaluator.h"
 #include "utils/trajectory_sampler.h"
 #include <cstddef>
+#include <memory>
 #include <vector>
 
 namespace Kompass {
@@ -71,6 +72,8 @@ class DWA : public Follower {
 
   void resetOctreeResolution(const double octreeRes);
 
+  void setCurrentState(const Path::State &position);
+
   /**
    * @brief Adds a new custom cost to be used in the trajectory evaluation
    *
@@ -123,18 +126,9 @@ class DWA : public Follower {
     debuggingSamples_ = new TrajectorySamples2D(samples_);
   };
 
-private:
-  TrajectorySampler *trajSampler;
-  CostEvaluator *trajCostEvaluator;
-  double max_forward_distance_ = 0.0;
-  int maxNumThreads;
-  TrajectorySamples2D *debuggingSamples_ = nullptr;
-
-  /**
-   * @brief get maximum reference path length
-   */
-  // size_t getMaxPathLength();
-  size_t getMaxPathLength();
+protected:
+  std::unique_ptr<TrajectorySampler> trajSampler;
+  std::unique_ptr<CostEvaluator> trajCostEvaluator;
 
   /**
    * @brief Given the current position and velocity of the robot, find the
@@ -149,6 +143,17 @@ class DWA : public Follower {
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
                                 const T &scan_points);
 
+private:
+  double max_forward_distance_ = 0.0;
+  int maxNumThreads;
+  TrajectorySamples2D *debuggingSamples_ = nullptr;
+
+  /**
+   * @brief get maximum reference path length
+   */
+  // size_t getMaxPathLength();
+  size_t getMaxPathLength();
+
   Path::Path findTrackedPathSegment();
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 0f0cd6e0..5c33370f 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -7,7 +7,7 @@
 #include <array>
 #include <cmath>
 #include <cstddef>
-#include <ostream>
+#include <memory>
 #include <stdexcept>
 #include <tuple>
 
@@ -25,13 +25,13 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
 
   trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
+      std::make_unique<CostEvaluator>(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
                         trajSampler->numPointsPerTrajectory, getMaxPathLength());
 
@@ -56,14 +56,14 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
-                        controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+  trajCostEvaluator = std::make_unique<CostEvaluator>(
+      costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
+      trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory,
+      getMaxPathLength());
 
   // Update the max forward distance the robot can make
   double timeHorizon = config.getParameter<double>("control_horizon");
@@ -78,8 +78,6 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
 }
 
 DWA::~DWA() {
-  delete trajSampler;
-  delete trajCostEvaluator;
   delete debuggingSamples_;
 }
 
@@ -94,17 +92,16 @@ void DWA::reconfigure(ControlLimitsParams controlLimits,
                       const double octreeRes,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
+
+  trajSampler.reset(new TrajectorySampler(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
+      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads));
 
-  delete trajCostEvaluator;
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -117,17 +114,15 @@ void DWA::reconfigure(TrajectorySampler::TrajectorySamplerParameters config,
                       const std::array<float, 4> &sensor_rotation_body,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
-      config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  delete trajCostEvaluator;
+  trajSampler.reset(new TrajectorySampler(
+      config, controlLimits, controlType, robotShapeType, robotDimensions,
+      sensor_position_body, sensor_rotation_body, maxNumThreads));
 
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -145,6 +140,11 @@ void DWA::addCustomCost(
   trajCostEvaluator->addCustomCost(weight, custom_cost_function);
 }
 
+void DWA::setCurrentState(const Path::State &position) {
+  this->currentState = position;
+  this->trajSampler->updateState(position);
+}
+
 Path::Path DWA::findTrackedPathSegment() {
   Path::Path trackedPathSegment;
   size_t segment_index{current_segment_index_ + 1};

From 5662c6c6cd54adcbf8ea49ace9d3a1a394883db1 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:31 +0200
Subject: [PATCH 011/118] (feature) Adds DWA-based Vision Follower

---
 .../include/controllers/vision_dwa.h          | 150 +++++++++++++++++
 .../src/controllers/vision_dwa.cpp            | 151 ++++++++++++++++++
 2 files changed, 301 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
new file mode 100644
index 00000000..3b5ef86e
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -0,0 +1,150 @@
+#pragma once
+
+#include "datatypes/control.h"
+#include "datatypes/parameter.h"
+#include "dwa.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+class VisionDWA : public DWA {
+public:
+  struct TrackingImgData {
+    std::array<double, 2> size_xy; // width and height of the bounding box
+    int img_width;
+    int img_height;
+    std::array<double, 2>
+        center_xy; // x, y coordinates of the object center in image frame
+    double depth;  // -1 is equivalent to none
+  };
+
+  class VisionDWAConfig : public ControllerParameters {
+  public:
+    VisionDWAConfig() : ControllerParameters() {
+      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
+      addParameter("control_horizon", Parameter(2, 1, 1000));
+      addParameter("prediction_horizon", Parameter(10, 1, 1000));
+      addParameter("tolerance", Parameter(0.01, 1e-6, 1e3));
+      addParameter("target_distance",
+                   Parameter(0.1, -1.0, 1e9)); // Use -1 for None
+      addParameter(
+          "target_orientation",
+          Parameter(0.0, -M_PI, M_PI)); // target bearing angle with the target
+      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
+      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
+      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("enable_search", Parameter(false));
+    }
+    bool enable_search() const { return getParameter<bool>("enable_search"); }
+    double control_time_step() const {
+      return getParameter<double>("control_time_step");
+    }
+    double target_search_timeout() const {
+      return getParameter<double>("target_search_timeout");
+    }
+    double target_wait_timeout() const {
+      return getParameter<double>("target_wait_timeout");
+    }
+    double target_search_radius() const {
+      return getParameter<double>("target_search_radius");
+    }
+    double search_pause() const {
+      return getParameter<double>("target_search_pause");
+    }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
+    int prediction_horizon() const {
+      return getParameter<int>("prediction_horizon");
+    }
+    double tolerance() const { return getParameter<double>("tolerance"); }
+    double target_distance() const {
+      double val = getParameter<double>("target_distance");
+      return val < 0 ? -1.0 : val; // Return -1 for None
+    }
+    double target_orientation() const {
+      return getParameter<double>("target_orientation");
+    }
+    void set_target_distance(double value) {
+      setParameter("target_distance", value);
+    }
+    double K_omega() const { return getParameter<double>("rotation_gain"); }
+    double K_v() const { return getParameter<double>("speed_gain"); }
+    double min_vel() const { return getParameter<double>("min_vel"); }
+  };
+
+  VisionDWA(const ControlType robotCtrlType,
+            const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+            int maxAngularSamples,
+            const CollisionChecker::ShapeType robotShapeType,
+            const std::vector<float> robotDimensions,
+            const std::array<float, 3> &sensor_position_body,
+            const std::array<float, 4> &sensor_rotation_body,
+            const double octreeRes,
+            CostEvaluator::TrajectoryCostsWeights costWeights,
+            const int maxNumThreads = 1,
+            const VisionDWAConfig config = VisionDWAConfig());
+
+  // Default Destructor
+  ~VisionDWA() = default;
+
+  /**
+   * @brief Get the Pure Tracking Control Command
+   *
+   * @param tracking_pose
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
+
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA sampling
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                            const Velocity2D &current_vel,
+                                            const T &sensor_points);
+
+private:
+  ControlType _ctrlType;
+  ControlLimitsParams _ctrl_limits;
+  VisionDWAConfig _config;
+
+  bool _rotate_in_place;
+  Velocities _out_vel;
+  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
+  std::queue<std::array<double, 3>> _search_commands_queue;
+  std::array<double, 3> _search_command;
+  std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
+
+  /**
+   * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param sensor_points
+   * @return std::tuple<Trajectory2D, bool>
+   */
+  template <typename T>
+  std::tuple<Trajectory2D, bool>
+  getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                              const T &sensor_points);
+  //
+};
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
new file mode 100644
index 00000000..90866583
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -0,0 +1,151 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "utils/angles.h"
+#include "utils/logger.h"
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+VisionDWA::VisionDWA(const ControlType robotCtrlType,
+                     const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+                     int maxAngularSamples,
+                     const CollisionChecker::ShapeType robotShapeType,
+                     const std::vector<float> robotDimensions,
+                     const std::array<float, 3> &sensor_position_body,
+                     const std::array<float, 4> &sensor_rotation_body,
+                     const double octreeRes,
+                     CostEvaluator::TrajectoryCostsWeights costWeights,
+                     const int maxNumThreads, const VisionDWAConfig config)
+    : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
+          config.prediction_horizon(), config.control_horizon(),
+          maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
+          sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
+          maxNumThreads) {
+  _ctrlType = robotCtrlType;
+  _ctrl_limits = ctrlLimits;
+  _config = config;
+  // Initialize time steps
+  int num_steps = _config.control_horizon();
+  // Initialize control vectors
+  _out_vel = Velocities(num_steps);
+  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
+}
+
+Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0);
+  float gamma =
+      Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
+  float psi = Angle::normalizeToMinusPiPlusPi(
+      std::atan2(tracking_pose.y() - currentState.y,
+                 tracking_pose.x() - currentState.x) -
+      currentState.yaw);
+
+  float distance_error = _config.target_distance() - distance;
+  float angle_error =
+      Angle::normalizeToMinusPiPlusPi(_config.target_orientation() - psi);
+
+  float distance_tolerance = _config.tolerance() * _config.target_distance();
+  float angle_tolerance = std::max(0.001, _config.tolerance() * _config.target_orientation());
+
+  // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
+  //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
+
+  Velocity2D followingVel;
+  if (abs(distance_error) > distance_tolerance or
+      abs(angle_error) > angle_tolerance) {
+    float v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    followingVel.setVx(v);
+    float omega = -tracking_pose.omega() +
+                      2 * (v * sin(psi) / distance +
+                           tracking_pose.v() * sin(gamma - psi) / distance -
+                           _config.K_omega() * tanh(angle_error));
+    followingVel.setOmega(omega);
+  }
+  return followingVel;
+}
+
+template <typename T>
+std::tuple<Trajectory2D, bool>
+VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                                       const T &sensor_points) {
+  int step = 0;
+
+  Trajectory2D ref_traj(_config.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
+  while (step < _config.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    simulated_state.update(cmd, _config.control_time_step());
+    simulated_track.update(_config.control_time_step());
+    if(step < _config.prediction_horizon() -1){
+      ref_traj.velocities.add(step,cmd);
+    }
+
+    step++;
+  }
+  this->setCurrentState(original_state);
+
+  bool has_collisions = trajSampler->checkStatesFeasibility(states, sensor_points);
+
+  LOG_INFO("Found reference traj with collisions: ", has_collisions);
+
+  return std::make_tuple(ref_traj, has_collisions);
+}
+
+template <typename T>
+TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                          const Velocity2D &current_vel,
+                                          const T &sensor_points) {
+  Trajectory2D ref_traj;
+  bool has_collisions;
+  std::tie(ref_traj, has_collisions) =
+      this->getTrackingReferenceSegment(tracking_pose, sensor_points);
+  if(!has_collisions){
+    // The tracking sample is collision free -> No need to explore other samples
+    TrajSearchResult result;
+    result.isTrajFound = true;
+    result.trajCost = 0.0;
+    result.trajectory = ref_traj;
+    latest_velocity_command_ = ref_traj.velocities.getFront();
+    return result;
+  }
+  else{
+    LOG_INFO("USING DWA SAMPLING");
+    // The tracking sample has collisions -> use DWA-like sampling and control
+    Path::Path ref_tracking_path;
+    for (int idx=0; idx < _config.prediction_horizon(); ++idx){
+      ref_tracking_path.points.push_back(ref_traj.path.getIndex(idx));
+    }
+    // Set the tracking segment as the reference path
+    this->setCurrentPath(ref_tracking_path);
+    return this->computeVelocityCommandsSet(current_vel, sensor_points);
+  }
+}
+
+// Explicit instantiation for LaserScan
+template Control::TrajSearchResult VisionDWA::getTrackingCtrl<LaserScan>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const LaserScan &sensor_points);
+
+// Explicit instantiation for PointCloud
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const std::vector<Path::Point> &sensor_points);
+
+} // namespace Control
+} // namespace Kompass

From 09e0d4343e06152e24819ffc16d2908c41462519 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:01:05 +0200
Subject: [PATCH 012/118] (feature) Adds VisionDWA test

---
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 src/kompass_cpp/tests/json_export.cpp         |  21 +-
 .../tests/trajectory_sampler_plt.py           |  23 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 210 ++++++++++++++++++
 4 files changed, 247 insertions(+), 12 deletions(-)
 create mode 100644 src/kompass_cpp/tests/vision_dwa_test.cpp

diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index 9600ca10..09d5ce29 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -41,6 +41,11 @@ add_executable(collisions_test collisions_test.cpp)
 target_link_libraries(collisions_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(collisions_tests collisions_test)
 
+# Vision DWA test
+add_executable(vision_dwa_test vision_dwa_test.cpp)
+target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
+add_test(vision_dwa_tests vision_dwa_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/json_export.cpp b/src/kompass_cpp/tests/json_export.cpp
index c3ea3747..7e4d053d 100644
--- a/src/kompass_cpp/tests/json_export.cpp
+++ b/src/kompass_cpp/tests/json_export.cpp
@@ -92,9 +92,8 @@ void to_json(json &j, const Control::TrajectorySamples2D &samples,
 }
 
 // Save trajectories to a JSON file
-void saveTrajectoriesToJson(
-    const Control::TrajectorySamples2D &trajectories,
-    const std::string &filename) {
+void saveTrajectoriesToJson(const Control::TrajectorySamples2D &trajectories,
+                            const std::string &filename) {
   json j;
   to_json(j, trajectories);
   std::ofstream file(filename);
@@ -106,6 +105,22 @@ void saveTrajectoriesToJson(
   }
 }
 
+void saveTrajectoryToJson(const Control::Trajectory2D &trajectory,
+                          const std::string &filename) {
+  json j;
+  j["paths"] = json::array(); // Initialize as a JSON array
+  json j_p;
+  to_json(j_p, trajectory.path);
+  j["paths"].push_back(j_p); // Serialize each Point
+  std::ofstream file(filename);
+  if (file.is_open()) {
+    file << j.dump(4); // Pretty print with 4 spaces indentation
+    file.close();
+  } else {
+    std::cerr << "Unable to open file: " << filename << std::endl;
+  }
+}
+
 // Save one path to a JSON file
 void savePathToJson(const Path::Path &path, const std::string &filename) {
   json j;
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index a80027a6..3a73acb0 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -29,12 +29,13 @@ def read_path_from_json(filename: str):
         print(f"Read file error: {e}")
 
 
-def plot_samples(trajectories, reference, figure_name):
-    # Plot reference
-    ref_path = reference["points"]
-    x_coords = [point["x"] for point in ref_path]
-    y_coords = [point["y"] for point in ref_path]
-    plt.plot(x_coords, y_coords, "--b")
+def plot_samples(figure_name, trajectories, reference=None):
+    if reference:
+        # Plot reference
+        ref_path = reference["points"]
+        x_coords = [point["x"] for point in ref_path]
+        y_coords = [point["y"] for point in ref_path]
+        plt.plot(x_coords, y_coords, "--b", linewidth=7.0, label="reference")
 
     for traj in trajectories:
         path = traj["points"]
@@ -63,7 +64,7 @@ def main():
     parser.add_argument(
         "--reference",
         type=str,
-        required=True,
+        required=False,
         help="Reference path JSON file name in the current directory",
     )
 
@@ -75,9 +76,13 @@ def main():
     reference_file = args.reference
 
     trajectories = read_trajectories_from_json(f"{samples_file}.json")
-    reference = read_path_from_json(f"{reference_file}.json")
+
+    reference = (
+        read_path_from_json(f"{reference_file}.json") if reference_file else None
+    )
+
     # print(trajectories)
-    plot_samples(trajectories, reference, samples_file)
+    plot_samples(samples_file, trajectories, reference)
 
 
 if __name__ == "__main__":
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
new file mode 100644
index 00000000..ce2771fd
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -0,0 +1,210 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "test.h"
+#include "utils/cost_evaluator.h"
+#include "utils/logger.h"
+#include <memory>
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include "json_export.cpp"
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <cmath>
+#include <vector>
+
+using namespace Kompass;
+
+struct VisionDWATestConfig {
+  // Sampling configuration
+  double timeStep;
+  double predictionHorizon;
+  double controlHorizon;
+  int maxLinearSamples;
+  int maxAngularSamples;
+  int maxNumThreads;
+
+  // Octomap resolution
+  double octreeRes;
+
+  // Cost weights
+  Control::CostEvaluator::TrajectoryCostsWeights costWeights;
+
+  // Robot configuration
+  Control::LinearVelocityControlParams x_params;
+  Control::LinearVelocityControlParams y_params;
+  Control::AngularVelocityControlParams angular_params;
+  Control::ControlLimitsParams controlLimits;
+  Control::ControlType controlType;
+  Kompass::CollisionChecker::ShapeType robotShapeType;
+  std::vector<float> robotDimensions;
+  const std::array<float, 3> sensorPositionWRTbody;
+  const std::array<float, 4> sensorRotationWRTbody;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud;
+
+  // Robot start state (pose)
+  Path::State robotState;
+
+  // Tracked target with respect to the robot
+  Control::Velocity2D tracked_vel;
+  Control::TrackedPose2D tracked_pose;
+  // VisionDWA configuration object
+  std::unique_ptr<Control::VisionDWA> controller;
+
+  // Constructor to initialize the struct
+  VisionDWATestConfig(const float timeStep, const float predictionHorizon,
+                      const float controlHorizon, const int maxLinearSamples,
+                      const int maxAngularSamples,
+                      const std::vector<Path::Point> sensor_points,
+                      const float maxVel = 1.0, const float maxOmega = 2.0,
+                      const int maxNumThreads = 1,
+                      const double reference_path_distance_weight = 1.0,
+                      const double goal_distance_weight = 0.5,
+                      const double obstacles_distance_weight = 0.0)
+      : timeStep(timeStep), predictionHorizon(predictionHorizon),
+        controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
+        maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
+        octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5),
+        angular_params(3.14, maxOmega, 3.0, 3.0),
+        controlLimits(x_params, y_params, angular_params),
+        controlType(Control::ControlType::ACKERMANN),
+        robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER),
+        robotDimensions{0.1, 0.4}, sensorPositionWRTbody{0.0, 0.0, 0.0},
+        sensorRotationWRTbody{0, 0, 0, 1}, cloud(sensor_points),
+        robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3),
+        tracked_pose(0.0, 0.0, 0.0, tracked_vel) {
+    // Initialize cost weights
+    costWeights.setParameter("reference_path_distance_weight",
+                             reference_path_distance_weight);
+    costWeights.setParameter("goal_distance_weight", goal_distance_weight);
+    costWeights.setParameter("obstacles_distance_weight",
+                             obstacles_distance_weight);
+    costWeights.setParameter("smoothness_weight", 0.0);
+    costWeights.setParameter("jerk_weight", 0.0);
+    controller = std::make_unique<Control::VisionDWA>(
+        controlType, controlLimits, maxLinearSamples, maxAngularSamples,
+        robotShapeType, robotDimensions, sensorPositionWRTbody,
+        sensorRotationWRTbody, octreeRes, costWeights);
+  }
+
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName) {
+    Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
+    Control::TrajectoryVelocities2D simulated_velocities(
+        numPointsPerTrajectory);
+    Control::TrajectoryPath robot_path(numPointsPerTrajectory),
+        tracked_path(numPointsPerTrajectory);
+    Control::Velocity2D cmd;
+
+    int step = 0;
+    while (step < numPointsPerTrajectory) {
+      Path::Point point(robotState.x, robotState.y, 0.0);
+      robot_path.add(step, point);
+      tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
+      controller->setCurrentState(robotState);
+
+      Control::TrajSearchResult result =
+          controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+      // cmd = controller.getPureTrackingCtrl(tracked_pose);
+      // robotState.update(cmd, timeStep);
+
+      if (result.isTrajFound) {
+        LOG_INFO(FMAG("STEP: "), step,
+                 FMAG(", Found best trajectory with cost: "), result.trajCost);
+
+        cmd.setVx(controller->getLinearVelocityCmdX());
+        cmd.setVy(controller->getLinearVelocityCmdY());
+        cmd.setOmega(controller->getAngularVelocityCmd());
+
+        LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
+                  BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
+                  BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
+                  BOLD(FBLU("}")));
+
+        LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, cmd.vx(), RST,
+                  BOLD(FGRN(", Vy: ")), KGRN, cmd.vy(), RST,
+                  BOLD(FGRN(", Omega: ")), KGRN, cmd.omega(), RST,
+                  BOLD(FGRN("}")));
+
+        robotState.update(cmd, timeStep);
+      } else {
+        LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED,
+                  robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
+                  RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST,
+                  BOLD(FRED("}")));
+        return false;
+      }
+
+      tracked_pose.update(timeStep);
+      step++;
+    }
+    samples.push_back(simulated_velocities, robot_path);
+    samples.push_back(simulated_velocities, tracked_path);
+
+
+    // Plot the trajectories (Save to json then run python script for plotting)
+    boost::filesystem::path executablePath = boost::dll::program_location();
+    std::string file_location = executablePath.parent_path().string();
+
+    std::string trajectories_filename = file_location + "/" + pltFileName;
+
+    saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+    // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+    std::string command = "python3 " + file_location +
+                          "/trajectory_sampler_plt --samples \"" +
+                          trajectories_filename + "\"";
+
+    // Execute the Python script
+    int res = system(command.c_str());
+    return true;
+  }
+};
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon =0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(numPointsPerTrajectory, std::string("vision_follower_obstacle_free"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon = 0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}

From f88523884ca179a2cadab0eca32efb0cc8bff46a Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:34 +0200
Subject: [PATCH 013/118] (feature) Adds path interpolation as optional in
 Follower

---
 .../kompass_cpp/include/controllers/follower.h   |  2 +-
 .../kompass_cpp/src/controllers/follower.cpp     | 16 ++++++++++------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
index e99f6607..7c483475 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
@@ -94,7 +94,7 @@ class Follower : public Controller {
    *
    * @param path Global path to be followed
    */
-  void setCurrentPath(const Path::Path &path);
+  void setCurrentPath(const Path::Path &path, const bool interpolate = true);
 
   void clearCurrentPath();
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index d8fc8ba3..a5cacbb8 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -83,7 +83,7 @@ void Follower::clearCurrentPath() {
   return;
 }
 
-void Follower::setCurrentPath(const Path::Path &path) {
+void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
   // Delete old reference and current path before setting new values
   if (refPath) {
     delete refPath;
@@ -98,13 +98,17 @@ void Follower::setCurrentPath(const Path::Path &path) {
   currentPath->setMaxLength(
       this->config.getParameter<double>("max_path_length"));
 
-  currentPath->interpolate(maxDist, interpolationType);
+  if (interpolate) {
+    currentPath->interpolate(maxDist, interpolationType);
 
-  // Segment path
-  currentPath->segment(path_segment_length);
+    // Segment path
+    currentPath->segment(path_segment_length);
 
-  // Get max number of segments in the path
-  max_segment_index_ = currentPath->getMaxNumSegments();
+    // Get max number of segments in the path
+    max_segment_index_ = currentPath->getMaxNumSegments();
+  } else {
+    max_segment_index_ = 0;
+  }
 
   path_processing_ = true;
   current_segment_index_ = 0;

From b2652ecedc20ebfd974efbd646e5b3dfff39fc8d Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:57 +0200
Subject: [PATCH 014/118] (feature) Adds tracked pose type

---
 .../kompass_cpp/include/datatypes/control.h   | 141 ++++++++++++++++++
 .../kompass_cpp/include/datatypes/path.h      |  11 +-
 2 files changed, 151 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 9c2a52c4..417fb019 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <cmath>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 // Namespace for Control Types
@@ -12,6 +15,103 @@ namespace Control {
 // Enumeration for control modes
 enum class ControlType { ACKERMANN = 0, DIFFERENTIAL_DRIVE = 1, OMNI = 2 };
 
+class Pose3D {
+
+public:
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation_.toRotationMatrix()){};
+
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation.toRotationMatrix()){};
+
+  /**
+   * @brief Construct a new Pose3D object using 2D pose information
+   *
+   * @param pose_x
+   * @param pose_y
+   * @param pose_yaw
+   */
+  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw)
+      {update(pose_x, pose_y, pose_yaw);};
+
+  void setFrame(const std::string &frame_id) { frame_id_ = frame_id; }
+
+  std::string getFrame() const { return frame_id_; }
+
+  bool inFrame(const std::string &frame_id) const {
+    return frame_id == frame_id_;
+  }
+
+  float norm() const { return position_.norm(); };
+
+  /**
+   * @brief Extract x coordinates
+   *
+   * @return float
+   */
+  float x() const { return position_(0); };
+
+  /**
+   * @brief Extract y coordinates
+   *
+   * @return float
+   */
+  float y() const { return position_(1); };
+
+  /**
+   * @brief Extract z coordinates
+   *
+   * @return float
+   */
+  float z() const { return position_(2); };
+
+  /**
+   * @brief Extract pitch (y-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float pitch() const { return std::asin(rotation_matrix_(2, 0)); };
+
+  /**
+   * @brief Extract roll (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float roll() const {
+    return std::atan2(rotation_matrix_(2, 1), rotation_matrix_(2, 2));
+  };
+
+  /**
+   * @brief Extract yaw (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float yaw() const {
+    return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0));
+  };
+
+  void update(const float &pose_x, const float &pose_y, const float &pose_yaw){
+    position_ = {pose_x, pose_y, 0.0};
+    setRotation(0.0, 0.0, pose_yaw);
+  }
+
+  void setRotation(const float pitch, const float roll, const float yaw){
+    Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());
+    Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());
+    orientation_ = rotZ * rotY * rotX;
+    rotation_matrix_ = orientation_.toRotationMatrix();
+  }
+
+  protected :
+  Eigen::Vector3f position_;
+  Eigen::Quaternionf orientation_;
+  Eigen::Matrix3f rotation_matrix_;
+  std::string frame_id_;
+};
+
 class Velocity2D {
 public:
   // Default constructor
@@ -37,6 +137,47 @@ class Velocity2D {
   Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0};
 };
 
+class TrackedPose2D : public Pose3D {
+
+public:
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Vector4f &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Quaternionf &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const Velocity2D &vel)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const float &vx, const float &vy, const float &omega)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega){};
+
+  float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); };
+
+  float omega() const { return vel_.omega(); };
+
+  void update(const float timeStep) {
+    position_(0) +=
+        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * timeStep;
+    position_(1) +=
+        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * timeStep;
+    float yaw = this->yaw() + vel_.omega() * timeStep;
+    setRotation(0.0, 0.0, yaw);
+  }
+
+  float distance(const float x, const float y, const float z = 0.0) const{
+    return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
+                pow(position_.z() - z, 2));
+  }
+
+protected:
+  Velocity2D vel_;
+};
+
 struct Velocities {
   std::vector<float> vx; // Speed on x-asix (m/s)
   std::vector<float> vy;
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index dcec89d7..3af7603a 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "datatypes/control.h"
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
@@ -20,7 +21,15 @@ struct State {
 
   State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0,
         double speedValue = 0.0)
-      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {}
+      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue){};
+
+  void update(const Kompass::Control::Velocity2D &vel, const float timeStep) {
+    this->x +=
+        (vel.vx() * cos(this->yaw) - vel.vy() * sin(this->yaw)) * timeStep;
+    this->y +=
+        (vel.vx() * sin(this->yaw) + vel.vy() * cos(this->yaw)) * timeStep;
+    this->yaw += vel.omega() * timeStep;
+  };
 };
 
 // Point in 3D space

From 0dfc4f8491aecb2a348a4198da42796ad775f297 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:54:17 +0200
Subject: [PATCH 015/118] (feature) Adds sampling utilities in trajectory
 sampler

---
 .../include/utils/trajectory_sampler.h        | 15 ++-
 .../src/utils/trajectory_sampler.cpp          | 97 +++++++++++++++----
 2 files changed, 94 insertions(+), 18 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 7e240de7..0444859c 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -7,6 +7,8 @@
 #include "datatypes/trajectory.h"
 #include <array>
 #include <cmath>
+#include <functional>
+#include <memory>
 #include <vector>
 
 #ifndef MIN_VEL
@@ -95,6 +97,9 @@ class TrajectorySampler {
    */
   ~TrajectorySampler();
 
+  void updateState(const Path::State &current_state);
+
+
   void setSampleDroppingMode(const bool drop_samples);
 
   /**
@@ -124,6 +129,14 @@ class TrajectorySampler {
    */
   void resetOctreeResolution(const double resolution);
 
+  Trajectory2D
+  generateSingleSampleFromVel(const Velocity2D &vel,
+                              const Path::State &pose = Path::State());
+
+  template <typename T>
+  bool checkStatesFeasibility(const std::vector<Path::State> &states,
+                                const T &sensor_points);
+
   size_t numTrajectories;
   size_t numPointsPerTrajectory;
 
@@ -131,7 +144,7 @@ class TrajectorySampler {
   // Protected member variables
   ControlType ctrType;
   ControlLimitsParams ctrlimits;
-  CollisionChecker *collChecker;
+  std::unique_ptr<CollisionChecker> collChecker;
   int maxNumThreads;
 
 private:
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 9a090f3d..912cffe3 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -1,9 +1,11 @@
 #include <array>
 #include <cmath>
 #include <cstdlib>
+#include <memory>
 #include <mutex>
 #include <vector>
 
+#include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
 #include "utils/logger.h"
@@ -29,7 +31,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const double octreeRes,
     const int maxNumThreads) {
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -64,7 +66,7 @@ TrajectorySampler::TrajectorySampler(
     const std::array<float, 4> &sensor_rotation_body, const int maxNumThreads) {
   double octreeRes = config.getParameter<double>("octree_map_resolution");
   // Setup the collision checker
-  collChecker = new CollisionChecker(robotShapeType, robotDimensions,
+  collChecker = std::make_unique<CollisionChecker>(robotShapeType, robotDimensions,
                                      sensor_position_body, sensor_rotation_body,
                                      octreeRes);
 
@@ -89,7 +91,7 @@ TrajectorySampler::TrajectorySampler(
   this->drop_samples_ = config.getParameter<bool>("drop_samples");
 }
 
-TrajectorySampler::~TrajectorySampler() { delete collChecker; }
+TrajectorySampler::~TrajectorySampler() {}
 
 void TrajectorySampler::resetOctreeResolution(const double resolution) {
   collChecker->resetOctreeResolution(resolution);
@@ -125,14 +127,7 @@ void TrajectorySampler::getAdmissibleTrajsFromVel(
   size_t last_free_index{numPointsPerTrajectory - 1};
 
   for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
-
-    simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                         vel.vy() * sin(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                         vel.vy() * cos(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.yaw += vel.omega() * time_step_;
+    simulated_pose.update(vel, time_step_);
 
     // Update the position of the robot in the collision checker (updates the
     // robot collision object) No need to update the Octree (laserscan)
@@ -201,13 +196,8 @@ void TrajectorySampler::getAdmissibleTrajsFromVelDiffDrive(
       simulated_pose.yaw += vel.omega() * time_step_;
       temp_vel = Velocity2D(0.0, 0.0, vel.omega(), vel.steer_ang());
     } else {
-      simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                           vel.vy() * sin(simulated_pose.yaw)) *
-                          time_step_;
-      simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                           vel.vy() * cos(simulated_pose.yaw)) *
-                          time_step_;
       temp_vel = Velocity2D(vel.vx(), vel.vy(), 0.0, 0.0);
+      simulated_pose.update(temp_vel, time_step_);
     }
 
     // Update the position of the robot in the collision checker (updates the
@@ -443,6 +433,10 @@ TrajectorySampler::getNewTrajectories(const Velocity2D &current_vel,
   }
 }
 
+void TrajectorySampler::updateState(const Path::State &current_state){
+  collChecker->updateState(current_state);
+}
+
 TrajectorySamples2D
 TrajectorySampler::generateTrajectories(const Velocity2D &current_vel,
                                         const Path::State &current_pose,
@@ -503,5 +497,74 @@ void TrajectorySampler::UpdateReachableVelocityRange(
       std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001);
 }
 
+template <>
+bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
+    const std::vector<Path::State> &states, const LaserScan &scan) {
+  // collChecker->updateState(states[0]);
+  collChecker->updateScan(scan.ranges, scan.angles);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if(collChecker->checkCollisions()){
+      return true;
+    }
+  }
+  return false;
+}
+
+template <>
+bool TrajectorySampler::checkStatesFeasibility<std::vector<Path::Point>>(
+    const std::vector<Path::State> &states,
+    const std::vector<Path::Point> &cloud) {
+  // collChecker->updateState(states[0]);
+  collChecker->updatePointCloud(cloud);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if (collChecker->checkCollisions()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+Trajectory2D
+TrajectorySampler::generateSingleSampleFromVel(const Velocity2D &vel,
+                                               const Path::State &pose) {
+  Path::State simulated_pose = pose;
+  Trajectory2D trajectory(numPointsPerTrajectory);
+  trajectory.path.add(0, simulated_pose.x, simulated_pose.y);
+  if (ctrType == ControlType::DIFFERENTIAL_DRIVE) {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      if (std::abs(vel.vx()) > MIN_VEL && std::abs(vel.omega()) > MIN_VEL) {
+        // Rotate then move
+        Velocity2D tempVel = vel;
+        tempVel.setVx(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+
+        tempVel.setVx(vel.vx());
+        tempVel.setOmega(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+        i++;
+      }
+      // Else apply directly
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  } else {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  }
+  return trajectory;
+}
+
 }; // namespace Control
 } // namespace Kompass

From 2ac1493aacb140bccf09a88c058b2a9066b5d9ac Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:12 +0200
Subject: [PATCH 016/118] (fix) Uses smart pointers in controller

---
 .../kompass_cpp/include/controllers/dwa.h     | 28 +++++------
 .../kompass_cpp/src/controllers/dwa.cpp       | 46 +++++++++----------
 2 files changed, 38 insertions(+), 36 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 57b4d51d..39e38349 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -8,6 +8,7 @@
 #include "utils/cost_evaluator.h"
 #include "utils/trajectory_sampler.h"
 #include <cstddef>
+#include <memory>
 #include <vector>
 
 namespace Kompass {
@@ -125,19 +126,9 @@ class DWA : public Follower {
     debuggingSamples_ = new TrajectorySamples2D(samples_);
   };
 
-private:
-  TrajectorySampler *trajSampler;
-  CostEvaluator *trajCostEvaluator;
-  double max_forward_distance_ = 0.0;
-  int maxNumThreads;
-  float maxLocalRange_ = 10.0;    // Max range of the laserscan or the robot local map, default to 10 meters. Used to calculate the cost of coming close to obstacles
-  TrajectorySamples2D *debuggingSamples_ = nullptr;
-
-  /**
-   * @brief get maximum reference path length
-   */
-  // size_t getMaxPathLength();
-  size_t getMaxPathLength();
+protected:
+  std::unique_ptr<TrajectorySampler> trajSampler;
+  std::unique_ptr<CostEvaluator> trajCostEvaluator;
 
   /**
    * @brief Given the current position and velocity of the robot, find the
@@ -152,6 +143,17 @@ class DWA : public Follower {
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
                                 const T &scan_points);
 
+private:
+  double max_forward_distance_ = 0.0;
+  int maxNumThreads;
+  TrajectorySamples2D *debuggingSamples_ = nullptr;
+
+  /**
+   * @brief get maximum reference path length
+   */
+  // size_t getMaxPathLength();
+  size_t getMaxPathLength();
+
   Path::Path findTrackedPathSegment();
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 0abe2f8e..228697e7 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -7,7 +7,7 @@
 #include <array>
 #include <cmath>
 #include <cstddef>
-#include <ostream>
+#include <memory>
 #include <stdexcept>
 #include <tuple>
 
@@ -25,13 +25,13 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
 
   trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
+      std::make_unique<CostEvaluator>(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
                         trajSampler->numPointsPerTrajectory, getMaxPathLength());
 
@@ -56,14 +56,14 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler
-  trajSampler = new TrajectorySampler(
+  trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  trajCostEvaluator =
-      new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
-                        controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+  trajCostEvaluator = std::make_unique<CostEvaluator>(
+      costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
+      trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory,
+      getMaxPathLength());
 
   // Update the max forward distance the robot can make
   double timeHorizon = config.getParameter<double>("control_horizon");
@@ -78,8 +78,6 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
 }
 
 DWA::~DWA() {
-  delete trajSampler;
-  delete trajCostEvaluator;
   delete debuggingSamples_;
 }
 
@@ -94,17 +92,16 @@ void DWA::reconfigure(ControlLimitsParams controlLimits,
                       const double octreeRes,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
+
+  trajSampler.reset(new TrajectorySampler(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
+      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads));
 
-  delete trajCostEvaluator;
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -117,17 +114,15 @@ void DWA::reconfigure(TrajectorySampler::TrajectorySamplerParameters config,
                       const std::array<float, 4> &sensor_rotation_body,
                       CostEvaluator::TrajectoryCostsWeights costWeights,
                       const int maxNumThreads) {
-  delete trajSampler;
-  trajSampler = new TrajectorySampler(
-      config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads);
 
-  delete trajCostEvaluator;
+  trajSampler.reset(new TrajectorySampler(
+      config, controlLimits, controlType, robotShapeType, robotDimensions,
+      sensor_position_body, sensor_rotation_body, maxNumThreads));
 
-  trajCostEvaluator =
+  trajCostEvaluator.reset(
       new CostEvaluator(costWeights, sensor_position_body, sensor_rotation_body,
                         controlLimits, trajSampler->numTrajectories,
-                        trajSampler->numPointsPerTrajectory, getMaxPathLength());
+                        trajSampler->numPointsPerTrajectory, getMaxPathLength()));
   this->maxNumThreads = maxNumThreads;
 }
 
@@ -149,6 +144,11 @@ void DWA::addCustomCost(
   trajCostEvaluator->addCustomCost(weight, custom_cost_function);
 }
 
+void DWA::setCurrentState(const Path::State &position) {
+  this->currentState = position;
+  this->trajSampler->updateState(position);
+}
+
 Path::Path DWA::findTrackedPathSegment() {
   Path::Path trackedPathSegment;
   size_t segment_index{current_segment_index_ + 1};

From 6aadf163e785d325ef17980007e5c5cc13c0c00e Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:31 +0200
Subject: [PATCH 017/118] (feature) Adds DWA-based Vision Follower

---
 .../include/controllers/vision_dwa.h          | 150 +++++++++++++++++
 .../src/controllers/vision_dwa.cpp            | 151 ++++++++++++++++++
 2 files changed, 301 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
new file mode 100644
index 00000000..3b5ef86e
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -0,0 +1,150 @@
+#pragma once
+
+#include "datatypes/control.h"
+#include "datatypes/parameter.h"
+#include "dwa.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+class VisionDWA : public DWA {
+public:
+  struct TrackingImgData {
+    std::array<double, 2> size_xy; // width and height of the bounding box
+    int img_width;
+    int img_height;
+    std::array<double, 2>
+        center_xy; // x, y coordinates of the object center in image frame
+    double depth;  // -1 is equivalent to none
+  };
+
+  class VisionDWAConfig : public ControllerParameters {
+  public:
+    VisionDWAConfig() : ControllerParameters() {
+      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
+      addParameter("control_horizon", Parameter(2, 1, 1000));
+      addParameter("prediction_horizon", Parameter(10, 1, 1000));
+      addParameter("tolerance", Parameter(0.01, 1e-6, 1e3));
+      addParameter("target_distance",
+                   Parameter(0.1, -1.0, 1e9)); // Use -1 for None
+      addParameter(
+          "target_orientation",
+          Parameter(0.0, -M_PI, M_PI)); // target bearing angle with the target
+      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
+      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
+      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("enable_search", Parameter(false));
+    }
+    bool enable_search() const { return getParameter<bool>("enable_search"); }
+    double control_time_step() const {
+      return getParameter<double>("control_time_step");
+    }
+    double target_search_timeout() const {
+      return getParameter<double>("target_search_timeout");
+    }
+    double target_wait_timeout() const {
+      return getParameter<double>("target_wait_timeout");
+    }
+    double target_search_radius() const {
+      return getParameter<double>("target_search_radius");
+    }
+    double search_pause() const {
+      return getParameter<double>("target_search_pause");
+    }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
+    int prediction_horizon() const {
+      return getParameter<int>("prediction_horizon");
+    }
+    double tolerance() const { return getParameter<double>("tolerance"); }
+    double target_distance() const {
+      double val = getParameter<double>("target_distance");
+      return val < 0 ? -1.0 : val; // Return -1 for None
+    }
+    double target_orientation() const {
+      return getParameter<double>("target_orientation");
+    }
+    void set_target_distance(double value) {
+      setParameter("target_distance", value);
+    }
+    double K_omega() const { return getParameter<double>("rotation_gain"); }
+    double K_v() const { return getParameter<double>("speed_gain"); }
+    double min_vel() const { return getParameter<double>("min_vel"); }
+  };
+
+  VisionDWA(const ControlType robotCtrlType,
+            const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+            int maxAngularSamples,
+            const CollisionChecker::ShapeType robotShapeType,
+            const std::vector<float> robotDimensions,
+            const std::array<float, 3> &sensor_position_body,
+            const std::array<float, 4> &sensor_rotation_body,
+            const double octreeRes,
+            CostEvaluator::TrajectoryCostsWeights costWeights,
+            const int maxNumThreads = 1,
+            const VisionDWAConfig config = VisionDWAConfig());
+
+  // Default Destructor
+  ~VisionDWA() = default;
+
+  /**
+   * @brief Get the Pure Tracking Control Command
+   *
+   * @param tracking_pose
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
+
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA sampling
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                            const Velocity2D &current_vel,
+                                            const T &sensor_points);
+
+private:
+  ControlType _ctrlType;
+  ControlLimitsParams _ctrl_limits;
+  VisionDWAConfig _config;
+
+  bool _rotate_in_place;
+  Velocities _out_vel;
+  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
+  std::queue<std::array<double, 3>> _search_commands_queue;
+  std::array<double, 3> _search_command;
+  std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
+
+  /**
+   * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param sensor_points
+   * @return std::tuple<Trajectory2D, bool>
+   */
+  template <typename T>
+  std::tuple<Trajectory2D, bool>
+  getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                              const T &sensor_points);
+  //
+};
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
new file mode 100644
index 00000000..90866583
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -0,0 +1,151 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "utils/angles.h"
+#include "utils/logger.h"
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+VisionDWA::VisionDWA(const ControlType robotCtrlType,
+                     const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+                     int maxAngularSamples,
+                     const CollisionChecker::ShapeType robotShapeType,
+                     const std::vector<float> robotDimensions,
+                     const std::array<float, 3> &sensor_position_body,
+                     const std::array<float, 4> &sensor_rotation_body,
+                     const double octreeRes,
+                     CostEvaluator::TrajectoryCostsWeights costWeights,
+                     const int maxNumThreads, const VisionDWAConfig config)
+    : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
+          config.prediction_horizon(), config.control_horizon(),
+          maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
+          sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
+          maxNumThreads) {
+  _ctrlType = robotCtrlType;
+  _ctrl_limits = ctrlLimits;
+  _config = config;
+  // Initialize time steps
+  int num_steps = _config.control_horizon();
+  // Initialize control vectors
+  _out_vel = Velocities(num_steps);
+  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
+}
+
+Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0);
+  float gamma =
+      Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
+  float psi = Angle::normalizeToMinusPiPlusPi(
+      std::atan2(tracking_pose.y() - currentState.y,
+                 tracking_pose.x() - currentState.x) -
+      currentState.yaw);
+
+  float distance_error = _config.target_distance() - distance;
+  float angle_error =
+      Angle::normalizeToMinusPiPlusPi(_config.target_orientation() - psi);
+
+  float distance_tolerance = _config.tolerance() * _config.target_distance();
+  float angle_tolerance = std::max(0.001, _config.tolerance() * _config.target_orientation());
+
+  // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
+  //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
+
+  Velocity2D followingVel;
+  if (abs(distance_error) > distance_tolerance or
+      abs(angle_error) > angle_tolerance) {
+    float v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    followingVel.setVx(v);
+    float omega = -tracking_pose.omega() +
+                      2 * (v * sin(psi) / distance +
+                           tracking_pose.v() * sin(gamma - psi) / distance -
+                           _config.K_omega() * tanh(angle_error));
+    followingVel.setOmega(omega);
+  }
+  return followingVel;
+}
+
+template <typename T>
+std::tuple<Trajectory2D, bool>
+VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                                       const T &sensor_points) {
+  int step = 0;
+
+  Trajectory2D ref_traj(_config.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
+  while (step < _config.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    simulated_state.update(cmd, _config.control_time_step());
+    simulated_track.update(_config.control_time_step());
+    if(step < _config.prediction_horizon() -1){
+      ref_traj.velocities.add(step,cmd);
+    }
+
+    step++;
+  }
+  this->setCurrentState(original_state);
+
+  bool has_collisions = trajSampler->checkStatesFeasibility(states, sensor_points);
+
+  LOG_INFO("Found reference traj with collisions: ", has_collisions);
+
+  return std::make_tuple(ref_traj, has_collisions);
+}
+
+template <typename T>
+TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                          const Velocity2D &current_vel,
+                                          const T &sensor_points) {
+  Trajectory2D ref_traj;
+  bool has_collisions;
+  std::tie(ref_traj, has_collisions) =
+      this->getTrackingReferenceSegment(tracking_pose, sensor_points);
+  if(!has_collisions){
+    // The tracking sample is collision free -> No need to explore other samples
+    TrajSearchResult result;
+    result.isTrajFound = true;
+    result.trajCost = 0.0;
+    result.trajectory = ref_traj;
+    latest_velocity_command_ = ref_traj.velocities.getFront();
+    return result;
+  }
+  else{
+    LOG_INFO("USING DWA SAMPLING");
+    // The tracking sample has collisions -> use DWA-like sampling and control
+    Path::Path ref_tracking_path;
+    for (int idx=0; idx < _config.prediction_horizon(); ++idx){
+      ref_tracking_path.points.push_back(ref_traj.path.getIndex(idx));
+    }
+    // Set the tracking segment as the reference path
+    this->setCurrentPath(ref_tracking_path);
+    return this->computeVelocityCommandsSet(current_vel, sensor_points);
+  }
+}
+
+// Explicit instantiation for LaserScan
+template Control::TrajSearchResult VisionDWA::getTrackingCtrl<LaserScan>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const LaserScan &sensor_points);
+
+// Explicit instantiation for PointCloud
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const std::vector<Path::Point> &sensor_points);
+
+} // namespace Control
+} // namespace Kompass

From d3e8281b5f468209fc0452a2ca5f63429d3325ae Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:01:05 +0200
Subject: [PATCH 018/118] (feature) Adds VisionDWA test

---
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 src/kompass_cpp/tests/json_export.h           |  21 +-
 .../tests/trajectory_sampler_plt.py           |  23 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 210 ++++++++++++++++++
 4 files changed, 247 insertions(+), 12 deletions(-)
 create mode 100644 src/kompass_cpp/tests/vision_dwa_test.cpp

diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index facf185b..e63a6eb2 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -34,6 +34,11 @@ add_executable(collisions_test collisions_test.cpp)
 target_link_libraries(collisions_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(collisions_tests collisions_test)
 
+# Vision DWA test
+add_executable(vision_dwa_test vision_dwa_test.cpp)
+target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
+add_test(vision_dwa_tests vision_dwa_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/json_export.h b/src/kompass_cpp/tests/json_export.h
index 01a06f7a..d99e2d44 100644
--- a/src/kompass_cpp/tests/json_export.h
+++ b/src/kompass_cpp/tests/json_export.h
@@ -92,9 +92,8 @@ inline void to_json(json &j, const Control::TrajectorySamples2D &samples,
 }
 
 // Save trajectories to a JSON file
-inline void saveTrajectoriesToJson(
-    const Control::TrajectorySamples2D &trajectories,
-    const std::string &filename) {
+void saveTrajectoriesToJson(const Control::TrajectorySamples2D &trajectories,
+                            const std::string &filename) {
   json j;
   to_json(j, trajectories);
   std::ofstream file(filename);
@@ -106,6 +105,22 @@ inline void saveTrajectoriesToJson(
   }
 }
 
+void saveTrajectoryToJson(const Control::Trajectory2D &trajectory,
+                          const std::string &filename) {
+  json j;
+  j["paths"] = json::array(); // Initialize as a JSON array
+  json j_p;
+  to_json(j_p, trajectory.path);
+  j["paths"].push_back(j_p); // Serialize each Point
+  std::ofstream file(filename);
+  if (file.is_open()) {
+    file << j.dump(4); // Pretty print with 4 spaces indentation
+    file.close();
+  } else {
+    std::cerr << "Unable to open file: " << filename << std::endl;
+  }
+}
+
 // Save one path to a JSON file
 inline void savePathToJson(const Path::Path &path, const std::string &filename) {
   json j;
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index a80027a6..3a73acb0 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -29,12 +29,13 @@ def read_path_from_json(filename: str):
         print(f"Read file error: {e}")
 
 
-def plot_samples(trajectories, reference, figure_name):
-    # Plot reference
-    ref_path = reference["points"]
-    x_coords = [point["x"] for point in ref_path]
-    y_coords = [point["y"] for point in ref_path]
-    plt.plot(x_coords, y_coords, "--b")
+def plot_samples(figure_name, trajectories, reference=None):
+    if reference:
+        # Plot reference
+        ref_path = reference["points"]
+        x_coords = [point["x"] for point in ref_path]
+        y_coords = [point["y"] for point in ref_path]
+        plt.plot(x_coords, y_coords, "--b", linewidth=7.0, label="reference")
 
     for traj in trajectories:
         path = traj["points"]
@@ -63,7 +64,7 @@ def main():
     parser.add_argument(
         "--reference",
         type=str,
-        required=True,
+        required=False,
         help="Reference path JSON file name in the current directory",
     )
 
@@ -75,9 +76,13 @@ def main():
     reference_file = args.reference
 
     trajectories = read_trajectories_from_json(f"{samples_file}.json")
-    reference = read_path_from_json(f"{reference_file}.json")
+
+    reference = (
+        read_path_from_json(f"{reference_file}.json") if reference_file else None
+    )
+
     # print(trajectories)
-    plot_samples(trajectories, reference, samples_file)
+    plot_samples(samples_file, trajectories, reference)
 
 
 if __name__ == "__main__":
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
new file mode 100644
index 00000000..ce2771fd
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -0,0 +1,210 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "test.h"
+#include "utils/cost_evaluator.h"
+#include "utils/logger.h"
+#include <memory>
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include "json_export.cpp"
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <cmath>
+#include <vector>
+
+using namespace Kompass;
+
+struct VisionDWATestConfig {
+  // Sampling configuration
+  double timeStep;
+  double predictionHorizon;
+  double controlHorizon;
+  int maxLinearSamples;
+  int maxAngularSamples;
+  int maxNumThreads;
+
+  // Octomap resolution
+  double octreeRes;
+
+  // Cost weights
+  Control::CostEvaluator::TrajectoryCostsWeights costWeights;
+
+  // Robot configuration
+  Control::LinearVelocityControlParams x_params;
+  Control::LinearVelocityControlParams y_params;
+  Control::AngularVelocityControlParams angular_params;
+  Control::ControlLimitsParams controlLimits;
+  Control::ControlType controlType;
+  Kompass::CollisionChecker::ShapeType robotShapeType;
+  std::vector<float> robotDimensions;
+  const std::array<float, 3> sensorPositionWRTbody;
+  const std::array<float, 4> sensorRotationWRTbody;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud;
+
+  // Robot start state (pose)
+  Path::State robotState;
+
+  // Tracked target with respect to the robot
+  Control::Velocity2D tracked_vel;
+  Control::TrackedPose2D tracked_pose;
+  // VisionDWA configuration object
+  std::unique_ptr<Control::VisionDWA> controller;
+
+  // Constructor to initialize the struct
+  VisionDWATestConfig(const float timeStep, const float predictionHorizon,
+                      const float controlHorizon, const int maxLinearSamples,
+                      const int maxAngularSamples,
+                      const std::vector<Path::Point> sensor_points,
+                      const float maxVel = 1.0, const float maxOmega = 2.0,
+                      const int maxNumThreads = 1,
+                      const double reference_path_distance_weight = 1.0,
+                      const double goal_distance_weight = 0.5,
+                      const double obstacles_distance_weight = 0.0)
+      : timeStep(timeStep), predictionHorizon(predictionHorizon),
+        controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
+        maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
+        octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5),
+        angular_params(3.14, maxOmega, 3.0, 3.0),
+        controlLimits(x_params, y_params, angular_params),
+        controlType(Control::ControlType::ACKERMANN),
+        robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER),
+        robotDimensions{0.1, 0.4}, sensorPositionWRTbody{0.0, 0.0, 0.0},
+        sensorRotationWRTbody{0, 0, 0, 1}, cloud(sensor_points),
+        robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3),
+        tracked_pose(0.0, 0.0, 0.0, tracked_vel) {
+    // Initialize cost weights
+    costWeights.setParameter("reference_path_distance_weight",
+                             reference_path_distance_weight);
+    costWeights.setParameter("goal_distance_weight", goal_distance_weight);
+    costWeights.setParameter("obstacles_distance_weight",
+                             obstacles_distance_weight);
+    costWeights.setParameter("smoothness_weight", 0.0);
+    costWeights.setParameter("jerk_weight", 0.0);
+    controller = std::make_unique<Control::VisionDWA>(
+        controlType, controlLimits, maxLinearSamples, maxAngularSamples,
+        robotShapeType, robotDimensions, sensorPositionWRTbody,
+        sensorRotationWRTbody, octreeRes, costWeights);
+  }
+
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName) {
+    Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
+    Control::TrajectoryVelocities2D simulated_velocities(
+        numPointsPerTrajectory);
+    Control::TrajectoryPath robot_path(numPointsPerTrajectory),
+        tracked_path(numPointsPerTrajectory);
+    Control::Velocity2D cmd;
+
+    int step = 0;
+    while (step < numPointsPerTrajectory) {
+      Path::Point point(robotState.x, robotState.y, 0.0);
+      robot_path.add(step, point);
+      tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
+      controller->setCurrentState(robotState);
+
+      Control::TrajSearchResult result =
+          controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+      // cmd = controller.getPureTrackingCtrl(tracked_pose);
+      // robotState.update(cmd, timeStep);
+
+      if (result.isTrajFound) {
+        LOG_INFO(FMAG("STEP: "), step,
+                 FMAG(", Found best trajectory with cost: "), result.trajCost);
+
+        cmd.setVx(controller->getLinearVelocityCmdX());
+        cmd.setVy(controller->getLinearVelocityCmdY());
+        cmd.setOmega(controller->getAngularVelocityCmd());
+
+        LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
+                  BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
+                  BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
+                  BOLD(FBLU("}")));
+
+        LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, cmd.vx(), RST,
+                  BOLD(FGRN(", Vy: ")), KGRN, cmd.vy(), RST,
+                  BOLD(FGRN(", Omega: ")), KGRN, cmd.omega(), RST,
+                  BOLD(FGRN("}")));
+
+        robotState.update(cmd, timeStep);
+      } else {
+        LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED,
+                  robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
+                  RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST,
+                  BOLD(FRED("}")));
+        return false;
+      }
+
+      tracked_pose.update(timeStep);
+      step++;
+    }
+    samples.push_back(simulated_velocities, robot_path);
+    samples.push_back(simulated_velocities, tracked_path);
+
+
+    // Plot the trajectories (Save to json then run python script for plotting)
+    boost::filesystem::path executablePath = boost::dll::program_location();
+    std::string file_location = executablePath.parent_path().string();
+
+    std::string trajectories_filename = file_location + "/" + pltFileName;
+
+    saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+    // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+    std::string command = "python3 " + file_location +
+                          "/trajectory_sampler_plt --samples \"" +
+                          trajectories_filename + "\"";
+
+    // Execute the Python script
+    int res = system(command.c_str());
+    return true;
+  }
+};
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon =0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(numPointsPerTrajectory, std::string("vision_follower_obstacle_free"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon = 0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}

From bb10d505940199b09a199bcd5e010ebc295acc6b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 10 Apr 2025 10:54:29 +0200
Subject: [PATCH 019/118] (fix) Fixes include and adds docstrings

---
 .../kompass_cpp/include/controllers/dwa.h     | 20 +++++++++++++++++++
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 11 ++++++----
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 39e38349..2aa94dd5 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -70,10 +70,27 @@ class DWA : public Follower {
                    CostEvaluator::TrajectoryCostsWeights costWeights,
                    const int maxNumThreads = 1);
 
+  /**
+   * @brief Reset the resolution of the robot's local map, this is equivalent to the box size representing each obstacle after conversion
+   *
+   * @param octreeRes
+   */
   void resetOctreeResolution(const double octreeRes);
 
+  /**
+   * @brief Set the robot's sensor or local map max range (meters)
+   *
+   * @param max_range
+   */
   void setSensorMaxRange(const float max_range);
 
+  /**
+   * @brief Set the Current State of the robot
+   *
+   * @param position
+   */
+  void setCurrentState(const Path::State &position);
+
   /**
    * @brief Adds a new custom cost to be used in the trajectory evaluation
    *
@@ -147,6 +164,9 @@ class DWA : public Follower {
   double max_forward_distance_ = 0.0;
   int maxNumThreads;
   TrajectorySamples2D *debuggingSamples_ = nullptr;
+  float maxLocalRange_ =
+      10.0; // Max range of the robot sensor or local map in meters. Used to
+            // calculate the cost of coming close to obstacles
 
   /**
    * @brief get maximum reference path length
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index ce2771fd..9085e9ad 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -7,7 +7,7 @@
 #include "utils/logger.h"
 #include <memory>
 #define BOOST_TEST_MODULE KOMPASS TESTS
-#include "json_export.cpp"
+#include "json_export.h"
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
 #include <boost/filesystem.hpp>
 #include <boost/test/included/unit_test.hpp>
@@ -62,8 +62,8 @@ struct VisionDWATestConfig {
                       const float maxVel = 1.0, const float maxOmega = 2.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 1.0,
-                      const double goal_distance_weight = 0.5,
-                      const double obstacles_distance_weight = 0.0)
+                      const double goal_distance_weight = 1.0,
+                      const double obstacles_distance_weight = 1.0)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
@@ -154,11 +154,14 @@ struct VisionDWATestConfig {
     // savePathToJson(reference_segment, ref_path_filename + ".json");
 
     std::string command = "python3 " + file_location +
-                          "/trajectory_sampler_plt --samples \"" +
+                          "/trajectory_sampler_plt.py --samples \"" +
                           trajectories_filename + "\"";
 
     // Execute the Python script
     int res = system(command.c_str());
+    if (res != 0)
+      throw std::system_error(res, std::generic_category(),
+                              "Python script failed with error code");
     return true;
   }
 };

From e5c83b415986a372645d351df58ad62ddb0de635 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:15:24 +0200
Subject: [PATCH 020/118] (feature) Adds kalman filter for linear state
 estimation

---
 .../kompass_cpp/include/utils/kalman_filter.h |  47 ++++++++
 .../kompass_cpp/src/utils/kalman_filter.cpp   | 110 ++++++++++++++++++
 2 files changed, 157 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
new file mode 100644
index 00000000..646e2f2d
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <optional>
+
+using Eigen::MatrixXf;
+using namespace std;
+
+namespace Kompass {
+
+class LinearSSKalmanFilter {
+private:
+  bool state_initialized = false, system_initialized = false;
+  Eigen::MatrixXf state;
+  Eigen::MatrixXf A; // state matrix
+  Eigen::MatrixXf B; // input matrix
+  Eigen::MatrixXf H; // observation matrix
+  Eigen::MatrixXf P; // uncertainty
+  Eigen::MatrixXf Q; // process noise
+  Eigen::MatrixXf R; // observation noise
+
+public:
+  // constructor
+  LinearSSKalmanFilter(const size_t num_states, const size_t num_inputs);
+
+  // set up the filter
+  bool setup(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B,
+             const Eigen::MatrixXf &Q, const Eigen::MatrixXf &H,
+             const Eigen::MatrixXf &R);
+
+  void setInitialState(const Eigen::VectorXf &initial_state);
+
+  // set A (sometimes sampling time will differ)
+  void setA(const Eigen::MatrixXf &A);
+
+  // state estimate
+  void estimate(const Eigen::MatrixXf &z, const Eigen::MatrixXf &u);
+
+  void estimate(const Eigen::MatrixXf &z);
+
+  // read output from the state
+  double getState(const size_t state_index);
+
+  std::optional<Eigen::MatrixXf> getState();
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
new file mode 100644
index 00000000..68b9e489
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
@@ -0,0 +1,110 @@
+#define EIGEN_DONT_PARALLELIZE  // TODO: resolve error when enabling Eigen parallelization
+#include <Eigen/Dense>
+#include <optional>
+#include "utils/kalman_filter.h"
+#include "utils/logger.h"
+
+
+namespace Kompass {
+
+LinearSSKalmanFilter::LinearSSKalmanFilter(const size_t num_states,
+                                           const size_t num_inputs) {
+  state.resize(num_states, 1);
+  // State update eq: X_dot = A X + B U + Q
+  A.resize(num_states, num_states);
+  B.resize(num_states, num_inputs);
+  Q.resize(num_states, num_states);
+
+  // Observation eq: Z = H X + R
+  H.resize(num_states, num_states);
+  R.resize(num_states, num_states);
+
+  P = Eigen::MatrixXf::Identity(num_states, num_states);
+}
+
+bool LinearSSKalmanFilter::setup(const Eigen::MatrixXf &A,
+                                 const Eigen::MatrixXf &B,
+                                 const Eigen::MatrixXf &Q,
+                                 const Eigen::MatrixXf &H,
+                                 const Eigen::MatrixXf &R) {
+
+  if ((A.size() != this->A.size()) || (B.size() != this->B.size()) ||
+      (Q.size() != this->Q.size()) || (H.size() != this->H.size()) ||
+      (R.size() != this->R.size())) {
+    LOG_ERROR("Cannot setup the KalmanFilter. Matrix size error. Expected the "
+              "following sized: A=",
+              this->A.size(), ", B=", this->B.size(), ", H=", this->H.size(),
+              ", Q=", this->Q.size(), ", R=", this->R.size());
+    return false;
+  }
+  this->A = A;
+  this->B = B;
+  this->H = H;
+  this->R = R;
+  this->Q = Q;
+  this->system_initialized = true;
+  return true;
+}
+
+void LinearSSKalmanFilter::setInitialState(const Eigen::VectorXf& initial_state) {
+  if (initial_state.size() != this->state.size()){
+     LOG_ERROR("Cannot set initial state. Expected the "
+              "following sized: ", this->state.size());
+     throw std::length_error("Error Setting Initial State");
+  }
+  this->state = initial_state;
+  this->state_initialized = true;
+}
+
+void LinearSSKalmanFilter::setA(const Eigen::MatrixXf &A) { this->A = A; }
+
+void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements,
+                                    const Eigen::MatrixXf &inputs) {
+  // predict a new state
+  Eigen::MatrixXf predicted_state = this->A * this->state + this->B * inputs;
+
+  // Covariance Extrapolation Equation
+  this->P = this->A * this->P * this->A.transpose() + this->Q;
+
+  // Innovation Matrix
+  Eigen::MatrixXf S =
+      this->R + this->H * this->P * this->H.transpose();
+
+  // Kalman Gain Matrix
+  Eigen::MatrixXf K =
+      this->P * this->H.transpose() * S.inverse();
+
+  // Update the state
+  this->state =
+      predicted_state + K * (measurements - this->H * predicted_state);
+
+  // Update the estimation uncertainty
+  this->P = (Eigen::MatrixXf::Identity(this->P.rows(), this->P.cols()) -
+             K * this->H) *
+            this->P;
+}
+
+void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements) {
+  // estimate with zero inputs
+  auto size = this->B.cols();
+  Eigen::MatrixXf inputs;
+  inputs.resize(size, 1);
+  inputs = Eigen::MatrixXf::Zero(size, 1);
+  this->estimate(measurements, inputs);
+}
+
+double LinearSSKalmanFilter::getState(const size_t state_index) {
+  if (this->state_initialized) {
+    return this->state(state_index, 0);
+  } else {
+    return 0;
+  }
+}
+
+std::optional<Eigen::MatrixXf> LinearSSKalmanFilter::getState() {
+  if (this->state_initialized and this->system_initialized) {
+    return this->state;
+  }
+  return std::nullopt;
+}
+}

From c443090a1efcf6ce574232fc137cba595c9e271b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:17:30 +0200
Subject: [PATCH 021/118] (feature) Adds 3D detected box vision tracker

---
 src/kompass_cpp/kompass_cpp/CMakeLists.txt    |   2 +-
 .../include/controllers/vision_dwa.h          |   3 -
 .../kompass_cpp/include/datatypes/tracking.h  |  81 ++++++++
 .../kompass_cpp/include/vision/tracker.h      |  47 +++++
 .../kompass_cpp/src/vision/tracker.cpp        | 186 ++++++++++++++++++
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 .../tests/vision_tracking_test.cpp            | 151 ++++++++++++++
 src/ompl_bindings/CMakeLists.txt              |   2 +-
 8 files changed, 472 insertions(+), 5 deletions(-)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
 create mode 100644 src/kompass_cpp/kompass_cpp/include/vision/tracker.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
 create mode 100644 src/kompass_cpp/tests/vision_tracking_test.cpp

diff --git a/src/kompass_cpp/kompass_cpp/CMakeLists.txt b/src/kompass_cpp/kompass_cpp/CMakeLists.txt
index c707cc23..82d8b43c 100644
--- a/src/kompass_cpp/kompass_cpp/CMakeLists.txt
+++ b/src/kompass_cpp/kompass_cpp/CMakeLists.txt
@@ -32,7 +32,7 @@ endif()
 # add project as library
 add_library(${MODULE_NAME} STATIC ${SOURCES})
 target_include_directories(${MODULE_NAME} PUBLIC include)
-target_include_directories(${MODULE_NAME} PUBLIC {$EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS})
+target_include_directories(${MODULE_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS})
 
 # Link fcl and PCL
 target_link_libraries(${MODULE_NAME} PRIVATE fcl ${PCL_LIBRARIES})
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 3b5ef86e..8237b8c8 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -126,9 +126,6 @@ class VisionDWA : public DWA {
 
   bool _rotate_in_place;
   Velocities _out_vel;
-  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
-  std::queue<std::array<double, 3>> _search_commands_queue;
-  std::array<double, 3> _search_command;
   std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
new file mode 100644
index 00000000..63a7cd2d
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -0,0 +1,81 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <vector>
+
+namespace Kompass {
+
+struct Bbox3D {
+  Eigen::Vector3f center = {0.0, 0.0, 0.0};
+  Eigen::Vector3f size = {0.0, 0.0, 0.0};
+  Eigen::Vector2i center_img_frame = {0, 0};
+  Eigen::Vector2i size_img_frame = {0, 0};
+  std::vector<Eigen::Vector3f> pc_points = {};
+
+  Bbox3D() {};
+
+  Bbox3D(const Bbox3D &box)
+      : center(box.center), size(box.size), center_img_frame(box.center_img_frame), size_img_frame(box.size_img_frame), pc_points(box.pc_points){};
+
+  Eigen::Vector2f getXLimitsImg() const {
+    return {
+      center_img_frame.x() - (size_img_frame.x() / 2),
+          center_img_frame.x() + (size_img_frame.x() / 2)
+    };
+  };
+
+  Eigen::Vector2f getYLimitsImg() const {
+    return {center_img_frame.y() - (size_img_frame.y() / 2),
+            center_img_frame.y() + (size_img_frame.y() / 2)};
+  };
+};
+
+struct TrackedBbox3D {
+  Bbox3D box;
+  Eigen::Vector3f vel = {0.0, 0.0, 0.0};
+  Eigen::Vector3f acc = {0.0, 0.0, 0.0};
+  int unique_id = 0;
+
+  TrackedBbox3D(const Bbox3D& box): box(box) {};
+
+  void setState(const Eigen::Matrix<float, 6, 1>& state_vector){
+    this->box.center = {state_vector(0), state_vector(1), 0.0f};
+    this->vel = {state_vector(2), state_vector(3), 0.0f};
+    this->acc = {state_vector(4), state_vector(5), 0.0f};
+  };
+
+  void setSize(const Eigen::Vector3f& size) {
+    this->box.size = size;
+  };
+
+  void setfromBox(const Bbox3D& box){
+    this->box = box;
+  };
+
+  void updateFromNewDetection(const Bbox3D& new_box, const float time_step){
+    // Compute velocity and acceleration based on location change
+    Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
+    Eigen::Vector3f new_acc = (new_vel - this->vel) / time_step;
+    // Update
+    setfromBox(new_box);
+    this->vel = new_vel;
+    this->acc = new_acc;
+  }
+
+  TrackedBbox3D predictConstantVel(const float &dt) {
+    auto predicted_tracking = *this;
+    predicted_tracking.box.center += predicted_tracking.vel * dt;
+    // Set acceleration to zero for constant velocity prediction
+    predicted_tracking.acc = {0.0, 0.0, 0.0};
+    return predicted_tracking;
+  };
+
+  TrackedBbox3D predictConstantAcc(const float &dt) {
+    auto predicted_tracking = *this;
+    predicted_tracking.vel += this->acc * dt;
+    predicted_tracking.box.center += predicted_tracking.vel * dt;
+    return predicted_tracking;
+  };
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
new file mode 100644
index 00000000..66aa7b6b
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <memory>
+#include <optional>
+#include <vector>
+#include "datatypes/tracking.h"
+#include "utils/kalman_filter.h"
+
+namespace  Kompass{
+
+
+class FeatureBasedBboxTracker{
+    public:
+      FeatureBasedBboxTracker(const float& time_step, const float& e_pos, const float& e_vel, const float& e_acc);
+
+      using FeaturesVector = Eigen::Vector<float, 9>;
+
+      bool setInitialTracking(const TrackedBbox3D& bBox);
+
+      bool setInitialTracking(const Bbox3D &bBox);
+
+      bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+                              const std::vector<Bbox3D> &detected_boxes);
+
+      bool updateTracking(const std::vector<Bbox3D> &detected_boxes);
+
+      std::optional<TrackedBbox3D> getRawTracking() const;
+
+      std::optional<Eigen::MatrixXf> getTrackedState() const;
+
+    private:
+      float timeStep_, minAcceptedSimilarityScore_ = 0.0;
+      std::unique_ptr<TrackedBbox3D> trackedBox_;
+      std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
+      bool tracking_started_ = false;
+
+      FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const;
+
+      FeaturesVector extractFeatures(const Bbox3D &bBox) const;
+
+      Eigen::Vector3f computePointsStdDev(const std::vector<Eigen::Vector3f> &pc_points) const;
+
+      void updateTrackedBoxState();
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
new file mode 100644
index 00000000..5dcf18b0
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -0,0 +1,186 @@
+#include "vision/tracker.h"
+#include "datatypes/tracking.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <math.h>
+
+namespace Kompass {
+
+FeatureBasedBboxTracker::FeatureBasedBboxTracker(
+    const float& time_step, const float& e_pos,
+    const float& e_vel, const float& e_acc) {
+
+  timeStep_ = time_step;
+  // Setup Kalman filter matrices
+  Eigen::MatrixXf A;
+  A.resize(6, 6);
+
+  A << 1, 0, time_step, 0, 0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0,
+      0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0, 0, 0, 0, 1, 0,
+      time_step, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+
+  Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 1);
+  Eigen::MatrixXf H = Eigen::MatrixXf::Identity(6, 6);
+  Eigen::MatrixXf Err = Eigen::MatrixXf::Identity(6, 6);
+  Err(0, 0) *= e_pos;
+  Err(1, 1) *= e_pos;
+  Err(2, 2) *= e_vel;
+  Err(3, 3) *= e_vel;
+  Err(4, 4) *= e_acc;
+  Err(5, 5) *= e_acc;
+
+  stateKalmanFilter_ = std::make_unique<LinearSSKalmanFilter>(6, 1);
+  stateKalmanFilter_->setup(A, B, Err, H, Err);
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
+  trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  Eigen::VectorXf state_vec;
+  state_vec.resize(6);
+  state_vec(0) = bBox.box.center[0];
+  state_vec(1) = bBox.box.center[1];
+  state_vec(2) = bBox.vel[0];
+  state_vec(3) = bBox.vel[1];
+  state_vec(4) = bBox.acc[0];
+  state_vec(5) = bBox.acc[1];
+  stateKalmanFilter_->setInitialState(state_vec);
+  this->tracking_started_ = true;
+  return true;
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
+  trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  Eigen::Matrix<float, 6, 1> state_vec = Eigen::Matrix<float, 6, 1>::Zero();
+  state_vec(0) = bBox.center[0];
+  state_vec(1) = bBox.center[1];
+  stateKalmanFilter_->setInitialState(state_vec);
+  this->tracking_started_ = false;
+  return true;
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const int& pose_y_img, const std::vector<Bbox3D>& detected_boxes){
+    std::unique_ptr<Bbox3D> target_box;
+    // Find a detected box containing the point
+    for(auto box: detected_boxes){
+      auto limits_x = box.getXLimitsImg();
+      if(pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)){
+        auto limits_y = box.getYLimitsImg();
+        if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) {
+          target_box = std::make_unique<Bbox3D>(box);
+          break;
+        }
+      }
+    }
+    if(!target_box){
+      // given position was not found inside any detected box
+      return false;
+    }
+    trackedBox_ = std::make_unique<TrackedBbox3D>(*target_box);
+    Eigen::Vector<float, 6> state_vec = Eigen::Vector<float, 6>::Zero();
+    state_vec(0) = target_box->center[0];
+    state_vec(1) = target_box->center[1];
+    stateKalmanFilter_->setInitialState(state_vec);
+    // Target velocity is still unknown
+    this->tracking_started_ = false;
+    return true;
+}
+
+void FeatureBasedBboxTracker::updateTrackedBoxState(){
+  Eigen::MatrixXf measurement;
+  measurement.resize(6, 1);
+  measurement(0) = trackedBox_->box.center.x();
+  measurement(1) = trackedBox_->box.center.y();
+  measurement(2) = trackedBox_->vel.x();
+  measurement(3) = trackedBox_->vel.y();
+  measurement(4) = trackedBox_->acc.x();
+  measurement(5) = trackedBox_->acc.y();
+  stateKalmanFilter_->estimate(measurement);
+}
+
+bool FeatureBasedBboxTracker::updateTracking(const std::vector<Bbox3D> &detected_boxes){
+
+    // Predicted the new location of the tracked box
+    auto predicted_tracked_box = trackedBox_->predictConstantAcc(timeStep_);
+
+    auto ref_box_features = extractFeatures(predicted_tracked_box);
+
+    // Get the features of all the new detections
+    FeaturesVector detected_boxes_feature_vec;
+    float max_similarity_score = 0.0;  // Similarity score
+    size_t similar_box_idx = 0, count = 0;
+    for(auto box: detected_boxes){
+      detected_boxes_feature_vec = extractFeatures(box);
+      auto error_vec = detected_boxes_feature_vec - ref_box_features;
+      float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
+
+      if (similarity_score > max_similarity_score){
+        max_similarity_score = similarity_score;
+        similar_box_idx = count;
+      }
+      count++;
+    }
+    if (max_similarity_score > minAcceptedSimilarityScore_){
+      // Update raw tracking
+      trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx], timeStep_);
+      // Update state by applying kalman filter on the raw measurement
+      updateTrackedBoxState();
+      return true;
+    }
+    return false;
+}
+
+FeatureBasedBboxTracker::FeaturesVector
+FeatureBasedBboxTracker::extractFeatures(
+    const TrackedBbox3D &bBox_tracked) const {
+  return extractFeatures(bBox_tracked.box);
+}
+
+FeatureBasedBboxTracker::FeaturesVector
+FeatureBasedBboxTracker::extractFeatures(const Bbox3D &bBox) const {
+  FeatureBasedBboxTracker::FeaturesVector features_vec = Eigen::Vector<float, 9>::Zero();
+  features_vec.segment<2>(0) = bBox.center.segment<2>(0); // indices 0, 1
+  features_vec.segment<3>(2) = bBox.size; // indices 2, 3, 4
+  features_vec(5) = bBox.pc_points.size();
+  if (features_vec(5) > 0.0) {
+    // Compute point cloud points standard deviation
+    auto std_vec = this->computePointsStdDev(bBox.pc_points);
+    features_vec.segment<3>(6) = std_vec; // indices 6, 7, 8
+  }
+  return features_vec;
+}
+
+std::optional<TrackedBbox3D> FeatureBasedBboxTracker::getRawTracking() const {
+  if (trackedBox_) {
+     return *trackedBox_;
+  }
+  return std::nullopt;
+}
+
+std::optional<Eigen::MatrixXf>
+FeatureBasedBboxTracker::getTrackedState() const {
+  if (trackedBox_) {
+    return stateKalmanFilter_->getState();
+  }
+  return std::nullopt;
+}
+
+Eigen::Vector3f FeatureBasedBboxTracker::computePointsStdDev(
+    const std::vector<Eigen::Vector3f> &pc_points) const {
+  // compute the mean in each direction
+  auto size = std::max(int(pc_points.size() - 1), 1);
+  Eigen::Vector3f mean = Eigen::Vector3f::Zero();
+  for (auto point: pc_points){
+    mean += point;
+  }
+  mean /= size;
+  // Compute the variance
+  Eigen::Vector3f variance = Eigen::Vector3f::Zero();
+  for (auto point : pc_points) {
+    auto diff = point - mean;
+    variance += diff.cwiseProduct(diff);
+  }
+  variance /= size;
+  // return the standard deviation
+  return variance.array().sqrt();
+}
+
+} // namespace Kompass
diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index e63a6eb2..7bcebad8 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -39,6 +39,11 @@ add_executable(vision_dwa_test vision_dwa_test.cpp)
 target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(vision_dwa_tests vision_dwa_test)
 
+# Set path to the test image
+add_executable(vision_tracking_test vision_tracking_test.cpp)
+target_link_libraries(vision_tracking_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework nlohmann_json::nlohmann_json)
+add_test(vision_tracking_tests vision_tracking_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp
new file mode 100644
index 00000000..412655a0
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_tracking_test.cpp
@@ -0,0 +1,151 @@
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/tracking.h"
+#include "datatypes/trajectory.h"
+#include "json_export.h"
+#include "test.h"
+#include "utils/logger.h"
+#include "vision/tracker.h"
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <opencv4/opencv2/imgcodecs.hpp>
+#include <random>
+
+using namespace Kompass;
+
+struct VisionTrackingTestConfig {
+  float time_step;
+  int maxSteps;
+  Eigen::Vector3f target_ref_vel;
+  std::unique_ptr<FeatureBasedBboxTracker> tracker;
+  Control::TrajectoryPath reference_path, measured_path, tracked_path;
+  Path::State reference_state;
+  std::vector<Bbox3D> detected_boxes;
+  std::string pltFileName = "VisionTrackerTest";
+  std::mt19937 gen;
+  std::normal_distribution<float> noise;
+
+  VisionTrackingTestConfig(const int &maxSteps = 50,
+                           const float &time_step = 0.1,
+                           const float &e_pos = 0.1, const float &e_vel = 0.1,
+                           const float &e_acc = 0.1,
+                           const Eigen::Vector3f target_pose = {0.0f, 0.0f, 0.0f},
+                           const Eigen::Vector3f target_box_size = {0.5f, 0.5f,
+                                                                    1.0f},
+                           const int num_test_boxes = 3, const float target_v = 0.2, const float target_omega = 0.3)
+      : time_step(time_step), maxSteps(maxSteps) {
+    tracker = std::make_unique<FeatureBasedBboxTracker>(time_step, e_pos, e_vel,
+                                                        e_acc);
+    Bbox3D tracked_box;
+    tracked_box.center = target_pose;
+    tracked_box.size = target_box_size;
+    tracker->setInitialTracking(tracked_box);
+    reference_path = Control::TrajectoryPath(maxSteps);
+    tracked_path = Control::TrajectoryPath(maxSteps);
+    measured_path = Control::TrajectoryPath(maxSteps);
+    reference_state = Path::State(target_pose.x(), target_pose.y());
+    detected_boxes.resize(num_test_boxes);
+    detected_boxes[0] = tracked_box;
+    Bbox3D new_box;
+    new_box.size = target_box_size;
+    for(int i=1; i < num_test_boxes; ++i){
+      auto new_box_shift = Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f});
+      new_box.center = new_box_shift + target_pose;
+      detected_boxes[i] = new_box;
+    }
+
+    target_ref_vel = {target_v * cos(target_omega), target_v * sin(target_omega), 0.0};
+
+    // Random noise generator
+    std::random_device rd; // Obtain a random number from hardware
+    gen = std::mt19937(rd());
+    // Define the normal distribution with mean and standard deviation
+    noise = std::normal_distribution<float>(0.0f, 0.01f);
+  };
+
+  void moveDetectedBoxes(const int step){
+    // Update the detected boxes using the velocity command with additional measurement noise
+    for(auto &box: detected_boxes){
+      Eigen::Vector3f noise_vector(noise(gen), noise(gen), noise(gen));
+      box.center += target_ref_vel * time_step + noise_vector;
+    }
+    // Update the reference state using same velocity command (real)
+    reference_state.x += target_ref_vel.x() * time_step;
+    reference_state.y += target_ref_vel.y() * time_step;
+    Path::Point point(reference_state.x, reference_state.y, 0.0);
+    reference_path.add(
+        step, Path::Point(reference_state.x, reference_state.y, 0.0));
+  };
+};
+
+
+BOOST_AUTO_TEST_CASE(test_Vision_Tracker) {
+  // Create timer
+  Timer time;
+  VisionTrackingTestConfig config = VisionTrackingTestConfig();
+  Control::TrajectorySamples2D samples(3, config.maxSteps);
+  Control::TrajectoryVelocities2D simulated_velocities(config.maxSteps);
+
+  int step = 0;
+  float tracking_error = 0.0;
+  while (step < config.maxSteps) {
+    // Move the detected boxes one step using the reference velocity
+    config.moveDetectedBoxes(step);
+    // Sed detected boxes and ask tracker to update
+    config.tracker->updateTracking(config.detected_boxes);
+    auto measured_track = config.tracker->getRawTracking();
+    auto tracked_state = config.tracker->getTrackedState();
+    if(tracked_state){
+      auto mat = tracked_state->col(0);
+      LOG_INFO("Real target location at", config.reference_state.x, ", ", config.reference_state.y);
+      LOG_INFO("Found tracked target at", mat(0), ", ", mat(1));
+      tracking_error +=
+          Path::Path::distance(mat.segment(0, 3), Path::Point(config.reference_state.x,
+                                                   config.reference_state.y, 0.0));
+      config.tracked_path.add(step, mat.segment(0, 3));
+      config.measured_path.add(step, Path::Point(measured_track->box.center.x(),
+                                                 measured_track->box.center.y(),
+                                                 0.0));
+      if (step < config.maxSteps - 1)
+        simulated_velocities.add(step, Control::Velocity2D());
+    }
+    else{
+        LOG_ERROR("Lost tracked target at step: ", step);
+        break;
+    }
+    step++;
+  }
+
+  // End Tracking error
+  tracking_error /= config.maxSteps;
+  LOG_INFO("Average Error Used Noisy Measurements = ", tracking_error);
+  BOOST_TEST(tracking_error <= 0.1 , "Tracking error is larger than 10%");
+
+  samples.push_back(simulated_velocities, config.reference_path);
+  samples.push_back(simulated_velocities, config.measured_path);
+  samples.push_back(simulated_velocities, config.tracked_path);
+
+  // Plot the trajectories (Save to json then run python script for plotting)
+  boost::filesystem::path executablePath = boost::dll::program_location();
+  std::string file_location = executablePath.parent_path().string();
+
+  std::string trajectories_filename = file_location + "/" + config.pltFileName;
+
+  saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+  // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+  std::string command = "python3 " + file_location +
+                        "/trajectory_sampler_plt.py --samples \"" +
+                        trajectories_filename + "\"";
+
+  // Execute the Python script
+  int res = system(command.c_str());
+  if (res != 0)
+    throw std::system_error(res, std::generic_category(),
+                            "Python script failed with error code");
+}
+
diff --git a/src/ompl_bindings/CMakeLists.txt b/src/ompl_bindings/CMakeLists.txt
index bc1cc094..52c48c61 100644
--- a/src/ompl_bindings/CMakeLists.txt
+++ b/src/ompl_bindings/CMakeLists.txt
@@ -16,7 +16,7 @@ find_package(nanobind CONFIG REQUIRED)
 
 # add bindings as pybind11 module
 nanobind_add_module(${MODULE_NAME} STABLE_ABI bindings.cpp)
-target_include_directories(${MODULE_NAME} PRIVATE {$EIGEN3_INCLUDE_DIR} ${OMPL_INCLUDE_DIRS})
+target_include_directories(${MODULE_NAME} PRIVATE ${EIGEN3_INCLUDE_DIR} ${OMPL_INCLUDE_DIRS})
 target_link_libraries(${MODULE_NAME} PRIVATE ${OMPL_LIBRARIES})
 
 # install location

From ae6dcbbb42da74109f75cb01d68c5185635f62e9 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:21:16 +0200
Subject: [PATCH 022/118] (fix) removes unused includes

---
 src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp | 2 --
 src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp      | 1 -
 2 files changed, 3 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
index 68b9e489..137106eb 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
@@ -1,6 +1,4 @@
 #define EIGEN_DONT_PARALLELIZE  // TODO: resolve error when enabling Eigen parallelization
-#include <Eigen/Dense>
-#include <optional>
 #include "utils/kalman_filter.h"
 #include "utils/logger.h"
 
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 5dcf18b0..d53b13de 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -1,6 +1,5 @@
 #include "vision/tracker.h"
 #include "datatypes/tracking.h"
-#include <Eigen/src/Core/Matrix.h>
 #include <math.h>
 
 namespace Kompass {

From cd51ed01e89bb082e024140d35d119b6c772a0b1 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:34 +0200
Subject: [PATCH 023/118] (feature) Adds path interpolation as optional in
 Follower

---
 .../kompass_cpp/include/controllers/follower.h     |  2 +-
 .../kompass_cpp/src/controllers/follower.cpp       | 14 +++++++++-----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
index ea2421a8..2e18f9c1 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
@@ -82,7 +82,7 @@ class Follower : public Controller {
    *
    * @param path Global path to be followed
    */
-  void setCurrentPath(const Path::Path &path);
+  void setCurrentPath(const Path::Path &path, const bool interpolate = true);
 
   void clearCurrentPath();
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index e37396ba..0c5e4a49 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -88,13 +88,17 @@ void Follower::setCurrentPath(const Path::Path &path) {
   currentPath->setMaxLength(
       this->config.getParameter<double>("max_path_length"));
 
-  currentPath->interpolate(maxDist, interpolationType);
+  if (interpolate) {
+    currentPath->interpolate(maxDist, interpolationType);
 
-  // Segment path
-  currentPath->segment(path_segment_length);
+    // Segment path
+    currentPath->segment(path_segment_length);
 
-  // Get max number of segments in the path
-  max_segment_index_ = currentPath->getMaxNumSegments();
+    // Get max number of segments in the path
+    max_segment_index_ = currentPath->getMaxNumSegments();
+  } else {
+    max_segment_index_ = 0;
+  }
 
   path_processing_ = true;
   current_segment_index_ = 0;

From da37efb284ba1d3ff29093ef56d2d440eb4178e0 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:53:57 +0200
Subject: [PATCH 024/118] (feature) Adds tracked pose type

---
 .../kompass_cpp/include/datatypes/control.h   | 141 ++++++++++++++++++
 .../kompass_cpp/include/datatypes/path.h      |  11 +-
 2 files changed, 151 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 9c2a52c4..417fb019 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -1,8 +1,11 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <cmath>
 #include <stdexcept>
+#include <string>
 #include <vector>
 
 // Namespace for Control Types
@@ -12,6 +15,103 @@ namespace Control {
 // Enumeration for control modes
 enum class ControlType { ACKERMANN = 0, DIFFERENTIAL_DRIVE = 1, OMNI = 2 };
 
+class Pose3D {
+
+public:
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation_.toRotationMatrix()){};
+
+  Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation)
+      : position_(position), orientation_(orientation),
+        rotation_matrix_(orientation.toRotationMatrix()){};
+
+  /**
+   * @brief Construct a new Pose3D object using 2D pose information
+   *
+   * @param pose_x
+   * @param pose_y
+   * @param pose_yaw
+   */
+  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw)
+      {update(pose_x, pose_y, pose_yaw);};
+
+  void setFrame(const std::string &frame_id) { frame_id_ = frame_id; }
+
+  std::string getFrame() const { return frame_id_; }
+
+  bool inFrame(const std::string &frame_id) const {
+    return frame_id == frame_id_;
+  }
+
+  float norm() const { return position_.norm(); };
+
+  /**
+   * @brief Extract x coordinates
+   *
+   * @return float
+   */
+  float x() const { return position_(0); };
+
+  /**
+   * @brief Extract y coordinates
+   *
+   * @return float
+   */
+  float y() const { return position_(1); };
+
+  /**
+   * @brief Extract z coordinates
+   *
+   * @return float
+   */
+  float z() const { return position_(2); };
+
+  /**
+   * @brief Extract pitch (y-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float pitch() const { return std::asin(rotation_matrix_(2, 0)); };
+
+  /**
+   * @brief Extract roll (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float roll() const {
+    return std::atan2(rotation_matrix_(2, 1), rotation_matrix_(2, 2));
+  };
+
+  /**
+   * @brief Extract yaw (x-axis rotation) from the rotation matrix
+   *
+   * @return float
+   */
+  float yaw() const {
+    return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0));
+  };
+
+  void update(const float &pose_x, const float &pose_y, const float &pose_yaw){
+    position_ = {pose_x, pose_y, 0.0};
+    setRotation(0.0, 0.0, pose_yaw);
+  }
+
+  void setRotation(const float pitch, const float roll, const float yaw){
+    Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());
+    Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());
+    orientation_ = rotZ * rotY * rotX;
+    rotation_matrix_ = orientation_.toRotationMatrix();
+  }
+
+  protected :
+  Eigen::Vector3f position_;
+  Eigen::Quaternionf orientation_;
+  Eigen::Matrix3f rotation_matrix_;
+  std::string frame_id_;
+};
+
 class Velocity2D {
 public:
   // Default constructor
@@ -37,6 +137,47 @@ class Velocity2D {
   Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0};
 };
 
+class TrackedPose2D : public Pose3D {
+
+public:
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Vector4f &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const Eigen::Vector3f &position,
+                const Eigen::Quaternionf &orientation, const Velocity2D &vel)
+      : Pose3D(position, orientation), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const Velocity2D &vel)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel){};
+
+  TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
+                const float &vx, const float &vy, const float &omega)
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega){};
+
+  float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); };
+
+  float omega() const { return vel_.omega(); };
+
+  void update(const float timeStep) {
+    position_(0) +=
+        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * timeStep;
+    position_(1) +=
+        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * timeStep;
+    float yaw = this->yaw() + vel_.omega() * timeStep;
+    setRotation(0.0, 0.0, yaw);
+  }
+
+  float distance(const float x, const float y, const float z = 0.0) const{
+    return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
+                pow(position_.z() - z, 2));
+  }
+
+protected:
+  Velocity2D vel_;
+};
+
 struct Velocities {
   std::vector<float> vx; // Speed on x-asix (m/s)
   std::vector<float> vy;
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index d33e1f71..e7c7da8c 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "datatypes/control.h"
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
@@ -20,7 +21,15 @@ struct State {
 
   State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0,
         double speedValue = 0.0)
-      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {}
+      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue){};
+
+  void update(const Kompass::Control::Velocity2D &vel, const float timeStep) {
+    this->x +=
+        (vel.vx() * cos(this->yaw) - vel.vy() * sin(this->yaw)) * timeStep;
+    this->y +=
+        (vel.vx() * sin(this->yaw) + vel.vy() * cos(this->yaw)) * timeStep;
+    this->yaw += vel.omega() * timeStep;
+  };
 };
 
 // Point in 3D space

From a204f45c5111288786306a3dc05259cae439fa4b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 16:54:17 +0200
Subject: [PATCH 025/118] (feature) Adds sampling utilities in trajectory
 sampler

---
 .../include/utils/trajectory_sampler.h        | 13 +++
 .../src/utils/trajectory_sampler.cpp          | 86 ++++++++++++++++---
 2 files changed, 85 insertions(+), 14 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 47c9684e..f571d287 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -7,6 +7,8 @@
 #include "datatypes/trajectory.h"
 #include <array>
 #include <cmath>
+#include <functional>
+#include <memory>
 #include <vector>
 
 #ifndef MIN_VEL
@@ -95,6 +97,9 @@ class TrajectorySampler {
    */
   ~TrajectorySampler() = default;
 
+  void updateState(const Path::State &current_state);
+
+
   void setSampleDroppingMode(const bool drop_samples);
 
   /**
@@ -124,6 +129,14 @@ class TrajectorySampler {
    */
   void resetOctreeResolution(const double resolution);
 
+  Trajectory2D
+  generateSingleSampleFromVel(const Velocity2D &vel,
+                              const Path::State &pose = Path::State());
+
+  template <typename T>
+  bool checkStatesFeasibility(const std::vector<Path::State> &states,
+                                const T &sensor_points);
+
   size_t numTrajectories;
   size_t numPointsPerTrajectory;
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 09400f1f..2c00d848 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -5,6 +5,7 @@
 #include <mutex>
 #include <vector>
 
+#include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
 #include "utils/logger.h"
@@ -123,14 +124,7 @@ void TrajectorySampler::getAdmissibleTrajsFromVel(
   size_t last_free_index{numPointsPerTrajectory - 1};
 
   for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
-
-    simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                         vel.vy() * sin(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                         vel.vy() * cos(simulated_pose.yaw)) *
-                        time_step_;
-    simulated_pose.yaw += vel.omega() * time_step_;
+    simulated_pose.update(vel, time_step_);
 
     // Update the position of the robot in the collision checker (updates the
     // robot collision object) No need to update the Octree (laserscan)
@@ -199,13 +193,8 @@ void TrajectorySampler::getAdmissibleTrajsFromVelDiffDrive(
       simulated_pose.yaw += vel.omega() * time_step_;
       temp_vel = Velocity2D(0.0, 0.0, vel.omega(), vel.steer_ang());
     } else {
-      simulated_pose.x += (vel.vx() * cos(simulated_pose.yaw) -
-                           vel.vy() * sin(simulated_pose.yaw)) *
-                          time_step_;
-      simulated_pose.y += (vel.vx() * sin(simulated_pose.yaw) +
-                           vel.vy() * cos(simulated_pose.yaw)) *
-                          time_step_;
       temp_vel = Velocity2D(vel.vx(), vel.vy(), 0.0, 0.0);
+      simulated_pose.update(temp_vel, time_step_);
     }
 
     // Update the position of the robot in the collision checker (updates the
@@ -511,5 +500,74 @@ void TrajectorySampler::UpdateReachableVelocityRange(
       std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001);
 }
 
+template <>
+bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
+    const std::vector<Path::State> &states, const LaserScan &scan) {
+  // collChecker->updateState(states[0]);
+  collChecker->updateScan(scan.ranges, scan.angles);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if(collChecker->checkCollisions()){
+      return true;
+    }
+  }
+  return false;
+}
+
+template <>
+bool TrajectorySampler::checkStatesFeasibility<std::vector<Path::Point>>(
+    const std::vector<Path::State> &states,
+    const std::vector<Path::Point> &cloud) {
+  // collChecker->updateState(states[0]);
+  collChecker->updatePointCloud(cloud);
+  for (auto state : states) {
+    collChecker->updateState(state);
+    // Update the PointCloud values
+    if (collChecker->checkCollisions()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+Trajectory2D
+TrajectorySampler::generateSingleSampleFromVel(const Velocity2D &vel,
+                                               const Path::State &pose) {
+  Path::State simulated_pose = pose;
+  Trajectory2D trajectory(numPointsPerTrajectory);
+  trajectory.path.add(0, simulated_pose.x, simulated_pose.y);
+  if (ctrType == ControlType::DIFFERENTIAL_DRIVE) {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      if (std::abs(vel.vx()) > MIN_VEL && std::abs(vel.omega()) > MIN_VEL) {
+        // Rotate then move
+        Velocity2D tempVel = vel;
+        tempVel.setVx(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+
+        tempVel.setVx(vel.vx());
+        tempVel.setOmega(0.0);
+        simulated_pose.update(tempVel, time_step_);
+        trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+        trajectory.velocities.add(i, vel);
+        i++;
+      }
+      // Else apply directly
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  } else {
+    for (size_t i = 0; i < (numPointsPerTrajectory - 1); ++i) {
+      simulated_pose.update(vel, time_step_);
+      trajectory.path.add(i + 1, simulated_pose.x, simulated_pose.y);
+      trajectory.velocities.add(i, vel);
+    }
+  }
+  return trajectory;
+}
+
 }; // namespace Control
 } // namespace Kompass

From 2799fcedd144fe233a526e2c9cdc83a810903898 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:12 +0200
Subject: [PATCH 026/118] (fix) Uses smart pointers in controller

---
 src/kompass_cpp/kompass_cpp/include/controllers/dwa.h | 11 +++++++++++
 src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp   | 10 ++++++++--
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 7285d12e..f25a6dd7 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -144,6 +144,17 @@ class DWA : public Follower {
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
                                 const T &scan_points);
 
+private:
+  double max_forward_distance_ = 0.0;
+  int maxNumThreads;
+  TrajectorySamples2D *debuggingSamples_ = nullptr;
+
+  /**
+   * @brief get maximum reference path length
+   */
+  // size_t getMaxPathLength();
+  size_t getMaxPathLength();
+
   Path::Path findTrackedPathSegment();
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 7515be37..077c724f 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -7,6 +7,7 @@
 #include <array>
 #include <cmath>
 #include <cstddef>
+#include <memory>
 #include <stdexcept>
 #include <tuple>
 
@@ -78,7 +79,7 @@ void DWA::configure(ControlLimitsParams controlLimits,
   trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
+      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads));
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
       costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
@@ -98,7 +99,7 @@ void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config,
                       const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads);
+      sensor_position_body, sensor_rotation_body, maxNumThreads));
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
       costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
@@ -120,6 +121,11 @@ void DWA::addCustomCost(
   trajCostEvaluator->addCustomCost(weight, custom_cost_function);
 }
 
+void DWA::setCurrentState(const Path::State &position) {
+  this->currentState = position;
+  this->trajSampler->updateState(position);
+}
+
 Path::Path DWA::findTrackedPathSegment() {
   std::vector<Path::Point> trackedPoints;
   size_t segment_index{current_segment_index_ + 1};

From b9678d87954c9e7e4f42794dc6a038e8e05d0f62 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:00:31 +0200
Subject: [PATCH 027/118] (feature) Adds DWA-based Vision Follower

---
 .../include/controllers/vision_dwa.h          | 150 +++++++++++++++++
 .../src/controllers/vision_dwa.cpp            | 151 ++++++++++++++++++
 2 files changed, 301 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
new file mode 100644
index 00000000..3b5ef86e
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -0,0 +1,150 @@
+#pragma once
+
+#include "datatypes/control.h"
+#include "datatypes/parameter.h"
+#include "dwa.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <cmath>
+#include <memory>
+#include <queue>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+class VisionDWA : public DWA {
+public:
+  struct TrackingImgData {
+    std::array<double, 2> size_xy; // width and height of the bounding box
+    int img_width;
+    int img_height;
+    std::array<double, 2>
+        center_xy; // x, y coordinates of the object center in image frame
+    double depth;  // -1 is equivalent to none
+  };
+
+  class VisionDWAConfig : public ControllerParameters {
+  public:
+    VisionDWAConfig() : ControllerParameters() {
+      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
+      addParameter("control_horizon", Parameter(2, 1, 1000));
+      addParameter("prediction_horizon", Parameter(10, 1, 1000));
+      addParameter("tolerance", Parameter(0.01, 1e-6, 1e3));
+      addParameter("target_distance",
+                   Parameter(0.1, -1.0, 1e9)); // Use -1 for None
+      addParameter(
+          "target_orientation",
+          Parameter(0.0, -M_PI, M_PI)); // target bearing angle with the target
+      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
+      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
+      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("enable_search", Parameter(false));
+    }
+    bool enable_search() const { return getParameter<bool>("enable_search"); }
+    double control_time_step() const {
+      return getParameter<double>("control_time_step");
+    }
+    double target_search_timeout() const {
+      return getParameter<double>("target_search_timeout");
+    }
+    double target_wait_timeout() const {
+      return getParameter<double>("target_wait_timeout");
+    }
+    double target_search_radius() const {
+      return getParameter<double>("target_search_radius");
+    }
+    double search_pause() const {
+      return getParameter<double>("target_search_pause");
+    }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
+    int prediction_horizon() const {
+      return getParameter<int>("prediction_horizon");
+    }
+    double tolerance() const { return getParameter<double>("tolerance"); }
+    double target_distance() const {
+      double val = getParameter<double>("target_distance");
+      return val < 0 ? -1.0 : val; // Return -1 for None
+    }
+    double target_orientation() const {
+      return getParameter<double>("target_orientation");
+    }
+    void set_target_distance(double value) {
+      setParameter("target_distance", value);
+    }
+    double K_omega() const { return getParameter<double>("rotation_gain"); }
+    double K_v() const { return getParameter<double>("speed_gain"); }
+    double min_vel() const { return getParameter<double>("min_vel"); }
+  };
+
+  VisionDWA(const ControlType robotCtrlType,
+            const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+            int maxAngularSamples,
+            const CollisionChecker::ShapeType robotShapeType,
+            const std::vector<float> robotDimensions,
+            const std::array<float, 3> &sensor_position_body,
+            const std::array<float, 4> &sensor_rotation_body,
+            const double octreeRes,
+            CostEvaluator::TrajectoryCostsWeights costWeights,
+            const int maxNumThreads = 1,
+            const VisionDWAConfig config = VisionDWAConfig());
+
+  // Default Destructor
+  ~VisionDWA() = default;
+
+  /**
+   * @brief Get the Pure Tracking Control Command
+   *
+   * @param tracking_pose
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
+
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA sampling
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                            const Velocity2D &current_vel,
+                                            const T &sensor_points);
+
+private:
+  ControlType _ctrlType;
+  ControlLimitsParams _ctrl_limits;
+  VisionDWAConfig _config;
+
+  bool _rotate_in_place;
+  Velocities _out_vel;
+  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
+  std::queue<std::array<double, 3>> _search_commands_queue;
+  std::array<double, 3> _search_command;
+  std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
+
+  /**
+   * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param sensor_points
+   * @return std::tuple<Trajectory2D, bool>
+   */
+  template <typename T>
+  std::tuple<Trajectory2D, bool>
+  getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                              const T &sensor_points);
+  //
+};
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
new file mode 100644
index 00000000..90866583
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -0,0 +1,151 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "utils/angles.h"
+#include "utils/logger.h"
+#include <cmath>
+#include <tuple>
+#include <vector>
+
+namespace Kompass {
+namespace Control {
+
+VisionDWA::VisionDWA(const ControlType robotCtrlType,
+                     const ControlLimitsParams ctrlLimits, int maxLinearSamples,
+                     int maxAngularSamples,
+                     const CollisionChecker::ShapeType robotShapeType,
+                     const std::vector<float> robotDimensions,
+                     const std::array<float, 3> &sensor_position_body,
+                     const std::array<float, 4> &sensor_rotation_body,
+                     const double octreeRes,
+                     CostEvaluator::TrajectoryCostsWeights costWeights,
+                     const int maxNumThreads, const VisionDWAConfig config)
+    : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
+          config.prediction_horizon(), config.control_horizon(),
+          maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
+          sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
+          maxNumThreads) {
+  _ctrlType = robotCtrlType;
+  _ctrl_limits = ctrlLimits;
+  _config = config;
+  // Initialize time steps
+  int num_steps = _config.control_horizon();
+  // Initialize control vectors
+  _out_vel = Velocities(num_steps);
+  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
+}
+
+Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0);
+  float gamma =
+      Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
+  float psi = Angle::normalizeToMinusPiPlusPi(
+      std::atan2(tracking_pose.y() - currentState.y,
+                 tracking_pose.x() - currentState.x) -
+      currentState.yaw);
+
+  float distance_error = _config.target_distance() - distance;
+  float angle_error =
+      Angle::normalizeToMinusPiPlusPi(_config.target_orientation() - psi);
+
+  float distance_tolerance = _config.tolerance() * _config.target_distance();
+  float angle_tolerance = std::max(0.001, _config.tolerance() * _config.target_orientation());
+
+  // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
+  //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
+
+  Velocity2D followingVel;
+  if (abs(distance_error) > distance_tolerance or
+      abs(angle_error) > angle_tolerance) {
+    float v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    followingVel.setVx(v);
+    float omega = -tracking_pose.omega() +
+                      2 * (v * sin(psi) / distance +
+                           tracking_pose.v() * sin(gamma - psi) / distance -
+                           _config.K_omega() * tanh(angle_error));
+    followingVel.setOmega(omega);
+  }
+  return followingVel;
+}
+
+template <typename T>
+std::tuple<Trajectory2D, bool>
+VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
+                                       const T &sensor_points) {
+  int step = 0;
+
+  Trajectory2D ref_traj(_config.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
+  while (step < _config.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    simulated_state.update(cmd, _config.control_time_step());
+    simulated_track.update(_config.control_time_step());
+    if(step < _config.prediction_horizon() -1){
+      ref_traj.velocities.add(step,cmd);
+    }
+
+    step++;
+  }
+  this->setCurrentState(original_state);
+
+  bool has_collisions = trajSampler->checkStatesFeasibility(states, sensor_points);
+
+  LOG_INFO("Found reference traj with collisions: ", has_collisions);
+
+  return std::make_tuple(ref_traj, has_collisions);
+}
+
+template <typename T>
+TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                          const Velocity2D &current_vel,
+                                          const T &sensor_points) {
+  Trajectory2D ref_traj;
+  bool has_collisions;
+  std::tie(ref_traj, has_collisions) =
+      this->getTrackingReferenceSegment(tracking_pose, sensor_points);
+  if(!has_collisions){
+    // The tracking sample is collision free -> No need to explore other samples
+    TrajSearchResult result;
+    result.isTrajFound = true;
+    result.trajCost = 0.0;
+    result.trajectory = ref_traj;
+    latest_velocity_command_ = ref_traj.velocities.getFront();
+    return result;
+  }
+  else{
+    LOG_INFO("USING DWA SAMPLING");
+    // The tracking sample has collisions -> use DWA-like sampling and control
+    Path::Path ref_tracking_path;
+    for (int idx=0; idx < _config.prediction_horizon(); ++idx){
+      ref_tracking_path.points.push_back(ref_traj.path.getIndex(idx));
+    }
+    // Set the tracking segment as the reference path
+    this->setCurrentPath(ref_tracking_path);
+    return this->computeVelocityCommandsSet(current_vel, sensor_points);
+  }
+}
+
+// Explicit instantiation for LaserScan
+template Control::TrajSearchResult VisionDWA::getTrackingCtrl<LaserScan>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const LaserScan &sensor_points);
+
+// Explicit instantiation for PointCloud
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
+    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
+    const std::vector<Path::Point> &sensor_points);
+
+} // namespace Control
+} // namespace Kompass

From 5b2a3412c7be13050c4ff0d67a669573da65e788 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 4 Apr 2025 17:01:05 +0200
Subject: [PATCH 028/118] (feature) Adds VisionDWA test

---
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 src/kompass_cpp/tests/json_export.h           |  21 +-
 .../tests/trajectory_sampler_plt.py           |  23 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 210 ++++++++++++++++++
 4 files changed, 247 insertions(+), 12 deletions(-)
 create mode 100644 src/kompass_cpp/tests/vision_dwa_test.cpp

diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index 5d0e0362..57d42fc3 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -34,6 +34,11 @@ add_executable(collisions_test collisions_test.cpp)
 target_link_libraries(collisions_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(collisions_tests collisions_test)
 
+# Vision DWA test
+add_executable(vision_dwa_test vision_dwa_test.cpp)
+target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
+add_test(vision_dwa_tests vision_dwa_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/json_export.h b/src/kompass_cpp/tests/json_export.h
index 01a06f7a..d99e2d44 100644
--- a/src/kompass_cpp/tests/json_export.h
+++ b/src/kompass_cpp/tests/json_export.h
@@ -92,9 +92,8 @@ inline void to_json(json &j, const Control::TrajectorySamples2D &samples,
 }
 
 // Save trajectories to a JSON file
-inline void saveTrajectoriesToJson(
-    const Control::TrajectorySamples2D &trajectories,
-    const std::string &filename) {
+void saveTrajectoriesToJson(const Control::TrajectorySamples2D &trajectories,
+                            const std::string &filename) {
   json j;
   to_json(j, trajectories);
   std::ofstream file(filename);
@@ -106,6 +105,22 @@ inline void saveTrajectoriesToJson(
   }
 }
 
+void saveTrajectoryToJson(const Control::Trajectory2D &trajectory,
+                          const std::string &filename) {
+  json j;
+  j["paths"] = json::array(); // Initialize as a JSON array
+  json j_p;
+  to_json(j_p, trajectory.path);
+  j["paths"].push_back(j_p); // Serialize each Point
+  std::ofstream file(filename);
+  if (file.is_open()) {
+    file << j.dump(4); // Pretty print with 4 spaces indentation
+    file.close();
+  } else {
+    std::cerr << "Unable to open file: " << filename << std::endl;
+  }
+}
+
 // Save one path to a JSON file
 inline void savePathToJson(const Path::Path &path, const std::string &filename) {
   json j;
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index a80027a6..3a73acb0 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -29,12 +29,13 @@ def read_path_from_json(filename: str):
         print(f"Read file error: {e}")
 
 
-def plot_samples(trajectories, reference, figure_name):
-    # Plot reference
-    ref_path = reference["points"]
-    x_coords = [point["x"] for point in ref_path]
-    y_coords = [point["y"] for point in ref_path]
-    plt.plot(x_coords, y_coords, "--b")
+def plot_samples(figure_name, trajectories, reference=None):
+    if reference:
+        # Plot reference
+        ref_path = reference["points"]
+        x_coords = [point["x"] for point in ref_path]
+        y_coords = [point["y"] for point in ref_path]
+        plt.plot(x_coords, y_coords, "--b", linewidth=7.0, label="reference")
 
     for traj in trajectories:
         path = traj["points"]
@@ -63,7 +64,7 @@ def main():
     parser.add_argument(
         "--reference",
         type=str,
-        required=True,
+        required=False,
         help="Reference path JSON file name in the current directory",
     )
 
@@ -75,9 +76,13 @@ def main():
     reference_file = args.reference
 
     trajectories = read_trajectories_from_json(f"{samples_file}.json")
-    reference = read_path_from_json(f"{reference_file}.json")
+
+    reference = (
+        read_path_from_json(f"{reference_file}.json") if reference_file else None
+    )
+
     # print(trajectories)
-    plot_samples(trajectories, reference, samples_file)
+    plot_samples(samples_file, trajectories, reference)
 
 
 if __name__ == "__main__":
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
new file mode 100644
index 00000000..ce2771fd
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -0,0 +1,210 @@
+#include "controllers/vision_dwa.h"
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/trajectory.h"
+#include "test.h"
+#include "utils/cost_evaluator.h"
+#include "utils/logger.h"
+#include <memory>
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include "json_export.cpp"
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <cmath>
+#include <vector>
+
+using namespace Kompass;
+
+struct VisionDWATestConfig {
+  // Sampling configuration
+  double timeStep;
+  double predictionHorizon;
+  double controlHorizon;
+  int maxLinearSamples;
+  int maxAngularSamples;
+  int maxNumThreads;
+
+  // Octomap resolution
+  double octreeRes;
+
+  // Cost weights
+  Control::CostEvaluator::TrajectoryCostsWeights costWeights;
+
+  // Robot configuration
+  Control::LinearVelocityControlParams x_params;
+  Control::LinearVelocityControlParams y_params;
+  Control::AngularVelocityControlParams angular_params;
+  Control::ControlLimitsParams controlLimits;
+  Control::ControlType controlType;
+  Kompass::CollisionChecker::ShapeType robotShapeType;
+  std::vector<float> robotDimensions;
+  const std::array<float, 3> sensorPositionWRTbody;
+  const std::array<float, 4> sensorRotationWRTbody;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud;
+
+  // Robot start state (pose)
+  Path::State robotState;
+
+  // Tracked target with respect to the robot
+  Control::Velocity2D tracked_vel;
+  Control::TrackedPose2D tracked_pose;
+  // VisionDWA configuration object
+  std::unique_ptr<Control::VisionDWA> controller;
+
+  // Constructor to initialize the struct
+  VisionDWATestConfig(const float timeStep, const float predictionHorizon,
+                      const float controlHorizon, const int maxLinearSamples,
+                      const int maxAngularSamples,
+                      const std::vector<Path::Point> sensor_points,
+                      const float maxVel = 1.0, const float maxOmega = 2.0,
+                      const int maxNumThreads = 1,
+                      const double reference_path_distance_weight = 1.0,
+                      const double goal_distance_weight = 0.5,
+                      const double obstacles_distance_weight = 0.0)
+      : timeStep(timeStep), predictionHorizon(predictionHorizon),
+        controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
+        maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
+        octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5),
+        angular_params(3.14, maxOmega, 3.0, 3.0),
+        controlLimits(x_params, y_params, angular_params),
+        controlType(Control::ControlType::ACKERMANN),
+        robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER),
+        robotDimensions{0.1, 0.4}, sensorPositionWRTbody{0.0, 0.0, 0.0},
+        sensorRotationWRTbody{0, 0, 0, 1}, cloud(sensor_points),
+        robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3),
+        tracked_pose(0.0, 0.0, 0.0, tracked_vel) {
+    // Initialize cost weights
+    costWeights.setParameter("reference_path_distance_weight",
+                             reference_path_distance_weight);
+    costWeights.setParameter("goal_distance_weight", goal_distance_weight);
+    costWeights.setParameter("obstacles_distance_weight",
+                             obstacles_distance_weight);
+    costWeights.setParameter("smoothness_weight", 0.0);
+    costWeights.setParameter("jerk_weight", 0.0);
+    controller = std::make_unique<Control::VisionDWA>(
+        controlType, controlLimits, maxLinearSamples, maxAngularSamples,
+        robotShapeType, robotDimensions, sensorPositionWRTbody,
+        sensorRotationWRTbody, octreeRes, costWeights);
+  }
+
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName) {
+    Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
+    Control::TrajectoryVelocities2D simulated_velocities(
+        numPointsPerTrajectory);
+    Control::TrajectoryPath robot_path(numPointsPerTrajectory),
+        tracked_path(numPointsPerTrajectory);
+    Control::Velocity2D cmd;
+
+    int step = 0;
+    while (step < numPointsPerTrajectory) {
+      Path::Point point(robotState.x, robotState.y, 0.0);
+      robot_path.add(step, point);
+      tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
+      controller->setCurrentState(robotState);
+
+      Control::TrajSearchResult result =
+          controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+      // cmd = controller.getPureTrackingCtrl(tracked_pose);
+      // robotState.update(cmd, timeStep);
+
+      if (result.isTrajFound) {
+        LOG_INFO(FMAG("STEP: "), step,
+                 FMAG(", Found best trajectory with cost: "), result.trajCost);
+
+        cmd.setVx(controller->getLinearVelocityCmdX());
+        cmd.setVy(controller->getLinearVelocityCmdY());
+        cmd.setOmega(controller->getAngularVelocityCmd());
+
+        LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
+                  BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
+                  BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
+                  BOLD(FBLU("}")));
+
+        LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, cmd.vx(), RST,
+                  BOLD(FGRN(", Vy: ")), KGRN, cmd.vy(), RST,
+                  BOLD(FGRN(", Omega: ")), KGRN, cmd.omega(), RST,
+                  BOLD(FGRN("}")));
+
+        robotState.update(cmd, timeStep);
+      } else {
+        LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED,
+                  robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
+                  RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST,
+                  BOLD(FRED("}")));
+        return false;
+      }
+
+      tracked_pose.update(timeStep);
+      step++;
+    }
+    samples.push_back(simulated_velocities, robot_path);
+    samples.push_back(simulated_velocities, tracked_path);
+
+
+    // Plot the trajectories (Save to json then run python script for plotting)
+    boost::filesystem::path executablePath = boost::dll::program_location();
+    std::string file_location = executablePath.parent_path().string();
+
+    std::string trajectories_filename = file_location + "/" + pltFileName;
+
+    saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+    // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+    std::string command = "python3 " + file_location +
+                          "/trajectory_sampler_plt --samples \"" +
+                          trajectories_filename + "\"";
+
+    // Execute the Python script
+    int res = system(command.c_str());
+    return true;
+  }
+};
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon =0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(numPointsPerTrajectory, std::string("vision_follower_obstacle_free"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  double predictionHorizon = 0.5;
+  double controlHorizon = 0.2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"));
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}

From dc383a6b39841395bb23782110f48045ab13c603 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 10 Apr 2025 10:54:29 +0200
Subject: [PATCH 029/118] (fix) Fixes include and adds docstrings

---
 .../kompass_cpp/include/controllers/dwa.h     | 20 +++++++++++++++++++
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 11 ++++++----
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 66748e98..7a6da55a 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -68,10 +68,27 @@ class DWA : public Follower {
                    CostEvaluator::TrajectoryCostsWeights costWeights,
                    const int maxNumThreads = 1);
 
+  /**
+   * @brief Reset the resolution of the robot's local map, this is equivalent to the box size representing each obstacle after conversion
+   *
+   * @param octreeRes
+   */
   void resetOctreeResolution(const double octreeRes);
 
+  /**
+   * @brief Set the robot's sensor or local map max range (meters)
+   *
+   * @param max_range
+   */
   void setSensorMaxRange(const float max_range);
 
+  /**
+   * @brief Set the Current State of the robot
+   *
+   * @param position
+   */
+  void setCurrentState(const Path::State &position);
+
   /**
    * @brief Adds a new custom cost to be used in the trajectory evaluation
    *
@@ -144,6 +161,9 @@ class DWA : public Follower {
   double max_forward_distance_ = 0.0;
   int maxNumThreads;
   TrajectorySamples2D *debuggingSamples_ = nullptr;
+  float maxLocalRange_ =
+      10.0; // Max range of the robot sensor or local map in meters. Used to
+            // calculate the cost of coming close to obstacles
 
   /**
    * @brief get maximum reference path length
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index ce2771fd..9085e9ad 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -7,7 +7,7 @@
 #include "utils/logger.h"
 #include <memory>
 #define BOOST_TEST_MODULE KOMPASS TESTS
-#include "json_export.cpp"
+#include "json_export.h"
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
 #include <boost/filesystem.hpp>
 #include <boost/test/included/unit_test.hpp>
@@ -62,8 +62,8 @@ struct VisionDWATestConfig {
                       const float maxVel = 1.0, const float maxOmega = 2.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 1.0,
-                      const double goal_distance_weight = 0.5,
-                      const double obstacles_distance_weight = 0.0)
+                      const double goal_distance_weight = 1.0,
+                      const double obstacles_distance_weight = 1.0)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
@@ -154,11 +154,14 @@ struct VisionDWATestConfig {
     // savePathToJson(reference_segment, ref_path_filename + ".json");
 
     std::string command = "python3 " + file_location +
-                          "/trajectory_sampler_plt --samples \"" +
+                          "/trajectory_sampler_plt.py --samples \"" +
                           trajectories_filename + "\"";
 
     // Execute the Python script
     int res = system(command.c_str());
+    if (res != 0)
+      throw std::system_error(res, std::generic_category(),
+                              "Python script failed with error code");
     return true;
   }
 };

From 5b74fcb12ecb64cc7aaa517b324f83bfe450db1f Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:15:24 +0200
Subject: [PATCH 030/118] (feature) Adds kalman filter for linear state
 estimation

---
 .../kompass_cpp/include/utils/kalman_filter.h |  47 ++++++++
 .../kompass_cpp/src/utils/kalman_filter.cpp   | 110 ++++++++++++++++++
 2 files changed, 157 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
new file mode 100644
index 00000000..646e2f2d
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <optional>
+
+using Eigen::MatrixXf;
+using namespace std;
+
+namespace Kompass {
+
+class LinearSSKalmanFilter {
+private:
+  bool state_initialized = false, system_initialized = false;
+  Eigen::MatrixXf state;
+  Eigen::MatrixXf A; // state matrix
+  Eigen::MatrixXf B; // input matrix
+  Eigen::MatrixXf H; // observation matrix
+  Eigen::MatrixXf P; // uncertainty
+  Eigen::MatrixXf Q; // process noise
+  Eigen::MatrixXf R; // observation noise
+
+public:
+  // constructor
+  LinearSSKalmanFilter(const size_t num_states, const size_t num_inputs);
+
+  // set up the filter
+  bool setup(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B,
+             const Eigen::MatrixXf &Q, const Eigen::MatrixXf &H,
+             const Eigen::MatrixXf &R);
+
+  void setInitialState(const Eigen::VectorXf &initial_state);
+
+  // set A (sometimes sampling time will differ)
+  void setA(const Eigen::MatrixXf &A);
+
+  // state estimate
+  void estimate(const Eigen::MatrixXf &z, const Eigen::MatrixXf &u);
+
+  void estimate(const Eigen::MatrixXf &z);
+
+  // read output from the state
+  double getState(const size_t state_index);
+
+  std::optional<Eigen::MatrixXf> getState();
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
new file mode 100644
index 00000000..68b9e489
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
@@ -0,0 +1,110 @@
+#define EIGEN_DONT_PARALLELIZE  // TODO: resolve error when enabling Eigen parallelization
+#include <Eigen/Dense>
+#include <optional>
+#include "utils/kalman_filter.h"
+#include "utils/logger.h"
+
+
+namespace Kompass {
+
+LinearSSKalmanFilter::LinearSSKalmanFilter(const size_t num_states,
+                                           const size_t num_inputs) {
+  state.resize(num_states, 1);
+  // State update eq: X_dot = A X + B U + Q
+  A.resize(num_states, num_states);
+  B.resize(num_states, num_inputs);
+  Q.resize(num_states, num_states);
+
+  // Observation eq: Z = H X + R
+  H.resize(num_states, num_states);
+  R.resize(num_states, num_states);
+
+  P = Eigen::MatrixXf::Identity(num_states, num_states);
+}
+
+bool LinearSSKalmanFilter::setup(const Eigen::MatrixXf &A,
+                                 const Eigen::MatrixXf &B,
+                                 const Eigen::MatrixXf &Q,
+                                 const Eigen::MatrixXf &H,
+                                 const Eigen::MatrixXf &R) {
+
+  if ((A.size() != this->A.size()) || (B.size() != this->B.size()) ||
+      (Q.size() != this->Q.size()) || (H.size() != this->H.size()) ||
+      (R.size() != this->R.size())) {
+    LOG_ERROR("Cannot setup the KalmanFilter. Matrix size error. Expected the "
+              "following sized: A=",
+              this->A.size(), ", B=", this->B.size(), ", H=", this->H.size(),
+              ", Q=", this->Q.size(), ", R=", this->R.size());
+    return false;
+  }
+  this->A = A;
+  this->B = B;
+  this->H = H;
+  this->R = R;
+  this->Q = Q;
+  this->system_initialized = true;
+  return true;
+}
+
+void LinearSSKalmanFilter::setInitialState(const Eigen::VectorXf& initial_state) {
+  if (initial_state.size() != this->state.size()){
+     LOG_ERROR("Cannot set initial state. Expected the "
+              "following sized: ", this->state.size());
+     throw std::length_error("Error Setting Initial State");
+  }
+  this->state = initial_state;
+  this->state_initialized = true;
+}
+
+void LinearSSKalmanFilter::setA(const Eigen::MatrixXf &A) { this->A = A; }
+
+void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements,
+                                    const Eigen::MatrixXf &inputs) {
+  // predict a new state
+  Eigen::MatrixXf predicted_state = this->A * this->state + this->B * inputs;
+
+  // Covariance Extrapolation Equation
+  this->P = this->A * this->P * this->A.transpose() + this->Q;
+
+  // Innovation Matrix
+  Eigen::MatrixXf S =
+      this->R + this->H * this->P * this->H.transpose();
+
+  // Kalman Gain Matrix
+  Eigen::MatrixXf K =
+      this->P * this->H.transpose() * S.inverse();
+
+  // Update the state
+  this->state =
+      predicted_state + K * (measurements - this->H * predicted_state);
+
+  // Update the estimation uncertainty
+  this->P = (Eigen::MatrixXf::Identity(this->P.rows(), this->P.cols()) -
+             K * this->H) *
+            this->P;
+}
+
+void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements) {
+  // estimate with zero inputs
+  auto size = this->B.cols();
+  Eigen::MatrixXf inputs;
+  inputs.resize(size, 1);
+  inputs = Eigen::MatrixXf::Zero(size, 1);
+  this->estimate(measurements, inputs);
+}
+
+double LinearSSKalmanFilter::getState(const size_t state_index) {
+  if (this->state_initialized) {
+    return this->state(state_index, 0);
+  } else {
+    return 0;
+  }
+}
+
+std::optional<Eigen::MatrixXf> LinearSSKalmanFilter::getState() {
+  if (this->state_initialized and this->system_initialized) {
+    return this->state;
+  }
+  return std::nullopt;
+}
+}

From 534557cb06cc99dcd0bb3fc2e143bde744c62b97 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:17:30 +0200
Subject: [PATCH 031/118] (feature) Adds 3D detected box vision tracker

---
 .../include/controllers/vision_dwa.h          |   3 -
 .../kompass_cpp/include/datatypes/tracking.h  |  81 ++++++++
 .../kompass_cpp/include/vision/tracker.h      |  47 +++++
 .../kompass_cpp/src/vision/tracker.cpp        | 186 ++++++++++++++++++
 src/kompass_cpp/tests/CMakeLists.txt          |   5 +
 .../tests/vision_tracking_test.cpp            | 151 ++++++++++++++
 6 files changed, 470 insertions(+), 3 deletions(-)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
 create mode 100644 src/kompass_cpp/kompass_cpp/include/vision/tracker.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
 create mode 100644 src/kompass_cpp/tests/vision_tracking_test.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 3b5ef86e..8237b8c8 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -126,9 +126,6 @@ class VisionDWA : public DWA {
 
   bool _rotate_in_place;
   Velocities _out_vel;
-  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
-  std::queue<std::array<double, 3>> _search_commands_queue;
-  std::array<double, 3> _search_command;
   std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
new file mode 100644
index 00000000..63a7cd2d
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -0,0 +1,81 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <vector>
+
+namespace Kompass {
+
+struct Bbox3D {
+  Eigen::Vector3f center = {0.0, 0.0, 0.0};
+  Eigen::Vector3f size = {0.0, 0.0, 0.0};
+  Eigen::Vector2i center_img_frame = {0, 0};
+  Eigen::Vector2i size_img_frame = {0, 0};
+  std::vector<Eigen::Vector3f> pc_points = {};
+
+  Bbox3D() {};
+
+  Bbox3D(const Bbox3D &box)
+      : center(box.center), size(box.size), center_img_frame(box.center_img_frame), size_img_frame(box.size_img_frame), pc_points(box.pc_points){};
+
+  Eigen::Vector2f getXLimitsImg() const {
+    return {
+      center_img_frame.x() - (size_img_frame.x() / 2),
+          center_img_frame.x() + (size_img_frame.x() / 2)
+    };
+  };
+
+  Eigen::Vector2f getYLimitsImg() const {
+    return {center_img_frame.y() - (size_img_frame.y() / 2),
+            center_img_frame.y() + (size_img_frame.y() / 2)};
+  };
+};
+
+struct TrackedBbox3D {
+  Bbox3D box;
+  Eigen::Vector3f vel = {0.0, 0.0, 0.0};
+  Eigen::Vector3f acc = {0.0, 0.0, 0.0};
+  int unique_id = 0;
+
+  TrackedBbox3D(const Bbox3D& box): box(box) {};
+
+  void setState(const Eigen::Matrix<float, 6, 1>& state_vector){
+    this->box.center = {state_vector(0), state_vector(1), 0.0f};
+    this->vel = {state_vector(2), state_vector(3), 0.0f};
+    this->acc = {state_vector(4), state_vector(5), 0.0f};
+  };
+
+  void setSize(const Eigen::Vector3f& size) {
+    this->box.size = size;
+  };
+
+  void setfromBox(const Bbox3D& box){
+    this->box = box;
+  };
+
+  void updateFromNewDetection(const Bbox3D& new_box, const float time_step){
+    // Compute velocity and acceleration based on location change
+    Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
+    Eigen::Vector3f new_acc = (new_vel - this->vel) / time_step;
+    // Update
+    setfromBox(new_box);
+    this->vel = new_vel;
+    this->acc = new_acc;
+  }
+
+  TrackedBbox3D predictConstantVel(const float &dt) {
+    auto predicted_tracking = *this;
+    predicted_tracking.box.center += predicted_tracking.vel * dt;
+    // Set acceleration to zero for constant velocity prediction
+    predicted_tracking.acc = {0.0, 0.0, 0.0};
+    return predicted_tracking;
+  };
+
+  TrackedBbox3D predictConstantAcc(const float &dt) {
+    auto predicted_tracking = *this;
+    predicted_tracking.vel += this->acc * dt;
+    predicted_tracking.box.center += predicted_tracking.vel * dt;
+    return predicted_tracking;
+  };
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
new file mode 100644
index 00000000..66aa7b6b
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <memory>
+#include <optional>
+#include <vector>
+#include "datatypes/tracking.h"
+#include "utils/kalman_filter.h"
+
+namespace  Kompass{
+
+
+class FeatureBasedBboxTracker{
+    public:
+      FeatureBasedBboxTracker(const float& time_step, const float& e_pos, const float& e_vel, const float& e_acc);
+
+      using FeaturesVector = Eigen::Vector<float, 9>;
+
+      bool setInitialTracking(const TrackedBbox3D& bBox);
+
+      bool setInitialTracking(const Bbox3D &bBox);
+
+      bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+                              const std::vector<Bbox3D> &detected_boxes);
+
+      bool updateTracking(const std::vector<Bbox3D> &detected_boxes);
+
+      std::optional<TrackedBbox3D> getRawTracking() const;
+
+      std::optional<Eigen::MatrixXf> getTrackedState() const;
+
+    private:
+      float timeStep_, minAcceptedSimilarityScore_ = 0.0;
+      std::unique_ptr<TrackedBbox3D> trackedBox_;
+      std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
+      bool tracking_started_ = false;
+
+      FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const;
+
+      FeaturesVector extractFeatures(const Bbox3D &bBox) const;
+
+      Eigen::Vector3f computePointsStdDev(const std::vector<Eigen::Vector3f> &pc_points) const;
+
+      void updateTrackedBoxState();
+};
+
+}
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
new file mode 100644
index 00000000..5dcf18b0
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -0,0 +1,186 @@
+#include "vision/tracker.h"
+#include "datatypes/tracking.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <math.h>
+
+namespace Kompass {
+
+FeatureBasedBboxTracker::FeatureBasedBboxTracker(
+    const float& time_step, const float& e_pos,
+    const float& e_vel, const float& e_acc) {
+
+  timeStep_ = time_step;
+  // Setup Kalman filter matrices
+  Eigen::MatrixXf A;
+  A.resize(6, 6);
+
+  A << 1, 0, time_step, 0, 0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0,
+      0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0, 0, 0, 0, 1, 0,
+      time_step, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+
+  Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 1);
+  Eigen::MatrixXf H = Eigen::MatrixXf::Identity(6, 6);
+  Eigen::MatrixXf Err = Eigen::MatrixXf::Identity(6, 6);
+  Err(0, 0) *= e_pos;
+  Err(1, 1) *= e_pos;
+  Err(2, 2) *= e_vel;
+  Err(3, 3) *= e_vel;
+  Err(4, 4) *= e_acc;
+  Err(5, 5) *= e_acc;
+
+  stateKalmanFilter_ = std::make_unique<LinearSSKalmanFilter>(6, 1);
+  stateKalmanFilter_->setup(A, B, Err, H, Err);
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
+  trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  Eigen::VectorXf state_vec;
+  state_vec.resize(6);
+  state_vec(0) = bBox.box.center[0];
+  state_vec(1) = bBox.box.center[1];
+  state_vec(2) = bBox.vel[0];
+  state_vec(3) = bBox.vel[1];
+  state_vec(4) = bBox.acc[0];
+  state_vec(5) = bBox.acc[1];
+  stateKalmanFilter_->setInitialState(state_vec);
+  this->tracking_started_ = true;
+  return true;
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
+  trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  Eigen::Matrix<float, 6, 1> state_vec = Eigen::Matrix<float, 6, 1>::Zero();
+  state_vec(0) = bBox.center[0];
+  state_vec(1) = bBox.center[1];
+  stateKalmanFilter_->setInitialState(state_vec);
+  this->tracking_started_ = false;
+  return true;
+}
+
+bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const int& pose_y_img, const std::vector<Bbox3D>& detected_boxes){
+    std::unique_ptr<Bbox3D> target_box;
+    // Find a detected box containing the point
+    for(auto box: detected_boxes){
+      auto limits_x = box.getXLimitsImg();
+      if(pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)){
+        auto limits_y = box.getYLimitsImg();
+        if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) {
+          target_box = std::make_unique<Bbox3D>(box);
+          break;
+        }
+      }
+    }
+    if(!target_box){
+      // given position was not found inside any detected box
+      return false;
+    }
+    trackedBox_ = std::make_unique<TrackedBbox3D>(*target_box);
+    Eigen::Vector<float, 6> state_vec = Eigen::Vector<float, 6>::Zero();
+    state_vec(0) = target_box->center[0];
+    state_vec(1) = target_box->center[1];
+    stateKalmanFilter_->setInitialState(state_vec);
+    // Target velocity is still unknown
+    this->tracking_started_ = false;
+    return true;
+}
+
+void FeatureBasedBboxTracker::updateTrackedBoxState(){
+  Eigen::MatrixXf measurement;
+  measurement.resize(6, 1);
+  measurement(0) = trackedBox_->box.center.x();
+  measurement(1) = trackedBox_->box.center.y();
+  measurement(2) = trackedBox_->vel.x();
+  measurement(3) = trackedBox_->vel.y();
+  measurement(4) = trackedBox_->acc.x();
+  measurement(5) = trackedBox_->acc.y();
+  stateKalmanFilter_->estimate(measurement);
+}
+
+bool FeatureBasedBboxTracker::updateTracking(const std::vector<Bbox3D> &detected_boxes){
+
+    // Predicted the new location of the tracked box
+    auto predicted_tracked_box = trackedBox_->predictConstantAcc(timeStep_);
+
+    auto ref_box_features = extractFeatures(predicted_tracked_box);
+
+    // Get the features of all the new detections
+    FeaturesVector detected_boxes_feature_vec;
+    float max_similarity_score = 0.0;  // Similarity score
+    size_t similar_box_idx = 0, count = 0;
+    for(auto box: detected_boxes){
+      detected_boxes_feature_vec = extractFeatures(box);
+      auto error_vec = detected_boxes_feature_vec - ref_box_features;
+      float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
+
+      if (similarity_score > max_similarity_score){
+        max_similarity_score = similarity_score;
+        similar_box_idx = count;
+      }
+      count++;
+    }
+    if (max_similarity_score > minAcceptedSimilarityScore_){
+      // Update raw tracking
+      trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx], timeStep_);
+      // Update state by applying kalman filter on the raw measurement
+      updateTrackedBoxState();
+      return true;
+    }
+    return false;
+}
+
+FeatureBasedBboxTracker::FeaturesVector
+FeatureBasedBboxTracker::extractFeatures(
+    const TrackedBbox3D &bBox_tracked) const {
+  return extractFeatures(bBox_tracked.box);
+}
+
+FeatureBasedBboxTracker::FeaturesVector
+FeatureBasedBboxTracker::extractFeatures(const Bbox3D &bBox) const {
+  FeatureBasedBboxTracker::FeaturesVector features_vec = Eigen::Vector<float, 9>::Zero();
+  features_vec.segment<2>(0) = bBox.center.segment<2>(0); // indices 0, 1
+  features_vec.segment<3>(2) = bBox.size; // indices 2, 3, 4
+  features_vec(5) = bBox.pc_points.size();
+  if (features_vec(5) > 0.0) {
+    // Compute point cloud points standard deviation
+    auto std_vec = this->computePointsStdDev(bBox.pc_points);
+    features_vec.segment<3>(6) = std_vec; // indices 6, 7, 8
+  }
+  return features_vec;
+}
+
+std::optional<TrackedBbox3D> FeatureBasedBboxTracker::getRawTracking() const {
+  if (trackedBox_) {
+     return *trackedBox_;
+  }
+  return std::nullopt;
+}
+
+std::optional<Eigen::MatrixXf>
+FeatureBasedBboxTracker::getTrackedState() const {
+  if (trackedBox_) {
+    return stateKalmanFilter_->getState();
+  }
+  return std::nullopt;
+}
+
+Eigen::Vector3f FeatureBasedBboxTracker::computePointsStdDev(
+    const std::vector<Eigen::Vector3f> &pc_points) const {
+  // compute the mean in each direction
+  auto size = std::max(int(pc_points.size() - 1), 1);
+  Eigen::Vector3f mean = Eigen::Vector3f::Zero();
+  for (auto point: pc_points){
+    mean += point;
+  }
+  mean /= size;
+  // Compute the variance
+  Eigen::Vector3f variance = Eigen::Vector3f::Zero();
+  for (auto point : pc_points) {
+    auto diff = point - mean;
+    variance += diff.cwiseProduct(diff);
+  }
+  variance /= size;
+  // return the standard deviation
+  return variance.array().sqrt();
+}
+
+} // namespace Kompass
diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index 57d42fc3..64c50817 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -39,6 +39,11 @@ add_executable(vision_dwa_test vision_dwa_test.cpp)
 target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
 add_test(vision_dwa_tests vision_dwa_test)
 
+# Set path to the test image
+add_executable(vision_tracking_test vision_tracking_test.cpp)
+target_link_libraries(vision_tracking_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework nlohmann_json::nlohmann_json)
+add_test(vision_tracking_tests vision_tracking_test)
+
 # find sycl
 find_package(AdaptiveCpp)
 
diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp
new file mode 100644
index 00000000..412655a0
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_tracking_test.cpp
@@ -0,0 +1,151 @@
+#include "datatypes/control.h"
+#include "datatypes/path.h"
+#include "datatypes/tracking.h"
+#include "datatypes/trajectory.h"
+#include "json_export.h"
+#include "test.h"
+#include "utils/logger.h"
+#include "vision/tracker.h"
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <opencv4/opencv2/imgcodecs.hpp>
+#include <random>
+
+using namespace Kompass;
+
+struct VisionTrackingTestConfig {
+  float time_step;
+  int maxSteps;
+  Eigen::Vector3f target_ref_vel;
+  std::unique_ptr<FeatureBasedBboxTracker> tracker;
+  Control::TrajectoryPath reference_path, measured_path, tracked_path;
+  Path::State reference_state;
+  std::vector<Bbox3D> detected_boxes;
+  std::string pltFileName = "VisionTrackerTest";
+  std::mt19937 gen;
+  std::normal_distribution<float> noise;
+
+  VisionTrackingTestConfig(const int &maxSteps = 50,
+                           const float &time_step = 0.1,
+                           const float &e_pos = 0.1, const float &e_vel = 0.1,
+                           const float &e_acc = 0.1,
+                           const Eigen::Vector3f target_pose = {0.0f, 0.0f, 0.0f},
+                           const Eigen::Vector3f target_box_size = {0.5f, 0.5f,
+                                                                    1.0f},
+                           const int num_test_boxes = 3, const float target_v = 0.2, const float target_omega = 0.3)
+      : time_step(time_step), maxSteps(maxSteps) {
+    tracker = std::make_unique<FeatureBasedBboxTracker>(time_step, e_pos, e_vel,
+                                                        e_acc);
+    Bbox3D tracked_box;
+    tracked_box.center = target_pose;
+    tracked_box.size = target_box_size;
+    tracker->setInitialTracking(tracked_box);
+    reference_path = Control::TrajectoryPath(maxSteps);
+    tracked_path = Control::TrajectoryPath(maxSteps);
+    measured_path = Control::TrajectoryPath(maxSteps);
+    reference_state = Path::State(target_pose.x(), target_pose.y());
+    detected_boxes.resize(num_test_boxes);
+    detected_boxes[0] = tracked_box;
+    Bbox3D new_box;
+    new_box.size = target_box_size;
+    for(int i=1; i < num_test_boxes; ++i){
+      auto new_box_shift = Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f});
+      new_box.center = new_box_shift + target_pose;
+      detected_boxes[i] = new_box;
+    }
+
+    target_ref_vel = {target_v * cos(target_omega), target_v * sin(target_omega), 0.0};
+
+    // Random noise generator
+    std::random_device rd; // Obtain a random number from hardware
+    gen = std::mt19937(rd());
+    // Define the normal distribution with mean and standard deviation
+    noise = std::normal_distribution<float>(0.0f, 0.01f);
+  };
+
+  void moveDetectedBoxes(const int step){
+    // Update the detected boxes using the velocity command with additional measurement noise
+    for(auto &box: detected_boxes){
+      Eigen::Vector3f noise_vector(noise(gen), noise(gen), noise(gen));
+      box.center += target_ref_vel * time_step + noise_vector;
+    }
+    // Update the reference state using same velocity command (real)
+    reference_state.x += target_ref_vel.x() * time_step;
+    reference_state.y += target_ref_vel.y() * time_step;
+    Path::Point point(reference_state.x, reference_state.y, 0.0);
+    reference_path.add(
+        step, Path::Point(reference_state.x, reference_state.y, 0.0));
+  };
+};
+
+
+BOOST_AUTO_TEST_CASE(test_Vision_Tracker) {
+  // Create timer
+  Timer time;
+  VisionTrackingTestConfig config = VisionTrackingTestConfig();
+  Control::TrajectorySamples2D samples(3, config.maxSteps);
+  Control::TrajectoryVelocities2D simulated_velocities(config.maxSteps);
+
+  int step = 0;
+  float tracking_error = 0.0;
+  while (step < config.maxSteps) {
+    // Move the detected boxes one step using the reference velocity
+    config.moveDetectedBoxes(step);
+    // Sed detected boxes and ask tracker to update
+    config.tracker->updateTracking(config.detected_boxes);
+    auto measured_track = config.tracker->getRawTracking();
+    auto tracked_state = config.tracker->getTrackedState();
+    if(tracked_state){
+      auto mat = tracked_state->col(0);
+      LOG_INFO("Real target location at", config.reference_state.x, ", ", config.reference_state.y);
+      LOG_INFO("Found tracked target at", mat(0), ", ", mat(1));
+      tracking_error +=
+          Path::Path::distance(mat.segment(0, 3), Path::Point(config.reference_state.x,
+                                                   config.reference_state.y, 0.0));
+      config.tracked_path.add(step, mat.segment(0, 3));
+      config.measured_path.add(step, Path::Point(measured_track->box.center.x(),
+                                                 measured_track->box.center.y(),
+                                                 0.0));
+      if (step < config.maxSteps - 1)
+        simulated_velocities.add(step, Control::Velocity2D());
+    }
+    else{
+        LOG_ERROR("Lost tracked target at step: ", step);
+        break;
+    }
+    step++;
+  }
+
+  // End Tracking error
+  tracking_error /= config.maxSteps;
+  LOG_INFO("Average Error Used Noisy Measurements = ", tracking_error);
+  BOOST_TEST(tracking_error <= 0.1 , "Tracking error is larger than 10%");
+
+  samples.push_back(simulated_velocities, config.reference_path);
+  samples.push_back(simulated_velocities, config.measured_path);
+  samples.push_back(simulated_velocities, config.tracked_path);
+
+  // Plot the trajectories (Save to json then run python script for plotting)
+  boost::filesystem::path executablePath = boost::dll::program_location();
+  std::string file_location = executablePath.parent_path().string();
+
+  std::string trajectories_filename = file_location + "/" + config.pltFileName;
+
+  saveTrajectoriesToJson(samples, trajectories_filename + ".json");
+  // savePathToJson(reference_segment, ref_path_filename + ".json");
+
+  std::string command = "python3 " + file_location +
+                        "/trajectory_sampler_plt.py --samples \"" +
+                        trajectories_filename + "\"";
+
+  // Execute the Python script
+  int res = system(command.c_str());
+  if (res != 0)
+    throw std::system_error(res, std::generic_category(),
+                            "Python script failed with error code");
+}
+

From 7c0c18d178cf3936c7dffe65b40df80e7cbc801b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 17 Apr 2025 15:21:16 +0200
Subject: [PATCH 032/118] (fix) removes unused includes

---
 src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp | 2 --
 src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp      | 1 -
 2 files changed, 3 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
index 68b9e489..137106eb 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
@@ -1,6 +1,4 @@
 #define EIGEN_DONT_PARALLELIZE  // TODO: resolve error when enabling Eigen parallelization
-#include <Eigen/Dense>
-#include <optional>
 #include "utils/kalman_filter.h"
 #include "utils/logger.h"
 
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 5dcf18b0..d53b13de 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -1,6 +1,5 @@
 #include "vision/tracker.h"
 #include "datatypes/tracking.h"
-#include <Eigen/src/Core/Matrix.h>
 #include <math.h>
 
 namespace Kompass {

From 2aed98ff2d10d51aed0fc29ca206c3ffd07e459e Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 23 Apr 2025 09:44:06 +0200
Subject: [PATCH 033/118] (fix) Resolves rebasing conflicts

---
 src/kompass_cpp/kompass_cpp/include/controllers/dwa.h         | 3 ++-
 .../kompass_cpp/include/utils/trajectory_sampler.h            | 4 ----
 src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp           | 4 ++--
 src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp      | 2 +-
 src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp  | 4 ++++
 5 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 7a6da55a..2aade719 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -7,6 +7,7 @@
 #include "follower.h"
 #include "utils/cost_evaluator.h"
 #include "utils/trajectory_sampler.h"
+#include <memory>
 #include <vector>
 
 namespace Kompass {
@@ -160,7 +161,7 @@ class DWA : public Follower {
 private:
   double max_forward_distance_ = 0.0;
   int maxNumThreads;
-  TrajectorySamples2D *debuggingSamples_ = nullptr;
+  std::unique_ptr<TrajectorySamples2D> debuggingSamples_ = nullptr;
   float maxLocalRange_ =
       10.0; // Max range of the robot sensor or local map in meters. Used to
             // calculate the cost of coming close to obstacles
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 11fb13f2..0862f92c 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -99,10 +99,6 @@ class TrajectorySampler {
 
   void updateState(const Path::State &current_state);
 
-
-  void updateState(const Path::State &current_state);
-
-
   void setSampleDroppingMode(const bool drop_samples);
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 077c724f..1791d043 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -79,7 +79,7 @@ void DWA::configure(ControlLimitsParams controlLimits,
   trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads));
+      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
       costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
@@ -99,7 +99,7 @@ void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config,
                       const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads));
+      sensor_position_body, sensor_rotation_body, maxNumThreads);
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
       costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index 0c5e4a49..1b084477 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -81,7 +81,7 @@ void Follower::clearCurrentPath() {
   return;
 }
 
-void Follower::setCurrentPath(const Path::Path &path) {
+void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
   currentPath = std::make_unique<Path::Path>(path.points);
   refPath = std::make_unique<Path::Path>(path.points);
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 2c00d848..f5084dcb 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -500,6 +500,10 @@ void TrajectorySampler::UpdateReachableVelocityRange(
       std::max((max_omega_ - min_omega_) / (ang_samples_max_ - 1), 0.001);
 }
 
+void TrajectorySampler::updateState(const Path::State &current_state) {
+  collChecker->updateState(current_state);
+}
+
 template <>
 bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
     const std::vector<Path::State> &states, const LaserScan &scan) {

From b41ecd48bde5be6b83748eee4403c16b50dd6442 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 23 Apr 2025 15:19:29 +0200
Subject: [PATCH 034/118] (refactor) Removes redundant 'points' attribute from
 path data structure

---
 src/kompass_cpp/bindings/bindings_control.cpp |   3 +-
 src/kompass_cpp/bindings/bindings_types.cpp   |   5 +-
 .../include/controllers/follower.h            |   2 +-
 .../kompass_cpp/include/datatypes/path.h      |  76 ++++--
 .../include/datatypes/trajectory.h            |  28 +-
 .../include/utils/cost_evaluator.h            |   6 +-
 .../kompass_cpp/src/controllers/dwa.cpp       |  61 ++---
 .../kompass_cpp/src/controllers/follower.cpp  |  21 +-
 .../src/controllers/vision_dwa.cpp            |   6 +-
 .../kompass_cpp/src/datatypes/path.cpp        | 248 ++++++++++++------
 .../kompass_cpp/src/utils/cost_evaluator.cpp  |  16 +-
 .../src/utils/cost_evaluator_gpu.cpp          |  18 +-
 src/kompass_cpp/tests/controller_test.cpp     |   2 +-
 src/kompass_cpp/tests/cost_evaluator_test.cpp |   4 +-
 src/kompass_cpp/tests/json_export.h           |   7 +-
 .../tests/trajectory_sampler_test.cpp         |   2 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     |   4 +-
 tests/test_controllers.py                     |  25 +-
 18 files changed, 326 insertions(+), 208 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 95f67c17..ec367184 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -88,7 +88,8 @@ void bindings_control(py::module_ &m) {
       .def(py::init<>())
       .def(py::init<Control::Follower::FollowerParameters>())
       .def("set_interpolation_type", &Control::Follower::setInterpolationType)
-      .def("set_current_path", &Control::Follower::setCurrentPath)
+      .def("set_current_path", &Control::Follower::setCurrentPath, py::arg("path"),
+           py::arg("interpolate") = true)
       .def("clear_current_path", &Control::Follower::clearCurrentPath)
       .def("is_goal_reached", &Control::Follower::isGoalReached)
       .def("get_vx_cmd", &Control::Follower::getLinearVelocityCmdX)
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 80eb3dbf..554dc8a2 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -49,7 +49,10 @@ py::class_<Path::Path>(m_types, "Path")
          py::arg("points") = std::vector<Path::Point>())
     .def("reached_end", &Path::Path::endReached)
     .def("get_total_length", &Path::Path::totalPathLength)
-    .def_rw("points", &Path::Path::points);
+    .def("size", &Path::Path::getSize)
+    .def("getIndex", &Path::Path::getIndex, py::arg("index"))
+    .def("x", &Path::Path::getX)
+    .def("y", &Path::Path::getY);
 
 // Velocity control command
 py::class_<Control::Velocity2D>(m_types, "ControlCmd")
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
index 2e18f9c1..e8298da9 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/follower.h
@@ -48,7 +48,7 @@ class Follower : public Controller {
                                             // robot reached the goal point
       addParameter(
           "loosing_goal_distance",
-          Parameter(0.1, 0.001, 1000.0)); // [m] If driving past the goal
+          Parameter(0.5, 0.001, 1000.0)); // [m] If driving past the goal
                                           // we stop after this distance
     }
   };
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index e7c7da8c..89dc7f10 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -4,7 +4,6 @@
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
-#include <cstddef>
 #include <math.h>
 #include <vector>
 
@@ -37,36 +36,47 @@ typedef Eigen::Vector3f Point;
 
 // Structure for Path Control parameters
 struct Path {
-  std::vector<Point> points;  // Vector of points defining the path
   std::vector<Path> segments; // List of path segments
-  tk::spline *_spline;
   // get all x values from points
-  const std::vector<float>& getX() const;
+  const Eigen::VectorXf getX() const;
   // get all y values from points
-  const std::vector<float>& getY() const;
+  const Eigen::VectorXf getY() const;
   // get all z values from points
-  const std::vector<float>& getZ() const;
-  // Max interpolation distance and total path distance are updated from user
-  // config
-  double _max_interpolation_dist{0.0}, _max_path_length{10.0};
+  const Eigen::VectorXf getZ() const;
   // Max segment size and max total path points size is calculated after
   // interpolation
   int max_segment_size{10};
-  size_t max_size{10};
-  size_t max_interpolation_iterations{500}; // Max number of iterations for interpolation between two path points
 
-  Path(const std::vector<Point> &points = {});
+  Path(const Path& other) = default;
+
+  Path(const std::vector<Point> &points = {}, const size_t new_max_size = 10);
+
+  Path(const Eigen::VectorXf &x_points, const Eigen::VectorXf &y_points,
+       const Eigen::VectorXf &z_points, const size_t new_max_size = 10);
 
   size_t getMaxNumSegments();
 
   void setMaxLength(double max_length);
 
+  void resize(const size_t max_new_size);
+
   bool endReached(State currentState, double minDist);
 
   Point getEnd() const;
 
   Point getStart() const;
 
+  Point getIndex(const size_t index) const;
+
+  Path getPart(const size_t start, const size_t end,
+               const size_t max_part_size = 0) const;
+
+  void pushPoint(const Point& point);
+
+  size_t getSize() const;
+
+  size_t getMaxSize() const;
+
   float getEndOrientation() const;
 
   float getStartOrientation() const;
@@ -93,10 +103,46 @@ struct Path {
   // Segment using a segment points number
   void segmentByPointsNumber(int segmentLength);
 
+  struct Iterator {
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = Point;
+    using difference_type = std::ptrdiff_t;
+
+    Iterator(const Path &p, size_t idx) : path(p), index(idx) {}
+
+    Point operator*() const {
+      return path.getIndex(index);
+    }
+
+    Iterator &operator++() {
+      ++index;
+      return *this;
+    }
+
+    bool operator!=(const Iterator &other) const {
+      return index != other.index;
+    }
+
+  private:
+    const Path &path;
+    size_t index;
+  };
+
+  Iterator begin() const { return Iterator(*this, 0); }
+  Iterator end() const { return Iterator(*this, current_size_); }
+
 private:
-  std::vector<float> X_; // Vector of X coordinates
-  std::vector<float> Y_; // Vector of Y coordinates
-  std::vector<float> Z_; // Vector of Z coordinates
+  Eigen::VectorXf X_; // Vector of X coordinates
+  Eigen::VectorXf Y_; // Vector of Y coordinates
+  Eigen::VectorXf Z_; // Vector of Z coordinates
+  size_t current_size_{0}; // Current size of the path
+  size_t max_interpolation_iterations_; // Max number of iterations for
+                                        // interpolation between two path points
+  // Max interpolation distance and total path distance are updated from user
+  // config
+  float max_path_length_{10.0}, max_interpolation_dist_{0.0};
+  tk::spline *spline_;
+  size_t max_size_{10};
 };
 
 struct PathPosition {
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
index 9cc9b619..20f76db4 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
@@ -148,14 +148,15 @@ struct TrajectoryPath {
 
   // initialize from a  Path
   explicit TrajectoryPath(const Path::Path &path) {
-    x.resize(path.points.size());
-    y.resize(path.points.size());
-    z.resize(path.points.size());
-    numPointsPerTrajectory_ = path.points.size();
-    for (size_t i = 0; i < path.points.size(); ++i) {
-      x(i) = path.points[i].x();
-      y(i) = path.points[i].y();
-      z(i) = path.points[i].z();
+    x.resize(path.getSize());
+    y.resize(path.getSize());
+    z.resize(path.getSize());
+    numPointsPerTrajectory_ = path.getSize();
+    // Set the current path points
+    for (size_t i = 0; i < path.getSize(); ++i) {
+      x(i) = path.getIndex(i).x();
+      y(i) = path.getIndex(i).y();
+      z(i) = path.getIndex(i).z();
     }
   };
 
@@ -278,7 +279,7 @@ struct Trajectory2D {
 
   // initialize with path object and velocity vector
   explicit Trajectory2D(std::vector<Velocity2D> &velocities, Path::Path &path) {
-    if (velocities.size() != path.points.size()) {
+    if (velocities.size() != path.getSize()) {
       throw std::invalid_argument(
           "Velocity2D vector and path points vector should have the same size "
           "must have the same numPointsPerTrajectory");
@@ -400,14 +401,13 @@ struct TrajectoryPathSamples {
 
   // Add a new path from a Path struct.
   void push_back(const Path::Path &path) {
-    assert(path.points.size() == numPointsPerTrajectory_ &&
-           "Path points vector must have size equivalent to numPointsPerTrajectory");
+    assert(path.getSize() == numPointsPerTrajectory_ && "Path points vector must have size equivalent to numPointsPerTrajectory");
 
     pathIndex_++;
     for (size_t i = 0; i < numPointsPerTrajectory_; ++i) {
-      x(pathIndex_, i) = path.points[i].x();
-      y(pathIndex_, i) = path.points[i].y();
-      z(pathIndex_, i) = path.points[i].z();
+      x(pathIndex_, i) = path.getIndex(i).x();
+      y(pathIndex_, i) = path.getIndex(i).y();
+      z(pathIndex_, i) = path.getIndex(i).z();
     }
   }
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
index 68442f12..bf22cdff 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
@@ -105,7 +105,7 @@ class CostEvaluator {
    */
   TrajSearchResult
   getMinTrajectoryCost(const std::unique_ptr<TrajectorySamples2D> &trajs,
-                       const Path::Path &reference_path,
+                       const Path::Path* reference_path,
                        const Path::Path &tracked_segment);
 
   /**
@@ -209,6 +209,7 @@ class CostEvaluator {
   float *m_devicePtrTempCosts = nullptr;
   LowestCost *m_minCost;
   sycl::queue m_q;
+  sycl::vec<float, 3> m_deviceRefPathEnd;
   void initializeGPUMemory();
   /**
    * @brief Trajectory cost based on the distance to a given reference path
@@ -229,7 +230,6 @@ class CostEvaluator {
    * @param reference_path
    */
   sycl::event goalCostFunc(const size_t trajs_size,
-                           const Path::Point &last_ref_point,
                            const float ref_path_length,
                            const double cost_weight);
 
@@ -283,7 +283,7 @@ class CostEvaluator {
    * @return float
    */
   float goalCostFunc(const Trajectory2D &trajectory,
-                     const Path::Path &reference_path,
+                     const Path::Point &reference_path_end_point,
                      const float ref_path_length);
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 1791d043..27d841f4 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -6,7 +6,6 @@
 #include <algorithm>
 #include <array>
 #include <cmath>
-#include <cstddef>
 #include <memory>
 #include <stdexcept>
 #include <tuple>
@@ -127,52 +126,48 @@ void DWA::setCurrentState(const Path::State &position) {
 }
 
 Path::Path DWA::findTrackedPathSegment() {
-  std::vector<Path::Point> trackedPoints;
+
   size_t segment_index{current_segment_index_ + 1};
   Path::Path currentSegment = currentPath->segments[current_segment_index_];
 
   // If we reached end of the current segment and a new segment is available ->
   // take the next segment
-  if (closestPosition->index > currentSegment.points.size() - 1 and
+  if (closestPosition->index >= currentSegment.getSize() - 1 and
       current_segment_index_ < max_segment_index_) {
-    trackedPoints = currentPath->segments[current_segment_index_ + 1].points;
     segment_index = current_segment_index_ + 1;
+    return currentPath->segments[current_segment_index_ + 1];
   }
   // Else take the segment points from the current point onwards
   else {
-    trackedPoints = {currentSegment.points.begin() + closestPosition->index,
-                     currentSegment.points.end()};
-  }
-  size_t point_index{0};
+    auto trackedPath = currentSegment.getPart(closestPosition->index, currentSegment.getSize() - 1, this->maxSegmentSize);
+    size_t point_index{0};
 
-  float segment_length = 0.0;
-  for (size_t i = 1; i < trackedPoints.size(); ++i) {
-    segment_length +=
-        Path::Path::distance(trackedPoints[i - 1], trackedPoints[i]);
-  }
+    float segment_length = trackedPath.totalPathLength();
 
-  // If the segment does not have the required number of points add more points
-  // from next path segment
-  while (segment_length < max_forward_distance_ and
-         segment_index <= max_segment_index_ and
-         trackedPoints.size() < maxSegmentSize) {
-    if (point_index >= currentPath->segments[segment_index].points.size()) {
-      point_index = 0;
-      segment_index++;
-      if (segment_index > max_segment_index_) {
-        break;
+    // If the segment does not have the required number of points add more
+    // points from next path segment
+    while (segment_length < max_forward_distance_ and
+           segment_index <= max_segment_index_ and
+           trackedPath.getSize() < maxSegmentSize - 1) {
+      if (point_index >= currentPath->segments[segment_index].getSize()) {
+        point_index = 0;
+        segment_index++;
+        if (segment_index > max_segment_index_) {
+          break;
+        }
       }
+      // Add distance between last point and new point
+      Path::Point back_point = trackedPath.getEnd();
+      segment_length += calculateDistance(
+          back_point,
+          currentPath->segments[segment_index].getIndex(point_index));
+      trackedPath.pushPoint(
+          currentPath->segments[segment_index].getIndex(point_index));
+      point_index++;
     }
-    // Add distance between last point and new point
-    Path::Point back_point = trackedPoints.back();
-    segment_length += calculateDistance(
-        back_point, currentPath->segments[segment_index].points[point_index]);
-    trackedPoints.push_back(
-        currentPath->segments[segment_index].points[point_index]);
-    point_index++;
-  }
 
-  return Path::Path(trackedPoints);
+    return trackedPath;
+  }
 }
 
 template <typename T>
@@ -199,7 +194,7 @@ TrajSearchResult DWA::findBestPath(const Velocity2D &global_vel,
   Path::Path trackedRefPathSegment = findTrackedPathSegment();
 
   // Evaluate the samples and get the sample with the minimum cost
-  return trajCostEvaluator->getMinTrajectoryCost(samples_, *currentPath,
+  return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(),
                                                  trackedRefPathSegment);
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index 1b084477..0591ec49 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -82,8 +82,8 @@ void Follower::clearCurrentPath() {
 }
 
 void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
-  currentPath = std::make_unique<Path::Path>(path.points);
-  refPath = std::make_unique<Path::Path>(path.points);
+  currentPath = std::make_unique<Path::Path>(path);
+  refPath = std::make_unique<Path::Path>(path);
 
   currentPath->setMaxLength(
       this->config.getParameter<double>("max_path_length"));
@@ -224,8 +224,8 @@ Path::State Follower::projectPointOnSegment(const Path::Point &a,
 
 Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) {
 
-  const std::vector<Path::Point> &segment_points =
-      currentPath->segments[segment_index].points;
+  const Path::Path &segment_path =
+      currentPath->segments[segment_index];
   double min_distance = std::numeric_limits<double>::max();
   Path::State closest_point;
   double segment_position = 0.0; // in [0, 1]
@@ -236,22 +236,21 @@ Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) {
 
   double segment_heading = std::atan2(end.y() - start.y(), end.x() - start.x());
 
-  for (size_t i = 0; i < segment_points.size(); ++i) {
-
-    Path::Point projected_point = segment_points[i];
+  for (auto projected_point : segment_path) {
     double distance = calculateDistance(currentState, projected_point);
 
     if (distance < min_distance) {
       min_distance = distance;
       closest_point = {projected_point.x(), projected_point.y(),
                        segment_heading};
-      segment_position = static_cast<double>(i) / (segment_points.size() - 1);
-      point_index = i;
+      segment_position =
+          static_cast<double>(point_index) / (segment_path.getSize() - 1);
+      point_index++;
     }
   }
 
   Path::PathPosition closest_position;
-  closest_position.index = point_index;
+  closest_position.index = point_index - 1;
   closest_position.segment_index = segment_index;
   closest_position.segment_length = segment_position;
   closest_position.state = closest_point;
@@ -283,7 +282,7 @@ void Follower::determineTarget() {
   // closest position is never updated
   // OR If we reached end of segment or end of path -> Find new point
   if ((closestPosition->segment_length <= 0.0) ||
-      (closestPosition->segment_index >= currentPath->points.size() - 1) ||
+      (closestPosition->segment_index >= currentPath->getSize() - 1) ||
       (closestPosition->segment_length >= 1.0)) {
     *closestPosition = findClosestPathPoint();
   }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 90866583..099cf1bd 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -126,10 +126,8 @@ TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
   else{
     LOG_INFO("USING DWA SAMPLING");
     // The tracking sample has collisions -> use DWA-like sampling and control
-    Path::Path ref_tracking_path;
-    for (int idx=0; idx < _config.prediction_horizon(); ++idx){
-      ref_tracking_path.points.push_back(ref_traj.path.getIndex(idx));
-    }
+    Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
+                                 ref_traj.path.z);
     // Set the tracking segment as the reference path
     this->setCurrentPath(ref_tracking_path);
     return this->computeVelocityCommandsSet(current_vel, sensor_points);
diff --git a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
index c250a96c..0c8858ec 100644
--- a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
@@ -8,40 +8,115 @@ using namespace tk;
 
 namespace Path {
 
-Path::Path(const std::vector<Point> &points) : points(points) {
-  for (const auto &point : points) {
-    X_.emplace_back(point.x());
-    Y_.emplace_back(point.y());
-    Z_.emplace_back(point.z());
+Path::Path(const std::vector<Point> &points, const size_t new_max_size) {
+  if (new_max_size < 2) {
+    throw std::invalid_argument(
+        "At least two points are required to create a path.");
+  }
+  if (points.size() > new_max_size) {
+    throw std::invalid_argument(
+        "Points size is larger than the allowed maximum size.");
+  }
+  resize(new_max_size);
+  current_size_ = points.size();
+  for (size_t i = 0; i < points.size(); ++i) {
+    X_(i) = points[i].x();
+    Y_(i) = points[i].y();
+    Z_(i) = points[i].z();
   }
 }
 
-const std::vector<float>& Path::getX() const { return X_; }
+Path::Path(const Eigen::VectorXf &x_points, const Eigen::VectorXf &y_points,
+           const Eigen::VectorXf &z_points, const size_t new_max_size) {
+  if (x_points.size() != y_points.size() ||
+      x_points.size() != z_points.size()) {
+    throw std::invalid_argument("X, Y and Z vectors must have the same size.");
+  }
+  if (new_max_size < 2) {
+    throw std::invalid_argument(
+        "At least two points are required to create a path.");
+  }
+  if (x_points.size() > new_max_size) {
+    throw std::invalid_argument(
+        "Points size is larger than the allowed maximum size.");
+  }
+  resize(new_max_size);
+  current_size_ = x_points.size();
+  X_.head(current_size_) = x_points;
+  Y_.head(current_size_) = y_points;
+  Z_.head(current_size_) = z_points;
+}
 
-const std::vector<float>& Path::getY() const { return Y_; }
+const Eigen::VectorXf Path::getX() const { return X_.segment(0, current_size_); }
 
-const std::vector<float>& Path::getZ() const { return Z_; }
+const Eigen::VectorXf Path::getY() const {return Y_.segment(0, current_size_);}
+
+const Eigen::VectorXf Path::getZ() const { return Z_.segment(0, current_size_); }
+
+size_t Path::getSize() const {
+  return current_size_;
+}
 
 void Path::setMaxLength(double max_length) {
-  this->_max_path_length = max_length;
+  max_path_length_ = max_length;
+}
+
+void Path::resize(const size_t new_max_size) {
+  max_size_ = new_max_size;
+  X_.resize(max_size_);
+  Y_.resize(max_size_);
+  Z_.resize(max_size_);
+  if (current_size_ > max_size_) {
+    current_size_ = max_size_;
+  }
 }
 
 bool Path::endReached(State currentState, double minDist) {
-  Point endPoint = points.back();
+  Point endPoint = getEnd();
   double dist = sqrt(pow(endPoint.x() - currentState.x, 2) +
                      pow(endPoint.y() - currentState.y, 2));
   return dist <= minDist;
 }
 
+size_t Path::getMaxSize() const{
+  return max_size_;
+}
+
 size_t Path::getMaxNumSegments() { return segments.size() - 1; }
 
-Point Path::getEnd() const { return points.back(); }
+Point Path::getEnd() const { return getIndex(current_size_ - 1); }
+
+Point Path::getStart() const { return getIndex(0); }
+
+Point Path::getIndex(const size_t index) const{
+  assert(index < current_size_ && "Index out of range");
+  return Point(X_(index), Y_(index), Z_(index));
+}
+
+Path Path::getPart(const size_t start, const size_t end, const size_t max_part_size) const{
+  if (start >= current_size_ || end >= current_size_ || start >= end ) {
+    throw std::out_of_range("Invalid range for path part.");
+  }
+  auto part_size = std::max(max_part_size, end - start + 1);
+  Path part(X_.segment(start, end - start + 1),
+            Y_.segment(start, end - start + 1),
+            Z_.segment(start, end - start + 1), part_size);
+  return part;
+}
 
-Point Path::getStart() const { return points.front(); }
+void Path::pushPoint(const Point &point) {
+  if(current_size_ >= max_size_) {
+    throw std::out_of_range("Path is full. Cannot add more points.");
+  }
+  X_(current_size_) = point.x();
+  Y_(current_size_) = point.y();
+  Z_(current_size_) = point.z();
+  current_size_++;
+}
 
 float Path::getEndOrientation() const {
-  const Point &p1 = points[points.size() - 2];
-  const Point &p2 = points[points.size() - 1];
+  const Point &p1 = getIndex(current_size_ - 2);
+  const Point &p2 = getIndex(current_size_ - 1);
 
   float dx = p2.x() - p1.x();
   float dy = p2.y() - p1.y();
@@ -53,8 +128,8 @@ float Path::getEndOrientation() const {
 }
 
 float Path::getStartOrientation() const {
-  const Point &p1 = points[0];
-  const Point &p2 = points[1];
+  const Point &p1 = getIndex(0);
+  const Point &p2 = getIndex(1);
 
   float dx = p2.x() - p1.x();
   float dy = p2.y() - p1.y();
@@ -68,12 +143,12 @@ float Path::getStartOrientation() const {
 float Path::getOrientation(const size_t index) const {
   Point p1;
   Point p2;
-  if (index < points.size()) {
-    p1 = points[index];
-    p2 = points[index + 1];
+  if (index < current_size_) {
+    p1 = getIndex(index);
+    p2 = getIndex(index + 1);
   } else {
-    p1 = points[index - 1];
-    p2 = points[index];
+    p1 = getIndex(index - 1);
+    p2 = getIndex(index);
   }
 
   float dx = p2.x() - p1.x();
@@ -92,13 +167,13 @@ float Path::distance(const Point &p1, const Point &p2) {
 // Function to compute the total path length
 float Path::totalPathLength() const {
 
-  if (points.empty()) {
+  if (current_size_ < 2) {
     return 0.0;
   }
 
   float totalLength = 0.0;
-  for (size_t i = 1; i < points.size(); ++i) {
-    totalLength += distance(points[i - 1], points[i]);
+  for (size_t i = 1; i < current_size_; ++i) {
+    totalLength += distance(getIndex(i - 1), getIndex(i));
   }
 
   return totalLength;
@@ -106,13 +181,13 @@ float Path::totalPathLength() const {
 
 Point Path::getPointAtLength(const double length) const {
   float totalLength = totalPathLength();
-  if (length <= totalLength or points.size() > 2) {
+  if (length <= totalLength or current_size_ > 2) {
     float accumLength = 0.0;
-    float twoPointDist = distance(points[0], points[1]);
-    for (size_t i = 1; i < points.size(); ++i) {
-      accumLength += distance(points[i - 1], points[i]);
+    float twoPointDist = distance(getIndex(0), getIndex(1));
+    for (size_t i = 1; i < current_size_; ++i) {
+      accumLength += distance(getIndex(i - 1), getIndex(i));
       if (abs(accumLength - totalLength) < twoPointDist) {
-        return points[i - 1];
+        return getIndex(i - 1);
       }
     }
   }
@@ -122,44 +197,45 @@ Point Path::getPointAtLength(const double length) const {
 size_t Path::getNumberPointsInLength(double length) const {
   double totalLength = 0.0;
 
-  for (size_t i = 1; i < points.size(); ++i) {
-    totalLength += distance(points[i - 1], points[i]);
+  for (size_t i = 1; i < current_size_; ++i) {
+    totalLength += distance(getIndex(i - 1), getIndex(i));
     if (totalLength >= length) {
       return i;
     }
   }
-  return points.size();
+  return current_size_;
 }
 
 void Path::interpolate(double max_interpolation_point_dist,
                        InterpolationType type) {
-
-  this->_max_interpolation_dist = max_interpolation_point_dist;
-  // Set the maximum size for the points
-  this->max_size = static_cast<size_t>(this->_max_path_length /
-                                       this->_max_interpolation_dist);
-  if (points.size() < 2) {
+  if (current_size_ < 2) {
     throw invalid_argument(
         "At least two points are required to perform interpolation.");
   }
+  // Get copies of X and Y vectors effective points
+  Eigen::VectorXf x = X_.segment(0, current_size_);
+  Eigen::VectorXf y = Y_.segment(0, current_size_);
 
-  // Get copies of X and Y vectors
-  std::vector<float> x = getX();
-  std::vector<float> y = getY();
+  this->max_interpolation_dist_ = max_interpolation_point_dist;
+  // Set the maximum size for the points
+  auto maxSize = static_cast<size_t>(this->max_path_length_ /
+                                       this->max_interpolation_dist_);
+  resize(maxSize);
+  // Remaining iteration when interpolating the path (interpolation points
+  // between each two path points)
+  max_interpolation_iterations_ =
+      static_cast<size_t>((maxSize - current_size_) / (current_size_));
 
-  points.clear();
-  X_.clear();
-  Y_.clear();
-  Z_.clear();
+  Z_ = Eigen::VectorXf::Zero(max_size_);
+  current_size_ = 0;
 
   float dist, x_e, y_e;
 
   for (size_t i = 0; i < x.size() - 1; ++i) {
     // Add the first point
-    points.emplace_back(x[i], y[i], 0.0);
-    X_.emplace_back(x[i]);
-    Y_.emplace_back(y[i]);
-    Z_.emplace_back(0.0);
+    X_(current_size_) = x[i];
+    Y_(current_size_) = y[i];
+    current_size_++;
     std::vector<double> x_points, y_points;
 
     // Add mid point to send 3 points for spline interpolation
@@ -180,11 +256,11 @@ void Path::interpolate(double max_interpolation_point_dist,
 
     // Create the spline object and set the x and y values
     if (type == InterpolationType::LINEAR) {
-      _spline = new tk::spline(x_points, y_points, tk::spline::linear);
+      spline_ = new tk::spline(x_points, y_points, tk::spline::linear);
     } else if (type == InterpolationType::CUBIC_SPLINE) {
-      _spline = new tk::spline(x_points, y_points, tk::spline::cspline);
+      spline_ = new tk::spline(x_points, y_points, tk::spline::cspline);
     } else {
-      _spline = new tk::spline(x_points, y_points, tk::spline::cspline_hermite);
+      spline_ = new tk::spline(x_points, y_points, tk::spline::cspline_hermite);
     }
 
     x_e = x[i];
@@ -193,53 +269,52 @@ void Path::interpolate(double max_interpolation_point_dist,
 
     // Interpolate new points between i, i+1
     while (dist > max_interpolation_point_dist and
-           j < max_interpolation_iterations) {
+           j < max_interpolation_iterations_) {
       x_e = x[i] + j * (x[i + 1] - x[i]) * max_interpolation_point_dist;
 
-      y_e = _spline->operator()(x_e);
-      points.emplace_back(x_e, y_e, 0.0);
-      X_.emplace_back(x_e);
-      Y_.emplace_back(y_e);
-      Z_.emplace_back(0.0);
+      y_e = spline_->operator()(x_e);
+      X_(current_size_) = x_e;
+      Y_(current_size_) = y_e;
+      current_size_++;
       dist = distance({x_e, y_e, 0.0}, {x[i + 1], y[i + 1], 0.0});
       j++;
     }
-    if (points.size() > this->max_size) {
+    if (current_size_ > this->max_size_) {
       float remaining_len =
           distance({x[i + 1], y[i + 1], 0.0}, {x[-1], y[-1], 0.0});
-      LOG_WARNING("Cannot interpolate more than ", this->max_size,
+      LOG_WARNING("Cannot interpolate more than ", this->max_size_,
                   " path points -> "
                   "Discarding all future points of length ",
                   remaining_len, "m");
       break;
     }
   }
-  if (points.size() < this->max_size) {
+  if (current_size_ < this->max_size_) {
     // Add last point
-    points.emplace_back(x.back(), y.back(), 0.0);
-    X_.emplace_back(x.back());
-    Y_.emplace_back(y.back());
-    Z_.emplace_back(0.0);
+    X_(current_size_) = x(x.size() - 1);
+    Y_(current_size_) = y(y.size() - 1);
+    current_size_++;
   }
 }
 
 // Segment the path by a given segment path length [m]
 void Path::segment(double pathSegmentLength) {
-  if (_max_interpolation_dist > 0.0) {
+  if (max_interpolation_dist_ > 0.0) {
     this->max_segment_size =
-        static_cast<int>(pathSegmentLength / _max_interpolation_dist) + 1;
+        static_cast<int>(pathSegmentLength / max_interpolation_dist_) + 1;
   }
   segments.clear();
   double totalLength = totalPathLength();
-  Path new_segment;
   if (pathSegmentLength >= totalLength) {
-    new_segment = Path(points);
+    // Add the whole path as a single segment
+    auto new_segment = *this;
+    new_segment.resize(this->max_segment_size);
     segments.push_back(new_segment);
   } else {
     int segmentsNumber = max(totalLength / pathSegmentLength, 1.0);
     if (segmentsNumber == 1) {
-      new_segment = Path(points);
-      new_segment.max_size = this->max_segment_size;
+      auto new_segment = *this;
+      new_segment.resize(this->max_segment_size);
       segments.push_back(new_segment);
       return;
     }
@@ -250,27 +325,27 @@ void Path::segment(double pathSegmentLength) {
 // Segment using a number of segments
 void Path::segmentBySegmentNumber(int numSegments) {
   segments.clear();
-  if (numSegments <= 0 || points.empty()) {
+  if (numSegments <= 0 || current_size_ <= 0 ) {
     throw std::invalid_argument(
         "Invalid number of segments or empty points vector.");
   }
 
-  this->max_segment_size = points.size() / numSegments;
-  int remainder = points.size() % numSegments;
+  this->max_segment_size = current_size_ / numSegments;
+  int remainder = current_size_ % numSegments;
 
-  auto it = points.begin();
-  Path new_segment;
+  auto it_x = X_.begin();
+  auto it_y = Y_.begin();
+  auto it_z = Z_.begin();
   for (int i = 0; i < numSegments; ++i) {
     std::vector<Point> segment_points;
     for (int j = 0; j < this->max_segment_size; ++j) {
-      segment_points.push_back(*it++);
+      segment_points.push_back({*it_x++, *it_y++, *it_z++});
     }
     if (remainder > 0) {
-      segment_points.push_back(*it++);
+      segment_points.push_back({*it_x++, *it_y++, *it_z++});
       --remainder;
     }
-    new_segment = Path(segment_points);
-    new_segment.max_size = this->max_segment_size;
+    auto new_segment = Path(segment_points, segment_points.size());
     segments.push_back(new_segment);
   }
 }
@@ -279,18 +354,17 @@ void Path::segmentBySegmentNumber(int numSegments) {
 void Path::segmentByPointsNumber(int segmentLength) {
   this->max_segment_size = segmentLength;
   segments.clear();
-  if (segmentLength <= 0 || points.empty()) {
+  if (segmentLength <= 0 || current_size_ <= 0) {
     throw std::invalid_argument(
         "Invalid segment length or empty points vector.");
   }
-  Path new_segment;
-  for (size_t i = 0; i < points.size(); i += segmentLength) {
+  for (size_t i = 0; i < current_size_; i += segmentLength) {
     std::vector<Point> segment_points;
-    for (size_t j = i; j < i + segmentLength && j < points.size(); ++j) {
-      segment_points.push_back(points[j]);
+    for (size_t j = i; j < i + segmentLength && j < current_size_; ++j) {
+      segment_points.push_back(getIndex(j));
     }
-    new_segment = Path(segment_points);
-    new_segment.max_size = this->max_segment_size;
+    auto new_segment = Path(segment_points, segment_points.size());
+    new_segment.max_size_ = this->max_segment_size;
     segments.push_back(new_segment);
   }
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
index f412aba4..92d4db15 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
@@ -52,7 +52,7 @@ CostEvaluator::~CostEvaluator() {
 
 TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     const std::unique_ptr<TrajectorySamples2D> &trajs,
-    const Path::Path &reference_path,
+    const Path::Path* reference_path,
     const Path::Path &tracked_segment) {
   double weight;
   float total_cost;
@@ -63,10 +63,10 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
 
   for (const auto &traj : *trajs) {
     total_cost = 0.0;
-    if ((ref_path_length = reference_path.totalPathLength()) > 0.0) {
+    if ((ref_path_length = reference_path->totalPathLength()) > 0.0) {
       if ((weight = costWeights->getParameter<double>("goal_distance_weight")) >
           0.0) {
-        float goalCost = goalCostFunc(traj, reference_path, ref_path_length);
+        float goalCost = goalCostFunc(traj, reference_path->getEnd(), ref_path_length);
         total_cost += weight * goalCost;
       }
       if ((weight = costWeights->getParameter<double>(
@@ -99,7 +99,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     for (const auto &custom_cost : customTrajCostsPtrs_) {
       // custom cost functions takes in the trajectory and the reference path
       total_cost +=
-          custom_cost->weight * custom_cost->evaluator_(traj, reference_path);
+          custom_cost->weight * custom_cost->evaluator_(traj, *reference_path);
     }
 
     if (total_cost < minCost) {
@@ -123,8 +123,8 @@ float CostEvaluator::pathCostFunc(const Trajectory2D &trajectory,
     // infinity
     distError = DEFAULT_MIN_DIST;
     // Get minimum distance to the reference
-    for (size_t j = 0; j < tracked_segment.points.size(); ++j) {
-      dist = Path::Path::distance(tracked_segment.points[j],
+    for (size_t j = 0; j < tracked_segment.getSize(); ++j) {
+      dist = Path::Path::distance(tracked_segment.getIndex(j),
                                   trajectory.path.getIndex(i));
       if (dist < distError) {
         distError = dist;
@@ -146,11 +146,11 @@ float CostEvaluator::pathCostFunc(const Trajectory2D &trajectory,
 
 // Compute the cost of a trajectory based on distance to a given reference path
 float CostEvaluator::goalCostFunc(const Trajectory2D &trajectory,
-                                  const Path::Path &reference_path,
+                                  const Path::Point &reference_path_end_point,
                                   const float ref_path_length) {
   // end point distance normalized to range [0, 1]
   return Path::Path::distance(trajectory.path.getEnd(),
-                              reference_path.getEnd()) /
+                              reference_path_end_point) /
          ref_path_length;
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
index 6b6d8623..71dde07f 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
@@ -193,7 +193,7 @@ CostEvaluator::~CostEvaluator() {
 
 TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     const std::unique_ptr<TrajectorySamples2D> &trajs,
-    const Path::Path &reference_path, const Path::Path &tracked_segment) {
+    const Path::Path* reference_path, const Path::Path &tracked_segment) {
 
   try {
     double weight;
@@ -224,15 +224,19 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     if ((costWeights->getParameter<double>("reference_path_distance_weight") >
              0.0 ||
          costWeights->getParameter<double>("goal_distance_weight") > 0.0) &&
-        (ref_path_length = reference_path.totalPathLength()) > 0.0) {
+        (ref_path_length = reference_path->totalPathLength()) > 0.0) {
       if ((weight = costWeights->getParameter<double>("goal_distance_weight")) >
           0.0) {
-        events.push_back(goalCostFunc(trajs_size, reference_path.getEnd(),
+        auto last_point = reference_path->getEnd();
+        m_deviceRefPathEnd = sycl::vec(last_point.x(),
+                                       last_point.y(),
+                                       last_point.z());
+        events.push_back(goalCostFunc(trajs_size,
                                       ref_path_length, weight));
       }
       if ((weight = costWeights->getParameter<double>(
                "reference_path_distance_weight")) > 0.0) {
-        size_t tracked_segment_size = tracked_segment.points.size();
+        size_t tracked_segment_size = tracked_segment.getSize();
         m_q.memcpy(m_devicePtrTrackedSegmentX, tracked_segment.getX().data(),
                    sizeof(float) * tracked_segment_size)
             .wait();
@@ -279,7 +283,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
           // path
           m_devicePtrTempCosts[idx] +=
               custom_cost->weight *
-              custom_cost->evaluator_(traj, reference_path);
+              custom_cost->evaluator_(traj, *reference_path);
         }
         idx += 1;
       }
@@ -427,7 +431,6 @@ sycl::event CostEvaluator::pathCostFunc(const size_t trajs_size,
 
 // Compute the cost of trajectory based on distance to the goal point
 sycl::event CostEvaluator::goalCostFunc(const size_t trajs_size,
-                                        const Path::Point &last_ref_point,
                                         const float ref_path_length,
                                         const double cost_weight) {
   // -----------------------------------------------------
@@ -447,7 +450,8 @@ sycl::event CostEvaluator::goalCostFunc(const size_t trajs_size,
     const float pathLength = ref_path_length;
     auto global_size = sycl::range<1>(trajs_size);
     // Last point of reference path
-    sycl::vec<float, 2> lastRefPoint = {last_ref_point.x(), last_ref_point.y()};
+    sycl::vec<float, 2> lastRefPoint = {m_deviceRefPathEnd[0],
+                                        m_deviceRefPathEnd[1]};
     // Kernel scope
     h.parallel_for<class goalCostKernel>(
         sycl::range<1>(global_size), [=](sycl::id<1> id) {
diff --git a/src/kompass_cpp/tests/controller_test.cpp b/src/kompass_cpp/tests/controller_test.cpp
index 43427d18..0f621443 100644
--- a/src/kompass_cpp/tests/controller_test.cpp
+++ b/src/kompass_cpp/tests/controller_test.cpp
@@ -34,7 +34,7 @@ BOOST_AUTO_TEST_CASE(test_DWA) {
   // Create a test path
   std::vector<Path::Point> points{Path::Point(0.0, 0.0, 0.0), Path::Point(1.0, 0.0, 0.0),
                                   Path::Point(2.0, 0.0, 0.0)};
-  Path::Path path(points);
+  Path::Path path(points, 500);
 
   // Sampling configuration
   double timeStep = 0.1;
diff --git a/src/kompass_cpp/tests/cost_evaluator_test.cpp b/src/kompass_cpp/tests/cost_evaluator_test.cpp
index 144f68bc..daadb8ab 100644
--- a/src/kompass_cpp/tests/cost_evaluator_test.cpp
+++ b/src/kompass_cpp/tests/cost_evaluator_test.cpp
@@ -128,7 +128,7 @@ Trajectory2D run_test(CostEvaluator &costEval, Path::Path &reference_path,
   }
 
   TrajSearchResult result =
-      costEval.getMinTrajectoryCost(samples, reference_path, reference_path.segments[current_segment_index]);
+      costEval.getMinTrajectoryCost(samples, &reference_path, reference_path.segments[current_segment_index]);
 
   BOOST_TEST(result.isTrajFound,
              "Minimum reference path cost trajectory is not found!");
@@ -183,7 +183,7 @@ struct TestConfig {
   TestConfig()
       : points{Path::Point(0.0, 0.0, 0.0), Path::Point(1.0, 0.0, 0.0),
                Path::Point(2.0, 0.0, 0.0)},
-        reference_path(points), max_path_length(10.0),
+        reference_path(points, 500), max_path_length(10.0),
         max_interpolation_point_dist(0.01), current_segment_index(0),
         timeStep(0.1), predictionHorizon(1.0), maxNumThreads(10),
         x_params(1, 3, 5), y_params(1, 3, 5), angular_params(3.14, 3, 5, 8),
diff --git a/src/kompass_cpp/tests/json_export.h b/src/kompass_cpp/tests/json_export.h
index d99e2d44..c77ce115 100644
--- a/src/kompass_cpp/tests/json_export.h
+++ b/src/kompass_cpp/tests/json_export.h
@@ -25,7 +25,7 @@ inline void from_json(const json &j, Path::Point &p) {
 // Convert Path to JSON
 inline void to_json(json &j, const Path::Path &p) {
   j["points"] = json::array(); // Initialize as a JSON array
-  for (const auto &point : p.points) {
+  for (const auto &point : p) {
     j["points"].push_back(
         json{{"x", point.x()}, {"y", point.y()}}); // Serialize each Point
   }
@@ -42,13 +42,14 @@ inline void to_json(json &j, const Control::TrajectoryPath &p) {
 
 // Convert JSON to Path
 inline void from_json(const json &j, Path::Path &p) {
-  p.points.clear(); // Clear existing points
+  std::vector<Path::Point> points;
   for (const auto &item : j.at("points")) {
     Path::Point point;
     point.x() = item.at("x");
     point.y() = item.at("y");
-    p.points.push_back(point); // Deserialize each Point
+    points.push_back(point); // Deserialize each Point
   }
+  p = Path::Path(points, points.size());
 }
 
 // Convert Velocity to JSON
diff --git a/src/kompass_cpp/tests/trajectory_sampler_test.cpp b/src/kompass_cpp/tests/trajectory_sampler_test.cpp
index 70cc2a5d..9ec5f326 100644
--- a/src/kompass_cpp/tests/trajectory_sampler_test.cpp
+++ b/src/kompass_cpp/tests/trajectory_sampler_test.cpp
@@ -24,7 +24,7 @@ void testTrajSampler() {
   std::vector<Path::Point> points{Path::Point(0.0, 0.0, 0.0),
                                   Path::Point(1.0, 0.0, 0.0),
                                   Path::Point(2.0, 0.0, 0.0)};
-  Path::Path raw_path(points);
+  Path::Path raw_path(points, 500);
 
   // Generic follower to use for raw path interpolation and segmentation
   Control::Follower *follower = new Control::Follower();
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 9085e9ad..7c7fee84 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -63,7 +63,7 @@ struct VisionDWATestConfig {
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 1.0,
                       const double goal_distance_weight = 1.0,
-                      const double obstacles_distance_weight = 1.0)
+                      const double obstacles_distance_weight = 0.5)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
@@ -172,7 +172,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
 
   // Sampling configuration
   double timeStep = 0.1;
-  double predictionHorizon =0.5;
+  double predictionHorizon =1.0;
   double controlHorizon = 0.2;
   int maxLinearSamples = 20;
   int maxAngularSamples = 20;
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index 1b2b507e..bf80d8b1 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -177,11 +177,8 @@ def run_control(
 
     # Interpolated path for visualization
     interpolated_path = controller.interpolated_path()
-    interpolation_x = []
-    interpolation_y = []
-    for point in interpolated_path.points:
-        interpolation_x.append(point[0])
-        interpolation_y.append(point[1])
+    interpolation_x = interpolated_path.x()
+    interpolation_y = interpolated_path.y()
 
     i = 0
     x_robot = []
@@ -275,14 +272,14 @@ def test_path_interpolation(plot: bool = False):
         x_ref = [pose.pose.position.x for pose in ref_path.poses]
         y_ref = [pose.pose.position.y for pose in ref_path.poses]
 
-        x_inter_lin = [point[0] for point in linear_interpolation.points]
-        y_inter_lin = [point[1] for point in linear_interpolation.points]
+        x_inter_lin = linear_interpolation.x()
+        y_inter_lin = linear_interpolation.y()
 
-        x_inter_her = [point[0] for point in hermite_spline_interpolation.points]
-        y_inter_her = [point[1] for point in hermite_spline_interpolation.points]
+        x_inter_her = hermite_spline_interpolation.x()
+        y_inter_her = hermite_spline_interpolation.y()
 
-        x_inter_cub = [point[0] for point in cubic_spline_interpolation.points]
-        y_inter_cub = [point[1] for point in cubic_spline_interpolation.points]
+        x_inter_cub = cubic_spline_interpolation.x()
+        y_inter_cub = cubic_spline_interpolation.y()
 
         # Plot the path
         plt.figure()
@@ -330,9 +327,9 @@ def path_length(path: Union[Path, PathCpp]) -> float:
                 )
                 length += np.sqrt(d_x**2 + d_y**2)
         elif isinstance(path, PathCpp):
-            for idx in range(len(path.points) - 1):
-                d_x = path.points[idx + 1][0] - path.points[idx][0]
-                d_y = path.points[idx + 1][1] - path.points[idx][1]
+            for idx in range(path.size() - 1):
+                d_x = path.getIndex(idx + 1)[0] - path.getIndex(idx)[0]
+                d_y = path.getIndex(idx + 1)[1] - path.getIndex(idx)[1]
                 length += np.sqrt(d_x**2 + d_y**2)
         return length
 

From 898d00dc37cdee97b3df7a5a3ca26c69dddcacab Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 23 Apr 2025 16:18:31 +0200
Subject: [PATCH 035/118] (fix) Removes interpolation of the reference from
 vision dwa

---
 .../kompass_cpp/src/controllers/follower.cpp       | 14 +++++---------
 .../kompass_cpp/src/controllers/vision_dwa.cpp     |  8 +++++---
 src/kompass_cpp/tests/vision_dwa_test.cpp          | 14 ++++++++++----
 3 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index 0591ec49..b8ac6650 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -1,6 +1,5 @@
 #include <Eigen/Dense>
 #include <cmath>
-#include <cstddef>
 #include <memory>
 #include <string>
 #include <vector>
@@ -90,15 +89,12 @@ void Follower::setCurrentPath(const Path::Path &path, const bool interpolate) {
 
   if (interpolate) {
     currentPath->interpolate(maxDist, interpolationType);
-
-    // Segment path
-    currentPath->segment(path_segment_length);
-
-    // Get max number of segments in the path
-    max_segment_index_ = currentPath->getMaxNumSegments();
-  } else {
-    max_segment_index_ = 0;
   }
+  // Segment path
+  currentPath->segment(path_segment_length);
+
+  // Get max number of segments in the path
+  max_segment_index_ = currentPath->getMaxNumSegments();
 
   path_processing_ = true;
   current_segment_index_ = 0;
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 099cf1bd..d5c2fc79 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -4,6 +4,7 @@
 #include "datatypes/trajectory.h"
 #include "utils/angles.h"
 #include "utils/logger.h"
+#include <algorithm>
 #include <cmath>
 #include <tuple>
 #include <vector>
@@ -58,9 +59,9 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
-    float v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    double v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
     followingVel.setVx(v);
-    float omega = -tracking_pose.omega() +
+    double omega = -tracking_pose.omega() +
                       2 * (v * sin(psi) / distance +
                            tracking_pose.v() * sin(gamma - psi) / distance -
                            _config.K_omega() * tanh(angle_error));
@@ -129,7 +130,8 @@ TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
     Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
                                  ref_traj.path.z);
     // Set the tracking segment as the reference path
-    this->setCurrentPath(ref_tracking_path);
+    // Interpolation of the path is not required as the reference is already created using the robot control time step
+    this->setCurrentPath(ref_tracking_path, false);
     return this->computeVelocityCommandsSet(current_vel, sensor_points);
   }
 }
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 7c7fee84..04c563ad 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -62,7 +62,7 @@ struct VisionDWATestConfig {
                       const float maxVel = 1.0, const float maxOmega = 2.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 1.0,
-                      const double goal_distance_weight = 1.0,
+                      const double goal_distance_weight = 0.0,
                       const double obstacles_distance_weight = 0.5)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
@@ -82,7 +82,7 @@ struct VisionDWATestConfig {
     costWeights.setParameter("goal_distance_weight", goal_distance_weight);
     costWeights.setParameter("obstacles_distance_weight",
                              obstacles_distance_weight);
-    costWeights.setParameter("smoothness_weight", 0.0);
+    costWeights.setParameter("smoothness_weight", 1.0);
     costWeights.setParameter("jerk_weight", 0.0);
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
@@ -113,7 +113,13 @@ struct VisionDWATestConfig {
       if (result.isTrajFound) {
         LOG_INFO(FMAG("STEP: "), step,
                  FMAG(", Found best trajectory with cost: "), result.trajCost);
-
+        if (controller->getLinearVelocityCmdX() > controlLimits.velXParams.maxVel) {
+          LOG_ERROR(BOLD(FRED("Vx is larger than max vel: ")),
+                    KRED, controller->getLinearVelocityCmdX(), RST,
+                    BOLD(FRED(", Vx: ")), KRED, controlLimits.velXParams.maxVel,
+                    RST);
+          return false;
+        }
         cmd.setVx(controller->getLinearVelocityCmdX());
         cmd.setVy(controller->getLinearVelocityCmdY());
         cmd.setOmega(controller->getAngularVelocityCmd());
@@ -172,7 +178,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
 
   // Sampling configuration
   double timeStep = 0.1;
-  double predictionHorizon =1.0;
+  double predictionHorizon =0.5;
   double controlHorizon = 0.2;
   int maxLinearSamples = 20;
   int maxAngularSamples = 20;

From 8280729ab083ac0f04a37b585fec67a8c6621259 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 24 Apr 2025 10:16:13 +0200
Subject: [PATCH 036/118] (fix) Removes unused include

---
 src/kompass_cpp/tests/vision_tracking_test.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp
index 412655a0..edea65e8 100644
--- a/src/kompass_cpp/tests/vision_tracking_test.cpp
+++ b/src/kompass_cpp/tests/vision_tracking_test.cpp
@@ -11,8 +11,6 @@
 #include <boost/filesystem.hpp>
 #include <boost/test/included/unit_test.hpp>
 #include <memory>
-#include <opencv2/opencv.hpp>
-#include <opencv4/opencv2/imgcodecs.hpp>
 #include <random>
 
 using namespace Kompass;

From b97950c5a10bdea6794f40e23c7e0ccddfa4a6e5 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 24 Apr 2025 15:54:36 +0200
Subject: [PATCH 037/118] (fix) removes unused params from vision dwa

---
 .../include/controllers/vision_dwa.h          | 22 ++--------
 .../kompass_cpp/include/datatypes/control.h   |  3 --
 .../include/utils/transformation.h            |  2 -
 .../src/controllers/vision_dwa.cpp            | 41 +++++++++----------
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 12 +++---
 5 files changed, 29 insertions(+), 51 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 8237b8c8..4fbd3409 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -3,11 +3,8 @@
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
 #include "dwa.h"
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Quaternion.h>
+#include <Eigen/Dense>
 #include <cmath>
-#include <memory>
-#include <queue>
 #include <tuple>
 #include <vector>
 
@@ -16,14 +13,6 @@ namespace Control {
 
 class VisionDWA : public DWA {
 public:
-  struct TrackingImgData {
-    std::array<double, 2> size_xy; // width and height of the bounding box
-    int img_width;
-    int img_height;
-    std::array<double, 2>
-        center_xy; // x, y coordinates of the object center in image frame
-    double depth;  // -1 is equivalent to none
-  };
 
   class VisionDWAConfig : public ControllerParameters {
   public:
@@ -120,13 +109,8 @@ class VisionDWA : public DWA {
                                             const T &sensor_points);
 
 private:
-  ControlType _ctrlType;
-  ControlLimitsParams _ctrl_limits;
-  VisionDWAConfig _config;
-
-  bool _rotate_in_place;
-  Velocities _out_vel;
-  std::unique_ptr<TrackingImgData> _last_tracking = nullptr;
+  ControlLimitsParams ctrl_limits_;
+  VisionDWAConfig config_;
 
   /**
    * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 417fb019..fc6037a8 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -1,11 +1,8 @@
 #pragma once
 
 #include <Eigen/Dense>
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Quaternion.h>
 #include <cmath>
 #include <stdexcept>
-#include <string>
 #include <vector>
 
 // Namespace for Control Types
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
index 52be646d..95bca210 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
@@ -3,8 +3,6 @@
 #include "datatypes/path.h"
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
 
 namespace Kompass {
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index d5c2fc79..7741a15e 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -27,14 +27,8 @@ VisionDWA::VisionDWA(const ControlType robotCtrlType,
           maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
           sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
           maxNumThreads) {
-  _ctrlType = robotCtrlType;
-  _ctrl_limits = ctrlLimits;
-  _config = config;
-  // Initialize time steps
-  int num_steps = _config.control_horizon();
-  // Initialize control vectors
-  _out_vel = Velocities(num_steps);
-  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
+  ctrl_limits_ = ctrlLimits;
+  config_ = config;
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
@@ -46,12 +40,12 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
                  tracking_pose.x() - currentState.x) -
       currentState.yaw);
 
-  float distance_error = _config.target_distance() - distance;
+  float distance_error = config_.target_distance() - distance;
   float angle_error =
-      Angle::normalizeToMinusPiPlusPi(_config.target_orientation() - psi);
+      Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
-  float distance_tolerance = _config.tolerance() * _config.target_distance();
-  float angle_tolerance = std::max(0.001, _config.tolerance() * _config.target_orientation());
+  float distance_tolerance = config_.tolerance() * config_.target_distance();
+  float angle_tolerance = std::max(0.001, config_.tolerance() * config_.target_orientation());
 
   // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
   //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
@@ -59,12 +53,17 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
-    double v = (tracking_pose.v() * cos(gamma - psi) - _config.K_v() * tanh(distance_error)) / cos(psi);
+    double v = ((tracking_pose.v() * cos(gamma - psi)) -
+                (config_.K_v() * tanh(distance_error))) / cos(psi);
+    v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
+                  ctrl_limits_.velXParams.maxVel);
     followingVel.setVx(v);
     double omega = -tracking_pose.omega() +
-                      2 * (v * sin(psi) / distance +
-                           tracking_pose.v() * sin(gamma - psi) / distance -
-                           _config.K_omega() * tanh(angle_error));
+                   2.0 * (v * sin(psi) / distance +
+                        tracking_pose.v() * sin(gamma - psi) / distance -
+                        config_.K_omega() * tanh(angle_error));
+    omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
+                      ctrl_limits_.omegaParams.maxOmega);
     followingVel.setOmega(omega);
   }
   return followingVel;
@@ -76,7 +75,7 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
                                        const T &sensor_points) {
   int step = 0;
 
-  Trajectory2D ref_traj(_config.prediction_horizon());
+  Trajectory2D ref_traj(config_.prediction_horizon());
   std::vector<Path::State> states;
   Path::State simulated_state = currentState;
   Path::State original_state = currentState;
@@ -84,15 +83,15 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
   Velocity2D cmd;
 
   // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
-  while (step < _config.prediction_horizon()) {
+  while (step < config_.prediction_horizon()) {
     states.push_back(simulated_state);
     ref_traj.path.add(step,
                       Path::Point(simulated_state.x, simulated_state.y, 0.0));
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
-    simulated_state.update(cmd, _config.control_time_step());
-    simulated_track.update(_config.control_time_step());
-    if(step < _config.prediction_horizon() -1){
+    simulated_state.update(cmd, config_.control_time_step());
+    simulated_track.update(config_.control_time_step());
+    if(step < config_.prediction_horizon() -1){
       ref_traj.velocities.add(step,cmd);
     }
 
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 04c563ad..eb847d32 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -59,11 +59,11 @@ struct VisionDWATestConfig {
                       const float controlHorizon, const int maxLinearSamples,
                       const int maxAngularSamples,
                       const std::vector<Path::Point> sensor_points,
-                      const float maxVel = 1.0, const float maxOmega = 2.0,
+                      const float maxVel = 1.0, const float maxOmega = 4.0,
                       const int maxNumThreads = 1,
-                      const double reference_path_distance_weight = 1.0,
-                      const double goal_distance_weight = 0.0,
-                      const double obstacles_distance_weight = 0.5)
+                      const double reference_path_distance_weight = 5.0,
+                      const double goal_distance_weight = 1.0,
+                      const double obstacles_distance_weight = 0.1)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
@@ -82,7 +82,7 @@ struct VisionDWATestConfig {
     costWeights.setParameter("goal_distance_weight", goal_distance_weight);
     costWeights.setParameter("obstacles_distance_weight",
                              obstacles_distance_weight);
-    costWeights.setParameter("smoothness_weight", 1.0);
+    costWeights.setParameter("smoothness_weight", 0.0);
     costWeights.setParameter("jerk_weight", 0.0);
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
@@ -201,7 +201,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
   // Sampling configuration
   double timeStep = 0.1;
   double predictionHorizon = 0.5;
-  double controlHorizon = 0.2;
+  double controlHorizon = 0.4;
   int maxLinearSamples = 20;
   int maxAngularSamples = 20;
 

From 721c97bbd5fd12be7aaede5c47401796ce87e092 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 24 Apr 2025 17:35:18 +0200
Subject: [PATCH 038/118] (feature) Adds internal Bbox3D tracker to VisionDWA
 and adds tests

---
 .../include/controllers/vision_dwa.h          |  76 ++++++++--
 .../kompass_cpp/include/datatypes/tracking.h  |  45 ++++++
 .../kompass_cpp/include/vision/tracker.h      |   6 +-
 .../src/controllers/vision_dwa.cpp            | 113 +++++++++++---
 .../kompass_cpp/src/vision/tracker.cpp        |  25 +++-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 138 +++++++++++++++---
 6 files changed, 345 insertions(+), 58 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 4fbd3409..bbe6893f 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -2,9 +2,12 @@
 
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
+#include "datatypes/tracking.h"
 #include "dwa.h"
+#include "vision/tracker.h"
 #include <Eigen/Dense>
 #include <cmath>
+#include <memory>
 #include <tuple>
 #include <vector>
 
@@ -13,27 +16,45 @@ namespace Control {
 
 class VisionDWA : public DWA {
 public:
-
   class VisionDWAConfig : public ControllerParameters {
   public:
     VisionDWAConfig() : ControllerParameters() {
-      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
-      addParameter("control_horizon", Parameter(2, 1, 1000));
-      addParameter("prediction_horizon", Parameter(10, 1, 1000));
-      addParameter("tolerance", Parameter(0.01, 1e-6, 1e3));
-      addParameter("target_distance",
-                   Parameter(0.1, -1.0, 1e9)); // Use -1 for None
+      addParameter("control_time_step",
+                   Parameter(0.1, 1e-4, 1e6, "Control time step (s)"));
+      addParameter(
+          "control_horizon",
+          Parameter(2, 1, 1000, "Number of steps for applying the control"));
+      addParameter(
+          "prediction_horizon",
+          Parameter(10, 1, 1000, "Number of steps for future prediction"));
+      addParameter(
+          "tolerance",
+          Parameter(0.01, 1e-6, 1e3,
+                    "Tolerance value for distance and angle following errors"));
+      addParameter(
+          "target_distance",
+          Parameter(
+              0.1, -1.0, 1e9,
+              "Target distance to maintain with the target (m)")); // Use -1 for
+                                                                   // None
       addParameter(
           "target_orientation",
-          Parameter(0.0, -M_PI, M_PI)); // target bearing angle with the target
+          Parameter(0.0, -M_PI, M_PI,
+                    "Bearing angle to maintain with the target (rad)"));
+      // Search Parameters
       addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
       addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
       addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
       addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      // Pure tracking control law parameters
       addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
       addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
       addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
       addParameter("enable_search", Parameter(false));
+      // Kalman Filter parameters
+      addParameter("error_pose", Parameter(0.1, 1e-9, 1e9));
+      addParameter("error_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("error_acc", Parameter(0.1, 1e-9, 1e9));
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
     double control_time_step() const {
@@ -69,6 +90,9 @@ class VisionDWA : public DWA {
     double K_omega() const { return getParameter<double>("rotation_gain"); }
     double K_v() const { return getParameter<double>("speed_gain"); }
     double min_vel() const { return getParameter<double>("min_vel"); }
+    double e_pose() const { return getParameter<double>("error_pose"); }
+    double e_vel() const { return getParameter<double>("error_vel"); }
+    double e_acc() const { return getParameter<double>("error_acc"); }
   };
 
   VisionDWA(const ControlType robotCtrlType,
@@ -81,7 +105,8 @@ class VisionDWA : public DWA {
             const double octreeRes,
             CostEvaluator::TrajectoryCostsWeights costWeights,
             const int maxNumThreads = 1,
-            const VisionDWAConfig config = VisionDWAConfig());
+            const VisionDWAConfig config = VisionDWAConfig(),
+            const bool use_tracker = true);
 
   // Default Destructor
   ~VisionDWA() = default;
@@ -95,7 +120,8 @@ class VisionDWA : public DWA {
   Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
 
   /**
-   * @brief Get the Tracking Control Result based on object tracking and DWA sampling
+   * @brief Get the Tracking Control Result based on object tracking and DWA
+   * sampling
    *
    * @tparam T
    * @param tracking_pose
@@ -107,13 +133,41 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
                                             const Velocity2D &current_vel,
                                             const T &sensor_points);
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA
+   * sampling by searching a set of given detections
+   *
+   * @tparam T
+   * @param detected_boxes
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult
+  getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
+                  const Velocity2D &current_vel, const T &sensor_points);
+
+  /**
+   * @brief Set the initial image position of the target to be tracked
+   *
+   * @param pose_x_img
+   * @param pose_y_img
+   * @param detected_boxes
+   * @return true
+   * @return false
+   */
+  bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+                          const std::vector<Bbox3D> &detected_boxes);
 
 private:
   ControlLimitsParams ctrl_limits_;
   VisionDWAConfig config_;
+  std::unique_ptr<FeatureBasedBboxTracker> tracker_;
 
   /**
-   * @brief Get the Tracking Reference Trajectory Segment and if this segment is has collision
+   * @brief Get the Tracking Reference Trajectory Segment and if this segment is
+   * has collision
    *
    * @tparam T
    * @param tracking_pose
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 63a7cd2d..084ef57f 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -1,7 +1,9 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <cmath>
 #include <vector>
+#include "datatypes/control.h"
 
 namespace Kompass {
 
@@ -35,6 +37,7 @@ struct TrackedBbox3D {
   Eigen::Vector3f vel = {0.0, 0.0, 0.0};
   Eigen::Vector3f acc = {0.0, 0.0, 0.0};
   int unique_id = 0;
+  Eigen::Vector2f yaw_yaw_diff = {0.0, 0.0};
 
   TrackedBbox3D(const Bbox3D& box): box(box) {};
 
@@ -52,12 +55,28 @@ struct TrackedBbox3D {
     this->box = box;
   };
 
+  void updateFromNewState(const Eigen::Matrix<float, 6, 1> &state_vector,
+                          const float time_step) {
+    this->box.center = {state_vector(0), state_vector(1), 0.0f};
+    this->vel = {state_vector(2), state_vector(3), 0.0f};
+    this->acc = {state_vector(4), state_vector(5), 0.0f};
+    auto new_yaw = std::atan2(this->vel(1), this->vel(0));
+    // Orientation difference
+    this->yaw_yaw_diff(1) = (this->yaw_yaw_diff(0) - new_yaw) / time_step;
+    this->yaw_yaw_diff(0) = new_yaw;
+  };
+
   void updateFromNewDetection(const Bbox3D& new_box, const float time_step){
     // Compute velocity and acceleration based on location change
     Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
     Eigen::Vector3f new_acc = (new_vel - this->vel) / time_step;
     // Update
     setfromBox(new_box);
+    // New orientation (yaw) based on new velocity
+    auto new_yaw = std::atan2(new_vel(1), new_vel(0));
+    // Orientation difference
+    this->yaw_yaw_diff(1) = (this->yaw_yaw_diff(0) - new_yaw) / time_step;
+    this->yaw_yaw_diff(0) = new_yaw;
     this->vel = new_vel;
     this->acc = new_acc;
   }
@@ -76,6 +95,32 @@ struct TrackedBbox3D {
     predicted_tracking.box.center += predicted_tracking.vel * dt;
     return predicted_tracking;
   };
+
+  float v() const { return Eigen::Vector2f{vel.x(), vel.y()}.norm(); };
+
+  float x() const { return box.center.x(); };
+
+  float y() const { return box.center.y(); };
+
+  float yaw() const { return this->yaw_yaw_diff(0); };
+
+  float omega() const { return yaw_yaw_diff(1); };
+
+  void update(const float timeStep) {
+    box.center(0) += vel.x() * timeStep;
+    box.center(1) += vel.y() * timeStep;
+    this->yaw_yaw_diff(0) += this->yaw_yaw_diff(1) * timeStep;
+  }
+
+  float distance(const float x, const float y, const float z = 0.0) const {
+    return sqrt(pow(box.center.x() - x, 2) + pow(box.center.y() - y, 2) +
+                pow(box.center.z() - z, 2));
+  }
+
+  Control::TrackedPose2D getTrackedPose() const {
+    return Control::TrackedPose2D(box.center.x(), box.center.y(),
+           yaw(), vel.x(), vel.y(), omega());
+  }
 };
 
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index 66aa7b6b..bd5734fe 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -5,6 +5,7 @@
 #include <optional>
 #include <vector>
 #include "datatypes/tracking.h"
+#include "datatypes/control.h"
 #include "utils/kalman_filter.h"
 
 namespace  Kompass{
@@ -23,17 +24,20 @@ class FeatureBasedBboxTracker{
       bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
                               const std::vector<Bbox3D> &detected_boxes);
 
+      bool trackerInitialized() const;
+
       bool updateTracking(const std::vector<Bbox3D> &detected_boxes);
 
       std::optional<TrackedBbox3D> getRawTracking() const;
 
       std::optional<Eigen::MatrixXf> getTrackedState() const;
 
+      std::optional<Control::TrackedPose2D> getFilteredTrackedPose2D() const;
+
     private:
       float timeStep_, minAcceptedSimilarityScore_ = 0.0;
       std::unique_ptr<TrackedBbox3D> trackedBox_;
       std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
-      bool tracking_started_ = false;
 
       FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const;
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 7741a15e..899fee3d 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -6,6 +6,7 @@
 #include "utils/logger.h"
 #include <algorithm>
 #include <cmath>
+#include <stdexcept>
 #include <tuple>
 #include <vector>
 
@@ -21,7 +22,8 @@ VisionDWA::VisionDWA(const ControlType robotCtrlType,
                      const std::array<float, 4> &sensor_rotation_body,
                      const double octreeRes,
                      CostEvaluator::TrajectoryCostsWeights costWeights,
-                     const int maxNumThreads, const VisionDWAConfig config)
+                     const int maxNumThreads, const VisionDWAConfig config,
+                     const bool use_tracker)
     : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
           config.prediction_horizon(), config.control_horizon(),
           maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
@@ -29,6 +31,12 @@ VisionDWA::VisionDWA(const ControlType robotCtrlType,
           maxNumThreads) {
   ctrl_limits_ = ctrlLimits;
   config_ = config;
+  if (use_tracker) {
+    // Initialize the bounding box tracker
+    tracker_ = std::make_unique<FeatureBasedBboxTracker>(
+        config.control_time_step(), config.e_pose(), config.e_vel(),
+        config.e_acc());
+  }
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
@@ -45,25 +53,24 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
   float distance_tolerance = config_.tolerance() * config_.target_distance();
-  float angle_tolerance = std::max(0.001, config_.tolerance() * config_.target_orientation());
-
-  // LOG_DEBUG("Current distance: ", distance, ", Distance_error=", distance_error,
-  //           ", Angle_error=", angle_error, ", tolerance_dist=", distance_tolerance, ", an=", angle_tolerance);
+  float angle_tolerance =
+      std::max(0.001, config_.tolerance() * config_.target_orientation());
 
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
     double v = ((tracking_pose.v() * cos(gamma - psi)) -
-                (config_.K_v() * tanh(distance_error))) / cos(psi);
+                (config_.K_v() * tanh(distance_error))) /
+               cos(psi);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
-                  ctrl_limits_.velXParams.maxVel);
+                   ctrl_limits_.velXParams.maxVel);
     followingVel.setVx(v);
     double omega = -tracking_pose.omega() +
                    2.0 * (v * sin(psi) / distance +
-                        tracking_pose.v() * sin(gamma - psi) / distance -
-                        config_.K_omega() * tanh(angle_error));
+                          tracking_pose.v() * sin(gamma - psi) / distance -
+                          config_.K_omega() * tanh(angle_error));
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
-                      ctrl_limits_.omegaParams.maxOmega);
+                       ctrl_limits_.omegaParams.maxOmega);
     followingVel.setOmega(omega);
   }
   return followingVel;
@@ -82,7 +89,8 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
   TrackedPose2D simulated_track = tracking_pose;
   Velocity2D cmd;
 
-  // Simulate following the tracked target for the period til prediction_horizon assuming the target moves with its same current velocity
+  // Simulate following the tracked target for the period til prediction_horizon
+  // assuming the target moves with its same current velocity
   while (step < config_.prediction_horizon()) {
     states.push_back(simulated_state);
     ref_traj.path.add(step,
@@ -91,15 +99,16 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
     cmd = this->getPureTrackingCtrl(simulated_track);
     simulated_state.update(cmd, config_.control_time_step());
     simulated_track.update(config_.control_time_step());
-    if(step < config_.prediction_horizon() -1){
-      ref_traj.velocities.add(step,cmd);
+    if (step < config_.prediction_horizon() - 1) {
+      ref_traj.velocities.add(step, cmd);
     }
 
     step++;
   }
   this->setCurrentState(original_state);
 
-  bool has_collisions = trajSampler->checkStatesFeasibility(states, sensor_points);
+  bool has_collisions =
+      trajSampler->checkStatesFeasibility(states, sensor_points);
 
   LOG_INFO("Found reference traj with collisions: ", has_collisions);
 
@@ -108,13 +117,13 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
 
 template <typename T>
 TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
-                                          const Velocity2D &current_vel,
-                                          const T &sensor_points) {
+                                            const Velocity2D &current_vel,
+                                            const T &sensor_points) {
   Trajectory2D ref_traj;
   bool has_collisions;
   std::tie(ref_traj, has_collisions) =
       this->getTrackingReferenceSegment(tracking_pose, sensor_points);
-  if(!has_collisions){
+  if (!has_collisions) {
     // The tracking sample is collision free -> No need to explore other samples
     TrajSearchResult result;
     result.isTrajFound = true;
@@ -122,23 +131,67 @@ TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
     result.trajectory = ref_traj;
     latest_velocity_command_ = ref_traj.velocities.getFront();
     return result;
-  }
-  else{
+  } else {
     LOG_INFO("USING DWA SAMPLING");
     // The tracking sample has collisions -> use DWA-like sampling and control
     Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
-                                 ref_traj.path.z);
+                                 ref_traj.path.z, config_.prediction_horizon());
     // Set the tracking segment as the reference path
-    // Interpolation of the path is not required as the reference is already created using the robot control time step
+    // Interpolation of the path is not required as the reference is already
+    // created using the robot control time step
     this->setCurrentPath(ref_tracking_path, false);
     return this->computeVelocityCommandsSet(current_vel, sensor_points);
   }
 }
 
+template <typename T>
+Control::TrajSearchResult
+VisionDWA::getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
+                           const Velocity2D &current_vel,
+                           const T &sensor_points) {
+  if (!tracker_) {
+    throw std::invalid_argument(
+        "Tracker is not initialized. Cannot use 'VisionDWA::getTrackingCtrl' "
+        "directly with "
+        "'std::vector<Bbox3D> &detected_boxes' input. Initialize VisionDWA "
+        "with 'use_tracker' = true");
+  }
+  TrajSearchResult result;
+  if (tracker_->trackerInitialized()) {
+    // Update the tracker with the detected boxes
+    tracker_->updateTracking(detected_boxes);
+    auto tracked_state = tracker_->getFilteredTrackedPose2D();
+    if (tracked_state) {
+      // Get the tracking control command
+      return getTrackingCtrl(tracked_state.value(), current_vel, sensor_points);
+    } else {
+      LOG_WARNING("Tracker failed to find target");
+      result.isTrajFound = false;
+      return result;
+    }
+  } else {
+    throw std::runtime_error(
+        "Tracker is not initialized with an initial tracking target. Call "
+        "'VisionDWA::setInitialTracking' first");
+  }
+}
+
+bool VisionDWA::setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+                                   const std::vector<Bbox3D> &detected_boxes) {
+  if (!tracker_) {
+    LOG_ERROR("Tracker is not initialized. Cannot use "
+              "'VisionDWA::setInitialTracking'. Initialize VisionDWA "
+              "with 'use_tracker' = true");
+    return false;
+  }
+  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
+}
+
 // Explicit instantiation for LaserScan
-template Control::TrajSearchResult VisionDWA::getTrackingCtrl<LaserScan>(
-    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
-    const LaserScan &sensor_points);
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<LaserScan>(const TrackedPose2D &tracking_pose,
+                                      const Velocity2D &current_vel,
+                                      const LaserScan &sensor_points);
 
 // Explicit instantiation for PointCloud
 template Control::TrajSearchResult
@@ -146,5 +199,17 @@ VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
     const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
     const std::vector<Path::Point> &sensor_points);
 
+// Explicit instantiation for LaserScan
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<LaserScan>(const std::vector<Bbox3D> &detected_boxes,
+                                      const Velocity2D &current_vel,
+                                      const LaserScan &sensor_points);
+
+// Explicit instantiation for PointCloud
+template Control::TrajSearchResult
+VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
+    const std::vector<Bbox3D> &detected_boxes, const Velocity2D &current_vel,
+    const std::vector<Path::Point> &sensor_points);
+
 } // namespace Control
 } // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index d53b13de..5c627a4a 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -1,5 +1,7 @@
 #include "vision/tracker.h"
+#include "datatypes/control.h"
 #include "datatypes/tracking.h"
+#include <cmath>
 #include <math.h>
 
 namespace Kompass {
@@ -42,7 +44,6 @@ bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   state_vec(4) = bBox.acc[0];
   state_vec(5) = bBox.acc[1];
   stateKalmanFilter_->setInitialState(state_vec);
-  this->tracking_started_ = true;
   return true;
 }
 
@@ -52,7 +53,6 @@ bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
   state_vec(0) = bBox.center[0];
   state_vec(1) = bBox.center[1];
   stateKalmanFilter_->setInitialState(state_vec);
-  this->tracking_started_ = false;
   return true;
 }
 
@@ -78,11 +78,16 @@ bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const in
     state_vec(0) = target_box->center[0];
     state_vec(1) = target_box->center[1];
     stateKalmanFilter_->setInitialState(state_vec);
-    // Target velocity is still unknown
-    this->tracking_started_ = false;
     return true;
 }
 
+bool FeatureBasedBboxTracker::trackerInitialized() const{
+  if(trackedBox_){
+    return true;
+  }
+  return false;
+}
+
 void FeatureBasedBboxTracker::updateTrackedBoxState(){
   Eigen::MatrixXf measurement;
   measurement.resize(6, 1);
@@ -162,6 +167,18 @@ FeatureBasedBboxTracker::getTrackedState() const {
   return std::nullopt;
 }
 
+std::optional<Control::TrackedPose2D>
+FeatureBasedBboxTracker::getFilteredTrackedPose2D() const {
+  if (trackedBox_) {
+    auto state_vec = stateKalmanFilter_->getState().value();
+    // yaw = atan2(vy, vx)
+    auto yaw = atan2(state_vec(3), state_vec(2));
+    return Control::TrackedPose2D(state_vec(0), state_vec(1),
+        yaw, state_vec(2), state_vec(3), trackedBox_->omega());
+  }
+  return std::nullopt;
+}
+
 Eigen::Vector3f FeatureBasedBboxTracker::computePointsStdDev(
     const std::vector<Eigen::Vector3f> &pc_points) const {
   // compute the mean in each direction
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index eb847d32..56904456 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -5,6 +5,8 @@
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <iostream>
 #include <memory>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "json_export.h"
@@ -19,12 +21,18 @@ using namespace Kompass;
 struct VisionDWATestConfig {
   // Sampling configuration
   double timeStep;
-  double predictionHorizon;
-  double controlHorizon;
+  int predictionHorizon;
+  int controlHorizon;
   int maxLinearSamples;
   int maxAngularSamples;
   int maxNumThreads;
 
+  // Detected Boxes
+  std::vector<Bbox3D> detected_boxes;
+  int num_test_boxes = 3;
+  Eigen::Vector2i ref_point_img{150, 150};
+  float boxes_ori = 0.0f;
+
   // Octomap resolution
   double octreeRes;
 
@@ -55,10 +63,11 @@ struct VisionDWATestConfig {
   std::unique_ptr<Control::VisionDWA> controller;
 
   // Constructor to initialize the struct
-  VisionDWATestConfig(const float timeStep, const float predictionHorizon,
-                      const float controlHorizon, const int maxLinearSamples,
+  VisionDWATestConfig(const double timeStep, const int predictionHorizon,
+                      const int controlHorizon, const int maxLinearSamples,
                       const int maxAngularSamples,
                       const std::vector<Path::Point> sensor_points,
+                      const bool use_tracker = true,
                       const float maxVel = 1.0, const float maxOmega = 4.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 5.0,
@@ -84,19 +93,56 @@ struct VisionDWATestConfig {
                              obstacles_distance_weight);
     costWeights.setParameter("smoothness_weight", 0.0);
     costWeights.setParameter("jerk_weight", 0.0);
+    auto config = Control::VisionDWA::VisionDWAConfig();
+    config.setParameter("control_time_step", timeStep);
+    config.setParameter("control_horizon", controlHorizon);
+    config.setParameter("prediction_horizon", predictionHorizon);
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
         robotShapeType, robotDimensions, sensorPositionWRTbody,
-        sensorRotationWRTbody, octreeRes, costWeights);
+        sensorRotationWRTbody, octreeRes, costWeights, maxNumThreads, config,
+        use_tracker);
+
+    // Initialize the detected boxes
+    Bbox3D new_box;
+    new_box.size = {0.5f, 0.5f, 1.0f};
+    detected_boxes.resize(num_test_boxes - 1);
+    for (int i = 0; i < num_test_boxes - 1; ++i) {
+      auto new_box_shift =
+          Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f});
+      auto img_frame_shift =
+          Eigen::Vector2i({float(50 * i), float(50 * i)});
+      new_box.center = new_box_shift;
+      new_box.center_img_frame = img_frame_shift + ref_point_img;
+      new_box.size_img_frame = {25, 25};
+      detected_boxes[i] = new_box;
+    }
   }
 
-  bool run_test(const int numPointsPerTrajectory, std::string pltFileName) {
+  void moveDetectedBoxes() {
+    // Update the detected boxes using the velocity command
+    Eigen::Vector3f target_ref_vel = {float(tracked_vel.vx() * cos(boxes_ori)),
+                                      float(tracked_vel.vx() * sin(boxes_ori)),
+                                      0.0f};
+    boxes_ori += tracked_vel.omega() * timeStep;
+    for (auto &box : detected_boxes) {
+      box.center += target_ref_vel * timeStep;
+    }
+  };
+
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName, bool with_tracker) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
     Control::TrajectoryVelocities2D simulated_velocities(
         numPointsPerTrajectory);
     Control::TrajectoryPath robot_path(numPointsPerTrajectory),
         tracked_path(numPointsPerTrajectory);
     Control::Velocity2D cmd;
+    Control::TrajSearchResult result;
+
+    if(with_tracker){
+      controller->setInitialTracking(
+          ref_point_img(0), ref_point_img(1), detected_boxes);
+    }
 
     int step = 0;
     while (step < numPointsPerTrajectory) {
@@ -105,10 +151,12 @@ struct VisionDWATestConfig {
       tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
       controller->setCurrentState(robotState);
 
-      Control::TrajSearchResult result =
-          controller->getTrackingCtrl(tracked_pose, cmd, cloud);
-      // cmd = controller.getPureTrackingCtrl(tracked_pose);
-      // robotState.update(cmd, timeStep);
+      if(with_tracker){
+        result =
+            controller->getTrackingCtrl(detected_boxes, cmd, cloud);
+      }else{
+        result = controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+      }
 
       if (result.isTrajFound) {
         LOG_INFO(FMAG("STEP: "), step,
@@ -144,6 +192,9 @@ struct VisionDWATestConfig {
       }
 
       tracked_pose.update(timeStep);
+      if(with_tracker){
+        moveDetectedBoxes();
+      }
       step++;
     }
     samples.push_back(simulated_velocities, robot_path);
@@ -178,19 +229,20 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
 
   // Sampling configuration
   double timeStep = 0.1;
-  double predictionHorizon =0.5;
-  double controlHorizon = 0.2;
+  int predictionHorizon = 10;
+  int controlHorizon = 2;
   int maxLinearSamples = 20;
   int maxAngularSamples = 20;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
-  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud);
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud, false);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed = testConfig.run_test(numPointsPerTrajectory, std::string("vision_follower_obstacle_free"));
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_obstacle_free"), false);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -200,8 +252,57 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
 
   // Sampling configuration
   double timeStep = 0.1;
-  double predictionHorizon = 0.5;
-  double controlHorizon = 0.4;
+  int predictionHorizon = 10;
+  int controlHorizon = 2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud, false);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"), false);
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_obs_free) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  int predictionHorizon = 10;
+  int controlHorizon = 2;
+  int maxLinearSamples = 20;
+  int maxAngularSamples = 20;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+
+  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
+                                 maxLinearSamples, maxAngularSamples, cloud,
+                                 true);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_tracker"), true);
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+}
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
+  // Create timer
+  Timer time;
+
+  // Sampling configuration
+  double timeStep = 0.1;
+  int predictionHorizon = 10;
+  int controlHorizon = 2;
   int maxLinearSamples = 20;
   int maxAngularSamples = 20;
 
@@ -209,11 +310,12 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
   VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
-                                 maxLinearSamples, maxAngularSamples, cloud);
+                                 maxLinearSamples, maxAngularSamples, cloud,
+                                 true);
 
   int numPointsPerTrajectory = 100;
 
   bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"));
+      numPointsPerTrajectory, std::string("vision_follower_with_tracker_and_obstacle"), true);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }

From d27ba247c5650e9fe6fb731a83c05928bad12837 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 25 Apr 2025 15:30:53 +0200
Subject: [PATCH 039/118] (fix) Moves templated functions implementation to
 header file

---
 .../kompass_cpp/include/controllers/dwa.h     |  54 ++++++-
 .../include/controllers/vision_dwa.h          | 125 +++++++++++++--
 .../kompass_cpp/src/controllers/dwa.cpp       |  66 --------
 .../src/controllers/vision_dwa.cpp            | 142 +-----------------
 4 files changed, 165 insertions(+), 222 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 2aade719..2ef5aa63 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -109,13 +109,30 @@ class DWA : public Follower {
 
   template <typename T>
   Controller::Result computeVelocityCommand(const Velocity2D &global_vel,
-                                            const T &scan_points);
+                                            const T &scan_points){
+    TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
+    Controller::Result finalResult;
+    if (searchRes.isTrajFound) {
+      finalResult.status = Controller::Result::Status::COMMAND_FOUND;
+      // Get the first command to be applied
+      finalResult.velocity_command = searchRes.trajectory.velocities.getFront();
+      latest_velocity_command_ = finalResult.velocity_command;
+    } else {
+      finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE;
+    }
+    return finalResult;
+                                            };
 
+  template <typename T>
   TrajSearchResult computeVelocityCommandsSet(const Velocity2D &global_vel,
-                                              const LaserScan &scan);
-  TrajSearchResult
-  computeVelocityCommandsSet(const Velocity2D &global_vel,
-                             const std::vector<Path::Point> &cloud);
+                                              const T &scan_points){
+    TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
+    // Update latest velocity command
+    if (searchRes.isTrajFound) {
+      latest_velocity_command_ = searchRes.trajectory.velocities.getFront();
+    }
+    return searchRes;
+                                              };
 
   std::tuple<MatrixXfR, MatrixXfR> getDebuggingSamples() const;
 
@@ -156,7 +173,32 @@ class DWA : public Follower {
    */
   template <typename T>
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
-                                const T &scan_points);
+                                const T &scan_points){
+    // Throw an error if the global path is not set
+    if (!currentPath) {
+      throw std::invalid_argument("Pointer to global path is NULL. Cannot use "
+                                  "DWA local planner without "
+                                  "setting a global path");
+    }
+    // find closest segment to use in cost computation
+    determineTarget();
+
+    // Generate set of valid trajectories in the DW
+    std::unique_ptr<TrajectorySamples2D> samples_ =
+        trajSampler->generateTrajectories(global_vel, currentState,
+                                          scan_points);
+    if (samples_->size() == 0) {
+      return TrajSearchResult();
+    }
+
+    trajCostEvaluator->setPointScan(scan_points, currentState, maxLocalRange_);
+
+    Path::Path trackedRefPathSegment = findTrackedPathSegment();
+
+    // Evaluate the samples and get the sample with the minimum cost
+    return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(),
+                                                   trackedRefPathSegment);
+                                };
 
 private:
   double max_forward_distance_ = 0.0;
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index bbe6893f..795a2ebf 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -2,8 +2,11 @@
 
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
+#include "datatypes/path.h"
 #include "datatypes/tracking.h"
+#include "datatypes/trajectory.h"
 #include "dwa.h"
+#include "utils/logger.h"
 #include "vision/tracker.h"
 #include <Eigen/Dense>
 #include <cmath>
@@ -52,9 +55,9 @@ class VisionDWA : public DWA {
       addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
       addParameter("enable_search", Parameter(false));
       // Kalman Filter parameters
-      addParameter("error_pose", Parameter(0.1, 1e-9, 1e9));
-      addParameter("error_vel", Parameter(0.1, 1e-9, 1e9));
-      addParameter("error_acc", Parameter(0.1, 1e-9, 1e9));
+      addParameter("error_pose", Parameter(0.05, 1e-9, 1e9));
+      addParameter("error_vel", Parameter(0.05, 1e-9, 1e9));
+      addParameter("error_acc", Parameter(0.05, 1e-9, 1e9));
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
     double control_time_step() const {
@@ -95,17 +98,17 @@ class VisionDWA : public DWA {
     double e_acc() const { return getParameter<double>("error_acc"); }
   };
 
-  VisionDWA(const ControlType robotCtrlType,
-            const ControlLimitsParams ctrlLimits, int maxLinearSamples,
-            int maxAngularSamples,
-            const CollisionChecker::ShapeType robotShapeType,
-            const std::vector<float> robotDimensions,
+  VisionDWA(const ControlType &robotCtrlType,
+            const ControlLimitsParams &ctrlLimits, const int maxLinearSamples,
+            const int maxAngularSamples,
+            const CollisionChecker::ShapeType &robotShapeType,
+            const std::vector<float> &robotDimensions,
             const std::array<float, 3> &sensor_position_body,
             const std::array<float, 4> &sensor_rotation_body,
             const double octreeRes,
-            CostEvaluator::TrajectoryCostsWeights costWeights,
+            const CostEvaluator::TrajectoryCostsWeights &costWeights,
             const int maxNumThreads = 1,
-            const VisionDWAConfig config = VisionDWAConfig(),
+            const VisionDWAConfig &config = VisionDWAConfig(),
             const bool use_tracker = true);
 
   // Default Destructor
@@ -121,7 +124,7 @@ class VisionDWA : public DWA {
 
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
-   * sampling
+   * sampling by direct following of the tracked target
    *
    * @tparam T
    * @param tracking_pose
@@ -130,15 +133,44 @@ class VisionDWA : public DWA {
    * @return Control::TrajSearchResult
    */
   template <typename T>
-  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracking_pose,
+  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracked_pose,
                                             const Velocity2D &current_vel,
-                                            const T &sensor_points);
+                                            const T &sensor_points) {
+    LOG_INFO("Tracked pose: ", tracked_pose.x(),
+             tracked_pose.y(), tracked_pose.yaw());
+    Trajectory2D ref_traj;
+    bool has_collisions;
+    std::tie(ref_traj, has_collisions) =
+        this->getTrackingReferenceSegment(tracked_pose, sensor_points);
+    if (!has_collisions) {
+      // The tracking sample is collision free -> No need to explore other
+      // samples
+      TrajSearchResult result;
+      result.isTrajFound = true;
+      result.trajCost = 0.0;
+      result.trajectory = ref_traj;
+      latest_velocity_command_ = ref_traj.velocities.getFront();
+      return result;
+    } else {
+      LOG_INFO("USING DWA SAMPLING");
+      // The tracking sample has collisions -> use DWA-like sampling and control
+      Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
+                                   ref_traj.path.z,
+                                   config_.prediction_horizon());
+      // Set the tracking segment as the reference path
+      // Interpolation of the path is not required as the reference is already
+      // created using the robot control time step
+      this->setCurrentPath(ref_tracking_path, false);
+      return this->computeVelocityCommandsSet(current_vel, sensor_points);
+    }
+  };
+
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
    * sampling by searching a set of given detections
    *
    * @tparam T
-   * @param detected_boxes
+   * @param tracking_pose
    * @param current_vel
    * @param sensor_points
    * @return Control::TrajSearchResult
@@ -146,7 +178,33 @@ class VisionDWA : public DWA {
   template <typename T>
   Control::TrajSearchResult
   getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
-                  const Velocity2D &current_vel, const T &sensor_points);
+                  const Velocity2D &current_vel, const T &sensor_points) {
+    if (!tracker_) {
+      throw std::invalid_argument(
+          "Tracker is not initialized. Cannot use "
+          "'VisionDWA::getTrackingCtrl' "
+          "directly with "
+          "'std::vector<Bbox3D> &detected_boxes' input. Initialize VisionDWA "
+          "with 'use_tracker' = true");
+    }
+    if (tracker_->trackerInitialized()) {
+      // Update the tracker with the detected boxes
+      tracker_->updateTracking(detected_boxes);
+      auto tracked_pose = tracker_->getFilteredTrackedPose2D();
+      if (tracked_pose) {
+        return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
+                                     sensor_points);
+      } else {
+        LOG_WARNING("Tracker failed to find the target");
+        // Return false for trajectory found
+        return TrajSearchResult();
+      }
+    } else {
+      throw std::runtime_error(
+          "Tracker is not initialized with an initial tracking target. Call "
+          "'VisionDWA::setInitialTracking' first");
+    }
+  };
 
   /**
    * @brief Set the initial image position of the target to be tracked
@@ -177,7 +235,42 @@ class VisionDWA : public DWA {
   template <typename T>
   std::tuple<Trajectory2D, bool>
   getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
-                              const T &sensor_points);
+                              const T &sensor_points){
+    int step = 0;
+
+    Trajectory2D ref_traj(config_.prediction_horizon());
+    std::vector<Path::State> states;
+    Path::State simulated_state = currentState;
+    Path::State original_state = currentState;
+    TrackedPose2D simulated_track = tracking_pose;
+    Velocity2D cmd;
+
+    // Simulate following the tracked target for the period til
+    // prediction_horizon assuming the target moves with its same current
+    // velocity
+    while (step < config_.prediction_horizon()) {
+      states.push_back(simulated_state);
+      ref_traj.path.add(step,
+                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
+      this->setCurrentState(simulated_state);
+      cmd = this->getPureTrackingCtrl(simulated_track);
+      simulated_state.update(cmd, config_.control_time_step());
+      simulated_track.update(config_.control_time_step());
+      if (step < config_.prediction_horizon() - 1) {
+        ref_traj.velocities.add(step, cmd);
+      }
+
+      step++;
+    }
+    this->setCurrentState(original_state);
+
+    bool has_collisions =
+        trajSampler->checkStatesFeasibility(states, sensor_points);
+
+    LOG_INFO("Found reference traj with collisions: ", has_collisions);
+
+    return std::make_tuple(ref_traj, has_collisions);
+                              };
   //
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 27d841f4..a63c62b1 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -170,72 +170,6 @@ Path::Path DWA::findTrackedPathSegment() {
   }
 }
 
-template <typename T>
-TrajSearchResult DWA::findBestPath(const Velocity2D &global_vel,
-                                   const T &scan_points) {
-  // Throw an error if the global path is not set
-  if (!currentPath) {
-    throw std::invalid_argument(
-        "Pointer to global path is NULL. Cannot use DWA local planner without "
-        "setting a global path");
-  }
-  // find closest segment to use in cost computation
-  determineTarget();
-
-  // Generate set of valid trajectories in the DW
-  std::unique_ptr<TrajectorySamples2D> samples_ =
-      trajSampler->generateTrajectories(global_vel, currentState, scan_points);
-  if (samples_->size() == 0) {
-    return TrajSearchResult();
-  }
-
-  trajCostEvaluator->setPointScan(scan_points, currentState, maxLocalRange_);
-
-  Path::Path trackedRefPathSegment = findTrackedPathSegment();
-
-  // Evaluate the samples and get the sample with the minimum cost
-  return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(),
-                                                 trackedRefPathSegment);
-}
-
-template <typename T>
-Controller::Result DWA::computeVelocityCommand(const Velocity2D &global_vel,
-                                               const T &scan_points) {
-  TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
-  Controller::Result finalResult;
-  if (searchRes.isTrajFound) {
-    finalResult.status = Controller::Result::Status::COMMAND_FOUND;
-    // Get the first command to be applied
-    finalResult.velocity_command = searchRes.trajectory.velocities.getFront();
-    latest_velocity_command_ = finalResult.velocity_command;
-  } else {
-    finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE;
-  }
-  return finalResult;
-}
-
-TrajSearchResult DWA::computeVelocityCommandsSet(const Velocity2D &global_vel,
-                                                 const LaserScan &scan) {
-  TrajSearchResult searchRes = findBestPath(global_vel, scan);
-  // Update latest velocity command
-  if (searchRes.isTrajFound) {
-    latest_velocity_command_ = searchRes.trajectory.velocities.getFront();
-  }
-  return searchRes;
-}
-
-TrajSearchResult
-DWA::computeVelocityCommandsSet(const Velocity2D &global_vel,
-                                const std::vector<Path::Point> &cloud) {
-
-  TrajSearchResult searchRes = findBestPath(global_vel, cloud);
-  // Update latest velocity command
-  if (searchRes.isTrajFound) {
-    latest_velocity_command_ = searchRes.trajectory.velocities.getFront();
-  }
-  return searchRes;
-}
-
 std::tuple<MatrixXfR, MatrixXfR> DWA::getDebuggingSamples() const {
   if (debuggingSamples_ == nullptr) {
     throw std::invalid_argument("No debugging samples are available");
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 899fee3d..10fe0036 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -1,28 +1,26 @@
 #include "controllers/vision_dwa.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
-#include "datatypes/trajectory.h"
 #include "utils/angles.h"
 #include "utils/logger.h"
 #include <algorithm>
 #include <cmath>
-#include <stdexcept>
-#include <tuple>
+#include <memory>
 #include <vector>
 
 namespace Kompass {
 namespace Control {
 
-VisionDWA::VisionDWA(const ControlType robotCtrlType,
-                     const ControlLimitsParams ctrlLimits, int maxLinearSamples,
-                     int maxAngularSamples,
-                     const CollisionChecker::ShapeType robotShapeType,
-                     const std::vector<float> robotDimensions,
+VisionDWA::VisionDWA(const ControlType &robotCtrlType,
+                     const ControlLimitsParams &ctrlLimits,
+                     const int maxLinearSamples, const int maxAngularSamples,
+                     const CollisionChecker::ShapeType &robotShapeType,
+                     const std::vector<float> &robotDimensions,
                      const std::array<float, 3> &sensor_position_body,
                      const std::array<float, 4> &sensor_rotation_body,
                      const double octreeRes,
-                     CostEvaluator::TrajectoryCostsWeights costWeights,
-                     const int maxNumThreads, const VisionDWAConfig config,
+                     const CostEvaluator::TrajectoryCostsWeights &costWeights,
+                     const int maxNumThreads, const VisionDWAConfig &config,
                      const bool use_tracker)
     : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
           config.prediction_horizon(), config.control_horizon(),
@@ -76,106 +74,6 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   return followingVel;
 }
 
-template <typename T>
-std::tuple<Trajectory2D, bool>
-VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
-                                       const T &sensor_points) {
-  int step = 0;
-
-  Trajectory2D ref_traj(config_.prediction_horizon());
-  std::vector<Path::State> states;
-  Path::State simulated_state = currentState;
-  Path::State original_state = currentState;
-  TrackedPose2D simulated_track = tracking_pose;
-  Velocity2D cmd;
-
-  // Simulate following the tracked target for the period til prediction_horizon
-  // assuming the target moves with its same current velocity
-  while (step < config_.prediction_horizon()) {
-    states.push_back(simulated_state);
-    ref_traj.path.add(step,
-                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
-    this->setCurrentState(simulated_state);
-    cmd = this->getPureTrackingCtrl(simulated_track);
-    simulated_state.update(cmd, config_.control_time_step());
-    simulated_track.update(config_.control_time_step());
-    if (step < config_.prediction_horizon() - 1) {
-      ref_traj.velocities.add(step, cmd);
-    }
-
-    step++;
-  }
-  this->setCurrentState(original_state);
-
-  bool has_collisions =
-      trajSampler->checkStatesFeasibility(states, sensor_points);
-
-  LOG_INFO("Found reference traj with collisions: ", has_collisions);
-
-  return std::make_tuple(ref_traj, has_collisions);
-}
-
-template <typename T>
-TrajSearchResult VisionDWA::getTrackingCtrl(const TrackedPose2D &tracking_pose,
-                                            const Velocity2D &current_vel,
-                                            const T &sensor_points) {
-  Trajectory2D ref_traj;
-  bool has_collisions;
-  std::tie(ref_traj, has_collisions) =
-      this->getTrackingReferenceSegment(tracking_pose, sensor_points);
-  if (!has_collisions) {
-    // The tracking sample is collision free -> No need to explore other samples
-    TrajSearchResult result;
-    result.isTrajFound = true;
-    result.trajCost = 0.0;
-    result.trajectory = ref_traj;
-    latest_velocity_command_ = ref_traj.velocities.getFront();
-    return result;
-  } else {
-    LOG_INFO("USING DWA SAMPLING");
-    // The tracking sample has collisions -> use DWA-like sampling and control
-    Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
-                                 ref_traj.path.z, config_.prediction_horizon());
-    // Set the tracking segment as the reference path
-    // Interpolation of the path is not required as the reference is already
-    // created using the robot control time step
-    this->setCurrentPath(ref_tracking_path, false);
-    return this->computeVelocityCommandsSet(current_vel, sensor_points);
-  }
-}
-
-template <typename T>
-Control::TrajSearchResult
-VisionDWA::getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
-                           const Velocity2D &current_vel,
-                           const T &sensor_points) {
-  if (!tracker_) {
-    throw std::invalid_argument(
-        "Tracker is not initialized. Cannot use 'VisionDWA::getTrackingCtrl' "
-        "directly with "
-        "'std::vector<Bbox3D> &detected_boxes' input. Initialize VisionDWA "
-        "with 'use_tracker' = true");
-  }
-  TrajSearchResult result;
-  if (tracker_->trackerInitialized()) {
-    // Update the tracker with the detected boxes
-    tracker_->updateTracking(detected_boxes);
-    auto tracked_state = tracker_->getFilteredTrackedPose2D();
-    if (tracked_state) {
-      // Get the tracking control command
-      return getTrackingCtrl(tracked_state.value(), current_vel, sensor_points);
-    } else {
-      LOG_WARNING("Tracker failed to find target");
-      result.isTrajFound = false;
-      return result;
-    }
-  } else {
-    throw std::runtime_error(
-        "Tracker is not initialized with an initial tracking target. Call "
-        "'VisionDWA::setInitialTracking' first");
-  }
-}
-
 bool VisionDWA::setInitialTracking(const int &pose_x_img, const int &pose_y_img,
                                    const std::vector<Bbox3D> &detected_boxes) {
   if (!tracker_) {
@@ -187,29 +85,5 @@ bool VisionDWA::setInitialTracking(const int &pose_x_img, const int &pose_y_img,
   return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
 }
 
-// Explicit instantiation for LaserScan
-template Control::TrajSearchResult
-VisionDWA::getTrackingCtrl<LaserScan>(const TrackedPose2D &tracking_pose,
-                                      const Velocity2D &current_vel,
-                                      const LaserScan &sensor_points);
-
-// Explicit instantiation for PointCloud
-template Control::TrajSearchResult
-VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
-    const TrackedPose2D &tracking_pose, const Velocity2D &current_vel,
-    const std::vector<Path::Point> &sensor_points);
-
-// Explicit instantiation for LaserScan
-template Control::TrajSearchResult
-VisionDWA::getTrackingCtrl<LaserScan>(const std::vector<Bbox3D> &detected_boxes,
-                                      const Velocity2D &current_vel,
-                                      const LaserScan &sensor_points);
-
-// Explicit instantiation for PointCloud
-template Control::TrajSearchResult
-VisionDWA::getTrackingCtrl<std::vector<Path::Point>>(
-    const std::vector<Bbox3D> &detected_boxes, const Velocity2D &current_vel,
-    const std::vector<Path::Point> &sensor_points);
-
 } // namespace Control
 } // namespace Kompass

From 6ea6d14cbcdda4651134f516afd2499c4d6a7545 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 25 Apr 2025 15:31:32 +0200
Subject: [PATCH 040/118] (feature) Adds orientation to tracked state in vision
 tracker

---
 .../kompass_cpp/include/datatypes/tracking.h  | 28 +++++--
 .../kompass_cpp/include/vision/tracker.h      |  2 +
 .../kompass_cpp/src/vision/tracker.cpp        | 75 +++++++++++--------
 3 files changed, 67 insertions(+), 38 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 084ef57f..f6dbc3d8 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -19,6 +19,14 @@ struct Bbox3D {
   Bbox3D(const Bbox3D &box)
       : center(box.center), size(box.size), center_img_frame(box.center_img_frame), size_img_frame(box.size_img_frame), pc_points(box.pc_points){};
 
+  Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
+         const Eigen::Vector2i center_img_frame,
+         const Eigen::Vector2i size_img_frame,
+         std::vector<Eigen::Vector3f> pc_points = {})
+      : center(center), size(size),
+        center_img_frame(center_img_frame),
+        size_img_frame(size_img_frame), pc_points(pc_points){};
+
   Eigen::Vector2f getXLimitsImg() const {
     return {
       center_img_frame.x() - (size_img_frame.x() / 2),
@@ -37,7 +45,7 @@ struct TrackedBbox3D {
   Eigen::Vector3f vel = {0.0, 0.0, 0.0};
   Eigen::Vector3f acc = {0.0, 0.0, 0.0};
   int unique_id = 0;
-  Eigen::Vector2f yaw_yaw_diff = {0.0, 0.0};
+  Eigen::Vector3f yaw_vec = {0.0, 0.0, 0.0}; // To track yaw, omega and angular acc
 
   TrackedBbox3D(const Bbox3D& box): box(box) {};
 
@@ -62,8 +70,10 @@ struct TrackedBbox3D {
     this->acc = {state_vector(4), state_vector(5), 0.0f};
     auto new_yaw = std::atan2(this->vel(1), this->vel(0));
     // Orientation difference
-    this->yaw_yaw_diff(1) = (this->yaw_yaw_diff(0) - new_yaw) / time_step;
-    this->yaw_yaw_diff(0) = new_yaw;
+    auto new_omega = (this->yaw_vec(0) - new_yaw) / time_step;
+    this->yaw_vec(2) = (this->yaw_vec(1) - new_omega) / time_step;
+    this->yaw_vec(1) = new_omega;
+    this->yaw_vec(0) = new_yaw;
   };
 
   void updateFromNewDetection(const Bbox3D& new_box, const float time_step){
@@ -75,8 +85,10 @@ struct TrackedBbox3D {
     // New orientation (yaw) based on new velocity
     auto new_yaw = std::atan2(new_vel(1), new_vel(0));
     // Orientation difference
-    this->yaw_yaw_diff(1) = (this->yaw_yaw_diff(0) - new_yaw) / time_step;
-    this->yaw_yaw_diff(0) = new_yaw;
+    auto new_omega = (this->yaw_vec(0) - new_yaw) / time_step;
+    this->yaw_vec(2) = (this->yaw_vec(1) - new_omega) / time_step;
+    this->yaw_vec(1) = new_omega;
+    this->yaw_vec(0) = new_yaw;
     this->vel = new_vel;
     this->acc = new_acc;
   }
@@ -102,14 +114,14 @@ struct TrackedBbox3D {
 
   float y() const { return box.center.y(); };
 
-  float yaw() const { return this->yaw_yaw_diff(0); };
+  float yaw() const { return this->yaw_vec(0); };
 
-  float omega() const { return yaw_yaw_diff(1); };
+  float omega() const { return yaw_vec(1); };
 
   void update(const float timeStep) {
     box.center(0) += vel.x() * timeStep;
     box.center(1) += vel.y() * timeStep;
-    this->yaw_yaw_diff(0) += this->yaw_yaw_diff(1) * timeStep;
+    this->yaw_vec(0) += this->yaw_vec(1) * timeStep;
   }
 
   float distance(const float x, const float y, const float z = 0.0) const {
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index bd5734fe..0df236ca 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -15,6 +15,8 @@ class FeatureBasedBboxTracker{
     public:
       FeatureBasedBboxTracker(const float& time_step, const float& e_pos, const float& e_vel, const float& e_acc);
 
+      static constexpr int StateSize = 9;
+
       using FeaturesVector = Eigen::Vector<float, 9>;
 
       bool setInitialTracking(const TrackedBbox3D& bBox);
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 5c627a4a..43dead0a 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -13,43 +13,56 @@ FeatureBasedBboxTracker::FeatureBasedBboxTracker(
   timeStep_ = time_step;
   // Setup Kalman filter matrices
   Eigen::MatrixXf A;
-  A.resize(6, 6);
-
-  A << 1, 0, time_step, 0, 0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0,
-      0.5 * pow(time_step, 2), 0, 0, 1, 0, time_step, 0, 0, 0, 0, 1, 0,
-      time_step, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
-
-  Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 1);
-  Eigen::MatrixXf H = Eigen::MatrixXf::Identity(6, 6);
-  Eigen::MatrixXf Err = Eigen::MatrixXf::Identity(6, 6);
+  A.resize(StateSize, StateSize);
+
+  A << 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0,
+       0, 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0,
+       0, 0, 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2),
+       0, 0, 0, 1, 0, 0, time_step, 0, 0,
+       0, 0, 0, 0, 1, 0, 0, time_step, 0,
+       0, 0, 0, 0, 0, 1, 0, 0, time_step,
+       0, 0, 0, 0, 0, 0, 1, 0, 0,
+       0, 0, 0, 0, 0, 0, 0, 1, 0,
+       0, 0, 0, 0, 0, 0, 0, 0, 0;
+
+  Eigen::MatrixXf B = Eigen::MatrixXf::Zero(StateSize, 1);
+  Eigen::MatrixXf H = Eigen::MatrixXf::Identity(StateSize, StateSize);
+  Eigen::MatrixXf Err = Eigen::MatrixXf::Identity(StateSize, StateSize);
   Err(0, 0) *= e_pos;
   Err(1, 1) *= e_pos;
-  Err(2, 2) *= e_vel;
+  Err(2, 2) *= e_pos;
   Err(3, 3) *= e_vel;
-  Err(4, 4) *= e_acc;
-  Err(5, 5) *= e_acc;
+  Err(4, 4) *= e_vel;
+  Err(5, 5) *= e_vel;
+  Err(6, 6) *= e_acc;
+  Err(7, 7) *= e_acc;
+  Err(8, 8) *= e_acc;
 
-  stateKalmanFilter_ = std::make_unique<LinearSSKalmanFilter>(6, 1);
+  stateKalmanFilter_ = std::make_unique<LinearSSKalmanFilter>(StateSize, 1);
   stateKalmanFilter_->setup(A, B, Err, H, Err);
 }
 
 bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
   Eigen::VectorXf state_vec;
-  state_vec.resize(6);
-  state_vec(0) = bBox.box.center[0];
-  state_vec(1) = bBox.box.center[1];
-  state_vec(2) = bBox.vel[0];
-  state_vec(3) = bBox.vel[1];
-  state_vec(4) = bBox.acc[0];
-  state_vec(5) = bBox.acc[1];
+  state_vec.resize(StateSize);
+  state_vec(0) = bBox.box.center[0];  // x
+  state_vec(1) = bBox.box.center[1];  // y
+  state_vec(2) = bBox.yaw_vec(0);  // yaw
+  state_vec(3) = bBox.vel[0];   // vx
+  state_vec(4) = bBox.vel[1];  // vy
+  state_vec(5) = bBox.yaw_vec(1);  // omega
+  state_vec(6) = bBox.acc[0];   // ax
+  state_vec(7) = bBox.acc[1];   // ay
+  state_vec(8) = bBox.yaw_vec(2);   // a_yaw
   stateKalmanFilter_->setInitialState(state_vec);
   return true;
 }
 
 bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
-  Eigen::Matrix<float, 6, 1> state_vec = Eigen::Matrix<float, 6, 1>::Zero();
+  Eigen::Matrix<float, StateSize, 1> state_vec =
+      Eigen::Matrix<float, StateSize, 1>::Zero();
   state_vec(0) = bBox.center[0];
   state_vec(1) = bBox.center[1];
   stateKalmanFilter_->setInitialState(state_vec);
@@ -74,7 +87,8 @@ bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const in
       return false;
     }
     trackedBox_ = std::make_unique<TrackedBbox3D>(*target_box);
-    Eigen::Vector<float, 6> state_vec = Eigen::Vector<float, 6>::Zero();
+    Eigen::Vector<float, StateSize> state_vec =
+        Eigen::Vector<float, StateSize>::Zero();
     state_vec(0) = target_box->center[0];
     state_vec(1) = target_box->center[1];
     stateKalmanFilter_->setInitialState(state_vec);
@@ -90,13 +104,16 @@ bool FeatureBasedBboxTracker::trackerInitialized() const{
 
 void FeatureBasedBboxTracker::updateTrackedBoxState(){
   Eigen::MatrixXf measurement;
-  measurement.resize(6, 1);
+  measurement.resize(StateSize, 1);
   measurement(0) = trackedBox_->box.center.x();
   measurement(1) = trackedBox_->box.center.y();
-  measurement(2) = trackedBox_->vel.x();
-  measurement(3) = trackedBox_->vel.y();
-  measurement(4) = trackedBox_->acc.x();
-  measurement(5) = trackedBox_->acc.y();
+  measurement(2) = trackedBox_->yaw_vec(0);
+  measurement(3) = trackedBox_->vel.x();
+  measurement(4) = trackedBox_->vel.y();
+  measurement(5) = trackedBox_->yaw_vec(1);
+  measurement(6) = trackedBox_->acc.x();
+  measurement(7) = trackedBox_->acc.y();
+  measurement(8) = trackedBox_->yaw_vec(2);
   stateKalmanFilter_->estimate(measurement);
 }
 
@@ -171,10 +188,8 @@ std::optional<Control::TrackedPose2D>
 FeatureBasedBboxTracker::getFilteredTrackedPose2D() const {
   if (trackedBox_) {
     auto state_vec = stateKalmanFilter_->getState().value();
-    // yaw = atan2(vy, vx)
-    auto yaw = atan2(state_vec(3), state_vec(2));
     return Control::TrackedPose2D(state_vec(0), state_vec(1),
-        yaw, state_vec(2), state_vec(3), trackedBox_->omega());
+        state_vec(2), state_vec(3), state_vec(4), state_vec(5));
   }
   return std::nullopt;
 }

From d4a4ac3461069fb0b0ea94dd87c4c431e05790f7 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 25 Apr 2025 15:32:19 +0200
Subject: [PATCH 041/118] (fix) Updates test

---
 src/kompass_cpp/tests/vision_dwa_test.cpp | 106 +++++++++-------------
 1 file changed, 43 insertions(+), 63 deletions(-)

diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 56904456..c6f18356 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -5,8 +5,6 @@
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
-#include <Eigen/src/Core/Matrix.h>
-#include <iostream>
 #include <memory>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "json_export.h"
@@ -63,11 +61,12 @@ struct VisionDWATestConfig {
   std::unique_ptr<Control::VisionDWA> controller;
 
   // Constructor to initialize the struct
-  VisionDWATestConfig(const double timeStep, const int predictionHorizon,
-                      const int controlHorizon, const int maxLinearSamples,
-                      const int maxAngularSamples,
-                      const std::vector<Path::Point> sensor_points,
-                      const bool use_tracker = true,
+  VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
+                      const bool use_tracker, const double timeStep = 0.1,
+                      const int predictionHorizon = 10,
+                      const int controlHorizon = 2,
+                      const int maxLinearSamples = 20,
+                      const int maxAngularSamples = 20,
                       const float maxVel = 1.0, const float maxOmega = 4.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 5.0,
@@ -106,12 +105,11 @@ struct VisionDWATestConfig {
     // Initialize the detected boxes
     Bbox3D new_box;
     new_box.size = {0.5f, 0.5f, 1.0f};
-    detected_boxes.resize(num_test_boxes - 1);
-    for (int i = 0; i < num_test_boxes - 1; ++i) {
+    detected_boxes.resize(num_test_boxes);
+    for (int i = 0; i < num_test_boxes; ++i) {
       auto new_box_shift =
           Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f});
-      auto img_frame_shift =
-          Eigen::Vector2i({float(50 * i), float(50 * i)});
+      auto img_frame_shift = Eigen::Vector2i({float(50 * i), float(50 * i)});
       new_box.center = new_box_shift;
       new_box.center_img_frame = img_frame_shift + ref_point_img;
       new_box.size_img_frame = {25, 25};
@@ -130,7 +128,8 @@ struct VisionDWATestConfig {
     }
   };
 
-  bool run_test(const int numPointsPerTrajectory, std::string pltFileName, bool with_tracker) {
+  bool run_test(const int numPointsPerTrajectory, std::string pltFileName,
+                bool with_tracker) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
     Control::TrajectoryVelocities2D simulated_velocities(
         numPointsPerTrajectory);
@@ -139,9 +138,9 @@ struct VisionDWATestConfig {
     Control::Velocity2D cmd;
     Control::TrajSearchResult result;
 
-    if(with_tracker){
-      controller->setInitialTracking(
-          ref_point_img(0), ref_point_img(1), detected_boxes);
+    if (with_tracker) {
+      controller->setInitialTracking(ref_point_img(0), ref_point_img(1),
+                                     detected_boxes);
     }
 
     int step = 0;
@@ -150,20 +149,22 @@ struct VisionDWATestConfig {
       robot_path.add(step, point);
       tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
       controller->setCurrentState(robotState);
+      LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(), ", ", tracked_pose.yaw());
+      LOG_INFO("Robot at: ", point.x(), ", ", point.y());
 
-      if(with_tracker){
-        result =
-            controller->getTrackingCtrl(detected_boxes, cmd, cloud);
-      }else{
+      if (with_tracker) {
+        result = controller->getTrackingCtrl(detected_boxes, cmd, cloud);
+      } else {
         result = controller->getTrackingCtrl(tracked_pose, cmd, cloud);
       }
 
       if (result.isTrajFound) {
         LOG_INFO(FMAG("STEP: "), step,
                  FMAG(", Found best trajectory with cost: "), result.trajCost);
-        if (controller->getLinearVelocityCmdX() > controlLimits.velXParams.maxVel) {
-          LOG_ERROR(BOLD(FRED("Vx is larger than max vel: ")),
-                    KRED, controller->getLinearVelocityCmdX(), RST,
+        if (controller->getLinearVelocityCmdX() >
+            controlLimits.velXParams.maxVel) {
+          LOG_ERROR(BOLD(FRED("Vx is larger than max vel: ")), KRED,
+                    controller->getLinearVelocityCmdX(), RST,
                     BOLD(FRED(", Vx: ")), KRED, controlLimits.velXParams.maxVel,
                     RST);
           return false;
@@ -192,7 +193,7 @@ struct VisionDWATestConfig {
       }
 
       tracked_pose.update(timeStep);
-      if(with_tracker){
+      if (with_tracker) {
         moveDetectedBoxes();
       }
       step++;
@@ -200,7 +201,6 @@ struct VisionDWATestConfig {
     samples.push_back(simulated_velocities, robot_path);
     samples.push_back(simulated_velocities, tracked_path);
 
-
     // Plot the trajectories (Save to json then run python script for plotting)
     boost::filesystem::path executablePath = boost::dll::program_location();
     std::string file_location = executablePath.parent_path().string();
@@ -227,22 +227,18 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
   // Create timer
   Timer time;
 
-  // Sampling configuration
-  double timeStep = 0.1;
-  int predictionHorizon = 10;
-  int controlHorizon = 2;
-  int maxLinearSamples = 20;
-  int maxAngularSamples = 20;
+  bool use_tracker = false;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
-  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon, maxLinearSamples, maxAngularSamples, cloud, false);
+  VisionDWATestConfig testConfig(cloud, use_tracker);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_obstacle_free"), false);
+  bool test_passed =
+      testConfig.run_test(numPointsPerTrajectory,
+                          std::string("vision_follower_obstacle_free"), use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -250,23 +246,18 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
   // Create timer
   Timer time;
 
-  // Sampling configuration
-  double timeStep = 0.1;
-  int predictionHorizon = 10;
-  int controlHorizon = 2;
-  int maxLinearSamples = 20;
-  int maxAngularSamples = 20;
+  bool use_tracker = false;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
-  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
-                                 maxLinearSamples, maxAngularSamples, cloud, false);
+  VisionDWATestConfig testConfig(cloud, use_tracker);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"), false);
+  bool test_passed =
+      testConfig.run_test(numPointsPerTrajectory,
+                          std::string("vision_follower_with_obstacle"), use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -275,23 +266,18 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_obs_free) {
   Timer time;
 
   // Sampling configuration
-  double timeStep = 0.1;
-  int predictionHorizon = 10;
-  int controlHorizon = 2;
-  int maxLinearSamples = 20;
-  int maxAngularSamples = 20;
+  bool use_tracker = true;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
-  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
-                                 maxLinearSamples, maxAngularSamples, cloud,
-                                 true);
+  VisionDWATestConfig testConfig(cloud, true);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_tracker"), true);
+  bool test_passed =
+      testConfig.run_test(numPointsPerTrajectory,
+                          std::string("vision_follower_with_tracker"), use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -299,23 +285,17 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
   // Create timer
   Timer time;
 
-  // Sampling configuration
-  double timeStep = 0.1;
-  int predictionHorizon = 10;
-  int controlHorizon = 2;
-  int maxLinearSamples = 20;
-  int maxAngularSamples = 20;
+  bool use_tracker = true;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
-  VisionDWATestConfig testConfig(timeStep, predictionHorizon, controlHorizon,
-                                 maxLinearSamples, maxAngularSamples, cloud,
-                                 true);
+  VisionDWATestConfig testConfig(cloud, use_tracker);
 
   int numPointsPerTrajectory = 100;
 
   bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_tracker_and_obstacle"), true);
+      numPointsPerTrajectory,
+      std::string("vision_follower_with_tracker_and_obstacle"), use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }

From 7fac3bc769a2f7aa3e9f3538e2930ab5c8881224 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 25 Apr 2025 15:32:38 +0200
Subject: [PATCH 042/118] (feature) Adds VisionDWA python bindings

---
 src/kompass_cpp/bindings/bindings_control.cpp | 27 +++++++++++++++++++
 src/kompass_cpp/bindings/bindings_types.cpp   | 24 +++++++++++++++++
 2 files changed, 51 insertions(+)

diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index ec367184..3aed271a 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -8,6 +8,7 @@
 #include "controllers/dwa.h"
 #include "controllers/follower.h"
 #include "controllers/pid.h"
+#include "controllers/vision_dwa.h"
 #include "controllers/stanley.h"
 #include "controllers/vision_follower.h"
 #include "datatypes/control.h"
@@ -232,4 +233,30 @@ void bindings_control(py::module_ &m) {
       .def("reset_target", &Control::VisionFollower::resetTarget)
       .def("get_ctrl", &Control::VisionFollower::getCtrl)
       .def("run", &Control::VisionFollower::run);
+
+  // Vision DWA
+  py::class_<Control::VisionDWA::VisionDWAConfig,
+             Control::Controller::ControllerParameters>(
+      m_control, "VisionDWAParameters")
+      .def(py::init<>());
+
+  py::class_<Control::VisionDWA, Control::DWA>(m_control, "VisionDWA")
+      .def(
+          py::init<const Control::ControlType&, const Control::ControlLimitsParams&, const int, const int,
+                   const CollisionChecker::ShapeType&, const std::vector<float> &,
+                   const std::array<float, 3> &, const std::array<float, 4> &,
+                   const double, const Control::CostEvaluator::TrajectoryCostsWeights&, const int,
+                   const Control::VisionDWA::VisionDWAConfig &, const bool>())
+      .def("get_pure_tracking_ctrl", &Control::VisionDWA::getPureTrackingCtrl)
+      .def("get_tracking_ctrl",
+           py::overload_cast<const std::vector<Bbox3D> &,
+                             const Control::Velocity2D &,
+                             const std::vector<Eigen::Vector3f> &>(
+               &Control::VisionDWA::getTrackingCtrl<
+                   std::vector<Eigen::Vector3f>>))
+    .def("get_tracking_ctrl",
+        py::overload_cast<const std::vector<Bbox3D> &,
+                            const Control::Velocity2D &,
+                            const Control::LaserScan &>(
+            &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>));
 }
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 554dc8a2..bea1d265 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -4,11 +4,13 @@
 #include <nanobind/stl/array.h>
 #include <nanobind/stl/vector.h>
 #include <nanobind/stl/string.h>
+#include <vector>
 
 #include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
+#include "datatypes/tracking.h"
 #include "utils/collision_check.h"
 
 namespace py = nanobind;
@@ -24,6 +26,7 @@ std::string printControlCmd(const Control::Velocity2D &velocity_command) {
 void bindings_types(py::module_ &m) {
   auto m_types = m.def_submodule("types", "KOMPASS CPP data types module");
 
+  // Path types
   py::enum_<Path::InterpolationType>(m_types, "PathInterpolationType")
       .value("LINEAR", Path::InterpolationType::LINEAR)
       .value("CUBIC_SPLINE", Path::InterpolationType::CUBIC_SPLINE)
@@ -128,4 +131,25 @@ py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
     .def_rw("img_height", &Control::VisionFollower::TrackingData::img_height)
     .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy)
     .def_rw("depth", &Control::VisionFollower::TrackingData::depth);
+
+py::class_<Bbox3D>(m_types, "Bbox3D")
+    .def(py::init<>())
+    .def(py::init<const Bbox3D &>())
+    .def(py::init<Eigen::Vector3f &, Eigen::Vector3f &, Eigen::Vector2i &,
+                  Eigen::Vector2i &, std::vector<Eigen::Vector3f> &>(),
+         py::arg("center"), py::arg("size"), py::arg("center_img_frame"),
+         py::arg("size_img_frame"), py::arg("pc_points") = py::list())
+    .def_rw("center", &Bbox3D::center)
+    .def_rw("size", &Bbox3D::size)
+    .def_rw("center_img_frame", &Bbox3D::center_img_frame)
+    .def_rw("size_img_frame", &Bbox3D::size_img_frame)
+    .def_rw("pc_points", &Bbox3D::pc_points);
+
+py::class_<TrackedBbox3D>(m_types, "TrackedBbox3D")
+    .def(py::init<const Bbox3D &>())
+    .def_rw("box", &TrackedBbox3D::box)
+    .def_rw("vel", &TrackedBbox3D::vel)
+    .def_rw("acc", &TrackedBbox3D::acc)
+    .def_rw("unique_id", &TrackedBbox3D::unique_id)
+    .def("updateFromNewDetection", &TrackedBbox3D::updateFromNewDetection);
 }

From 2d456f026d393b13cac8cf7e36e09446286e435c Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 25 Apr 2025 18:16:15 +0200
Subject: [PATCH 043/118] (feature) Updates VisionDWA bindings and adds python
 test

---
 src/kompass_core/control/__init__.py          |   7 +
 src/kompass_core/control/vision_dwa.py        |  97 ++++++++++
 src/kompass_cpp/bindings/bindings_control.cpp |  48 +++--
 src/kompass_cpp/bindings/bindings_types.cpp   |  14 ++
 .../src/controllers/vision_dwa.cpp            |  15 +-
 tests/test_controllers.py                     | 178 ++++++++++++++++--
 6 files changed, 320 insertions(+), 39 deletions(-)
 create mode 100644 src/kompass_core/control/vision_dwa.py

diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index b411408a..3f99af13 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -11,6 +11,9 @@
 from .dwa import DWA, DWAConfig
 from .stanley import StanleyConfig, Stanley
 from .vision_follower import VisionFollower, VisionFollowerConfig
+from .vision_dwa import VisionDWAConfig
+from kompass_cpp.types import TrackedPose2D, Bbox3D
+from kompass_cpp.control import VisionDWA
 
 ControllerType = FollowerTemplate
 
@@ -102,4 +105,8 @@ class ControllersID(StrEnum):
     "TrajectoryCostsWeights",
     "VisionFollower",
     "VisionFollowerConfig",
+    "VisionDWAConfig",
+    "VisionDWA",
+    "TrackedPose2D",
+    "Bbox3D",
 ]
diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
new file mode 100644
index 00000000..5227f457
--- /dev/null
+++ b/src/kompass_core/control/vision_dwa.py
@@ -0,0 +1,97 @@
+from attrs import define, field
+from ..utils.common import BaseAttrs, base_validators
+import kompass_cpp
+from typing import Optional
+import numpy as np
+
+
+@define
+class VisionDWAConfig(BaseAttrs):
+    control_time_step: float = field(
+        default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
+    )
+    # Control time step (s)
+
+    control_horizon: int = field(
+        default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
+    )
+    # Number of steps for applying the control
+
+    prediction_horizon: int = field(
+        default=10, validator=base_validators.in_range(min_value=1, max_value=1000)
+    )
+    # Number of steps for future prediction
+
+    tolerance: float = field(
+        default=0.01, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
+    )
+    # Tolerance value for distance and angle following errors
+
+    target_distance: Optional[float] = field(
+        default=0.1,
+        validator=base_validators.in_range(min_value=1e-9, max_value=1e9),
+    )  # Target distance to maintain with the target (m)
+
+    target_orientation: float = field(
+        default=0.0,
+        validator=base_validators.in_range(min_value=-np.pi, max_value=np.pi),
+    )  # Bearing angle to maintain with the target (rad)
+
+    target_wait_timeout: float = field(
+        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # Wait for target to appear again timeout (seconds), used if search is disabled
+
+    target_search_timeout: float = field(
+        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # Search timeout in seconds
+
+    target_search_radius: float = field(
+        default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4)
+    )  # Search radius for finding the target (m)
+
+    target_search_pause: float = field(
+        default=1.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # Pause between search actions to find target (seconds)
+
+    rotation_gain: float = field(
+        default=0.5, validator=base_validators.in_range(min_value=1e-2, max_value=10.0)
+    )  # Gain for the rotation control law
+
+    speed_gain: float = field(
+        default=1.0, validator=base_validators.in_range(min_value=1e-2, max_value=10.0)
+    )  # Gain for the speed control law
+
+    min_vel: float = field(
+        default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )  # Minimum velocity to apply (m/s)
+
+    enable_search: bool = field(default=False)  # Enable or disable the search mechanism
+
+    error_pose: float = field(
+        default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )  # Error in pose estimation (m)
+
+    error_vel: float = field(
+        default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )  # Error in velocity estimation (m/s)
+
+    error_acc: float = field(
+        default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )  # Error in acceleration estimation (m/s^2)
+
+    def to_kompass_cpp(self) -> kompass_cpp.control.VisionDWAParameters:
+        """
+        Convert to kompass_cpp lib config format
+
+        :return: _description_
+        :rtype: kompass_cpp.control.VisionDWAParameters
+        """
+        vision_dwa_params = kompass_cpp.control.VisionDWAParameters()
+
+        # Special handling for None values that are represented by -1 in C++
+        params_dict = self.asdict()
+        if params_dict["target_distance"] is None:
+            params_dict["target_distance"] = -1.0
+
+        vision_dwa_params.from_dict(params_dict)
+        return vision_dwa_params
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 3aed271a..67efca73 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -8,8 +8,8 @@
 #include "controllers/dwa.h"
 #include "controllers/follower.h"
 #include "controllers/pid.h"
-#include "controllers/vision_dwa.h"
 #include "controllers/stanley.h"
+#include "controllers/vision_dwa.h"
 #include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/trajectory.h"
@@ -89,8 +89,8 @@ void bindings_control(py::module_ &m) {
       .def(py::init<>())
       .def(py::init<Control::Follower::FollowerParameters>())
       .def("set_interpolation_type", &Control::Follower::setInterpolationType)
-      .def("set_current_path", &Control::Follower::setCurrentPath, py::arg("path"),
-           py::arg("interpolate") = true)
+      .def("set_current_path", &Control::Follower::setCurrentPath,
+           py::arg("path"), py::arg("interpolate") = true)
       .def("clear_current_path", &Control::Follower::clearCurrentPath)
       .def("is_goal_reached", &Control::Follower::isGoalReached)
       .def("get_vx_cmd", &Control::Follower::getLinearVelocityCmdX)
@@ -191,12 +191,13 @@ void bindings_control(py::module_ &m) {
       .def("compute_velocity_commands",
            py::overload_cast<const Control::Velocity2D &,
                              const Control::LaserScan &>(
-               &Control::DWA::computeVelocityCommandsSet),
+               &Control::DWA::computeVelocityCommandsSet<Control::LaserScan>),
            py::rv_policy::reference_internal)
       .def("compute_velocity_commands",
            py::overload_cast<const Control::Velocity2D &,
                              const std::vector<Path::Point> &>(
-               &Control::DWA::computeVelocityCommandsSet),
+               &Control::DWA::computeVelocityCommandsSet<
+                   std::vector<Path::Point>>),
            py::rv_policy::reference_internal)
       .def("add_custom_cost",
            &Control::DWA::addCustomCost) // Custom cost function for DWA planner
@@ -236,27 +237,38 @@ void bindings_control(py::module_ &m) {
 
   // Vision DWA
   py::class_<Control::VisionDWA::VisionDWAConfig,
-             Control::Controller::ControllerParameters>(
-      m_control, "VisionDWAParameters")
+             Control::Controller::ControllerParameters>(m_control,
+                                                        "VisionDWAParameters")
       .def(py::init<>());
 
   py::class_<Control::VisionDWA, Control::DWA>(m_control, "VisionDWA")
-      .def(
-          py::init<const Control::ControlType&, const Control::ControlLimitsParams&, const int, const int,
-                   const CollisionChecker::ShapeType&, const std::vector<float> &,
-                   const std::array<float, 3> &, const std::array<float, 4> &,
-                   const double, const Control::CostEvaluator::TrajectoryCostsWeights&, const int,
-                   const Control::VisionDWA::VisionDWAConfig &, const bool>())
+      .def(py::init<const Control::ControlType &,
+                    const Control::ControlLimitsParams &, const int, const int,
+                    const CollisionChecker::ShapeType &,
+                    const std::vector<float> &, const std::array<float, 3> &,
+                    const std::array<float, 4> &, const double,
+                    const Control::CostEvaluator::TrajectoryCostsWeights &,
+                    const int, const Control::VisionDWA::VisionDWAConfig &,
+                    const bool>(),
+           py::arg("control_type"), py::arg("control_limits"),
+           py::arg("max_linear_samples"), py::arg("max_angular_samples"),
+           py::arg("robot_shape_type"), py::arg("robot_dimensions"),
+           py::arg("sensor_position_wrt_body"),
+           py::arg("sensor_rotation_wrt_body"), py::arg("octree_res"),
+           py::arg("cost_weights"), py::arg("max_num_threads") = 1,
+           py::arg("config") = Control::VisionDWA::VisionDWAConfig(),
+           py::arg("use_tracker") = true)
       .def("get_pure_tracking_ctrl", &Control::VisionDWA::getPureTrackingCtrl)
+      .def("set_inital_tracking", &Control::VisionDWA::setInitialTracking)
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
                              const std::vector<Eigen::Vector3f> &>(
                &Control::VisionDWA::getTrackingCtrl<
                    std::vector<Eigen::Vector3f>>))
-    .def("get_tracking_ctrl",
-        py::overload_cast<const std::vector<Bbox3D> &,
-                            const Control::Velocity2D &,
-                            const Control::LaserScan &>(
-            &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>));
+      .def("get_tracking_ctrl",
+           py::overload_cast<const std::vector<Bbox3D> &,
+                             const Control::Velocity2D &,
+                             const Control::LaserScan &>(
+               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>));
 }
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index bea1d265..7cde19fc 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -152,4 +152,18 @@ py::class_<TrackedBbox3D>(m_types, "TrackedBbox3D")
     .def_rw("acc", &TrackedBbox3D::acc)
     .def_rw("unique_id", &TrackedBbox3D::unique_id)
     .def("updateFromNewDetection", &TrackedBbox3D::updateFromNewDetection);
+
+py::class_<Control::Pose3D>(m_types, "Pose3D");
+
+py::class_<Control::TrackedPose2D, Control::Pose3D>(m_types, "TrackedPose2D")
+    .def(py::init<const float &, const float &, const float &, const float &,
+                  const float &, const float &>(),
+         py::arg("x"), py::arg("y"), py::arg("yaw"), py::arg("vx"),
+         py::arg("vy"), py::arg("omega"))
+    .def("x", &Control::TrackedPose2D::x)
+    .def("y", &Control::TrackedPose2D::y)
+    .def("yaw", &Control::TrackedPose2D::yaw)
+    .def("v", &Control::TrackedPose2D::v)
+    .def("omega", &Control::TrackedPose2D::omega)
+    .def("update", &Control::TrackedPose2D::update);
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 10fe0036..3d9180f6 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -63,10 +63,17 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     followingVel.setVx(v);
-    double omega = -tracking_pose.omega() +
-                   2.0 * (v * sin(psi) / distance +
-                          tracking_pose.v() * sin(gamma - psi) / distance -
-                          config_.K_omega() * tanh(angle_error));
+    double omega;
+    if (distance > 0.0){
+      omega = -tracking_pose.omega() +
+              2.0 * (v * sin(psi) / distance +
+                     tracking_pose.v() * sin(gamma - psi) / distance -
+                     config_.K_omega() * tanh(angle_error));
+    }
+    else{
+      omega = -tracking_pose.omega() -
+              2.0 * config_.K_omega() * tanh(angle_error);
+    }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
     followingVel.setOmega(omega);
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index bf80d8b1..bd3d0788 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -19,6 +19,10 @@
     DWA,
     StanleyConfig,
     Stanley,
+    VisionDWA,
+    VisionDWAConfig,
+    TrackedPose2D,
+    Bbox3D
 )
 from kompass_core.models import (
     AngularCtrlLimits,
@@ -426,6 +430,142 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw
     assert reached_end is True
 
 
+def test_vision_dwa(
+    figure_name: str = "vision_dwa", figure_tag: str = "vision_dwa"
+):
+    """Run DWA pytest and assert reaching end"""
+    global global_path, my_robot, robot_ctr_limits, control_time_step
+
+    control_horizon = 2
+    prediction_horizon = 10
+
+    # Simulate tracked pose and vel
+    tracked_vel_lin = 0.1
+    tracked_vel_ang = 0.3
+    tracked_pose = TrackedPose2D(0.0, 0.0, 0.0, tracked_vel_lin, 0.0, tracked_vel_ang)
+
+    # To plot
+    robot_x = []
+    robot_y = []
+    tracked_x = []
+    tracked_y = []
+    my_robot.state.x = -0.5
+
+    cost_weights = TrajectoryCostsWeights(
+        reference_path_distance_weight=5.0,
+        goal_distance_weight=1.0,
+        smoothness_weight=0.0,
+        jerk_weight=0.0,
+        obstacles_distance_weight=0.0,
+    )
+    config = VisionDWAConfig(
+        control_time_step=control_time_step,
+        control_horizon=control_horizon,
+        prediction_horizon=prediction_horizon,
+        speed_gain=1.0,
+        rotation_gain=0.5,
+    )
+
+    controller = VisionDWA(
+        control_type=RobotType.to_kompass_cpp_lib(my_robot.robot_type),
+        control_limits=robot_ctr_limits.to_kompass_cpp_lib(),
+        max_linear_samples=20,
+        max_angular_samples=20,
+        robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(my_robot.geometry_type),
+        robot_dimensions=my_robot.geometry_params,
+        sensor_position_wrt_body=[0.0, 0.0, 0.0],
+        sensor_rotation_wrt_body=[0.0, 0.0, 0.0, 1.0],
+        octree_res=0.1,
+        cost_weights=cost_weights.to_kompass_cpp(),
+        max_num_threads=1,
+        config=config.to_kompass_cpp(),
+        use_tracker=True,
+    )
+
+    steps = 0
+
+    # Robot point cloud
+    point_cloud = [np.array([10.3, 10.5, 0.2])]
+
+    # Detected Boxes
+    ref_point_img = [150, 150]
+    detected_boxes = []
+    boxes_ori = 0.0
+    for i in range(3):
+        new_box = Bbox3D(center=np.array([0.0, 0.0, 0.0], dtype=np.float32), size=np.array([0.5, 0.5, 1.0], dtype=np.float32), center_img_frame=np.array([150, 150], dtype=np.int32), size_img_frame=np.array([25, 25], dtype=np.int32))
+        new_box_shift = np.array([0.7 * i, 0.7 * i, 0.0], dtype=np.float32)
+        img_frame_shift = np.array([50 * i, 50 * i], dtype=np.int32)
+        new_box.center = new_box_shift
+        new_box.center_img_frame = img_frame_shift + ref_point_img
+        detected_boxes.append(new_box)
+
+    def moveBoxes(boxes_orientation, boxes) -> float:
+        """Move boxes in the direction of the tracked pose"""
+        for box in boxes:
+            box.center[0] += (
+                tracked_vel_lin * np.cos(boxes_orientation) * control_time_step
+            )
+            box.center[1] += (
+                tracked_vel_lin * np.sin(boxes_orientation) * control_time_step
+            )
+        boxes_orientation += tracked_vel_ang * control_time_step
+        return boxes_orientation
+
+    controller.set_inital_tracking(150, 150, detected_boxes)
+
+    while steps < 100:
+        robot_x.append(my_robot.state.x)
+        robot_y.append(my_robot.state.y)
+        tracked_x.append(tracked_pose.x())
+        tracked_y.append(tracked_pose.y())
+        controller.set_current_state(
+            my_robot.state.x, my_robot.state.y, my_robot.state.yaw, my_robot.state.speed
+        )
+        current_velocity = kompass_cpp.types.ControlCmd(
+            vx=my_robot.state.vx, vy=my_robot.state.vy, omega=my_robot.state.omega
+        )
+        res = controller.get_tracking_ctrl(detected_boxes, current_velocity, point_cloud)
+        if not res.is_found:
+            print("No control found")
+            break
+        print(f"Found trajectory with cost {res.cost}")
+        velocities = res.trajectory.velocities
+        print(
+            f"Found Control {velocities.vx[0]}, {velocities.vy[0]}, {velocities.omega[0]}"
+        )
+        my_robot.set_control(
+            velocity_x=velocities.vx[0],
+            velocity_y=velocities.vy[0],
+            omega=velocities.omega[0],
+        )
+        my_robot.get_state(dt=control_time_step)
+        tracked_pose.update(control_time_step)
+        boxes_ori = moveBoxes(boxes_ori, detected_boxes)
+        print(f"boxes orientation: {boxes_ori}")
+        steps += 1
+
+    plt.figure()
+    plt.plot(
+        robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path"
+    )
+    plt.plot(
+        tracked_x,
+        tracked_y,
+        label="Tracked Object",
+        marker="*",
+        linestyle="-",
+        color="g",
+    )
+    plt.xlabel("X")
+    plt.ylabel("Y")
+    plt.title(figure_tag)
+    plt.grid(True)
+    plt.legend()
+    plt.savefig(f"logs/{figure_name}.png")
+
+    assert steps == 100
+
+
 def test_dwa_debug():
     global global_path, my_robot, robot_ctr_limits, control_time_step
 
@@ -538,32 +678,36 @@ def main():
     robot_ctr_limits = RobotCtrlLimits(
         vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0),
         omega_limits=AngularCtrlLimits(
-            max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
+            max_vel=4.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
         ),
     )
 
     control_time_step = 0.1
 
-    print("RUNNING PATH INTERPOLATION TEST")
-    test_path_interpolation(plot=True)
+    # print("RUNNING PATH INTERPOLATION TEST")
+    # test_path_interpolation(plot=True)
 
-    ## TESTING STANLEY ##
-    print("RUNNING STANLEY CONTROLLER TEST")
-    test_stanley(
-        plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
-    )
+    # ## TESTING STANLEY ##
+    # print("RUNNING STANLEY CONTROLLER TEST")
+    # test_stanley(
+    #     plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
+    # )
+
+    # ## TESTING DVZ ##
+    # print("RUNNING DVZ CONTROLLER TEST")
+    # test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
 
-    ## TESTING DVZ ##
-    print("RUNNING DVZ CONTROLLER TEST")
-    test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
+    # ## TESTING DWA DEBUG MODE ##
+    # print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
+    # test_dwa_debug()
 
-    ## TESTING DWA DEBUG MODE ##
-    print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
-    test_dwa_debug()
+    # ## TESTING DWA ##
+    # print("RUNNING DWA CONTROLLER TEST")
+    # test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
 
-    ## TESTING DWA ##
-    print("RUNNING DWA CONTROLLER TEST")
-    test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
+    ## TESTING VISION DWA ##
+    print("RUNNING VISION DWA CONTROLLER TEST")
+    test_vision_dwa(figure_tag="VisionDWA Controller Test Results")
 
 
 if __name__ == "__main__":

From 0f2c148b321297fef2eb95dc777216c1c92063b9 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 28 Apr 2025 12:38:28 +0200
Subject: [PATCH 044/118] (feature) Adds VisionDWA python API and updates tests

---
 src/kompass_core/control/__init__.py          |   6 +-
 src/kompass_core/control/dwa.py               |   2 +-
 src/kompass_core/control/vision_dwa.py        | 248 +++++++++++++++++-
 src/kompass_core/datatypes/__init__.py        |   3 +
 src/kompass_cpp/bindings/bindings_control.cpp |  14 +-
 .../kompass_cpp/src/controllers/follower.cpp  |  16 +-
 tests/test_controllers.py                     | 124 ++++-----
 7 files changed, 328 insertions(+), 85 deletions(-)

diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index 3f99af13..c5f9e46a 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -11,9 +11,7 @@
 from .dwa import DWA, DWAConfig
 from .stanley import StanleyConfig, Stanley
 from .vision_follower import VisionFollower, VisionFollowerConfig
-from .vision_dwa import VisionDWAConfig
-from kompass_cpp.types import TrackedPose2D, Bbox3D
-from kompass_cpp.control import VisionDWA
+from .vision_dwa import VisionDWAConfig, VisionDWA
 
 ControllerType = FollowerTemplate
 
@@ -107,6 +105,4 @@ class ControllersID(StrEnum):
     "VisionFollowerConfig",
     "VisionDWAConfig",
     "VisionDWA",
-    "TrackedPose2D",
-    "Bbox3D",
 ]
diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 553de937..778250b5 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -351,7 +351,7 @@ def logging_info(self) -> str:
     @property
     def control_till_horizon(
         self,
-    ) -> Optional[List[kompass_cpp.types.TrajectoryVelocities2D]]:
+    ) -> Optional[kompass_cpp.types.TrajectoryVelocities2D]:
         """
         Getter of the planner control result until the control horizon
 
diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 5227f457..bd88fa45 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -1,16 +1,34 @@
 from attrs import define, field
-from ..utils.common import BaseAttrs, base_validators
-import kompass_cpp
-from typing import Optional
+from ..utils.common import base_validators
+from kompass_cpp.control import (
+    VisionDWA as VisionDWACpp,
+    VisionDWAParameters,
+    SamplingControlResult,
+)
+from kompass_cpp.types import (
+    Bbox3D,
+    ControlCmd,
+    LaserScan,
+    TrajectoryVelocities2D,
+    TrajectoryPath,
+    TrackedPose2D
+)
+from typing import Optional, List
 import numpy as np
+import logging
+from ._base_ import ControllerTemplate
+from ..models import Robot, RobotState, RobotCtrlLimits, RobotGeometry, RobotType
+from ..datatypes.laserscan import LaserScanData
+from ..datatypes.pointcloud import PointCloudData
+from .dwa import DWAConfig
 
 
 @define
-class VisionDWAConfig(BaseAttrs):
-    control_time_step: float = field(
-        default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
-    )
-    # Control time step (s)
+class VisionDWAConfig(DWAConfig):
+
+    use_tracker: bool = field(
+        default=True
+    )  # Use the tracker to get the target position and velocity from the detections
 
     control_horizon: int = field(
         default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
@@ -79,19 +97,229 @@ class VisionDWAConfig(BaseAttrs):
         default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
     )  # Error in acceleration estimation (m/s^2)
 
-    def to_kompass_cpp(self) -> kompass_cpp.control.VisionDWAParameters:
+    def to_kompass_cpp(self) -> VisionDWAParameters:
         """
         Convert to kompass_cpp lib config format
 
         :return: _description_
         :rtype: kompass_cpp.control.VisionDWAParameters
         """
-        vision_dwa_params = kompass_cpp.control.VisionDWAParameters()
+        vision_dwa_params = VisionDWAParameters()
 
         # Special handling for None values that are represented by -1 in C++
         params_dict = self.asdict()
+        print(f"as dict {params_dict}")
         if params_dict["target_distance"] is None:
             params_dict["target_distance"] = -1.0
 
         vision_dwa_params.from_dict(params_dict)
         return vision_dwa_params
+
+
+class VisionDWA(ControllerTemplate):
+    def __init__(
+        self,
+        robot: Robot,
+        ctrl_limits: RobotCtrlLimits,
+        config: Optional[VisionDWAConfig] = None,
+        config_file: Optional[str] = None,
+        config_yaml_root_name: Optional[str] = None,
+        control_time_step: Optional[float] = None,
+        **_,
+    ):
+        """
+        Setup the controller
+
+        :param robot: Robot using the controller
+        :type robot: Robot
+        :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
+        :type params_file: str
+        """
+        self._config = config or VisionDWAConfig()
+
+        if config_file:
+            self._config.from_yaml(
+                file_path=config_file, nested_root_name=config_yaml_root_name
+            )
+
+        if control_time_step:
+            self._config.control_time_step = control_time_step
+        logging.info(f"use_tracker: {self._config.use_tracker}")
+        self._planner = VisionDWACpp(
+            control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
+            control_limits=ctrl_limits.to_kompass_cpp_lib(),
+            max_linear_samples=self._config.max_linear_samples,
+            max_angular_samples=self._config.max_angular_samples,
+            robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type),
+            robot_dimensions=robot.geometry_params,
+            sensor_position_wrt_body=self._config.sensor_position_to_robot,
+            sensor_rotation_wrt_body=self._config.sensor_rotation_to_robot,
+            octree_res=self._config.octree_resolution,
+            cost_weights=self._config.costs_weights.to_kompass_cpp(),
+            max_num_threads=self._config.max_num_threads,
+            config=self._config.to_kompass_cpp(),
+            use_tracker=self._config.use_tracker,
+        )
+        # Init the following result
+        self._result = SamplingControlResult()
+        self._end_of_ctrl_horizon: int = max(
+            self._config.control_horizon, 1
+        )
+        logging.info("VisionDWA CONTROLLER IS READY")
+
+    def set_initial_tracking(self, pose_x_img: int, pose_y_img: int, detected_boxes: List[Bbox3D]) -> bool:
+        """
+        Set initial tracking state
+
+        :param detected_boxes: Detected boxes
+        :type detected_boxes: List[Bbox3D]
+        """
+        try:
+            self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+        except Exception as e:
+            logging.error(f"Could not set initial tracking state: {e}")
+            return False
+        return True
+
+    def loop_step(
+        self,
+        *,
+        current_state: RobotState,
+        detections: Optional[List[Bbox3D]] = None,
+        tracked_pose: Optional[TrackedPose2D] = None,
+        laser_scan: Optional[LaserScanData] = None,
+        point_cloud: Optional[List[np.ndarray]] = None,
+        local_map: Optional[np.ndarray] = None,
+        local_map_resolution: Optional[float] = None,
+        **_,
+    ) -> bool:
+        """
+        One iteration of the DWA planner
+
+        :param current_state: Current robot state (position and velocity)
+        :type current_state: RobotState
+        :param laser_scan: Current laser scan value
+        :type laser_scan: LaserScanData
+
+        :return: If planner found a valid solution
+        :rtype: bool
+        """
+        self._planner.set_current_state(
+            current_state.x, current_state.y, current_state.yaw, current_state.speed
+        )
+
+        if local_map_resolution:
+            self._planner.set_resolution(local_map_resolution)
+
+        current_velocity = ControlCmd(
+            vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
+        )
+
+        if local_map is not None:
+            sensor_data = PointCloudData.numpy_to_kompass_cpp(local_map)
+        elif laser_scan:
+            sensor_data = LaserScan(
+                ranges=laser_scan.ranges, angles=laser_scan.angles
+            )
+        elif point_cloud is not None:
+            sensor_data = point_cloud
+        else:
+            logging.error(
+                "Cannot compute control without sensor data. Provide 'laser_scan' or 'point_cloud' input"
+            )
+            return False
+
+        try:
+            self._result = self._planner.get_tracking_ctrl(
+                tracked_pose or detections, current_velocity, sensor_data
+            )
+
+        except Exception as e:
+            logging.error(f"Could not find velocity command: {e}")
+            return False
+
+        return True
+
+    def has_result(self) -> None:
+        """
+        Set global path to be tracked by the planner
+
+        :param global_path: Global reference path
+        :type global_path: Path
+        """
+        return self._result.is_found
+
+    def logging_info(self) -> str:
+        """logging_info."""
+        if self._result.is_found:
+            return f"VisionDWA Controller found trajectory with cost: {self._result.cost}"
+        else:
+            return "VisionDWA Controller Failed to find a valid trajectory"
+
+    @property
+    def control_till_horizon(
+        self,
+    ) -> Optional[TrajectoryVelocities2D]:
+        """
+        Getter of the planner control result until the control horizon
+
+        :return: Velocity commands of the minimal cost path
+        :rtype: List[kompass_cpp.types.TrajectoryVelocities2D]
+        """
+        if self._result.is_found:
+            return self._result.trajectory.velocities
+        return None
+
+    def optimal_path(self) -> Optional[TrajectoryPath]:
+        """Get optimal (local) plan."""
+        if not self._result.is_found:
+            return None
+        return self._result.trajectory.path
+
+    @property
+    def result_cost(self) -> Optional[float]:
+        """
+        Getter of the planner optimal path
+
+        :return: Path found with the least cost
+        :rtype: Optional[float]
+        """
+        if self._result.is_found:
+            return self._result.cost
+        return None
+
+    @property
+    def linear_x_control(self) -> np.ndarray:
+        """
+        Getter of the last linear forward velocity control computed by the controller
+
+        :return: Linear Velocity Control (m/s)
+        :rtype: List[float]
+        """
+        if self._result.is_found:
+            return self.control_till_horizon.vx[: self._end_of_ctrl_horizon]
+        return [0.0]
+
+    @property
+    def linear_y_control(self) -> np.ndarray:
+        """
+        Getter the last linear velocity lateral control computed by the controller
+
+        :return: Linear Velocity Control (m/s)
+        :rtype: List[float]
+        """
+        if self._result.is_found:
+            return self.control_till_horizon.vy[: self._end_of_ctrl_horizon]
+        return [0.0]
+
+    @property
+    def angular_control(self) -> np.ndarray:
+        """
+        Getter of the last angular velocity control computed by the controller
+
+        :return: Angular Velocity Control (rad/s)
+        :rtype: List[float]
+        """
+        if self._result.is_found:
+            return self.control_till_horizon.omega[: self._end_of_ctrl_horizon]
+        return [0.0]
diff --git a/src/kompass_core/datatypes/__init__.py b/src/kompass_core/datatypes/__init__.py
index 717457ba..3df8a617 100644
--- a/src/kompass_core/datatypes/__init__.py
+++ b/src/kompass_core/datatypes/__init__.py
@@ -15,6 +15,7 @@
 from .pointcloud import PointCloudData
 from .pose import PoseData
 from .vision import TrackingData, ImageMetaData
+from kompass_cpp.types import TrackedPose2D, Bbox3D
 
 __all__ = [
     "LaserScanData",
@@ -33,4 +34,6 @@
     "PoseData",
     "TrackingData",
     "ImageMetaData",
+    "Bbox3D",
+    "TrackedPose2D",
 ]
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 67efca73..4162392e 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -258,8 +258,7 @@ void bindings_control(py::module_ &m) {
            py::arg("cost_weights"), py::arg("max_num_threads") = 1,
            py::arg("config") = Control::VisionDWA::VisionDWAConfig(),
            py::arg("use_tracker") = true)
-      .def("get_pure_tracking_ctrl", &Control::VisionDWA::getPureTrackingCtrl)
-      .def("set_inital_tracking", &Control::VisionDWA::setInitialTracking)
+      .def("set_initial_tracking", &Control::VisionDWA::setInitialTracking)
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
@@ -270,5 +269,16 @@ void bindings_control(py::module_ &m) {
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
                              const Control::LaserScan &>(
+               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>))
+      .def("get_tracking_ctrl",
+           py::overload_cast<const Control::TrackedPose2D &,
+                             const Control::Velocity2D &,
+                             const std::vector<Eigen::Vector3f> &>(
+               &Control::VisionDWA::getTrackingCtrl<
+                   std::vector<Eigen::Vector3f>>))
+      .def("get_tracking_ctrl",
+           py::overload_cast<const Control::TrackedPose2D &,
+                             const Control::Velocity2D &,
+                             const Control::LaserScan &>(
                &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>));
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index b8ac6650..b9653a36 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -246,7 +246,7 @@ Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) {
   }
 
   Path::PathPosition closest_position;
-  closest_position.index = point_index - 1;
+  closest_position.index = point_index;
   closest_position.segment_index = segment_index;
   closest_position.segment_length = segment_position;
   closest_position.state = closest_point;
@@ -273,12 +273,14 @@ Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) {
 void Follower::determineTarget() {
 
   currentTrackedTarget_ = std::make_unique<Target>();
-  // closestPosition = new Path::PathPosition();
-
-  // closest position is never updated
-  // OR If we reached end of segment or end of path -> Find new point
+  LOG_DEBUG("Closest point index on segment ", closestPosition->index,
+           " max index on segment is",
+           currentPath->segments[current_segment_index_].getSize() - 1,
+           " its segment length = ", closestPosition->segment_length);
+  // If closest position is never updated
+  // OR If we reached end of a segment or end of the path -> Find new segment then new point on segment
   if ((closestPosition->segment_length <= 0.0) ||
-      (closestPosition->segment_index >= currentPath->getSize() - 1) ||
+      (closestPosition->index >= currentPath->segments[current_segment_index_].getSize() - 1) ||
       (closestPosition->segment_length >= 1.0)) {
     *closestPosition = findClosestPathPoint();
   }
@@ -287,7 +289,7 @@ void Follower::determineTarget() {
     *closestPosition =
         findClosestPointOnSegment(closestPosition->segment_index);
   }
-  currentTrackedTarget_->segment_index = closestPosition->segment_index;
+  currentTrackedTarget_->segment_index = current_segment_index_;
   currentTrackedTarget_->position_in_segment = closestPosition->segment_length;
 
   currentTrackedTarget_->movement = closestPosition->state;
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index bd3d0788..e55cb3f0 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -12,6 +12,7 @@
 from kompass_cpp.types import PathInterpolationType, Path as PathCpp
 
 from kompass_core.datatypes.laserscan import LaserScanData
+from kompass_core.datatypes import TrackedPose2D, Bbox3D
 from kompass_core.control import (
     DVZ,
     DWAConfig,
@@ -21,8 +22,6 @@
     Stanley,
     VisionDWA,
     VisionDWAConfig,
-    TrackedPose2D,
-    Bbox3D
 )
 from kompass_core.models import (
     AngularCtrlLimits,
@@ -430,8 +429,10 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw
     assert reached_end is True
 
 
+@pytest.mark.parametrize("use_tracker", [False, True])
+@pytest.mark.parametrize("point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]])
 def test_vision_dwa(
-    figure_name: str = "vision_dwa", figure_tag: str = "vision_dwa"
+    use_tracker: bool, point_cloud: List[np.ndarray]
 ):
     """Run DWA pytest and assert reaching end"""
     global global_path, my_robot, robot_ctr_limits, control_time_step
@@ -449,11 +450,13 @@ def test_vision_dwa(
     robot_y = []
     tracked_x = []
     tracked_y = []
-    my_robot.state.x = -0.5
+    my_robot.state.x = -0.51731912
+    my_robot.state.y = 0.0
+    my_robot.state.yaw = 0.0
 
     cost_weights = TrajectoryCostsWeights(
-        reference_path_distance_weight=5.0,
-        goal_distance_weight=1.0,
+        reference_path_distance_weight=1.0,
+        goal_distance_weight=0.0,
         smoothness_weight=0.0,
         jerk_weight=0.0,
         obstacles_distance_weight=0.0,
@@ -463,29 +466,19 @@ def test_vision_dwa(
         control_horizon=control_horizon,
         prediction_horizon=prediction_horizon,
         speed_gain=1.0,
-        rotation_gain=0.5,
-    )
-
-    controller = VisionDWA(
-        control_type=RobotType.to_kompass_cpp_lib(my_robot.robot_type),
-        control_limits=robot_ctr_limits.to_kompass_cpp_lib(),
+        rotation_gain=1.0,
+        costs_weights=cost_weights,
         max_linear_samples=20,
         max_angular_samples=20,
-        robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(my_robot.geometry_type),
-        robot_dimensions=my_robot.geometry_params,
-        sensor_position_wrt_body=[0.0, 0.0, 0.0],
-        sensor_rotation_wrt_body=[0.0, 0.0, 0.0, 1.0],
-        octree_res=0.1,
-        cost_weights=cost_weights.to_kompass_cpp(),
+        octree_resolution=0.1,
         max_num_threads=1,
-        config=config.to_kompass_cpp(),
-        use_tracker=True,
+        min_vel=0.05,
+        use_tracker=use_tracker
     )
 
-    steps = 0
+    controller = VisionDWA(robot=my_robot, ctrl_limits=robot_ctr_limits, config=config)
 
-    # Robot point cloud
-    point_cloud = [np.array([10.3, 10.5, 0.2])]
+    steps = 0
 
     # Detected Boxes
     ref_point_img = [150, 150]
@@ -511,39 +504,50 @@ def moveBoxes(boxes_orientation, boxes) -> float:
         boxes_orientation += tracked_vel_ang * control_time_step
         return boxes_orientation
 
-    controller.set_inital_tracking(150, 150, detected_boxes)
+    if use_tracker:
+        controller.set_initial_tracking(150, 150, detected_boxes)
 
     while steps < 100:
         robot_x.append(my_robot.state.x)
         robot_y.append(my_robot.state.y)
         tracked_x.append(tracked_pose.x())
         tracked_y.append(tracked_pose.y())
-        controller.set_current_state(
-            my_robot.state.x, my_robot.state.y, my_robot.state.yaw, my_robot.state.speed
-        )
-        current_velocity = kompass_cpp.types.ControlCmd(
-            vx=my_robot.state.vx, vy=my_robot.state.vy, omega=my_robot.state.omega
-        )
-        res = controller.get_tracking_ctrl(detected_boxes, current_velocity, point_cloud)
-        if not res.is_found:
+
+        if use_tracker:
+            res = controller.loop_step(current_state=my_robot.state, detections=detected_boxes, point_cloud=point_cloud)
+        else:
+            res = controller.loop_step(
+                current_state=my_robot.state,
+                tracked_pose=tracked_pose,
+                point_cloud=point_cloud,
+            )
+        if not res:
             print("No control found")
             break
-        print(f"Found trajectory with cost {res.cost}")
-        velocities = res.trajectory.velocities
+        # print(f"Found trajectory with cost {controller.result_cost}")
+        velocities = controller.control_till_horizon
+        (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
         print(
-            f"Found Control {velocities.vx[0]}, {velocities.vy[0]}, {velocities.omega[0]}"
-        )
+                f"Found Control {vx}, {vy}, {omega}"
+            )
         my_robot.set_control(
-            velocity_x=velocities.vx[0],
-            velocity_y=velocities.vy[0],
-            omega=velocities.omega[0],
+            velocity_x=vx,
+            velocity_y=vy,
+            omega=omega,
         )
         my_robot.get_state(dt=control_time_step)
         tracked_pose.update(control_time_step)
-        boxes_ori = moveBoxes(boxes_ori, detected_boxes)
-        print(f"boxes orientation: {boxes_ori}")
+        if use_tracker:
+            boxes_ori = moveBoxes(boxes_ori, detected_boxes)
         steps += 1
 
+    figure_name = "vision_dwa"
+    if use_tracker:
+        figure_name += "_with_tracker"
+    else:
+        figure_name += "_no_tracker"
+
+    figure_name += f"{point_cloud[0]}"
     plt.figure()
     plt.plot(
         robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path"
@@ -558,7 +562,7 @@ def moveBoxes(boxes_orientation, boxes) -> float:
     )
     plt.xlabel("X")
     plt.ylabel("Y")
-    plt.title(figure_tag)
+    plt.title(figure_name)
     plt.grid(True)
     plt.legend()
     plt.savefig(f"logs/{figure_name}.png")
@@ -652,7 +656,7 @@ def run_before_and_after_tests():
     robot_ctr_limits = RobotCtrlLimits(
         vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0),
         omega_limits=AngularCtrlLimits(
-            max_vel=3.0, max_acc=5.0, max_decel=10.0, max_steer=np.pi
+            max_vel=4.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
         ),
     )
 
@@ -684,30 +688,30 @@ def main():
 
     control_time_step = 0.1
 
-    # print("RUNNING PATH INTERPOLATION TEST")
-    # test_path_interpolation(plot=True)
+    print("RUNNING PATH INTERPOLATION TEST")
+    test_path_interpolation(plot=True)
 
-    # ## TESTING STANLEY ##
-    # print("RUNNING STANLEY CONTROLLER TEST")
-    # test_stanley(
-    #     plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
-    # )
+    ## TESTING STANLEY ##
+    print("RUNNING STANLEY CONTROLLER TEST")
+    test_stanley(
+        plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
+    )
 
-    # ## TESTING DVZ ##
-    # print("RUNNING DVZ CONTROLLER TEST")
-    # test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
+    ## TESTING DVZ ##
+    print("RUNNING DVZ CONTROLLER TEST")
+    test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
 
-    # ## TESTING DWA DEBUG MODE ##
-    # print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
-    # test_dwa_debug()
+    ## TESTING DWA DEBUG MODE ##
+    print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
+    test_dwa_debug()
 
-    # ## TESTING DWA ##
-    # print("RUNNING DWA CONTROLLER TEST")
-    # test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
+    ## TESTING DWA ##
+    print("RUNNING DWA CONTROLLER TEST")
+    test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
 
     ## TESTING VISION DWA ##
     print("RUNNING VISION DWA CONTROLLER TEST")
-    test_vision_dwa(figure_tag="VisionDWA Controller Test Results")
+    test_vision_dwa(use_tracker=True, point_cloud=[np.array([0.3, 0.27, 0.1])])
 
 
 if __name__ == "__main__":

From f7e8962615002e78f94c854a5258edb110a08b23 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 29 Apr 2025 17:52:00 +0200
Subject: [PATCH 045/118] (feature) Adds DepthDetector to convert 2D bounding
 boxes to 3D boxes using Depth Image

---
 .../include/vision/depth_detector.h           |  50 +++++
 .../kompass_cpp/src/vision/depth_detector.cpp | 172 ++++++++++++++++++
 src/kompass_cpp/tests/CMakeLists.txt          |  14 +-
 .../tests/vision_detector_test.cpp            | 137 ++++++++++++++
 4 files changed, 370 insertions(+), 3 deletions(-)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
 create mode 100644 src/kompass_cpp/tests/vision_detector_test.cpp

diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
new file mode 100644
index 00000000..0ea0c34c
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include "datatypes/path.h"
+#include "datatypes/tracking.h"
+#include <Eigen/Dense>
+#include <Eigen/src/Geometry/Transform.h>
+#include <memory>
+#include <optional>
+#include <vector>
+
+namespace Kompass {
+
+class DepthDetector {
+public:
+  DepthDetector(const Eigen::Vector2f &depth_range,
+                const Eigen::Vector3f &body_to_camera_translation,
+                const Eigen::Quaternionf &body_to_camera_rotation,
+                const Eigen::Vector2f &focal_length,
+                const Eigen::Vector2f &principal_point,
+                const float depth_conversion_factor = 1e-3);
+
+  DepthDetector(const Eigen::Vector2f &depth_range,
+                const Eigen::Isometry3f &body_to_camera_tf,
+                const Eigen::Vector2f &focal_length,
+                const Eigen::Vector2f &principal_point,
+                const float depth_conversion_factor = 1e-3);
+
+  void updateState(const Path::State& current_state);
+
+  void updateState(const Eigen::Isometry3f &robot_tf);
+
+  void updateBoxes(const Eigen::MatrixXi aligned_depth_img, const std::vector<Bbox2D>& detections);
+
+  std::optional<std::vector<Bbox3D>> get3dDetections() const;
+
+private:
+  float cx_, cy_, fx_, fy_;         // Depth Image camera intrinsics
+  float minDepth_, maxDepth_, depthConversionFactor_;
+  Eigen::MatrixXi alignedDepthImg_;
+  Eigen::Isometry3f body_to_camera_tf_, world_to_body_tf_;
+  std::unique_ptr<std::vector<Bbox3D>> boxes_;
+
+  std::optional<Bbox3D> convert2Dboxto3Dbox(const Bbox2D &box2d);
+
+  static void calculateMAD(const std::vector<float>& depthValues, float& median, float& mad);
+
+  static float getMedian(const std::vector<float> &values);
+};
+
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
new file mode 100644
index 00000000..ebe347a1
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -0,0 +1,172 @@
+#include "vision/depth_detector.h"
+#include "datatypes/tracking.h"
+#include "utils/logger.h"
+#include "utils/transformation.h"
+#include <memory>
+#include <optional>
+#include <vector>
+
+namespace Kompass {
+
+DepthDetector::DepthDetector(const Eigen::Vector2f &depth_range,
+                             const Eigen::Vector3f &body_to_camera_translation,
+                             const Eigen::Quaternionf &body_to_camera_rotation,
+                             const Eigen::Vector2f &focal_length,
+                             const Eigen::Vector2f &principal_point,
+                             const float depth_conversion_factor) {
+  // Set camera tf
+  auto body_to_camera_tf =
+      getTransformation(body_to_camera_rotation, body_to_camera_translation);
+  DepthDetector(depth_range, body_to_camera_tf, focal_length, principal_point,
+                depth_conversion_factor);
+}
+
+DepthDetector::DepthDetector(
+    const Eigen::Vector2f &depth_range,
+    const Eigen::Isometry3f &body_to_camera_tf,
+    const Eigen::Vector2f &focal_length, const Eigen::Vector2f &principal_point,
+    const float depth_conversion_factor) { // Range of interest for depth values
+                                           // in meters
+  minDepth_ = depth_range(0);
+  maxDepth_ = depth_range(1);
+  // Factor to convert depth image data to meters (in ROS2 its given in mm ->
+  // depthConversionFactor = 1e-3)
+  depthConversionFactor_ = depth_conversion_factor;
+  // Set camera tf
+  body_to_camera_tf_ = body_to_camera_tf;
+  world_to_body_tf_ = Eigen::Isometry3f::Identity();
+
+  // Set camera  intrinsic parameters
+  fx_ = focal_length.x();
+  fy_ = focal_length.y();
+  cx_ = principal_point.x();
+  cy_ = principal_point.y();
+}
+
+std::optional<std::vector<Bbox3D>> DepthDetector::get3dDetections() const {
+  if (boxes_) {
+    return *boxes_;
+  }
+  return std::nullopt;
+}
+
+void DepthDetector::updateState(const Path::State &current_state) {
+
+  world_to_body_tf_ = getTransformation(current_state);
+}
+
+void DepthDetector::updateState(const Eigen::Isometry3f &robot_tf) {
+  world_to_body_tf_ = robot_tf;
+}
+
+void DepthDetector::updateBoxes(const Eigen::MatrixXi aligned_depth_img,
+                                const std::vector<Bbox2D> &detections) {
+  alignedDepthImg_ = aligned_depth_img;
+  boxes_ = std::make_unique<std::vector<Bbox3D>>();
+  for (auto box2d : detections) {
+    auto converted_box = convert2Dboxto3Dbox(box2d);
+    if (converted_box) {
+      boxes_->push_back(converted_box.value());
+    }
+  }
+}
+
+std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
+  Bbox3D box3d(box2d);
+  Eigen::Vector2i x_limits = box2d.getXLimits();
+  Eigen::Vector2i y_limits = box2d.getYLimits();
+  float depth_meters;
+  // All depth values in the 2D box within the range of interest
+  std::vector<float> depth_values;
+  for (int row_idx = y_limits(0); row_idx <= y_limits(1); ++row_idx) {
+    for (int col_idx = x_limits(0); col_idx <= x_limits(1); ++col_idx) {
+      depth_meters =
+          alignedDepthImg_(row_idx, col_idx) * depthConversionFactor_;
+      if (depth_meters <= maxDepth_ && depth_meters >= minDepth_) {
+        depth_values.push_back(depth_meters);
+      }
+    }
+  }
+  if (depth_values.size() <= 1) {
+    LOG_WARNING("Could not get any depth values for 2D bounding box at ",
+                box2d.top_corner.x(), ", ", box2d.top_corner.y());
+    return std::nullopt;
+  }
+  float medianDepth, madDepth;
+  calculateMAD(depth_values, medianDepth, madDepth);
+
+  // Get min and max depth
+  float minimum_d = maxDepth_, maximum_d = minDepth_;
+  for (auto depth : depth_values) {
+    if ((depth < minimum_d) && (depth >= medianDepth - 1.5 * madDepth)) {
+      minimum_d = depth;
+    }
+    if ((depth > maximum_d) && (depth <= medianDepth + 1.5 * madDepth)) {
+      maximum_d = depth;
+    }
+  }
+
+  // Convert from 2D box center in the pixel frame to the 3D box center in the
+  // camera frame
+  Eigen::Vector3f center_img_frame, size_img_frame;
+  LOG_DEBUG("Got detected box in 2D pixel frame at :", box2d.top_corner.x(),
+           ", ", box2d.top_corner.y(), ", size = ", box2d.size.x(), ", ",
+           box2d.size.y());
+  center_img_frame(0) = (box2d.top_corner.x() + 0.5 * box2d.size.x() - cx_) *
+                        medianDepth / this->fx_;
+  center_img_frame(1) = (box2d.top_corner.y() + 0.5 * box2d.size.y() - cy_) *
+                        medianDepth / this->fy_;
+  center_img_frame(2) = medianDepth;
+
+  LOG_DEBUG("Got detected box in 3D camera frame at :", center_img_frame.x(),
+           ", ", center_img_frame.y(), ", ", center_img_frame.z());
+
+  LOG_DEBUG("Median depth = ", medianDepth);
+
+  // Size in meters
+  size_img_frame(0) = box2d.size.x() * medianDepth / this->fx_;
+  size_img_frame(1) = box2d.size.y() * medianDepth / this->fy_;
+  size_img_frame(2) = maximum_d - minimum_d;
+
+  // Register center in the world frame
+  box3d.center = world_to_body_tf_ * body_to_camera_tf_ * center_img_frame;
+
+  // Transform size from camera frame to world frame
+  box3d.size(0) = (world_to_body_tf_ * body_to_camera_tf_ *
+                   Eigen::Vector3f(size_img_frame(0), 0, 0))
+                      .norm();
+  box3d.size(1) = (world_to_body_tf_ * body_to_camera_tf_ *
+                   Eigen::Vector3f(0, size_img_frame(1), 0))
+                      .norm();
+  box3d.size(2) = (world_to_body_tf_ * body_to_camera_tf_ *
+                   Eigen::Vector3f(0, 0, size_img_frame(2)))
+                      .norm();
+
+  return box3d;
+}
+
+float DepthDetector::getMedian(const std::vector<float> &values) {
+  float median;
+  auto sorted_value = values;
+  std::sort(sorted_value.begin(), sorted_value.end());
+  // number of items
+  auto num = sorted_value.size() - 1;
+  if (num % 2 == 0) {
+    median = (sorted_value[num / 2] + sorted_value[num / 2 + 1]) / 2;
+  } else {
+    median = sorted_value[(num + 1) / 2];
+  }
+  return median;
+}
+
+void DepthDetector::calculateMAD(const std::vector<float> &depthValues,
+                                 float &median, float &mad) {
+  median = getMedian(depthValues);
+
+  std::vector<float> deviations;
+  for (size_t i = 0; i < depthValues.size(); ++i) {
+    deviations.push_back(std::abs(depthValues[i] - median));
+  }
+  mad = getMedian(deviations);
+}
+} // namespace Kompass
diff --git a/src/kompass_cpp/tests/CMakeLists.txt b/src/kompass_cpp/tests/CMakeLists.txt
index 64c50817..8e85866c 100644
--- a/src/kompass_cpp/tests/CMakeLists.txt
+++ b/src/kompass_cpp/tests/CMakeLists.txt
@@ -2,8 +2,11 @@
 set(MODULE_NAME kompass)
 
 find_package(nlohmann_json 3.2.0 REQUIRED)
+find_package(OpenCV REQUIRED)
 find_package(Boost COMPONENTS unit_test_framework REQUIRED)
 
+include_directories( ${OpenCV_INCLUDE_DIRS} )
+
 # Specify the location of the Python script
 set(PYTHON_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/trajectory_sampler_plt.py)
 # Create a symbolic link for the Python script
@@ -36,13 +39,18 @@ add_test(collisions_tests collisions_test)
 
 # Vision DWA test
 add_executable(vision_dwa_test vision_dwa_test.cpp)
-target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework)
-add_test(vision_dwa_tests vision_dwa_test)
+target_link_libraries(vision_dwa_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework ${OpenCV_LIBS})
+add_test(vision_tests vision_dwa_test)
 
 # Set path to the test image
 add_executable(vision_tracking_test vision_tracking_test.cpp)
 target_link_libraries(vision_tracking_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework nlohmann_json::nlohmann_json)
-add_test(vision_tracking_tests vision_tracking_test)
+add_test(vision_tests vision_tracking_test)
+
+# Set path to the test image
+add_executable(vision_detector_test vision_detector_test.cpp)
+target_link_libraries(vision_detector_test PRIVATE ${MODULE_NAME} Boost::unit_test_framework ${OpenCV_LIBS})
+add_test(vision_tests vision_detector_test)
 
 # find sycl
 find_package(AdaptiveCpp)
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
new file mode 100644
index 00000000..b83c1cb6
--- /dev/null
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -0,0 +1,137 @@
+#include "datatypes/tracking.h"
+#include "test.h"
+#include "utils/logger.h"
+#include "utils/transformation.h"
+#include "vision/depth_detector.h"
+#include <Eigen/Dense>
+#include <opencv2/opencv.hpp>
+#include <stdexcept>
+#define BOOST_TEST_MODULE KOMPASS TESTS
+#include <boost/dll/runtime_symbol_info.hpp> // for program_location
+#include <boost/filesystem.hpp>
+#include <boost/test/included/unit_test.hpp>
+#include <memory>
+
+using namespace Kompass;
+
+struct DepthDetectorTestConfig {
+  std::unique_ptr<DepthDetector> detector;
+  Path::State current_state = {0.0, 0.0, 0.0};
+  std::vector<Bbox2D> detected_boxes;
+  std::string pltFileName = "DepthDetectorTest";
+  Eigen::Vector2f focal_length = {911.71, 910.288};
+  Eigen::Vector2f principal_point = {643.06, 366.72};
+  Eigen::Vector2f depth_range = {0.001, 5.0}; // 5cm to 5 meters
+  float depth_conv_factor = 1e-3;             // convert from mm to m
+  Eigen::Isometry3f camera_body_tf;
+  Eigen::MatrixXi depth_image;
+  cv::Mat cv_img;
+  std::vector<Bbox2D> detections;
+
+  DepthDetectorTestConfig(const std::string image_filename, const Bbox2D &box) {
+    // Body to camera tf from robot of test pictures
+    auto body_to_link_tf =
+        getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
+                          Eigen::Vector3f{0.32f, 0.0209f, 0.3f});
+
+    auto link_to_cam_tf =
+        getTransformation(Eigen::Quaternionf{0.01f, -0.00131f, 0.002f, 0.9999f},
+                          Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
+
+    auto cam_to_cam_opt_tf =
+        getTransformation(Eigen::Quaternionf{-0.5f, 0.5f, -0.5f, 0.5f},
+                          Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
+
+    Eigen::Isometry3f body_to_cam_tf = body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
+
+    detector = std::make_unique<DepthDetector>(depth_range, body_to_cam_tf,
+                                               focal_length, principal_point,
+                                               depth_conv_factor);
+
+    detector->updateState(current_state);
+    cv_img = cv::imread(image_filename, cv::IMREAD_GRAYSCALE);
+
+    if (cv_img.empty()) {
+      LOG_ERROR("Could not open or find the image");
+    }
+
+    // Create an Eigen matrix of type int from the OpenCV Mat
+    depth_image = Eigen::MatrixXi(cv_img.rows, cv_img.cols);
+    for (int i = 0; i < cv_img.rows; ++i) {
+      for (int j = 0; j < cv_img.cols; ++j) {
+        depth_image(i, j) = cv_img.at<unsigned short>(i, j);
+      }
+    }
+    detections.push_back(box);
+  };
+
+  std::vector<Bbox3D> run(const std::string outputFilename) {
+    detector->updateBoxes(depth_image, detections);
+    auto boxes3D = detector->get3dDetections();
+    if (boxes3D) {
+      auto res = boxes3D.value();
+
+      // Draw the bounding boxes on the image
+      cv::Scalar color(0, 0, 0); // Green color for the rectangles
+      int thickness = 2;         // Thickness of the rectangle lines
+
+      for (const auto &box : res) {
+        LOG_INFO("Got detected box in 3D world coordinates at :",
+                 box.center.x(), ", ", box.center.y(), ", ", box.center.z());
+        LOG_INFO("Box size :", box.size.x(), ", ", box.size.y(), ", ",
+                 box.size.z());
+        cv::Point topLeft(box.center_img_frame.x() - box.size_img_frame.x() / 2,
+                          box.center_img_frame.y() -
+                              box.size_img_frame.y() / 2);
+        cv::Point bottomRight(
+            box.center_img_frame.x() + box.size_img_frame.x() / 2,
+            box.center_img_frame.y() + box.size_img_frame.y() / 2);
+        cv::rectangle(cv_img, topLeft, bottomRight, color, thickness);
+      }
+
+      // Save the modified image
+      cv::imwrite(outputFilename, cv_img);
+
+      if (!cv::imwrite(outputFilename, cv_img)) {
+        LOG_ERROR("Could not save the image");
+      } else {
+        LOG_INFO("Image saved to ", outputFilename);
+      }
+      BOOST_TEST(res.size() == 1, "Got different size for 3D and 2D boxes");
+      BOOST_TEST(res[0].size_img_frame.x() == detections[0].size.x(),
+                 "Error parsing box, size x in pixel frame is not conserved");
+      BOOST_TEST(res[0].size_img_frame.y() == detections[0].size.y(),
+                 "Error parsing box, size y in pixel frame is not conserved");
+      return res;
+    }
+    throw std::runtime_error("Could not find 3D boxes");
+  }
+};
+
+BOOST_AUTO_TEST_CASE(test_Depth_Detector_person_image) {
+  // Create timer
+  Timer time;
+  std::string filename =
+      "/home/ahr/kompass/uvmap_code/resources/depth_image.tif";
+  Bbox2D box({535, 0}, {520, 420});
+  auto config = DepthDetectorTestConfig(filename, box);
+  std::string outputFilename =
+      "/home/ahr/kompass/uvmap_code/resources/image_output.jpg";
+  auto boxes = config.run(outputFilename);
+}
+
+BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image) {
+  // Create timer
+  Timer time;
+  std::string filename =
+      "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif";
+  Bbox2D box({410, 0}, {410, 390});
+  auto config = DepthDetectorTestConfig(filename, box);
+  std::string outputFilename =
+      "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg";
+  auto boxes = config.run(outputFilename);
+
+  const float approx_actual_dist = 1.0;
+  BOOST_TEST(std::abs(boxes[0].center.x() - approx_actual_dist) <= 0.1 ,
+             "3D box distance is not equal to approximate measured distance");
+}

From 142a3738ff059dcb1474fd9eabec82a1fe6d0f7b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 29 Apr 2025 17:52:55 +0200
Subject: [PATCH 046/118] (refactor) Uses Eigen vectors for TF arguments

---
 src/kompass_cpp/bindings/bindings_control.cpp | 29 +++---
 src/kompass_cpp/bindings/bindings_gpu.cpp     |  5 +-
 src/kompass_cpp/bindings/bindings_utils.cpp   |  6 +-
 .../kompass_cpp/include/controllers/dwa.h     | 43 ++++-----
 .../kompass_cpp/include/datatypes/path.h      |  1 -
 .../include/utils/collision_check.h           |  6 +-
 .../include/utils/cost_evaluator.h            |  4 +-
 .../include/utils/critical_zone_check.h       |  4 +-
 .../include/utils/critical_zone_check_gpu.h   | 10 ++-
 .../include/utils/trajectory_sampler.h        |  9 +-
 .../include/utils/transformation.h            |  6 +-
 .../kompass_cpp/src/controllers/dwa.cpp       | 48 +++++-----
 .../src/controllers/vision_dwa.cpp            | 88 ++++++++++++++-----
 .../kompass_cpp/src/utils/collision_check.cpp | 24 +++--
 .../kompass_cpp/src/utils/cost_evaluator.cpp  | 29 +++---
 .../src/utils/cost_evaluator_gpu.cpp          | 23 ++---
 .../src/utils/critical_zone_check.cpp         | 12 +--
 .../src/utils/trajectory_sampler.cpp          |  8 +-
 18 files changed, 192 insertions(+), 163 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 4162392e..ff2c66f1 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -1,6 +1,5 @@
 #include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
-#include <nanobind/stl/array.h>
 #include <nanobind/stl/function.h>
 #include <nanobind/stl/tuple.h>
 #include <nanobind/stl/vector.h>
@@ -167,8 +166,8 @@ void bindings_control(py::module_ &m) {
   py::class_<Control::DWA, Control::Follower>(m_control, "DWA")
       .def(py::init<Control::ControlLimitsParams, Control::ControlType, double,
                     double, double, int, int, CollisionChecker::ShapeType,
-                    std::vector<float>, const std::array<float, 3> &,
-                    const std::array<float, 4> &, double,
+                    std::vector<float>, const Eigen::Vector3f &,
+                    const Eigen::Quaternionf &, double,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("control_limits"), py::arg("control_type"),
            py::arg("time_step"), py::arg("prediction_horizon"),
@@ -181,7 +180,7 @@ void bindings_control(py::module_ &m) {
       .def(py::init<Control::TrajectorySampler::TrajectorySamplerParameters,
                     Control::ControlLimitsParams, Control::ControlType,
                     CollisionChecker::ShapeType, std::vector<float>,
-                    const std::array<float, 3> &, const std::array<float, 4> &,
+                    const Eigen::Vector3f &, const Eigen::Quaternionf &,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("config"), py::arg("control_limits"),
            py::arg("control_type"), py::arg("robot_shape_type"),
@@ -245,20 +244,24 @@ void bindings_control(py::module_ &m) {
       .def(py::init<const Control::ControlType &,
                     const Control::ControlLimitsParams &, const int, const int,
                     const CollisionChecker::ShapeType &,
-                    const std::vector<float> &, const std::array<float, 3> &,
-                    const std::array<float, 4> &, const double,
+                    const std::vector<float> &, const Eigen::Vector3f &,
+                    const Eigen::Quaternionf &, const Eigen::Vector3f &,
+                    const Eigen::Quaternionf &, const double,
                     const Control::CostEvaluator::TrajectoryCostsWeights &,
-                    const int, const Control::VisionDWA::VisionDWAConfig &,
-                    const bool>(),
+                    const int, const Control::VisionDWA::VisionDWAConfig &>(),
            py::arg("control_type"), py::arg("control_limits"),
            py::arg("max_linear_samples"), py::arg("max_angular_samples"),
            py::arg("robot_shape_type"), py::arg("robot_dimensions"),
-           py::arg("sensor_position_wrt_body"),
-           py::arg("sensor_rotation_wrt_body"), py::arg("octree_res"),
+           py::arg("proximity_sensor_position_wrt_body"),
+           py::arg("proximity_sensor_rotation_wrt_body"),
+           py::arg("vision_sensor_position_wrt_body"),
+           py::arg("vision_sensor_rotation_wrt_body"), py::arg("octree_res"),
            py::arg("cost_weights"), py::arg("max_num_threads") = 1,
-           py::arg("config") = Control::VisionDWA::VisionDWAConfig(),
-           py::arg("use_tracker") = true)
-      .def("set_initial_tracking", &Control::VisionDWA::setInitialTracking)
+           py::arg("config") = Control::VisionDWA::VisionDWAConfig())
+      .def("set_initial_tracking",
+           py::overload_cast<const int, const int,
+                             const std::vector<Bbox3D> &>(
+               &Control::VisionDWA::setInitialTracking))
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
diff --git a/src/kompass_cpp/bindings/bindings_gpu.cpp b/src/kompass_cpp/bindings/bindings_gpu.cpp
index 0e1657ed..4bd75b13 100644
--- a/src/kompass_cpp/bindings/bindings_gpu.cpp
+++ b/src/kompass_cpp/bindings/bindings_gpu.cpp
@@ -3,7 +3,6 @@
 #include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/vector.h>
-#include <nanobind/stl/array.h>
 
 namespace py = nanobind;
 using namespace Kompass;
@@ -27,8 +26,8 @@ void bindings_utils_gpu(py::module_ &m) {
 
   py::class_<CriticalZoneCheckerGPU>(m, "CriticalZoneCheckerGPU")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const std::array<float, 3> &, const std::array<float, 4> &,
-                    float, float, const std::vector<double> &>(),
+                    const Eigen::Vector3f &, const Eigen::Quaternionf &, float,
+                    float, const std::vector<double> &>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
            py::arg("critical_angle"), py::arg("critical_distance"),
diff --git a/src/kompass_cpp/bindings/bindings_utils.cpp b/src/kompass_cpp/bindings/bindings_utils.cpp
index 23c3398d..dc5b7a0d 100644
--- a/src/kompass_cpp/bindings/bindings_utils.cpp
+++ b/src/kompass_cpp/bindings/bindings_utils.cpp
@@ -1,7 +1,7 @@
 #include "utils/critical_zone_check.h"
+#include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/vector.h>
-#include <nanobind/stl/array.h>
 
 namespace py = nanobind;
 using namespace Kompass;
@@ -16,8 +16,8 @@ void bindings_utils(py::module_ &m) {
 
   py::class_<CriticalZoneChecker>(m_utils, "CriticalZoneChecker")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const std::array<float, 3> &, const std::array<float, 4> &,
-                    float, float>(),
+                    const Eigen::Vector3f &, const Eigen::Quaternionf &, float,
+                    float>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
            py::arg("critical_angle"), py::arg("critical_distance"))
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 2ef5aa63..0d90eccf 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -7,6 +7,7 @@
 #include "follower.h"
 #include "utils/cost_evaluator.h"
 #include "utils/trajectory_sampler.h"
+#include <Eigen/Dense>
 #include <memory>
 #include <vector>
 
@@ -23,8 +24,8 @@ class DWA : public Follower {
       int maxLinearSamples, int maxAngularSamples,
       const CollisionChecker::ShapeType robotShapeType,
       const std::vector<float> robotDimensions,
-      const std::array<float, 3> &sensor_position_body,
-      const std::array<float, 4> &sensor_rotation_body, const double octreeRes,
+      const Eigen::Vector3f &sensor_position_body,
+      const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes,
       CostEvaluator::TrajectoryCostsWeights costWeights,
       const int maxNumThreads = 1);
 
@@ -32,8 +33,8 @@ class DWA : public Follower {
       ControlLimitsParams controlLimits, ControlType controlType,
       const CollisionChecker::ShapeType robotShapeType,
       const std::vector<float> robotDimensions,
-      const std::array<float, 3> &sensor_position_body,
-      const std::array<float, 4> &sensor_rotation_body,
+      const Eigen::Vector3f &sensor_position_body,
+      const Eigen::Quaternionf &sensor_rotation_body,
       CostEvaluator::TrajectoryCostsWeights costWeights,
       const int maxNumThreads = 1);
 
@@ -49,25 +50,25 @@ class DWA : public Follower {
    */
   // void reconfigure(DWAConfig &cfg);
   void configure(ControlLimitsParams controlLimits, ControlType controlType,
-                   double timeStep, double predictionHorizon,
-                   double controlHorizon, int maxLinearSamples,
-                   int maxAngularSamples,
-                   const CollisionChecker::ShapeType robotShapeType,
-                   const std::vector<float> robotDimensions,
-                   const std::array<float, 3> &sensor_position_body,
-                   const std::array<float, 4> &sensor_rotation_body,
-                   const double octreeRes,
-                   CostEvaluator::TrajectoryCostsWeights costWeights,
-                   const int maxNumThreads = 1);
+                 double timeStep, double predictionHorizon,
+                 double controlHorizon, int maxLinearSamples,
+                 int maxAngularSamples,
+                 const CollisionChecker::ShapeType robotShapeType,
+                 const std::vector<float> robotDimensions,
+                 const Eigen::Vector3f &sensor_position_body,
+                 const Eigen::Quaternionf &sensor_rotation_body,
+                 const double octreeRes,
+                 CostEvaluator::TrajectoryCostsWeights costWeights,
+                 const int maxNumThreads = 1);
 
   void configure(TrajectorySampler::TrajectorySamplerParameters config,
-                   ControlLimitsParams controlLimits, ControlType controlType,
-                   const CollisionChecker::ShapeType robotShapeType,
-                   const std::vector<float> robotDimensions,
-                   const std::array<float, 3> &sensor_position_body,
-                   const std::array<float, 4> &sensor_rotation_body,
-                   CostEvaluator::TrajectoryCostsWeights costWeights,
-                   const int maxNumThreads = 1);
+                 ControlLimitsParams controlLimits, ControlType controlType,
+                 const CollisionChecker::ShapeType robotShapeType,
+                 const std::vector<float> robotDimensions,
+                 const Eigen::Vector3f &sensor_position_body,
+                 const Eigen::Quaternionf &sensor_rotation_body,
+                 CostEvaluator::TrajectoryCostsWeights costWeights,
+                 const int maxNumThreads = 1);
 
   /**
    * @brief Reset the resolution of the robot's local map, this is equivalent to the box size representing each obstacle after conversion
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index 89dc7f10..ee3fe241 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -4,7 +4,6 @@
 #include "utils/spline.h"
 #include <Eigen/Dense>
 #include <cmath>
-#include <math.h>
 #include <vector>
 
 namespace Path {
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
index a8e98ff7..84f86b54 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
@@ -2,8 +2,6 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
 #include <memory>
 #include <vector>
 
@@ -50,8 +48,8 @@ class CollisionChecker {
    */
   CollisionChecker(const ShapeType robot_shape_type,
                    const std::vector<float> &robot_dimensions,
-                   const std::array<float, 3> &sensor_position_body,
-                   const std::array<float, 4> &sensor_rotation_body,
+                   const Eigen::Vector3f &sensor_position_body,
+                   const Eigen::Quaternionf &sensor_rotation_body,
                    const double octree_resolution = 0.01);
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
index bf22cdff..c35ad07a 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
@@ -63,8 +63,8 @@ class CostEvaluator {
                 ControlLimitsParams ctrLimits, size_t maxNumTrajectories,
                 size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize);
   CostEvaluator(TrajectoryCostsWeights &costsWeights,
-                const std::array<float, 3> &sensor_position_body,
-                const std::array<float, 4> &sensor_rotation_body,
+                const Eigen::Vector3f &sensor_position_body,
+                const Eigen::Quaternionf &sensor_rotation_body,
                 ControlLimitsParams ctrLimits, size_t maxNumTrajectories,
                 size_t numPointsPerTrajectory, size_t maxRefPathSegmentSize);
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
index b7754581..b6b633f7 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
@@ -21,8 +21,8 @@ class CriticalZoneChecker {
    */
   CriticalZoneChecker(const CollisionChecker::ShapeType robot_shape_type,
                       const std::vector<float> &robot_dimensions,
-                      const std::array<float, 3> &sensor_position_body,
-                      const std::array<float, 4> &sensor_rotation_body,
+                      const Eigen::Vector3f &sensor_position_body,
+                      const Eigen::Quaternionf &sensor_rotation_body,
                       const float critical_angle,
                       const float critical_distance);
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
index 2a56a43e..d3766c92 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
@@ -18,8 +18,8 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
   // Constructor
   CriticalZoneCheckerGPU(const CollisionChecker::ShapeType robot_shape_type,
                          const std::vector<float> &robot_dimensions,
-                         const std::array<float, 3> &sensor_position_body,
-                         const std::array<float, 4> &sensor_rotation_body,
+                         const Eigen::Vector3f &sensor_position_body,
+                         const Eigen::Quaternionf &sensor_rotation_body,
                          const float critical_angle,
                          const float critical_distance,
                          const std::vector<double> &angles)
@@ -44,9 +44,11 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
     m_devicePtrBackward =
         sycl::malloc_device<size_t>(indicies_backward_.size(), m_q);
     m_q.memcpy(m_devicePtrForward, indicies_forward_.data(),
-               sizeof(size_t) * indicies_forward_.size()).wait();
+               sizeof(size_t) * indicies_forward_.size())
+        .wait();
     m_q.memcpy(m_devicePtrBackward, indicies_backward_.data(),
-               sizeof(size_t) * indicies_backward_.size()).wait();
+               sizeof(size_t) * indicies_backward_.size())
+        .wait();
 
     m_scan_in_zone =
         std::max(indicies_forward_.size(), indicies_backward_.size());
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 0862f92c..0055a706 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -7,7 +7,6 @@
 #include "datatypes/trajectory.h"
 #include <array>
 #include <cmath>
-#include <functional>
 #include <memory>
 #include <vector>
 
@@ -79,16 +78,16 @@ class TrajectorySampler {
                     int maxAngularSamples,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
-                    const std::array<float, 3> &sensor_position_body,
-                    const std::array<float, 4> &sensor_rotation_body,
+                    const Eigen::Vector3f &sensor_position_body,
+                    const Eigen::Quaternionf &sensor_rotation_body,
                     const double octreeRes, const int maxNumThreads = 1);
 
   TrajectorySampler(TrajectorySamplerParameters config,
                     ControlLimitsParams controlLimits, ControlType controlType,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
-                    const std::array<float, 3> &sensor_position_body,
-                    const std::array<float, 4> &sensor_rotation_body,
+                    const Eigen::Vector3f &sensor_position_body,
+                    const Eigen::Quaternionf &sensor_rotation_body,
                     const int maxNumThreads = 1);
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
index 95bca210..b4fe99e4 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
@@ -23,12 +23,12 @@ getTransformation(const RotationType &rotation_src_to_goal,
   // Create a transformation matrix
   Eigen::Isometry3f transform_src_to_goal = Eigen::Isometry3f::Identity();
 
+  // Set translation
+  transform_src_to_goal.translate(translation_src_to_goal);
+
   // Set rotation based on the type of RotationType
   transform_src_to_goal.rotate(rotation_src_to_goal);
 
-  // Set translation
-  transform_src_to_goal.pretranslate(translation_src_to_goal);
-
   return transform_src_to_goal;
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index a63c62b1..3aa2db22 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -3,8 +3,6 @@
 #include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
-#include <algorithm>
-#include <array>
 #include <cmath>
 #include <memory>
 #include <stdexcept>
@@ -17,9 +15,8 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          int maxLinearSamples, int maxAngularSamples,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
-         const std::array<float, 3> &sensor_position_body,
-         const std::array<float, 4> &sensor_rotation_body,
-         const double octreeRes,
+         const Eigen::Vector3f &sensor_position_body,
+         const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
@@ -43,8 +40,8 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          ControlLimitsParams controlLimits, ControlType controlType,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
-         const std::array<float, 3> &sensor_position_body,
-         const std::array<float, 4> &sensor_rotation_body,
+         const Eigen::Vector3f &sensor_position_body,
+         const Eigen::Quaternionf &sensor_rotation_body,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
@@ -64,17 +61,17 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
   }
 }
 
-void DWA::configure(ControlLimitsParams controlLimits,
-                      ControlType controlType, double timeStep,
-                      double predictionHorizon, double controlHorizon,
-                      int maxLinearSamples, int maxAngularSamples,
-                      const CollisionChecker::ShapeType robotShapeType,
-                      const std::vector<float> robotDimensions,
-                      const std::array<float, 3> &sensor_position_body,
-                      const std::array<float, 4> &sensor_rotation_body,
-                      const double octreeRes,
-                      CostEvaluator::TrajectoryCostsWeights costWeights,
-                      const int maxNumThreads) {
+void DWA::configure(ControlLimitsParams controlLimits, ControlType controlType,
+                    double timeStep, double predictionHorizon,
+                    double controlHorizon, int maxLinearSamples,
+                    int maxAngularSamples,
+                    const CollisionChecker::ShapeType robotShapeType,
+                    const std::vector<float> robotDimensions,
+                    const Eigen::Vector3f &sensor_position_body,
+                    const Eigen::Quaternionf &sensor_rotation_body,
+                    const double octreeRes,
+                    CostEvaluator::TrajectoryCostsWeights costWeights,
+                    const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
@@ -88,14 +85,13 @@ void DWA::configure(ControlLimitsParams controlLimits,
 }
 
 void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config,
-                      ControlLimitsParams controlLimits,
-                      ControlType controlType,
-                      const CollisionChecker::ShapeType robotShapeType,
-                      const std::vector<float> robotDimensions,
-                      const std::array<float, 3> &sensor_position_body,
-                      const std::array<float, 4> &sensor_rotation_body,
-                      CostEvaluator::TrajectoryCostsWeights costWeights,
-                      const int maxNumThreads) {
+                    ControlLimitsParams controlLimits, ControlType controlType,
+                    const CollisionChecker::ShapeType robotShapeType,
+                    const std::vector<float> robotDimensions,
+                    const Eigen::Vector3f &sensor_position_body,
+                    const Eigen::Quaternionf &sensor_rotation_body,
+                    CostEvaluator::TrajectoryCostsWeights costWeights,
+                    const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
       sensor_position_body, sensor_rotation_body, maxNumThreads);
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 3d9180f6..5395da8c 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -1,8 +1,11 @@
 #include "controllers/vision_dwa.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
+#include "datatypes/tracking.h"
 #include "utils/angles.h"
 #include "utils/logger.h"
+#include "utils/transformation.h"
+#include "vision/depth_detector.h"
 #include <algorithm>
 #include <cmath>
 #include <memory>
@@ -16,25 +19,37 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
                      const int maxLinearSamples, const int maxAngularSamples,
                      const CollisionChecker::ShapeType &robotShapeType,
                      const std::vector<float> &robotDimensions,
-                     const std::array<float, 3> &sensor_position_body,
-                     const std::array<float, 4> &sensor_rotation_body,
+                     const Eigen::Vector3f &proximity_sensor_position_body,
+                     const Eigen::Quaternionf &proximity_sensor_rotation_body,
+                     const Eigen::Vector3f &vision_sensor_position_body,
+                     const Eigen::Quaternionf &vision_sensor_rotation_body,
                      const double octreeRes,
                      const CostEvaluator::TrajectoryCostsWeights &costWeights,
-                     const int maxNumThreads, const VisionDWAConfig &config,
-                     const bool use_tracker)
+                     const int maxNumThreads, const VisionDWAConfig &config)
     : DWA(ctrlLimits, robotCtrlType, config.control_time_step(),
           config.prediction_horizon(), config.control_horizon(),
           maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-          sensor_position_body, sensor_rotation_body, octreeRes, costWeights,
-          maxNumThreads) {
+          proximity_sensor_position_body, proximity_sensor_rotation_body,
+          octreeRes, costWeights, maxNumThreads) {
   ctrl_limits_ = ctrlLimits;
   config_ = config;
-  if (use_tracker) {
-    // Initialize the bounding box tracker
-    tracker_ = std::make_unique<FeatureBasedBboxTracker>(
-        config.control_time_step(), config.e_pose(), config.e_vel(),
-        config.e_acc());
-  }
+  // Initialize the bounding box tracker
+  tracker_ = std::make_unique<FeatureBasedBboxTracker>(
+      config.control_time_step(), config.e_pose(), config.e_vel(),
+      config.e_acc());
+  vision_sensor_tf_ = getTransformation(vision_sensor_rotation_body,
+                                        vision_sensor_position_body);
+}
+
+void VisionDWA::setCameraIntrinsics(const float focal_length_x,
+                                    const float focal_length_y,
+                                    const float principal_point_x,
+                                    const float principal_point_y) {
+  detector_ = std::make_unique<DepthDetector>(
+      config_.depth_range(), vision_sensor_tf_,
+      Eigen::Vector2f{focal_length_x, focal_length_y},
+      Eigen::Vector2f{principal_point_x, principal_point_y},
+      config_.depth_conversion_factor());
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
@@ -64,15 +79,14 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
                    ctrl_limits_.velXParams.maxVel);
     followingVel.setVx(v);
     double omega;
-    if (distance > 0.0){
+    if (distance > 0.0) {
       omega = -tracking_pose.omega() +
               2.0 * (v * sin(psi) / distance +
                      tracking_pose.v() * sin(gamma - psi) / distance -
                      config_.K_omega() * tanh(angle_error));
-    }
-    else{
-      omega = -tracking_pose.omega() -
-              2.0 * config_.K_omega() * tanh(angle_error);
+    } else {
+      omega =
+          -tracking_pose.omega() - 2.0 * config_.K_omega() * tanh(angle_error);
     }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
@@ -81,15 +95,43 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   return followingVel;
 }
 
-bool VisionDWA::setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
                                    const std::vector<Bbox3D> &detected_boxes) {
-  if (!tracker_) {
-    LOG_ERROR("Tracker is not initialized. Cannot use "
-              "'VisionDWA::setInitialTracking'. Initialize VisionDWA "
-              "with 'use_tracker' = true");
+  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
+}
+
+bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
+                                   const Eigen::MatrixXi &aligned_depth_image,
+                                   const std::vector<Bbox2D> &detected_boxes) {
+  if (!detector_) {
+    throw std::runtime_error(
+        "DepthDetector is not initialized with the camera intrinsics. Call "
+        "'VisionDWA::setCameraIntrinsics' first");
+  }
+  std::unique_ptr<Bbox2D> target_box;
+  for (auto box : detected_boxes) {
+    auto limits_x = box.getXLimits();
+    if (pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)) {
+      auto limits_y = box.getYLimits();
+      if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) {
+        target_box = std::make_unique<Bbox2D>(box);
+        break;
+      }
+    }
+  }
+  if (!target_box) {
+    LOG_DEBUG("Target point not found in any detected box");
     return false;
   }
-  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
+  // Send current state to the detector
+  detector_->updateState(currentState);
+  detector_->updateBoxes(aligned_depth_image, {*target_box});
+  auto boxes_3d = detector_->get3dDetections();
+  if (!boxes_3d) {
+    LOG_DEBUG("Failed to get 3D box from 2D target box");
+    return false;
+  }
+  return tracker_->setInitialTracking(boxes_3d.value()[0]);
 }
 
 } // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
index 2e327246..53702707 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
@@ -1,8 +1,7 @@
 #include "utils/collision_check.h"
 #include "utils/logger.h"
 #include "utils/transformation.h"
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
+#include <Eigen/Dense>
 #include <algorithm>
 #include <cmath>
 #include <fcl/broadphase/default_broadphase_callbacks.h>
@@ -19,8 +18,8 @@ namespace Kompass {
 CollisionChecker::CollisionChecker(
     const ShapeType robot_shape_type,
     const std::vector<float> &robot_dimensions,
-    const std::array<float, 3> &sensor_position_body,
-    const std::array<float, 4> &sensor_rotation_body,
+    const Eigen::Vector3f &sensor_position_body,
+    const Eigen::Quaternionf &sensor_rotation_body,
     const double octree_resolution) {
 
   collManager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerf>();
@@ -63,8 +62,7 @@ CollisionChecker::CollisionChecker(
 
   // Init the sensor position w.r.t body
   sensor_tf_body_ =
-      getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()),
-                        Eigen::Vector3f(sensor_position_body.data()));
+      getTransformation(sensor_rotation_body, sensor_position_body);
 
   sensor_tf_world_ = sensor_tf_body_;
 }
@@ -104,6 +102,9 @@ void CollisionChecker::generateBoxesFromOctomap(
                                   sensor_tf_world_.translation() +
                                       Eigen::Vector3f(x, y, z)); // translation
     obj->computeAABB();
+    // Uncomment for debug
+    // auto trans = obj->getTransform();
+    // std::cout << "Adding box with translation " << trans.translation() << std::endl;
     boxes.push_back(obj);
   }
 
@@ -111,8 +112,6 @@ void CollisionChecker::generateBoxesFromOctomap(
 }
 
 void CollisionChecker::updateOctreePtr() {
-  // Create fcl::OcTree from octomap::OcTree
-  // fclTree_ = new fcl::OcTreef(octTree_);
   fclTree_.reset(new fcl::OcTreef(octTree_));
 
   // Transform the tree into a set of boxes and generate collision objects in
@@ -166,7 +165,7 @@ void CollisionChecker::convertPointCloudToOctomap(
     sensor_tf_world_ = Eigen::Isometry3f::Identity();
   } else {
     // Transform the sensor position to the world frame
-    sensor_tf_world_ = sensor_tf_body_ * body->tf;
+    sensor_tf_world_ = body->tf * sensor_tf_body_;
   }
 
   // Clear old data
@@ -192,7 +191,7 @@ void CollisionChecker::convertPointCloudToOctomap(
     sensor_tf_world_ = Eigen::Isometry3f::Identity();
   } else {
     // Transform the sensor position to the world frame
-    sensor_tf_world_ = sensor_tf_body_ * body->tf;
+    sensor_tf_world_ = body->tf * sensor_tf_body_;
   }
 
   // Clear old data
@@ -215,7 +214,7 @@ void CollisionChecker::convertLaserScanToOctomap(
   // Transform the sensor position to the world frame
   // NOTE: Transformation will be applied to the points when generating the
   // collision boxes
-  sensor_tf_world_ = sensor_tf_body_ * body->tf;
+  sensor_tf_world_ = body->tf * sensor_tf_body_;
 
   // Clear old data
   octTree_->clear();
@@ -230,7 +229,6 @@ void CollisionChecker::convertLaserScanToOctomap(
     x = ranges[i] * cos(angle);
     y = ranges[i] * sin(angle);
     z = height_in_sensor;
-
     octomapCloud_.push_back(x, y, z);
   }
 
@@ -246,7 +244,7 @@ void CollisionChecker::convertLaserScanToOctomap(
   // Transform the sensor position to the world frame
   // NOTE: Transformation will be applied to the points when generating the
   // collision boxes
-  sensor_tf_world_ = sensor_tf_body_ * body->tf;
+  sensor_tf_world_ = body->tf * sensor_tf_body_;
 
   // Clear old data
   octTree_->clear();
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
index 92d4db15..40bd8c08 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator.cpp
@@ -2,16 +2,13 @@
 #include "utils/cost_evaluator.h"
 #include "datatypes/path.h"
 #include "datatypes/trajectory.h"
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
-#include <cstddef>
-#include <cstdlib>
+#include <Eigen/Dense>
 #include <vector>
 
 namespace Kompass {
 
 namespace Control {
-CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights,
+CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights,
                              ControlLimitsParams ctrLimits,
                              size_t maxNumTrajectories,
                              size_t numPointsPerTrajectory,
@@ -23,37 +20,35 @@ CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights,
                 static_cast<float>(ctrLimits.omegaParams.maxAcceleration)};
 }
 
-CostEvaluator::CostEvaluator(TrajectoryCostsWeights& costsWeights,
-                             const std::array<float, 3> &sensor_position_body,
-                             const std::array<float, 4> &sensor_rotation_body,
+CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights,
+                             const Eigen::Vector3f &sensor_position_body,
+                             const Eigen::Quaternionf &sensor_rotation_body,
                              ControlLimitsParams ctrLimits,
                              size_t maxNumTrajectories,
                              size_t numPointsPerTrajectory,
                              size_t maxRefPathSegmentSize) {
 
   sensor_tf_body_ =
-      getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()),
-                        Eigen::Vector3f(sensor_position_body.data()));
+      getTransformation(sensor_rotation_body, sensor_position_body);
   this->costWeights = std::make_unique<TrajectoryCostsWeights>(costsWeights);
   accLimits_ = {static_cast<float>(ctrLimits.velXParams.maxAcceleration),
                 static_cast<float>(ctrLimits.velYParams.maxAcceleration),
                 static_cast<float>(ctrLimits.omegaParams.maxAcceleration)};
 }
 
-void CostEvaluator::updateCostWeights(TrajectoryCostsWeights& costsWeights) {
+void CostEvaluator::updateCostWeights(TrajectoryCostsWeights &costsWeights) {
   this->costWeights = std::make_unique<TrajectoryCostsWeights>(costsWeights);
 }
 
 CostEvaluator::~CostEvaluator() {
 
-  //Clear custom cost pointers
+  // Clear custom cost pointers
   customTrajCostsPtrs_.clear();
 };
 
 TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     const std::unique_ptr<TrajectorySamples2D> &trajs,
-    const Path::Path* reference_path,
-    const Path::Path &tracked_segment) {
+    const Path::Path *reference_path, const Path::Path &tracked_segment) {
   double weight;
   float total_cost;
   float ref_path_length;
@@ -66,12 +61,14 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     if ((ref_path_length = reference_path->totalPathLength()) > 0.0) {
       if ((weight = costWeights->getParameter<double>("goal_distance_weight")) >
           0.0) {
-        float goalCost = goalCostFunc(traj, reference_path->getEnd(), ref_path_length);
+        float goalCost =
+            goalCostFunc(traj, reference_path->getEnd(), ref_path_length);
         total_cost += weight * goalCost;
       }
       if ((weight = costWeights->getParameter<double>(
                "reference_path_distance_weight")) > 0.0) {
-        float refPathCost = pathCostFunc(traj, tracked_segment, tracked_segment.totalPathLength());
+        float refPathCost = pathCostFunc(traj, tracked_segment,
+                                         tracked_segment.totalPathLength());
         total_cost += weight * refPathCost;
       }
     }
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
index 71dde07f..2cba6a7e 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
@@ -3,10 +3,8 @@
 #include "datatypes/trajectory.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
-#include <cstddef>
-#include <cstdlib>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
 #include <limits>
 #include <sycl/sycl.hpp>
 #include <vector>
@@ -33,16 +31,15 @@ CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights,
 }
 
 CostEvaluator::CostEvaluator(TrajectoryCostsWeights &costsWeights,
-                             const std::array<float, 3> &sensor_position_body,
-                             const std::array<float, 4> &sensor_rotation_body,
+                             const Eigen::Vector3f &sensor_position_body,
+                             const Eigen::Quaternionf &sensor_rotation_body,
                              ControlLimitsParams ctrLimits,
                              size_t maxNumTrajectories,
                              size_t numPointsPerTrajectory,
                              size_t maxRefPathSegmentSize) {
 
   sensor_tf_body_ =
-      getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()),
-                        Eigen::Vector3f(sensor_position_body.data()));
+      getTransformation(sensor_rotation_body, sensor_position_body);
   this->costWeights = std::make_unique<TrajectoryCostsWeights>(costsWeights);
 
   accLimits_ = {static_cast<float>(ctrLimits.velXParams.maxAcceleration),
@@ -193,7 +190,7 @@ CostEvaluator::~CostEvaluator() {
 
 TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     const std::unique_ptr<TrajectorySamples2D> &trajs,
-    const Path::Path* reference_path, const Path::Path &tracked_segment) {
+    const Path::Path *reference_path, const Path::Path &tracked_segment) {
 
   try {
     double weight;
@@ -228,11 +225,9 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
       if ((weight = costWeights->getParameter<double>("goal_distance_weight")) >
           0.0) {
         auto last_point = reference_path->getEnd();
-        m_deviceRefPathEnd = sycl::vec(last_point.x(),
-                                       last_point.y(),
-                                       last_point.z());
-        events.push_back(goalCostFunc(trajs_size,
-                                      ref_path_length, weight));
+        m_deviceRefPathEnd =
+            sycl::vec(last_point.x(), last_point.y(), last_point.z());
+        events.push_back(goalCostFunc(trajs_size, ref_path_length, weight));
       }
       if ((weight = costWeights->getParameter<double>(
                "reference_path_distance_weight")) > 0.0) {
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
index 265c7a04..f4f8823a 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
@@ -13,9 +13,9 @@ namespace Kompass {
 CriticalZoneChecker::CriticalZoneChecker(
     const CollisionChecker::ShapeType robot_shape_type,
     const std::vector<float> &robot_dimensions,
-    const std::array<float, 3> &sensor_position_body,
-    const std::array<float, 4> &sensor_rotation_body,
-    const float critical_angle, const float critical_distance) {
+    const Eigen::Vector3f &sensor_position_body,
+    const Eigen::Quaternionf &sensor_rotation_body, const float critical_angle,
+    const float critical_distance) {
   // Construct  a geometry object based on the robot shape
   if (robot_shape_type == CollisionChecker::ShapeType::CYLINDER) {
     robotHeight_ = robot_dimensions.at(1);
@@ -31,8 +31,7 @@ CriticalZoneChecker::CriticalZoneChecker(
 
   // Init the sensor position w.r.t body
   sensor_tf_body_ =
-      getTransformation(Eigen::Quaternionf(sensor_rotation_body.data()),
-                        Eigen::Vector3f(sensor_position_body.data()));
+      getTransformation(sensor_rotation_body, sensor_position_body);
   // Compute the critical zone angles min,max
   float angle_rad = critical_angle * M_PI / 180.0;
   angle_right_forward_ = angle_rad / 2;
@@ -40,7 +39,8 @@ CriticalZoneChecker::CriticalZoneChecker(
   angle_right_backward_ = Angle::normalizeTo0Pi(M_PI + angle_right_forward_);
   angle_left_backward_ = Angle::normalizeTo0Pi(M_PI + angle_left_forward_);
 
-  LOG_DEBUG("Critical zone forward angles: [", angle_right_forward_, ", ", angle_left_forward_, "]");
+  LOG_DEBUG("Critical zone forward angles: [", angle_right_forward_, ", ",
+            angle_left_forward_, "]");
   LOG_DEBUG("Critical zone backward angles: [", angle_right_backward_, ", ",
             angle_left_backward_, "]");
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index f5084dcb..1d44142d 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -27,8 +27,8 @@ TrajectorySampler::TrajectorySampler(
     double predictionHorizon, double controlHorizon, int maxLinearSamples,
     int maxAngularSamples, const CollisionChecker::ShapeType robotShapeType,
     const std::vector<float> robotDimensions,
-    const std::array<float, 3> &sensor_position_body,
-    const std::array<float, 4> &sensor_rotation_body, const double octreeRes,
+    const Eigen::Vector3f &sensor_position_body,
+    const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes,
     const int maxNumThreads) {
   // Setup the collision checker
   collChecker = std::make_unique<CollisionChecker>(
@@ -62,8 +62,8 @@ TrajectorySampler::TrajectorySampler(
     TrajectorySamplerParameters config, ControlLimitsParams controlLimits,
     ControlType controlType, const CollisionChecker::ShapeType robotShapeType,
     const std::vector<float> robotDimensions,
-    const std::array<float, 3> &sensor_position_body,
-    const std::array<float, 4> &sensor_rotation_body, const int maxNumThreads) {
+    const Eigen::Vector3f &sensor_position_body,
+    const Eigen::Quaternionf &sensor_rotation_body, const int maxNumThreads) {
   double octreeRes = config.getParameter<double>("octree_map_resolution");
   // Setup the collision checker
   collChecker = std::make_unique<CollisionChecker>(

From 3ee023660fe257b985fd579885c0cd0c2ef9833b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 29 Apr 2025 17:53:23 +0200
Subject: [PATCH 047/118] (feature) Adds DepthDetector to VisionDWA and updates
 tests

---
 .../include/controllers/vision_dwa.h          | 116 ++++++++++++---
 .../kompass_cpp/include/datatypes/tracking.h  |  32 ++++
 .../kompass_cpp/src/vision/tracker.cpp        |   6 +-
 src/kompass_cpp/tests/collisions_test.cpp     |  33 ++---
 src/kompass_cpp/tests/collisions_test_gpu.cpp |   5 +-
 src/kompass_cpp/tests/controller_test.cpp     |   4 +-
 src/kompass_cpp/tests/cost_evaluator_test.cpp |   4 +-
 .../tests/trajectory_sampler_test.cpp         |   7 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 139 +++++++++++++++---
 9 files changed, 273 insertions(+), 73 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 795a2ebf..98ffa628 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -2,13 +2,13 @@
 
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
-#include "datatypes/path.h"
 #include "datatypes/tracking.h"
-#include "datatypes/trajectory.h"
 #include "dwa.h"
 #include "utils/logger.h"
+#include "vision/depth_detector.h"
 #include "vision/tracker.h"
 #include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
 #include <cmath>
 #include <memory>
 #include <tuple>
@@ -58,6 +58,17 @@ class VisionDWA : public DWA {
       addParameter("error_pose", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_vel", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_acc", Parameter(0.05, 1e-9, 1e9));
+
+      // Depth parameters
+      addParameter("depth_conversion_factor",
+                   Parameter(1e-3, 1e-9, 1e9,
+                             "Factor to convert depth image values to meters"));
+      addParameter(
+          "min_depth",
+          Parameter(0.0, 0.0, 1e3, "Range of interest minimum depth value"));
+      addParameter(
+          "max_depth",
+          Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value"));
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
     double control_time_step() const {
@@ -96,6 +107,13 @@ class VisionDWA : public DWA {
     double e_pose() const { return getParameter<double>("error_pose"); }
     double e_vel() const { return getParameter<double>("error_vel"); }
     double e_acc() const { return getParameter<double>("error_acc"); }
+    double depth_conversion_factor() const {
+      return getParameter<double>("depth_conversion_factor");
+    }
+    Eigen::Vector2f depth_range() const {
+      return Eigen::Vector2f{getParameter<double>("min_depth"),
+                             getParameter<double>("max_depth")};
+    }
   };
 
   VisionDWA(const ControlType &robotCtrlType,
@@ -103,17 +121,23 @@ class VisionDWA : public DWA {
             const int maxAngularSamples,
             const CollisionChecker::ShapeType &robotShapeType,
             const std::vector<float> &robotDimensions,
-            const std::array<float, 3> &sensor_position_body,
-            const std::array<float, 4> &sensor_rotation_body,
+            const Eigen::Vector3f &proximity_sensor_position_body,
+            const Eigen::Quaternionf &proximity_sensor_rotation_body,
+            const Eigen::Vector3f &vision_sensor_position_body,
+            const Eigen::Quaternionf &vision_sensor_rotation_body,
             const double octreeRes,
             const CostEvaluator::TrajectoryCostsWeights &costWeights,
             const int maxNumThreads = 1,
-            const VisionDWAConfig &config = VisionDWAConfig(),
-            const bool use_tracker = true);
+            const VisionDWAConfig &config = VisionDWAConfig());
 
   // Default Destructor
   ~VisionDWA() = default;
 
+  void setCameraIntrinsics(const float focal_length_x,
+                           const float focal_length_y,
+                           const float principal_point_x,
+                           const float principal_point_y);
+
   /**
    * @brief Get the Pure Tracking Control Command
    *
@@ -136,8 +160,8 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracked_pose,
                                             const Velocity2D &current_vel,
                                             const T &sensor_points) {
-    LOG_INFO("Tracked pose: ", tracked_pose.x(),
-             tracked_pose.y(), tracked_pose.yaw());
+    LOG_DEBUG("Tracked pose: ", tracked_pose.x(), tracked_pose.y(),
+              tracked_pose.yaw());
     Trajectory2D ref_traj;
     bool has_collisions;
     std::tie(ref_traj, has_collisions) =
@@ -152,7 +176,7 @@ class VisionDWA : public DWA {
       latest_velocity_command_ = ref_traj.velocities.getFront();
       return result;
     } else {
-      LOG_INFO("USING DWA SAMPLING");
+      LOG_DEBUG("USING DWA SAMPLING");
       // The tracking sample has collisions -> use DWA-like sampling and control
       Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
                                    ref_traj.path.z,
@@ -179,21 +203,13 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult
   getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
                   const Velocity2D &current_vel, const T &sensor_points) {
-    if (!tracker_) {
-      throw std::invalid_argument(
-          "Tracker is not initialized. Cannot use "
-          "'VisionDWA::getTrackingCtrl' "
-          "directly with "
-          "'std::vector<Bbox3D> &detected_boxes' input. Initialize VisionDWA "
-          "with 'use_tracker' = true");
-    }
     if (tracker_->trackerInitialized()) {
       // Update the tracker with the detected boxes
       tracker_->updateTracking(detected_boxes);
       auto tracked_pose = tracker_->getFilteredTrackedPose2D();
       if (tracked_pose) {
         return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
-                                     sensor_points);
+                                        sensor_points);
       } else {
         LOG_WARNING("Tracker failed to find the target");
         // Return false for trajectory found
@@ -206,6 +222,46 @@ class VisionDWA : public DWA {
     }
   };
 
+  template <typename T>
+  Control::TrajSearchResult
+  getTrackingCtrl(const Eigen::MatrixXi &aligned_depth_img,
+                  const std::vector<Bbox2D> &detected_boxes_2d,
+                  const Velocity2D &current_vel, const T &sensor_points) {
+    if (!detector_) {
+      throw std::runtime_error(
+          "DepthDetector is not initialized with the camera intrinsics. Call "
+          "'VisionDWA::setCameraIntrinsics' first");
+    }
+    if (!tracker_->trackerInitialized()) {
+      throw std::runtime_error(
+          "Tracker is not initialized with an initial tracking target. Call "
+          "'VisionDWA::setInitialTracking' first");
+    }
+    // Send current state to the detector
+    detector_->updateState(currentState);
+    detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
+    auto boxes_3d = detector_->get3dDetections();
+
+    if (boxes_3d) {
+      LOG_DEBUG("Got 3D boxes from 2d");
+      // Update the tracker with the detected boxes
+      tracker_->updateTracking(boxes_3d.value());
+      auto tracked_pose = tracker_->getFilteredTrackedPose2D();
+      if (tracked_pose) {
+        return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
+                                        sensor_points);
+      } else {
+        LOG_WARNING("Tracker failed to find the target");
+        // Return false for trajectory found
+        return TrajSearchResult();
+      }
+    }else{
+      LOG_WARNING("Detector failed to find 3D boxes");
+      // Return false for trajectory found
+      return TrajSearchResult();
+    }
+  };
+
   /**
    * @brief Set the initial image position of the target to be tracked
    *
@@ -215,13 +271,29 @@ class VisionDWA : public DWA {
    * @return true
    * @return false
    */
-  bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+  bool setInitialTracking(const int pose_x_img, const int pose_y_img,
                           const std::vector<Bbox3D> &detected_boxes);
 
+  /**
+   * @brief  Set the initial image position of the target to be tracked using 2D
+   * detections
+   *
+   * @param pose_x_img
+   * @param pose_y_img
+   * @param detected_boxes_2d
+   * @return true
+   * @return false
+   */
+  bool setInitialTracking(const int pose_x_img, const int pose_y_img,
+                          const Eigen::MatrixXi &aligned_depth_image,
+                          const std::vector<Bbox2D> &detected_boxes_2d);
+
 private:
   ControlLimitsParams ctrl_limits_;
   VisionDWAConfig config_;
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
+  std::unique_ptr<DepthDetector> detector_;
+  Eigen::Isometry3f vision_sensor_tf_;
 
   /**
    * @brief Get the Tracking Reference Trajectory Segment and if this segment is
@@ -235,7 +307,7 @@ class VisionDWA : public DWA {
   template <typename T>
   std::tuple<Trajectory2D, bool>
   getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
-                              const T &sensor_points){
+                              const T &sensor_points) {
     int step = 0;
 
     Trajectory2D ref_traj(config_.prediction_horizon());
@@ -267,10 +339,10 @@ class VisionDWA : public DWA {
     bool has_collisions =
         trajSampler->checkStatesFeasibility(states, sensor_points);
 
-    LOG_INFO("Found reference traj with collisions: ", has_collisions);
+    LOG_DEBUG("Found reference traj with collisions: ", has_collisions);
 
     return std::make_tuple(ref_traj, has_collisions);
-                              };
+  };
   //
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index f6dbc3d8..9cb944db 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -7,6 +7,27 @@
 
 namespace Kompass {
 
+
+struct Bbox2D {
+  Eigen::Vector2i top_corner = {0, 0};
+  Eigen::Vector2i size = {0, 0};
+
+  Bbox2D() {};
+
+  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size){};
+
+  Bbox2D(const Eigen::Vector2i top_corner, Eigen::Vector2i size)
+      : top_corner(top_corner), size(size){};
+
+  Eigen::Vector2i getXLimits() const {
+    return {top_corner.x(), top_corner.x() + size.x()};
+  };
+
+  Eigen::Vector2i getYLimits() const {
+    return {top_corner.y(), top_corner.y() + size.y()};
+  };
+};
+
 struct Bbox3D {
   Eigen::Vector3f center = {0.0, 0.0, 0.0};
   Eigen::Vector3f size = {0.0, 0.0, 0.0};
@@ -27,6 +48,17 @@ struct Bbox3D {
         center_img_frame(center_img_frame),
         size_img_frame(size_img_frame), pc_points(pc_points){};
 
+  Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
+         const Bbox2D &box2d, std::vector<Eigen::Vector3f> pc_points = {})
+      : center(center), size(size), center_img_frame(box2d.top_corner + Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
+        size_img_frame(box2d.size), pc_points(pc_points){};
+
+  Bbox3D(const Bbox2D &box2d)
+      : center_img_frame(
+            box2d.top_corner +
+            Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
+        size_img_frame(box2d.size) {};
+
   Eigen::Vector2f getXLimitsImg() const {
     return {
       center_img_frame.x() - (size_img_frame.x() / 2),
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 43dead0a..8d399dd0 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -1,6 +1,7 @@
 #include "vision/tracker.h"
 #include "datatypes/control.h"
 #include "datatypes/tracking.h"
+#include "utils/logger.h"
 #include <cmath>
 #include <math.h>
 
@@ -60,11 +61,12 @@ bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
 }
 
 bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
+  LOG_DEBUG("Setting initial tracked box");
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
   Eigen::Matrix<float, StateSize, 1> state_vec =
       Eigen::Matrix<float, StateSize, 1>::Zero();
-  state_vec(0) = bBox.center[0];
-  state_vec(1) = bBox.center[1];
+  state_vec(0) = bBox.center.x();
+  state_vec(1) = bBox.center.y();
   stateKalmanFilter_->setInitialState(state_vec);
   return true;
 }
diff --git a/src/kompass_cpp/tests/collisions_test.cpp b/src/kompass_cpp/tests/collisions_test.cpp
index 0b1e4f34..dc62c46e 100644
--- a/src/kompass_cpp/tests/collisions_test.cpp
+++ b/src/kompass_cpp/tests/collisions_test.cpp
@@ -1,4 +1,7 @@
 #include "test.h"
+#include <Eigen/Dense>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <array>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "utils/angles.h"
 #include "utils/collision_check.h"
@@ -63,8 +66,8 @@ BOOST_AUTO_TEST_CASE(test_FCL) {
   auto robotShapeType = CollisionChecker::ShapeType::BOX;
   std::vector<float> robotDimensions{0.4, 0.4, 1.0};
 
-  const std::array<float, 3> sensor_position_body{0.0, 0.0, 1.0};
-  const std::array<float, 4> sensor_rotation_body{0, 0, 0, 1};
+  const Eigen::Vector3f sensor_position_body{0.0, 0.0, 1.0};
+  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1};
 
   // Robot start state (pose)
   Path::State robotState(0.0, 0.0, 0.0, 0.0);
@@ -78,30 +81,26 @@ BOOST_AUTO_TEST_CASE(test_FCL) {
                                octreeRes);
 
   LOG_INFO("Testing collision checker using Laserscan data");
-
+  LOG_INFO("Running test with robot at ", robotState.x, ", ", robotState.y);
+  LOG_INFO("Scan Ranges ", scan_ranges[0], ", ", scan_ranges[1], ", ",
+           scan_ranges[2]);
   collChecker.updateState(robotState);
 
   bool res_false = collChecker.checkCollisions(scan_ranges, scan_angles);
-  LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x,
-           ", y: ", robotState.y, "}\n",
-           "and Laserscan with: ranges {1.0, 1.0, 1.0, 1.0} at angles {0, 0.1, "
-           "0.2, 3.14}, Collision: ",
-           res_false);
   BOOST_TEST(!res_false, "Collision Result should be FALSE Got: " << res_false);
+  std::cout << std::endl;
 
   robotState.x = 3.0;
   robotState.y = 5.0;
-  collChecker.updateState(robotState);
-
   scan_ranges = {0.25, 0.5, 0.5};
+  LOG_INFO("Running test with robot at ", robotState.x, ", ", robotState.y);
+  LOG_INFO("Scan Ranges ", scan_ranges[0], ", ", scan_ranges[1], ", ",
+           scan_ranges[2]);
+  collChecker.updateState(robotState);
 
   bool res_true = collChecker.checkCollisions(scan_ranges, scan_angles);
-  LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x,
-           ", y: ", robotState.y, "}\n",
-           "and Laserscan with: ranges {0.2, 0.5, 0.5} at angles {0, 0.1, "
-           "0.2, 3.14} -> Collision: ",
-           res_true);
   BOOST_TEST(res_true, "Collision Result should be TRUE got: " << res_true);
+  std::cout << std::endl;
 
   LOG_INFO("Testing collision between: \nRobot at {x: ", robotState.x,
            ", y: ", robotState.y, "}\n", "and Pointcloud");
@@ -123,8 +122,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   auto robotShapeType = CollisionChecker::ShapeType::BOX;
   std::vector<float> robotDimensions{0.51, 0.27, 0.4};
 
-  const std::array<float, 3> sensor_position_body{0.22, 0.0, 0.4};
-  const std::array<float, 4> sensor_rotation_body{0, 0, 0.99, 0.0};
+  const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4};
+  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0.99, 0.0};
 
   // Robot laserscan value
   std::vector<double> scan_angles;
diff --git a/src/kompass_cpp/tests/collisions_test_gpu.cpp b/src/kompass_cpp/tests/collisions_test_gpu.cpp
index 507c582d..18d98cda 100644
--- a/src/kompass_cpp/tests/collisions_test_gpu.cpp
+++ b/src/kompass_cpp/tests/collisions_test_gpu.cpp
@@ -1,4 +1,5 @@
 #include "test.h"
+#include <Eigen/Dense>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "collisions_test.cpp"
 #include "utils/collision_check.h"
@@ -15,8 +16,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   auto robotShapeType = CollisionChecker::ShapeType::BOX;
   std::vector<float> robotDimensions{0.51, 0.27, 0.4};
 
-  const std::array<float, 3> sensor_position_body{0.22, 0.0, 0.4};
-  const std::array<float, 4> sensor_rotation_body{0, 0, 0.99, 0.0};
+  const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4};
+  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0.99, 0.0};
 
   // Robot laserscan value
   std::vector<double> scan_angles;
diff --git a/src/kompass_cpp/tests/controller_test.cpp b/src/kompass_cpp/tests/controller_test.cpp
index 0f621443..dee10d9f 100644
--- a/src/kompass_cpp/tests/controller_test.cpp
+++ b/src/kompass_cpp/tests/controller_test.cpp
@@ -65,8 +65,8 @@ BOOST_AUTO_TEST_CASE(test_DWA) {
   auto robotShapeType = Kompass::CollisionChecker::ShapeType::CYLINDER;
   std::vector<float> robotDimensions{0.1, 0.4};
   // std::array<float, 3> sensorPositionWRTbody {0.0, 0.0, 1.0};
-  const std::array<float, 3> sensor_position_body{0.0, 0.0, 0.0};
-  const std::array<float, 4> sensor_rotation_body{0, 0, 0, 1};
+  const Eigen::Vector3f sensor_position_body{0.0, 0.0, 0.0};
+  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1};
 
   // Robot start state (pose)
   Path::State robotState(-0.51731912, 0.0, 0.0, 0.0);
diff --git a/src/kompass_cpp/tests/cost_evaluator_test.cpp b/src/kompass_cpp/tests/cost_evaluator_test.cpp
index daadb8ab..2fcd54be 100644
--- a/src/kompass_cpp/tests/cost_evaluator_test.cpp
+++ b/src/kompass_cpp/tests/cost_evaluator_test.cpp
@@ -175,8 +175,8 @@ struct TestConfig {
   ControlLimitsParams controlLimits;
   CollisionChecker::ShapeType robotShapeType;
   std::vector<float> robotDimensions;
-  std::array<float, 3> sensor_position_body;
-  std::array<float, 4> sensor_rotation_body;
+  Eigen::Vector3f sensor_position_body;
+  Eigen::Quaternionf sensor_rotation_body;
   CostEvaluator::TrajectoryCostsWeights costWeights;
   CostEvaluator costEval;
 
diff --git a/src/kompass_cpp/tests/trajectory_sampler_test.cpp b/src/kompass_cpp/tests/trajectory_sampler_test.cpp
index 9ec5f326..62ff806f 100644
--- a/src/kompass_cpp/tests/trajectory_sampler_test.cpp
+++ b/src/kompass_cpp/tests/trajectory_sampler_test.cpp
@@ -6,10 +6,9 @@
 #include "test.h"
 #include "utils/logger.h"
 #include "utils/trajectory_sampler.h"
+#include <Eigen/Dense>
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
 #include <boost/filesystem.hpp>
-#include <cstddef>
-#include <cstdlib>
 #include <string>
 #include <vector>
 
@@ -61,8 +60,8 @@ void testTrajSampler() {
   auto robotShapeType = Kompass::CollisionChecker::ShapeType::BOX;
   std::vector<float> robotDimensions{0.3, 0.3, 1.0};
   // std::array<float, 3> sensorPositionWRTbody {0.0, 0.0, 1.0};
-  const std::array<float, 3> sensor_position_body{0.0, 0.0, 0.5};
-  const std::array<float, 4> sensor_rotation_body{0, 0, 0, 1};
+  const Eigen::Vector3f sensor_position_body{0.0, 0.0, 0.5};
+  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1};
 
   // Robot start state (pose)
   Path::State robotState(0.0, 0.0, 0.0, 0.0);
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index c6f18356..b8311ea2 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -1,11 +1,15 @@
 #include "controllers/vision_dwa.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
+#include "datatypes/tracking.h"
 #include "datatypes/trajectory.h"
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
+#include <Eigen/Geometry>
+#include <Eigen/src/Core/Matrix.h>
 #include <memory>
+#include <opencv2/opencv.hpp>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "json_export.h"
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
@@ -45,8 +49,15 @@ struct VisionDWATestConfig {
   Control::ControlType controlType;
   Kompass::CollisionChecker::ShapeType robotShapeType;
   std::vector<float> robotDimensions;
-  const std::array<float, 3> sensorPositionWRTbody;
-  const std::array<float, 4> sensorRotationWRTbody;
+  Eigen::Vector3f prox_sensor_position_body;
+  Eigen::Quaternionf prox_sensor_rotation_body;
+
+  // Depth detector
+  Eigen::Vector2f focal_length = {911.71, 910.288};
+  Eigen::Vector2f principal_point = {643.06, 366.72};
+  Eigen::Vector2f depth_range = {0.001, 5.0}; // 5cm to 5 meters
+  float depth_conv_factor = 1e-3;             // convert from mm to m
+  Eigen::Isometry3f camera_body_tf;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud;
@@ -62,7 +73,7 @@ struct VisionDWATestConfig {
 
   // Constructor to initialize the struct
   VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
-                      const bool use_tracker, const double timeStep = 0.1,
+                      const double timeStep = 0.1,
                       const int predictionHorizon = 10,
                       const int controlHorizon = 2,
                       const int maxLinearSamples = 20,
@@ -80,8 +91,8 @@ struct VisionDWATestConfig {
         controlLimits(x_params, y_params, angular_params),
         controlType(Control::ControlType::ACKERMANN),
         robotShapeType(Kompass::CollisionChecker::ShapeType::CYLINDER),
-        robotDimensions{0.1, 0.4}, sensorPositionWRTbody{0.0, 0.0, 0.0},
-        sensorRotationWRTbody{0, 0, 0, 1}, cloud(sensor_points),
+        robotDimensions{0.1, 0.4}, prox_sensor_position_body{0.0, 0.0, 0.0},
+        prox_sensor_rotation_body{0, 0, 0, 1}, cloud(sensor_points),
         robotState(-0.5, 0.0, 0.0, 0.0), tracked_vel(0.1, 0.0, 0.3),
         tracked_pose(0.0, 0.0, 0.0, tracked_vel) {
     // Initialize cost weights
@@ -96,11 +107,32 @@ struct VisionDWATestConfig {
     config.setParameter("control_time_step", timeStep);
     config.setParameter("control_horizon", controlHorizon);
     config.setParameter("prediction_horizon", predictionHorizon);
+
+    // For depth config
+    // Body to camera tf from robot of test pictures
+    auto body_to_link_tf =
+        getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
+                          Eigen::Vector3f{0.32f, 0.0209f, 0.3f});
+
+    auto link_to_cam_tf =
+        getTransformation(Eigen::Quaternionf{0.01f, -0.00131f, 0.002f, 0.9999f},
+                          Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
+
+    auto cam_to_cam_opt_tf =
+        getTransformation(Eigen::Quaternionf{-0.5f, 0.5f, -0.5f, 0.5f},
+                          Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
+
+    Eigen::Isometry3f body_to_cam_tf =
+        body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
+
+    Eigen::Vector3f translation = body_to_cam_tf.translation();
+    Eigen::Quaternionf rotation = Eigen::Quaternionf(body_to_cam_tf.rotation());
+
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
-        robotShapeType, robotDimensions, sensorPositionWRTbody,
-        sensorRotationWRTbody, octreeRes, costWeights, maxNumThreads, config,
-        use_tracker);
+        robotShapeType, robotDimensions, prox_sensor_position_body,
+        prox_sensor_rotation_body, translation, rotation, octreeRes,
+        costWeights, maxNumThreads, config);
 
     // Initialize the detected boxes
     Bbox3D new_box;
@@ -128,6 +160,45 @@ struct VisionDWATestConfig {
     }
   };
 
+  bool test_one_cmd_depth(const std::string image_file_path, const std::vector<Bbox2D> &detections, const Eigen::Vector2i &clicked_point, std::vector<Path::Point> cloud){
+    // robot velocity
+    Control::Velocity2D cmd;
+    // Get image
+    cv::Mat cv_img = cv::imread(image_file_path, cv::IMREAD_GRAYSCALE);
+
+    if (cv_img.empty()) {
+      LOG_ERROR("Could not open or find the image");
+    }
+
+    // Create an Eigen matrix of type int from the OpenCV Mat
+    auto depth_image = Eigen::MatrixXi(cv_img.rows, cv_img.cols);
+    for (int i = 0; i < cv_img.rows; ++i) {
+      for (int j = 0; j < cv_img.cols; ++j) {
+        depth_image(i, j) = cv_img.at<unsigned short>(i, j);
+      }
+    }
+
+    controller->setCameraIntrinsics(focal_length.x(), focal_length.y(),
+                                    principal_point.x(), principal_point.y());
+
+    auto found_target = controller->setInitialTracking(
+        clicked_point(0), clicked_point(1), depth_image, detections);
+    if(!found_target){
+      LOG_WARNING("Point not found on image");
+      return false;
+    }
+    else{
+      LOG_INFO("Point found on image ...");
+    }
+    auto res = controller->getTrackingCtrl(depth_image, detections, cmd, cloud);
+    if(res.isTrajFound){
+      LOG_INFO("Got control (vx, vy, omega) = (", res.trajectory.velocities.vx[0], ", ",
+               res.trajectory.velocities.vy[0], ", ",
+               res.trajectory.velocities.omega[0], ")");
+    }
+    return res.isTrajFound;
+  }
+
   bool run_test(const int numPointsPerTrajectory, std::string pltFileName,
                 bool with_tracker) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
@@ -149,7 +220,8 @@ struct VisionDWATestConfig {
       robot_path.add(step, point);
       tracked_path.add(step, {tracked_pose.x(), tracked_pose.y(), 0.0});
       controller->setCurrentState(robotState);
-      LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(), ", ", tracked_pose.yaw());
+      LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(),
+               ", ", tracked_pose.yaw());
       LOG_INFO("Robot at: ", point.x(), ", ", point.y());
 
       if (with_tracker) {
@@ -232,13 +304,13 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
-  VisionDWATestConfig testConfig(cloud, use_tracker);
+  VisionDWATestConfig testConfig(cloud);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed =
-      testConfig.run_test(numPointsPerTrajectory,
-                          std::string("vision_follower_obstacle_free"), use_tracker);
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_obstacle_free"),
+      use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -251,13 +323,13 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
-  VisionDWATestConfig testConfig(cloud, use_tracker);
+  VisionDWATestConfig testConfig(cloud);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed =
-      testConfig.run_test(numPointsPerTrajectory,
-                          std::string("vision_follower_with_obstacle"), use_tracker);
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"),
+      use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -271,13 +343,13 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_obs_free) {
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
-  VisionDWATestConfig testConfig(cloud, true);
+  VisionDWATestConfig testConfig(cloud);
 
   int numPointsPerTrajectory = 100;
 
-  bool test_passed =
-      testConfig.run_test(numPointsPerTrajectory,
-                          std::string("vision_follower_with_tracker"), use_tracker);
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory, std::string("vision_follower_with_tracker"),
+      use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -290,7 +362,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
-  VisionDWATestConfig testConfig(cloud, use_tracker);
+  VisionDWATestConfig testConfig(cloud);
 
   int numPointsPerTrajectory = 100;
 
@@ -299,3 +371,26 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
       std::string("vision_follower_with_tracker_and_obstacle"), use_tracker);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
+
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
+  // Create timer
+  Timer time;
+
+  std::string filename =
+      "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif";
+  Bbox2D box({410, 0}, {410, 390});
+  std::vector<Bbox2D> detections_2d{box};
+
+  auto initial_point = Eigen::Vector2i{610, 200};
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+
+  VisionDWATestConfig testConfig(cloud);
+
+  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d, initial_point, cloud);
+
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
+}
+
+

From 1f4cdbf7feffd759855d854639108b0220ab691c Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 30 Apr 2025 18:15:02 +0200
Subject: [PATCH 048/118] (feature) Adds VisionDWA binding for using DepthImage
 input and updates tests

---
 src/kompass_core/__init__.py                  |  14 ++
 src/kompass_core/control/dwa.py               |  26 ++-
 src/kompass_core/control/vision_dwa.py        | 104 +++++++---
 src/kompass_core/datatypes/__init__.py        |   3 +-
 src/kompass_cpp/bindings/bindings_control.cpp |  55 +++++-
 src/kompass_cpp/bindings/bindings_types.cpp   |   8 +
 .../kompass_cpp/include/controllers/dwa.h     |   8 +-
 .../include/controllers/vision_dwa.h          |  17 +-
 .../include/utils/transformation.h            |   2 +-
 .../include/vision/depth_detector.h           |  13 +-
 .../kompass_cpp/src/controllers/dwa.cpp       |  19 +-
 .../src/controllers/vision_dwa.cpp            |  12 +-
 .../kompass_cpp/src/vision/depth_detector.cpp |  72 +++----
 src/kompass_cpp/tests/controller_test.cpp     |   2 +-
 .../tests/vision_detector_test.cpp            |   4 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     |  41 ++--
 tests/resources/control/bag_image_depth.tif   | Bin 0 -> 283426 bytes
 tests/test_controllers.py                     | 177 ++++++++++++++----
 18 files changed, 404 insertions(+), 173 deletions(-)
 create mode 100644 tests/resources/control/bag_image_depth.tif

diff --git a/src/kompass_core/__init__.py b/src/kompass_core/__init__.py
index e69de29b..3b588519 100644
--- a/src/kompass_core/__init__.py
+++ b/src/kompass_core/__init__.py
@@ -0,0 +1,14 @@
+import kompass_cpp
+
+
+def set_logging_level(level: str):
+    """Set the logging level to "debug", "warn", "error" or "info"
+
+    :param level: Logging level
+    :type level: str
+    """
+    level = level.upper()
+    kompass_cpp.set_log_level(getattr(kompass_cpp.LogLevel, level))
+
+
+__all__ = ["set_logging_level"]
diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 778250b5..48ea656f 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -96,13 +96,15 @@ class DWAConfig(FollowerConfig):
         default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
     )
 
-    prediction_horizon: float = field(
-        default=1.0, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
+    control_horizon: int = field(
+        default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
     )
+    # Number of steps for applying the control
 
-    control_horizon: float = field(
-        default=0.2, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
+    prediction_horizon: int = field(
+        default=10, validator=base_validators.in_range(min_value=1, max_value=1000)
     )
+    # Number of steps for future prediction
 
     max_linear_samples: int = field(
         default=20, validator=base_validators.in_range(min_value=1, max_value=1e3)
@@ -112,9 +114,13 @@ class DWAConfig(FollowerConfig):
         default=20, validator=base_validators.in_range(min_value=1, max_value=1e3)
     )
 
-    sensor_position_to_robot: List[float] = field(default=[0.0, 0.0, 0.0])
+    proximity_sensor_position_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0], dtype=np.float32)
+    )
 
-    sensor_rotation_to_robot: List[float] = field(default=[0.0, 0.0, 0.0, 1.0])
+    proximity_sensor_rotation_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32)
+    )
 
     octree_resolution: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e3)
@@ -221,14 +227,14 @@ def __init__(
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             time_step=config.control_time_step,
-            prediction_horizon=config.prediction_horizon,
-            control_horizon=config.control_horizon,
+            prediction_horizon=config.prediction_horizon * config.control_time_step,
+            control_horizon=config.control_horizon * config.control_time_step,
             max_linear_samples=config.max_linear_samples,
             max_angular_samples=config.max_angular_samples,
             robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type),
             robot_dimensions=robot.geometry_params,
-            sensor_position_robot=config.sensor_position_to_robot,
-            sensor_rotation_robot=config.sensor_rotation_to_robot,
+            sensor_position_robot=config.proximity_sensor_position_to_robot,
+            sensor_rotation_robot=config.proximity_sensor_rotation_to_robot,
             octree_resolution=config.octree_resolution,
             cost_weights=config.costs_weights.to_kompass_cpp(),
             max_num_threads=config.max_num_threads,
diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index bd88fa45..8e98a057 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -7,6 +7,7 @@
 )
 from kompass_cpp.types import (
     Bbox3D,
+    Bbox2D,
     ControlCmd,
     LaserScan,
     TrajectoryVelocities2D,
@@ -26,20 +27,6 @@
 @define
 class VisionDWAConfig(DWAConfig):
 
-    use_tracker: bool = field(
-        default=True
-    )  # Use the tracker to get the target position and velocity from the detections
-
-    control_horizon: int = field(
-        default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
-    )
-    # Number of steps for applying the control
-
-    prediction_horizon: int = field(
-        default=10, validator=base_validators.in_range(min_value=1, max_value=1000)
-    )
-    # Number of steps for future prediction
-
     tolerance: float = field(
         default=0.01, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
     )
@@ -97,6 +84,26 @@ class VisionDWAConfig(DWAConfig):
         default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
     )  # Error in acceleration estimation (m/s^2)
 
+    depth_conversion_factor: float = field(
+        default=1e-3, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )  # Factor to convert depth image values to meters
+
+    min_depth: float = field(
+        default=0.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # Range of interest minimum depth value (m)
+
+    max_depth: float = field(
+        default=1e3, validator=base_validators.in_range(min_value=1e-3, max_value=1e9)
+    )  # Range of interest maximum depth value (m)
+
+    camera_position_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0], dtype=np.float32)
+    )
+
+    camera_rotation_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32)
+    )
+
     def to_kompass_cpp(self) -> VisionDWAParameters:
         """
         Convert to kompass_cpp lib config format
@@ -108,7 +115,7 @@ def to_kompass_cpp(self) -> VisionDWAParameters:
 
         # Special handling for None values that are represented by -1 in C++
         params_dict = self.asdict()
-        print(f"as dict {params_dict}")
+
         if params_dict["target_distance"] is None:
             params_dict["target_distance"] = -1.0
 
@@ -125,6 +132,8 @@ def __init__(
         config_file: Optional[str] = None,
         config_yaml_root_name: Optional[str] = None,
         control_time_step: Optional[float] = None,
+        camera_focal_length: Optional[List[float]] = None,
+        camera_principal_point: Optional[List[float]] = None,
         **_,
     ):
         """
@@ -144,7 +153,7 @@ def __init__(
 
         if control_time_step:
             self._config.control_time_step = control_time_step
-        logging.info(f"use_tracker: {self._config.use_tracker}")
+
         self._planner = VisionDWACpp(
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
@@ -152,14 +161,18 @@ def __init__(
             max_angular_samples=self._config.max_angular_samples,
             robot_shape_type=RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type),
             robot_dimensions=robot.geometry_params,
-            sensor_position_wrt_body=self._config.sensor_position_to_robot,
-            sensor_rotation_wrt_body=self._config.sensor_rotation_to_robot,
+            proximity_sensor_position_wrt_body=self._config.proximity_sensor_position_to_robot,
+            proximity_sensor_rotation_wrt_body=self._config.proximity_sensor_rotation_to_robot,
+            vision_sensor_position_wrt_body=self._config.camera_position_to_robot,
+            vision_sensor_rotation_wrt_body=self._config.camera_rotation_to_robot,
             octree_res=self._config.octree_resolution,
             cost_weights=self._config.costs_weights.to_kompass_cpp(),
             max_num_threads=self._config.max_num_threads,
             config=self._config.to_kompass_cpp(),
-            use_tracker=self._config.use_tracker,
         )
+        if camera_focal_length and camera_principal_point:
+            self._planner.set_camera_intrinsics(camera_focal_length[0], camera_focal_length[1], camera_principal_point[0], camera_principal_point[1])
+
         # Init the following result
         self._result = SamplingControlResult()
         self._end_of_ctrl_horizon: int = max(
@@ -167,7 +180,35 @@ def __init__(
         )
         logging.info("VisionDWA CONTROLLER IS READY")
 
-    def set_initial_tracking(self, pose_x_img: int, pose_y_img: int, detected_boxes: List[Bbox3D]) -> bool:
+    def set_camera_intrinsics(self, fx: float, fy: float, cx: float, cy: float) -> None:
+        self._planner.set_camera_intrinsics(fx, fy, cx, cy)
+
+    def set_initial_tracking_3d(
+        self,
+        pose_x_img: int,
+        pose_y_img: int,
+        detected_boxes: List[Bbox3D],
+    ) -> bool:
+        """
+        Set initial tracking state
+
+        :param detected_boxes: Detected boxes
+        :type detected_boxes: List[Bbox3D]
+        """
+        try:
+            return self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+        except Exception as e:
+            logging.error(f"Could not set initial tracking state: {e}")
+            return False
+
+    def set_initial_tracking_depth(
+        self,
+        current_state: RobotState,
+        pose_x_img: int,
+        pose_y_img: int,
+        detected_boxes: List[Bbox2D],
+        aligned_depth_image: np.ndarray,
+    ) -> bool:
         """
         Set initial tracking state
 
@@ -175,17 +216,23 @@ def set_initial_tracking(self, pose_x_img: int, pose_y_img: int, detected_boxes:
         :type detected_boxes: List[Bbox3D]
         """
         try:
-            self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+            self._planner.set_current_state(
+            current_state.x, current_state.y, current_state.yaw, current_state.speed)
+
+            return self._planner.set_initial_tracking(
+                pose_x_img, pose_y_img, aligned_depth_image, detected_boxes
+            )
         except Exception as e:
             logging.error(f"Could not set initial tracking state: {e}")
             return False
-        return True
 
     def loop_step(
         self,
         *,
         current_state: RobotState,
-        detections: Optional[List[Bbox3D]] = None,
+        detections_3d: Optional[List[Bbox3D]] = None,
+        detections_2d: Optional[List[Bbox2D]] = None,
+        depth_image: Optional[np.ndarray] = None,
         tracked_pose: Optional[TrackedPose2D] = None,
         laser_scan: Optional[LaserScanData] = None,
         point_cloud: Optional[List[np.ndarray]] = None,
@@ -230,9 +277,14 @@ def loop_step(
             return False
 
         try:
-            self._result = self._planner.get_tracking_ctrl(
-                tracked_pose or detections, current_velocity, sensor_data
-            )
+            if (detections_3d or tracked_pose) is not None:
+                self._result = self._planner.get_tracking_ctrl(
+                    tracked_pose or detections_3d, current_velocity, sensor_data
+                )
+            else:
+                self._result = self._planner.get_tracking_ctrl(
+                    depth_image, detections_2d, current_velocity, sensor_data
+                )
 
         except Exception as e:
             logging.error(f"Could not find velocity command: {e}")
diff --git a/src/kompass_core/datatypes/__init__.py b/src/kompass_core/datatypes/__init__.py
index 3df8a617..d518d01e 100644
--- a/src/kompass_core/datatypes/__init__.py
+++ b/src/kompass_core/datatypes/__init__.py
@@ -15,7 +15,7 @@
 from .pointcloud import PointCloudData
 from .pose import PoseData
 from .vision import TrackingData, ImageMetaData
-from kompass_cpp.types import TrackedPose2D, Bbox3D
+from kompass_cpp.types import TrackedPose2D, Bbox3D, Bbox2D
 
 __all__ = [
     "LaserScanData",
@@ -35,5 +35,6 @@
     "TrackingData",
     "ImageMetaData",
     "Bbox3D",
+    "Bbox2D",
     "TrackedPose2D",
 ]
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index ff2c66f1..6fd38195 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -167,7 +167,7 @@ void bindings_control(py::module_ &m) {
       .def(py::init<Control::ControlLimitsParams, Control::ControlType, double,
                     double, double, int, int, CollisionChecker::ShapeType,
                     std::vector<float>, const Eigen::Vector3f &,
-                    const Eigen::Quaternionf &, double,
+                    const  Eigen::Vector4f &, double,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("control_limits"), py::arg("control_type"),
            py::arg("time_step"), py::arg("prediction_horizon"),
@@ -180,7 +180,7 @@ void bindings_control(py::module_ &m) {
       .def(py::init<Control::TrajectorySampler::TrajectorySamplerParameters,
                     Control::ControlLimitsParams, Control::ControlType,
                     CollisionChecker::ShapeType, std::vector<float>,
-                    const Eigen::Vector3f &, const Eigen::Quaternionf &,
+                    const Eigen::Vector3f &, const  Eigen::Vector4f &,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("config"), py::arg("control_limits"),
            py::arg("control_type"), py::arg("robot_shape_type"),
@@ -245,8 +245,8 @@ void bindings_control(py::module_ &m) {
                     const Control::ControlLimitsParams &, const int, const int,
                     const CollisionChecker::ShapeType &,
                     const std::vector<float> &, const Eigen::Vector3f &,
-                    const Eigen::Quaternionf &, const Eigen::Vector3f &,
-                    const Eigen::Quaternionf &, const double,
+                    const Eigen::Vector4f &, const Eigen::Vector3f &,
+                    const Eigen::Vector4f &, const double,
                     const Control::CostEvaluator::TrajectoryCostsWeights &,
                     const int, const Control::VisionDWA::VisionDWAConfig &>(),
            py::arg("control_type"), py::arg("control_limits"),
@@ -258,30 +258,67 @@ void bindings_control(py::module_ &m) {
            py::arg("vision_sensor_rotation_wrt_body"), py::arg("octree_res"),
            py::arg("cost_weights"), py::arg("max_num_threads") = 1,
            py::arg("config") = Control::VisionDWA::VisionDWAConfig())
+      .def("set_camera_intrinsics", &Control::VisionDWA::setCameraIntrinsics,
+           py::arg("focal_length_x"), py::arg("focal_length_y"),
+           py::arg("principal_point_x"), py::arg("principal_point_y"))
       .def("set_initial_tracking",
            py::overload_cast<const int, const int,
                              const std::vector<Bbox3D> &>(
-               &Control::VisionDWA::setInitialTracking))
+               &Control::VisionDWA::setInitialTracking),
+           py::arg("pixel_x"), py::arg("pixel_y"),
+           py::arg("detected_boxes_3d"))
+      .def("set_initial_tracking",
+           py::overload_cast<const int, const int,
+                             const Eigen::MatrixX<unsigned short> &,
+                             const std::vector<Bbox2D> &>(
+               &Control::VisionDWA::setInitialTracking),
+           py::arg("pixel_x"), py::arg("pixel_y"),
+           py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"))
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
                              const std::vector<Eigen::Vector3f> &>(
                &Control::VisionDWA::getTrackingCtrl<
-                   std::vector<Eigen::Vector3f>>))
+                   std::vector<Eigen::Vector3f>>),
+           py::arg("detected_boxes"), py::arg("robot_velocity"),
+           py::arg("sensor_data"))
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
                              const Control::LaserScan &>(
-               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>))
+               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
+           py::arg("detected_boxes"), py::arg("robot_velocity"),
+           py::arg("sensor_data"))
       .def("get_tracking_ctrl",
            py::overload_cast<const Control::TrackedPose2D &,
                              const Control::Velocity2D &,
                              const std::vector<Eigen::Vector3f> &>(
                &Control::VisionDWA::getTrackingCtrl<
-                   std::vector<Eigen::Vector3f>>))
+                   std::vector<Eigen::Vector3f>>),
+           py::arg("tracked_pose"), py::arg("robot_velocity"),
+           py::arg("sensor_data"))
       .def("get_tracking_ctrl",
            py::overload_cast<const Control::TrackedPose2D &,
                              const Control::Velocity2D &,
                              const Control::LaserScan &>(
-               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>));
+               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
+           py::arg("tracked_pose"), py::arg("robot_velocity"),
+           py::arg("sensor_data"))
+      .def("get_tracking_ctrl",
+           py::overload_cast<const Eigen::MatrixX<unsigned short> &,
+                             const std::vector<Bbox2D> &,
+                             const Control::Velocity2D &,
+                             const std::vector<Eigen::Vector3f> &>(
+               &Control::VisionDWA::getTrackingCtrl<
+                   std::vector<Eigen::Vector3f>>),
+           py::arg("aligned_depth_image"), py::arg("detected_boxes"),
+           py::arg("robot_velocity"), py::arg("sensor_data"))
+      .def("get_tracking_ctrl",
+           py::overload_cast<const Eigen::MatrixX<unsigned short> &,
+                             const std::vector<Bbox2D> &,
+                             const Control::Velocity2D &,
+                             const Control::LaserScan &>(
+               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
+           py::arg("aligned_depth_image"), py::arg("detected_boxes"),
+           py::arg("robot_velocity"), py::arg("sensor_data"));
 }
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 7cde19fc..7f21ecbc 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -132,6 +132,14 @@ py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
     .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy)
     .def_rw("depth", &Control::VisionFollower::TrackingData::depth);
 
+py::class_<Bbox2D>(m_types, "Bbox2D")
+    .def(py::init<>())
+    .def(py::init<const Bbox2D &>())
+    .def(py::init<Eigen::Vector2i &, Eigen::Vector2i &>(),
+         py::arg("top_left_corner"), py::arg("size"))
+    .def_rw("top_left_corner", &Bbox2D::top_corner)
+    .def_rw("size", &Bbox2D::size);
+
 py::class_<Bbox3D>(m_types, "Bbox3D")
     .def(py::init<>())
     .def(py::init<const Bbox3D &>())
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 0d90eccf..5e4ad3df 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -25,7 +25,7 @@ class DWA : public Follower {
       const CollisionChecker::ShapeType robotShapeType,
       const std::vector<float> robotDimensions,
       const Eigen::Vector3f &sensor_position_body,
-      const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes,
+      const Eigen::Vector4f &sensor_rotation_body, const double octreeRes,
       CostEvaluator::TrajectoryCostsWeights costWeights,
       const int maxNumThreads = 1);
 
@@ -34,7 +34,7 @@ class DWA : public Follower {
       const CollisionChecker::ShapeType robotShapeType,
       const std::vector<float> robotDimensions,
       const Eigen::Vector3f &sensor_position_body,
-      const Eigen::Quaternionf &sensor_rotation_body,
+      const Eigen::Vector4f &sensor_rotation_body,
       CostEvaluator::TrajectoryCostsWeights costWeights,
       const int maxNumThreads = 1);
 
@@ -56,7 +56,7 @@ class DWA : public Follower {
                  const CollisionChecker::ShapeType robotShapeType,
                  const std::vector<float> robotDimensions,
                  const Eigen::Vector3f &sensor_position_body,
-                 const Eigen::Quaternionf &sensor_rotation_body,
+                 const Eigen::Vector4f &sensor_rotation_body,
                  const double octreeRes,
                  CostEvaluator::TrajectoryCostsWeights costWeights,
                  const int maxNumThreads = 1);
@@ -66,7 +66,7 @@ class DWA : public Follower {
                  const CollisionChecker::ShapeType robotShapeType,
                  const std::vector<float> robotDimensions,
                  const Eigen::Vector3f &sensor_position_body,
-                 const Eigen::Quaternionf &sensor_rotation_body,
+                 const Eigen::Vector4f &sensor_rotation_body,
                  CostEvaluator::TrajectoryCostsWeights costWeights,
                  const int maxNumThreads = 1);
 
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 98ffa628..9c6f0df3 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -8,7 +8,6 @@
 #include "vision/depth_detector.h"
 #include "vision/tracker.h"
 #include <Eigen/Dense>
-#include <Eigen/src/Core/Matrix.h>
 #include <cmath>
 #include <memory>
 #include <tuple>
@@ -122,9 +121,9 @@ class VisionDWA : public DWA {
             const CollisionChecker::ShapeType &robotShapeType,
             const std::vector<float> &robotDimensions,
             const Eigen::Vector3f &proximity_sensor_position_body,
-            const Eigen::Quaternionf &proximity_sensor_rotation_body,
+            const Eigen::Vector4f &proximity_sensor_rotation_body,
             const Eigen::Vector3f &vision_sensor_position_body,
-            const Eigen::Quaternionf &vision_sensor_rotation_body,
+            const Eigen::Vector4f &vision_sensor_rotation_body,
             const double octreeRes,
             const CostEvaluator::TrajectoryCostsWeights &costWeights,
             const int maxNumThreads = 1,
@@ -224,7 +223,7 @@ class VisionDWA : public DWA {
 
   template <typename T>
   Control::TrajSearchResult
-  getTrackingCtrl(const Eigen::MatrixXi &aligned_depth_img,
+  getTrackingCtrl(const Eigen::MatrixX<unsigned short> &aligned_depth_img,
                   const std::vector<Bbox2D> &detected_boxes_2d,
                   const Velocity2D &current_vel, const T &sensor_points) {
     if (!detector_) {
@@ -255,7 +254,7 @@ class VisionDWA : public DWA {
         // Return false for trajectory found
         return TrajSearchResult();
       }
-    }else{
+    } else {
       LOG_WARNING("Detector failed to find 3D boxes");
       // Return false for trajectory found
       return TrajSearchResult();
@@ -284,9 +283,11 @@ class VisionDWA : public DWA {
    * @return true
    * @return false
    */
-  bool setInitialTracking(const int pose_x_img, const int pose_y_img,
-                          const Eigen::MatrixXi &aligned_depth_image,
-                          const std::vector<Bbox2D> &detected_boxes_2d);
+  bool
+  setInitialTracking(const int pose_x_img,
+                     const int pose_y_img,
+                     const Eigen::MatrixX<unsigned short> &aligned_depth_image,
+                     const std::vector<Bbox2D> &detected_boxes_2d);
 
 private:
   ControlLimitsParams ctrl_limits_;
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
index b4fe99e4..3aa5700e 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/transformation.h
@@ -27,7 +27,7 @@ getTransformation(const RotationType &rotation_src_to_goal,
   transform_src_to_goal.translate(translation_src_to_goal);
 
   // Set rotation based on the type of RotationType
-  transform_src_to_goal.rotate(rotation_src_to_goal);
+  transform_src_to_goal.rotate(Eigen::Quaternionf(rotation_src_to_goal));
 
   return transform_src_to_goal;
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
index 0ea0c34c..ac460c84 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -13,14 +13,14 @@ namespace Kompass {
 class DepthDetector {
 public:
   DepthDetector(const Eigen::Vector2f &depth_range,
-                const Eigen::Vector3f &body_to_camera_translation,
-                const Eigen::Quaternionf &body_to_camera_rotation,
+                const Eigen::Vector3f &camera_in_body_translation,
+                const Eigen::Quaternionf &camera_in_body_rotation,
                 const Eigen::Vector2f &focal_length,
                 const Eigen::Vector2f &principal_point,
                 const float depth_conversion_factor = 1e-3);
 
   DepthDetector(const Eigen::Vector2f &depth_range,
-                const Eigen::Isometry3f &body_to_camera_tf,
+                const Eigen::Isometry3f &camera_in_body_tf,
                 const Eigen::Vector2f &focal_length,
                 const Eigen::Vector2f &principal_point,
                 const float depth_conversion_factor = 1e-3);
@@ -29,15 +29,16 @@ class DepthDetector {
 
   void updateState(const Eigen::Isometry3f &robot_tf);
 
-  void updateBoxes(const Eigen::MatrixXi aligned_depth_img, const std::vector<Bbox2D>& detections);
+  void updateBoxes(const Eigen::MatrixX<unsigned short> aligned_depth_img,
+                   const std::vector<Bbox2D> &detections);
 
   std::optional<std::vector<Bbox3D>> get3dDetections() const;
 
 private:
   float cx_, cy_, fx_, fy_;         // Depth Image camera intrinsics
   float minDepth_, maxDepth_, depthConversionFactor_;
-  Eigen::MatrixXi alignedDepthImg_;
-  Eigen::Isometry3f body_to_camera_tf_, world_to_body_tf_;
+  Eigen::MatrixX<unsigned short> alignedDepthImg_;
+  Eigen::Isometry3f camera_in_body_tf_, body_in_world_tf_;
   std::unique_ptr<std::vector<Bbox3D>> boxes_;
 
   std::optional<Bbox3D> convert2Dboxto3Dbox(const Bbox2D &box2d);
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 3aa2db22..84c325e9 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -16,7 +16,7 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
          const Eigen::Vector3f &sensor_position_body,
-         const Eigen::Quaternionf &sensor_rotation_body, const double octreeRes,
+         const  Eigen::Vector4f &sensor_rotation_body, const double octreeRes,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
@@ -41,7 +41,7 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
          const Eigen::Vector3f &sensor_position_body,
-         const Eigen::Quaternionf &sensor_rotation_body,
+         const  Eigen::Vector4f &sensor_rotation_body,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
@@ -68,17 +68,18 @@ void DWA::configure(ControlLimitsParams controlLimits, ControlType controlType,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
                     const Eigen::Vector3f &sensor_position_body,
-                    const Eigen::Quaternionf &sensor_rotation_body,
+                    const  Eigen::Vector4f &sensor_rotation_body,
                     const double octreeRes,
                     CostEvaluator::TrajectoryCostsWeights costWeights,
                     const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, octreeRes, maxNumThreads);
+      sensor_position_body, Eigen::Quaternionf(sensor_rotation_body), octreeRes, maxNumThreads);
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
-      costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
+      costWeights, sensor_position_body,
+      Eigen::Quaternionf(sensor_rotation_body), controlLimits,
       trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory,
       maxSegmentSize);
   this->maxNumThreads = maxNumThreads;
@@ -89,15 +90,17 @@ void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
                     const Eigen::Vector3f &sensor_position_body,
-                    const Eigen::Quaternionf &sensor_rotation_body,
+                    const  Eigen::Vector4f &sensor_rotation_body,
                     CostEvaluator::TrajectoryCostsWeights costWeights,
                     const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       config, controlLimits, controlType, robotShapeType, robotDimensions,
-      sensor_position_body, sensor_rotation_body, maxNumThreads);
+      sensor_position_body, Eigen::Quaternionf(sensor_rotation_body),
+      maxNumThreads);
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
-      costWeights, sensor_position_body, sensor_rotation_body, controlLimits,
+      costWeights, sensor_position_body,
+      Eigen::Quaternionf(sensor_rotation_body), controlLimits,
       trajSampler->numTrajectories, trajSampler->numPointsPerTrajectory,
       maxSegmentSize);
   this->maxNumThreads = maxNumThreads;
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 5395da8c..57d1d83a 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -20,9 +20,9 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
                      const CollisionChecker::ShapeType &robotShapeType,
                      const std::vector<float> &robotDimensions,
                      const Eigen::Vector3f &proximity_sensor_position_body,
-                     const Eigen::Quaternionf &proximity_sensor_rotation_body,
+                     const Eigen::Vector4f &proximity_sensor_rotation_body,
                      const Eigen::Vector3f &vision_sensor_position_body,
-                     const Eigen::Quaternionf &vision_sensor_rotation_body,
+                     const Eigen::Vector4f &vision_sensor_rotation_body,
                      const double octreeRes,
                      const CostEvaluator::TrajectoryCostsWeights &costWeights,
                      const int maxNumThreads, const VisionDWAConfig &config)
@@ -100,9 +100,11 @@ bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
   return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
 }
 
-bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
-                                   const Eigen::MatrixXi &aligned_depth_image,
-                                   const std::vector<Bbox2D> &detected_boxes) {
+bool VisionDWA::setInitialTracking(
+    const int pose_x_img,
+    const int pose_y_img,
+    const Eigen::MatrixX<unsigned short> &aligned_depth_image,
+    const std::vector<Bbox2D> &detected_boxes) {
   if (!detector_) {
     throw std::runtime_error(
         "DepthDetector is not initialized with the camera intrinsics. Call "
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index ebe347a1..bc322fc2 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -9,21 +9,21 @@
 namespace Kompass {
 
 DepthDetector::DepthDetector(const Eigen::Vector2f &depth_range,
-                             const Eigen::Vector3f &body_to_camera_translation,
-                             const Eigen::Quaternionf &body_to_camera_rotation,
+                             const Eigen::Vector3f &camera_in_body_translation,
+                             const Eigen::Quaternionf &camera_in_body_rotation,
                              const Eigen::Vector2f &focal_length,
                              const Eigen::Vector2f &principal_point,
                              const float depth_conversion_factor) {
   // Set camera tf
   auto body_to_camera_tf =
-      getTransformation(body_to_camera_rotation, body_to_camera_translation);
+      getTransformation(camera_in_body_rotation, camera_in_body_translation);
   DepthDetector(depth_range, body_to_camera_tf, focal_length, principal_point,
                 depth_conversion_factor);
 }
 
 DepthDetector::DepthDetector(
     const Eigen::Vector2f &depth_range,
-    const Eigen::Isometry3f &body_to_camera_tf,
+    const Eigen::Isometry3f &camera_in_body_tf,
     const Eigen::Vector2f &focal_length, const Eigen::Vector2f &principal_point,
     const float depth_conversion_factor) { // Range of interest for depth values
                                            // in meters
@@ -33,8 +33,8 @@ DepthDetector::DepthDetector(
   // depthConversionFactor = 1e-3)
   depthConversionFactor_ = depth_conversion_factor;
   // Set camera tf
-  body_to_camera_tf_ = body_to_camera_tf;
-  world_to_body_tf_ = Eigen::Isometry3f::Identity();
+  camera_in_body_tf_ = camera_in_body_tf;
+  body_in_world_tf_ = Eigen::Isometry3f::Identity();
 
   // Set camera  intrinsic parameters
   fx_ = focal_length.x();
@@ -52,15 +52,16 @@ std::optional<std::vector<Bbox3D>> DepthDetector::get3dDetections() const {
 
 void DepthDetector::updateState(const Path::State &current_state) {
 
-  world_to_body_tf_ = getTransformation(current_state);
+  body_in_world_tf_ = getTransformation(current_state);
 }
 
 void DepthDetector::updateState(const Eigen::Isometry3f &robot_tf) {
-  world_to_body_tf_ = robot_tf;
+  body_in_world_tf_ = robot_tf;
 }
 
-void DepthDetector::updateBoxes(const Eigen::MatrixXi aligned_depth_img,
-                                const std::vector<Bbox2D> &detections) {
+void DepthDetector::updateBoxes(
+    const Eigen::MatrixX<unsigned short> aligned_depth_img,
+    const std::vector<Bbox2D> &detections) {
   alignedDepthImg_ = aligned_depth_img;
   boxes_ = std::make_unique<std::vector<Bbox3D>>();
   for (auto box2d : detections) {
@@ -108,40 +109,39 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
 
   // Convert from 2D box center in the pixel frame to the 3D box center in the
   // camera frame
-  Eigen::Vector3f center_img_frame, size_img_frame;
-  LOG_DEBUG("Got detected box in 2D pixel frame at :", box2d.top_corner.x(),
-           ", ", box2d.top_corner.y(), ", size = ", box2d.size.x(), ", ",
-           box2d.size.y());
-  center_img_frame(0) = (box2d.top_corner.x() + 0.5 * box2d.size.x() - cx_) *
-                        medianDepth / this->fx_;
-  center_img_frame(1) = (box2d.top_corner.y() + 0.5 * box2d.size.y() - cy_) *
-                        medianDepth / this->fy_;
-  center_img_frame(2) = medianDepth;
+  Eigen::Vector3f center_in_camera_frame, size_camera_frame;
 
-  LOG_DEBUG("Got detected box in 3D camera frame at :", center_img_frame.x(),
-           ", ", center_img_frame.y(), ", ", center_img_frame.z());
+  center_in_camera_frame(0) =
+      (box2d.top_corner.x() + 0.5 * box2d.size.x() - cx_) * medianDepth /
+      this->fx_;
+  center_in_camera_frame(1) =
+      (box2d.top_corner.y() + 0.5 * box2d.size.y() - cy_) * medianDepth /
+      this->fy_;
+  center_in_camera_frame(2) = medianDepth;
 
-  LOG_DEBUG("Median depth = ", medianDepth);
+  LOG_DEBUG("Median depth = ", medianDepth, ", min=", minimum_d, ", max=", maximum_d);
 
   // Size in meters
-  size_img_frame(0) = box2d.size.x() * medianDepth / this->fx_;
-  size_img_frame(1) = box2d.size.y() * medianDepth / this->fy_;
-  size_img_frame(2) = maximum_d - minimum_d;
+  size_camera_frame(0) = box2d.size.x() * medianDepth / this->fx_;
+  size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_;
+  size_camera_frame(2) = maximum_d - minimum_d;
+
+  LOG_DEBUG("Box size in camera frame :", size_camera_frame.x(), ", ",
+            size_camera_frame.y(), ", ", size_camera_frame.z());
+
+  Eigen::Isometry3f camera_in_world = body_in_world_tf_ * camera_in_body_tf_;
 
   // Register center in the world frame
-  box3d.center = world_to_body_tf_ * body_to_camera_tf_ * center_img_frame;
+  box3d.center = camera_in_world * center_in_camera_frame;
 
-  // Transform size from camera frame to world frame
-  box3d.size(0) = (world_to_body_tf_ * body_to_camera_tf_ *
-                   Eigen::Vector3f(size_img_frame(0), 0, 0))
-                      .norm();
-  box3d.size(1) = (world_to_body_tf_ * body_to_camera_tf_ *
-                   Eigen::Vector3f(0, size_img_frame(1), 0))
-                      .norm();
-  box3d.size(2) = (world_to_body_tf_ * body_to_camera_tf_ *
-                   Eigen::Vector3f(0, 0, size_img_frame(2)))
-                      .norm();
+  LOG_DEBUG("Got detected box in 3D world frame at :", box3d.center.x(), ", ",
+            box3d.center.y(), ", ", box3d.center.z());
 
+  // Transform size from camera frame to world frame
+  Eigen::Matrix3f abs_rotation = camera_in_world.linear().cwiseAbs();
+  box3d.size = abs_rotation * size_camera_frame;
+  LOG_DEBUG("3D box size :", box3d.size.x(), ", ", box3d.size.y(), ", ",
+            box3d.size.z());
   return box3d;
 }
 
diff --git a/src/kompass_cpp/tests/controller_test.cpp b/src/kompass_cpp/tests/controller_test.cpp
index dee10d9f..73feb953 100644
--- a/src/kompass_cpp/tests/controller_test.cpp
+++ b/src/kompass_cpp/tests/controller_test.cpp
@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE(test_DWA) {
   std::vector<float> robotDimensions{0.1, 0.4};
   // std::array<float, 3> sensorPositionWRTbody {0.0, 0.0, 1.0};
   const Eigen::Vector3f sensor_position_body{0.0, 0.0, 0.0};
-  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0, 1};
+  const Eigen::Vector4f sensor_rotation_body{0, 0, 0, 1};
 
   // Robot start state (pose)
   Path::State robotState(-0.51731912, 0.0, 0.0, 0.0);
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
index b83c1cb6..b0e44f9d 100644
--- a/src/kompass_cpp/tests/vision_detector_test.cpp
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -24,7 +24,7 @@ struct DepthDetectorTestConfig {
   Eigen::Vector2f depth_range = {0.001, 5.0}; // 5cm to 5 meters
   float depth_conv_factor = 1e-3;             // convert from mm to m
   Eigen::Isometry3f camera_body_tf;
-  Eigen::MatrixXi depth_image;
+  Eigen::MatrixX<unsigned short> depth_image;
   cv::Mat cv_img;
   std::vector<Bbox2D> detections;
 
@@ -56,7 +56,7 @@ struct DepthDetectorTestConfig {
     }
 
     // Create an Eigen matrix of type int from the OpenCV Mat
-    depth_image = Eigen::MatrixXi(cv_img.rows, cv_img.cols);
+    depth_image = Eigen::MatrixX<unsigned short>(cv_img.rows, cv_img.cols);
     for (int i = 0; i < cv_img.rows; ++i) {
       for (int j = 0; j < cv_img.cols; ++j) {
         depth_image(i, j) = cv_img.at<unsigned short>(i, j);
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index b8311ea2..9c7cb1e2 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -6,8 +6,6 @@
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
-#include <Eigen/Geometry>
-#include <Eigen/src/Core/Matrix.h>
 #include <memory>
 #include <opencv2/opencv.hpp>
 #define BOOST_TEST_MODULE KOMPASS TESTS
@@ -50,7 +48,7 @@ struct VisionDWATestConfig {
   Kompass::CollisionChecker::ShapeType robotShapeType;
   std::vector<float> robotDimensions;
   Eigen::Vector3f prox_sensor_position_body;
-  Eigen::Quaternionf prox_sensor_rotation_body;
+  Eigen::Vector4f prox_sensor_rotation_body;
 
   // Depth detector
   Eigen::Vector2f focal_length = {911.71, 910.288};
@@ -126,7 +124,9 @@ struct VisionDWATestConfig {
         body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
 
     Eigen::Vector3f translation = body_to_cam_tf.translation();
-    Eigen::Quaternionf rotation = Eigen::Quaternionf(body_to_cam_tf.rotation());
+    Eigen::Vector4f rotation = {
+        -0.5846, 0.595, -0.395,
+        0.385}; // Eigen::Vector4f(body_to_cam_tf.rotation().data());
 
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
@@ -160,18 +160,21 @@ struct VisionDWATestConfig {
     }
   };
 
-  bool test_one_cmd_depth(const std::string image_file_path, const std::vector<Bbox2D> &detections, const Eigen::Vector2i &clicked_point, std::vector<Path::Point> cloud){
+  bool test_one_cmd_depth(const std::string image_file_path,
+                          const std::vector<Bbox2D> &detections,
+                          const Eigen::Vector2i &clicked_point,
+                          std::vector<Path::Point> cloud) {
     // robot velocity
     Control::Velocity2D cmd;
     // Get image
-    cv::Mat cv_img = cv::imread(image_file_path, cv::IMREAD_GRAYSCALE);
+    cv::Mat cv_img = cv::imread(image_file_path, cv::IMREAD_UNCHANGED);
 
     if (cv_img.empty()) {
       LOG_ERROR("Could not open or find the image");
     }
 
     // Create an Eigen matrix of type int from the OpenCV Mat
-    auto depth_image = Eigen::MatrixXi(cv_img.rows, cv_img.cols);
+    auto depth_image = Eigen::MatrixX<unsigned short>(cv_img.rows, cv_img.cols);
     for (int i = 0; i < cv_img.rows; ++i) {
       for (int j = 0; j < cv_img.cols; ++j) {
         depth_image(i, j) = cv_img.at<unsigned short>(i, j);
@@ -181,18 +184,20 @@ struct VisionDWATestConfig {
     controller->setCameraIntrinsics(focal_length.x(), focal_length.y(),
                                     principal_point.x(), principal_point.y());
 
+    controller->setCurrentState(robotState);
     auto found_target = controller->setInitialTracking(
         clicked_point(0), clicked_point(1), depth_image, detections);
-    if(!found_target){
+    if (!found_target) {
       LOG_WARNING("Point not found on image");
       return false;
-    }
-    else{
+    } else {
       LOG_INFO("Point found on image ...");
     }
+
     auto res = controller->getTrackingCtrl(depth_image, detections, cmd, cloud);
-    if(res.isTrajFound){
-      LOG_INFO("Got control (vx, vy, omega) = (", res.trajectory.velocities.vx[0], ", ",
+    if (res.isTrajFound) {
+      LOG_INFO("Got control (vx, vy, omega) = (",
+               res.trajectory.velocities.vx[0], ", ",
                res.trajectory.velocities.vy[0], ", ",
                res.trajectory.velocities.omega[0], ")");
     }
@@ -210,8 +215,8 @@ struct VisionDWATestConfig {
     Control::TrajSearchResult result;
 
     if (with_tracker) {
-      controller->setInitialTracking(ref_point_img(0), ref_point_img(1),
-                                     detected_boxes);
+      controller->setInitialTracking(ref_point_img(0),
+                                     ref_point_img(1), detected_boxes);
     }
 
     int step = 0;
@@ -377,7 +382,8 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
   Timer time;
 
   std::string filename =
-      "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif";
+      "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
+      "bag_image_depth.tif";
   Bbox2D box({410, 0}, {410, 390});
   std::vector<Bbox2D> detections_2d{box};
 
@@ -388,9 +394,8 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
 
   VisionDWATestConfig testConfig(cloud);
 
-  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d, initial_point, cloud);
+  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
+                                                   initial_point, cloud);
 
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
 }
-
-
diff --git a/tests/resources/control/bag_image_depth.tif b/tests/resources/control/bag_image_depth.tif
new file mode 100644
index 0000000000000000000000000000000000000000..d48f681a883a434de5fc5fe98de0981e08c75fff
GIT binary patch
literal 283426
zcmY(qi9ghj7ytXlU}kJHV;^ER8jO9&&WvsB`@V$ip)9GSn%-j>vhQ0OLbha!Hf7JA
zrG!*SK9nS-RI1zeckknI@BJUn<2=vvb<VM{kOz3JAOPKfVgw}!Z~g`Qd|R2;WWD{A
z0e6H7L<ujN0kBWodkcw+X7%BBEqFD}#S18yJ!WUH8(-0V|D4j3jpB-B)~inS8BrFJ
zED96}oq*$I^83S7H}3|(#cw|IyHc;{2$cZS{a9Nx1Ni4kOTWU+%$RXb{-M|Q;}FwP
z-sl1J?(m>$DR1n6#OdL8$s=Du-GKaZq3YuS6}G!~J(N)X{zdD--z4K?J{A<Ltg3lO
zLID42Gx75chv{(9r;cA|j@WZ}1<bFmEHba2V7s~;b%NVS_C5Mf>y7KtLbBh`?BFg=
zkHxV!Fa;V^RXq7?c=N@(h4VR;%c4z>cKt83+J+Q~{oV%j{5VhXqiJu?Vz2#myZH|C
z^Ws~ByLKCnf3%fm`~S)acr~!;WtV>hY6Wi0pX%x3dCLVL@b8&D{NuY{drrp*^Wjz6
zRRIbyLtl`B<O$=cYD2-<t~1m2*oKfKFV2DJPDTEqQ_mDdaTvHV7L!|(e)S#{!wb^u
zNyRc4mdQ0Di>22jYXv2gftItgaBuw)+EW-Sui&2C7!?2ZC|xi>P}3$uOppwqmY2Xs
zgo;chI?xE`Vm0gnC`^?+T#!{oZ3mNRicrvD1<(S)xN074$FF0!*<-^_#%8#T2kfn7
zQ2)9VF)J8vifFxmaBC6G|AZ@vz_!b*P&?n7B5Bnx*3C)<#TGGX;l7DgMO5e}AmS(y
zjMW64co#pvSBk$*WC|W#qRBJ_dopb*mY0>`JW+H}#iB2-m@?PS_z;8oPU6&5*ffUr
z9%ju>ZDt%CT%R99!zSnV-^EvTotbg)Jr*y(OFGt$I(S;#d62X=H}n+<=+?w$2e?0b
zo>_K)LP`4FT~j)ES3G3^NJE29FfgT+=#-85c;O^Z?YDh7>k+<m0Ogo4k^(}NvXXAq
zkDNPSji;K|-3raBZ>=ARWTrRET|hD0-S;6i1bEOv?TKEluCeN$O93O;v^!VtMj(BM
zpAtGzwo`ZfxJKr2AHyp^c}ZLiAiqq7H)GOgp&fV~gR<qV@FnFDMPgPX5EH6M2b74o
z4N$2bw*x8@ao<4Y21l{dGcQ~8?^OT%bFZ`GW$=HmzWv_mPg!L?iD)A<OZ|9!>d4dl
zON5W+rM*8TIT9B>bhCh$XQf5}U9A~{d(x=(BUKyUP33{z-i3>en>{LbO1_u~yjcH~
z@cJ2#&>!)~dmElvLDN@eI=0MZe+&1lUtyDe@cB}T5*yZSH}&;TAw0C=l@SzS?3N^Q
z`RM+)>Jr)g$~`@^=@?GD(>MCcXVD~kGq(P_q{9{QfaG0Le+^ZFueifFQLat*D5Sp3
zh&?#S;+nFF^ZVhs_C%(|XYD%IV*+LH-{}@TI&#b_0v-s!&H-s~bD#Z2fqbfiJP9N|
z$BIF5`IvN9jYqUHfEQhXZ5vr`9oaVK4zF<@t8!7GLYqd?+c4=9p!_WER$>2rxAX&0
zdp~L0Ix#22t|7W5wmIMhxJk0Qn52}ZH7RDbk7?@N9o<rzoUi^wQoHzBB6<6f=&~`u
zv-q}d=+E%G3rfKsCdCbIkov`6Dklb-rd!o1t5%O6Nta1?(r<`)TxS)Ot}aDvUksqS
z-p~x53%aiEl7H-lH!Gwgt+P{8*3zxRloaSwGu74X9(bVw%SbWJF}tlhf-esUOkOQl
z!)+h=(61KBd^U;q-Yef)jZaa0iTVjpz49Ni;vmc7Pm0XS)khMX!x=MO(<a_x!0FDk
zfxE@1w*W^Lz;BeHeYAOl7D>W=lq$8$TR4983a@1CND5z`m?qP23d^Xjy1$?DN>qa<
zPeXAT<re9*S^hZwDd*Cd%)#e+4UveiK9A|#*Ub4M{S#!j-XqZkRNsMbC!WS2`>Xp8
z+`w-tAwyq{oV9*lPefb}T@!IzaHaa+2`Aax|2Evj`UY{Pm>?XvZrtBy_;Y6K(ORCk
zE_fX$VgG`Nex5~lmGp=}qPcyYps@m;*ROzV5}(e~_e=4le~r?eoT(SRaTibW2x+D~
zO_4;N`6z-TQ*DZ2nKuVNq8umMZD&Z}@UK#YU|?%{*o@Zlv(0NQ1ztrK2OyCh$|^`t
z!CaA20{|B^z)7qX>en>qX|8*N70XIl)6Fpxm!dK6WCtPw5Pez3To$~dgDQSg0HEYj
za0%NEO-bJqYZ<}yoPi&v*r5xoJSNW-p7OM@2$r;zS)07**d7s}+2&GvQmXX!ZJ{-}
zQ6gbX!8&hPay%U=RQu4obZ^RGE$o7pFQm74VLze$VWg<6WP3^b6Y<;%*B#YG*b}vp
z$w~1vkq~X9$5pk+4!QdiD&)SQyQxG}MR}g<(4mHnRD90j8a1TmTV?G0d8e>l;OGn|
z2193K^8+}Z|Aiz`jRa_Va&vqPYL$Z+NfRZKMrEBg3dCsw|07{3ue_CbjwKc`>a3)w
zP~3;hDfMHQMx8M^>Lj<^r7q1fuR=1{{~%?uM>jv0IU`h|ywBwAjh<l~pnUUKw#EH;
zd;W|C8P<Vmy3k(A!(a?ILE{Nu%z)?a{Dj6Gz7S&{(fWwjKB&^^rWtR^dBH1vFQUb0
z72o8v)AVn)Rusu1;C<8%lsp-ghMt)AyK>Gf4@#&VtZBE8@<M${nT&Vs8kqCxUZ&di
z((pfDz}}c#-+j$X<biTfkLB3<T(}CmWSpR4Tf%X5;Pq!+pVCvJ*9|`HB-*z;@qF=m
z%Q&&eSK)gX@P1eE(q{3E<p16r7#d#s^!EL4x`4v^j$ZbK0ifsohnBzE<6dLGj9Lj&
z-}^%ekR+o`Yo$M}r@j9Ck+Ij){36rZY|Kr`d4@_dnQ=CRR@qN@05Sj^!G=Y)L)mVq
zu0h|jvaBy9vGJRv;W2a_qzo-4luZ^uK`N6h9Z7jfY-&0%(uk6C4nT8aS67)J9*)Xe
z&Ax})KN+DB+pGAfP*fFLOmh+^(a1%siH2(j6_^;=@p0(5=A@n<hkUX_sKrjVVlWjQ
zR#k^XpQx&{L?2fSnnj<$)}hcrw*uAXL#pbQ&{0)4g)!if%=G!P>ePO8Vvo{||Ik>0
zbhY;{U?&@Y0wO#sDGP;rGJ%W2V?6;fBFTdOx8kc|Cp>8~X%P!sC(Dv1yLpR}Y#JmV
z6SdoumNq84f;#Uk%)49-e)X;AT$;~WnXeo8W6pDOPtN{sF7%5^-T{1y4`0T4pIvVO
zEqftLNo$(F|1#WYGLZ>%u?4oM6Z0Bx6R_@zf~WCX?VjhPiCAV`GZTZ!sO0hJ@2t9a
zALfR+>PYAeY4Sah<UJ8C<LPP<P8{02#hyMDw!&3bS1T9wufOxE`Sn#_-9tSNi->tq
zN7hxUYTSBB&8v%K=2e37x?mk3Uyr+OXvgB@$gi?e6MT$CO9MrlP}f8N-2|?ultaVH
zQY!W>4?yLxjci6OucyPUn8~x`jBULS0&QxfGNf4>*`WNnI*Y>~snaLcse#<FQJohb
z8Fi5C;(b%LT+y%e!>a?P{qDQo-6S8B*6gqHLX*w>rmgiH>C$(2gF&qq<^qyIJVtML
zVF6k#O)t3gADcLcw!c2~D>$!ec@g=`-UNm;z4WH|!{yxTJ3n6s$Qn;`{=h+m`rh<I
zE}sep458>0jJ(PfWz~0m$9^72`-__dojC$<-v==79fNqa=_)A_Fh>T`5|%7S9kDIQ
zOOms1rV7>xmX(eWq*Jyos>^(><)8Bs#H2k0MFQ6W!77OD<f7iuYJ|Xui3I@XqYzEH
zs?zpSk14T0(_Y}l2=)}tk_O6|c^jK1+=w1(_G{QTMl&&=u+=_J_i)fA@uk}Ud=S*`
z|8qj@Gtt*XCQ3C0YUdUW4?MY;MN>oK0xf)U+Vv*{H>`)<e|9I-{In?17=np;AH~Wg
zr>J}*^w-psc;zXXs+ls|GTMLmsZ9wYWq!kC?#bsj#as8t^7jWpj{3-jI}KQb4vF1M
z3zTUI8s8Fo_|9=EPj+?Fgh+Gq@c&of8l@$gae(X(7}|Q3m17QH9kDXT*vNV0FC1>Z
zLYB&Kl>l~T^UpkMeyFPV+{&r{Z8-YD7t42LR%~=-$tM#QAJw<`G{%Pds{9ndHwfr9
z%2L^M&)coAXLfgUxm74!CJ}=A#LB2tN!>I|@*pUx27ODzXJfKVK;3EJUD$)V>~@!z
zxh03Da=*A^H`u}Hv(y`~ZQrYdvWf`0f{27ZuliJHcIjTuM;qNPth3`)bicX{eBZC`
z*;<9Y3}pAULZW~j_ORz1aAht+(`<j@WsgPm)i+I_FT2#de6$HUs64Um@wD=x#RDEL
z0l}S%(@LBas5)D7oY3Xx_Ym$6@Fz%xneFwef^NT%z{-P9SD5kLy}zV>S7ugsxCX;8
z5U;@3of4h#)!gn(Z68_1dr2|#s0nQw-deJfjfI(L|Al{Z<_eH(r>)#3ZuWQ~b2L>H
zwOTQAT%&#q$}xx$$)WoT(r_0r#c?1Tv6_mjaY}!IBfS_B@_frl!z3@K$yGxepqL?S
zTMlrX&SuSYtj91+?xZcey5yY3hlXfIKRLHAjsN9-Ggd#_Ei8^sh%3S6+lDlAS)>GQ
zKP--c`BZA$lX(}BIas5B%-=FRCBs{HZ?Ei)SLqdIG=*50XDpnTaXfRQxxR$Z<<h(b
zBj-TcyH$n-mmh{ccKbDrRi*~#G&f;OMQAO1yUXh+nS#<Aidd(JG@IF;V_!_x0p4A}
zHs*!J2$t=C@PiUW__z`tuVmBg*#)^7131Ki9zTYc39#?Fx3a53vY*^g6C>xTbER4I
zqDoCPy%0raJ}Qe9D7j-PLs-=2`Zw4S1}Cc>&B?ERBxke+rA7RwfgmGn*FjI{m}@hI
zqt!mH@!?)2MWKyHQc~JGV{uJD0?PJ;ua$ysoqwp&#uqW*Wk0{HF>@(pB6xmLFaC^p
zpK+_^vVKBzR>fO2EiL>@nZkZb%<~oJKn3xsr<=Q;LR8%&v%~e%lq1J{5cL#!=bnO~
zr=?8amCTW-J?QH)FSFa8-$gT@<X6&j%=f#?C;sy~-Zd>L#K-xr53u)rgX29+IlyGs
zpZp{7l<`v^amritCF1+{N5>6qx+kX(0e$<|*Z&**ZJ2d+?@zA)V`{IPUw7>rg{Z#s
zvv>MxHg{k4#1W4E5fU+@?^K*1-(zJHse?iLUN+?53>AhjYH;>|_t|xFikh8%NXQ>@
zNXJSJ&qQ1hGu>TB+%{=3V&B2EG-71{)QNV04N8Q6z_h1BKd|W5^`K=ahJV}257G))
z?XB@BV)8mi<tM)IUOzfXBR|oyX@5ANG~vV!bsJ3|0dBGAdvFm}6$lq%Rk?71AdP4V
zf}x54V8gFV{i)Tw#DC#d=Ug14lo(c&Qc@q5>)?Df{5^;!hHa!2(!w&GS*c6t;Ho+h
z9fqxYkN&5DCqzoz{n++Wt_$!LFfF!p|GdQ&=|KPXX8-yd28cAXGd-_Hw*K(gB*Cn`
z6e926_7a)JMlV}M4CgPl+pL2Ydx2w&*WY`#Ge*DnJOY=AG7F%Ws1_Rovw#+mwRlm&
zc8*mNh1b_QFax$c&dT?sS=1|j$+pS+MoGXQd-!^74dBch!+h(RNO>uy<R7{6quS2k
z9dt^&oOLtoOAL-1ZE(8kJ`Q#R;0b_6D?y_Q7%fB{$T}{~3e9ZaY%F2oBT+D8O{Pu(
zpn2-r@qkElzerat$ub;>+`b@ObTm>e5g^v%9ji|!%n!6}hsQt{YP`{KzSEmTdytWy
z4j10!r;gphe2}*r!z?B6`2pzk4ba7p{eK21WAKP|^{-RFPKz!dWdE11``~_0)a{oi
zbqh7-zlT0eU^;GUC}kZ)O6|Wr#fRf**=2Eff~SqRs~M-ST$fy8m20tml$^GxczwwC
z3uI~~W=|Aq+fNt^5)PamWKB)Ys(-)x)cDVy>E%HEeJXNpBlWn>F=JZEZx-45@>63t
zy*W(U@Gd%QI&6q8urJ-a?q~5dopdGi8(%M(3Hnu_O_jG`_yVcRGwV`dQV{0xh)T|?
zH-c`rA)a&2iF>s`z+^X10`Kl2P*tO2V4?a`Wxb?i4t>r#tU<NGJ#S<o!QcAiD?K^q
zXFYSywgG9=Op~Mm;rxgG82a+X9Ew2&4MT8<OavLig6Lj0H%t~8loNYTalT(jCkWdj
zxi4AXIJ^jObf#oc(Q3?-uQ2HaAP)*n9tnM>c$4ijg7TQ)GRi0Rewxu3163DZ<jbQ2
zFPl+IdLq=JVAJ#T_66Lyj5n|6#N-v#BB7xY!tG0thBIUcv_I<?xfr1uVpZojAXfbg
z$9plYU8Yn>^yl0O2sO0BWqy2YZ~u6P7Q;H87+oiM%P8wr#l*AuQ7C@sn&7qd!VMek
zpxC&8y?0SorWwO-)?F0RT0}>~V?!q1AUhJ~N_MR<_VkCuX6o19LGgNcmOcsp=;<RF
zkn`$w>Fx711sPpV>)+BYgx8qKbQR6Ne)+0Tu-L33<KdXnE9SkX8*yV<TEYFRk`%~I
z4BOC?V=aJW(gnhfb;iAb+33(9ZcLsSXt4rp`Vax9y&^{gw5ffXmq&HhNa3-g;S{fY
zx3e~Z4D*9OyO|s#*POb#j+HHh(>_d`&u!Qg)`|EM_A)5qcG!+)Kl=L!?mh<GP8mH<
zGB#(6ME!?qHWHoFbBGYUIH9dN=(Zjs-Nttr@$nC3;Uv$~8o{<!Phq3GB3DQgFAS$M
z_XpfP|6XrYOKG*`?T4P{TN5%&p3lSFfTaJ8{QWpt<Tb{4;q>1sr0>0BVufB_202xe
zQ<u9HCB40(5wldQ_S`hRse=BVwKB(t7G)tmiwiw(Ip3wF-BY<qthwg+k9?oA)acMD
z69pDvpW6zWtwF1qGQYBvRB3)*4~rtjfE<@19~x?@iAG@J*d{I@D6@q%S0H&NREIHL
zUO;65Xf<ND8EJp%RLcf&%X4iU3&6_Dzg4LGV(E%0a#D%D5&Oh(pP{zaqqx}l#UN%s
z#_FqJb`0;G;TB7}sn`P{*=_q1`CpFOgfY!Rguacd&tIB~xKVuba3e@Nf-x?F8n!D~
zde~ra=l1-L#ebK4+WC&F2g<aX%Djh^HSeyUNMxCrGYtC(2v73A?m@JOn*K`e%3|A~
zK%xI%POfm*5ZPeKg8}yEBoX4A>2waH*s>>F24n=pXys2G;Y#UWpSG}8v*Ropli4*l
z(VpXH0<yan6Sxq;jA6gNbAuwvJXH=24x2tw0=B%fz3idHa5yoi&s4>JTj+={ld+~O
zerP#_zdf@3S(ku($KOSl%-Jx}zRe#oP;UR9{+{IW^@+d|?E<A(u+lF?>>>G2Z1$5A
zfIvisvRqMnAH_lP6<!0$+Qd|alTd0na0Ua?NO5cqU<4raz7u8I)!ZjJ{c&087bL{E
z4DoJR`|)t9*3qrAErQpc_Wc89_quKq<no4J_RciG!^uWTIBh3j+|9p3k+$D?S}D54
zIK&-SKK9@P;b-h)j6f*I%HJMn^e=#wbzgw((X{%?KVn|EziC*n{_V%j-E<yU|JJYX
zJO<Ot(1X5BlGx6`bnHid!|6l&0}9frjZZ%p|3HZ90E!}OqP-Qw%|3U)P(venXG9;h
zdM7eyq2*%$pv;bAdq`7w1Ol^paERq5sSyPBW_grYdF4F{R(W5?)#-qB2iXc$Gx0Ce
zg>*5dVjFohFd3U{Dz>PMY4r6|4KO`SUpSnP#kwYF$|`W$$&=dopfviRGu_;RHEbyv
zbaY*A8$6;OR+oxS`|&VUaTB|+t_6)wPZe+Y?F))Qqf1i7L!*aNW$QgwQe|%TOD5i*
z;ge=xXtpT;Ok2r(!3$B_o(zwzL*7kW%?F=CF-2u~k%hD)cTPO5_hx~%MDl+In7ND}
zvCAyZ)p%Cdt-<TdB9=$v9(_+f$bB8VClI}u_PZbOD_WD?uZgXAt8d4g_z`atyM)(T
zQHmG1;CFVOzhc#G{94AFFj=|d(nNRfo0zIungHo!N?^B}i&-$>qN4p@oE!<4z-)tG
zFsZ|QjP)rxrdXLDgGuugU~P>?F&b{}zmFR|8hSB<MZWK3-4JsB+aN>kr98eyQ^zWB
zi4Gq)4Jw4<?t_XLuJ;VZP_?zbPsFU9)|&Dqo{ATtxc8vaCbsppvejPbkWAX2nVWmC
zGsUd`4l|1<KF~a#jeY6iQ(=C&hK9{yzRdFJN9emgXuY8*`*@N5&)lB_Y@h+y<>lFA
z%(@>R^O`P&<9$QtheKVfhTcmaZyAZMyk4mkRr;YSD!<{PS<aZbszq!@(I$n(CHBYo
zbF+2YfWUnB-o?#~D|?R+>c2qK;uqtRG((rHJZrSk<q?Pdnxw>Ej)Gf;(Cysoof#Kd
z_)QNO5>ve(#l+Mgadz=g{Y^n`hiy)r3L8@6F5+;e0?tq#73(LuL7_TU7B_Q`wh{&_
z=1`FUV(Uzs^Fc{*l40uTCEU@HtNe2b(YAw1GO8{x{`|FQpFv?Gq70PctO{{}YnTC?
zM?@?8cpL|9EbfDRVk7zWP9{iO#2vkQVN}i0w*{&E?UIcn*e5PENDn$^0QK$rJDBuk
zP$3VxYQ*7+8T&m6a3)d2UVipwif89+N%i@a0C@^=JFSJc2sHyoyfc!+6oo&Dx&7QD
z@swUc1ikJpB{X<mU9)A$8Bpgx?;n3+;C|bcPm$@xT@(`&pu%_{?;d^KhSw6G6UrJI
zQ6Q+qMA=VhXS6Tqau?(UrdgKODg<Y&VCbCMh5fkkGzk@K&~UtZn1C*wEjolCcB!Lt
zWx)bpp;BWrnp-S8lb*|^`7!-UPC$KZSu|HTK_@96F4ij9L<ye^EGmhWF(?xQu206@
z)D)R;vdqtq2_C!hU=8|`4)Lm0F9F$}0JLRfI3e>8Jhl`*K_l!1w}@}vy(vcFS%jQN
z$*0ohM=a1anbpJO<QXS|UBwrc8h^#kOnUvjBmNaPlzM0G@4-LPIqse{LF8zbO#l)n
zB5LiR*NgK+?R_oy&nF@OLzuJHr1GC7%%rIFG?;3v6o_4J&;qUQhWXFyOeJ^miy1M=
zbEG&PTB|DNk=yp8>tEoPLoab#LN#-@3#8k!+o!1#=5XmVb#0rK!tO^u1-zNy>q69t
zx=Xl)zFFt$>U!VjHNu{)C<N_RDSEanA#!82qOq_@SnVj`^>ra!VroM*#^hWqa-m`@
zU=SsxA~%k5O4LdFEG1hfR?o6iX-nT|Q5yQXe<3R|7^li&KB2Q+l)oqV81ATPjuXK<
z|Io=IARTk=Zc^~=jV{?G_Z7fPtGz%sc0eqZh5<BeEevz>beI-ADi$IWth^LUtJa1F
zrIUwWi~cJYQnB#velf3DlrJhu9#-XZGt=_bdbDw(B7yR*;88JZK43D!nYO4cSpJ_b
zVnUow*$|-?kmRG@HlG8;l6~(*;}vgPCz@=#nvPjZSxr_{K2dEsj#*SbG9~$p5`yK4
z3Y3aNYCi*nCCJxe`%UcBc&?0;{y*v{B+i*&xYBx8VP2S*Nx?)z^-%J_)nUJ?60(rQ
z5zu@MMa(FIeo5_rhoD(kIzdy%dQl63vtc}O#;;U<)aZB!TnC71_MK+Rf*F)B<T>wf
zMNEq6dcnfJkp^c|A1A^-PUsjXf1hzKBC!nmxp=X;EBs^QByT50%Kp2gev`n-ftQGH
z6Z%gt^_s}em)Sh>HypIy6tj5EBMPY+i2h2F;Z_v55V(G?LNi_rUQ3BX5;JjDwCXKP
zSq_lJ(*nXU)$YVH_|e-@enr!e-0^dd{3Yza#p#O**!`P^_Sf~hlcJ25N=#0^)PFHm
zNzdh8H++D&Jn-f<2?K|7fZtXsPaO}yzX1~URO}>;Kk)wgW@zxKOYrMo#ZNDL_Y89I
zYpK_p@jE|sr^BH?9wXF5+4QHAjsk|QlJKY0oN7_1acjQ~52g)1i84N&A<bztZS91>
zeoi_L!>Jmv!Of;0s->7Ytb6bgruTRxdsPKLNth#R>|_AsHuEV!R9FBkiTD`>3`-u)
zVHHZ`2d}7oy$@P_A#|v-@}5nAR{8F1YIH$36k_dEF$~zSIG&~0sZV^@(Qq34pl|!d
z9{2~x)f3v-$UG>F)d@mp=t4PcmMRl(buM)9%4pc)gl4Vmu>~^)TRK^-K-@Gx8v3A`
z+c6;ZTIhvKpd|EsPHJg{Ogt#Ec|Iexgr{BFo0WZKCH1;UfK+1N#P+uXU4dj4=ut0o
zo8zVH1z0#vHOA&NPNc##+B@Up48&_&X6Vf04Dxq{s^Xe&$*(gtxD&8v%Wp?x&svJo
z^}olNY#%Sq;tqWcaG{g0y9X1PzAs*JTiU`+_er)3)p-5a2a5Uex<&hK`GCl4iPT#^
z`1NBRemMWh7>D(Qhe<{3wx_p*f@?qsi~apQmBRbh&=aAaQOtG)&Agi`_tTZK<hJ0f
zhQ`}#?SQQ3$Kb(Qi;YKRt!ZPh4pkZ+okG;{(!0?J_spmdhu1ZypVZ0fzSR`@G7caS
zal=f-h@DG$yA2BG2X%h@VN}tD>A*wQ)g|cvG*PX0yPTahpb}x3FsyP-0yEa0a8hn=
zpU$cA9D7Fitz{T_fXWObdO5tL^PTXUXLX(imN1#^&}B^e-@KMnI>(4WEY!a&b%BWW
zUDvifQL*U)Wi%xx!c?~ReN?xTzD+9I4G4SXMU)B>W-HEMRyMPzzF+!VVaQ>y>r4BE
zIDx0z-8=G_ODIpeQSNf+_JC1fc=pUg7(Kh~x6y%EnnMP{;6oao6*A$EN<^9ll@faE
zRdQgYBT3VlKM5TNzgL}wXkjXOHx^Dp_y_hd4bFhFDQ}MuJ7|+6g1HIVs}KZJ(k%J$
z^%~;H0Nh!-K~sZAbvgjt#0Habg$w{u)pn<!JET0DF>{gn`6@TqIk)x&VxLBR5`GPb
zd<Zt0+ETdulZ4*aM-tfvji=JvBEp}6<e(ft_yBOLF(L?nQ1l$}E_SUqHNzPCW!~#(
zzIAPAo~yS-&5vCJPR@wY5w5QJmu%jfX;(}`mMlE{4}(s2GF3(U``y=)iWR;x$J64B
z1(O~fA3hHnY5OaPsju3i8g|FExJgEku8SSo31eH}(4-vFF*nA0bWc?61uUtcL&~P%
z#;S(QNfpu@tYo+@)P(zkw*JI6$QvWII-)?4wc%uS23XGWs7EDos(@EgHl5K`J^0A=
z<UF(1S4-=tlY{ruHS@%*)2h_MYzk7lz|f@;uSn2l0bo0O?p3I+OE}q}uv-l}KZ^G{
zZ^YI|hX=2QHSk0eOrMCEi%`NABS=XiiDc@O@e`~7`UyL=LiCu^W*Q=r3k{0>XR?rC
zL8;+7^8EP{^*t@29fv}8$-Or_^Qva4oM1FhE+T)DQ-wh6ARq$;=goX@nN#t1fB7it
z%W@m9dlQh_69bK*VaG9`P`~+U;8+IU>%ym&F@*UOV8uiMn`UEd^KR{LRiH4q8I5<n
zK<>XjanyWTI4x&jm1|n#HbXO<Tl|Ash(Ad!DQAtxVB9IxQSru|)9J4$$A11@%^YRS
z7p-^`Bg0wVig%~M4G*tV@Tl6dB+Yj}Ud7(#XA<;M=Q;rsu_oa)HAb518DkS>b$LER
ztmiZQ_~@^v%4;xMIx<qubm7ruJQ8tk%vW8`eVTcr3gTpWNgFXT@2^4wvIY(Ga-rWS
z_$^5GI&uVYKcPFc=_xjV)z79a2@w7#lqgFC<&r;FvO9n(j2!cr@)u05R3G;U3E+My
zFH`*<bo?1(xyPgGCh>Q8yU24Xta1F^;r1A6muYi`<7laHOR2VOK%nruB5O_B=(*p2
z*OR+T_NWIB({{eT;XX&*SZ)cZEjw)o8<PJ|@zz94P4_!erLn9yuSEXCsVi4DP1w%3
zglY)0G_?<I<s#YWkpDYl<pz7CcbS2b;=H^R|6N)MyZpb9)US<62uDf*G|Vh_g^~n!
z-sr!K)w|3Co@DiIHdMXZM&)12&QEQAZKGdk>=#W_<X1$cn1(qsKgz1xf?N&zV#Fx8
zWMdy?|6EX%^XGiMx*Yp6@eHo^euZ8h2yO`S&mHrEJ4<iN#*CJ$h3M~vK(Bdh?xqck
zT-3e3+A9Rr`yvP#4vaW$?rIHu7(an|8qoz8gID*(A=1ebR#jlwz7CyJOqml&G0Abr
z5DMZHoc9B-O|!oU1JsmegNHq?$l8q-%m#Lr2FV_OSLzu2&BONF)A$RHv8^cRVZPzs
z)2(B9&g|e7zn-0c+6wGa^z5?}k9}DD_3JW5;7u3wPrdTXjX!z~>T7^>b0`1zKIKC`
z_DA+0|L<OyGZ%x}TCM~3Zw9KvjYCazxlJ;J%-Iccp=@k}NFf<6s>6u^(cy@%kk9Qi
z1x`F>s)Ig99?u+-3(Y>u^9c?hCoT$)M$Jtp0YoC#*D>b5%bT%H3<4aFX@_%+?+p(A
zOIr!OpuQ9Np2{Q*Zw6SeK<)!p7>)(T)R*QDx>Mog_mG}YQYB!cek1ha2oYm?tkZT}
ztI%OfduQB{oGA-B5;c?nC!(U}S!I}NQT~ygj#dEexHXus4yCSj)49j~07{z_`+RBZ
zHfX*mZ|m!EG_S*MzAH^8<x|acxPaEw9<M+ET}1x`H>)U_#W4DyU(hkpEL<4D>RMuO
zyxhJs6o@I-?T(uIw*593Tl8Kcn03=)FNtHWQ9Qv1s^1qk<o46<jGtSXI9OF8lOOJR
zj!TlUBq}RX=+zy#Xl&ELaA!Caz+sn8(99?i{>;en@P$&FU-^AVN51nXMhyJ3$7ufd
zs1GoGn@OA_jXds3M72FoDHP{X#rjX3bVa|Rf2y{=j0rFs?&oO%@}7;<2G$NcIv3Lr
z<j0UHR}3zdB{{m6wOR8>4yuXAh~2<7YSyR9Wk`L^30d0s*?7`|y%{|1#|>ZMXu?h0
zZr061-j4R;#()u_ipPgpR~uoTbTswfZB4y7mGL!(hwm<ML|0@8GZFme`Hp_JAEUjV
z(wpzpX~lW0e&K`bY}C1z?l;&+6nsHQDA5m0swW07W2zop7-UseFXW?J(9o?aejTXA
z|3RjXZLQ6#i~MgFIR-x^s#B)#Npw(pcD=*<j1S`+4lN#=s9nr+l({%jc)Chf2FQJH
zhPl1|)Zm$OilYh7l~4Op1nxhn(z6ycgc$@YrZes!uVayO5#L8M%NfzbF-$6vM(fG&
zHx_)!)77Zndig)A%~}k^LLX6$x4;SK0`Q#CzUllQ(*1t4o%QpKdgrx@xpcafwJ$-P
zim05E5VJLF(;Xsgv#L>Nxa6@W_3emHX}b5Em<=|i-KQ-ImWmv6`*je>nzO><P}Eoj
zLp~4MID)$sz~vF97z0;^sVotmK$F;VcP*3heqye2J|ia=bQU`)2NT3|)YSxJ79DmA
z)Wk0hh-+RhF%}5SsCGekSP*N&J_gKiX53~+9<5pBWj9foL=3KtvIw<AwLoyJ#Hc!0
zom6VAreWPBty(+gThvB)I!U`FI%4v;U>(abQ6zS!mY2Io@JN($JA*W;H5dZH_7^cs
zrq(1bYw^3>Kn15;(eO0PBr%CRYHjbyNQnBlo0;R<KEDj(jR`!`YUf4_Dp!@SR$qPA
zq*<|Gp&k`kr#Jk#?!5_|k-#D&vb*#6D5D(Uu@*_NCtSpHB=jwVOk|bsiR(-C2;4Mc
zn^Z8RQjV?w^@P>g9M5!rq*LP=+xuCF5Z}OSh4%5ms<h{p!54hiGSBfJ&KC$eQ^wQ>
z&kYK%=a?5!q{1NQ*AdG(<~6Hwk<m&5{vQGEyOh_vKq3TTljpBwk!&F=vf=JiyoON2
zum8P>LwplVj^(bJsl23I^;2cw4L7!0>ax8L#+c)Gh(PRjdT8Ml(#`jSZ(iL<+U%O{
zU9btMx>#m5A1Ul{)q}T6qZmA$j8^$|<`kBo+oAFbw<wXxyXw+U7m*+F;>k+7sz2H;
zDl&}-O{4@sjF}EdjY(p+&{~8?(HQEaY5k3}9(-@ET7x8?%}h}!$=CK%KWS*iL7abD
zGZUhwaeQG>^4Z{-%s`Z8j{CbSyq#yAO-fFXIPIkA5W3oir`w2=A`VJO$K<InMz*j7
zI_G&MHNX4-GdazT0kx8IMW@He!Y&hnd+KuuQI^8b72f%(rWM!fw)`3=`)5oK#q!`(
zG}+VZWYtG`)k18w%kve@sAXF(q;0gGo{uh)%Xd01Il=j}<D)ThOtU(sUG8Z*l>A=C
zY_||-SONaaH>?(1^tk#~(V>^^|K(s3)!*JIP)L<CrH@0!y+RF+{uiC@=EMbyLbLj%
z><;i#7Tmd`fqS7Qt!zrg`Z6)+3%YDK%WVDfnfQ4-uCga<2zN=GG?Yn~BcFzZD?)BM
zR%wWl1FAUr<}vLE&S0njn2PCw3&}h&Y+v##9G7W?f6gB>5;=!2vVD>Nij@_nzXzkl
zE@4W<esc*^25sqr9}cgJbF<#=xo_8eSGbzr)Bd9=J{}IQ4p}AJbLCsW=Qh8p7<CFM
zbBgwvAsh;X!Z?K}rYa7;drwZ~-#Mp+Vc)ZToL^P5bx+6Fwjux4!PR>PDE^TPArE{y
zF@h41ixzM8&VEOYaf06;oVFK|`LTZB`S9Xx68;1Cul~m>$KT9NbQ^ps0e<zS>Ap^q
zzMa99-$x|CL8DDQ$e+H@Ln6b7MppV`xJgh;8aUOdb7*q<&{VK0T1^yA&8ZL;#pFaN
zhz|ph@ZWm21TRiw<hoXpsXgyS0h4H+7cg^2OGC`vf3Xj@MLSyLTNMC@v$1YdVI&Nl
z%A<^-lOL<H=z8togp<OY`lM4R8<{Ss28B%IXME7or|Eqr+vUC2S7CMJu<4CEOD-Pk
z=6=~JLFTd2V$5u6;K0fq8ENy`dKs08yQwjD)^uTZK0Bp<ZTKOUi_s3Co!Fk`x1e-3
z9@hMaXF65x=hTwtF>If?2>U-_j@a0Vd&XE1@;qp#l7UO?9o1)no}qfUsYO>yLA{PH
zl&CmWwXo^*-J(P@hL`%CQlnS-XE@_rUGGeFo6$5{KEB>HF;8TDZ{)e;Eds%YIpt=K
zUjal#5DfQ&pe4ZltMHrDnQoWdtL|SW?<p*v%`>F0#MdFCS4DBpQ{L9Qs#OV-Nq>@?
zMs)7Bw^+^kmfnuSH#0D{vw2euI&itFh86bax*K6!zU>>K^6SNzrAwz7@1d4{LzvOn
z=1Kt0e|G@We5tScj*il$Dpq<xUj4@lRQ|>=9V)D3Lw+NmtC>b!N_J~1d|P^KXZ~k?
z3!N*{%lY0@AwXww30`uyT<ZRiDyieqM9il8(eYS-(>E#hE92Pt2ktFnK-{F4)pLy_
z0ZH?lw-TVpE}I(>%g1c~t+$(Y*rBUu_iJt6-c#}_%g^7^_FWT6(_UGV5cd7MRd4$q
zc8apX(Vt{l^62l*>uKjqsmC)W8nb4X^@BbSqB5#-=%VYK&_|z(JAPRHn_`ZdMk`PM
zl2)Z&4ilCkBq_2T$EnlodXgRyVmXixK3Z(%VvIm|rk^D572@_CBlg+9(vEWAv@N1f
z>?k)={gwOfE#o#>sS>ev&=wA(-Yf|ncqXKhs?uZ7vzbuo1fEXeG!t)I2(Ox>6CKRX
zn<2l+*F2l!1dtT_hR~XNp=Adw57J&92X7Pa=mp>`Mq@R!iH5IoyLqesZY-0NM$B`Q
zJ@0e(&cji0q_hty@=R@)PKRs8`Aa0^f%3b+G5i*Hc5>}0L>+}AP*C<nFI#w#6K%*p
z<`O0)m!gz6rfNPK&e_o0P1+E7{DuNQo`g2G(3b35ASdUjQ7J<Wp$9=3ou2JoCaPRg
zgYM_K>>d5H>Sf(<9uCyP_A>8L;%7xdZcuT%QdnC*RIW4{8U?|Y<OG+QFE%;J3>`4;
zu0fw#e+f~vfmP?)12B*C4x#T%T5P(3`0_8=TA8#$!LXl`%M(9U0+)w0zAzWEuoiJ&
zS{Vgh%P-ORn8+_f#odmbo+HE~3V|{`IVJORwH8#x)X2UZ+R`mw1W=KmJ6vIop-Sc#
z#zgrhX+I%sEN^AU5zyCiT%=B{-MD;)fUCNHPxAwlQl~i8+kh|+1ddceGy@S!IY!_P
z4Wo6|h&>Zwr1yyqJag^^0wpPa7K7M94m<2Sy(bl4{{^QlCfE-Oo08}F@vm}E*Pgq6
z-VJErC7ACz_ERNz<5x`?jF`zoL+%egD^5EXRV@DWxpM@~bo_1AwB@L|3kycNq;_-#
za~X98|ISTtY&#v!)9Y>%M#`+blT$e=;)P1AI+Bc40q6TT(i9?*+{sR!#}TGHvhJNa
z{+~tOTDyA*2%Z_sx5P#gVS<SK1_#?>cYx^j;Ha~fOj>Z93~`BJSKZl_p6j>9|8h>4
zCbNTugGzF<w#QD2Eod<a`B{bjSg4oa{gdaXv)XMnvIsG*Myuo}=R;)NGFNP76~k|B
zNt4|&dgAs!X1&-$5?bT#sqS?EA|9#yg3T2t+P@}S!c66OoF|I*r=0zNT_%YfcH|!H
zc+~&!ys=)`i&bg=&^E82k|`p1H}_+~vc>Vt+9HR%k`MC8Ly!Cu`_J#P0K>_$qnc$N
zFDpV^8gfoFuQQ?lulO;rh`R)N2^f>|Kj0xGIIs3`I53Gi+Pt874T?Me58_KcWxdtH
zq}_?Dh%T!ROBCLciyaGAO0#|qGXU&~0v^pcZh;yAJDuR8q+pr-3^#puguTfPOnG~!
zY#E{J!(goYac2s#DYPvGs(1-E3Iu_kiu~GEzGviO#=KuN43)(icGzQ{$!*upC4&ws
zhzxrUiR^-PRkdQDa`d;3V8z~g-x!!V5JE@u5k#%7CvRewn#6o@zzYfo;{S*r`hUP1
z6msLd-N!NQMQ3Y}3m@|k2}-Jk1@l=t?G+~0ev)%tWKE{tdC>D_*BD_$!@qahNCH@o
zEF^y2dHtOtUHSLatkK_&M8CiF$M)cVu?Y6(Y>Fy^yP%<61>&&w=zAwlK;YRC&i<VA
zNsj)f&1A-az=w|<2Gd^#*!!-==_L0$KsW?yRR+HPHk}T45*=zQ|0vd9Pqu?U5qN0D
zJCNHBld8&HHkWLUAw&NTe?B~e1yE$!9E(u=GY|+Rb&xRs)?VNhLCE00_2J>j|K2dh
zl`wf<pB|$Ny0fWF3D&$-2LxjT^!4eGU;4Il?TwJ9g4VY*^kb=T)b&%)f~V>hoH&Lf
zxpCqHtDp=X-=Wm*WQ!H*1<pE+n9@Lr7OG<eY1oRl;O@=wkJ^~Iw%vkj*42(DNrSnc
z@T@C?ue={w*aQWz^2+!WJ5Nf!Gg#d8$pT!zSxcW>ng`f|?dmncOlo4cF`fOWYmI<s
zOgWli8DBQ1O?E0+=dq5x4>MS{QcXyFNvU}iT@_P-rF&`4@_>MtX(8j$f>2N(AR3vO
z`ie(mE+RmydM?;2fAHsw*NEcV(_ZIgbuU!K2a6>6iz=z61&GDNQ#_bK4-~PnTb#y<
z@{PRBUKZ*9ENm<9a1gU8*k5}~{-p0sTzYzAX5&fUMu6b1hUk<Hm7gvne7Nt^#(K|{
zer<~vFY?4-lB*&q@_i0)vmw0Yee!#zPU9_1`9^CC=4#a6Cd>|esza+@^D*P3p5UXk
z77NQwQ0XOZ4-EC=R<C9)DL~32^?8ooX?ytByen?I*x`A5bAXeLlPbT-ROhR5hnI^z
z1GbDi*ZOA{c6>R4LCt!66nKn?KkLV;8iTLirFo>eV6rR`?^#Kx2_6}O=Y*3kkNUk^
ziP!PNdk;U5ZyS<g8Y%mI`US4LLyi!W-(%2_SHCYEZx_k72jv2CIMeaw8##_9+?SKE
zGKBTKtI{NPlWEGK(Z~^Yro6rxj3U=ZsLoyq*Gm%E;Jxwv1J{Z*G%<SzRkW$+>4@bF
zI(@OUT7{i*+)L69&X#a9aJ@1=U(eP`4MR-&fXuI;*;#Bdjt*N092xLP{fylo6x!u}
z7PbPfJAWp}g+#?20Pz5ugPGV^!wI2KXUjCFoHHixdV$w6sw1F$Kcb0=9g3F8l2&%)
zy*DL4h*`Bc{>aGW@j|&OX<YSn(G8>P9u9Ke+oMTsHD;qDI&XqteV(lY{hiZEtXwr`
zP*G#Kqng69H3JPsyB7wbY8OQncJGj6G5w+^#Qg2{g9dOD<5pyK%*1_DYgYw1_nMof
zydnXrd9V0u<7_b*AVFF7bOnJyC{p(gLBX!5%UUTHf7&n4(+nRKUxdacXViA*KeF~D
zeI^a=CfS+g<3j>}L_vgC3xlg2y~d5MPazub6pwj(A>~PvXW9kPl7699l``s!cZ38<
z9_7)uF_XiWx&x|TEj=)hSN!o<qv&v@Y?4ch@BARDSmcLym>;&_VY}mh1Ya0!y-Kxj
z;$E@N@sOtK>Z#lsu{r(^*DP{Lv(WKpH%~atL!OmI%{SzjZtgG@-Og%6WnSAtJ5EA=
zLsAw%FaXX+BWfOdert1nmjCPPre~N>et>>8;yV=o8Y@5tVT&7QL-nsDR_(=1-c|LV
zy&<N%+dgP4=ET-)_4V@Y`v5f*K8%^QPug|l?7#UbWF<kORr&sy$Dn;?gz@(dIevaB
z&6I5wZq%izgQj>z<bS;IhY%)?Ul>0binTPoGZ(+?O^cu|uAA7wITMD+tHMmYUP#&!
zR@nuHoC`nKI@_Z+aKC=##hq8SNEcI|JeUa2=`i(y_M3=v{k2Cfes(@KM$Vx<y?DFk
zWG1qcVRrD}<+qKuTv3^^Iip@EnhD*Fvy3W*&?~PM<Scc}VJvv4YFry!^lP{EQJMQF
ztMYt#Pz!A*#fw+ntvg!q^gFN1J-67<86JPsM7LFdfRZYy0Gqd+WFg|AHDaRvq@+4{
z?V$|u-NbN-X;!Gu$kn5=%`}gzrkXzE-Pv{f>BVyrg6nc4XK(7Y@R2?vr=FSX$pgT(
zK-2|9#1e(}|7YJhXB-;frBGV!CuHI1G&8?kEsPEs$fmA4YU4-$LeKvTJ;O{I`Q$)r
zKu|8R;i+b!zKm_!#5b4@s2?<aR}v*Hz;IY;`*&tjZkGK75GHES95{1EFjaSn25`xi
z{tqf)er#{$+efh?T0RLc!aMsScC8e$oZl5{%Qz_vA}8e^_si~DOG=$Lybh?Wa9cMf
zGm}oN{2<loq(aUFv9cqkIkd~im#Tnz-#n-=IAesq6)2?GuwdNjB^Eqe0-z{n_6}sB
zlbjM1(?lZi!c|Tv#nbVoP(xFdt7UEiLN-}xAC&4-G1*C;{<e6K=xazI9YFFDumT@>
zB^Fn|(+vy^uQh#u96bJNc)6hI;19u`org_7lIQE8;j}dzly0>LE>Wg+e?h1@WdblG
z<z;e`F+x&v&$!iVEV(zQ-4wz4tnfu(fJh)544rO1!DG^95+Ww!&>0KuY5gkCF#WuI
z>5eH<(gtUm-FZROJg1o<tH*+iu(McbILE(F!`q~BCPqMUe1!A}w9LoxsL@f+CRJGZ
z_Q1!<eCuAo3Tpfeu>I1!rG3Y8+(q>%s(GUeL!bG!o2jU7@RUp{6g97P1`!u#$&D~e
zvQQ5Ra1vT`vAs6QM+KExY!3_3TMm6LGFBzPL>2d7v9_&3jKDF>^E`w=<tL)P&UHCw
zESHQc9O=*ca-UBDli$N;q~Yqw6X<~mhlR_yc8mtwYesUO@&-8X=Gr{tcoM$gD(~V0
zh-kcwt4R&}g2#Z8`${$ORMQC0Z{x-nv1d(x=J|=>kWpSVO&;z}Et{JuuiVAhB7I)O
z%Q0Es<~h^m!-Vfe*{P#ML)M)#?-|*CNAu_|x$1``zjga2Q?l}+PVbKGTkDx}LgagB
zB$?O*tq2V@7Ku`?SA*;|Kz++V&CP_`Dgc_Ga<eh?XM2Nv=^CQFjw!!Z(kWLo*}h5V
zfU{JsZ5O-LECiivH7!$E*;PNQ>+3%;ad)$t;7bF&Sl|=K?dMHqI&F$B?R+i&Y6*CN
zsi<~+@I@F8Ot<Q~&UL9?WOp8}Ct32{Wi{WndQLy$=4CVU?2Vnr9UC=)ml{yK*DFjF
zk9RWo!BOft*{d2;+f5_dzQI&k!X;T%0dSFfP_H6TyB=Seq(j8t)YX~l1wQI#ed)V>
zQQDG531SId6q=8AS=lqW9cOsN#O|*s_HSnSLH<m&w3qo%AU;m=Pu>;$6@fl}Lh;Yg
zJcUmeEhO^{zrW>Nu~ei^vvUwO`J3PL0*o{`7nM-Xo)cqqX}1>HC-yJtRKqc7m4bZ>
z0q0S<P#d0Tec8=~M?&lgfoaWvEE60xVyD}}X9+jS%i~T<t9B07OA0|_WAld&3MN=%
zl+j^Ws%N=F)TDL&$XE?i=UOJR!LH$suEeP04b10J$A1^1jWk?Swi01NL1TeZ-ATiY
zt$<JV;I{>cXxnD*e}_mT?N&}l)8On1Ey4#+#PTJAx0Xl<GYU#P&;v6=iS3XT$Gdrn
z`Am=)%J_c%7xyEvG00*S^Z#T^61%Xx(~PnHX9Feq@Bq}jwdI-DldjYqX}c*iYgbXV
zrwY=6(bz(Sc|=ED4XeQMOw{e7<0>2EhZXj{pC>&q6gDs2sKLwzBvk~TS_R>^Lx6T%
z5o~aUdQ2v1kNhQ^ioSa8+N6Keoubwkp6h}UrXM}-$xc~?GActmExJkp;N_A<_&Guz
zed^v41zju6dX~|6SN3qVm*X-3Uu?M>0;P=0w~*2&M?_|Mtg>+}F}5D#A}&^T%m1KT
zCwwQr0~PQJn{h*_HCN|-MToQ4zo!xXdkD9PkC{&QG=isJJm`nH(ex#*rOd1q{Fn7=
z1p5G!O#&tMpkJ8yX-qZ^)VKJypUJa`Sz9TD8B<06%Y?VO^4@{Tdg}4<>|DM*COUz7
z9#BP6zqso;)ITB(Y3|!i%3c&#KCocJHcOk>--mm8X(dnp+Q-J{jY@er|BaN%NZAdS
zFg|Gq9VKA95wFCT&bb8njenxU(DOK&KQuB7PlsBvEp`m?8*%28nV_V{6>f=C>qOhb
zs_{p)Qp(pLw%CXixufK-;WhAR+wXB8y9@t$++k3o$r00G6hik%E~i6uyV{!}{mUo9
z%P_ek40EcpMw<N_m)vbo{+%CfT@})!N=~w}CISH1<C`Z}*5-mfL&(TaL%Bwco<FZ7
zrvJ7)@6vw2L+A4WNu!re-ix>DH_<9C?I;p+jZ(|w$t>aRE|`P+s22a1D?sQiS;Q~`
z>yBmA8H<$(CQaAhyUu%*yz$^m*~v9Iea-g6iV4SWr^X*<QQYrTwkG9Ve47)dk8bM6
zf3#<i*^y>Y>{WspX;ib{#K`FXCxhplQD`7_ps%%r7|bIGa#er6i_sAk1I(*Vcjx~x
zKMLrr&z>jYTp-1L8!BN}E8R3fTYx{!YxaWSKOn|1F}_T(bmImL4B}Y`$#FihYyZlT
z2M4(L(SJT9>G~vD83~{zMTYRr4uJ58%%$hhb_!{?Te4;745%G+wyCfPx>(p%cc4Ys
zgBkRXz0zv%fg`i>*VWvYYq?y%jnUN+*2~gZ{FOd7dml+bDgf_``+2&nk>^$tbbp$e
z`KS`8iQMXCBmXZ3-NoFfC$__-z|_?!ai2k8?lB;W>bz;v_4Q@^1)oR%#l!R1sp_qM
zLc+flzKrQ(1iU3)eEJ#}>AP@z7PgyY_(vssP<s2Xj`FYNADG*72fur=(hdrHr!8Te
zk9GqQe|rBfrp`L7$v<rSH!xr$#|UXQ7~S2-2Es_`?odie5dn3KfwVBXWOO5;fP{3H
zfTSp(sI-V+AnLo{^FGIWyno;S-{<!_uk&+WK1j}tz7yu@-NBO${w$rQv#U%xP^4b0
zu1tvOzY&>fL8jiDhZI*v%w9`!jDE%YoR2i`FwtO{jAIa5EppfSb&sbrSTRhxOjM)h
z+&LLo5%J0YhQU@GFfPI*Z47%)LqTlahsYW4iqOi=Sb9LDXGGx4GFWD9GI`Tor<0Ru
z%foM1k+fd_)MjdsPtoug(eKBMGvaFF8b0D`ce;c<rFPq@d!=Rt!<JNVmeWgWyTudE
zhIp$j@}B_bg#F9xB8Vd$AnSuz65wM<rBpEoC?hgv`(wVqC@e|#DW9kl!d#uON~Q&{
zg&i(U86wL7`)~((f8ufoy^rY%IjxTqMzA!nyM2Mydrc<NHXO@33Gsi6K@CT@4{Ur@
zb4n6y6L8(V_Tr(vf%6^~v%J9?w?%?D{NJ&;AaKM-%OZdaSV_`LO@-4u(4j{6aA9rN
z#lY7o^iFx!6xew#2mYhUGwuDZ;`U=lc4JIt$=!Z{L1ABwZM}V3o2=Z%m*>W~&jcsv
z7_rA&R=JzxAy!##h*Pac&<vuAyL}6$cdOK4PW{bT@;enGoa%DVebmZmHp*xmv>$au
z{%w!-7n^No5Sv`5?MJY{7F&pEp}i>h<Opa-X~Tl%v+7jWVfIs2iyu>^bmZUxVRe(6
z?eOeI-wtBK1=~dHTOYDz`{Mh`tKDk?7W0aKri@8h()j(d(x~LX+Dh<)y&ibuK6sOu
z##pf3*`!YR_@dU4n8kFQAafYN`=$4b1|xI1&LBY|ljlC~zd6n^7~MlggA%;a`9zJA
zrT54}7SpAEP{}%Bb5dM?oaX{=6ZRwH8M&qzJ)qro`rCr*uS?)xu-47DWbp1oeUVzK
zvT?D$#6imk2BHGm1bqPhi`roi^<$v*b+(l}!rQkuTi83|3F~`*Q?c(nI-^!knOj5S
z(~mTypG{8lOX!<{SW&|GBStH=aC7}2-2z*y>bDsni`mZE{MB~U>#4RBS$DHhIfgoG
z11f8=Mw|3&E-2Q-#ITA+$MkDEj!W-xro$Yrtz~r0Y41mp>DH1PLf*CI#ERSvk>VhP
zGj!+q^WC70%a&dZpIVXIa|glQ+CIO=3|#8jPWL~Tfn68PGjGpY(6a0g=N-z+D$L$N
zBs6=!8(Dp$>WSxfYRT>6uL#IO&^<965{(lu;LcMIn6$y5&1M-E<??mequM@UZxxnO
z`Bm1I)9IA6z6pMe>4(1RL0?y`TGY91;)COA<=r&QI)ooZUN5{c4bbMx2!?JHC_U1-
z`Wd2rtC6FM2gZ9+{09<ov|8Cgs6un?Iryji>Cfq)LT|_-BWp57*=QtC+%bV6rb4N6
zv1Or;1O=_bD)96;8`bI)AdH1Z)l&aD_uF8BB8I^y9NB#AmB9kxqZx12S0emr?DY^V
za2Sih^pa07=$<yfTqOJi#ee{*yvE{iKWfm_G4qAPOo!=@IyC*S`RZQB$4=!=URaPF
z+|c&}^|Mn@y9SUw2plCJ{JibB^_ru}Knr3!MVCn&xf7E;B7hcQGIQr<h*dnE)b@w{
zbEk}HVdMEJu&VUXf25=(T#o^zYXE6@Mx$#JNi|sE^L*=Tdptm<)9|qvinuh)@`CYJ
z`#hO!+@nmeIRu>4gGwr4v4lwmZpG)6LCbhEZ|&4;#hvx0RvG!p`>3^%Q+kfF{qmHw
z8M8W=Ufl<KajxM8|9XKkInPq@w2yTfV>`R3@-@nI>f0!FG_9n^NGKwd_tMIMgCFCt
z@-jk@6fQ~@5Yn7N2H-GlstBnBjbeBVgSotfLik4r*f!JTPMZbWbd~$O{q%e5=J)j2
z<|au&*r<bSxNzLZ$wM`{@8ev)F)~6{!;kKMG8z&kn6t&p3@&#$@4O&}-^kD{s7DZZ
z3YgSr1WfJ->q|Q0pd^v|kEI9XAk=uYTXT&UjGiHOXgf$dK#!3XN9FOgvmyk>5I<HS
zr%F5XPvD-xww#X@X~h?6SQt_pT4t?;(-gF<5m>$}hSwzq)ns90*%I`9;oxD@eE!&Y
z;&$grNHsO}kk~83X&8zoYk*e)g5>W4o(=|l4?54`@QiZ;u83!*S?y#y6?H@*nOem1
z2DmI5xM4xS(Dqxo2B+`8AtmTmINcK)>l~bB>uJOCO+If*)R~MSJ0MQIkl-G3OP5=^
zslu{G4wEyR%UKD4UIlBqfJI}|+}@9?v@`I<;^D{KQA51L*#cj;A60fYt{jp?u9F`F
zp}B8Z@=oGm8sroHLX&a&pX)sRk0DJLd&_?lpD)lauRdORhyH!_=@#&(-`D)&pH>hH
z8F{}M5{yUr=)%(b(~}sfWl!2V4^Ur1-%#qDc!|>W$+BF541YO8up%GpTEuC81yQp0
z!$FEn=<5weTn)|pti0&2G*etdO!zxwdLqVH93Y|siU)kuNtnd0Mjeg}*2*YB#40?)
z0j)pD#IekDwX(*9EdVqX%F-)Uni)YRI_snHQPsrIZ%zrGNkS(KzNvwkN^Z}K%(wcW
zmS@^t`-^1rq<rAR5Fhml>RY@XAw^vjcF?`VM<Lc`09C71V5L6!?NQPvt7wcRsiVil
zlAp<;6dUt}9t#w0B1=Rq*YxfD!zR=a%%pUULR&?-06ZI~k1TD!?<;GXD*0H{F_EVs
zemy}mIk@FJn!}@%3i8bbLGH9ea7yHv50?Ew1Mzq1r+g2%Di=T=BIQheM3=HjeskMA
zk+a6T8dwhDiFqDO$SphT9)kV2W}}?VBAc?Tz0y8&;0`8W4~yue#ExxEKfvHzJH#+i
zZ{5hgd3PO&2OL(X#PW`Ii@*}r)%L|b5=Fbv3>%_HlLb|!w1!+Y&WRzD&6NR>PI*EG
zl{Q2EXOXN=KHpnTi>=vr$*SBF`Cfw)!|>O>fFGEC0wi|P9X+>eZZyA8r`4)x2uk2z
zKMDe??e<g}wSs%36uJ1u0V)TpfP)&uY&KURLk!Ve<R*G7tniBQG(h*oWa*CSNUry@
zMs-Z=bBv|^8Zq@@XxpwYXUgMSe&O!{<?iwaTb;MXAAcmK-Q<tE*H30PR-j+JcJP~b
zs@cRaP>(NuG=u?;Uu`aI;nS3rp82ZxO4o>cv=1K?J6>XCgd7akwWs<QV*BnlA3@>1
z^vH$ymDUTUeFXaHUvjAaeo6*R>R~e`qr!Imr+v)dYhQkG_Wz`&+`2GQmo8({#&JN*
z*Zz7Wq-RttfQj~8{GLe@vO~H%`OKH{#*k83p^Bdc+pXb<#CcU+4DPmelE?ME34fgv
zl~U8mM`3lMdTQB9Uau=`5a!;dU4zzFJT3|?>%!veuIZv!`K#bbM4VVvKZ=vQ$w7w0
zzg{s2T{tme8|dze%HeO$dLxn$5F;z`yuHMLf5A;iGi}T#!eT|P&1>q3wj}lV6ca6A
zaxDji!Ldy2)NiRv)aJ_Bi}F=;fAFXf%}K9i8QvMLLD0$BwE3ccEQ`b7hfPM3HZr4C
z<ZT(vOHt!z2eMvgvYDrUZS)QN2JD{IW`rtv6O??0?T3TX@QQ6bH}$@AGC`Co<bu0D
zM_3m6zVO8(w$tOE=x}o^-#YR-Qp7=*5;T6o)7)oZ)yiDcI_`x>>8mpfMPJ@n_Lu5?
zy35KOy{JtQb^GfMPPqfLS>!HI7FRl$Q^v5}Pd16rC%GE$^_*&DL(3Iva4K75MztXo
z2nO@1>m71{@J}H08QI*31()*MYpA5Anu(DQ&kn}bTkY{YZ?H54zJ9(i(q6CjnMcm-
z{s?Uu$Y8t2LFMZ>Tm`<>^f*1p=^H>PieM1@M5wMpKdE1DvYzeI9Hx|l7nK$;o*bcl
zWI?Xa7@jKrrM)V0g(84l@gZXu7<F^`@s{Verza6V(UZH$H#UGa8=7HlcGC8@G7%4T
zV6jV3raGd3)M|#CL(w`s+_FGCtG=<;C!$QVv?CMOPv$S_^|~N)@OGjlCy%jFDr4y(
z+LYM}&~?n_x$VoFC^joN%^q{jBgUI~A3oNVlu*InV1K8AS4y-Mx?fJ7-80N9pI*3L
zD_~e{EftZ?AiyV8K8!a4Wyq2J{C?iQ5ZDuZiBn8eLt9o1*&BIS8Xm~Y+0*~d(;LNX
z_zEiuvNR8|9(K6ft0V+95The_rwANRdIhZ`M`%yRCV$<P2)-dYYVn4ndFpFQaK%>F
zl)ainyK8W>$+D*k`T&}u85=ZtQlJu?*QmjZx$y$QZRvIR%414-b_4{+gZ?iCNJ1V+
z^7g96m>0qT=<)-rPB2jr-4KP@cqh3!m|6#EmT)Qi>0ie9JBtM4C#+KVcJH)%MJ@A6
zMJV3Zv5MBaG_(V5Bzdy}*vs`j!t!?5ltJ_Zo~7fGdSJ$QYk15WhTIkuF@>~Fu!?_5
z*9uU)GQ-PUrHrsE&0!ZD9gye(q@`57PazKg)!SxO#;x9<TSl@ndOJJ7UXB+$b8VS{
z`6_S&!STq(QRbk;h<9D$c^E*0+lR+Eu&_urEe|N3{FJ1Ev?t7$K~vvy8tA2r)NZH<
z<^iI(G<aF3WD{5l_vUONC(Um8+ury8X1Ke(AQLjg^$E`l`<<`9?wjeaG?0BdzEm%f
z&Jx%=e)#+4W{vd9#gx{r<!PL@bkt9rmKiraNb{93Bjq53g@;@l+CWK*3_OKUsvln}
z><yh<s>JI`JkzJT1~&LjJrY!*w9vO_EDT1ON%%9PzG?`tW0_uSW$2qITw}jBTNKNy
z7ntS9J8&ce!eK>3s6|PBW*rW@&;_QL0!ws~<n3%ka@q~lCvQZ0^vO+*Biob_BzaTS
zu*5->W(?)lai(q415!lnukXj1N0th>Ho76bNfPX?qxUSqnj6P^mc)Dw%Tfn1w$!GS
z!ShT%N8@Cy`R)9LY7qT7);<DAfZMp`8$Up6Jd3{$VVPxoa<UJjH#xhYwI?S11kTh#
zj{@bMTk8JA&DAn}bwJvrOhRUI@Kajr<JMjHwJWreUI>`OwGefi0!Wyk((k@-gmP%F
z);qRq)3`M@!N>N3xmMP}l?3O*Z(Az-koyLAm(D{f^PHoB>#YTlL=R>$CRO9_R<$?Z
zhcB=uA=v)x9KY+7T**Y<jXOSm@_EuCSkXGlxU}NdKFg&0_wPQN;xF1l%9ITWc_{*h
z{`joUwW&T&1SGy-N@k1P2j}NTejDevMHO6ReWreUwX}-{T%lB;^#I=~VrYm1UBHOA
zdJL(r30|1ASV3%;J8dC0$u0H~>y0XapwM-_ZlwPlw5Lea+oSvGo_pPt6_J7l=O<xD
zHY3|LBrw++7OAh>HsPrjC;x|4)?HA^54x;)3kNAsa-;Iu2;Y^OXshX#`bb1Ly3D54
zZc!(FPzA%SJ`zhk3vh8iqw3<+E^HeQenXg0YA<cSeI$%1vsm)g_aT%1jJG_KCI169
zHexAdI^&|AxLRb%^_OV}ne)K)>KV#)pTi-YoUH!y43I%kc{um+%F9d%!mHondPxfu
zeRiC9c#FV8kPz^4RV|#LL1*yj21Osc$OFa@qu86bgj-gf0QRyF``U>V5y~Qfna1mp
z*<1Ol&hY%jVJ%bN%D^E#P3Yna$^>21knn08gNF{@%nNC)_StMV<xO3(F7%aXt6jyo
zv72QDVu>(5in4m2ld{8Po(Hr0=#&rQ+HX}8`^?=3G1hk9T5>6U6<+wE$VF3}jXM7Z
zifOg%=8Y8Z)Th6w?lK=JWCSza8opC!>k9jwaVV4ZlFDKr$7scc4j(Va&mo+kLy>W?
z7a?8wNB9bbNVbI!AS1*%$*vob)68-#Y{13j)u8T?qGyCz!-U&&mt>r-;!vWN2zyT{
z^;ju!m{Pb7$5-%8Mz<IZimJJ43Xi*A=y2@>wUcU9=RiiUy=Q|oi`7wP){>Yp#hmtC
zx``ZR5TZbRdG>hH@g;)3Vl%yJeJ!aOxGfFoGOH#>`S^lmK#1OX@_C&pO^%>Py6vnh
zP)w0|gdnLOK;`tz)hRPeFwF^2&Hp4n9rE-hmB+30(OxYpZ<T7jpefOWu1C&TphjwG
zU5PsAmh<cPC3BDxtI>_GX>i~ki1lN-P9BOk7^K=ywwqqIfaaD3m+!E!6(_FQAD}j)
zd@8>n66IQXFW4~_r!;*&!W5;<U+#~8JYVtAbS|;{fZt<dT*+f~*)mweONznDjHs!_
zv(s>2+QDU0{Bc{(ciU_zdDfEE!H3sMn`=483jD8l1{4$8%us)g2BBFPq+~oyi9^T7
zueh|w%=r6}+_3mP7zT+pHT)zn>>nKT-kLr9)%xa8kSfc#uC<`TJBh`D2W5%gS1p;a
zcs;Qw&Zdj39*~&Q3%e~NBp9&nsEfAnfDR>CMcU4Pb=Ch~Q&R~#&Tr+k7++(Rxs^|Y
zvq)*0U>k^Ta_;wN2xeIRP9`)iF`Xk9Ze~P&4e{Bl%5kBDnxMER0(1DAP1pp>-1AQS
z@<w<BPVV&HgEiS@`eocVa-L~p4w;X?=!V#(84V@G@j=HFHt7aA#cMU^RlU9l<o_pR
zsBLJclNc9O0*b5!3Ni(A;6}=t$2SaB`Ka|t+c~7<&OgEcZwMCD1pbtx`Z2ZEbHQ%6
zaK`(wy;l`21&&G}eV+x!!}$m;08@GOM4pm<Kqu>`TX-cU^?;Ymgof+~P25sxcAj85
zKx(9bmmL^~EP=oS&fmeFgZAM#+pRRNo|bBf;4LXEm2S&~g!dFj8;7S>C@5@}m^qGI
z*UyUkByZWX!?Y9|WyBMqdIlAzTpS=@{3-R+nuT^CT){_@g^Z!{Xg~eJD@2i#-iBC~
zCsIcvj!TBOC%_-P@$S%<hPL?68U4?k_QDVg0Yw|}<thJ2o&kNm5PV{Hv-R3B;ZmSP
zM7aA%;^8mNhkbzu7Z=TeQ-1-i6A(3-_TwF93IM80^P}%o06$(U-Qfxdoyt#3Q_(!A
z0+gqJYh%y}DxhUTkN<wlg4#v>ro1*U$HAt@q`^w9SA+EB(KE}UrW?!PR)m)ZBNJIM
zK|Pt&15D8zEXWpeFd3Z4U5AT-4gL?i$4J82FNdU!0njjDM6w{|jD3)c@<teBXS}H!
zX#dNsrNpLU7nnar-6Rvg4p2N=XSXIZ?jg(&Q@Mn46hs910rPQZp>PX``xoQVtYT#Y
znXsQ^2Bp*?_LlJY_jZ1ji8yeOU^WgMQ2B_ND!Mt4hC_M9fHf$d=4N*%Kpk4(6>kkt
zECAv)PDOF^B3>te;cM=HzU#FnPmZR)UCt@5e@h_qUBCS{H?%q2f$0vs{r>7XA~%A}
zwVQ3KBcwcc5E5eN;~aITXN$7L<y{WRj6~#CZsfT82yw*-=Gh21ZW9czMhWE1G~+g{
zD5+C7LtdhBo^~^*1kVm73ifv@O%54vd=badn>C`jKIPvov{-pxG*%WXC3p}BK9RI{
z%0<EAA)6gOdU++{_r5*7QBfP>Xw#Cw?=#u5MIPK_emwp64MMU`w4yY1Qmd-NF9}>*
zK3Vrv3(J|rREI1`n(3`)hydE*$&ptE{;T%P=(=5S21q9eRP_;482Ja9YXlf@vLXU(
zIJ+$ho@W2)U0_OHf^ba_;L*39w`kh<`x;9hM0WMNGl6>z%QKjeq%5WuPY`ej4c{Ub
zBq!ih#s$)Gsu+PhKy`!R_a35%ZTY~@W$yVAOXe`2%e(7$=BfD1r!9WHpBK}o88~SO
zrn;YT=*Wd4fFys8mjncUdkB*tzf4b37Zl|Xgeg>#neKodR`8P{@oD;3GGz_EeEmmM
z-uSzTXMOle^P&%<OuZ0;#HK3{(CliMEtt6X+!F?9Cqp(EN$pkpQCY}pKYzO`<`E60
z=1}Eu$D%0MwDujB6!Ncq&Z#$0!KwuS3Z)5l&u@ZQrkQ@&tXgxFStgmif{n=3fceX;
z@T}LbwKB1%kX&pr(!@LH(%^|}v#Fw+BdA~_U`EvNdP?^=uTMK*hY9=O)CgFPHnF&n
z+snxJY|PV+xB^OwZ%9=z5X=_v>WPaE@yd`EIm4{{qRjjpoB~s_6ih&0`dP%Io8u8|
z?DAxpubOOSz6&B;B^tBz7@P?ChUo2r*V&L9ooV|kh>yl&-gE_qOy{|v1c%f7L&`_3
zgBH`v+!lybva3uJotXS>5VyIhB@0#3{OkZjvFV@A+4!8>@90hx8rO~pN++20XTSUz
zf*ZP&y7olKVvD{tmYd0ke<_gnhd+f0vr^zuT*0&wCa;@cotg-7EiMA|U-wBVH97+U
zoSGM6D7^z_#O<Gw<^}a*=jY(U0xN>CZaH*=c1nBQ&i==hi_ln^J^r?`q_a~Ajz<pR
z#rGO(EyGrxx#_MI$hIr9aBnKC#ArMb9a5?0-YIIOIn$=VNzB>zF<t1{xeT8-S~sL=
zu9^aT<a;?7F_-?1C1An%UaZIr<y934yjySJ)47dH+c)&I0X*R?Q1X$?T?hrtm|CaR
zV{9dgf_g4B&ig<O7%=?hdU(Nh|D=V_K0)J81MX_mca!shdb0%od47Dl$s;WvV*qiF
zGTy>ot?GAVzNLn`*<C7Lo%i5bDVA&pb2d)dgDikQ0AnV)ELc@{g$1m;9#$t-)+H!Z
zdHd2Cp7B6plxyT3#>oeflz?YgGBpx#11j52aF&8DMywU@(-MW$vDC<(rHNPuTJW*{
zdkCCWoGhIXL}*k`@2vb2kT3~)<obG3&a3P+dzjzGV4}k9Qv=jZg3Z`H5$o%1@xg0H
zRzBmrTIf?dcT@PXovf<Dh-}4tT*w%A!;2`1aj#xv?Fz{@u_{JB8<?;cW}Oxl6&?2)
z$X{4wvTx^02%Zouqz*ZPCidmdNNfYlDJ4+o|1%|Vs8VRB7cVR2BOUjx)fA{abrMPd
z4!l-UCQWT#B(cje0o}X0^H|l4+#ZyR<z86PSa3MY=@R<ZiG0t&h}YRQw{tR0<Ra=5
zzpe_(09>)-ZrNbB@N(*Ul$hBI<03Lg>VZQUBX;shXlyd^1BYZ8_#|hL=Lfsy;HG5I
zJHaMVe+!2XT`11h<(}s)k~fSTBWh=r*w6d1!moPp8H*W{SRRXw*>H!0@j-c+1a28|
zYFa<|g&9>9Gc^-xh{fc|OG)-Jv_SALsV~6}u}N<4YgC$AsM!XFE^BUeu_0nNwsU>#
z?tFIn&y;+*!-E0guC_}1`hOGnbV8ciuIoyV?8Qgz{1d&hz;$}*<$Ag~H^%?BckaVE
z+0{8b1;y1M1>hG-Mg{ndOb2n|(a@-WWc?EgV#nPw1xEBQPx!Ae>d0I;!T?4;=!3ys
zBo`FL6u@(3_$!Mz3p%5LmTA;{vX>kFwI?!uoN1kj1>Nj*jZFs&fMT&CDpax98Wp<O
z7+5O<#S{klLfVK3>XS7J6@y4jGh+Zs<9<+6yvet)A(dIeAOonu#OaO7WDWA9s>B!J
zuY|nC$uQmKBOrGQPbG)w0W2X`Fg%I3;88tcZ34#TXK(n!E2~2sZcEqt_T&_?q8)q%
z8@j*|ZAUa*vL&Lo{NnlO61Jiy%Cp|N%84CX%*rWtv7I0Y1st-Z=8bHj?i_!6+0yS`
zhw=6RWow3Jiv^=oY0kmy_(Tq;K`=Z=vFE8{NM=jGJMgVHib$K8S8*FP*9Jmeb0xoc
z+KubHxg*%|o#q_NE(eKoWo>8p?v#>R5=k;SKi_m77i>%RR9aR~CH|sPOz&p9UwCsn
zQE>&oP5O;wl~$L2^MRjn_ADzTircaB)*wdE$o^Qvitb2$_#2-s^7PF;vC!yv<uoea
z+KO<PZr%aJ@5YnL+9dy`06IcMS((Y~bZWqb&B9yJD8H)u(j+3#P=Bmd(PV@17|>`a
zBtbZ;K5&<KoRr^wP8Y_pW?x$IzD;kd1O+H3Uqa3ypt5R_9VhJcX-p?TlI;|*o-T9L
z#nPR#+Eo+-WJT~JHSn?;PoWi#eD`VyPKihz^{(u<PT)4eFSIMQ-e*;1yR*9)*dZ2y
z$@UNoAx!I7Zc-K#!O&j&<&v}TWxU)y$Ms(>znsYUP_I_>k;4|c*S_D+z)P}^J#>ow
zIl8&ep2{S~^e~a*Prt6y<-Gj1P9p1Xg7P-m77G&qF3o<LM+@3Qt3NRFBkO-12O@g$
z#k}&$9C&t*j5y$ezrH}*Pi2>BaSvpOWV%`XdMAI8#?w^XUQx$ujB&AR#W;OiQ5Xg-
z#vn2AUnPANMym4MTOSG}fdD!xZ0TFBCUaUQ)0!p4D+wN1$y;QmFhzI;Sl1iDL_!eA
z!8Hl+MXL%X%k*oOJVp2DLd~(6^?KYo+!<0~9tJM3JJBdMGCDi`=PNR7m{#i|v~fV~
zd~v3oKSQWcg3(Au_Q&Iwc{-oM0tSLwm!aWyJq1WYP~3zn?^vZhb>@!Z{q%U!!eXPt
zjW#u`Ii5rM<5@NQAluUr<APo-IQ87gzS%PFEB+~zJ+sZw&cwhDEo8jtWyzD>ILbdP
zc(~=>mXIMY!ZMS{)f_&YQQWq`rta~A&c_6P{LCYMfzxpqaim(MG<s}Mkhm@+9a9et
z@mQY7^z@%enRvA*pjl|7SAvvEB=9R)CWVHGdm&yZqdW8yZc!prk6tcVOoxIIvq4_6
zgLA?RD5_r>YDxXw>H3@i?FTJ6A^M4Q9U2s!Do7T|^b||M_PI{S<4z#qQ5*XU3sNHU
z4nxfh2P@K`0F+BgSuS&K0mu8lS{qDHVtd$LpB$)<I4~lrG+M#IV8kdqb$(Y<&1`dA
zeHniCEc5ZeNAd>B81?HZ0Arb>dxBsBYHV^uE5U$bGXR1Q{K#Y1@$O$k5j}oxkJ8@i
z3Iq}Q{Xvb#_}a=p@2zYcvN`^>fV#u|fO_)6?_IzfBRN82V`*4W0W&hn(TBjVgJa8K
zStDEvbnAWT9%4lpQYaepHUsQi#NfJFZx}QEJd=!+cu9E>rp4mzu6d@F;hpt%OLl=I
zJioZwYMbR-6xc$~+wk>sA)~+=AZ4dr5gPSfNBYpYcpslL#t(KaFRQBoJ@d}zOaqW~
z?Q*ggK?F4_9WR6nlR=Ly2d7UT-87r{;*CAy7n-3QU6DQUEjJq0>_$GFy}vI{69V1P
zMSm@NKf03)lZ|#4U+rls)Xk3aL|>7_XmsSt75s3we3N<_-II5!&LeaFI|-sOKb8`P
zH|gn3tZKbA^&%90L(gS6QKG{=-~0hIW_|jQWfy5#cRD-tNWCrWE5ZwerTz~^{~rU>
z-K-PI3uec~y1VBo=+ST{qxj9n;cBWhsbf3Wl`oS_Xzv-g%V7ixtffe6Ig4@QNr*zB
z!8UYvC;l#N`Wzy2x0j7{<6*4SDghpzNm;G$4XafYgE8Wk;n_sWpsTb`2gsRgJ`7rY
z&9*0|aabn3vBEv~wiOX@v0BZR7c`C9sGWb1ZF*DgX5*lz*x>e7UY=1s(_@@xscDZw
z5G!TOfIOUxffR7V#huC6;h_eUNhn1)y%YXAZN0s4bMp?1H6`n7Qn!;Np6q4IYN<Py
zz}JthJ0bU+UOJ>hK=%#1H0c@;zZSBuRys!r`?0>m|3vVLxMucce*-ia|LNbXr#y+i
z|7SByeC_NqvbV3K@8I;TSNdS#YhU0A@VEc5`Y9g8DE^s3H9d)$YJha>fnwl<YbzU_
zF278w0A-P9&<)%{QRzY#APhq%-QG+BU#(=Xz+3Rbu<{HqS{C@DL*7ghc206EL-=Vx
zm|YQi(!mFfLLoRI1WZ1$3uhL?!qUl*hH}PTI0GN6LBKdvqEvGFaQ%h&)D}gF68`2o
zi<{i;2ljZ`6i*}|`=|{L@MK;vU4@fiZZyDxI-Z;ddvoIEeazRh&8Z5kyqf}QgN<6N
z)~$kDtEa>ztuTUyX+jC^$f#;vt3hjZUu)Cj>xov;JL5H0oq(-;Xfj`gWpwKtJo}9@
zDK*KO?Mlq9EVf%~$EiA&XO}nn+T@g(xRkQ7cDs?9upxm)<0j*A4&%-xK5wy<zLwRB
znl~m}a*HA--zbmVvUj!Z{d3nfC@bahs`XJPZmUy|I@mVP0^+i2vDEiyd-Vpx<2QB=
z98myU<>rG@MKx$7p9&1@Fk9o66Ird-Zt=!DVf#%kF#*O>iZ=LQ2#BOwh^30Gy_ZW<
z5@n0Zw3Z=Q6ju2@C>Q}CqfY=Cf1#jsMC40+{O-7e49<UxJP+`xQxB^Zt&6-`xtrkd
zqN84NzaQ5r<?$MjOm>*<AjX%Ag;?&HXLVQVO<v%n6ZlElAq~Gf>ZOr&;IO%G)BiN*
zSXc*Vo`HqcfY7}Ux-;-bHQv{EIKE?Xin}(GpL~w$z;B4DvkN$iaS`ViDj1vn_jkud
zS3m5$lTQ6mdONiCBbK3XC_tAtjQR6uMoRGBMes@^P2Y(G1<UZshRN~obiDt`Me~`#
z(dDLuTD+cXTnzZPOZ5@2Kt`71?}vd|HWz%|pV42=Ci`q#03z+b`_kKg;6Lffd4C@w
z(lvkZQ@=wmKtLET7#$X@1i;GNoN7~@D=S8GN@<2nYW>)xte!(K4ma>-Gjc+A!t)j_
z>`YQ*C52f&&47*45vFz{go^KF%zI*Tcu`&5B~j*a{tSeercS<H1)8PE8lGf_6UBj@
z!I&CpA?rRUYO)WqaB3x@IRq^&st0eS8T04+LgQ+**lCvJ>Wkx><g>akmK+{<QFo4K
z&zf`OS?!ThT2k#mw-@NKgO3(wg8ig#T<40@w6%A}cZ4w5q1z*P_wo3i2`6VojVB%v
z*$O@!XFt2=aPTL30(T8*k_k!o3UQXbAujy!4a}*6-(7z%TnEV_=1!P3(7)$}^6KUZ
z)g2-9#Dk_1%*1XWW%JXk0?-cgbVW$X;;btc%b#`Jky5?oGag|JX2}6U_*Z#~S9mlF
z@F&Hfg5c*kt>tQeonl?qmTrC+#xU6{;7s&+pB(I9a+~fh61r*5w!48r_#jL+@XG0A
zdRVgCfsKFq)AgxPv;&esTV%rv6US~T^#q|R>q%&xU!#YlYBfMM4ed-o8^0-%Kc{bk
z>gxe?Z>j*g!=-}e8?PKWz;|{K?Ogl~*2P31yq_FDF9RI!R$hrfGHpR~4)npIqs2>2
z?HcCD!Z}cy2BX1O?cDXhF1>f6M2=`}?_U0ci*Mb}{=)?a(Nb<oK71HxI>hJ4ed&KN
z*1~%6QoHQBr+N2|USQpnc7!;v>3kQLlQg;3t&!$GQ}SRrZDN4)$iKm!KeqY7TDqMj
z1kq}T|Gp-HUbUrdBKI&pndBV8-0n1d@9-!<7dP}CRt&yGCrH@{T%!pu<9D*`npcwq
zz)dKRvc%H)8((0h6^8M!e1v%d?n)Uehc@V@)oV3HXq7JdgD)g?ht$r3+3miaHIWOG
ziJPX#z<zu-=F!S|O^O3L;>|ssb<lo8wu?ug%4;IiBF%KVhgL*L&SzNmBwj+aOb1yj
zRv;0LGhyaP92cvufQ@QTQC4Ehr=J(-9)Mu?qJ)Bw-MKI5<EEW-(IHB@s&dL)?`unv
zJ!clYvOC0JA>>$^|E*ZT17zuWZY0<PH@Fp(7i|OPOa{na<>BQt`EaA#jKY^V*f+ui
z@f4$529W|(D`k9R7c6CBO)AZ5SIO0R6H%W}CN4c7S4lQz#<)nhj!E^{$IMk^5(8s8
zHL>)kqz4q*G~gnyB0e|)3YrWE(krSQy9uZYO8OePSgdAE6VvXQoc<6k24qw?W`=0N
zVcW#cJb`7)w9!dtTRJR-xDYKWR0c)IBK-tEL?pFxT7U&p=CPc0#{7+RwM+0NcViM@
z$A<DZGyHVg0cl2&tBa6yZTLT3d$Dc?0Y5NwU3ikhT)mw2vWX|+Y6$(i^aunmoH}#-
z-=0S|@73#VZ$D;TXg&o03qAV!{f~NY-`L8}U)_&0zyB(I%ui2-d?i14E@*~^eY?7=
z!TnG3MEx5u_~nqAe(+WKIhn?Z=keoVTD&j=yhXhaJk-2#9Xd20U`sP1Jw+gyl{7S;
ziC7X(KqTruAno5cA^b$FAr1L>KM0F6NI;edW8?zjL=ke@WRgY%WUs(v4L=o5e4%1M
z!UO>H?w3*lERS1vjl@-34Ga#)W)~-+le4?X3*h>$!ar?db<bJss|3V))~j(uMOL1H
zK|v>rs6qR4L7_T}KtbWjts?}>xek>TBx&o`lVdw0NqF`C)_$e=78xHrJKNh1xirXZ
zi(oC&bc8=YYXXejC?AOumw7^MyAn+zvTZSc+<mZYWhQPmHKe*-!|*}IK*!}SnrKu`
zrF)3Vvwh1jxfO?PbAZ;4?B@E?+XB4(uA+OS{jvundZ|N#!`^mWiI!(oMK>xAetw=I
zj!-=BO;lxDr}eY>VZjvc!ko$y{UV=1_c76w9~bMjYh?UH<VZ}ejM(U>@JVr3+#{Wo
zPY-(X8BXq&Cyi0_D{P|U*M6<)#~E3&m!Bk8Sqix*!ugTHIRzE|aUaw759lVI`i+a@
zvL(g^Ki=au!o9;uVfab#+&KRU&PV)v^vh<hNk}<@>jS7$w(vKThMa~7FfkX+Z#9GE
z_Ef{kU&f+w&k_n~D}=|zt(Fmpm$%CNHEA@~m1c&*Hap%LGPZQy7D&zb&hywSR&_S^
zo1Ly3r4h#n6nTZ6*o<R>{e*(-xB9dM&dCR5#_oUq^+5m8ClY_+$`y$@v!)(+;+c4K
z!Si_-f5~%r^&$Kx#pjPv{nS$%e=-_0DI?#F9lZbZ?n~R(zkeTJ{NbBahzx?9^?7=w
zf%(-OB_tIZ!d~VR)+h}OP(qao<seuQ13Xz&sPgq3$nBEXZkW}h%Bm`dP+)1|`&>EW
zlLl(BJU*C)Y(zv}q;$Il{0PaTH-rguVzCEvh@lKb(OdE?lZBqTJa5yjw@_vGAXbf_
zSg|;(`YXS8oy=qa$&dR-xkDGW4YD{P)<llHXfEOU{HUiY!_Pe<50k%k$)?Bkw<j3B
zb;on6_jh*lei(L3gF(VEb~$Ud0qz0w&(AR`G+kci)UF}2XP7n4mw8-}wE71o`ae5I
z%65El(2(oRqoRCy8sSKoDFy(Sq1Znp!H1rwtE&ct*o$flH&o!|T5;Fc98LI-RO;H$
zusOquap8uedE?Pzqqh`jpUOcOIiW1mPt5u7lu3UqRo&|(OE;xZo<_Y>;}wU?y#V>z
z7J@HyZNkVGLEq4sE4S}FH5E}n`KPy|8l+$N?Lm+NWJKo*BmIT}*sh*+3=8P`aA$x-
zgc1OAZR4pbF3CbKmi6>_FRM?Xn)$kjIjCM0K>q@y>_(oA`!$S0kx|~Mh`3A0hoZ2%
z-*V25pTMa7k;UChcdzs8YE%FGRs3G2mhtGYyMT~LR5Y!v@!i()%eH(c|EHc+qWGol
z8}o6`FS^!D5WS1K#N~?Advm|8*eDY({T4T(r6<1Gh)`SC#RX!=C#!5D`}_U#`C<Ta
zN`RKyZHX{s7gR<)m{mx57{(IQpp8dI@PV@l4lQQsDXYT^Sp#AL@Vys9&~X=3jchhg
z7(!Ph>}Tm~78NPzS-kMiSVk;J2cD#h6P1G^ld+>uhDp7v*S49hIY2K!JQ3_n{Z0V*
zr<Y%o#JI@MVK2}F`;cd`1|cZXojWV7chkVo36z;s<8H6*?enL;$`KXkUweMwv^F^m
zq66bTA;eA2Fuv}HkJ(rkIg0}~{#tQqI#bt9wzIbfo5F#wD+i604hJaX!Ub%YQz{;O
zn@C<5W$!VH`w;1{xM8eMLcss8FwxP)k~&WkoLIOh;{ghpT@gQVN@|o&aDtT(A5U7(
z<|$ZUQXd&>Pzh@WC7FxmOl&RerNDgU^HDCDc}P8bs^sgt=Bq!+leuu_ZUf^LP<6^G
z6xr9+X)$-Ih>86ZhYhhdn%V&|I4zjG4q(&|Xr25?zW(8fW5toZdXDK%W;^oOf1`LV
zxOhPN#F#FVHZInE=Zn1->tgaaN;X~JdQ1qI8ixlNuL3%xrZ4n`2%bf@3YSQ1Q4yp{
z<-{d<KEqOmGb`lXs-TFF)B=m+*4>KWz)M<xTmrn5!|^5QXx}53Fs*7^QXumB0*U-S
zHU0Z#;`iH4I_3eIZ-e*c=A53s`E)wy`h)Lb{odD$iwAvIAC<q4y(*sk^Y_)ceau;Z
z;Nn-ZtGf~`2imtQ$V<^8hr$$1A_`)q=o6z4j0pWhd1|d)q!NJolEuZW8zg@~uiIi_
z_GeTS#Kb-fL0-Q?Y}s%Cbj{>z{?jR1emFrJIR3BSL>P{h`!C^7fQT8M7SNDSSxiEt
zjQT-Dz=#>9E;rc^T2v&qdMydh(!DQKo|c!Ggq8TB{3r$KCe@~1K~$Hgq=Z|q>&RJY
zvc!>+`&@d&v7U!|#n0>juZmAF(;H1l!h28Tr&iNI<mXA#`}*f{5Gz4eGhncYHi0}^
zT?At!ju9~$#m|h(P%D)`-?w#M@lg!*==yf&0nTJmV(g|-tsp+m_Rb`|mqrq6GvFBB
zYU@1_PMIH!PzJpX-N+0Zjdi$7V^$*2zIM|2E`a}fWRq<xt1ZtQ>m|-{x$FaW#XvKy
zb7@tuJ68YvR6PjEW<8$n#4PznZsUprZOVtx1hP;&l~a_5>-SF_0a(_sOB~nOjDPMY
z$WNcy@{ZVsgqoUUyOrlPUEsF%WHs$dLEkR5w}O%AxN@<nw7fTatG7LCiIZZFRck{F
zqXCKAXqL7rT0e`<J=;lG>p{(j_~#=2e(yRP>&w91@Z>>oFFg6vX`c+&$Fm{X?&H5x
zEz=3W<h+I<^Dr{yK#dmOZ~<l__LPAas~g3@OYr<RPAf{uTt2Z9-LTHg4w~h%I3)}`
z=p88zg2Tn`e(}SRgQY{_R1*Y>aB95d@7cN{t5ZMxJbq`+vXc?!57Q~girdxB$dV-=
zVRCId)caa=<rwv3nCvJz_-669i&s7Oz>zMSb?LhQq&I2x|0TcWj@kl+dg`AXdR%NS
z&LOTJU1+ON0xoA%kv<%JS~oW?;c7C`x_6xV$+ZatIRnf=hAcL_tiZC>8gHP-*vRtg
zncafBNjiZ|YG*f};7b`hV#tIA_2(lpD6d2Iv^XBEqq1m8m!wcvYdz@7Jviem=Ak&<
zexM=&eu&(+`ftjrO5iU-7)Hd2k%MK7_CVH?Em^tj6CA9;iAi2XVOOA9jSS;5hLbPi
zvV^odXWM2F5<CtqA}0oHF*F`il@YnD?d=mDOOJLM<(ekI=)9eW&M<=v%y~ugN=1I)
z!B%@7j;4=8B1w?xbx9NW7l$1v-s><QTK*kwxDs#|nPQ#T$l=Xb8q4QIpo^2Lm6;TH
zT`d?Zo2!wS63$WZ#rtq;=>-w^JQHIe#(XWW-R8;ay5IAH<Dxi|)`0y_s=R{UY=QC<
z)30bMUf-hT&Tc0+-Afb=zHNezAmC=r9oBA290jDS*;=85W1dUhCU)l*fZu)k{ZJ^L
zwR+;yUys6HkISSuMs=^SJ>CeW3;{7s0MwHDsXF-;+GMj#LigoAm6zShB5O5^q}04w
z3b{ihTgm$DaX2o}fl%0ci=~M~74HIfx5&0$>&Jh+NpDcAOEg3dEi~;%30{+)1W-3?
zF6%~Y^`D;q>#crmu-Ud-|D~0(*UBQzukbZC-L!Rpy4;bsRSBo)N6C|Wc_FT|9do__
zvhh2&@o%(#dVbox(#hleu7O_PjCZiaPMm*h9ux~F6GYZVqYy<Zan12;gWZa>haZWt
z;kP;0b`}aV4LxJqTG@uerf#^eA+O&S**Eu-WG(~g!CUy>>aOO(60FPsCJgS{HK{5C
zm0l@+<Ia)#$7n%qAeAZKvd$I+NGaSxnD;u2i40G;?t~M@EHqs>?C-52Byvs)468VV
zX>)q&+{`~&w?ViBD^qfe@4lR>pYWG9<UDevIf|A}f~QNpph`>QG<S*SiuU#waaW!K
z$t6#Z$j~zO9puulbA934NiBgdoL-T*RKACi7=)L7@`%-4K-~~D8r4n5VzJQwy#z#s
za(VnyLx~oGqad|7N3$rh;o`SiMBpLv4CxrRry^c)p9Rwm5UsT=?asovn95A{ut8tS
zLW}?wGOc!3omzkU89UOCRKYb?^DRPt)JP)7l9;JFqGird1_-;aO5SL5%CLZE!zuNi
zS`wanwcSK!xA+rYG(j;=uv>SY*!N#aP<;PKutzNj=ETBuAb7_^+AdmgpPmBe1(36g
zw9^RS65*u@8>ZqQ=23WdfeeL_TkHasZS_{hl+81po6qb-s%UFiKe`Y6lse4?fl(1M
zEBM@G&t68}u-Ti+#dEtT3oPkEp5Z0ooLHQ;d9#=P2+)vY<nEYS>b-Yqw;TFzaWnSk
zeb=k_*tffv(f9w-u52!6XkCx}1N4c=50Ukzi_x45&Ye14AkD(q{#40$I6|(bkCTC}
z%6#}6JM>EaH;ty*usG9z*=tg<bB2>S<8U*J$ZtI#2@@urr}af7ed46$g{PH)Dd9vl
zk~P?B2&OR5255yQc9H2gLpUf^)SyP8S{&Z$6)QCjyUZ1sPJqL34ceeMQdvnSnKVF#
z`W~No%I`4&pY~wXyq+j}*lXGk`a4?sjFVC4x|Jebthvz=U3dkl&e^r13TEi?RJY*l
zMn#q3dqRn*=xnc*2;%Go@my_}P&^?z+)G_OR3I=lEkb62Wq8l<U_eCUts-&$^qLEz
zdhp!CR;tU$OEib&HEt#}?5*VLctFQZlW=lcXTjz7fj3l_2YDok%L|ZTV{K4Jz;yfl
z#c)Ii&NC36)0i1q(b4)B=fvv3<z#EQbjQoqj`KVcU>A&vbIq~fde9}i&l3v&aqYg)
z%!-s?*vjdxkCGp1Zqs^hkE@166Jsj2aQXRq0wkrP0TuAmzKz7o*YJzrZq#%^N)#s9
za5zO8kMXRP#sY>qCDo+5-MzFMS-C5<>!drNaPrnzg}RGw1^){_?N%{})OfGZgnL%6
zKR79~)I@E)Ib)^PeauP`Oa89=7AyLe`R0T?b^lXpeJa3a>Dk#ZvB>ND$W!A?<Rm<$
z6+8`3Spd)8T3gft6jc8Xm7Wzyy~72H`5CV4)!wKsRa)n#**I-{6V<H30OS6E<@uC`
zd*N_5Zu@6i;Wxa7qLn+0M1J|R^qIZ(rAI?AgLg0H{I~Zn{vJxZ^gnTOJi095E8auQ
zC;1*nx#|;7Z~XJm$LqYhTF5vuZ(DNtKnst)FcANRlmW+HwQ~7W;vgM0mw%}Js5R6A
z0PLq!#ab!XpJmbn7)+qTE3K<?+<G`nd<Rs}Dsnl~1YZYHMHOM>E|CNr!)y062zI`7
zOZy4tua3|n$UlsT(>M!RdIN%ah{3h5k<2$laCA6B#n->1ttQh6Jylt!$%BpXq@MVC
z)*HE5>42MYdVw69n2{asJA4WeB{K?}Fd+B$m{OILjawhdD+eQ)_5%A>XS+P>z9Qzg
z!&}Zv5i3#58MHGll^qei6}Vs0=5^fC36ZEgaw5TqKDMFVh^suz|M8U4HJFeDDl=p@
zWq$J7!1)~iqCLSmBrCR6PFE~HbU4>yFCR<gFSWMj5PBT%&Sj9YXByfkcuTM=x2jxf
zf+8e9U$xTs)Zt))`Bq+|&m+Z^Ab{S&S8%2XryO43$aVPnR$5ANLAl|Efv<hRj8f>8
zT(kW*XTOrP$@=9zbe%WU<ARQbgaB$D`t2hy{1}6Kp$#x#M5N_!+<RiK4ZPwct1Q=h
zyO<ufV9zzZ^JKKq3Pxdgs7+Q=sjE5#P{afDCxD4UPptZf6c?QPQ~?WVgqoL0e@xR=
zBWr!yzjA!Z7seO`m4sgHsQb-0DWP+Rt;%ZbHO7^88@r?jyrlkN$<zZxpDR@?S<a9E
zoZbi7tpCajGAZhwJI9FVm6xh;*w^=l*9BU<&`cWqSF%AKGp(HquslEo$XUUgt%U}>
z-cVU;CbgRRs-Sk!&r7|M@i~6XB<D{&U;-33%x$Pc&PA8VO7q<86PGH7IP?}?5uT*9
zHVhtAhlr5E;P77hm_rn$pJn1OOazMlsP)b&&+6K~1k9EWC4!)*6&ufs_v%scKzgKs
z*ao4yvhaKsFZkDXlhJQoLxHM@?Bpsn`v`>>NFL4Z>@rJCyCNLv$d(f#C?Qq2{Sc#e
z`f?{;3u?f3&nsyp1m<ug{oSw$S<aZ*Q5yBmmm`km24z3;on^(EwFLY6ajaR>UjX8L
zzvbHAkn!v5Mui`fCxXfT6RH2@%Sihxw;K%fu>(Y0L^QWDTMFsSTu>O#&cAr{6FCUo
z1D0<D9ptNgr+8=SJd{9lZNTVt(Z{Aj3yIIr-mE2qRc??I@_5GBNAR=6XK4^|Qg9@+
zpMnqu)LHGH*(y3Ia;MDKKnu7B{Xuos#58+Bi?_Gxmw0nIduPF)3C>uCM8Q+=IYG@U
zbx)wq02}84u-AAofZ1Yq;IVlsFB#Zlhlm+)DjzaeoDaYhA(W|4ojM?DR_8`uY)$Y#
zFf%eEUz1v-34o%O%h)7ISlRZ4D;3+x>Vzt^%yahE+-_K;vtTFen(ap{--7;5W(Vka
zfY18DrSQ;t{j^5&W9cTwV_dW*?DN0OBbx7jwy%~{M_pcON=KdHwQAdb{u+QvFj2fT
z6Zo6=@_H7_cWvpUr;vVwFwWeez<sB0rRi3Tbm)08W+rVP4Fk$SBFGkk+Vz|wHENYX
zxKPrsZE1AoHl+UW)gp0w|50VJ<27)a*a;svEH(yAN^xLMD=K0qPOb9A;e?`RIKvub
zS)4H(Gzu8Eo&X0%tz8f$fZ1wEzGV{mR>3GUiGhXB$6SO`aIwy96n@5kO9!BCZ5lDh
z2y)`*6G`onr7ZW>2|S620Kyf81&mwWCozl{GQJDbM9k_QCTRE|zS)SK`vASt{S+Xa
z%`T=jT4+e2*I`MUd=}97q7jeBhTs^zN-c3rQ22K>Xf!hWzSFToD{C{(0ppynVGMSf
zlAmsq*j8~oE-b%I%=xqCg$NH}41K5M{5fjo$MDR3<8NwA4_$4uzcG$9%{(DEJSKmN
z$Fh5z``d&sv-%VByf=M3d`upDI=WFwC#tCo>cptYOZJ6qDYsKA&M7Nq2NBcQZ164#
z*2!#{V>3&|pY!*jLS_aBRkI=u-*n5Kts6xXSIzyPE0soN&hD%CPNA(Gk?nPH2mZQ_
zO%2JsEmb0QYJ|AwU6D;S$kcAPxLcEzX85`UbW+(fLoVaeny&5=_j+}|icbHDF35|g
z7`^@2r$&Pi2k8bzBu-|doblS}8LhxfYe2nJEWVygBT1p2%Vv?7>SGgE<Cj!2Kr9Yv
zCMb1>ux}hl?kjAT2DGxgN0?5z#=?(pH-7ka#j@_;(^8ymO3!)bREEaMz7BX)X8Jy$
z-}1TR;J2)D@7Iy*D~$)y84Bj4@jIG>lhg+If_X&d^}}Di4xuFn_k(n~e?$0H^cWM5
zuKsxWkFQQDMuz?$^jR=jx&{<0y-zL>0ZU_3`yvEE0>}^uHOvRVi+bO$T6XJXUNAKZ
zdIGJgm;&2@gq;%KDhh*k`k1@!h%~>@s4DdY+wb2ots@yfa!r*iVv-iOz3{`N8Oq}x
zGTbl#C%OP(1;~jA!vvfd8eChXinJ4(u$QIuLbGN>a-|uMWCxR5kL)3@45z!lV6`}8
zl;?3ixG&$7eKs&|@x?s_mB1SgA6+A(^V-|NODQ=_+WO6lef{+WRTU-3;VDmmmKIFp
z842ee7G^eN5O}D#2L*LF96~A+S_~Y<@(!RLBhLG*_n;jD3zC^2e#e#?k=Zqc$KL*B
zCav+dbZaROGegKcj837mF2?hIGr*qm;*)v032L8@ah2-rd8=HOme-mjSBeGWS7%dp
zoh`9NguQVi!QasVJ7WGO?_qo6I==8DbU2pU>rHAc__kPaEi~tgQi8^pfX2jz*$zro
z=fb$#SxjS7KX`PFHp0ur-Ycn^k?EHqXW?7%8R<d)hpP8}XZsKT_Fo8MN9<JzvG?9;
zi@j&<+Iy6u+D3>~En?Pc?7gW{)E2Y0QdO(1(M3_!YVW*1-_Q3xj{7gjPdTpF<9a@>
z^VIv40z(ub=*u(jB99o_>P7iG%W`C_ZdQy0irrDGUf`iu@Jncb+kgR1aw|iy(I8v`
z+>kI*B3qBKfRj+j3#ML<-{$3t6;#VIo;dw`W9dyJ#OBIYkpmEs^Jr}4Vjf7Lym)rJ
zb-%0Z+du)6LqoevyxazP*_6(8)@S;-t_8@zr*96Qb?-m${rci2D;u$h%yrv$6B7G7
zUS2n@zF|j&X*IWrz;JsYF^!l(D<y`CVMeS=v@a2VU`Dg<+?oT~Z>s?<=7CjjMxRh3
zum@=o%Q~iy>y-?2#)NHlQ9uvjs+Ed$zh;H9!n!guR8fP-)+-`44D<1^2o}PSyA6<1
z<RnZ$DLC=4>6bP98f}a2Qi!ElxUoi}5nl)4!!t4e2dT8NkZMUtVyP$gOX+$?1wZ0g
z*44PzePeV-Ff^Dx`;AbkVU&mnSX<h?#^bAd_}>X*>x4Wk0@HDCW8CXda!lH=4xnJ6
z_)cAtkzQ}s^h2@4cwZ{}(mZoFP+qG#Ner(uNC;UB|9{9D+A)8jC`|}iWNu}(#f)5}
zj6>R?=5dacrw>P98En*PnmhyuNb96(Ig40=?uW(0oEvbNoT*O`gT#SI!CXWB9|R&K
zGZ!XVAY1^EJHZ#eEU-<PPCv^RWPKpxE2>PPkY=}Ya?A)#HOe#a>gxFn?GC}NZm)D#
z=DrTY_DU$c;H@@#fL5-2A;`O0lhmmRl5}!{=o9e9I+^ylIYhYt#h$|pMDQ$WrB-z>
zgj7Aozs4GuYfjo+Qwjvi6Tfg-1dBcKgonL>X;FU1PQtY(N(Bhjk#M(07(y`aH(Dg7
z_3mcw`6~96idu?*Klm^9QsuhmeHz`zgqVJatLqb{FA1-X9@5pk{vvp8{ns_ESmx-j
z+91gVpfX5uiq;5KnfpB;dHb47BgE+H;--1gdnzf3#2Xk*3CijRBa=ER0B(Vkg;o=P
zYZg3aAZkK0)Y?bSI3AWm!=jg!ZpNi$(RddEw<Hb)a5yMo9r&aGAYiZzDM?}N9$=Cw
zLD4!P98R=J42NB$5CWimd6*duogS&_1JPVb6Kp+&3@n9nPtN!Us2I&GeabqDu*xL5
z$se(oGhFY}L5rQo+du)c97h`6l2oB~Y~G}h2R46F2whiDCzOdTJQN!2gFWdtB*ZP@
zBaDq3sbjxH{ZlJrVQ+S$Td<_qOF$AJNld_-3;XZ^5#<){ur5O;FMxc^9E-R0s-lK1
zF{NRQZD$|fd>i$Gh|d5i*RP3!GEEY!IHXI`unx?tpm&Rw{dXtjH*=%QB36d;HJa<0
zd|(A2aNRai28FhH!T5{#bpcaBf=0JA3-LObFTq#!iK%$WuED$hAeUOcenJ}arwVai
zJ^ysm<!(4l{w-f`LF{&|kZRoKuem!3FoTWYeHXv}kF6+nUe7szhR6Ro?)+*Ir_Jsi
z^|ZFu<w3r<p-W}PMv8blMlgWq1x7YSI_HB>Ew6FC98$%;0hcz#)PiD%j-!23dqaBV
z!QR6qU9;++ECocuyKKa#3csRl0iqYLPN}KLwdYee|4>P#2;M;_fd!3TqS)Dm=MHHS
z^f1*;AL5pSoW0+yoWK2~*@4@!q}m|&6;vfG21Vk~k<OPy3FpBakLrHiFl7FyG=~BX
z;dbANtYV{QsE#yRt!C43r?SNVCLV%fv()K54Jh^AGv4|!v4HkDzb`fN67uI+PRPZB
zlT9OPO;_%jOY^!90j-q8yfs&vmQZ$hVC0`y-QRz??n3kh`_Fjs!v7YW=rttWsWl1M
zKU~DG_)N(JL~?J|>V}6QMSnJbPVduo05CI1RpSSZ7BDx884LBuN+4s4+(QB9$zL}S
zwZYj)!D2H5)v8ggJY0$}O7TQ<3Ps9XVXevch*A(+2L)H+d2-Dt=h0MEiUu|dAZrA@
z#S1SpjzQuXn@Tp@`cp%NR1rUbL`xbH84B=TE<I}Mj4Hd3Ze3FH)o4`n>_E0_G8bnG
zhlD0bbhhygl5iUz_ekk@drYn`Bg_LH-xKd@qkW3dx)m^IJ(c@iD}TYrwGpa+Q|@mP
zZyjHB)|8m&DdVUc2TA@Z`z=wBlqA29OpZ=jR&`9FR&|_;r%1T5jzXN)dSF#yphjJa
zcj##i@xD*fYG^Nu#zE0Yl^=~zGc>8#tRhS?AR>1e`my<xql~OHpk&~+9m>*i!HTcK
zQT2rufrLg93ebw#DQbC}%#0osCpkWNPBv=1ar;iFIP()01qyAZAnLGYHTy+#z+Ffs
z$0D0_x(5sXi03Wg;WuyYm4Bq1{<{+PIJZr?>VeTM2Qj&%_Q#pG-Avw<kwuNtZ>rS9
z$bI@nY4;711e(ZD6CRmlmbU*v7ttfxK=S6cgdgW%^o?t7F2DPsjvcR9DKEF<ryf9v
zX<ixPGa6)R(5)C&b@G2sFe<Bfn}PmJYZJ17!0|+8_D*}dw~p`67}{EPA!{r@T(_I@
zS|i*0?XQkd?wmu3ucHAiKUGb*Q#~e&jBNze{&c4?H9Kk!R^8PN5wlh^8{Jx(6}$&c
zv8hv{H<%VwtTV(T?6OWAU9cE9J7alXMq2?!XeGRsLX6U;Y2ipg*HI;RGa8n4qz=)Q
z)@sH9RwwnEFNrDOmek37s~5UPyR;n)`&myEHiDt4Vj+25&0SGfTkn4%!*HHCLIF@K
zNQ@P&cj3=m$dTR1XFmBa59o#5lR`4x5jm|^M7O0sBpNk*ZU~vlz76{FRc&0sBb5;^
zr8s%V^{_->U34kCO>e_Q1NfdIyFOy?;hE{sZyT<u2!s7M#{fY7Kh6gshk3$e$jLNE
zECdyU>B!KOZrcZn>KvfxS371xjB=m&GKLV?r=HJma$@xOmXth8Z;BL}dMsXk7Iy=l
zjR-(eM;s0Jb2K;GZD(?4`64D=unwKiz39+71WF`g%1b42elN5qXEj^6-q9`BUe~t4
z0&TsW50I1_MG+1&n$|_?4bMAJ67!89VRFz2ig-SZk<*!+Rr462E;aRX0_;T@egjeW
zsftZGqj01=YkXCQAEoS_(Q+WBp|Q-+GtQgOONT2Z^^?dT#xZlzPU-^8%zea~lCAk?
zx!?Hi8@QH;CtFf1p(E}|yR6BOyi1CPUB71uX8Q0grcC1RzwXzY@1w5=>GV4uADn5a
zJ_8H9J@|X`o7JCd^-$g|jE3*6pG4{s*3{=ibpfYOUr|$ClcFGflnKvTAHAgE{nn`Z
zYe>?Q^B3<42+ja+9j0gE%MvAF>X)1W?hntWK>*w~l<o!$B}_{K+qR?y;kH$i5tAv1
z95fP(rh^+SI-xO>+W<ZPv?Za2J%vOJ0){d?bYe3w66jOo@R<&2taLeKv<xcktjr%V
zlK!EL*5UK3%8n}vO@LvSPKzs5sPz9%uRDVUotB0HbyL~ML}f8cXb(Uh{uNua!ei)F
zSxrLhURkYW7*yG5c#7?A<T0YH8f2%Fs(vyE#I?$k<Y3>}T&LddJz0sutXVdd5DFvH
z4Dw}>yL8?)a>ER+irHMPiH=K5c^D%Z&IwtgTE2yd%_z*+-7*C&dL^exlVO1CYdd?K
zs}1S(gL!6liU)98vDm@4NU!_B+UYd+gWSJA;lbRxXzU4exg2{N=uB<go&iIlS?+k|
z2Yraj(4T&s`{AiSsJTjb<)#<hxL&9L_K4VXt(Inr{avGE+M9$%hhRrs`7f8u9bX(-
zQodiWBjxKd2V&^6Bc!%*;sN6@c)5&!Od8E$h|zoT5d}yR<ZQeGPYE8M{^y^XZY9<q
z_C^CWOH)l6z2c9CkFYIBo=ddk9TAez*Ns%r(KcURcC^$1FK@T>EsyS^4i%hIFP!?6
zqR}^1L}xL;zj>%%<cmLDsk|WX|4@uW-umG6;vLJH$sWJI1c-`4lYBPR_hALH=7806
zX^M|)TOmc%cJqyNY(w84HnI0ff?R1vzgY`>AAiLexD&UvDaEACYL@@``ro79^nVOQ
zy)NUPy-GNJcw?OXuUR4Yz1L!_u*&ch3j{@6Mj`PlMc{n?-qpiIE;b&4=I2?%;2<FY
z;N7JlXTW-y4iX_m&!&p7yI#h1lL}R`txp$s4~*-cuvP6(Qx}z`V*J9PC?d|{6y;a|
zlAfm{nD%X1h8UovzbVK}tYxj02TPMh3!Z{2k+31lEF>UoJK)DeG8F_8tbYL?txG|;
z0F(sIM>%5yn5P+*Hi#C10R-`BUsxsmPx=el(A+bRm^q>S##FL4dw1^JBDzt-3G}@7
z9gK=$X8;l#!Lx7q#qrTlY;&~<HDVv<JUxnO(cREy=^b!Ol&7AwG!p)B3+;L$_+hG@
z9+v8vhfRbNUTHX+)|Oww!yWErru+r?RmZc9Tr8wM#^F<}__C*mvRlY3|9%`@2Z+3R
zA=QMiNF*EdHFUagLeGAEQYb{Ms(LoB-FRBWx=0r?*j;YFY=k(4)7U4Pp3duU9M(#T
zrJxlCfpw!IAAlh-=XrbkI{}WO;8~Mm1v|8<KpQ!3tjEN>P<0#6i_W0s9MZY`2?qyi
zg~Ny@QRKn^XHQOqtSXu~>R>KK<ym=i(j5zrP&sGK$_mP$UNfkS5{jf80QM8mOQD+V
zo$~;V!@B~O`>IDnveBMGyP0K)1vdiGDqV?3jrjxu^wW2-E$GhGiLP7Evj4_^%X^{G
zNkqb7bp5c_gXpH!3Lm%Jy{1fCqBVu*CNM1YK}OiWHG$X8u*fGZ&!xeTm*Dg5pbXtr
z$Kmx_IxqT?)D~u2JnmA;2k*(5Fv=GwHJTxquk*xsTAi8i*uM~|<={*)#VG;yFLdv9
z;Ws~G5xu-ehyU{qZUKd1i*mGr;|!`8OZjznnIe%0A6S~WQ0)hpfyV-+Wp}VFKyIh+
z$+=zHDf~tE?KB4f3WiG!0{Sq(_mHv;QPJhrQtl^(RL#RqGkFyEQNSy;{jve#b%Q>(
z2s^Zc6P2{VS}}2As8j9J0Cm$TxwSyajl&#I9)A=#J|%R;8-TvSOs(6999VZ$L7O$D
zq;K(6?GNyto}*`|Z6w=l88T*qt%lEc0`g)~i$4FlBO|K(lD|W^LCJWRa4E*DNvOv&
z(~zV81NM7DI4ICbxYkRAA))A=P$6kQDhr%G9KI3Gdd95Dmt*LbRc9?NM3ZOaZCR&U
ztq;y0;g?3miQlCv9N`St7tLxxBq&v;uk)57-(uAePttVqm_XDzu-s6<n?<78Xm^kz
zZs4Iz#w7pu4QrkapiWb0-pJV!y!qc&ah<mwd0syx$tp^HgB(nysbg+&htYr(y4ujj
zS*fhVk-N5CwmC?g3@Y5%%&X&1Hc(6*9#e|u4+kruAGwuXie7*r8$&MA`bmrhKRQFB
zofL1&Qh?b8w$n`&iLMb347j6R;^@y2$3x2ZgD156K}RTMg20q}WG5jOHt?V_du4n0
z>V9iRG||5wpB})Ux%w>aZVw#%xcZ?sFARP#_<DL9tsxk2d94;G%Kl3;l!u;d=vapz
zgA#O-|EB}#_yX#a{KLe6n;&Ih9P_<(%&a3>sZTjhuj+yxGw-`arbS<=`fp-kS(_6D
z(V-p2;{%n^0MeceO=!kL(D8zzH3Y~9K}myJ+yE0#4}xAw%m~Vk14fpklXAvLks^6q
zcHC>3sUK)Oy7forAld{5sz=i=ZLq0U0hTOUiZZj1L`h#Y_5_QX_P<z5q^>YvQ3FN#
zRASnZ_LWJ}NVnE%9;AO|(kU{^Cin;vw}MyNsIJUe1Xtj4Ex~Sx++B!!Ws(;1HHxa0
z$QG7c;_|IJB9`?Ow~D5+hvijCVjWj!+r0Uh@03Zt?uh%X{#t?Yoq%GBDmKRvsoRG(
z6byVHv-uNq1epA2I=}Z`VV6aD_6$lB%e!=cBk36})*uo0C<WNU6`+U+ZwFf(L1lDe
ztJv*}^emXF?T2ctf#RxW!s}YEEx<*|ktX!*4=?;AY_^q1xuJmXM8sup;m4f=cB+(k
zG0MHfR_TtHqK~>n=L#|i&hCUV#Q<gR;*MJHAe_gfX-%zU?=XEsfSu{fI>9s<%I>;H
z*wWhi7-H&X5#hIp7CDaRd4Onuczbp*(xn{JbY!~I9mXuai<bCrz)8&Tu??0nOheI9
zzA(KBX^w&Hfi4vB?FPOtcXiNj9!rB4aB0HtG6NmWft5o7TKf&`F&4Z5d)g7U)orpD
zx9wjlk)G3&*)a8G)D%zjDW>#p@o8TRUF{!jDV7elds%nxG3uXE2W4tJc(=r$=Q2Z%
z_){ma03LJ|c*S!w)YydMk15OhlY7z#!wU!&fs*)e30eF4fV*4~r4tLPm>45kc(kKA
zg9h`;nquuY^m!*m0mLGt&n_*}OlU=>S@y*X21*}uV~SG#=OL8tN+9FUhe{)?E?Y}8
z!p1^>06{3Y3NM`$UYQ4H^@5gC_-DY{geCz}apf7|e2WGcKy8VXn1r<=(7!C|ZA|NJ
z{6J|LiUc(}Ueneb5O*WG1(1dUO!I)<0)DuW7~Gn|GitwJ5diTPJfE~8yZtFEi$I*}
z+LulBlorCaliBvUONU`V^x@TPH&$TNnl7T$*pAArX}jLvCh3PLiEtVr_lOzT6h#Rq
zzu*tn3R(|igEYBSE^ed<f5LvAapMmv@f4*Gj!Q%tn&lk)cqg>;G%isF^oz<$lgfKO
zebO-LE~0u6fXrO)&93(?P!#DYiow!4*F5!|v!qHuQbUF0LJBD=o_x*YF{)&V>mReI
zo`K*ky-|s)bJV}Z#Jjp6@$Cj=E8wtTLA0pvQ~IKLAUIL9nQ8pH=i9drDnzQIGQ(FL
zr|;1|4r{tC>cb9jMg!r}aDCHL>JRA_q!C_j=x#?Tlc{Zi;-&Ifw#AukLByXR@xBZd
zwgqLbisvv~<C`Lx@Svl6OrF5Wt{G*cw%#rABHk@h{0jk|(mNxz(!?#c`b%~$M@(_^
z^ncaP{I8zn`Zvv5Qub$0{cF&HHu<J?4*7h)+>VU9H%oE-hJgMx=ZZvy#8VDAUOf&t
z{uh59HM}lDL)sWe3eRI67_#Qzcky}%n~w%Grwza!T7ikJ!#*4971SxBBCZ$Xz5;Io
z<lT$qpMQV1uKkoBAab7qh^pZ_#8mWtaXRKFTxI6xJO(`IJj<{YtgZ(IGZ^6r%L*l|
zn;~_N`n3TT-P?0l%8q*u=CA}EEDaA*dJ+)@`#EW$UqJY(^|FN1|J7;{ihv!Ry~xax
z3qsZ<PD*?(&JVXJK!HyCyvr|4uxPJsY0C+uXg>kvN|_mD+fzMLlcV?~u*QigMDiC^
zF1BEMRcm#mDm%F<T?r{VDU-}%{mtzMaXX@z)GkH#ai)w6ou{5l|I$&Umz+L$L8we1
zzwd4RFOxf&|97mBk!cdPj&1-4Y2pz4L{QWiAc)t>TVE_gm4TR>JC>}0c}n~_`XA_h
zW$TUUvZoO3203hdJV~>B9S-{h7C)x%6Rp80Tbp8pDTj64NO_SNTFtkWK<DQH8j|3V
zu`j%-T+-~_7hW=6U_c!sLGnE#rJ-lfmjT8D-s;eI#mY5If;OjKHh`NXv7q1+Do3;j
zz)bC=MJYgFT*a_$-s9yiRPvAbHSW)3fSrc*DDr0#kW|TT<u3@W4{r1fES@HvIfU<q
z>ZiB;A1|n<i4CG9^#=k0<@-adP70sct`+0vxSc5`<Oyjl62;a0$*x54(~v5Rj+0}W
z^yJqcCtshM^@E>RJ7$0XP0<~lM&Gn_9shZNQJW{}L*M*1{RJp%75V<Ra$bvuxW8qS
zI$@YD;Vv1Xwc+y*Epuz-tKrZ%K3a_|kO63PzLkhhJv4iQM*VQq?T=nax)x>}l75Rw
zrxt#-#~sFQ{J$kcLRlUJYXqQZ)G3;h#1M(7Aeg#EYY3-bi%h~s;Q!LvHfTcXMT9h!
z?9Ax~1;3~<XhM3Xk%&kFj<Sj#ge@&ns0-wfM7@GdpM36rG2sP4TOi@3#msd>;l(U<
zL#REr)S<$;8gj=ly=o&v>dq!S{CP3E&`@PDo8M5<p3MO~y_oaPRZ_A1fzk<4H(sWT
z`=xj};}?m!G>Y3QQ4k_e*eWp{pQ7al+#v3G5s@OmT+-$@D=548g^z`p#Yf?JAzSdQ
zuhWwBJBIs9cVXEkZ86T4&RYrZ+K(uAH^kl%L@bZc;ApW+;fe&<h3<nrr^yd^^rk&P
z<fGDdp6jakrZOl59v!rcS<h?w77ULN@$xtO(yZR#^0SaqpIh#$*VB(FQ2&OHzU|GA
zd{TrZr$uY$66X-n8`1d+v0Cg4z7%<;Q{7W!z6oo}CHLxJ$A&2w!2@0mK**PW7~pt>
z{en8c!^9`NQY<^3i~91im9#4zF;xRp9!C8kB2VVg)(xdI#ev)8R9dh;E&m}{BmeTq
zS0Y}XajBwqx@p-B+cpbW-)2xB?-wmF6AZUY$qd8&aLom1&_?yRrn#o|?xcWZYS<~I
zQQEk?TJ#rp)BJ}nD&T-F(lN=K*<0f$yBzE~H}p%$;rEAW5HW4~xnG9EJOvK5N?pT_
z^atXc_Vii@mcGAwc5@n}ub8|5SATC-kn~{TW<UMCXBkJQ@26MphwN~uk%<xERjBw|
z+<77Tt)&dY4*&`^$!eW<q_}!7Gm2T!%omCU*xev6zk2#%K_tLDA|OfCiXKApYg+a2
zp>@tR0WcqpfsP_#P7u%@qt#?Qp-ldMR!qwiDsPR(TBbP|3sosWXr;a2=@>mmbb%&W
zyrLBX;=vFf7gm1NZkx<Tu+xFH^0fXr2w#xub>)TS@k~XFUnxVPZeR_hk3Eg6s;jes
zLI(t|LZhAn1pn2z;Pf&><lPfURp&I)@l951xt>JiX`$!vx$D$Pyq-6`fYDhAY+*LF
zbtDG9j|hsUJn_*rpw#qKqRtZ8FU2!;@VPY#8#K4c=0+ZZ*AqGmtf&)YJUr*vvXz57
zMB8M`MZM`eius#STe9VBGLw5|z7dd24Qb=g{5wjDO9WnKI~|*cwnxZp)=>AU=f+z<
zdy!zR=q&F~G92l@V6)quU8ihHoK*2*(sz1I$7!9NcpkyNpK?xj-_Ya?1*CMHCz%C=
z)CJbFA`oJ)Wi6SM1Scu)Aea^*`o@}EnxIG>`Za*e%*dc^Jbj;&pQCQX<Xt<5%O}5g
zs$S9BLZAI}0}Xaxj&z<NZ%@RT>3%4W_<q672|UPnP;FHed!8b)rAasRQ>}pddci_1
zE~CB0M&a6?+n=qzC8~Kf2K;xyny7vyi($>NJ$Y;IPfYG4)ABO=)v=nZ!y>NO2+BeN
zRpmmr8nW=)w3a~k+1t!ZR3Tn%WINO>CXcol_+<c_9mk)ht;189wca=bxf(>XlxDGS
zT#c!$w!{J}+B6fWMDk?A5*UlA6Ve-yJVmgy{Sc{e6OmjJ*T67Ov~DXA3liZD`)S~i
zsX5S8-av92v0G;HA=%w#zq!_pSWYzcmWa@w7qG)zzjg2>!25$2J2Pcn5kPI=Y4|DI
zZumOWj~l#?y?|T({p+T(!tY5>TJ3rEMXz>6!1J@;%Vc&-<h|3879;3wi&gn8k>@5}
zMbeHz9tK0~Tt?OFc9=)QsnQp>O-|7Lgr(yDrmz1yrl_cg2%hX>Wj<1Jt=+Z)f+>au
zP~AHPFN-T7&;}nMzMIWpx@@Cr^V+kyAo*zg$CLGCW?fjgncL%34sX4Ute*X#Nse;8
z!>oQph$DoUoHB8^ej6BNZU<V{JTmRmg{R5W#=ICVgW72f(M||C_8{>ZvtF(G{Q9P^
zbhoJNaOHwxX+vnV-RJYmrcwu%wtWp~tw>&jKjv*^D{hm~ij-*_R*ol3CkQ%uy8~?f
zk@0(d_Kk@C>Q%<q#G2fTYMUYvpa;nJk@gDY@uTSP+Zn0ctiZ32XVKS^B*#_cTPPK^
z-j;Bt^@M2n1F<#UY1nC0earQyyKmqByn4RHEz7<B>2u`YOtTjx#b>WVI*tKYi&0<F
zt5z98b-qpqe2IVrQeM84G-k%UYQDop^$L=G0+eO`*#A>z-tmi4E#!_FSi{1@hrB9`
zVevs_SUKYdfL@xG2<}En`5pxY{wHZAp^>!&F*fSdvlwkasCl5L!Yv8?L=@*^VOYCU
znfMGC5f3vO1Wn7}>Nf_O@X!<@1rbkBeG-wuMz=D*;pW$;64R&D_rwqQ0F1dC(P~yn
zny{W@rPq|6N2Rw`y;r5TzaHUlbgFm2H**9&h8t3XD_}{5_4i8;V2>4GRfc*lwP(7X
zXnC-8;6L@&MdnXRD=$k-VdqzRA7Oc<A#Qdp%MY+7i_5Qk>Q^1XO|whvEHK5EdNvo~
zu3jG>zNoRvBFDv<ZYjr~T+^^kU%62<@>?O}Cwmm>Q5`IQ<eB^X6q~Fd^~lo(&z`U*
zg@mnWX-&6V2jZYiB-0|3|Kv94*sm*yfguf#6f%z6a-h}umEN(Xmnj40nd@u^w_9Zo
z`1w0cS=D9RCwzQ59938q^U)1!MWGk>%#C6fJPmi$AhHA~wZK8mwLArAVXECuOv8cn
z^AV9_TE_{|^ev~{4y5Iq9&sbDvl>_B6yTLVj`S?S3@?6!4!%eO5AONGBxGCrXs-aN
z*uP_W(hhSu6VKPDgkR$x)4HH#Q+SQh1Qm>RepxIfl_Ty;-opjlmr-XCT+3f3s2!`L
zzIkN#JN4tYB}H|>;CJhr6ld3;^^(-2CmwD^X60BAXk>|8j6Xxh`RGR^KRM9!LpZH-
z`=yrYAXk%jJB|ncE*&3?4&;a&k3uFEc4JGE*J!US*50!GuIqk$E7Dgs>{le%I<6N6
z04_rOL<pVXqBr%yCX=&DEl}#>B~coqR7Xm2JaHkc-pj<47#OTo1Q@bl9{cOrkZ3*Q
z9U++zVP+<*9A()uJulmSx+_#Fw8}cwhE3<O6$I%9kX!Hu);&#YBzM!yWiU!kE3rnA
z+G^#7fowq#AC^qufQyLCeP8ZVQ+6FQUizPX!x^C4l|DFCui-@1lh#bSN1h)~>O|En
zFNqEQf>hq(w`jzQpK@?lG%^G9JyFedG{Eh%2k{egqP+$9fk&rj?o^~+(t>AS_6a|6
zymEU|v7gPXfjT2l*)TJT=e~XAZI3{IkdhL}c1Y%iCoR@dS<gred0R;GBQh<%963`p
zL|FjBWlxv0&LM9rz?OJy8RV0Y{Bc77ZLBYoeUdiY-XrOHLA2~Pungu61Za!_1?Gc=
zTMOhOU#^F`K9Sva91#mkyb;i;#W+x8Y+G#7q|wdu-Kc1iI<cdfuE6_+2bCtI40<?f
zrY-)arg%EgR0B{F79MwMtCKx*OPEDA2bvTwdVYO+*wB3%{Xx{7K>XoVF?8n2;&^>8
zq-|HV-uH)}$iQ9Xhwth>l~)EY-^b8#{p3!&YcBLbv!=!7blLxbrUer+noep}oxSvr
zaJWcxq(^Zk!Bgw1=2XX&)U;eUZEe6iSwa`aED|Fjh*&;&c=|MKP?`$76z=r|`g}lR
zY)*4KkpWd_NK$RHM#h6+N(l6zZ6o;2@lFC}2<#;E!15VoqeO?yxmGQff*Q|*g=KD{
zg#xr`%bnB_VIZ`Q@|mVz;0hry)u*Qv8j_-2l7qS&^|Yw_Jr;YazK%=YqI6*6l@rK@
zkg40R%yQbIgI%QNdb3}g2<(2Y<Xe-!nJsu76^x04KWaV|&%O^2Q4)_iHQbW_(cJGv
zzuZT*+MPM66i&gR>Xr|+d~fM>>)Sf@ODt)qUAt*fs<MX2ZE1%^QO-C#7{&I#TfhGv
zvrvHuHiDO$hk%6=C(Iq)3X2z=rZSv>{vf^LW|}-*x?bfXg(v;~zf7auMWLLqJxR>0
z+iRuK=l&l`hkPM=Gdo?1L33OTN&7oXim`Jya##`S^dyNkD57XF|4!ECQCL|)WEvYq
zscw^GY%%?7#jYi{oEN&CL>6f0x>Ksr@HX-$J6QhJc@l+N{W|W#*x2RulZ1`!MPpl2
ztIPx>M$*EGQH3-|leE?R4<j6hp51*-F_KgZ$n|}AsaCyG2zqo=fbLx!Aq>3>_Mv+^
zavVRhd$L^&A(y6^d4GC-4I#I>zf>Gq;an>aPPcVD)?4!F-|d&{gr_U6{?7uFhev-@
zBQhkOeR=;w^Y2UbpRY$Ra2ao}F5zYyjOPOfvY=n;8PmgnW=J2;9}UTfH;Ec}QF6**
z^G@Y+%{Gt}z?=EjhkD@4E(H_p#MAyUO3)}2t>^YXgGP`MjUrGmZXm&kdvHPuLIe5-
zI_YQw7$~0u*>+aNZs-OI;GYJIP>3N<1Cs!Wy|dE9iJ<bo7~`h;=ERm-sn2UI9kKLE
z;)kOLQ37PKp`HR{l%d{TAC*IWYbS@6i@E%UzORiRXzA?28I{2y=0P!;_i%{>;Tbk{
zNW7TUFjwag&ge+bQYI%&X9HXCvPZAXzRdwF!75K3E5hojiEwR=uUwI5gx2O-Tekm4
zCA?S`)CSxYBWIYRo9$Z`#iVfCF{`O75O%R4=FDqbRHQ3F<3ZuLVq@(Prj+MT=YwL2
z+_?)|u?dLkVNvsmT(v>mg7V&tJn`YRXxfpOJ|B(b#Z3^9PadpK5RY;8rTnQ4$WqzF
z_H?w}BWXOpI;PV8h%T%0^MFAn;cZVY-vTf0RwVbU2$9F%^R%^?b6FT%(d_S0Qr03C
z+nHeeS-QJQve6e9mkJQM0eBzR?^xiDA3SP8QI)@`4WJ0z$n#Ua(_JHSWowMNtztR9
zR_@y%Ip$ewBs(WFtF42^{9dG8f!~z`*5EWx97ZaVOa>F41x1t6Xl9zh@57$RT|1tx
zaa~7+OJwl9{xM&p^LU7hn)c7D*J~DYLx&+otGL!;ljY(2$(nEBVfw~DBTLdLCVz%;
z{M$J<d*w%WM=jg+L$ot@%*UU!H39GM+{pg%PVF+$IPW(-&F<R!$2B+5F3|fJPF%Ke
zKkxKEr>BC~r`8QW9zb;(MBcp0_;MZi1Js*EKsv!GD)XLO(h;$ylLdOxD?>bY1D49}
zn}MJdDB?6th8>XED7^**1JY#Vy=guL4~Jk9y^$i6urwqfgw#kS_G_`KgqUQ<$E{Me
zy-sZS^GNuVg0A$>%{a)H?+w~6O>ankIiCo5Adtm5Jh5B1D*a+cBfYhXci-E8>>ul?
ztzoVJxdK$fLJ;4VY<asqaFLXkIw=?qysGe{>3Q^+B|TS)<ArO$M_!42`~I~<s+7uL
zC7y$3Bc7x0CZcf6=mq=8rcM@CM<{Jl+HsuPio-uJF|+-L%M|OLFrwP#C5M%^fI)E{
z?DK4%;fUp|<4cGSe}74)-aT;Dh_kN_q#s3bkJd4Z{9!d1kX=t(EQ}|6Dkf|yXFH%t
z<uoWZzg@+C`q|Yxi!<Kq%5Bi^u~drecs7~l^UT!ZO)uUXFkE2b+%VV&34i9P!$=1)
zck<&nAqxeu4M7N^u}#@}k2%2&#YM>FJBlqQeM=1``^yN<$>j*i!#$O?`ctlZgLl5s
zllR&+=^iw3#(X;YX0fKCzeOWdvA{>Gy05F{;>>#YtZPv<xdv&Dk*N}F`@BS6rdP*K
zS#ABiw7<Ldpzh_EBNJ;TT;}3sC7Wm&T@HF6hW+m<$7jQquB%MX!fe79awFYV`_0P2
z)<;)<JdsZX@=xf2VpTHOJp7Ty@;ZnnHZ?x=O|hoEH~<F&32uOY;YqqFUoGGZy)0HF
z^UnXEycV9w*#i(Ynvz+xBq4a%P+)&e(oU5W%DON~Av#X598EHcC{jel$Zrq&9d1Ms
zeao;uYd4!r{`u2~x+zVp2L8Kn+fyl>H)HJF2*XEhrF>3kC6?PB9nQ@1??<7`FPbqc
zFS2@%MeO$n_X=m8gM_v49En3n<Fp2|O7xS9boE-MlhA$8|1(cs*n3mbB#!{}V1%Ur
zG@Yb^o^TvRe<NY6jPTII$W4JYM6|!SJ^4?x0N|TXP9LC?N$C5y#ERGrBMYA`lJSvU
z@760);GZjWT0c}tOn^%&@)?3z&?_jW`h8|DaLdrKMOqwBB&Z!F`u(0!c^RnNI7lkF
zX@Di7arsE0u<3N{!6VximUXctQT8Xgd##z9EF6U5gLbO}5KAeJPTnW5<(YQm3x{OB
zC)2!H{ZZ;4X|9Kadi;I~;93r!FGBvj3_{U8eZC96Dhg5fy@g=7PrmoH=T=cCM?UzS
zk+O<<pg;}i6H58IuH#?8%;&p1B?EWuCf<F&tE&FWoTUHLL9DwA((KjSA1C*>Uj@JS
zrvA-Kcl7zMn%FY<vW)!r^xsf$IR(ID4y6aHWr%W<4S&~uOQ{_?Oh>D1hP5N<pX_7;
zjo?Q-3Cb=JCo~b5Fh#}%|BEXpqmisIoG=v!F6)#Y20$T1Jo-|d_qYk=2v^>*#lW%~
z1h)hcTY0-v&VMteQ3pBe$RYx_934%(@mdn-ouz79Eiw^A>;hbA&_Lhnamqmd=5gu3
zK>qQ?LbR+9_KSGs=`M5!c4P?-;2*dA%-=kQ4b<fyGYpKg3g}XNJ38I*WDmD|MW<(-
zB5Dd)uQ=<yv{|x##$1+FOXLVJb%otBYQG<cUZe1`#n_w^vsq6rF%iV9zGC?TYqiGD
ztY9ncGy+w5)UR!<=0xsVIDz?!Zo@9dN<vXAtVAAfe=;Nx8kCPW6D?BYmJ<}el1>t=
zm%Vm8#*0TZ;%k8WdC(54@m##Sy*Bk|8S*)w6rZ4nSQ_q-SSOcX)>Jz6Hmg7M*^Uw)
z_@(Ex^>VtvbUPX?=*yFa7W`G7t%;)aSdbI-6>V%1p(Z?Xz?BglWs*XB<hee+JgW&}
z4eh%He!kXk9v{Yhu1^)I_3nLzdqhdo1aCB=uV~XClVohei}0YOefd?VVd=x*Sy=iJ
zWC6t}ewiR!t2z3TUQAC{O0I`#AJc8BW?tl)<Mk`bAudq$pYrB_-OPwNHPx)Ppeo{G
zAhVHIPGj?*@tzu8vMzlD!TR$@jc}AVchmft`{rotQU^qH{#(H6&k{qUg`tv1>u=8_
zyD{U@rQr_SG0w59zisZ?*e#ZYIfn>NVf*}qj9y7hz^1I|mQL?Kq)v664uAH6$8bqn
zjF(6{{}gS^sUG()b<fV!Q%Io(u|tICiG-z5l4iSCn<P0zu}xm9Ky;0jvn`!%XJX<Z
z`D9u|p+Dgyw6rMTplW%l1U(%Y-JC2~>+C7zkXd)9=|f&`neYdD5MO|H_i*!jCGuoM
z>t1u!n*j#P*L=ZBvm-qAJ$Z(raiAMF#-MJ`3$%``7o@^<Fdwyct)(3T)<Ym(tb4nE
ze3M!yzv$V#Xxl^|xF!lcgxUg6b{W-00&f8NZ>~7Ac}-JWi7KC?smI@HGw_Qhj^c;a
zN2$wHZ;w9C)Zb;CvQiN;Z+Am?UKpA@jRwrA2a9OKGc_4Q0glF<`utt$*LV?45l^NH
zNK63ymXRmiM6@gKY3y=ZSX6GiLv?0UvsNVqIACAcEC1t|NJ=&x>~TqFTR8MB7@h&p
z^Z^PGU~{JDsZ1i@fS@Jydrk`Sgnh2z87(&%kA%*<Sy%y;lo!2wsh}I*Hy()VOMahB
z|Hm5wU+V2f9+e!A-INIXswNPcOgs5j@tei5g(K&2snI-c?)u%lu#E1z50J7D-Az@0
z+5=}U#!5zXmbt%U@*ziWOz`J)%bEn$6stE$khug%O&@?1*+QnRUpEwd&$raoPYbEL
ztn|-<SW4^MKQXQ26F5juH?hcIV$`+9lWAE=i9VHifgVJx5NQxcH0f-R0${dVxH}NE
zxuvC6I|UiolU>GNFv+x?dKRh3EIwcS-&MAtyF32zLi>Z)kJ!}gsvd{=l{kzHGeCH<
zchYdg@X_A8ERA>EoT*%1Lv)XHD%hZ<^0p#`np780NnKY%sUlCx%OIA|2`PD^n2meJ
zePWaik4?qp*THL{ejC4fgzA+y!)chgnV#M|v8QAz_~jrzR8QD?amEum2TULTp@;vg
z(#dmCkPT@Pc2nXal#j;Ua0hWwVjLMrSXQ)gxHcvEgy;OBQz_x3No&hI#d?l;uTUTN
zgEjCC@S#wi|MAm`Qs1|Q3jB}PE2@3ZiYGimk(^M%B778Amf{tzPf9%^d~%d19t9Al
z#YKTp`7;a5TY-F)9Qxx>jE(!sJ+8DassU}DpKqL#UV&&d!z2xyu#}wWG#udtCW=Z3
zO+X<1*<^&5qF?Z5aEuK5?(`|Pfb&aUJHx%>5#J807jF=w2foowj~w~_Dhz=Y9q_(S
zm_P@iIFWXgOj94UVkPIE2?RuEL~3x=hG?B^VJ-o>zkN5y7QbC6mln8Yk9_#@N5$pq
z!;a&QzWXY#==2@I*RbZLxhI2kKfYqrGZ}V>2P9&_4+iFqna?%x5g&hReXnPr(3s!k
z|HChN`#051iCrqN=3yo47X>hk7(IpqsL}CYK?Zh0mmnNH?u&@z*7xQ0#Gu-Csd(^U
zL@YpHv}ds)7a1!A4J?cE`iYRj=iDeLE0MU~0h~zySJNp77zw4FtyPQkvdF-)(M$>e
zL(YB_e|A=9p_wndy0V3={rj!z<l!Qj>GUfK{>iK{=K0RUvVuNJpI%cb179&}OwU|6
z^ph??s=RM)P^E1Ke?m-QF(&6Um)~M}3MIm|pL@X;+Kpd2R;}nd+xB$vaJX7G-8XZG
z=P8@r!sS+0V%AZlCC(dQv6RFO`ToW}YuVp-W!~0(kk4E}MryoY&NC+p-;}!~ihB3c
zB+Jd&)N46u3v9CES0-apiQWkRu5sU_K%6F_tQI<Jw*wB~9BxHcBSugZfo~q|&Uo`+
z_FyEEfNYA3ddBTKagOZD!_&CLeHfYTlg5LK8yL|~Jm|95X=(sk3=i+?t~}~JYiJN|
zfBp(Z*(*K*H|F8&ZE_?lM~f*pS0){?iWesy3yPOD9fE_42U}UUuul$CUNQqgf&Q#`
zSUMK`-#slwMhB8tsb3b%#|X$G`5hBGHyP)|x_a~=evb0Sb0-Scd-v`*&&ecP+T@8P
zP!5^Nv(fg){nRD-(T})Eb~e9E^7@>fQ*BP&Lgw%9oA<f>lXdT>i)>ziAI6ZJUx(O{
z+&$si{y;#z={$K^%1OTwvqi1HKca5Oy&CIGjl8(9dmO(z4i<Iwp1R|8t!IRFdM1g2
z#`XA4{<8_=TX>e<<YM@0rtW$gx)Pe7O+;$!H?}JA!EHC?^bTC;O`iagRg+;91WP>w
zG_`V`<xnP=*K;P<=5jW>6Q_p~76%Yrey!#Dd@BTfSkvx}k9HzYosN6l>wP4<xjg<n
zq%y}yvPNlH?j0lt%O;5;y+3X)%O$0@MT%9$Go)B9Bu`so#uUq|OFT`I>Ba6!U{z>w
znjoh(p>N_{`!Dl7-rG|h$&BWh(Nr*orY|%rj9%_H;yqj(!{Q%iYIqFsQpacNFk$4B
z$dC?CDf|*OQ&GgS;{_E`funLy+E%?M?0+Xn@AqJ_LUhDz7SUdfeaVwN>IUl#3NQ?m
zqNALGbdW?1puVnGLsKr?QUc|ZI<*)*ja$SE$_QW!09uc<<wvUR0jVX;c(y`Esu4He
zQMI<`Y>i`t$6C<1aIgUdmp}@VL%NcYGt>5TBK<h7M1$y6W`RC`iW47vj>jxts<eZ5
z-$esYP58PtkASDjhEIrdj7`W!mj>V>t%Y{XyHOWe998AwC5!Wg9NGsFE9DVi4pbCk
zFAj-l?*2GAw#e_j@bzMkS;l6S*W8KTI>txlIObaB7=AsxH&m@R@W?VvGJY-o?AdEk
z%zz}2?k{r&Ybp6vc_+v!cj>!fNYCjC`2}{-(WL=$Ur&Ab9FiC7`fwDYgGk)h$4QBs
zgj168Tb?-J1&__-<|qK74HJsdrriC7o0(l8O-dP6{wIupy%u`~<L!rSAn9QvA$u*<
zRUQIf&x8UyKF2%mB{4G1N<^o9;L!<0oSfW(j8#{Nsj8$%Uc2ODCe$k;$=dfz#q@m;
z+FueiGDLwVwfGZz4@`N45%jh0df+bvO%Oa_@RMHA<A~3n?{?7V?dl6YsSOUJv=*tm
z|Mq1TxBDS!#*fugNi^|)(*q!x{RjI0irDg8bYvq$nJMW|Vc^lYJ8)3RLjy@h@n*Je
zCh1d><Gsw+Rom?(3XHu%1Qm(>sG?`RPhb2|mXHdcb^KvFv(lwk*exVud$w{YSAf}J
z5=5ASSrcuATT!#&Dj*tTT=pBILjO-SCO*vsB<?f1?l;Zmh0DTwG!2H|6jD6@(%)+_
z+P&Za1;64+GH{E3qwx`PBhk<;{!-6M01VdVWN?^K8wtWR$pWM*bDwIGWd<`_MKRYE
ziLbm{nmE1&w4j4v8BZHd{eP~1i<UGwlL!U&PNN`CR27P57Ji&_HY@0dtR<f-G1&d{
zVA@DLd->;uz@xEuN5AfE1#wqQ9OYm3slSJjJbHUJ+`l#VI->DUnMB0L%bRpd|A=1&
zpF#f&8fEVvz(WJUpKi`0n17-my&M$U6xK>aWpgTC;OeG!3c4~BC4>l0ZwV%W16q)H
z0^bg@I(DEz$EBZw2nFPVJm^_rS)2Fa<5CAiJO<%f6tKy*nMy#<qR|I|uhl8z1?bzs
zy87fDM~U%9IljyM<PG{#@CXma$m)(e`-LTSbrUK=gu$^;SXVbt*pvd_E}Zv`YY>)!
zjzKW<I8tIju-0DvUe%s;K_WK|$JfLqaUTCIs}-uxJFqM+z2t#&VkVPjOEiIW2XJ;d
z;Bv7P+S7Mz-11ja_>=)_yvaSA>aO1&3#)h=<KF`p&azi;gs6M$R)uJ26W66NNl7cV
zK?WYy&tqAFR<m_2e4R8%ZG4>MnAY9+pFe&V^5$odS+vXAt>k-Kc4OCp@H`W2F8RdD
zY{Cw!A*gL8+R)>bh;Ik&V-F;cSqi^_vg1e<{uW=d6!?Kcre}&rR7qLKfI(K7!Nz}C
zgCHakJmB{y>|prA0ijx+uLZ&=GIU>aL;Yzpu24%dQ8L9+oCD4l?%x5A#Q~kL!ePpV
zPrrn<o6Fu3LkHo%d6+tGbI9l9O2*!%MoT$RGQDU^f!R*9i#DH4<KihHcjr?Z)L*MN
zL75kirx|SEH*4=36KbfkRQOv>*B8G}nK?TD@`c6$Nvbpv{SX~4s?#sp=805`cbo#H
z7&S^{DSr(9`Dr(=UUE}^MMG^<U{mM2su1+<Us3w^8r{bk%a`xIIqwW^{$V4Hx#61i
zNk=gxb?z#~*~CiK!h@Ry;AX7QovzjC1N2Wldw!1p6lS=Xbgc^EGk7*TeQkK>=Bn<p
z@=c>u&GTwva8ZiCm#9I>Xy)h&(UL<&{5FwNrskVsr#6t61~E-X=Y`hNA;@f>UcYvQ
z-I>ucSw><L)93;5#v>%*8gN4f*yk)}_E*V9OlD2z1>I}0!?OPMaCS^oCM)F!Ln=P)
z+GuiUiq^ihVOVrCmi_)t_=Ou?EVuT`#}}^~EmJAf{4={+*jzOu>;qzj##kFzdE;qQ
zJ}XO!Uw(n`j=H*@5bO#HuUPU=ui(Mn3XOGC%y=teerjei{2Fw4V;#-=+IOifY5Z*p
zfnfL9Syrn2%Ajb5#w(HE0{;;@`MQzAeK7|u$M3d0SvxC4eW;2eS~T(Mk&qB0#dUhL
zyJ?UZf==Cg44xaCI79>fV>ul&tE^gY8LNF3&3iM~#Uv@LeEF#@z1Zb9#%^VYc|D#_
zR0s!qf_b^62Z<x+p8wJ0Bu0nouMA~=qSh+bS>z}2Q0iN=4%>x;!%aOjxFb+4<)4G1
zBy0^;eC)qId3qde^*2>bnfa`wpSA28*P4oeXEIkVN2Lz@ql!Y7fpbUav)Al7u-IG7
zB{mZ9x#9F|Ve6zDU83c+o>wpZHSYUWRB5%d-BX8Z{o<B3e)ziXa(EE<168w#`!*Y1
zHTt0JeR$f#jMAnRECum16H}B?9&t6pu{}jP^)qu*5F~Ab5V%;;D|o;>TCs$p_AP&F
z9EN5|PpWJmAsZ&AtMy{Bnub$4Nr#`ESujKk1yU(vHi<OP{l>x4r(2unSs<!7*M#&i
zPYu(m(Yc)T`@A2PmUSUnDEHu%f%VO<tfN`oh-2Rh-SkvBWjXO&lYq6#xQ?Khcr1r|
zXny^l3<cp0NXj?P+rM6Bwu3Q9-7j~XjE=LikG``1zavBAjSf2#U5bu}d5Whh8<<9i
zR}xtZa51J%l+#}pNAV&sB>U1mPUQ~Ze4~$6N;skq<VB`%3AWKKZmGhlvLATfywJ5`
zc>(J65&zK5nmH~xq2(kA5Y;d&R1?9|3{Y#4$O~$eN=asF3NjSLDKinY;We}|W4+L`
z`q|*6Tl;z1xAWI_r2VL)RKk-dZo7d$;}m?8)5?hO8c`6jL~|4oSmU7zn05FDL(-o;
zeTL92cSC2(l27;F7;yl|qTe<YZ#5NQ<a2dD6(5*#TRr~FkPO;IadXmoG9=Kw`}&h3
zhRzVax^dL2W}3Ly{q8%U@>hI$=ik;@bnN}74{uL`eq7$)di8LX;pDfL+WVUXX2_qL
zAx5W{8fN7@KQ-ut`TwYAQgVa3&16YI8qEzhWXj*ps1vmz>8k&RTM_E-Mry5o(d*1F
z!l6S=7FWf(Ct5r}`G32pGC&vtDg$)xah3t932<E2a#{+oNK7ckVi}*H_3?;o{)tm8
zy$}jI+9iTV0Zd#|I8ZuZusGAJYaEou*i?nRx^Yhz`ShsRl;NI@5-z)zNR2DIJT&U{
z86`NVhOEdsxJyqu^aCzg+eQ$@h=D<jp=eky>=BAcpNG8HU(+y1nMn^I*pB*OxxCp{
zHcxzsSeKh@j1yOx3cYSw;^RV77n$A=@!S2No_4cO!GvP0SNQz@*<V=idD^VrDq)qg
z;q!Blvl3=N5>Tg)gy9#YPg0gwb3M>bc#{l!6J%~&?z=o$XWy;7l7-kZJGs4kHWw9H
z4;2cCp?jO#WCTNfZNZcK?ux-&$v7m>#S?^L=?zRd<!|av6t$!Iu)rY)WoA?0>W!}I
z7(I#HnrRQlaxc88b^bnH&#eNMrYl`lS2v~zuv3<U>skf1+R-NQXh<tAUK)><awimX
zrH(D>^U6fR@V#BuOAx>cEMj9V&-&ap{}9g$>NsNE<*naQ8XrymN*!E1)ZSQnik2ai
zgy&%CKZ)mMO?Zuqy}>3riCu#5l+~`^Djvaaz&b&6gI@(?lY7M8S?@#y+c?{n{p7eR
z9R1GDM;>W#Yy9Mg6sG{yMcjnh%KEjZ(Hm{G9B#<Dvxgm6SM8MP-6dqxVn;XBE@2~j
z3nBDYtyJZD`SkC@hkNSZSyDgf+-T`Nj~QrqXKzdILC+5VJ9}ShB6pUT&lvl8T548$
z|C!W7*T%*d+(oBGA=fe*y0AZPU?84~=87+Is*uewkk9W0Mxgwv6juKL{@q<o051zL
z5`hvWQN?oYY5qA<bdZWSXb}1CE*_0FmV^gGl7{Dzuyr{NXEW%KY0fenltfu&C$F#m
zii?-jHIOM%k_U2_Qk2U>HNaXC4(Z-D^h&A|i=mgp4)PeAxMqn9((@VD?-9R&pJWA&
z*+eq!B?54fRG(%$eRjJY`qV&mVS2@%3~R-=U8$PvBr(5Xm70`FqV)Ehd1MU*O5lvo
za;_jf*8R#+0@GBtrwz@&Ub(<^+c<PgPX4pb`qOmJM9so35AZ70ew~-JUvm4pn<l5B
zzl<e5k%qvCCUa0sHl!}rVMfiPNOa)lZi$_tKoNh0A?&dvAWt9>u^xi`Eqm(altKjD
zWlP|~zTM0phgWT5=mQ^hbzv_w0D-{uphol5Zqkj-f{sXkuf``&J-}XI#9x5s3{d!?
zu1ORF28lDG+3gc|nWGovG@J0#T2~*q1G=L`Ngwu%IjOxP{!>ZLp4n>kj?FA$y-L<U
zPu<YzqEzVoZt~<tZ%yTQE~B~%pM`xjLyzdK-~Bbm>bO3i8uRy1&KmNo(wB)hO}5@i
z$I^B_RnA{At5=hW6Id-@@@cVtPcjm-$YyW(f4F)Nf2#lZ@Be)`ILGQZ_NsFZhhy)V
zaLi-R?5xO0vLZq{IF41v&OY``2oc%iWE7H+Bq1Xut5WGYpYL_uuHWtY{SS`k`}ul4
z9`}3e`QHHT*I3dxjfm6o-wT7drgO*g{p*932m!jsMxL}oiqD$a1;dg1+6!0|l8)&M
zm#~6t0(R~j!|Fk#rDZ0Yj&;y7(Nqknd%LUiR^*krMkrEI6%eZ<=#+hwNjlQNvmV;k
z=5QE<$%zP$j+wAmypT_^>V5T&0<T2Pz@7%nOE){r1bXUsb|&;HP*mr71EWJzW`cv>
ztHPhAt|xR!tn6ioi3A93OLqV8N4spjMS5l5CjH2LU~ST62(ba%&Kmq^dN-)!)y~yt
zs!9bM{LgC^wLwGON6xAI5f?m%k9Q|i((IscJUyAb)l8#y?!iz6Y@3<q0+iHK)&d2$
zUIAxLP=`x&Jybcq$2$~fTH+z^KfZL(;qvH-#En_>4XAT<;YIv&f0%X;7Xz4$N+_@#
z=J2LV$Ab4QmpQ17(g7cvNax&Y;3-xb!Jyd03l4qv^^hVl`ym?87tK^vW@N=u5vIWl
zii8;H_JKu}KQ3o7Iq{#?<=MPAF42u8Aj*DXdKr=U9`uc0uJzE2Tt)927o$8r$1R)j
zui|uni-A92FDRc~V6^*_^L*oGla^@SD}0D5*ID+(cm@zQYCrIm#hpGN{G+Dy(S;YA
zImds_`YfJKyuJIoNBN%y|Dw$AIL%A{Bqip4Y27S1|EIUsl;&R_34RXfeU<g+>A?Lt
zL0ZiRVAx+x3ypJ7GU;6?%~11`0s3K?FKGCH<>7SFpsX<q*8tAe{jjvo<vKg!{thcZ
zZkeXX$JJuksJC<~;FNM;OkG7nXxN2}T8SVG0M~&c$F2LRAa3i*ZWNhp+K)H2?#TuW
zt($=rM3yzPzEK)YQxeFG0^HH_Px+vM$Xa8qv#4|V6K-i90tue3MN)DKQ=>NHd6ON#
z>!M#vK9wamS;OWLgu%3@_|MN=ZN^!GfQU!ON~V2??83CIuJ%MPz+mfQeZ1Y4)b)!1
z+o_wfnXg%l$>i$*BHjUVIV3^7sUZb$Y)`+z?__5);PmEARXu(&{LQso2QI-({Ic?;
zJ0UW;d)Ix*c~?3T>~C!(<hv?+eu%j8H0HUlQ|{keu^;lL_U-{(G5(uw6cIN3D(9n}
zWd~nNVvMUplawpM;ThM{H^flpMBxB=gN#N5jX`S}^AchZ4T(RWC?eq*`EbRtS}E1a
z?~eO4inlrd{}}-y<>cF*QrnO?YI2LeK8j5nu~QFKYrvp7@DjDC-ZXLpA2b_a((G+(
z-*yT%BsUanJ|m|t9VA@cGFQ))@w?{OiM-frIx1Ah&oOZ`(mb+Q>Pvxhzi?!Q;-o^Y
zkS1OcgK@zVq}1U)mo>&=e&}ob{_i$_K8fJg@VinH?|5>j!3^MW<@PzA&w3SjB<K8y
z@rYy!Wi^n~;AHBBy$XK!LsK-J_h(R#5woG-?gx#L^Y$?;k#ZbX48L0LCVh!q@rlj(
zbw0pjqdFMOz4v3rK94iz%8~bO^qEZi+OwZgj)I1{`ZJyjnT;=V;N`()ruuSm;I$&_
za(@$}5ou|!=OF9yT)afSl1SdkKcY^MX!2yPsJ%bgDJt1~<9V&<1_PwnlSn1&lWS*T
zOAu7X<;q94u!J_9>=t@LrC?PYeF~Od>?X4~F^`#}jP+e4gW$EcoLZa@`vUcLnxGaf
z3upbCx(-I}f`Uo<Nwl5^n}l`rlGI4$PHyIV^_I)mVaLt=Le@Xr^u$6)#BTZv+w^9`
zFK!v+nf8_sXgGw>g=YSOGzrB68O(4c3KTvd?DX4xOh*Fd<2vMJ6`dq3d_fMUCAtBk
zJ2xGKS5f@(-eh|<P#8&iI@@pP3`$pEzE{$^?XLqJQ1IE@yfO~=oi@69F$BIGKQWJ=
zrU+{Rbb=LaB+xl0Kd{(zLIMqnaW0P55nyovK$(nCr)@eqy0R9Mw)%u436Pf^<l>sO
zdk28-lvrp3#-10%A$Ou=A8p3+MmiWe|2mH-vkQZ5x?(+SchKJSVNV$7jhF`>1d~By
zakTofRI)wdzAc^)58H(3R3qky_^v_7XCk&EZ*(=Ndg$Qby&Byu38VRooUXfG<?q$M
z=UVjoyZ$bgn2IvrUeN#p`z2G_+BXM|dy`5nOX>8ZDxY&+Y=!-F-7~AZh2EX32{;zN
zvl4mh-X~4RZ9DNdZuJ=S$(!*JOWZAAM0-0OE(Cstc-EiB_EAY2T+~9;8H!W-E4F~U
zbY9~9oqOwW=)#C>KH^!h19aH5a+F+@oRbt@_cLC{LEp!NLnJ}BLnN~sS6&kgNgO^;
z5Wnr~{D~Km<pmcc`QDK)heC*6y+Y19_Hdk{2_mIH6rgMExT<IE^!?ZE^N_Ycp`M7;
z-Gc4DK}Uhh#V=Ccdc=&#J92sjSR#$*ACi8T3fe&-_w+<AWlao)fBFq|^FP2YZ*1Kb
z+Iiyfqxa7vWoYbrjB1~?HLkgHL60Zq71BC9XLVjtTRZN;{~S@a{`V2Z9rPIjU_Z9V
z0`fd81bTPsn}NRy)l7?>#r&~m7s}EF9r)>)ZNRc8ycN_c#l4k^j9s#zlkl5NrE%zB
zLji0C?D{ZoFns{_6)eye#j?F_%>ZT^(1lqEb18x01A6;bdiW_Y>kzv2r_d;3qdrbF
z$RvSn3YIh=BfFcXtj7NQm3!V#vwtkyneMA)6feRIUE8$qoa$w+cQF59sSLG1YksMh
z1NtP(BMv0pIKyfM4<^Zs?&k1$ZtmFewU0#ECefM_rWkzCJ;a0E2XO9DKr>LIR#X-^
zC5AqOnU{(neiF4VYPhcayGtp$jp%v6eIB{~HL^4IE{=9)7q9iV^xUl<V<*RYfBM`W
zta0pV$=n8hX%bCN{`OC#LC&xwQ${>hX~aiRgSo(d_tjOho|MTx*-X@t9TrI(mYI0Y
zFodzCJExf;$;+j0adZWWYzh6rqhnQI`X5b)5kw}7Q@N$S7*=XOlQ|uY$Ta3gBB;cC
z>e#6)7AKQ!bE$ZO^;8+7)}J7Wd-6I=^5iUSkeCTP?IFM}e{bg>TKt)hl-{=7m(G{I
zE<dzT3{FU`I1V%<yqv)ksfS2?2s|q@PT`s)r$3TVM`msTUy#=Z5x(VgPyWc3Y?rl&
z3S#aJcTVHb9C$(2=U$uLWH89tuGa6j)BNFhurg8^d{@paQ{%m?nXmU9Z@Z>jH+`Cv
zo`>U|pj9<4PQ=8==BV)QqHGGpWie<`h2v>R95)rhz37X6>>tM+6M|pbJAQM`g;G`d
z@5<Bk>rv#Vdm9l>%JVGY?k7W~cgtDp*`1$t^7+Ie@=(?nE?hoQO8-QJW0LL1qoPug
z0jQqLILHBH3}E_=%@&bl*Ju}>T=B$Tn>1G~^zqwODXLCkG|v&9Yz8XGDaMd0a*97R
z1K`LluX_y<{?bTJS%Ne*@~T7lB-5N;migeWt5hHr<5etVOa|Bbo1O7@vgDkboQ6dg
zb$Jpu(uklwUtfogEGddm5dN~SNcv!PWT7x#{@!(0ypl_j2%tn1a!Kr<Vh0ju`x!J7
zgH~z75})RORb3rn6X3l`ZVIj3`uO~jFWxyX0Udj6l}2@O-~#Aa`ekk=of~80@Y@ic
z?sI1RP@<A??pMss2GG!%=#};18SbtRcLe`+&yTY8Ri+raKA}6j-C}_!7_haeL2kqB
zcj1>h%YDuxGET$`=Vl8WUZkt^D>mxF%L9x_f?`!UFRQcqfjF^zFKs2oe0DXud${A!
z<gnUrI4`z{dRRiuEGHBqo^ZLI6FW#P<pMO10oWjq7!7=mh?_*QCQfH1Dn5mn1OSY2
zcySb09dxG#ky14*J_6U{OzpJuaSRx-<M_s1-s4|6bt^;lnT|v2u*-Ifnf^ixXHIu>
zij`whH?Hw4*?8I0WG_Khx99nX)3GZsmf(SM)Bf_g@RW^FRTyQk*Wy=j>KD2M(jtg|
zL5#`MVn<zy`f~Nj))r}UC5Z?9o7)~CepME4ZEVdmMl3{#vQUU4t$Z)|yO<;nMzTAU
zU0zmo^94^HSStd+bd`elEz>i5Wl>3e5fGE_xFx`xnXO0v0>&STtAk;?xi;b*02con
zB<n#ey3iewn2lFy1}2y*UjQscogL*!Sn6_PPQu3=9@;-1@1EQ%I`~*@p2^5Kg$5xA
zMJXn&@K1sg$pEXg)U@9z)+L7)(R8`h3I*BJrY%I8hvh$$XEzj7V`RE^;$HaKVYg&1
z|NOdkOt(A8_+^!Jcrf{7G&J^Kap>C`oYjuT(AMt{{>-<xI!bzeZ0euCg+ZOGN<<2s
zoYm8*)x08<*@n4UxzVUKaQatjMYaAF`+d^zI4yZ@Xy|gOl*t{Xl!kf3h1iCxgH4ze
z9n#aEfY{)=3rQ9bscgzY$bXLietJo}69XXH&fS|pF;z>zAKIAmHa7?1wdL6vs)SLM
z&CQ8}p71haf=C<VGj7XvZoi4LV1Hw5tuEy$3?g+N(E{<9vgU+CEOQ+O=Q@e616@qI
zn0VK*B&kuXz)BkF{u6rru}n^0Ni^}9in88oBXAFQB88;4riSDE8?sxc@PMg$dsK;F
z;Z>PO3Ne<V+$qtOY*BI?`;`pK7ZvteI((id{;j#@Sj20x=;2c-cZw$yrf~&&OAq|N
z2M!kteU#Sfey7AsPKJablx4pc2R}3OfUcD<CQuE`fc28a_25NClW8~LYI+^!2T2p&
zncJ1-K1xNVqHtfAdJC1T&X0QzcSO!lBz1V~J#)F#buMtyo6P~?%E;3&px5j$3G<`l
z!0Ki!Nd5$AC(j6fb-3etfi-a%{T7p*D;h?)^4Ri)SJZ8w+eC79z=h)y0iS^8@>Ldx
z$l!aBUodwFRp8qjtH1Iu6--`e9SDP&22lkMuLqQ5Bcip?;Q7Jh{qQpjMd!$uKb$)i
zqM=N8b$`l7y!88aE@tS1<(US1-pi!#Q82@Gt%m;HYfnk;9NbCl--df+UkR{}1BXBU
zUTONl^nP<H>I`Q1uW#ndub)~MV#khv)5>pIzd&TF1Lim4l?HrD8^-^dwwI*)0;08_
z^_RZKV)=aH(7Ee}tbNl?ez1!{OXukOt@NZA`!I?|Y}!OgBRT_%B{Z#;T)FvIs&I(u
z0u_M6NX56A67Uop;+!zGmx3p#YzdbGM)H*p&<L_#p(J(cbmC2Hz`X#DBAhH~iW43O
z3^UuRM37!R^?Q{EM_ViBCuer1EHM8Ghe8i7XccGkoi9Tq`CrgSq<V7I%+yA{OzJ?;
ziRYFwzrGgF4~Zm9G1NRWr+y_jC_0rw977ezV+x8sWm`m$DLE%Q2v9#B`r-0c=^2a5
z%k8l0WE@1JhI&XYD)u6GP(kb$G3cAV>bd_J)88EMx+B^rD&yZaz8O+?Xf2+&HsO0E
zx5(M9p8Mvzyj$_t%(#nJ|GtA<IlR7X`sN4!J50>GAa@6r<LfB9u&DP7<eB40@+zJ_
zzJ#F~nOfru_j5#<407LB@h@lXmLSt8X9}&-{CQI8Xgs|lm%1rS5H$*S%dgIV=2uX|
zWZ;RXt!q#Pt~FQ!UQ)G@50eUQGtc`XKkFS$dAidy<NZ%t8QCR>81rUH6BJ~iiuVyL
zd8VIc8Gp5({@wk)>#a`DA8#qp=5FxOf6oo@#W)Nj(s=-iVzM@hVErXE054B*B_s!L
zUDcRT)L??og|gVg-daESz?zu#W%~B=XP)lpe}>w0z42<#ZL+t@j;q4(&R_5Pyc^|y
zU|euZPEmntXy$<xqqeK~1<u=S%%Z?xMuOYw&*>g}5W49fDgLzN+~W_w&uiF!)^$6V
z1NnJZ=y=88b>GvC5mLl+fxl<m@4jk$FZ}VDx?Z;ym~}|j=Un!rIQrRvYQW3bH=ZMx
zNi?*Q3G=O7I^h5l3aGSg0=QgQ5q0h2Rf?Nfo2+gG376Y$%}h35x{I<b)4JczOfHK9
zEcS{G0Q#3pIC&DlQe{*~UxXFRfQMUw@$D#xBJuXNEh7<NR+`7s4HzOnnbqdkmg~F;
zW?~jv$J2Ygg3Ui^aBetC9M-Qq^<ZOgsN@wJeKS4yI#MyhDmn6E)#%>)*R=Ov_9u?r
zitRnOd_8>?6gtKutBZ;hbr1d<(M%j2=Ca|o`~EQb@d0D+r@6QXjhvAZkqW%)J7*cg
zQi(VN9)pLG7ontwiD~+ZBNe7CSM|C`-0VN3jNYLFr^4{^p}@GKW6aF}k5W>2SkK}|
ziCoF!i<7xKo-cH<BIzvUq!SXB-vRLRVyh<Mq5!^oVB+w34%oH&c^_Tpyg1Nfl7Stg
zGYkkw=t=5=L#9kf>r(5QKFV9ulkd8i$WjMxPs`8T3lHM}R_Gu=YphrFtI@`AZ#0h9
z=TdFkKg68k6FwVb|0zc|q_<vH-DPE6K`2vLsFXTOvhlvvJ$f}!Q@}<R*vtLhYOlCd
zY^<{UlcIj(mhi`wo+S^f5Al<CWeiHmDvz!FZ9d<5Y+PFs5q4Zs_#x&&Q{@|#qo4K1
zE{#g}zS?kHzwDO!rgn0zMU%1H=0oanV-E7yTfHkzGnbqUeb0J@k0u|kiXY$I5A2PD
zu+y!2$Krz=6&E@U9<J8=<M%IxRa00Ph*U{y5b6E+AXLq4SsY1fP8!_uEZx&dvI?)m
z)gD|p2Z?s9kOTgX(r~M;VVS@)vhjLr+?@R&YBb9_7%^gv9Z_#YlwFesAkS||$3MM)
zf6b5-DTdniU?N%bso?w=O)84Ko@El$$Hi14-{0EG+Wj7ayZH2KmC|x^BapSixgi88
zmNDky`s<p&_{HZ>ZZ_-6@dpEF+^1eTk*A(}I3=Pn7Y3RmymmP=(j}lOanT3-f5#95
z{`rOdq#|iHXp-)ll`M4((J_yZ%WCIfDpb$V)qNvob2Cwf7wfIJ^&V}oB~5H(8Wqj6
z$QPQ4Dv-fzEE2iKUIUBEY@b;DXOWY&p`HSZNopS_e};*(*`;80VGfNjZ$SXMZfRw9
zpQ;|U6p`K0Km~0inIoHxj<`OvcMp^wIaBojetk>mFr$s~X=<=|wfrZ>fKe{%Q+3nN
zkW+#EB-N0U7YXcg`lF7kWh??5y4Cn}<lgd^3t}bn16ijtpI-iY+N%s?`kXwy1Bn?b
zwfxFVz2!s?mBK!MX3v@2(otDuE#hxDM}2NPbX|Vc{o%vy2-4S5JapqADtiC(zlOS&
zz35A^nt4Rf-szo>0v{UL_v4?R8yDX0+h+!TXnlRMN2m2FjQ&K^Vpil3v2T2VR%e=h
zE#acfZZPwprO<me&7f??`C*tBYl0r@4hz?yoO&uX_{qn_|9^&$H6!g<2y+O245t?a
zP}2YpgyZ0n87Kr<hY>t3_oe>7t4_WcA`{#$Hogk(0Sq!jBamg_o}y(Fc6Z=jbxk8G
z`TSJCdV(M^>?>ei``;MCAeWK^d~ea?1=9UGldODEm3V>4CCEk@gKo(Jl6e;5*Iw@N
z^`FVegp)_vsFR>MBD)oGr=5&Fz;hg!XabhCXc|B(j!w))9djup{zCg+sK-lRsdyFP
z19cXKI4<ve@6qi`58Ui)b3)epZtYda+iyudOK|2YXS+du%5+m`G1qn|4Dr;ga{k@%
z8q{w^C7oRGo`NpPT2ZN5onI!RL+`pooSMb0;4V#FMK0*?5vYHRX18#&`A6l+S&6p5
zRsSIeyr7h?ZPX&K__ceO`oV)j36^Ki02y~0dHglZrf*-BA3u;K#pHjj4P|2OpQj^H
zZ&oJSA#GavxIl-6j64lFKP3F?B)XDZ&<|82QbHiLR^tWr^Bc9Xu8+w1QGJEvRJcZ~
z%xP?S(tNG@N@v-k({|6`#c5EXsGsZAYSH}HV0?4_0;nF5rUMy4q(gug)ZCxhI6D0{
zFku7GDaP+GDmXiqyTk#rh+=KvHMyw90Z_sjXk})l`|`c>i+S0t@Nr8hC7$y4?_<|x
zX;sjt+vc1NEFa~p(R`6FG_GuGPS;ex2fj;+(dxp&WWMRTj^%UeYyN9z8@XUO#M%4L
zei0s1(qpzCi;<Z)7wcNY|0miv+C75Liv8}lsl&cDorDV<^mk<D=`?-832yPHrU@JX
z$<~j~M#l_ilzd^J?O|4y(~QZsI7{W0=0oc;ut^+9G7e(TWW6ky;7-FaxaCbxL@7EZ
zr!DGm`~p1uDdym*OIg`x*m5yCyX`%#n&ZH-H{}H6IQlYaZ57Mm3-FKVCCRmiH&Nuf
z!xA`O)#F)fazJ2PJ)|`ZOV%we<iG00Yj04Q#YGXiWfvUdA?wLHz+*ft(ujPogF46f
zcIlKfY?P$Y-WnowNc&(S7}DGU9=dvzATnvRD!p)1kUiu$3+|F7!wbj09iI}Jke8KX
zD0uo1d>?>=+>PWf2-$;MTtgPJbYF0bz7C@C++5|8t8h}pg(8v6_jM+x0o3dGSaga;
zafdZef@7-8HOZ;IjE-QH#gBA8BXnKac9knH8VrK<R)ERPcr{#M8RD>b8s@l;Eoi0s
zYhl+(+ox|SK})IWiN*!<t`nP|WWXc$`f2ct4E$*)C1BoXc_QD{0ovH|;ru=YZhGTX
z+Js=!EzTJfWKH{6By3Z7gk<;Dn1Z1GfN_Otc*aeF2P^*$4zPK8QB#nQE$n688}%P9
z{$?(F$gjS3-9Et=3moRM*Ev6seDL4J2Sy{!f4TN}uZ8WLvPGRcKGW>vI&MJ08^6tb
z)m-;V_)y7oS9I}r?~mn(>yhsb9t48zeHjz4r%pcRY&q&@oNy{VV0wh<9TB0RfiD7;
zG0h@0j!qj+?!54($C>>@55?#j|L`VEzn7~V9`jvjR50?>;jP@VXYq*gNDO);qU&t>
z@Wv%_hEZ5j^3NEy7aCZz8RlHQ%<DlAJyYb1Kev`!1=+tw+bC58N?-;B&jaAO)Nz;i
zQ6Z-~B4Q3Xgh*mG8Pz3-S0HD4@P<6k8ZO@kxaC%2B(8TMh(+aj;l{>}qIV>Btn@ue
zOYg^%-K}np{O#vHwHp5wV&#3EF5~+@pu+KzzxU-HB&_k>heq!BJT7|S+3GqHHP_ri
z^EcOe*519`QCjv)dZhL2Z-xegOqSQ6Sr(`5WQc$yiRj^#gH@pt*H8ohH@{|q)8OK#
zeo-Mac+&X9!1ROEAloEN0m6bqf;zR;4bcg-YX@Ck;$~7rleKS{=vF`3)ShdtUAAk{
zK)#Nz9Pby*Mal9(KlvAP@B+YJ*Qt5_xi#M+p`)xRYWWMW_1IuF7XgkPgRmP4TVnMP
zYW}4!QT@;!u?g2_%U@NX3F8;Fvj#+LoulEYhIoc5*|-FR7KD3{-hrZ>0)v-=J*9tM
zm*@LH=utT3JL)Kfjt$5)W0{4N>JwOyG^(lHCXkoL@0#atMJ^mX-@mO*e;bZCdmMs}
zCexnq{rNoP`j<y&hyUfyG4cvTRN>K3VkoLa%qJ_4<q%LSDK!;c-1-@Orun$=-{()i
ze}(`3@TvszThn5l=2RPJ3!*`Ni)NyI{;F{PvGzBAHimLoY8Hfw$C3e+le|P(%J&cd
zJk+uHqkYs3(~|;?xP$ff`<fN8tV6N^5`1L<HPco`jU~{OvC2be%it&ot>J-WmQcCe
z`a`@?KbVBa!Eb}a<EV)@a2b3SLTB=X5h`PBz4Tu|LGcONIOEGgB2F8mC5dEceJ-m3
z^`|d7-!h22hz{L>Y6Bd|dBSv(CtrA3n>8dvVsweGn@m{ayD3qN?4Ly>4ff2S7$b)X
z^%x|H@Q`@J9M4n#olu8-v{bC$FjlvZ=WZ=%!wZD@^IIV9f9iPbfHO<vSg6%c^d6Ek
zN%SITrHEl(DmNZpLvdYfzu_SJ;!q1|M<e6oXnag5GRBxysgx<$9tIC7<7IA@bDPHB
zso~D)3+Tp~WrVub>c0eBan%fg4!#72JBWvT?3mi{lo(7&($vcHtiwT!Y)NOY29a#!
zbd-SdwKE`k)diX-%Iu=fo&ZorXg&ZH0{1sFD{H2BiiI6KD=(nMzpmq5Lgo9^Tcd_i
zJFa0xVmmR$fVeNJ9xq)z_}Da#aSfSMrbck@+e|$R0A#*M&r~2%OA_$bMC${>7pUi1
zO5m?S?<dXIL%HyNX?7)mk~^(6Ade9enOF8rbp;fd-g^NdH)x;}01E|V85;DGh!6TB
z;e-$VBSD2E5aR=K4skUiE9ER_i^ZMI>so9dPIe&M{Y^iCn6l@`8y*+mf&)5-n+@;h
z4rV0X_&jj_=7a6<?_n0qR3jDBCbH#j9nJ5vryay!nm3;_7+_4Dw}1CpH1O%gx~&lZ
zoF%;piqy)!xuuq2GMj3CbvO0_Ij7H7=L|NWYG{O25t$bzy^iuVf!0Zi;NiLO!C-uy
zlsrkV^C2~Zan&djj^%h+_*$654eCOyVG}QAddC-jgI9-5A6#aOWg+aSQqeR#SmYWB
z#RdUX*s80R)(R2#gM(ptsydh9m6!Dt9Y`}Jx(2BO$O^Or%uvMU<1a?L@sC$vZ_x1h
zcc2KpfdnZ6q>~4$<!UmWD21~Zga!mUjJYZ{FP!QWzX-lBN*9yI#RCYHf4y>lDoNV<
zhoG?fyc^YLIMzAeS79~fkOn+&wxbjA3SiFI`pVU!xN_0qi2)Dn#YE1>Z2N?6L4pqp
zDI;Wa99Peq$DGXEItyE%Xp73E7q@etoWA2T!d#tt-X+bTB}u6;_nBNKnu$D|EfSa!
zON_#hu=jNhT3CqbzIaYN<ZZ#WYhY!RTi)pSOMathpMb_^PnDbBiw@kmDXbIo#4$&!
znhf|Qyr|}&8sHcur~)EkK(!gz>Zh$k9pOs5RK$HLLIt528pO#{o=;oxiSjSYMrZQU
zbOaq4Lk(^dnV;NKWT%X}W}dg~*;=`GWepPX3i$FOqTzgL;fa>TDEWC!j+N`gX_U;B
z@8oCsRwWx|wcm7TFXauUe(<D?Q@XU@7ghPI^p<r4C4;wDPXVZ}gw#IxT4<CsK;!Qu
z(x`_5>=|<$Ui|*2D=YaQEY=O4#*;9u!yxidjTsRw*&-V_SWVO{_(<cHAe%dgeJ5E^
zW-1VGosHKy9FP#hM-nOx_-pl{tZ%-9)5MQmIXBmqoeD1lY~qV`{Xl1B9mHQ)F>GWS
zB=N?}PGU%M#lVxVDyK+WWqcWs$V5opXo0#|u&NqT-daLk;xoN;G>lEU1Sx4HH8V}x
zql;LPt5fr;;TvI&gPf>FlXF*(=D*7*7T!j~_ucaDn~5!|nXQ8hJ#0(P?7o2R<>n8V
z%j>I9Ld3-Ry&6y;8b=uEwJ$ooOvVfR-x#8TKba5j4xA@FR8(eTAq=ju$$~X8z<+HV
z%SkQ;Y9Gc+R(qLhYRDWA-q((Hy&?k`uN{gd3~Vr`m~E07($*rhiYL5Zi&b(Zvak+s
z9o?>y7rjKZ1z0gx&-2U^)R5`PM{G}+L*HI&=@gCz38fhe&0hD;y{DCmt<h#Ga@u9?
ze7DBst5jek*KSO1E8<{>02N8K)BpASsa^&HODkp$Lz#(T8u=g5a@{2am5=!gLPLh0
zuT5I^O_$jvz;EKfjyEdVjc!IbbbOg$m)eM04bo<I7uIHAk6kE#uyyr-cM`7|**rEU
z_k+r_E*B&-gm%$y>HkqUCtxuFJB+)e$r_XTmhR|JOz(ptm7$M!e|Nc^o1-c`dra65
zP7x2X8BfX60<^UK4{YfGt#1dsblTG)P&&0B9XDnzQh<D(&Vywh!VoFjG<CSSm2Z9c
zrkC_-9}LXSq8DuPf%~zAsdYSn!GNi1K%x71pjI*k4{TwW@h{|s^M#Bd@u{MS%!eS6
zIJiqK0D?zBrO9#DOtL0f0y#1!3K|&-W+BUugvwf5Bve&#2_x#SAYFTG07E_RC2}_S
zlpvUm;t4NH5`?!1)Oz#b`G_9%1Obk6p1C-NL7r`KjAv!oEY2gFz*Fl{vQkC};ki*p
z*tn#NV(|EH0?jfz%|ovDv|QZ2)5T?PK7!A)NsYN9UUeX_h9948Wx2W#o__BNIhRh!
z|5a|ejJv9GVkdqPS5=g}XfH_mWPy?D-FubC`Qn-@obydMxnq{)x};qJ@2w?F)lahX
z+p6j3CJ?1b!m?Q8QuJekwr^U9BpV%qr%`aCR-<t_RL>IQ@wEjs);>!h8-++FBNt$x
zY!RmMlXRg}C+!SbNg7;73ojpEd2?O;{W?*FwatgVysz;!KDF{*725$$4pkP6xs?QK
z3|a)wcNE_CSd!P8xm8N7;C<nj$d1WgAIFd_g7lWiQp)a&*qPSH+nHxW)y{*ABYs;#
z{Jn83snjm#jS!LfI3Sa0IN0o}>NFzl6f&bc*KJO;fmB=_R$67aIIMtR(1};xs$rWA
z0T_e{d1hznZy!2ne812w^C;!3dacmor60MeU62vW)9d^{WfZ+X={JAj`fX4mNoBvv
z{>a$W`yNreF&cEUY;$xv0rd6P;pd9opKKp;*6wK5Bz?!8J<d7$>Dnr`uOEAJ?BM73
zu@}&Paw2O-$?w_#CNXbO@am(?K?ie~SP+|1KoNVCKL8z&M<-B<UyHk$C08jAP%hDB
z>A?(#%_K!tTmi(eC<@F>l%w${mxbU7j2k*F?Oa$6D;QN7Z!bn?T}4E?c#8MraiW_5
z#u=~#i#vfu$<!fk{0TtADRSSl=`Aj9Aw?gMk?`JW&b=e<E`97PMZ?_U0P+ksc^=x>
z8o_HnK4`x#FZB3l?>GCye{OYaN{=y11NUj6=$R!A_7m1F8=W5yOmq}s4m-`2!grjZ
z^K!Yrz-A8O-%$rwBv^BKf*uBg6DMr}HR^p7Qo((-5}OXts{$PC7f{Ru_Eu|u2VA^A
zDy=zhqS_1K2MGpLk=s<R1-UKhZ2P1?e5i1esPN*Qexlx)*JMY*D`RJK<P%}Cl03w=
zpce371vQdz5v=3Cz<FR-57_La6O){LiLhy3HZ*q{C*&_3wD)vk?P&QO1PE+=^PJ~e
z&<Ea$*g|v23tmIk8@TJ~0Bi9W37e}+f0!Fmo&DaboNse0ln^}3Ha#Pfw)5VR{?-$-
zjBXwap!nCkdeWQaIEHXQtkEOlF=)+Z-Zs%K>szfwLh&3U;cUgs2sk%Oo~wCk<#v6i
z?wf11W<>9Aahv(rZ?}n;om%yF4&T<bOE+rPtRC$O`qbuR)@Y5-|H^1Msh5FwYPKE&
zWRw*?$Qb;#mIsS~W8<fjrTTv=)8Hg|Omn57SVtuq#QVDd)CmOhH7@2A5UWanbHNr|
zOp!$mxnOy2T}zN(RkRwwRU5C{83l+k)+18v5G^>O{+gU)%6D13-WsHil3<0%u*B>8
z=5R>~G^Xfm%D}WAP<b<6&T=WL?@?C-APE^3@<=iE-_3uo?2KgmaK#xgDF;<fy+_ZS
zm=!U#3sQQLvg|7X*W(2}VLxz>&#jpECatByJt@7rRT5Vy3?la#9R#eSSI0T&vEYL)
zR9q!~ydhK9JK%9Hnh4F1JGo?22l{`<kt_`x3!t;crdp}a%ke+uQ0i7h$AUwUw=jW*
zXng!yYnMBX5iD;)8(!x;%5#u9{Fx2fHXfrzHbXw)Q5;PW%%1h+z>swu_KOBY$qXEF
zW6b_Te6Md`&Fy9mn7d{*4JJUn9xkCm{K4>K$>Nc6ZoVV<r*niAbO&y@1R9WMjL1$6
z<Ay5Wlj^jAKU~G&G6<pqgCS5OSj3I|y2D<c34%&l4n!z+q=e{)?ED$%1kJ`@L*(}Z
zCNgw$&R3>K_mZs-I`(f{UW2hGNr13!q_HEpAIveK>H=Sjbo0Qwaca%xAN)t?zm90g
zzq$}IvhhvtlHdo!)K}k*qc6#R(EYcu`g<TI29qQ@nbZCF$=bi(2cI@mv@I_l{nBh!
zct_VyGWkJ+m^S$is<v!H(sas3L0Pm(0U%m*b2baRE}QOWm~I^SGE-SglqeSlcEI-!
z02rxBh9-IF1puI4mngm%79k^u@O>iA>Xb=@BKIYeik`Pk!GmxZHYt<LRw&gKM3svh
zg;aJcATkr;;tko^I5B7<UyNkuR=|dOxlcXM9b^_2*lBUN!xL`l>#@{;B+L#6A_)aN
z*UAWZo&Y3aiKm{NG(KWxgYm4<K_sDgUL%rdczhZSb$FbSNeTu80mWy-!51LNP<e8>
z+5w(v*17e|D}XKz|2h{w;?d2eJ{;S9!W<zc`r0|0a8dc{BY$LWGZ5{pd?V{-9M_u+
zA19l}+d0d%H)eB}cW<=iQgLXn!zt|SNx?SPIXj6CwN)#nD`MXNf*j=OwtZf_ZX6D_
z4-a8he3?Tdez~LxqOWcVfWEd|#mN0g>D+?b^`LM~MmI%FW<^?suB$UXAg9WY_W{f_
zp8be%ul=XZbthpZ63I?$@#43`(?)7*3(|(<B7ehj+KCmH%k6NeCcFeSO4lv1h1vlm
zr5P!F&BlCc1wNRP)M{Vb6MPrIl(I$e#)<5tJkEFZD-38<_Z+G<wL#!pYK0sqtv$ww
zMS!J-CtjJT5I|I^6%_|UjuD_K#MA`gEx-~78dWYT;2#ayp$(lQSG~ShaY9cwacju8
zGU&t^B2<D`SE2q#im1K*Wc3HET$EY|c$K~}V8v>+J{&ak{xdqbQUyBFl2o12FYEJs
zQ>R7r`fj6@&CI(2ikIWsZ;Yf@>7CvO@3swM|6Kk2$MPT5$f@<YHnnu<R2P!`t1nD_
zoZW-YP>(IXIlKI!bthojY$itXn#)=pW;Lx#VIkRW0qg(}zn>^S$U)Ax2B5}%DNLmc
zsKOTq5$NU^_aTwA(l}b)B<5>^7;DII_C-4@aVdk^V^rl_!)1W!*b_FAQbX}9SFDt?
zQA+fILy+ayjR(;%Z{J5b17LeSn3Tj8tnDS^n+O@Gw^RtCpHjbfAIuy67!~{M^6C5Z
z2_C#u)k){ofav7wk}ZbTOacp7&hUgLT{bA!!<QWCj<X1&5qkD{@}D4=U|GE30gEUk
zARE;PE;U9kn6SeSo#2>UmQB8yK^wNT9WYUDR%Fy+xn0t78vlSx+r*Jw1eundA|JN%
zo}wTbuzov3e(h}0v5OsJ`n_{PfyPuM^Y)D!A%U>PjxStH<^}1J18wu9tM}P$@>;|i
z;-K#?I{gm4K9b^Py4W=3WIIu^S4C!{Wz}a&wvpcJjZ~pIr&Mhc<@03CXS(ChJAfAN
zw<$Z)8N%pA6e5R#d1qnv=da-ks}V0<reV4v3OSK56@|K5RWh&z;zS^_iTJ{{ot+8x
zrXm)>McgOK_yryCMd`n*a|#Xof}ggd&!wN*zAdy_8I+AW7XtJ<RsM5hI;dBYuX$d7
z7s$B2&~OXiF!%YBy&H-x_Dxi?kWa^PiFe@bZ@KNgJG+kJ8$Nf=<sV<RpZ|CL_nR`#
z;CHx(+_YbGEaOS~7>9C^e2*K%R@ftjL4L{nkCME2>n2Y0`iv*ra5x?e=iyoiD0<0R
z<2b#CHNk~m*QSLN84-9)IidoBx{tum5txYtEPcgI|HPLS_dBBj&R}pCf;!FL$O0Ud
zjuKWuc$c3&kBqXt-4j}^jnyEX+=3sw*T>c5lcfYd>ElRjd!A#z^v~OVxk;)tw=$B@
zkA!+X=-r_CEt+hGy1C4Tf%|JeP8_&LeDn_;5f<-W`$-(_5;iDoZaBU@AnEaEK6kz|
z9{We-Iz_lk#QJCH|9-0be|98Bg082Ps__5Wk*HAx>QFL7lywPk2%?i|*Jo6+4P?Ms
z!_~5jwp2*-?Fn+`;_^<gX5qB2EpMZ;4?XMnYPRK(c`7}`a)kpTxd~Y#sO~Ct3gz-@
z0QL<}w%V+9WwUj?NXDk)Nrq)zqHk&&<u|TA8*Xe<{i6I^p*Km`jv~#NN7oT2MsiAn
zY0;H`(x&BkNMK}}7|;vWScajMBXqef$D-wmnd^qzn$64@(7E^kfJSnTTMIrtz53|A
z$cgJjWMHR@sssRqe;NQRtzLhHjIMtzogVva&GTy)tKMurY;?^0jcQ4#^|>#znG%JH
zea!KD0r>H&SJSD|<iqxl7>_=!wqIv|`kI-302rA}o`3xopFUDYkskd?KTctaL32xz
zIdAZ0r4W<$w>b$2vN;TWPLG(a&#DItfczN#ZfXhWkc6*-^qZqSSPW(w4b~7XvjG6P
z$q7ouLx?j8;cEXUXlMICw97gUB=Q0yR3U5}l3fcJ`66KWiG!m7IdbOmkTjwx?Wyzx
z20^7whcJq4O(obimyrW`IMyuW)}*7#l3IDS5!swf*s>&VcoV=x2D;-Ip7HpV5w4mN
zcp)Bb1iqBh=mum1EfrcG<^%~v=I98Ao!^1&N*DTlcaO&C%+>CdwGXmfG52e~{Z;hM
zPmqU$;qI~OtA5(@c*k4E_)>>g=s5fZh<ADFTh5_pfF<Py>7!#Ux;Mo|`QS##a*E)X
z*Za)&sN^gDoR6<8ZlNCsEWI3;iR{dUi^aOK8;f45I;^G(FE{8qOhA^~LG{rmd)?vh
z+A<Svm(;_-dF2#WRl?QR2PergwM)><N@gD5NgR^{FyvhylAYQJDPL_vi**D7lAJUw
z1>~;XV}hf2?6t~k0j?os@;C*j6kEnYsG)!M!J%PUDqNsc=9is&?>19NVsZ5mp2ZL~
zM`+K8<G&i#yW4e<DMwBXpEA6yO)EUsyd@+&jxx)V#Rs|5ssqYYl=LupiUPceqrW1J
z=il-XPxjlXkEuWu0l=a>wnDxv1X3eN39Bu<SAL5qLC))HikV?{vwQ2mTk}TH;LslS
zhkNNZTU*3O{eAnGkJbD3S@infZB@9V1pg(kj%2vy!=ifcR(<>}*Crz{c3#izLX;L9
z%J<`g$?;LN+?U|5e^#=6_M>E9#sB(wSF~g8F0PVDecc7_ZhO**Xal+IQJ3S^&6Lrd
zjQQ4c^d`^}Mqvtb?rUcTv$}GK$oi_@Dib{-dEsNt>#zYvW)YF;+GLF0nJ%Yz5<#1W
zTMhuR6x6}(gxQQ?`jS`{H87QAU`E0QS`^ICI|obdi(gx+H#%s%JU-wkp@V;vo7v@B
zd1Z0rVRmfZfR5V~Tgb>ln_kY~j|evJ8>4W(j!)Jnq7%d9CWoyjkfN}(iOP>-5l*e<
zY1i(jfdXV|`}nvgU41iQDFzCp{<rGpS7!buQjHKSWmtAkGgcg%h=>{CkXO|-Ahlyu
z2fGL^!kD#WtDqn+@}0%K)4CqYuQ6LW3t_cP_lmm)(@Y>&HS_e0%8a}hrFmyae!?8p
zZBJbErUC{LDfcXObNJqrM}lwYXEZ4&(2N&CSpsJr4Na|wl=<e25RW>0<tu>YMI`as
zHR0jO*Bn^qPf_`LFc|ev_WASgB*Yvfxr+e}6KIEqGR910sAz0KB+7#lS>_7kqd#p~
zaSJBVUlslAsz2NRTg&J@_bg_S-A`?$iit%4_xa?cG<7`kG3PFa=mXOaV!G89`X`s_
z!;e<7PVT&NC713Hzt_t{W7;+j=bojQZ0~7DzQG+=i2D=WVvK4zp>$eFqsdm!H5&|#
zj!SFKuw=#$&ifjG$Zc459TI%6eUM4Jax$A-?-}3T@ob=Np`elk+>;HwBr3j+^$-?Q
z&GgoAn02F)>7)q~!P>QWC`;L3f#{Ht5ov&^OMj1<$1V)dLLpg{d%gx3@AI>HELkKm
zKEj|lm0^}YIMe4GsuGO+i<BlzDgvBUY&#1n@T8kx+33<Gc2<{a-y{3M!O!F-@f6A-
zJ+b`5{fXy60=Pc|cA?Cj!@bz?hKR_onIohz_^yG(WEbvl63VFLAoF(#)*zm#qSbX2
zwy45PQThw;Fj{2((g~t6Isd;0Ir0OXsW=-GVX)jvHCvN|1s{)SAqx9L1ps<-D+Zy?
z$e%UfHLV?OeMKfGPDIu(V9*ta)OD5Pk=#UU0_K}X>S!)M@xsY#PcTx_#q3oxwF;CR
z#u{+&&~;g3I>!kXvi;{{W3Si@boOvvDBoq{bmt=TvJQzf_;y$-_Di-J-N{RU#2^%8
zx%ZEWDww04TfoFJ!bY)RHaTVvA-eGa`F*T_#YLHjA;+LMUzGY-Zu=vf_;We|Y5e+X
zJ_#6SDowojIQSAX`yIx)Z%=~^uTESUTL*aE%}u-hvcwFzk={-!9nbDKs_&aG&{AP)
z0~h~%<)Ix~PmWw&sVyyi@AYH%dFH>;ioM;Rju-xwS{!hk_MO}9C-mJc=cfXpQ_uct
zO*2i=mj_8+01c7~IH8)&)?!fX^cNwB_B0H{ij~#iWgLJA%l^|3hEsb)zS$mdo#rFk
zgYpbhgM&ECLojt<&@j?aHy{`l1Fc7O%Mr6P@kXmu=_mQV7xif_4tyVP5;DsqHj(*f
z1cPX8tCD}=4zSC`!5PsChKDGKzEM3N{acW^#@9THK<p0T)fVy#q1g8ZJ>C)lS|H5$
z5AxlRBxLgWl~tqosPqF)5kg@qYKv-l|I~^{QM=BBG9q%R^?cGZSDkST<|G=u;nnYB
zlPWa&o%~w9$`y4>;rSIqR{vx-krUk!Pm~4k=@%=mIFR1rR!*XilW|UVqFFy(N4#^#
z`)=3Hlhd2Q3vbQZZ#g!g8$1?$b3V8$y^ZO|ue{ZmjW>D9@<S4lrvr_C|1^ce_kxNi
zfqHkMV@>qfQbyrFlIg`$Jh#NMquvwYu07;-V}XNp2TS0AEM$TnFPzMHmW{~Aj|1Y$
z^4Iz{YjXoz$SJMNWS_l2(>xz>k%;`Q27d=inUi=@DayZP_>m-EQ&iVZKb>YtV{(di
zhm^Yizbd5B{kTCyed{wqQ2~6Yu_;OZ1qst|FxeKkN=w(X^~D7Ku*FxD29Spc3A8I?
zBm$Cy$bcRmZ`vC8AwOM3GFgDdHK9(KIZJLtd8;t2{OR5)^J<)+yvW*C6o20Qz{_+!
zywlfK%AJ!1g)x>(k0o!d7(D-C`Ona5Ds^>O<{6Xf;B@FNUu26+7QfC4IO5|k*+~nY
z-zBqdJXqJeG3=3=Nj-T{=WB}hPCw92@L)eARWbc3S$8}At5uq|`DaBHIAq%fN2cCV
zuj1$j;Av13VLmz-j<wiE{+3CzQ5<?pkj`C$!C0(-w%V(_V6_{B3es1EVa1E>6nK?h
z92E`@xs=<QoU@fw)5cH&u=Foj_|O@~FkJ&I=Kz?8o=8Zy=2J>VYCGuhd~HM<>*AiE
zE_)f-Wqwy~#38)x61trA=t2hU?=iZ}#HGk6wPNinn$f#Aq)EQ7`718cr%mX`OX*Pd
z90Y<Lc6XF2g;(?z;43aQ=e0+xUh}*(^}8RO5RZF88esSTd3PdQQiMk@$No>Q_YCD=
zG$UltOHP-LRF_<fqKtT0G3FpD@hM!YAr9ZA1LHuMB*$yms`@Y9;CtDyYfckJwDJMW
zBh}LRgegWNl^X&3iKbn;a>fdV0SUaVireH+r+Z+3f7K?X%%0^7-|4UK5&TGYCkutc
zQ<4O9yQ0X_n~Ap767iBK07@llgV($Rsy)r~6PB2<cY^|1Mmr;^=C_P?RG`ur$q;p|
zD4!*y683sVhq8%7JPGyY9l#&*eWoDfLJT40&*JYIrCZ6+R=o6S^eD(Fk(e-iHTk_8
z0b;kyc#nAbadDJ8{IA=)aXpI?a&JWJ&sP~<kvW3feJaDLrLPBL->8#e_RChTt?GB%
zSt~C^)PIMe-J@&oZB>YOTe*2(Hu&aoWLGI=w=3lum81D|-f+&U;ht_B$GAxxgs~72
z0LfB@s@bnE6uWD%<LknQt10pYHj3w)p{?43Msm<H>{Jfgm|Jutt^6`rNi{cx&#auE
z6(@WVWqGs#TfhS-0Zn4ddEt6_PID>2OQ;$Bu6jJ*RzL8}YJ;Rd(#kCM8hC|l^E5C>
z9)bq?A2{aemr?LyD502_5qiq{jcB;0R13X>{jCTt6Nw8IoLeqAC&!N{hKQ-K#HyzQ
zWt6I_@T-$$&Mh5BG7A#)tMzB--K(HnBdzEcK|G1p)z&SZjC4HJRD0l8+wo5Y)JoC+
ze*!TQ_1vc^*|yYZQM`K#G0vaqJl=qOuM?wgLYIjJefRUyF$3ofR)uU|9q0Q_7^t3s
zF3<!^ku61n9n<)(>gLX@wh`0i<Kd-)MV4;`l<!=s?ed>qdZVczny~<ItO6m0p2g<w
zWeZZ&17e8TZcZmU3>2OeC!<nul)@R?Z!Bp|0*@}r5<m8Xc3kwd=*Gs$)ZUN|Bis1K
zp`~_qOC*bru9I@D5P`oMZ+m@#g6Koi!PXDm8DcZV_X&IMg+!4pP)|yd>#eV%T&O9k
zpWvM@iBag~hAwpiEvw{#V{RcIr$1TYU&M#~7QFG4dag^3LTT0}HG?*PMn-4c4CCLG
z{1JBP4@KrfIk^{e@9dW*N$BLHH%Ofm)VFW*8r=J84N9-Euke>nhr}#FKS)Yr0_y}R
zYNQN3{(@%En_=erp(~3Q;u;3PCY&8JMOO6WnFMb(wXcc>hXxc9?RoNl>kcCBzdGv{
zhFyf5>4~Jexv5_=M%Lm?l5v1unKZfeC`)!CRB{YMZZd`{89z~cr$9~V2`L*Ne7q<z
zHXcP!eib_I6xA{2#IKD=@}?)0Q2eX$d=S4jLN^7OO~@zu-6!N@{D_2t+8fMx(YBvJ
zbocj5xbM_bCtnKkjT2u4vRXm=9h=V}!e&AwiZ4jr>f$ijEay}>w&Ts;L@m@4&*$D^
zCrVNKWgTcTRYpq(0WRiN-0)x0rn~D;p5I|nL$P~P5?x*wFol-e8%tfg@>OZ!lY^W_
z%xy<V;(fgH)Av2^T<ph%LML%{&|8yPdT$e6Di}N7m7g!*u(_bh1!e}}2=e4l?(`QQ
zke_^2kC(?-O5)eIM644LxYnQeH^f|5@}_u{YF~x_sm_CDTs)-wa|7SfJIUnxaW6!O
ztx@q(Gn#p2oeikx70r$Q_$oi1R)^YloOpw!bn?kyPm{ECdNI{(me%51AC*+yE`eWv
z_C=y~pYX=v32pJWf~M0aHyiCiZRAJS5{O=1;)HR;Ln1I4{HPQGDAYdruhS}IKvV)4
zE;f+~mF6RKkD{QN2`u=MRm9Zcy_d&C;=_--SLz{tW)94r@y6d?PX+dH3L4|pYtgNV
z)(D1px1TZ*#q5J;X;RxmTa7XdYb!lA`~y~Ak?()UM*D%Vnt2UuW12~QU&~tN?sDt9
zejXA0!#3=8{P+Cwt#8r6qN{`p&VQm_{r#a85%Bv$vJ}os`!70dCv*1H!bA^FS4Bh&
zW<_(wLxUxnZMzsAdb%W7x&R`>TZQCov<Fj;OpVuf5Jf%)-8{|%^KNRNV*^{sZc#6n
zKzTn+I*K&_5R->g;ZS*q6feA3E2IV&w_-D_<lh5Fb2<+@BCjp<)Q`AhIkO(xr7R>u
zkEyFI<C&ZE_T8e(`w#L8{<uzF@)=$D(r(u<=JfsJjj@IG-t^X=E(g$CFe3)Ww|c}Y
zRhlZzO51`6jdah6U$KpLz>SKOevfGnWwc)H)cP)S!lZ1n#_c(*{UHaThBHmByo8D?
zYzCNfGT@{3Qj}CFRO2L|V2zZ42J~W>m*crN16TS%2!I|IuW*3!;J>4Xk}4u9&u8B#
zyvOOcH&WY}T>Ft?V~f&fsnezJyixEI5Z`-wOP>rZnhSrBnW}A3N*-)wy5o+>s_bT8
zJk>Q+{EB`85GRvKrY48+C0?8Z59`LU0t(6g{h&FLp~$E7WLQXG8=;{bBFoaIM`SOF
z;b6dRSLM@{lWEej89Z;|u0O8E6fcf<fWHd;p+&JC@AW`x_xou_kQV1P#xBPaN?xDD
z)&^E<DJq_b%?#I}Y3k+<zk@DV%hkU+Mri#wv@Xq5YEY8<(Z^S8CqcUzo0E_#%s&5b
zXU{L@aSnUGtr~Eh{Y@Os^(mXLxL)aU=)DAvztXoa&<h^~B3@YGtGt$T5bMZRJvdAh
z+?j(GV2{_P;h-3BU!!X76AzmB*qDs<Z)hm%?Z4M0;bK_(>6(*fXppcnlCGyLz+0*4
zMw&^`y|D<c7{+vqi$f;r!47%Hpw)M?|BI~iU~4k$*6a%e2%(tJyGdxF_bMO>#n3y_
zM7s12BBH`e3st&+h)L*Gx`2puLKP6Ds)(ovih!b`sFUxUGc(s*^B<Dz?ET#LT1>%|
zgoLbQeq~q80@^W;z3?4_+NG}i7gaMs($kvhvcGN~HU|#77+}NT_Pt^x4|wj6DO~IN
z`5P@G3<Fi&#+U0}iVAi8iD|dd$_?y)yuHC!sNiXZGh+6&rQt4|I^mdV2(lgG5J9i_
zfiyT6Jw8VmnZJ~2nrdf&|NmB_GpuO_o(hyg4L&ZA&J$H46F|MgVfDk|QQW4Gym92<
ziAp}ROq%gKs}F^$Z&}M`(HxrF<1RWy)3qG{J-e8Cnl-VL(5%F&3D^o2^oS{upr9QF
zU@ENPGAK2xw=ymY@qJsa9}K@BTeBJhXp*`J&5I9(GhWHt9?-`<qG#JgYw#lDU}s0B
zH_1?H<{J(YJT-PA2l$_+wo-XtS*l}d^A3^WGNRI~kWnoC${%mdH<jStZtF6Ox(}CX
z|EKlG+k8zB@O(!+W)Q|ao^~Gfpu$zX2=ZRPVq-<*?Y-sAk=O(_YUUg9Wu#wcZu!jg
zYFY%!?YvOv#BS1c_b2i^_a~3P;qL!F^K|0V_wze}-~aZXQ9h{94&#^lJa}e?i$U+{
zqhsh05potvwzBR2eV!sJPpdx<Q#|uHXv3I|+`7aJ$NBH=R2byNi5~0d@xy3||6wXj
zgzqv5p%%8FkPFoP_AI3xpg{rbqG^J}_AnYTlYHp}kg9L%5I45*ZcoA=EiwxjLs8ZW
z4;yG1vdsOr*V2U_Y|>~D3##5zccH)<(9#`3h0yGvh_j_of9fQv^S_x|Vy7=R+gyiQ
z>7B#p@7&NzpjSwfBM@mUdF%Z1XLD<Z1uZEI&iqlz5z`eJ&p&_st?7V~87MMKe|JgK
z!7n<}joi}Cbn*E<fyPU+Smm=9@&)~6cjWU1MVV!cY8_$iXmlb7t=1e=<ZyhH7&fa`
z_$<O)(th@e@>S*QlE(So)oN4rQ}Pn8*@Jt7MY!QKp68(U+DV)fv(J)9SU6|J8^16N
z2-f8YgXTSc05=-h4nv6H*%clkd-z3i)vvK%zfLk8X2}sb@dI+JVeCD57-k0Vw)eqn
z#75<S7l=}s-j<M}E3k9w<&u<~`~@{XbWPe70{Y{9e}c_$&V$nS_%xLsqnI99cUlrY
zK>$tc4X`c7SNPyFe%z7Y6ZZpE`t~5hWLZ4T<4`jYIt2(EaSbA8+MzSZ!T>>k<!PHY
z4QS#Pb5<xT4)&Z_&I6$)6beFW)aoR(2oE*?nGqfydSB*8tH~-aYuU}7CfSA+N~IfF
zmMgwIuk+*@{g2@F*IYjlQHJc5h<MA_!$O}lz_CjglbEg|RNlQmffstg;^wubcYfXb
z#d;7oP^kX_^=jk7hp`G*r^8?Wyj>tR%^x;09_z7ogJ#OCTX{D5?nt5-rZOtO@a#7I
zWl2d%F}tb!7r#!;3=nc+p6C*XDO+UyJvD`>SE0z?H5QXA(GtnWOceAj00sdtN`hnx
zIt>YaORckkDlA2S%3krQ^-9ps_(DBoBi~y0LqGjX4MaPWJX?6R6_(2;-{JOik1-r)
zr%*sMhWVO*uKfnqb4bUX(Jr-#{<OVtYFat46*S`d$3>iaam!nZWr7!tBX4t{8_Ajl
z8b3p}P9B*mo>HrFR|F~rFJ_D%|9epMQE(M!lW)4OgAn_JzNl1@C(3(K;fh7biZz?(
zC|{<c0x*gftpiw`vf;+ghCbH?MOb3{YQ@bg(TY;V+kAxx^ud(Cb@`B>DzI04bN+7O
zr1ZN3V&@{~@>;!RUJxZ+g}p$PUvyQ3Ri?sxSWHC#5r&*K0My!v+x-0#$ZY>cUUf?i
zSdvGOw`vD`p}m^-<nA5s2Dh$MS2w;SO#rCz$o1WQIY?6*>MJ&_S->CY%7+LWq!|Sm
zIdSEl3E{K$6oh2f7)2HqYKRz8u_+GDS|>?~Cua}}q9r~ithmZ;Q}5L8TN(TDwu(^D
zwq(-MvZL!dlyvmOfGtBxV5{C<@NK_|hi!w)Dy~cH$79Cr?@}X2@oXnw?Y!L&cX70_
z$KfZ>?>w`oDpVTmYZ_1ig9exTnBt-qAKFcQw%u)KyNRCxpOB#~mv;^Gh_p|zP<AX5
zeR69a&FUEaZNNP^D>2uZN$e4Cfn+CVeYd7KwNqsKhRL^6293yFF@<^P0hX-|+_)_+
z7676Iz~@1IcN#V*Iz7!mOV|)|v%G$5qSm}B>zrf0zC9C!M?A=6bdi-~5Mm9$PaF;X
zX$`$17BcPsn&5F_(aWy38J(wqk>9gxbBT8y7uq5xR=x;(HSdE4{6CFxVmAVZ`uh&2
zPMV)xL-WFB%Y65Tir#Hwy|Z9#+fu;9)hj$Du=fa?*d%)Igx;#pClc++DJDwHl8&h;
zWPU3+btEQ(mkOr!S_1!{cOsG=W0g41nWPai0vconuw()>t&XGY>cUj5HmN5*9uNd4
z6BkMpbeW|=GnCE8EL}e!c9i%46ZM`+J7r!hPqPr0sg|~|_3T<O8<TD=Hh_JK3zvdR
zvM0@FuxPc(x8k<v-9L+uaNJ$ml{5YvC-{zv0D_xr(#*Qt!|k{FH5)Mrr5Lx`2x!z?
zIMY6)h-7i34yuVE-#1V>8z>^}9mx_JNh|m!?FuqmBhp4FsCL6Iw$}jeL_dM0Uv(|?
zC6QD7RL}L4iG8@a8RqdhK~(n7we$qiriS!)!5?Y5q^A|PMM#kN$Tu6Y5++O=q@Q9H
z82N%H>)Y7=!842xo>P8YyP++AcI3l{Q{3Z$9Q9Y6x1QYgrcFAVE3ggfK|7Y|pUivS
zgA7_uzd9X0vmr)D4r`)49<kizfuiA2=zoLr{Rs9^KNYP%s0D~3Q(btJIk#>O%SAqn
zJ99Be#aP22|23&dM`$JyfE{uhNr~`eLLu{Zp%7}WyIx|dJ-Jz&m?P5%P7gxzWR2Nu
zc$6j<ouQ5<mSl!n^PTdy{FM?7Lg$bLgH*npU(_#9_}P(QIBGm8_cCdmUC@m*juZ4D
zjVlQT_A>kujM1v==m)fNb`LF4fuGgQAJljXdTK)I4^j$_67QS#!lihe(9z`64;DF|
zJPI??$3bb!J7VF^r010#9;g@E-#^ZM_vFzd7FDOY_i(UAr8rY=bf~&OWm}3bcF-z^
zZrE1Oq)_<`*TOem@S~iaEsrUgW=gn_L1XqlIq>zx-Lp54&sd9S9eZSb)8w1S>MovD
z*QH*92W@0vNMa)zq@~CyffgFO!U&Ok$z71TE`3jX(D>b=)(3MPAc)S;QIB|dXM8R)
zJ3ytb#<<VB8sIz1eTAI*eXSDUr`D1D<M`8SC41tCT^40FRnG-Oa4AJ3b6nc}Y8l*2
zS$*82qzBTb1$Mm&%nkc}=f_{b|LK+%>z4z3b<`OANe?ihh?nMGCT0@=3bI@em{m3#
z=BB0<MzmAPFp+prg(5!xTrAs@c^D8>_63!VA4R38?HfX01!GDBUcdR1fehY|Gf_Iw
z+NAk$1>ogp-gtIn_Dzyeh4kK;*aulxn9<?vPRg-Uw~O8nJbikPCGPNiGp9oyXE<*>
zBJS*`afR27h_RdA`rKo}-r>i{fq!jZ$1Vrvah>We*|OZUzJHYJQn`h^t@jHpwsC5C
zq=gE+q*k+32ymbcP1D7hhB({k2Wf*Z8wapfJ8C`TalY)5>&((zwFn4<cpm?*Rrt3L
z%p`wiORHJmT!X&=s&76G#)>beQYJVPA=b%=3vfILaON@FWY>Xg&vgx!*6snYpuYO{
zpEaI$Bas<YJ08g0P~_R-z#^<9qn|`G2m1;zr6h21)eAg5dKQ~0>Csem%vsq>Ni!Yu
z8|2z~NK%Akn+7Z5;WYer?ARseo`)2^TkI{a%F0XTH{jx(?-Lfilb>Q3ch}}0Wciua
z<w*;DxcAyV#d*rflI0V1!5PlcXGxV<AhfATjSbQX+!5uvtvIgxlHx(}t}<^ij7(C*
zPs^f$`m+*?hhGUSFoe$jEag^GdTM%3&(A=nv#T<9&sp-Xf6BeO&Xb-c9%b9xW2}uT
z5rQvF_QKG!vBhc(2g~9eiK%j(eO+6-FDQ|r!BrZO<*B_&C6O@&U-iI;<3U(gnyfI3
z_!ZMM&bQF%9DuHWKKFwZZaj~Tw__mRV3_Ni9^K8z@xF|n()m!<A%<x0&F+G)r+e?p
zJ7Q9_Xmyo4v0}J#Ckba6Z3#k`aOur>Li)!ijJdz2qK#`O7;4U6$vruH{r&am5Wwyy
z<nO%gH`JHvuN5%Lzx(eTZg3U-ej#zkB{1%6vjO(h{@30`LdTyt1=dKt0P0(y9VV9@
z#xeL4k0r+86AuD79#pL}vd;*&D<npY_eeutfy-u+-JH3O7cKCvk1cHG_$BfMwMb3p
zkQtr=`om1@c)Rn2Ll(fmG4YJ2btq~<>zX0sH7;n_z9uY3hG*TKME=!?GU;5%eKUWn
zGI7W`-(P4LEdjNlUjTS4cI=dxIE$)c9}Ut98KPNZp-~YUN{XV!U02;dbtOOkF=h1H
zWy5OwUXfV7j+QZ>X|&A-GwMgFyI>Ym+yU(mDxduZFVlsD&`yauQGR>7>*K?rd<@)X
zYV&9H5=IHO+{i2|kd_t(b$lU7in+}-PX(a<_hpZj;o6hUgR50iJZA_v)B-d}h7XaF
z>P}qQ;8Z=sI^?aO7Zsxj&mW>Wu-ewW$Xu8pw#k@f0Xw>j6>l{jt*}%~GJD7VQ3uAX
zeD5joTba33sAlz&1QB*myq3Mfj`~FSEyDOr$ujZHT6A$VTm7(m4nDe}J~&OY1%5#X
z_mUxFXgzFi1}D!Az8>V3ftQ^<3&KRLtj#zt(zpM|?)K-bBt6s@^@dk^fZCo_31G!U
zatpqFHqsc@*Pn!v0W*=LPyKS)S3nJqrCE!u*9u^pa7>5<x5aB(-0SC)=jEU$X;GR7
zAKqcVu*K`&xBBopKxht#MZThD6cpbdEa?cJxp4c*40t{M37hJ<cZbK<{|P>L`t|!a
zV%qQDfZlDdV_Gzv<231UXyr@#Aq7GHe?wL>XBdeGO)US2^Gl4d5d`xGU@)B1n9aa1
zO9zT<b0Pd5GfANRRdpEt>z5Hg<pfRmVPIe)2E&q;a74?DCQ@lyXNd_0@!;g5YYN81
zPc%*1B!9FMFa{tkfTmI*HNLQ;QmSyFPIN?0Ls`ork(kyb2+pyI?(w6(7H*|k>d`Ev
z2}N!R##lkaThsVqi3w@E`0%VWK4^Gx8ec<V%X6#1GW2I+;U@4<b|QjgX_xs<u#Tm6
zSiy4YeVJgCZcYP_lSUDlRiJ)hd#PYH(ix@tf+Fkh(ZDq<;(Si7jLBI`5AxePfw6;J
z=hRi5?Oy-<TTa?CUVAxYedk6e=*q*@(>|p<d^YG3er!Bx`e7Vv2=PS#D#mS2^>s*1
z5a;gDB@IYBa7mi$wY%KhI49`L4Ia&VWF&gWzwBvXL(<?Kr9hB?(F2n)7}-1p<(np;
ze4P}eR&M8}t+r2{n;W2De53-(n8Gbu6$Ac;y%m=%b6&raZX?_%#cyyxRHBTRR*JCp
zd4u)!x(%;oW&?N{AO6}eOln@62cGw2a#sK{7&%H(u$-J%OT%?%ST8a&W(V*MY}u`o
z*R1eO^6l02D+u!wVM%EN=r}=9cRGGhaYqCVDEWy<*8ze(zznf`>kJ`sCla6X&7IDy
ziJ01~xD*6S6PQR4yziZM)W3FiUOoG()$1VR%#_SnXl5R_?u&bcfOAVXH()#W`5fJF
znD=||g5^K*{kWZ59h}!I3J-bX6kb0*968I0<vhJ?W|DS#aCu1ZCe-!P0qJ$QJm<(j
z+82%?gyq^lw>GO=UxI-1Kc;RVB6t%*Edb8izge%CPqmqO2_l7y0EgBUvvh<A+@LSN
zz|-9LW3X~d;lQ0YO7^Dn4XllwW3bI{Qf8xEVZ%E(fB_vM!t`9XV0+)mNq{@8ic7cl
zhzXHEn`r5cz`^l_0H-FzT2$6B*N`8}c@1JCiZISKl)^3bLW1&CE$<teyrLjK+s%+w
z`zdQz$O~3*SRElf+C=~l&LgK=J1&e}V--17lbCMvllAem;})54W>)pBUFGd!Be6Lj
zDBhsZPl^#T-sNx0nNTDZ`nP|*2a&YrO;oqLw-1@!@;&04dXeGYi?95f%|KppdRa&Q
zkI4|eq$Y9pQC|S?7>`dgs<*>BmRFV<V%!z;*`S$H|By|yMTIr5Zn&J#mwh#>(_rWJ
z5EMxdmu;))PJ=p?G0}_5mY2>IcRTZ&9cXm4-k(-NE~VeKLC%J66mewOTUL4!+1eg<
zi&LE4y?o2R){{$=v$0nSyrfuPKDq$HUR6pfkr!H1vPVA35-_}B2E6RK7uf(o)6DQu
zPA=ZJZdlgJ;^L6l(%voPj*uinRLi}VzPd33lOe|FZ;w}&a4etw+vu+{?AO4t^#(2+
z`e}@8KIPb%5=;F#quc4e&#QH$y&@-UrxpL`?(qH*Mj;XiWmH|~p;9A`yJ@1Leb9OL
zSH-SE!c)@E7Y)Duy0b>=V}xyp7e4)Pcjmki!!Y!sV8QK^C*pzpi?QX8M#qlzo2|zy
zwOD}07dcax$~??A603LTqRAE0gJLdRrc2<P3>Wj5hQ;dDSZpw)gg<u|I`Z?Xy{8jn
zI)V!~x+U0|J;^4Zj!6@WfGXK}*5)XNn{bf@)eC1mW1pw)bAQajK2H&Rl=5B!fOccw
zmh$F|A}+dNzM_$yKL(UMZASukvD;=<`J(JFxkqoLtL)eFdSDk{H=f*jm&aSIG)i#q
zHNYNtlM4JvSGnyU-NKKhT<Ts)Ybn+1#>={2=6T9Ay{z!Wm4oJvTJieq5pyKLv&^Hd
zYyQ#^i>zby;DiCrH@BB)TT`@;toq+orT?B@J>IGxVW|lyML0y4n<e9a)~o-jDz!#&
zs?+CBR4T0EfwPzr+`9wgpgFcff;JiMKPne3U8XO-NReerxd2*|B3JuyrP}w#uY6mn
zwqUibvD*rZ>*Si&BsjbaKY(;kG8C^No8z*6ts^20<P<LG4D3Og`(%IAi8`OTnff^R
zU`5bPlb*%nji)1~%#g|}GE}g(qmD7QA^mx)Kin|*Q>+T&LNc}*zBt4m&QOJ&ga<kz
zi+(`@#Y(_5szBQ}q=2QyR?y*xFoot4s3Cma?TXqa^&r^%WhKqD9Jpzqm1{AB_yNCR
za8~Uf3&#ZH=)3l%%ii)$z>lfF3YROE_YQ{soYxV!^<@9wlhBct)WI{`tTf0k-{|P|
zKKx#XU~_S-v}A?IjlV--ED|il+)fnZz`@s7`(*ieL%8grwmjPiTw5(=WFejd7{_>{
zQ^&0Gnfb@&y!PKGSP4Q27Zg=gzyyp8_@B~J*?*OmQfV+UjTjS(Or;Dmzx<Jei7gBY
zG1G%QRz`M+J`p!-NnTY=VDF)kC4Qye3KOXH+H?E`OUYTS4fM1{^L}2C4699=C4#kz
zGc=ma2|}-<8DYqvX6ny_H<^JQ>4oN8g!*)4UiSnAD2Xe_kGb4|Z;z$c_LF-2P%n+p
z%wOF=-o|St@#ti`{;-P*Ecm3e?Am_`bI;lZu4v|oRS^tw)<nD?WnP~<)_zfa$zVNZ
zjWu|!@kS1~26beVMcu*EL=y|T-e^^--+OuO(!(jHYrWm=7b92wf_748@j5@V=9bx&
zgEU_R@M(!tc7TNh-eOsqnX5)BDNiYq`wt&Xz2aRWH{*~|&F`7MCcG;R42VxOZ>CH4
zfjUFhnjD#d)GL}1lj)+t?Ru$V^;%$&*2f0V_IiG=fF{{}Tta6jMz=6S(m1A}sD4MT
z57bbz&3U-@o4=t}W9n5^Gn$nw+WN^9J(ub?7gLqm;?D5ZiTJ$i=<!OOTTIG=3YvMC
z*vZVEa_G*;&!@P9v?x&95s{x(cAYkC*dnNPw3lI$96;4Nu^7Paw*t*WnfN;DSj%37
zzCs0m@2vg|6$?n}sYBP+YdT#`d2Iglar4<H){ZoR204G6zi1al;lG{CpI+aaaI=#{
zow#kAa{oSmChwC^-jjm88^Yg2&||{OLxMN{o$o)<Phec78$BO%h2h`jr>%MSBVS$!
z9Xrxf!kpwK{X$X>51!yK73{QhvGbU$+T?@&XNys0+kBc5q8}OWc!rsM{HQ!^w^(X>
zjy~DHOF;svhkMo@YiYa^+V-rR<lw4dxsOb#+XO1{$+|XrH1>7JcGXLha6SX94ct%<
z%LND0i8PD3S8W>bqK0Vcpl{7xzOSLuj~uoU290Fo1HAJSM`;#<!Z1rG#Wf^02$fw3
z`7{XC4s*FJ8!DSGDa<3X=(5|JBD->mT|PR#Y&u|=>+=cgsCAfUW5R39Bp~FSu9qkM
zDb={={owX!^rg`jO}Md+q*rG&az<}VQPX*hf{t)+(#(O6>*6NjgTv^L3*bfp*j2m8
z5=*8MWl0x#in6*-(>Fqo(ivqwuk#VIumEnOjK%fRu7J;4A)4=tP1X`a(Aid{h|+F-
z=Sj6dQPuXj<p=|pM{WTa`ucM(UVW)8jSX%dz9HbSZRAovE9FU_YuarpFZNYJ_TZPW
z{x>?dLKZFBeQ}2#GVfd0_-7H`h`v&F#S4RU-7zy(cQs!;)F!x)R85MW3D9NjK_|T{
z=lVBtC)@#=V2P`?1J}7=&MVBZUo<0G@38qS9$yyNI%oY2!1EAsTc6G1ZhxoMTb}z)
z@r<ok4QU|p!vD@i8yn$m9w2`#CB1(1vEkVMr(WF3DPi_<vwREed~&)y(mdvYD)?Zq
z?=?-}OD{v!vL5s_jWsk2eqq9*Z>6wEtRODZ7~ZAPmMJac0lSv`ahPv7Q>~152Hhcf
z?;Orl86!%*+Mc^8Ah;5h+kAbPfXGL#J7#m+WnDmKxTlQq7S}EPBB?C3CS^~44|*PL
zeeGC<V0iSJw`jP#dl0z9edyi_rN(ITh1HkFa#t!Jc{&|6Xk8lS6TTypcAF<EWofUJ
zszunepU(LLchc7Pp8A>J*kJX#T}0%wSx%vW(rK>aVXS%ck5~ti&MJ#Qf`xZo!NOa*
zSH_xtmmYB`{|o{Z#hGO)oA03ulw5O-tu6x$7=xVs<6P^KsZU0V=meagBAPXufLRev
zmZ4(7|7Yc5Ovb(n{b?rlk%c<6!g;Gi1(;ZdJP-7iMAiAhBx2i7(G2Jj!BCIg9njU6
zPc|j7Q-a9Q!@?oo#d=q9A4IX`<`PDhoyn*ITMJL)d{6yvXz`}AN5_Q20o2$+Xl3JK
z-Zihd=LDpPBV5zQ1do{wanBf|3}nCmFvIOZqYZ?+m^W+~p-&PN6x{oHXjw+6bfa<u
z4ju>})=QCsAzz^995M{E_TkaF046JMvT~x{ScQ%L&(Lbd%CKMe!Qp}R_uH+xpEzAA
zlWhHu%TjDFk0d>M6%%qr4fCEBcQ%~yxS08Qatt7(A##M;0Im`%py~wRgmCTgC4AC%
z(%R2x;(g(#Pqu;2`nMGBzkd4ZpWc~~{lB%jXuDr}mQ8fDSUC3YcWfAV26EoAUpNDs
zE3lu0Bgn`@M&VInY&eUh^9&6L?F(nlAF{}@VvV=Yv+H40?0Lr*T<!@GH#uYf;;ipJ
zU?mdyVLQ}B1&j_wO-VwC?Ga2ElZXsvF=T?l9cjwZ8k4xG(g76^{*|)|FasdDfGLMT
zQ?l7t*Bhd<n>4*@<R-zJprwf^e^T>Dn_qxDMvM`cz(|XXUhqp?y`Tn5Bog@WiNyq7
zN(!}fYVJu4L9>?7SnEJh-w1Yf881L<6HjE%JSB01U`@YBbUX_su`1X<fDw;LXOySv
z%wssOs*<{K9A1v{rt4SEwTo5y5^qP>d?hoESv{8GGPeyxt9dVFP$*)Bple;$aujh{
z>rB+_)#%OSmsiQm;f~#f-!on*$vld5n=&7B2Y475Jum-=V^$KYlli;YSrWq|u@wvX
zn0b+c{o)5O#%<R*0CX0~0xMhmzGu~6*%y(3<lpCss~{{$n1G`%mQJ^)P5z+1rk2$1
z&Tun_#5!=ZSnZ={D^bPY({EE8np50>Tyth2HmwbN-VZETtAEhb@bycBgylf-3dtnt
zwsjLeAK89|_t(-IvGmn00a5&pI=#O<=6g(CC%+7#02>(WU<Z#IB__1PhKY$BoZ}He
zNuuCR0-X$~_Gt?G5@6ZGF1)D|N8aCpsu?A9VMP8j!^D?6?VR1N&ncb$B`2xZiQHP$
zz<~9glIGFq<g+bqPVa~lhI3zC;WM5G+OH~~TwaeEfPQvGR9#}ZV9od#M&c-!;Eg}X
z3Pepnn^?c26)!9w-_Sk93jBR_XACK+#Itt#I&;T>QcMfKaPT9K<D1^?Eqd~j7VhlV
z<9MGk=->ej&Ff@D{A~q5_ev?Pd;YeEWS2^=_goPGk?>4LDAz<SmU)OUnae2g)rdZ0
zE@=3ZlCFjrS%lu(pkc7AVY)G@WH)oS7i#6rhpmhGMz-^Zgl^JY$i+7R!YF!MDt$E9
zP!7uwQHF*^FLN=>Pnwe;F?*h>I=Mi@7TAK!O2Ia&dmGTVS@D=`69}S%Yb)px=VYOH
zg#Bpy#qCtxPlzNX@0jl5MnQWeoEiWv#yT85Mdx#7$Sc7g@j`)uQ6nqGl-+Sr+>VEI
zPZ2(Pe;~(OtI(#!YHGQ)Yda|EQj=(sTrux1D-%6!mOV+Kn74x0tR=e&aoZq&(?H0H
zA)Ce5l#8Zi%6&^)=%5g!;cHW~h*b9pZ%TdWyEx6y#1JXVlH8d}URYi|?>`%r)OiG7
zT1RjO8XVmy)ckn9)Stdg)b0(yK*7N(9gv-ZEqO+B>&`A+!QU)=5}0OY@J${l7??eJ
z^u@4<^dr3rJCKL3A#R`$)uMbO@Cyjja@#C`O#`GpeQjq&7S_r+f)WQ7c%d=ue+(~L
zOxu2ghn3Ny_RsqcfKPgK9b^c$`l(kK7CUW)%=re`-ATauNsk06Zft3%6>0Ff%;km}
za_4#t##A&4UT=SFs}tRFnnxdbGdK7TR(<f_8x~l+>hVv(X(so)%wG@SO0-Wdz6PK{
z{H@&<PX;;Zyv|nfw!z(ossseQgZ>nv)l4YP7^$B@&wgBX&^bG_B?aJ{5UvjSHuCS~
z?n2ElR8c)AF8wxZ{+5pye85xRU^e`nQgrhCFSc6iN+qbaK4GlvV=f_=$oy_K42Hrs
zuGH*#R!(Hn>jg_1>hOxNm#bQr5X_aX@;^6rwO1VKfyZf30UzzLqwLzbgzttT$KGXG
z!k6TG3$gD!d<VbaF33h*6O&$G%+Fx88>PIIv;NCeWME^vNie{P6;>80&25)|;*k-1
zFPRoW_B35&PWI+Zc9Fiy={ciTvTHXK^$c;pb<#(cU@%+7p^>E4{_0%l3wmUMTc_a1
zZk+gcbIt#|i2ct@Q-gg=#gP?ussxppSg}|tx)muzWsZ9t;5XPWaAnYK+w>ST=x|*#
zcq7kONM@2tGv0ibz9D{%{{AfSD_#svW0bm3qiABleTS`fs63q4mA#TaV$^@%FFS(C
z9xy;BP#U!YfDot83KyCTpCQ@+A%|Gi?o~wSsD1ZUu6th)*PVu}tjstinMQ`_4y>A}
z`xi6Tu+^$xLK?z?4a&vJ@SKBij92k%AZvy6iUA3l&jxT54$wx+YGm4UA?k;uRfGEn
z5Gar>x;5+*kCS@xYrjXCI((Be$>-g!v(-9acBsQ`L3ji4d!vKuMtTB4oFI;{+HNK1
zE1<u=8Qd5=!?QdEeEB|b@cQB2>0NES9|U~zAn*sR?g0z@Utg~6(!r2Ivhcsb2Z(=A
zY9C6B6`dO)#9U|fP5ZBYSf?VJAp&BRgsaQ4N9Z2Lvn3l@%{ca<6zz=JnhqDGX~*ld
z&Gzv57yux*eVrAafD#$N$sHstdCE@&_MepuK5R-5eJF_}stkiuM93(34kP+h+0-`Z
zx!i0)d@DGMgzU+XI0k8GW}u=!0ZXH-sbpeE7c9-{qrO_2oOHX;1v#p+t`{xgjE=GC
zd_)A)F`Au(Qu79M2QqaS?MJ4@b<LY#*NzAStZ!enkR|9HKp|+h;8l5Ox?@MF#4tw3
z?05=(LVST^_Q4O*m!w&=%ii}U%g^2u@9SuMAFu1sAQB?CBj%QDy)1ATvf7=&fmve*
zIigWK-g{av3928W=kCnhLpx6z)?OtkKi>pQcXU>7lGJW=cWk`4$(Z=EL@I2#iy|ip
zs@NPKpHt($_NvDls$T)hNd$<vIqqq;#IN=Suw9+?!-y37tTxb>B{0*~^$=HID3`A@
zVQ37AlpUH%KfIE~Ccy6*jh5h#GrNBUCe)f*htaKij;<f?XzI&Q8w-+jLC-akGj5!7
zX!^WD^H6$_VJmVQqf1EMu}8CYB{1xwy_(*z3lj>SKcg8T7Pi9&No+Q3Pl)AIh}_hS
zG8}C46Z!9}B2^JCBA4_nXo|_OW0TQlo?NDE2Kwj7)qpX2v=+~I3An5^Wa0i2;q%nZ
zg>{t##=yCs>49RIC<&NI+YfwtV-UYZMPs>8qy<8(Px0JXpl+zk{&(l1GWd7H)*1IN
z`r%KWf1mi(&%?DEUT#hMXOZU}!_8Yy5B^=g`_WMT+HbT5@DD*=x#35{PTqm0+A#k$
zQs8D9IFf|8pZldXDaDFiWMyQBa3XWV_Z~;(K{su`C%voSsO_XYDww3VG~<%4gKQ4j
zE(V&_KUW<Y9Umvth6C{Dsx{q^@n;JRmd1aSYQOC&+3`WCb^ysSv^1AXTdQ@W2OhyC
zUbSHb*s_V4pL?#hotoDXXJ8PB;4;H<VIlSgr~$Q(IDki~4WQT6QRfO4f@X{Zh@FSI
zxFamr3Di-V)w4MI?+@-r8hP77iIYnF9j`X>d>j2kTT<q*JT#21(<ObA9|r$UcF>_3
zpJ(U#JK}hHAw{3URpiqHWhqe|V(h-AQ^~uecLc!A49GaE{JyK2g@l|@>1aI(8fVkz
zD$}FMthMJZb(Cqi*;Mu5p+hXziO&NqQvxa_v)~xkRg&xB*E-mZwQ8N7bHKRn3;FMw
z*PP%gw)sd@H{U`TmdQVqtNET$rQW?6G0Cc`71i#+u(jW^ar8nR(r6*v+l{)bUt=;k
zzi&7SWJZQm@!x{S95t`+-I93&#N{UJyyJ|_kFZWo(nPboP5Fq3fUNQN>H!dntR6C9
zYzBVO4WMD7r%&b@f~T2aFDU>GqOPF5QO?tskz+Ac70u<!IoP%7M>+c^4r&o|pUls7
zG@X(2&%II}v6tk4yls}D<FqyHHvQ>tvas0z*JIJM?0oSBA?A)eU+6YJl_rM%5jfAO
z4{g%szFDj)?D$*K`%`v_UZW0b^)8?9*{O5-2UUA|Jz_BUOi4RDb$V@DN#A|!PN(=V
zJGM|KIH;&Cse9{DfuBpL%&9aA-Yu?A%T_@u3$wI(A*g>q!?qu#OOR{^YkJZn$y$+j
z5EyX7DeV4MB4;MTJ;@DU7bGdkkQtaaB8lm5cGUx#4>Ob!m?+q#PyWc91K^?_jexx*
zLO|5eGB@R%6J|RWEnU(o=XdJ#wv=%Ypv5IHu>qpSjCtOI#^TpTct2>el`N8OnZD;p
zmq9T+=pkmv<CrBW1|;k0!j8I+w<SuW*4&g4NeCx!)R9fM=b2CTw5F8P;ic<hynh|G
z*M4NLQrEe!(mciS*(?Q-u2Jv(-~04Gjy#&vDzGdf`UuWoKQN1VADx3*GRLidh;g@U
zU)WB7-QaX>S^(%gJd|x{6U1r$56RD}N0guMOtKF05E`$=$(l-sLQoYP2w{+pls5=O
zzy+uzzx*E83>TInQ*o=>EDI;aQqvmS^ilEalBKigNJlYtl+iVFQlY<(IFALOu@&xT
znlJvYestq`-QAE5<$MMYmR&nUFGtAe%6BUonmq`TmYALC^-BDEw;M8N-RFoWrWEwx
z!5}RVsUV97%}>1=I4YSLMT>EFgY!<mcQ2ONxT#(NnxG<rS!wm5Y`ZFhOsBH79Q9Z!
zF4ipWRjcHk1pX4_hl@;SJbxTyC=aJir1UiB7M+_*db)#q>5doqzH-_fO}xExKk^~R
zjUVyCa;F?4%%8A2NVWsIr=Q*p>pVFxG0d~}8#j>qB<TFF)bn4Yzx{d=Is;1{#3XY-
zhM2wCPIT)`ml+2yoH8*#xt*m{I;7C4$$DNPNbPjw>GX@_CvaB<z@W`e8afQEa)D#h
zT?6@y;OrcHV~0yX`e;j53Tk{|%9{qoj)VTkH^@r``+-mhiiMqt8k%pVkwWH2K<SL@
z3<g210@2ZA(*($b?97g6m+EZ9=oDZIHTxhBaFiDUmK>!wv{6ug9xR#@&7p541yjjD
z<59ca*54w;qiy_32Op8Iz{F#0{mM>L)N1m@mx-yK;-Ta~GBk+nySTE}-2lqOlcUK3
z&W!{yxs*nVQ&ajqRV{JH=2X-{O1>?B6KG4QvPjm6i6;=8ZKwDGNXPA5;#w{JdLQOu
zHi1*;+F=QN0*_&5+WhTHU02aDW3_>(tL$er=O~6_ny=Y2E{4rQ4Nq>M?(8jnT7PQz
zB+(<^84kr?RANeZ^}ELpT=FxL<dnVBEuiI&L5CYA@QEWq?^5(x-M2s2;6Ju<)Wo};
z+-b8aZ3xw)!S~h6z=+vMmPb-M1Q8}(RQ_UEkmJb#kSHEN=BpQv9VpN0*xA!dkuVm)
z1M22MgnOuSXphD^e9dUur*+%3yrBKe!j$?Pkx+O@U1eCZT1MYWlbSsRXeOp<NsKt`
z(ZX#j?&9D*ErHDnDG&7Mb(4>F^Z|g+)Vu2`3CM@DcbS>M`A9$muK<ZA06vXO<VWNy
z^Worg0FyXA7ogFyNDllBgWprnUccBy5-0%zd~@_@9+NB3aIVjrbRejBH+3idz0<?;
z<P-e$yA$z*#95`sZDtNX9zXHU{-I}ecjBy`m5gM@-&_?T{#$v127m82{=S8I39sbj
z0*Ep+9#)bx%}YjqHbwr45u#I`6rASeN@*vY*@6sptTWs{J(~mt;UZrvA$V`GTM7X%
ztRGA6Wf{f9G##DVu2CY-(M4}bU%`>ln)w1dJ@X(|`H{{SWws`c=910Y2II_djwLG;
z(;sZ^7=p`l2$r!unXec<-v>3j>}UX|+Bl}LoQpHIRq(ameCVtz0N5n}0prwP_ypQV
z0PI_ecHP_$0(#BC+uX^h6=;MKcZ9d$SE|K8EW;rO=QLMMZc%<q#TL@J(Fj&$*igP)
zz^MBvD*0@(D1ldL-Rlqzn81NC?q_nCD)R{Gbc1o8tb48y#oO>+n?VkhA(Zj++qIah
zilozFlw?&}mk)8+=UPxvfh<b68`CG2$Wd(b#=lmTi9Opz!n<WosHX6X)XGATFS}1^
zZg;@!Qll1%<%kd7oq&5b92>>TkAxQ&;s6rVaoW6&L%c3HCu^>vBfHsZIYOWH*=5n&
zT-|w$#ARVV>@{Zd{ydnv5mTvlh$MT?qQica-)ElCDt3+Bk}W$yk)*J1v!l5UoPf|s
zmKF*#tX*AZXLpQ6=LB8lp31-qt}@ljijhY)0qW<r3VEAz41U21D`d?aos3w9UvLM6
z3c8UwFH&t&JDe`P9Z;!aPlr!b+wDv;SAJ2ts=X_>E`$N%Zu_O^gaKzNnuW4OCHDu|
zdC?Zp%&GGCXmqO}*IyU171i@kpuKiak-?=P`vTVxh86Pqldz#*AL~$rq(6!U+7YIN
z&^veT%JRLs6s4F&7rR8iPw!e1a~oLKDOOliN^SQYmXr+v6CBFtVG8B@nC``WQ&f{)
z8iHF$eCN9ED*X~cmd!&#BMnh2qmv#EyV>^bpyEC4H-L`zW6K!3+16TernjzK%k@vV
zw+M(m^_wAC%4+#GaJD)lD}BP{TZ3KOCE2`_4|vI-uTwvP+9UMc%UjT+s#qXj6mD8P
zR4ayek1z|3N}KGBl+R#k5zFrlnDS-vuGZu%5*52KmmZ@3e&%zqLU=%R(!rQmi#n>L
zH*WOQM;`S`mGZ;k*r52lCc%3KRrs32tNN=FY4vH@@+&_IU#AQ2<ig$MjIcSA(Yxa2
zyFNuC9Jx*1Sl)qf&!-xdb`jp{rJrX3j*S${N=92}?fNR)1JFu^K<$!IXCoDzLA4?R
zf=_>%{byCwr?#0fN-)+;sx72MF9aq?%>=btMcKfP2qWID#;zr2#Y?aiN+sn39<mwc
z2^@7QM_x*0Sn;q7L1F9jBr846I2FSQM8sl)f{C{rlLMf+MSbjuFdK}sL2eW65MziK
zTe95xB`KC4gh%xkF=E4rm^51VhXe%^YHy9ny|NBNW<Lw&x|1oG%T!vtVT6@g7P}%O
z6HBL9hpG4{`g9uEt|6XFqX9l8asD#55iz?z9ku5EU6`2w1|)DgDyJ465LUPFckX3o
zPb59k%e^pT_y=+GYoFDH4a1XXK$QEvSP5Ok8gR8==Qhv9M<$1H@fA5~Er_k8c=Rp#
zTaP9_oZk3%S$I@^C+W!n^DpqR<@C?fK_bmRnjyH?_iep-^aeQOpG*%OsuPv^gO&e1
z#70)wkY^mRiegI|@vHp%4;R{i;2fs9Ab%TJ2LaVa@Z_j8!|)728e}^yl$O|zhEtUx
zTCm5+d>bZlfIYb!pyHhx0Q?|bF}2N1W|m8Y$~;gY79eRPXBPWwspt%qRpj&zhEao<
z3Rz1>`iUMDSo*2_(4~@{im+%qB#pknPWlVNLy8zbprJ>*0;&=;DLrKIEp!N3{7A%r
zm_{pNEi+}kL|FP90gYGP$t(kfqazk9>y4iCR`XNRTM79l)-~~H{lbP#6x-GrYFZ!1
zAxG-A&lGTyvK<myU0meQByp(B%B>evH+?3n`Zy5CP(wGs^&(D0Uz^8t$jiA-wKj0o
z?_Q}UvO~2kl(hKi#ZtRFeOIWm`n$?76p#64wR87A9B_VUaV8pBs5(`>p;pjT?Y3i3
zEBQ21DH`1DP%{nnc%jReMJyS(6oHnKjs;r?;r;Ow{)yAlly+xW`{c=={)$bgcmkl#
zJ`A&RWKyDsN;CbHFDGe8%K-s9>oUo$04=t;vl)V15@&r;_o8~?1tsaBi&{p~`|CJ?
zp0`k(^9t%*%>l??m+hHsvxBZl38iEQwKT?nuvQ#18tO>ahSE&>)`d!!NtzBHrlXXV
zSTXVx2^<jE#10)sW+@@6+VCR0pyJCURKjLcOIt#RpLi#rOcC!Z{_&aL^I33!XGTq^
zzp~F8WcCAx_0KyB$mEsmHIdzYf<pJ!r@y=KcbYjPgEu5CRWt~v^XA*XvFA-!Sw{07
zeLXX@@X6<Q{J&)E@6m;Cj#+u<b}vw}|5U2K0WM#EWpE%1)2KTG%nhsFG7DfLU;LRx
z%-6mET>%D{Wa;jw|4YJDuvCafLftoD*B`t&6-Cl87xd|y8H?Rl;SDNu+I2_`o0OB?
zesE@a=A66Lb9w!$vWKLMQ&WXLaeW+ADqkr9g}^6`8T>WaB5_)cs_J7>IGdNkhh!V9
zsx=LjYL<38?WA%P;VCwN$ThP|Vv3Ou4}jijV~8C@Xe5DN7+W+6x#*2>%T2jykYr;Y
z;B6l8fICfu&q=|#aTcoZe5c!!MW?{@u&~KzvCrG(jcY`<e#IhuU|H*fQ&Uj7;tN)>
z?X8~${(Tbiqcer8T0ZXiI%#cuj-#UKj0NqdNt)xrW6S4wa;$9J;pwi}k`sDw*~j_D
zxioS<SsloB)x8cT{PenhTuCx#xH7>G$F~ag`$!dhR}mK|$`@#}p`eaNEWq!f;Qs-A
z4o>Zoy`h;17)^QE6~VIX{QF^$czRp8??$rlq-wAoKH2Ih=9P&(BE+D)T0JUQgP?!I
z0Z~|oU~dzu{OBe<YCW!;_q{-{&=+mt3fk%c?@K+{A{P(~eQJG$Pz6mSacl>!chi%C
zk#(2c4=BChhrdJt8sxsG0D#TPJ&_sIQ+RT}XW<cEIZ(oAk(?WoZ&BL{G|Ss1>7+k0
zdUXxLcsnAp^5Kxs+{k<92R*0D6I<B(Do~Fvi_F7mZ#-mF_zho3p02FG2(SNSrKGJf
zU|j^5+|6@S90Hix{pfv~M$dHArhDO}G5?6i6}RdJ;O8@?PCfW_I~Dj&T&Ns|z51_2
zQE?vZYM0J4JA-?92$@l){izTYLqa(JyOGS&u;AY&1>v$9aJ;LGN0w8WwR`ed)Im6T
zmA+q;U45cUxN<3@$mTysavxQ5FDi^>4hgamR&RLuGiHAgax%g1ys@_e!o2xx02Uaw
zFr9%;R^>jW*aa&7t~YN+=eX$`O8U+!t4k0g>twgh9q(h!lFoOTbv}!|hdO^yGja|O
zY(xiEEfmHk?}eJiYkYdQbhkG0<ySNRySFJd920x|Rtb!aO5$ImJ@X4eV?COz)~Xkl
zK}nKu=VxjPpH_>G3gDY3Px76LE|>e1MY`OjJe7DnqOQmuOAYI3uc<ktAp!WM-hC_q
z`hUhC*#G)~;7-d0jl;cZ)2Kr7(KC!AskRcNIz8J3J(?A2rJAb{n|M^}-3c6~n^s5c
zRFz38r_YITFnJ&OW3HLGckHqtCfLbC8*SqK#<T2c2?Ev<IyNZ~cO8kN4=du(?Q}<E
zW=0Rqp-<}LEr(dc1}8U=7WohWj9{XvZpfq(%sA{UczcsN{koZXK)ub91O^4gGzwNk
zL2rgf8p1(IQHmA4$s}4fBEhi}EH0pyG)EP<9slJ^04-?Ym&y@u{4zdomf_tGKe%j$
zd1fDoB2Ij6jNad<yCcpt{b3(jzH?rW=Ed8gv;A{fBF!(7uKv!w`TA<3&jB{x+Vb+{
ziO)adwA{X2xjl09HSYSWf7^(^fBU~ES^d$lv^_|A^3iMSlx$`Ck@0bzir^<*ii`wo
z(5mD8?-5LuAQhq8q?@XjCnLauJ?Pg04Dsbmh{1*p=q7-d!GRiedN43yx;!3~OVGXv
z;PIl@MUX^(X2Aq@?;OB*uQ0b~B44l?Z5%NZEejylnE0o6WCj7w#Uqw6bR<qd71`m%
zo@zeFwA+3UpEr;$s5~qhjyBPP#yoA5>`51*6fuATQ|-_w^aWZmBdRqRq4h8TCmK&S
zK2`v2{i0<+Nk2nD(9TasG$~19_@*L$f9lLdF|K6KR0q6a0HEC!ua+@MP$|qXIr8Fj
zpU(Hm0-sNN&a|&O*q}iRKd~Z<wWz-7%gfNBP`B#tVOHsNZY=~lf&KBMxXI^6esJo$
zXuWJ-p!4)ahn?U(Zx?(~Mbbm#8^>m3hwKi2wEgT2IftYEg$Q$ATR+OiL!Zoa61RQ9
ztxW=IHDQw*oKAH3fcWZEEn!yy&P<bcn27bmQDCA|h%nVwBKBOxoI{BvQDQg4weIyb
zv`n6oJFUhfD(e8q{mAlL15iceTqdEL-=@@8Yh@Z_>_J1C()SKC%}CESA*Vo4G;K+3
zFah285N)(J<tyz>_S0!0$QBmLzHnZ@MB%+d3(8T@-;xgYwxCat+0@=TlGF!7nD->l
zk4Hkigr|@sDfOovb#g_`QL;G|ODgenHW^*Pq5R7z!RY712|V_{l7x2dgn+Lf!C<dL
z*4q{Y`{t@I(Q4fej(j^1J;M#yuc)vx7~YIy{h)WSefIP9iu)wGlSAdb!{0Z5bU{aR
z>DIYVFUwCh+-SXHf6#x+o8}}g7o6t)-R5kS1&7J$hSB^T*tW@F>-dg3bNDTv0zGh7
zQZZIAj}-%<y?oa6NpOFuY6YOjX3Nq<A(}9>wbNTdVl<mnRIKqHm*QK1T1b{>)ba+E
z^K;N5Ctun~kf9Y+3B8pVZ+inAAPqu{f;WNMx-R~G($Qa=py*CnA1NFR|0}2|DA|BK
zRc}q(AfD0sR$kri@f@e$^$-5>1;^d`+;1Q*ql(dD(W!TMBFxbZy~GEpN&wGBqF3G@
zi)P%^(X>psc&0}fE}x|rh0_SLcutU8Il$^Qbh??WyuYup^q6-vL8dK2(gf22@Ur(h
zS>lUvP`P)uabEy7H{*$GVzO@+I6V*lTruvo=0Yvdmq?-VhG;R91Z<RVGZxjp#iuk|
zpx_%Z=P0(wD`^|4iz@IdR$;UYWAQTdYZgrEbSQiV&>5=wk`}ev;md2E#!)#xi!Ir2
zoinyOD*Qmxq<WNZnTcQwI^?a%#oy)Nv;xwDDaq=_-%RLiuX$D(4>Z;M!WR4cnC#s^
zJu*?lV);^#RN-3^8Rkn7$wlViK}}=NzK_>S6+`@{HJ9}k?r~`%3%aT~(pP3`S7j?k
zv%tPh;B9$0LyQ5}+Bwguee=%OsD!B3^Iww>o+^*)eGxsRq@7~jMo!}76MsMcupl7Z
z?0r7E1!%nb(FJk$ePS!$_s0S6zi!&mw5YZ>p666`Me5Vvz?1u5L^-(!zL8BlzB$f*
zm-}ztrE7^r?URE%hxWvD7qdW5&0K?-UgUZb0QY^0Gq?|X!&;LcmUD4hZ0?R|oM*Q~
zkwPxs-4Io-ng*YGDJYqWT_OT@Z2;hkPeI6GO{_*u<gI+pa_nZE&q(IqEW|fU;#Hh7
z!?}}leM}UYF*;cWM0u%|r*;H`42PqeLq?7-(O$`%=m=*gQ~yY&(EbeM!jbZY69|@P
zoz}S_;<8Tc8g?1QT9hnogq;iry~}g+1FH#g<ei=ekADxg{RD&zKTEV~{k|F$AcbMj
zNgt%W*W`_!dmWDo^sScnSepAasjYak^&Q`n#gjyiy<4H<-^My1+}`kOh0mr*8m0av
zFmp}vY)ylo_ckXr+aigjE_Yn-sqSI-d4ipq0Areg$5Xtw35*H~@lvo_(o%)64(aFh
zAY))`1!D=o&&Y9U`eKtBTaC^>fAthI-v~nIZuohk+57q0LZ#I0J41qD6EcbqSR(CE
zIdSER4~+q3;X?jxjU}c`UDWMZpA07N)YSy;Iy_n^1aw3u^2!c#ih!B!l?2Q#OcKmS
zR>op7nCU<xR8Infv;f-#1Cs2B@x=oi)K&xjR4NvjO;G+?FGPDo60}Hq2E`1(&Za?-
zWM0`tnJ-P+(gFnWT^XTk%ds}KvFy;3)bpg<Jzhs`Q(z?W9>8e*d-B6~_j@<*dos=K
zfB!w-QftHU?(l^8c<#aNr~BZqeuW2&-2yu&rvtBhZtuN1dGbYM@u%*A!24T+^PE7@
z`8k1KpHWr@Flyh;>)_85t3oaoy#$CV(0F@^KXv3w7LqX9TCT~CRiK`y#->K~z%hl0
zD?DT*yC4m*ZHu4U%+`WYsj=;!(fE8?C>^rhlSvF<Q~J-%GpbR%rF|Is2ye3if>Zfq
z4!H!xB9k(J_(lec*g?T@367*Q>eaUS3320i$)!|5sK;<BU!7+$C}7gwO6BzK^1cEX
zj(DR`*#|Uu^*ZnGpsWQB8b!*G&Plb^qfJeIHb`@9g-Ua7jX|XawAK+*oLhzOGZ7z4
z=++lS5NGhxdzT*Vj;tgvf1UDon+o5x7crGFfx*nUGq%#40J_~yl&S4*2C{gQCe-EG
zm_n+nLs#<qmrT`ZLaRi`q?B1^&=YyLd@0zf6GiL*z!$Jq3pmz=(5AR>A7fJN3Fx$o
z$UCyrRs#>6nN(5gppcScmQM-jbumOO<?;(QgRYmFVtwr!r4|BS07L0l0i%iXrX{g!
zl@?1xqW%7h%TBK}H7ETdAZ2K%Lq@6JE^B|O;>><|nn*pZ!E_ZxME5CKbHJmSq$=E7
zA>B7D)ExM0EzPEGcZ>m$tV36T4gkYe1M)7J)3))t6!@U~n{#WIYzCX|I?5G&0LtT$
zt^-684tADE^6~}2o&JyUZ99SJ$@Zp4%Js^GNZt9)xVz>HL9A|JpejYgJ@s`k`Tdr<
z>`Rei4S{E=TbmY_Z_qzGsH|ig#e2&f4zxT>fA`b!?J{IgF<#yi2Y*t_5evPO`RRA~
z<$GLl6mPHn|Hags|3mq|;osM2Ff+C>c8QrW7&~KMDq|g6+4nvBmMuxrbQz2_`xY8&
z_ALpitTkCmmO_+-kV>@CcF+6s{oeQE{uizvuH$u{=W#p_gN~~)8S$P1&CvdvV)ubR
zZ`hsr43)L*g?}}@U)lO&?SM;}p4hkyH5I0<M;PQ2;XWDCQtfI#880C2YpqaF;oINB
zylMx9=^%N%pZI&UK29@um8xWI!wphrrdwU44!}aFFtwAF{7?{lYfVs{mz&+H7TKIL
zk7Quvq1Ne|pwa~i?t)Gyd~gIPW3S&}H(r5BuUI8B+Zi~K0B?#kgTW%)3hM|r9XIvj
z#*d#t)%s<bSG?}hoW6th330}$T`yVNJy*&u>I>Bhx%=I$7v@X$3&|$7`3PSY6z04x
zol6k#TYvVlB{pCXK=cK^7cFXp%bSo&>4gbTNaoDeJFN}{L6#VMwtv?Q8#9Ow*Vt&3
z9FK|l>z($02VczMefN=o(x#nZY4m%nL1Af*@db$$S%*+4u7Xhp`{2p6WS+m~BMIM@
z=_M68j65#GayW73iLE7JGlp`9M{k=9sR*u5E3<e)Ib8bsxzx9@6Zzta?yVb$G;6af
zNc|iGSJ(4l$)>%0_Am$LnoBoX+2x8Qge2>pyGQtHjR@o+*65f-bT-4f!US8Vigy8=
z_LFB=mD$`Dq0sCYP{;?abqgcnpPe(M4&MoA>}0-RC{xkiaRo=W#MYicQm6M3wN$gr
zM#RrcR$aZHxoRE)sE9u@yo&Z>xBp&(NxZvX{PV!Wb~PTV_2JPL%kRoxpNZGoIvti~
z9bX^(YWe2ZK6r84S!@@$Qn=ZD8u94B+Hgto^X8q08h6}f^QW$I2^BfmodlZpRXYrk
zK+Xq#D;}dliw|9n&Wiys{)(#G+qtq5umcwK?s4}<XRJh^y#M-uIJ%?o>M+QAveftK
z83hj~{gQLObUW3QQIrU8LrfJ_JPbHym13v|)bQ*S_Ul1Rd9~IW>NJ70@#@3^(yfcw
zPN($Zz!rBDulnOY)lA1TDvu|~hUZM3vZ7Apy`H}=B3Sm%rE@;^aq{0>wyt@v$ih(m
zx&fE!@jN@(OIG&{#uRZj|8g^WOkTY@jVs-T=CygtD9+eReqyr9FVQyevG3&Gz(6xD
zx_FKkIWy5iTe}6Fv)k?FQ^X5z=4<qOh&e(OCv3S2pu8bnM-@^2c%bwN@>!{L=I#BY
zj=YfFM~Z!otTvsH|DU#bTlHFq3UH<vdY>m5nxX!uZS*;$8pGp4oPptEjii#3)}Bj?
zaBuJxk|k*H1eXA{60)cYeDXVw%uO@As%92b8hgb=b$8COyi3r?F_$lna`saW>3wUh
zO>vOGuQdyyR{C^F*ypwf*q(+vUA!=G8C{G9jD=!B*lINi{~DnUrlYGH@NEb=F!^nc
zrkf8uGx;1DlCjLIQUpF;&0TN^_QQb|>q=&-PsLSc81aKCHzbBxjCf(MlU*PLI$+p}
zyqkP67I;1U=DMsHf66`sf)(lb)v=iEJz1>$Q-EWkJLSNug2o#8>&14Q&TEBHfxh?q
zM4i2dCg+E^zYq5(K06zGZ+Z9d%A2!qUj6)sIh(PUqIVYZ_s77r!O^=@Tr(035|++C
zriUE`!0%pCi`>r<WWNxb<}^h&!_XQE`!)1j<rgQqW0_GhFrDeiIyBC1&wUGxLIAYR
z8GZ<jrVIAc1F=)$Br1*q0(3@vu8-jb228tr*ks1@EV}yWT?-J23A)8CDO&CxM1_RP
zL)ckRO+T_iOx<25KgGMEDP27Mg7J?Uo#)caW%6V~t8bS8p0f38GQrsTeVI4qux4=8
z#>`zVxiQ@*BB8o_4OWkl9{So=5xj#g$Ec>m44SK}_A)qTS|tx}snrUmIMC&F(;N!N
z)1=3vNf0k{lG#>2!6I*4d7M9Vo}E^>(|@@&un7;!KlYOW9b<bT?p10vF97+cs<)IB
z@{i#tD<t`Rc_yk{cK?KJ7&W=|Y1VN1%R%O^VC#!K7}fQ~e0BQjAY>`Mg5L5I^!|Qy
zFL9D2pBz2W0&Zvdyo|Xcy%_W@h`iME=tLe_FrB9mUEg=$+8`y@DTOz^fd|jfnAgC7
zKwEzx;P4mbW|u{hggT9saFl_Cg%?%5qW7y$)nrcSJ)<#7i}O`wP>D75M~Pl(cE^PU
zYb52>Co{r0<<-m^n|7)}nLtcOdNnT=eqNS}p|zD~u)Hjh>m$H}Bk1-MWZ53w?&T`^
z@vopF75f`hf{8;xC8{_(s0<V5(>WXbDHH4xX_lNs7kfGZDwFSYOK)F0GXUPv-hnd;
zMpRs1)2gn5uP|Bnw+5<8g5};+$saywtH_pOz0=2e@7L9%XIx!Be<9g+DE&&ckjSwd
zHy*wLF{y_Kzuu^G!T(&1iR2u4C<ozAeAB%1$@m{*@9lBp;0jeuP+Vj5pV$%BfjTB~
zy5tz->~AO;YV`NvPnjc_8@oYA(05`?(6Q%zM3W5NWI5bYzd(VssUcr^{mfQJ-tZ6e
zC*E=bq~KzkVK4s>O+a#PvJA6YY1yF*s1)q^tPo}WlDSXk7%{Ez3c2&AOBglG$SA*9
zDzN=HG&XjpcbRWdQ-1stK_gk)$ezgJ4Yg0s(eEZQGqu{Un>yXS^E0WWiut+(cVZiJ
z<cT+zw9G<;vWH2&bkxje^PJqv22WVU*5U9-w%h{#4avI&W%n{#>h+W*XjTiO8Rolj
z$!#xPr2$AxK=b5svFUL*%<Rx7>cSp>sdlR%#{C3$3urtI?AN|SX6`3Y$5Ff`>=eB6
zc96G_G`AeTTzr0Af3SrTaJMAWyab1jB>bcrrP}F)aJGOd7s*8SJ}6VIa+P7eyUYnK
zHp^=IBh?w9lWDs@F}7Q@J3>|g>~G<*mTu2c4#A7BAk0}R2^s26qOVNvu*WA~xLe57
zZ%@LU34dM!YBaVv(3`Jf!dcSz1=u7+bnr!lh2-9C!*!T!9-$T~fVA==61A9PN0Gt<
zrp0Fmnc@pR<yFlI2WSD%ZGf{sM$Ny$B%S9@>}Foly}J>?O?}Wf`=GVi_t}6*DWU{w
zw25>qcl1!2h_@?jkl>4rd)>F{W>0Tx)A;7*HSz=7-gtO==fF*N{tEd8=a6IZR@$57
z6!Q5lR>y-6tjz;rWIVOx<tl_Vd+pCPe*VG5i|>xPa+`GVW8qc5VER=YnZ~}7EYA0c
zEMd;FU6iA_`~~9<y04^p-SbQBszYr4K90FB;j(7<^O1fh=&6Rv{*n$s(QRaaYG)>n
z>QA<=N8R<2p99zzs)@(90Aat_FFC{(8Wo#=-*T|tw5z1;b3!1)zOUG!)v7-TL3$I8
zG{~?u{rRX4v*!4d_v<bN`X2phn>8b?K7krw5_A>vP$;=ZE4q#0bb<M|Ysf|^H8+dm
z3_@UACtr|QN6^WV$?%IpLe|(iOXWhAO-C~GX2E>b^Zck6dhd#?Q|;xq86~rn(V3Vh
zpIa(Vk+jb#Ou7=<9x0<U5XFA7k><WBETg<N5|)`c`t@@wtrUfboSsl(fU<F;ZA<dg
zCGvCSFpJdM|8KhE|9|6ll0wY?H7sj7sk8B?4;fflE%Ry#fT`!E2NQTt@D}J+*--Nz
zIkMzUp!c^6UPvh4Ag&*ju7Br{NuC0Dfn(X8%u(aZc|D_T*u}!Wjj%q~D2GQiL&u&u
zzq8|kULIeWxyO!r?B7C`TV+jXdj(B!me&w;pTWVv*XY8ZXk(#7(0COj;rkIvVA^VR
z3H}8F0}ig1qFHR%*;D+guEpWc>99W|9wTt@hLC{lAvH97SyTzgC92&!LE$~gT+X(7
zufb_TjCl}%M!&k!wDLHu>$UAC-;FIOc*^hmw(E~uvRA&XUVqvi2w6`4R&|9Wl>^z_
zJq$Z_KH%cx*WZsGcwD_W`+hSi5&cl`^ugC(_nwW<LQd`-d(U*box<{GP|24a5Zepo
zV%55S6wQX&yFCRPY`^{BTV2HZN$BZZo<pb+%nMpM0`oOO8N$jv5k`2NCEPGjF8uIu
zgqeWQaA3JhDG5$z0{dZ3EDRd4&py~rhL+tQhv7M7(KNXZ&=d`2RNm(U5M>MVxyMo#
zDuz#!C%R_fe6;)sh~}9<>603w&88ojZ;@@W;4`|b>J!@SiLxZCK}cI+X`>4bJZ|b-
z-#rtEX(@ajh*>S1jhS(Xln6%cCTWwf$u?v@BddG%rtVhMc?6a{R{DwaQsE39<I<g;
zIxMMW!CL+P6#6rh#5oCmwVV`Pt6x3ci$MkV6vm^zRwhr*>`tkuz$EU<Oi_LNlU&p<
zOnxQvqgD%794$2q+vdzT9PD2tQ&X4n?WTxi{*OoCo0nfFSKmBk-%uDQe9Vfm3-o*L
zry~G5p2KQ{X(z3HtA<^$lXmBExRvMD3$E)yr-GMS>K=eDJ@f4$QW<x$;?_tKJcbL4
z{WGyQRZk^=HyNcJm9({$CI++13v+t9EJRlR&6eBxwSZM7Z0gQdeb?rlkMZm;J1q5$
zLA8(7r;=}R1dBOKHK9VwC0|8J_XamUodZAKXj|%TNXJsDrSS|9aYO*YaRi;72V-<5
zb-`LN!At!Q<k|>>ozXCt7i(=#WjMy)-gS{GB%?CGBuyPQ7^p-SM}q3t$`D+4k1qU7
zB2-MMgSV;T`oP64jl}1>R7ENfDu3Bw{?lJYuYBC8^zLPi#OBLOWKOn}O3i^+IcYIl
z(lHyKoe$sO?jvpB{tM4E76r-tq1;g`TnG0L^>22I{kUR138v=E{xQI>0abHn{(9k<
zFgYjwR12a~P4^8Xa!iG**3<vepqy9t_}NY`;8(nk)Dw{rsY4f6iMhL&q>gZo(_8NF
zvZmu6>(=6JDA*Hc14T1gdwd-)y~qWe4MzS^ZQU3wTLf)_p+f4lDI}t8wL4GR+mi+a
zUz*D$Xa=+@Q4uRGVCpm}aZa-^i^)MXy)h`$5Jf=hmoY*e0(QHxnlhxVCdbC-{+(+l
zO{RfUX|9*>zO7kmJ~PS_B#7f^rx(&_X<)5u6Z4_Sv{PlL57)_-M|puEJn|^5_B}(L
z$!h2djOnd2Ec2Mq1r?H)+>1l_uSdpf5|}e^e(aUSHl?`}JxoLwXK3+ft=+O%ZhVne
zRH4ls?@9XVn1Ih`OsL;T=XR@DAyM78@j_l#%-neCW%>O5+9DO{0IkK&AiT{qpz?S;
zS{ShqND-<*_T=ervOC{Avx6PCVV_&+(+%+zy>=%p!bo_VJro+**@K0hIj#wW;a(6m
zggFggRT1-4{V(R*x*7#;<HHTP9~9+fuBN#rMclR@E!3QOH({QKUhUMIi2!QehmJ=L
zLdJ`_UJxt4MnsSDP0N|;c|L`jpr4e^Pp<+W^dRY%){)Z45%kCEUlunFg<g{p{hwd6
z0WHQwK8l;e`+?qU)$#=(c$fC|1LHbfNKZMw2Dg98XNo41Qf&7TKg>z5<rOK(54828
zO+vPO@E82)O#FX5lYX8$Kf|RTg}8lI<zrWS8#DT+EGybq2!zc*^(RgSc}FRcAIA%d
ze{d&x6?P`__plqtT86aVslKBw`n=|seAru~hsob#Uy9FhN4DaycN-k|^S6SDn`=kZ
zz5VU3ucJe2><0I2zl&KUH%-?YEu?(W_l5JW8}KZDu3Q(DFU=FufhBmMb6Wu8X^0RN
z7Z};uq+CG%AyPvJ?2PKNEi;+6e1+H?1eu-be4#Ezi$P3Las{Mmh*q7qTz;Itg~gpI
z-(teXh;P@K(b6G~YV%spg+djNOF6?u;%xbv9w)~SYeb~HWBrSwxlqabvrRf@N}`G}
zW#4{>ELiAa!yU%G3$yaYm)jn0)aw^uX4M9di9L$DmN$_DQQ!PcAN?W`+*;&WOidJY
zo48YPrqJSzQfxSS6?esTau<NQn%v~ezG6SCV8Fxue}}^V+${eK^Qp73rRrDZ3X&L-
zNHGQ=4fy}uEJMUEKP|GQnd3%ZTWjeGJ94w?`zxiX_JnDaT5jxZmoQz_ESS1=(CAy8
z0j=osiD?k`kwOAm)!}hB!X=l}IvJ7IgBM6SW}-->`lRJ&p3BayRwglLUcv5?6%>Bn
zSbG87)0aD-qK(-SK_+iB6`yASMGYBFTkG=X!VyFI(@r8hvYZn`U5Ak(yo&G<A|%IM
z1gRo0Lp+yb>BOWaN+F?hX}gS+L}e<i`r|V1Iry6)sFhXWzcl9Fk?^ANSMhgQ28l1v
z%D#7O`N$X(pKGj~zY9$qePjRNbI5(3y<weIF1O<oyJ<&%J%!)gdJ9dwqWk7y@3Gy(
z`)6l3ztcLz4*|XK1jNBZswy{>IL(Wu3`gkV(r)hg@&Nr$1~@pXrhmO;AGy9O#;#|T
zY0f?TMNWrv<O|M#!-%BoC;S*D=Z6^O9uQ!8Y;BQP^!Q2lsb2;(1ZtN?7DgPSwTK^+
zqP6hWgJv*XE%3iV@k!qLAUM&0n^3lx#Wky;^Vz$SCJ$l;K(cM^CdfT=B+@;$6^*6H
z;WHHChe)`esa6c^eF)ZXNiq=YZ}Ta<+CNOROQE53KdnJ_`0g3n+!EXx;FBN?&~}7j
zjLp{KLLoLF_^t^CDBZikWh=d!_E2#Qk@T|R0+{3!D4*o@gkLSLb>YrFw-RlZqjubk
z;PzPFrIoJ@uplgdO<Ib*K>6exC}C|US}+ceS*BOOC~iNZpZhz0&uo}*#p@`{kOek=
z&$ZtjSCe(k?-#jG<_`pZEPS}1WOwq1pDySaXygIf-m+W=-M`6?|C5cnPzkO($#n)>
z_US?7pE^9UEp~&|S*NVTEkLs0-C;sh+=|;_hu*aPlw3uIy1k}=f&egad&}r&lEoH3
z6_9~HYDy2@?ov$>C(XJn0311x4#|5c_OjZqAMJ`h1`;mc#r0RF-L1Y6zEOeI={Y#t
z9BfvU);RAX*@#ZBhBc$phheQ4#Lh##mO$({=*@eIo1&m#o^ux)?jZ?y^XYR?q&P*H
z7$;d{c*g}DHr_|dlSvYVD)_S@2^WE5+F($FjzQwKrM604$do>k7e|#GT%u+5!&cB)
z@2gieo)X|IWX|-+XVE07lh4pA1WvpD8%6cIrwhgZK~8>Y;r1Onk8|ZAon?K`^YCV9
zDx)c;`RG?}wE>xHj7`sE%h2=P4<jVzpLp?P4jNQ3Z1}`1Mwf=<W*l6b)=e2I$O635
zjjnEUa_i;6;htXw455|WsF>{gq-rzYAhm@0?{JP;S1EtCE1Ipn@sm8dM33!ito(Oc
z&usjgGqorQGAxd0XlQITJxIYvu``hVcL4-0$;gLI04C>Q#Z5_TV0<5dDF>ZSqHq|=
z)Gg=joSXE~Li&FZc{kkZUWc(1#UC}yC-KXk3v5l>m&Dad^9a;hT*y^^V7QR4YP3&E
zc_NY1zFs}<X1*lU6{PDovs|A4B-}>+&WKc+4kE85(NuxDK0wm1^)bs$KFuWZ#AfMy
zfn4L&*2aXr6FRbSgAmY6l6M;>i)7e4$IT8aL<f%v0Ei3D%Jjd(j-~UyUF8!KDfS9}
zgjdC(1F0(Nr$!4t1luNuJ>$8<B}fL)fl5*FJl7JQ)3I+ZG*%=UVqtT%%gz$ru#{<i
z8GXwTcco8#8z}Ke1G+(oROIXUe1|O}{6k=6?fTB$`m_^^>vrg`))j?aG%dtsavhF)
z7+_@wMD&12J;)<uWRBrjBaiToS0_LCX(Qv_io%(QO-ojNhfRYfrSndJ_KU0fD<MA0
zzTBGtYYq`OWInqJJh@kL$B}O74sd>Zo#UlM4AFh)c^&pr*T7zhuI5;k#ki(3Mn#Sf
zvlvz=;5btKU35$z(_}?C`7QA~ReH82a3BWv`1S8MbKH4*iA-H=GVvQNeB}4v>5F%N
zL1@d8$r12BRkyzQf8?mYZ79r36jVQKIh5~FYMQO4iwAx5749xl;255hhG9gcdifWT
zt~x0WAH#-71=Md7K9TIDnX3NCC6P(<W`z>f)A=uh<ezO(Z+!Y{W2usdYwaz&JIijJ
z-kq*c*xNB)-3PDa2BmZPP*sn-dH4;_+<4U0qed!u_K;$6!9+s4asGF_z4D7ZIuk5z
zFD5Q5nv_J+=f+TR)Zu*@W72l1I?S+iR4&oJO;)mZeYtE@z=Yp&>;PZ1bLf|!5@l9q
zBvJCatqe7oG$GyYZ`%Dj)fPrk>?u_AXAUdgTZ@tu%68avkR16YcA~IHRyDvGTGsML
zY%1?i{l)T%CjrEaC!CWag>H^_&Z_h5b`jr)oqA9!dWj*4`T>2go2IvMD{D;uXGi*<
z9O`>r*Z3_X0;CwMJ{K%N9%tkLeZB<#Upd^fipfLmrwnZ%O)~3c71Kzoc}|AY5JIZC
zKLf}j6Q%>0ZSj=G@Fyc`p1!x*u~4vCFBlW)Dps?xd0)`8A^*f&I0Ncy=Fn9%P<LB8
za@=F4_<qykc~GZ$m96lR?FHbJg20W69#J?2L{|yo&3HsXZ(0cq2f`%5RIh3(qqBw&
zOr~{cN=9*2CIzY}Sv|-@34%LlGF_nyCX{7(XAm|i9t@11^Hn0c@j5f}4;$@yh#>u#
zk*llKn&uDQvkVaXrq7g%!MWUZ8?HQfM}tGJnk6Wid``FdU2vEn@>8+q$u1>sP)xD7
z`Tgd%p_9{B@6PW1dZ+}2{(bm4kM9_|{a~$ch@{KSJg5}R!7_aPw-k#3G855vb8iw#
zBYs*(Gd2q{wwxo@7R<Z`dlu@TQO<z<u?O5JPr+(yRpTTRB)czQ(!m>(G?piFiOUe*
zk|cccO9uc{DICEdG=_2T*zG`BLBKp}xdSv~oJ+uQJ%EE?3Bx^OxLTYbN>ekxr&K3N
z7Voqz0&(LI^ldtXkjt}=xN|QC%gK!HHqfR#C!J(ZvhKbjm?mgy>RZr)#}xFHHJaYE
zkdU&mxi67sdSLzNjyBw0!d@R)WuNM2>0AY?#wUruP2&oRgG^%zZoo|g3rd4bR|{@@
zK3VWb;ry%bNftdZAwkYX%|H9*s3$8cCROA%!pBu=N0gQX5MqQXhb6IKfNSOUMf;b*
zoAGs0bwOb#m76$ZEMW2vD7U97p)Xdc?;Pd5dFHm#-SwFMTylBw<cD87RpR!=*Lbz-
zHnid%O~9kj1OE#>n<uN~I@83K2yj$d7kWEq{?o?xK9Y}X40cJbkWgslxT)Zs_Dw3V
zOhzPneb%3=j?7cAqJcIA)vI1?6*Vy?#Mt_z;P@LzAUHGv(tZoxRR%~05-91RUr>n)
z^+70$)Q_K*F86J{3NGmTOp<(7DLoTKsgeoAHjl{2jJFk(A$B%{<;<~@chLpvViQp%
zQJVvbf>*U4ydf3z4_6Uh@hOOJ$)Z65NjwT5(8ZNN6+6$+XVn%zf&tYAlXyUlbc(fJ
zUD&^Vv1d!={L5XJU-6*EmN-b!q_H-=qh*`HK9D(mkULdp(h)nQD_lL`jqn#)-)Pz2
zTA7SI!4oO8MV$N3u|0t;a4phKhU}S#!bo$7pN?_4PCI^RswDVl&`|gKn<LKC+bj?s
z7=J!(fHleF3HD*pab~oQlqmyW^I$kO@2wol#Frn5)pTW<UdLU74G9mG5R5zv@aQx>
ziWEKmrfmrP0*eAhV$-X_IWOFzvGCR?-_cB2=ox=slo-I`RI!+scsAq+jU=LJRB;SG
zm_)(P>eg8E`dY0^7)aZ|>geb}S-(ji&0!mYDQV5)gKv_|n3I0ZmuTrID9BZ9(<0Ev
z6Ny86U5|pP&m7&yFqwMG8D*|5_P=nKvr1BU{QXYtm*`V7$x;f?cJ7z51dFaR>G4Gy
zgJL2)|HGP<BHvp{r)#X47IA>d!O{cMBsmLtNvSdZlD)-Hlgk;|7;_YI#3_E)$HF>e
z!k<8NdKKtVTTi5yCgztW+$w45ELOm6qZVts$}+|rEQ>Wa7eAB+LNQhPLAzFiJ`S>m
z7!8Y*3f|=0>dGnKWGj6hBGte(oP)eFJrXlShNy?HW4a51_^mvZ6~nt3C~nEM6{$Xr
z^RToBQ`KIJiz`B5F}}5=D4Vx>$5QK6J+6?MI*1}cP}xykJ}w(1tiU=bs_x^Y9RW7-
z^x8$m@C1roQt#HJd0k8{5@fO!w_k}YBy48Annpt$vpC!?QY79Cpg0H|H^xd;^%f_F
zIb5!t<Y30}@*y27i7O2LLoF|34eJvn2Z3H59T+&_I7oCbRl0n=KIg#@y?_S)muM53
z`Y_;+5{mnucaQPJ!wl^-bb_?Wp1$y~p#R^h!qVi}L{<Ys6OK229o^sa1YClmaz9=^
z?0V6}hcgV}lIqsJ+eqNh|9Ul&M!+QhkeSK(<!ewi^$=_AAaiSd#SVL7@gBVYgA}Qd
zw>s3S<YF9UWd?q!S`X-NTm{GWxdqdq!8UMT&={0Rpvvz(AVWxbCCh*~8OSz7%W?U@
zQ0#tM0QV|rQg5UnzWW-3LMBZuivl>6M34Q$QKXSCQMwnXuUj)kUcwXkg`iHop!r0a
z|2^CX-m#xg{1?+mS#>`DFg(>h@1D4no?F|*hpIc@4vNltw$xHw67?#_HTJ=p?x^LL
zsB;}IFALu2j#)^D2bk+~shW~LcxW9bSoT$#`7K`_iJO<Q*2=oE;bAu+_&1sdUZ<lR
z6w_#k9qzk<GGr?;42;##M|vUpV21w`2$}%XeeN4{We8dB+W$>K=zEbD%%}b#K!a*z
zTn>{gON!x$l@lE}Ap)l2kfh&8F&;pzYL&I1&6T@wTv89VD{L;B`xJwUGknc=s`Lr1
z8^J#A0mM!E@y4l%<`Zkif(l&9xhG_!9V8+hmRq062AVC*HHh0dJS^<>3rlm&cKBX+
ze>dkc1+@X|C!ctEd}HYdxHq(?<Yghn1{)drg25{gU_4-Il#;a}?`dYlkXMAo0w526
zI()nDC)$Ij9z2HbZ!qqFv?Ybyc|JWU6$0fMGKgT%I_;VHhNG;AyS(nqf<uP74fyPg
z%b?E26}&;>djxFA^}LMY#rv=r2>SXH*Y6I0501Pl1Es2^J^=~n*YwnpCm+5gTs@7K
z{rvskeTI-7okGgCfe&-oUiS?t&hZ`%S`oMzO>QmtWc{2_!f)E+-<p)6w%=06MkCxr
zlTIPOivBvSY$waBXC0%)HM%9|mJBGP{EpXgvvUgr-0Kc~j~H-6ED0G(NqYQT_CAwD
zlv@;yOp<G2fXo<WZ=HlC*k~u(hA~jm2yAk+d;;u?C0=d;u(YL2XwmZaAOy?kYA)3?
zHlyE8F{%y@HJ(u@hbIheQOIf9t)@5P)^n~(U`3EKrLf4<ncSDb+Qq1u`?>`4t_zL>
z5)=Z)9qq9fydaMDFbQ|(<_j)vg*WZHkm(ia`wneYj4|?a?yU7z;e%D?d|%oNm-hj>
zu~%|~mheXI{^*|Mg1)IOyN?>r0O)R#<-wH)lvhpiGqhLyO&F(F7cX=tdGIA)h<n3T
zo(Y*C$^WFBnZ2$3^2}7hGI&pAa;JLb?#MCU*8(Y+V7qPiY>Klg6_PRkvtBLbxxfy0
z#IlC!<D|Ea{8o(UpzfP6A88LY+Y@*Ckx|;0#$j;|4SLiozS^A0M7K*KUoVPop&hHA
zZkb*+4h*zWNTJgSvW-<i`Wh{@Y2)EH>KpazZeW+1GCDroGYhP$l3wGi4*SBt(yP<d
z#F&zs%PQ^%G^Bhy5l(Mv1CQ(MJOG^lNN=Ztzy8zix&V};M>`$6KhU8&ygAU%&h&Ti
zE87iL1*H{P<fCuwmnA_>uVXSGVN0CwRD~o<O4<QfqhH;DoxYr;*6PBu8Wt!Xp7ir*
ziR&eqV+Z+Wn}l})dmGRmo9JxM+uYh@mbk6D#3Qaxv<1t8Pr;|nM_3;&48Q+A_615l
zG7zk-!aQn_ddu+-rL*_XCfFkM*~ULToF&9_9i}fW46T6yd1xHx6=}^+gL8>Qlnj?M
zz7AI-sC_Tb4I*jA0w2QqX08silaPKeJ(4q=YG$5-w5om>1G>$Ws#wV|pOW(J-$orW
zTU)BB-`eCKw~dcXGu_rKDajkB6B5qI<ra=Sa<~bNN~P55V{!tb9%Vd1oU^|4AA*y}
z+Xod$o@8Zw%?22@8bIWLGJdPX!s=F!5A&Cs)?}K~wfh{5raWZVV2b7?%1!}sMw0co
zF~Q&iR{}9eQBqnIH;|t<iYdzR-F@QB&Fc0A6N8n;-9r(U@(BLKwyi@+x(>q||ECaX
z8YET6cC0{=TjPPhrHoi~p(pz&$O$@%({b(S15Ea0p{=a&aSLkx8P6$WNBp8U-f!2&
z2Vn6xTWo|KKezEmGPVDQCyx`r{#X9#3>xB}FGZyRNMuQUWm&=}orA_kjTzo0QF%xx
zN9P?Y_z@AIWg?%Ml4NDZY(V`GgKnLyzKg)q89dMPP`-N_T}V3{H6a<EpOjuD;H~L3
zXv8XHv<L-LEf2ZQfWd)USlEWhL6P+VLMg`<b5`ouO~rMg>58v|X6zAwvg&U@dp3#)
zd@0Io+g{2s2@gy&azwX%ch@mVa-3rPyp3zYfd~tthwbF0(Gy$1F_0))I*M+0|M=1h
z_@4cBE8uKg-veW0kEh|0;n~&%bBSf*&1H5H+qRS#8|t4|;q_})C;u%H0${WOmP94v
z$-x^ZnO3z0ZugLrh6U}tTE?>UNEgyEq%Z`sMXsB4YL*~qsuN~;qes1RWvPKHEOfpF
zuf)vKz<Zn{FW1P5@cfj)&!p%byDDYrN797eo4RXm2Wd~Xi&kvld_ooue#$=bMvuS%
z_emep^U4EzKVh>+a9KX?E)8VhD!@D>o1Kge7@xt2x(-~43d<v+#v(DtxB2QR^@jA@
z_r(1ux6u1dX5Jz)#1{_)oSP#GOb^@i{YT{UYbA12Ma+}y5r^nr{{YWj32&-ycctu+
zRfqJ&tPqjKf+8`LMXa3L=FZm}8E-HO8@qRCLa+ULCxi**C$qo2uy(7hDpB4rbD-+2
zl_z6VJ)R6N1~KZL<e7&2?PA!=e1BB>Ldoj?iQfO*p$rF&uPU%l(WmWg6&Oo#vW$o;
zkrsdx!r<ezoDzzHq6A>H9+@UWg`6lhuSY2*q&TW4PkH5S5Aa)a#Ep8lnep-`K?_XR
zRDP=QUp||v<)aW>ZuE>hY}AFDzfcpPQ@g4a1T)&OyVU#5PnW(m!NZeJepEj0i`s~#
zu6$zD$hGh=KOAFng5l2~PB89=Ra^fX3jezw0xl*6DH~PZ37Nb?70K_vYHj&bA@#E?
zUe$s4U<4ja_eB!j7!Df#A){&Q1YQGX!8vq0EaQRrD5&vand-;_`+X^*-(Ax@2M$eq
zd9(2Sw@W{+fZx|DN<3rXo5022U&Kt(PwW8kr#znBYTpI+6A~Y`6<0g_Lw6ke3^`4;
zU18Gu{A%-8RfijbGBlk&aP?kJa|5HsM@|knNw>hiA0lNOu<COypJp0mbk^~VcK8TF
zq`y!UlJ#;zHTaFr4n#sml;ke5KE~T!J#HK!zW=)kVD|vY3;^{ogLg&)lnB*q(&)Sf
z4k>ik670jo{%*T6njvs1zya==0wx;z0<=^C*)&WK5OB$-B#e~yqo5D?NnAin{B11R
z2En-BqF`Rt68ZzIqI9R6!D3ii3bkR&dY?KQSf>rxZdt0Fo&BJnL%nI{Zq<(c7W{}>
zHooVRfK9F)2*WB~xZw7ZT6YoYgxt97jZPg)<)pPwUG>o$|GG?>nvIs01Nal(hdia*
zw|bg1I&X(p6&Zmk%l6Ev=V^s(BiuR;F(oN3`Gb&Pm!cb{_{l(V%fMIsest;rU1B=f
z3(bO{thPthg}bW@AHQ(+?hel-4~>tF8fVm7e<iwV{7Irzt6D)UnyAjB3AEJA5BdOG
zlTC)~aN0NOW@%HUkW0P;!K-o$3(a@qE-R_14L;*lS<_(VpS&IPAhmdI3u~9lfT!hw
zHT~i3)gfhF5vfw7eZflfJp@k|CjN?%Xam(<r6w4E#hJEAm*fY6`x#Ovi^ukWvJEbS
zpd1NH2mJ~eu>p)$6sk9pD)WtAU49YwKoP#%lrrAc%KqQMko&nh9*=+A4IGm;)Xq8*
z(DSso|9Kwg_1J$7T_x-AqACl79an$N7O}!$Pm{O%vI($FEs|>qCHYq6hix+F?T$ZV
zoi+DGmm4f{(f7G1CqB_0@*0R<;d=Zu)wql8AIC2x9QUsKRSR6Y(doM{cSqiU#=oqF
zP9|Dt`QGeC5xKQDi1dnI_ZyJ^awFM{4PV1M1%Y4OgyScX1Q-q`no`B2AD-im)ffNt
z<H?R0Xg&Z+J=P^UO;nhLSZCJzYu;Fx`DxMEsA@2}&{I06<k|Q|Ls61Yx!Bx+G5wul
zST(mI#0|IR>@&>e4s@jSe#53)fd9>bi>4lS>5-z1-w}=H#3Qkvq|e?_hq?=E0w<t`
zfrYC%t+1$`fU!$CN@FE`XJf8h--Vnj+$hA5TsB-IHjNd1mmV$TiYMeAUKf!Deu$&q
zXUVF4pcgJzyrjtGQY3|S)D<51`fB8b5QropScl!wL|)fezdl7_pa}fqlSkAYFTEsD
z7eDE1P)i-%7Z6z{+M^k^)5j<jf9Z|6GFuC=+lBnkNLE|191XYfwtMGp)I<bBS6cJ-
zU2K;U!pnWtFCZ7kU%33<kd&P2<Tf5&l4?YRP9>yF9hNQG+j(c%K>>|RGs{cf;+2L<
z1sZuRhQ^WH$8)FKIS2zX=H*Z1zxpO?Av8V8uZTpAqH@Vh-%MZC+`#0}L9IPtOYc}s
zy`<cu{xWwS_Hs*Hq!nef#=^j&sy@w7;+2kYX3)ySJ#<QM^1W4HkE3Q7W4sO?+X*Ie
zMgUB>Oc>I!3=sA9h&b3vIVn`1`TF(_s1)C09w_{S_MH0^QnW{uVtlk37JJo~z(ylU
zd4?=w+7;XS^>mosJm)Z1d!+x}HF*Q%EN!%XVBr`vnPn9-Uo`nHm`b!U3CNSrPJp$I
z0&L31T8%R9DE5N2I>&us?zf6Y+xU<Tk^@zd%yrXa(sj<}oG-@1Q)a4E<Vlw!FvAHL
z>C#hf5fE<7Zyw&D7x;U)fHPQU_L9-pJ2|9!qBE}}CemNN(MYZyH^Fh=P+P^ccabRW
zcB8_AqnP_r4j|r?cO9yJ5@3$#@XLFfi;7J0(sCyY_`}@J&!v*OSI)mIuR7OdY{q-&
znEW#6WZwQorWR-Q%eAN2q&i-^Pb{N@>U6b>Cj!{LcllByuYD>i$zbAKzl_~1?=2K8
z<|DJHaBN^^2M=J>Nqu-M-SUTDmV>`^>Ab4!DQo{(yUH9SSsU`7tgSOb4-8o&X>-6*
zh@z3OOk*xoy3skkg=wY}CsK#kqVSV>L}<3@Af&-7Tu3b00!t|QswBo-Vj;QTxC(j4
zP`oW=_-|CQc-4?dCxTZp_YBQaGU}%vd<xk-B=UtNhS-M;oM!Mhe-jIlmkm$EX80=1
zLK!ze#dzjNYaeNb;l&8%PnQwUA)?h_Ij>F%m`bbqDaQ#<0?(0!<O-x@*(cT`JD2Hz
z(2XqSx7Vl&dGLCG#TyL7SN<n!Ue))@d0m;sd{!A%s>a$#aM9~Jn*?M;uVOg4UFV-h
z`8wnD*U9?rSToy);r=Vxw_*ieCC7T^a<OK^7k7s*pW-r!gKUGp68@gO*mUsa*AH<|
z#jEd+4hOk@L;vbdA8$Q2sMyW)lNg~dbM-;vy5Pg%2-{(*UI*nE(}-dW8=HZZD(W{T
zl3fZ>L*uo1FV$&i0*pS3@%f|wMeYO}Mw#8^pV1)HAJBzSnJTsMxD*EHOqL0RpzO*)
zQ!i+Z<p21d=q@}1;|as{1Lpg+L$b6ki~9t#2H%IW7W?{a>gLh$Y<y-gX)#TeTpz^o
zZkVlM;n^+gt$$W{Qy&^Uph*?~#4jH0-H;C1r`iNc$fVEg$m9Xi2yJvM7Gb17xoOWp
z^3}jF(;avw;$CuD)OHtD#A+-wd1OmI3N&CeohLZX@4&qr3ks7wF=|VE@)*M0m<qsy
z;!$?a8yE6(<=Pkb#_i7+hBY8%hkP8r)+OvOq9j^CAW;71MMpE{H<Iqr>G0r%J7ZU(
z^tuPwqTB^$>mnY$9Q~T(-rzPyS<+~^6KL+FRu{5bJT+mCWi>-z!1KAl?$8RGQ?q99
zPLo%>4$s&ZR%8B3_Nnxlm6hXEmxyQP7JcGx(fZab^4||K)q-0=`hrE6peX)Qf9mV9
zWG(4&#QJ;29fpRto7b(8rh>(GRW9sKWs+t7y`U_bWu}#uGIQSn<DZ8P_N8KW?#MUT
zyt~(aV`shpv+Sea4;3ztqaGUNWuTLl8l;2)MxuciWNP94HP)3suF7_{FY}0Udv{j<
zdG~!8*gPi82Tmt*fUre$)-9h`8Za-9*R(b!{x|5<%juv9r&>3dmhInM*e@vJkv_U}
zY!NniDp8(1YTZKqak?_}_=cV}1K<1W`K^(iU#A{Vef-raHT(5+&fPn#NAS~w*^fKQ
z5xbASzdv9#`FDOU+cV<;6EvMZXp;T!^{?LvTZca%tdX;61`{B(@#yAyVh+j21h>bX
zA`h+4pD92mZP%%{2U--r)!(BuUJ2CfK^vfEB3zK2TDS0~@MM`dapMcmw8bUg(`Ayy
zJ7J6tA6agIDHWYHot%1sSHHj;YCXzLLK>;++x6F=JA*Qf{QDP#7XUjBPyt{zTfnLi
z0hxjPW)!St5%Ji|P&!<$@P`PQ=^vIYhtj4lE9xQq1CufNOuw#Zq@;#F`}t($V;k+e
zHHT==@p7uhAOyFz1>{Y8B|mZyEUNu=*4vlwG2N&Ui}U&7Gw~(b?DjUc&h}*?PCkjW
z9mEdXt+Jef$D0oVBNDoIOKh059ybzJ$;U)6=W=N5Gw4VdZd_^%l0SH<RgK=PAV2+$
zUxfg(K=8iCwyYMaOWI5asCfBr7^bK?P@a$17>iq)-m=^eoqeSQ1XI4Ws->9KzsKGP
z*@>W?$@^<5Y|SS#=0#N>1YS|m)7WD%G!1;witlXP`B91cck^~vkAI~{w6Xsa;B{J~
zo2M6~bdTmZ%A8xiR}pP!|I}bjoD24$3S(?mA%rGvQ|+}c=rL7wr@Dk%HbPm?OoKY{
zWuIW;{g^X*fX*5$f_!WU6ChHk>)cUUY0-7%yz=oJ$2zBHE)R6XKQRx!?>)4xi)gVz
zbmj`Y{`uxgRJ)b@zf<YU-$KUxjHhk%UKo5&y4i=k8tRN(C<t<XlfV=IEHg(hFxZ$j
zE7N8F4RZPw-!V@cp4+hq7Lf%R<K~ljQXRoeQqZ?%`K_H4!pD@6m<PoIP2QIL^UAOA
zL5fA8>Hu<zuowXeDQ?k{uv<GZi{<Ae$!qr;?pGl5t@Mq8kSJy*DQ=oo-k8Wgj7%`$
zwevs0_4~q^hWftJaasJh+MOijjvV7v2#;{rc2P9yFX`T)tYG&epZCEhAAftOAaTIe
zPY_;RE%oW~OM#qT&^MSrR06Q7MZ-HDmN&O`@B2MhEAIMRxTaQ>`U%>3y>n&zahlc%
zTsCa@?!mFO@=*OwcEwS~e@ZFzN_t&3P@^0lSO724-;9Ko@@kDDotqu^Q>-yKhG&{c
zW>8w#_pc6BF-A3Y_5;cY?I|hqlsGJTR9x9fE|^a=g^sySS`Wu@qygv%Q^PJcdB8#v
z*iNVqg5DUZj~GngkJd^$>!<WM)58qN1n?au^1^V_>$YBbG8Lk!ETxv-(Pidk;vS%S
zwR~?z6n3SiyITHdMpO1T>N5IzNS5e!bpP<DRkl}M;+JFJtSKh&^1$PoN3K~7S_lY0
z;X{UdR$qB#vqA6xY4IW&#b8u)YYVDp3)QBWeYJ9TpqMhV2YXeAmGi3opBe@?-V4l(
zne8_oZ!CyR(Up%BwNsc+lOH(vbE;N;5}N#AXZrDvaQMpSQO`~>;Ml~*mm#rA()Ey$
z&&ij5dpt`m;@Cg>{g?N2_pPHpwDw<<H4l-;4~`8^Td+a&+6Q1qM5}lK2>S7_fuF;l
z*A-c?imq?jP9gUo+=kpD7*+;_PaIyYEOVSw8I7_P!s@X#r5a7MpFM%jm17e#h+Mu5
z$4v7^rRnFco#4~$(rG--@Z<mta!WPr5iK(gB4-{b*UMN<8a9Dsx@-q%(SUmjSfZh0
zsun7^`;7c6v>F_@?Op(%+0MO`1yF?$J-%)xn^_flzCPP+c|T!L%`W5AJbEgf-=P`@
zX6sZo<XFq`2BsG!+XakYx5Zr0FQB&5CG0NDQ!|b^Vx=W)U8-fOd4Zdpjb~NMBX$+J
z(a-s)-~v7Ei#NK+b>1J~kK*HVage7~O~p<gn%Kbnx|0_*nA?5bF7iz#TSoS`+q*dA
ztpoOs=MaGV7yFi-XD^m-n@m0Vx&~UV0<0>SB3;`+7i(Vo2(myAqGt7_(fugrPpz|Q
zRbd<lWOT2{Jb3PqQzCtIQBvsQ$_FOi=!Q*uU+M<cssD<*b6ePr_b>u9O<HSQ$Q{#A
zZBdK=_9Cl#_=t(k6|-dHmZoO*w6dgGkEyq&0p=519^$wUtHPKb*;d~Ej^J+$OurIE
z{V04YtT8x!3DMTPl|Bl}QL&StJZE$fC{OpWzO=GM_<nz{L6OU;6JYj#pmgyD7abEd
zdrQIGW)%-6wbaN>k=Y-pB;v!2P}8bE-t)|Mw-%q7x2C$w6*9u>2=<-^@o-QBFCGiB
z;_<Oy@@p&tso5Z#liWvR0CO%is8lxhYyD^ev_DO%NEq!=N?|AYlg5Aec34=a&~sC#
zB=mY?oN=Vn3CaKw={fZ4??a2UgTHU`>+@`c=E+>Gho@goQyoAF!?TT_X%v~?o~4dv
zhU)N~QJOxj`7b6-`xuUO3qXJ6COzBz#2Zuo!}A*ITF14IHFkR?vrYfZ8{M@=NIs8Y
zSh8hftNO@-?+qt}n`L8U#w{k;<NRd}fIUol_-8abz?7^kgPFruBX)h7yDxl*0EG8p
zpM<ZmXL2xJ{5DwZxTczIttMLHnvJ1}DYv~@mUWKbJCQ=3Ob@7$6Vjye-3@0pF&ijA
zBEWPCM-s5&uo^0Jyg}h1H1t_PN+-YI{Wr33XLBOAyba=xAVuES3vzz6w|9`FT9-GI
zoM)OME-8_O4v;gplAAdYov-bQY<}Tzf>ctO_q@srw?Xa@%qg2c&h8ze@7HCr{M5&=
zQDy&rCUPr--^ce+kxfxZeUFgF#yx5L?r1V$4fbw{8^O;0{KM15<!9HJ3M$t5wR_Za
zmTnCKjz;?W+&-}JGT*<LIpz;%CF|#08kJ-@?2^>-_+KpDb`Qx)NKu!(gr}<Jqo-)K
z1-YFV;h3WW%b{MY7n>^qzhdS53T_v~tr1zu@1mb_44Fs$u$bsNxZY+3$SlRJZy)3<
zhFf0Hq6PyZi_qv=<amyWAt1>{yU^*<WK?X)PYQEv+U!v<N|qHk(d~H=4Ot<x#sepQ
z0&zI*w>c&mu9Z4Z(AVXXO}-3T&kMfQqPnS7NN-MgOmgF+PyRi*jvl_r{jpPVJb?Cc
z;qUci%H^CQ-UV4Pi082EZ7X5O!qy4D-EOgjb#<PbTNs4g;t^3r8spsH7(lfXsbNWE
zZ|)8Vq84@$kty6mzr4fK@89oL%1Iu+XXfM84|<w^KW?B?Ui`K|yU6ND+>M@pp_<R6
zY`X_1Irl4S*6ndR)1s_>{4M+HjrSRxlS$GN{rc~OIB0`R%K5NKqBxN)%TUg{j(lWz
z?h>?a|E+zF=?8@fY*U0P6}0WYa<)9aKWBlUpDTnU3ZJ_Ipp=s7o1xfzi(;QVTkdMf
zk`dfoE4r!4zBA}<)RbMr*w4o3HAzc+eZ9h?I#HKaq4Os;=5>oRbL|ysU)uR-|2}8F
zAHL>lAEJDZwPM!TnO8R&PY-?hBs4vYAZ@NnEm$35Gqrvj>o9R{J{}X@_Hc7~*@_M$
z!yx}@-+v|+Z!43GzXGh63}~M<3yk6XfDwi}Rn<&QD2-V2*_56CG$PMn!>U&VeVn<7
zEU{-=KF*_7Hgs*eWK`y&D)?-W7q9w)!c{oqo$K_C0Nc35@L3+}N;Y$q6GN2Jd7~sw
zFtpFIH$Tc88a`T~ELm>)Apme)Eykam+4|n=j($mH1fM^oc~+DN=D(Im0=?UNT`4V6
zGLMHKR#v-=2m@j$DnidAn5QCj3llFdnP^{qKZtfMP9eh?R+56ghe!J(BpCg&)p7WG
zgeI8nRYU(T-wsmHRiYMr4^a_rm<|(m)3;e*+3T3L_p$u{RW__p4SvvpFGD9!QHyWP
zqY^JCa!b8=_U1>@(VsUZ#f--ry4?-htH&h=I~Z&~xA*U1ht3zs&|auGe2C<R{!5Ht
zmtZ%@-bE#gebyCB9a7B1|G6Kj1mQD6#>nyy=R(~+OX#8OfJY4$VeA8>0s~R~cDJkO
z$J5IuoU|^H>|`Rr#vVGdCFcd0KCg`cO}rS^6*7Zp4PKKE9a5&{?fjQ>Uw#S>sJJZy
z<_8JyG%fOL$H6I>;T&K|gHD}33&XRjTiIdZCyQon4U<({*1YBcjyPZmy`3dT>7mE_
z@W_%(%hFI8WX||17Lj3qXDintO}mLPL(S8*_8VRVV&>{;nM|-zfbZ=qH}rdL+(iXe
z6z*cRxfv`_fzj}qT5vFRf$zbKYB-+}Nv#VUGZ~gR@AAEMD8!Ab783O;7saFXtXmjX
z^6YjQ$EjyMOWcvK%#%AWxG7ijoqD6O!}(`9(q=n)`J%Sc9Ig1|v#Vz_zlnhEgUrYE
zeB#o%<Cl6Qp?_Y#6#rhhcDIa&Y}FvCMLico;7WfQ{b#Y@>B5Xa0Y>08bM%<~5&XK7
z?6rcC`3&;l6~D4Gg>~D3s9su~>4&~X%%>6{QwC<-MV0-|)Yb^V%{V)Qs4meab+7cp
zx*HH9eYa`psH}#tbYVnoXn&W|^$&>j#&!kaql_~j8?<{qe`rQL>T2V+f;a(e!JEB|
zK3T!u)`@2Xio#amt)TWv9vB!f$n<+Gv_TdNsQh=EW6IV>O;()<<}jMH<iHqT4AW*a
zRim5xEYiB^z%n|E(K&?9+J~*8vr(`$G-GSIj?T7$ZC+N0ruEv>SXw?*eR^p-H^99k
zCRcZUzWImO+4gro=+iUs+`rQ;#lMIDEwdlxbSda>8zHc@w3Ig=ev?U0ne|wI|GhbG
z;hAOwA0(j%bNX!q^dQo;jL^1Myd@Y)l+C%gke$8GpR^5OtJQS*<Lw<`*5Lu<{0xB5
z<84y{Vo_~#0~Pj~09Bbo0nf+9UD|aC;Fi`Jr6&@xY|3d+p_8}f{@z@(<x73CQi!xp
ztgbi^l9Vxh)l-MBdwH1xKu62+1z1){gq!Ge3PncEta08HY9C;grWSynFD7(j%~ss|
z-lG6E0ElT#xFV;P-uc25<xIf@$TQd8@mgf@WUDK0J=7g6+P&!&l}7G*<Ue-Y=BwXK
zla*r4Q@S%8Q6yLCq9kc4PnCsZEz^?5C3!dJ^J|iFvdWBNagPMMjnElS$_Ra_vYrFz
z{CwToE(dq{#mfpsfin)4rNxBO7T0oIV>$AY`qLN9b%9VJZ{)R4r${(Y41~G%)TNIn
z-}fqHP3flI(a&cbAmXu>p-EQy2uCU|4HwSh-U8oSdgrVWW_^FexSDtQxUz!+$JB6v
zK7!@tH&N_Y_FlD~?;SW(*$C^#k>_SgRw<*Yp=6*7Nt||LBeY`;E=_5T$8Sm><Togk
z8OWAWwlZC7OU_gs^xt*Zbx-QTa84)P3y31F48fXJBv$NeddJ=`HaG3qbRq3EiL#o&
zB|U%_X%$Y?Cyr>|Aps|8zz^Bx*XZjxNm+tN7q07UR_%6NxSjfi5vJZx;+Kv(Eqh#&
z`$DQg&YJKD$<9r#uz(yf{Ny1C;=lWwtrPYLw&diK#s)F~(89-Q8M+zM%g0HfDFPE2
zXNYfKHBxGYd2%u?`*{X47`$r&n0u~aAN|_8W7k-4OdOo`7+7k{bFEQQ6;cXnb~aM_
z)M!J6#^+;d>C*G3rzrmqRe#~u<m0|^<2Nv18#P9!+ZZr9MOrp$q;v^NNlOW+hzd6~
zx>LY`fTKGFX&s#^q9PzDWq{cA?EU@xe#dh>|G~Cn+jU>(b)M(zA5Wf{e44GAf)V_I
zBQC?UVX+!Cih3d>WOupBdKg?*(spHzJwY?vv`rbsN)Zc`9F%_V@gSAZJns#2kfMe~
zFv0oh#9Q|bINMB=ImC%Bi<B7{ICT)zhgy3x-kUpPYEpaE=l#2s!<Be*>H}4%_6s-u
z3TM2=v`Xz5y2vTqUGbaT#eI{=Vh594OUT~?+aFav1`=P|exX$Zc(t^ai=;a|*^0uR
zDh`_WE*i=P&(`k$n>c>Q1u$LD1(*=1=TtDuWCM`QVXi3T(DhyJr&V^c#Qong$R3Z!
ze<~K6_{W+!hOsJ+;GE<D4E5NZ#-_CR<Bd_7e&e~&&*?f?gtGzCUb0|J{7BuLou3Vy
ztyCdl_t~^dt^BN9Ab(SkWCAL`I6F8QSwrwzFyWSHum#Klm7UX<y7b9TexI-97X#1J
zHQuoGj#c%)OxN^c+dp`E;KpLmykNFU|BLD2Yd7oUPLF2^W1gr?>R1mqFuFX<5xpw`
zhl2?lmF6$G&m!UD9f;uzM7kO<K<X!@qMfCA<=~wX>fscWAN-*9$y)6gE=Wtz-@7J-
zfop?lKa^1E+-Sy-N1{AW(>+fwCp~k2e81w(;A>EGIQ#L(0Qld}c%b#7EY~9B=A%<N
zS=SieKw^I1I~B@22mOauXjm)NrKrL`H?%aef73xhcmcgoRpd`XwznM6&<f#!jed$z
zBaeNG5JJ%MzNe65oT|o5Gy{tU{aLGhCAx>L5IVlIM>h_H&$fArBekJKNF~wFI}J6C
zXfyzgb0G>6lh)hT95U_sxB05VJM(x@IaNw>QX6m<G3rMb+CXe<hn)tvykF8LrR>5N
zOQOtUl{pFu0nT4p`Jp=h*>xkuas#pjT6L<^j(prWW-V0bO$1nC=ZSrC+GN>o?LH+h
z0+6)(nGQ0@l@?aY+H1Ol0+bQ$<_hL2{*YONEk&DqOk<6`&-^@98)aC9c+;ip>sN9b
z>Pk-1MW_>npm5|CzwBKOx^o&2SW6XSjBvE+{-Tw!7jz9fyb><cDl#2XnJ+C#SX52;
zbG`Bmm<XVrjal3@;V%DUv_L9&9->JcJ1!Dq`M^Hp*a&_Al@$|*C#~&ljrFbk#JS_o
zkr}b2de?+8;OopkuZZhT^a5Gi-!$(QR;sVtJn`zZe}DTe^fXKE%bq#5;Cl{8u1eJ7
z%43l4q#cU7>-eGk{=Q&Z=3N{v)h`3J)-jNQ`7lMvBBzda);B&{Qf!j&Q(HWxEm&i3
zCB>tBwN4^c`VkQPB0A|lIyJTGqmz*CFd#eij5WXELHJ#}k3xe+360YPm{%-|C$tA$
zHVpK6uxtx;k{kZ%C!CPH=+ph2(l|&;I+v1Ym7P(-lgcFT=V={8F?EoEDnkH0-^bEz
zP*uk)Hc{(*`7eT@m(-U(;KjmUqcZmYzoXG{^sUAms2D19AFlKzaOy^1e{gdC$DGmK
zvQrAWya)S#Jwh424=?JU7%H-Hi!1l%Y^Z$ENs)$B_X-z4Jq6=PXaRAm@Sj{8Nih2Y
z+pt{=Bl!fSIten3VCDbvsY)5A7a(e&+=Ux&EaM=i0@=iX6>B(0;)0I!NbwDp&2+qB
zohL_j{s@FW_uS<KaV354M|eBWD;D*1ctfY!M|o3eY0|t>N~;58&tAZ*aWOr+fPcVq
z^q+N#NLH;4Txrc3c5NzRHQTsiEB|L@uyjt}nZma(rs!Pkt2rIB@$QZU;EOJdV!|1x
z_G48FB5X!XXl+;BA7&sZW!$m^m~qaf(sRzaDSjpd)-I^KfR7&F@URclU8FDXJJXlj
zEZ*JYc>XM?yzbV#lu+s1Am!CKawq>g@Nmmnzqo@lPVPBPrI==s2$EXLDTyFBrQCEr
z<V-bkke~395x&I8nBBArnY8?@cqzR)-m))xdWyT@(m6d)t6+c1M{3<Yi0yAL2<;$x
z<)g+%?hV0NHMu+(1r6ML5yLAw6y-lCi<9NkKCO7YJ){go7%WJ<C*}QK2-U#dj4+z1
zA7&ueaMw|KtK!V=<Ymd67@b{q6gYKf6BaVbIJ^Y5Zo2Zwgh??zQvaEbH&gNn&vc??
zzF6Ou?MRhYTNY}H5qDpyK2fxFFgHoIGs3>;0NBS%m5@mvSP%NIK5~1D@FiUnD`~);
zlW%zJSW38XV;(h6d^PAu0q8Us?<<4?Ivc=ufMKgmAsZAvhDH#@cF|d_fU~K*)y12t
z{_~Ua<3vMW_ePUM$>YmIKU!E2LG&pI&-r21a=$7A4KOO{IX>_rQx#)NTx${<|KcV>
zZl@PO<g(p!y6K0y!jR~<r||NI>X1htQ@<3`YK>4idvk<-k6N^t+w^tL#TZ;~B9lZc
z^tobHwdbHzyL$_9Zp!n}J?+Rv=@D$_#v3#K3~O!BXqAjN-14~&;U0LbM|XjkA3Z$2
zU(^#LAo?;e8lw-S!>bgfum-8ROUlM3^w4ha5=cE(&`k5WiW4uIza5~TG}+0M0vLgM
zyLhgJoqED0Yu18l&Po%oOWrr;@ug{6YYJ=X=-V4Q-ieWA>uH}ACB45-USf(c2jg{}
z;3r-kyUoq^?R{o@Djt`<w%bTvQY$VwSH|}?WJmaJ{@YDhS*M}#C7EA^z60&5%afUN
z`yaJyF9a7)CkD?y(@HmePmk(EzUvZ1=;NFrmDLi-yUPRr|B1;h!br<OkiXkWaKNvh
z-L(3|kckDt4cC>T1}=!f!zW%YM$Nd*^F<r3hm!^{wrs_C_bUj<48T@`_eIooVy?mp
zW^s1v=6`Q$R^xFixEfIsK#mprR3ePLXHUc7AlEFAm(6M%{hyd{m(D^W98m=g7R^tT
zqF?MR-pJ^OMJB8(SYNeYl0O)lSYPEA>tN)IdP(lpye19j0uz-Lts416AXBq+p)X&Z
z)iRhLRn?fqa_RCV&2}LwvD`|GTtO(><`@h=1LmS=o8!<8Hs}Ze;qcpSBqVg=ha&Nn
z_Q~h_C(*WiWF+_B3EFM&*5eBbFTk@}N4L2-zwzBP{QZ~z!Mn+j4>K`{yXj&FY`;d%
zo;{ZwzW(E(BFpbTdLi2|hFFf5j1au#4D#;?EP;=Kke!c)4<Q~@-yUwgvvoZ3Vfi(a
zcJK)?Q9nCfd2fP*gH#$G1v$bAVdnaRaz`ES=^qnig}5Y?4rBj3TKrB2V2+U_V1kNc
z0{f}7`4@+irNsi<#~W$s`86p06c-%IU^WbCQBhv1UY4_ng-{dC+JCZ~iRGe)smF4X
z=p@N`D~AAI-cR^qrCxhDc-oy2nIJJ64ZS0Cq8VZ$%UwEVRx3d-Z;#o*tfNFJ=pET*
zDq8go=3RXLL%9!rbdJ(~9q_bjoLDNTzKgL;lsRV#y(&%;_XZRwRx2@Ry_rv7L6;|V
zSen{*#967t*x0ic;t30i<=w^y>-rf!>=}m|+*3t6Y9C#h`KB{}1>Ga}hS|s@aXsl(
zo=na4^N|+SL-NB6FCVl)kJldoEE6%suVWeKbpfvNPt#i}NUlh7Z?NEx&7NjClG{?`
zQO*m?@*?GJ?m|n@8z+`@|Bd5cv%qmDw%9}`ipf&Uo9FwUXO-XBtovi$ZmkQU8$l?d
zf{JQLMK|U`5vY5EgenMZA6iRZ^wZ*9^_wovbf2W30<m3BK+5I7GUq1A)c2UuVD_5b
zq<U$=+x_=XntwJQ<c}s%>ir0<4eu%P$8EUtrD>1Qrm6SbVukPHnl2C1uQh)7j`3?!
z5P=BKw+Kvq1%1_G>7GQIE5j1d{6>fyO~+Ka)Bv{!Qm12C=OboSn;7m908uf+jn4O`
zEQaJt5Y9@bDXeU2emEO28Hx*kMXo$%sYhk9yCyx|U<^-qGUr5RFZJep<=tDb$d5v`
zNQ1ac12RPK%fEA%DgXEbxNcGWDzV;C*$Yz8Thvz^digQQ?%|Sg*_<e}r$0cB;lnSe
zvEf$~Q&9;>1Lld}c=0hN_cD~Bm0ji8GaGrQQlKVQVM5hT1cFDYHkh@<%>^l-+uVdC
zP12*s5p*0>S;cd*DH@Vjue~pCzh-_!1n+-!>E{Z+WPWJ<!f<@0m>O{H+}`5Z`w1uG
zTt|$&QqQ-v4O#E4yM|>#q>Q;7D{92XK$&{GVyP<VeJG{|EgfeqO^~m9GWr)4@0Xz>
ze&M-k4xdwSFCb$DIAA>>N?6Xsg4$pJ`{SC3jK*i)BWc>d<5bS&oE3hf)ghwIx=Xs6
zvl(N57a#q0VQ`eJ0<>(JOV+Vy+QE&>NQfmfUtWR+gE3zs^*{+PWo%7i&8}(#*q1pN
zF8jFbLO!J}FP@?gf5Q}aM<+^B;0djdd<DMRs*v;%zKd=o`ISE2671UwMtH=m@6Van
zGZk`&+X*V|+%T_AxX7-06bII60g9Tzym9Yo)KF1lxnT;hDcwI!&;8*d00d0vH0IFm
zTBc$b4bxb?8cba&Y1f@(*lp4Z?knQ`A4F(M<NGw|k*`b=HE@}-rDxg&UsLUgHp4)w
zx=k_OyA2v{Q9h@^$0a<OVU%Tm`Eirrz>b-YySuzkB`x)$@89`^Vwn`sAMiq^+=DMS
zb9kaS9Qqz==ceq<nX0w_^CK)F<k~s<QAFNZA!6W+HM{CFrzPkKRBQ>AWd#ZmpkJtB
z&rw-HpwKAvTWJYnj2>N4cG1)is{US%A?G*C$6LBK=`3e2u}9AJ{5WIH*sEh^gF3Iw
z0^4*}ie321jZX@M2OB=>!?}sMQe-nxU&a!%F&pD7cOywbhDq5O*e0SsJvrTon0x#T
zv09*oo<ZuK3HUU2Fc!V}YPG+b_bZ_x=cdOfdx~|1^nCOWzF~;9d7F*Id?Rbga+Pmd
z0Nc92Z%7$`r{~bPcyjd;ROXErb)-<f5m$I1V$zkJ`9wnN$yB~Me7F;HpbEl0*ZEy~
zUz=g_(^4m_@Tc5?suNzLW!4zOzygKb?TaGWj`D`vzbi4vnGV!k9;w6Vu&V8=68-J=
zVzucHy!$TYHcbYeZHD}tF^@>9y_%U{-07G&U2*HJdG|!Efa1*ZTOW7-Y<-_N#?sv9
zi)NljY5`Ga6}Tc2ERNR=C1bX|xH3K3SjWy@fh#YnJNOdzw&#7TM@Fy7X!=6WS13RA
zJfC+}yIYxCMPO5@l2JD?&s0c%W)O+Ww@S43ffFbT9#_4WvQIlS|0mY|{|=QL@w6bt
zuAQ0)2cZ7?lX1Q|u#;wl$?;Pul@kRuwq_|EOJ>60WEH|sxcR?BC0+Y>**Ui}2?INb
zI=d|)#dwQa#Wfz@K;trfR8HNDOC${eOFQ@8G`*@%vZ}R98H6`rvzMzsnU>W)hP992
zkbQ?=4t(R!ALaz+>+bKTT|gC)8JcAe`wc6e#0YQjFpiDTT_fQf1>h6glF#{X*@zT^
zJ1YDXzl&Nj#Z0LF|4Fm7-Wt;Xq*?O4RoqWnfelTX<(GLV`zh_r{s$2n;>t0Zcm2u-
zUp4L@|Cw)mzuD%<?2Vkt38pgiKyFUlRSN#j@EEfH9W!4)RLk`t{_3UI(2ymb55I3j
zzfxFbI6gVs((wlvvSRp1FNxs#`*y@H0uqm>$cizbKiM|_C1jtU`gZCgmXog1@=%VU
z((0h>EfAX9OlR=H8}h@@YD$jFfU^<R@F<6$PS9wV#*{Et*k?nH=3;~H8j`TNfFTa`
zKXrD7)=9%9WlBM@(m;e__vC2CQkc|C_$YKd7CA64jmop4!&TbG(kaXyZ?`AR<@(Wy
z=jZOqCUw}l1M_l3Ia-s7oH%0Hiei&!1OQ8<yVBxSvJO=f%FT)u6!g$(Zc3X|nlQUG
zv=uFBmYybD;8K`|>57KgJPqhu=qRUl#DI=6YszUt0eyBcO1ANdF7+(Px1cmd>4-zi
z0p#*sQ=`xfqhE7bj~}$i(0~!k)sq_Jj+&XIxx!=k@hhVJi*U4Ul}K3SGwo@kb=ctR
zAj$5D)(uqQ*c>3z70z`bmK6u|lUGGXljd!!7d0%47^lE_>%^wZ53(QJA#LqS=z(ac
z-O1Yv&QQ9r?duop!`7*{Y)BWQ3RctHokyGBR2q34C(@@P9b?g7HIeLn{(Z6IC8sg6
zTWF&(NW&~ZQzTH8E-Yusr5gYzid?RFhfGW|O|G*J5KR$_%vEs0Jeti1ux9Ls4bC@g
zT#G|ew|WNAPXh!y`x;!JO*S!_z4s)Ak=*t5^;yvRPi4wn<6)*#ERO-<D!Km84d6_L
z*QB>)cjIr&e#H|aX8%EpPAaW=JDu6BuF}cIP1S<6df~A}P*K929%#H&l9e5tyI8rx
z(?16NVjpH`!yRu&V|<9Yf@)APZ%~aS76e`+Gem%ANHngzU#T)1<z}(6>rUvYdcj?N
zr1s4$ZN*xqf0jqQv8=4e!wjn_9M>Jai1t~i6VD9m#SaF=o3J4YDtYmc*Lb^`t&*t1
zKtB3Hw9;iCE=K5+%g`=VIYD%<F+ArbzvMz(7YbYWrxJ<QYjp*&ex_OI8D0_sBZx1K
z$wd5?(x9<`?pODhJN5#~DU)a9Rkw>{A?GY}JZ0t82fUWe-IY`N&LDhn<;(f!QmScX
zMR>y9e1=J@c^&*?<Lat6m3UM=K}RDSaJ*Au%{%Vo$mtPnKkm=V4RNxiddjYlFg>Vv
zQbW<`_eMPi?aS}GEn*q{B@5bn9Sxil_(qFci8f<S_1Evq78E<b+*<WH3<9UctA`4g
z4!qhI%9OkZN{S{GeFAqIrj<h!-{RRWqz@LTAYEtA&-9`)ILoq);CSpJlLxERHo{1F
zilicEZS4}O1PakVi+&kV{iE=HN$EJJ_GGGTSFt&)tRHs#(RpU=d<CXi&iTg~KkQ}|
z;@wNU3trvdo3pkt7WfTS&GwM90%?z#PoZKiflWbRH$8MUWq4O5bR#kQxg!3K36p(O
z-y9FJepf2KuKe<zp{5@#29IY7O|)vrd#0{m_wsreR-(){S>V)5hV0<rXve;)FiU7e
zY}iiu0*^D+VxMI?;$(852swA)<Q#`KJX+P#oG^GbtAdt4!fQiogf9rlS9=S*DY#m2
zet)s-B8OU~V*%oe*z>y6KAm8KC!Bq@gpu*>`Xf%}r}$FUyLYfWRnE%LKT=rr+W@`z
zsWabiU;Mcg8%6puyc!h~Xs4P$kz*WhaAw(9;ZBZfURk?#iYe&xuO!}wT;u%yoD0?R
z#0FD1Std4K#ztQHGy4uI-#18ZTd5~%O>|7AA(-6G-nTo69KYjkR^|QHWb&1&k-<j|
zF2*;5GH(?ocphK3<N1Wj{fSe^-x2h>v~}+4{+aCBLE_qRvIIXgi9-2MEyxpRJtsFy
zMarS5*hIA6^)_&f3q!?bpmK#lYv~qo8KHIn>1n||WBzW(A6CEpwV}xInd$c7UDQ}a
z8rz%rRR4(9;*O(0bN`^>uuWL;OjY;<ub0l`_+Al~a9uK`Kv!AyW6@q`QuEP=xppS!
zTN?gBCUwmO@5RXKZQhU;p_B1ih_CnZU6JO!s6Y3EeW`WIn&zrsl&N-KrUKuIz0)cw
z2#4)F{=^t)VZ)^dO=Li;au+=8CjDl-Bg!d<Wf$NyA(g6hqQ#uE1oUutd{;&b0Pq=W
z)at-S=>BJ8{omOW*fpE{$CA*8-e8usc*hm5QMGtR+oGH+!U)x3!YyqMFwtzRB=cG*
z8#o=y1vQv5fu>Eq-GI&1!?Y63D{YC|Qz=H^qLl5P-PdxVyDQiglP{QaI`Nk8x+rBI
z;>=5@D0anip2lvS1T3wxvDOsm!Hc%3{kD7ui8Oe1)$h*Q>szh(I4f6SBr^m)68b5|
zRY*ws@hC(cG4Pm2ZU!&mpv4|A31R(T%Z)9qc-2_Z`X%=bMn2j_)S|59bx8k+Fz1<c
zi_MVrQI-&G4!#U%OdOiyMU2#+S@7Xj%y+T3l1t!O%+28WkQT1@1kG3uVWDZpw<f>;
zHr#HRT>g&NzIFHZ^qViguMZC0zWVL!ktPBHi62@JI{B)*AoQ5Ndcl<*IQ8+(hfhNs
z!eWePKQ*3W8EL)o^36ERPB&q6et?4wmAx&%MP9Vi?itJRHesDuFh})_<}@59#9D8`
zY5k@&8htAX$0bJe!*OECv8o_ANfo1UMZ&8KWXS0_I<eLzX_!3f7z0xRki~Rz=6R~I
zpfLsely>K&JBvnbbjT-IQpKd_&zZL)3eTkw;0rao7SLK;YBV&9oH$Bj4sJQ`gFvH{
zvvJj>V~L<Mq>|2_792)+029d%L}4Nc&{Q%lbk)=3PJh*@d4nMs#Mrw?ysBi{mk5z3
zl{=+Mn{FXuw0X@QT#7~1<oGy$y__FEntd-##hj|zpiJ(b=r1_`^il)!+S8w$K`*H~
zW5K32!*rL)%Qepf<lgm5`n_iR^6@4)6ADE2vg{c2JV;S}5;5)Yk_tngF)aRxdL~rF
zJiGVh*_l^<4l1W!QSEyoxA`nZuHRoRsPS{TmhnBlz<6pHE=>}<)#|Xp<O2xfN_?4f
z<WaPib8I9CD)`}>nA_Z|;6iG)y#ODoV-*lZ)l}~0l{BWZ*&4`83&fH0ZP+t{WM~Cr
zL3{M;Exm^?l;al;w|slvr);0{Lj7vqCpX%uoW)xWz&VO5xlLcU$%Mub8nDyK&mV;S
zK6h&3<AC&Z*WrsM&+=y6SuK|y6k|Z5oeuKfgxQQwd6UwWsgfqN8)#oA;qr8+$4YC5
zzRK#oAaseGp-JmU$#8)_C1*rKaVRnfhyI|PBg99X-k`hfv-hdwZs5ax^KEi*JOd{=
zbL<HGy->^YMeD2gIi5kz=#lS63lW!(obT42qbFD?!~fnD`+b}GTtLsjAj!(>Ypj**
zJ5JR|da{j19Kw<Tk>HuWpuZk(<-;`MZT|1|{+ZZNIpm04RI1gCNL;8cvlz+ck5zOS
zYI5RC3wF4B#>LwlmUFoV0Sx4+>LyPIV>I*lIG5sJ$^JS@)e~>;Y*sQ_O3iAkZm}Me
z-?Kh9!3|QFftNFT5>`8M?T@U0djNeVDpfB*N*q^1(}!&hI4u>PXj6EkxFYIH?4=_U
zjCs~tF<$-n9JPR18V)ijmhJ#?DF)9hr0<lvuW|-t?Q~Y&xdcI)E7Rzgcb_wP2PH;L
z*}r`>4JmO=d}!VJ+)b*;m1eg95~k?62dmfGFNJ+M<o#m4OO@`*r@eH8?PabS?k-Ho
z=AS88F}}Ed!%fyBqOGp}F%*RoH^1fBT-W^Vl3kfgc$90lj|)K{PVxqpr!?FDDPCX_
zB8W8)J4cX^Bo<d;JP|3>Pjo^6L#1D}$i3`XUA#em9(Jrt4q;`4$79_Bnq2cv=UP5P
zJ(mpJptDuOD7SgjH(fSB4;J%<@j>;&(DIy*dcvy-ca|wv<p$!7)O!Sk1*AjAH|jK`
zRe0xQBJZ#K%zcDpeVM9>lDu4170c+^B&6JR<!CPsU^Y{PqGP4<Pd}OPDLY8{+>Yi+
zRp=7qHLv<&OyOP}>K*lF%0N?8$GO={tKcj|oW*~Xr)*=g$vc0>==#VKTNLnh@6Xv7
zfZq3^s`8J!ChIZP<~Mbp6Yx&5@YrspALvyLC_4UI;zIE^Qz#w$GK4kuQ|9t$;2EDL
zi`LG<C|(z?BS@vlovyF^-|#ndzqzyZcj3A@Uig^>WRK&zFU5%v8Z!T8)y%g}jswjF
zI~;-$nkfrkB^1;X9@OOeHq=E*&@P~nTj4$ztO-&GYs~?YOmhlfHei_l%Y23PLRbMM
zZ`qd-^Fr?h#<Mde4YSVgXH$Rz{ZdkMB^04$NAkR!2StridXpdcG2sdT5d%XD-zd-F
z57ui~lEm7NaOEh=R&+Rr;@@FsT>>=<-Vn~=#34<EtZAaAUTK*Erl`l}d7{l<UPZUd
zh+VQJfu6rF%eQ;q%6!V5V0QdkF)e1?Z8Fzu{K?!-Q2$oz^hM09@WX-*gY{Y2=#Xvm
zt46+1;F0=c$vGca{5ung^8MAGcLU<uRK4+v&)X>}qY_HQ;-y^6{CJ&!Rsd%L6}Arr
z`1?iixD-g*tP==}$It{{6}&qw0}}He_@YJK04D?|>2G9H)Ai`M)5bjHudVRxv1Q`L
zj&VVma*`wl%u<~>uM?00h=1lce`o<qH`!h;?p%lOW=@GCe>C>9D6uY5Yz1mW`S6M7
zFp;nmWD23ts=lp6-0}gg>B;`0e@1FMkVm}C1GS)s1p_+XtuOL9mm`w;TM%X|disq0
z&o{zY`59Oe^AZnFWR2u`6h^1Vkp1@TlLhUxs#aPv+!;ph5&c%{8Cv2)V#G+%DuG+f
z>FSCo;_2_xW9{8|lPvA1Z#NM!aj2|Ub8>&IXz-@cbyv<GY%!nFEzL!mm5X4wo(Sb!
zILmnD)hbb(V_w(0q^|>_P9bL>Q%`<tomKE1dhh%Brq<9o$&aIdbrtS>_)BhH{0BS;
zB_O}y6%lj{Ba_-N=y0>lej<TmTy}jlG%)5^UtvZBL6|qdfd&doEZjq%BAi(&6<paE
zX^vJ}FF383^8c6@5LBaV3`nPRc#CF+mAwWU>p^lsA~GRM3Y9KfHAckArjkrHAjt|u
zDt1Ubmb0DKh|{4u1?F{N;4Eg!jawzCM!W5{d8n<Vc5T4;FYW3B>+YliioZ97Rk9v+
zZu*j`pt~A1-~&mR=>IgSLbY8m-Ddw2a0T5(52ZyP{9pDpQv*Fg;R3i|s5H~QU|40>
zWoJj*Cr-ePdgieg7yF+=>6jd!@}zmJvi%NEGimSVl9{BK-|=hU#Z)rk3$5q^ObRa*
z1y98?R*Ikh`f4g~jVgYPbowi|Kx8aiDeOAHtBK6BRYP{?O&t#`kesP<trVT8IY#dn
zCOY@SU+)>-k*aixPL!Fys)p>?c#7omTKz5v%an#39aqw)2Jl3qPe0j<zKEiYbV1X&
z;G`m=zk5<1$v-veElw=pp)?N&9j-)ozEelVaqoi(emEjnheKONqY}mhfdDF4O{o`9
z7QSEA|3r;qM>@va$p!rB?6{mhCY^9FIa=3N1z&&9*7hRkxcc&8KdePUbA6{RxE`EM
zdw-suyZk-u8S7NYuc)t$a>UW@bfHvmBhMx<b78n;;5e~LMsT1_>txU1eZtA?0cvXR
z^OhW?(yhv}R*EX72CD3e5kOTxlscH0p$tDo<Gi`9N(=mXL#z@Y$i6xft(U!VzHr3=
z&<UQ%25-#y0s9(G1eT8mi=gb6p-BU+_kR>@_tM8+hincQ{A-kcV`wdGnt>4dqdz|Z
zU{*xIQ>H9&kkfpXSlQC-gZH=Qw-|;!+kb7`d#|r(@@H7Nl8M?0!64Mwvy1g^Zj|C;
zq1OfUsLF;pqZ<G$N2aEgj;*r&qS>CCe62xgk>dfvbkA9%-!G`hW68rD`d&5LL9jH>
z<}!v2y;6KddVePECMw;o=>BKfZ96(%D+5()#G<7gw-khHTh%dp(dpjwR54A*CL{3M
zuK@aTnOGnSvHBCw9O!G4Y+)-UZr5Z#4EPyHnPwaMixtF#p;C9#Pj@YS+a43mV~x8?
zI7WSS%7MFv-@MXfBRE*BC8#tbJ2CZ0wzn9xNSePSGqwdEI8_bLYoH{x_gTKAipk;A
z4$};{=aq<^rA7+HAn?OhLbS2j#imfe-2WnOC!zYzIp+wu$QuefYhTJBQaK1c)5(HG
z=L_le7an7v)58opX!99R#RjV2rNkx(Iqi%v`3Z=I!6M{oO0yGXi6qwGU(LB1%k0N7
zHRc?c2=&Dqv%kLgBGY#IRVUJ)6f&*CgKJ9Ev>{kNed(20V<`{k<wJSBED(=s-f6k>
zwR@v>(>MGxeR_8Q-UxGY!AfbXJlj~=K=@^nU+^sxO^h{O#M~}o(R-qQ?Lv3Cs+)3G
zfMT|B&<cF4Hy!wY{jw-JnqM{*&!z#n4naK0)_1}4`Qys5Jnzi*;<Ze<R_?2W9a=o@
z{^nq|nVnT=uM4HsPr1^^7vldpJND7JpD^7B)%g9qz0J3=tL(NF(-iueq2GH|`HpVg
znKhCH{5>~c2cr+yLy8>K`&hKQ+wy7Ui$Hq!L#*Na-zEHctKZE+d`Zd=1;rc6axivU
zCM?;Y_=_e)aa<yu$(#_PIP1!m+opL+&S{C=QamOHUT@7Sb6?Y_qV6UpDq^9-CYy%$
z^lQG0C8)+ipW?;*AsO48L}M#pnrI#YuqSEN?VcEk{f<GmLrA=Jg5S5L>vF?UptKaX
zo#H?iz@KW^FCZl<zOq%O9AMV=B7C$+cC}!2E&$Q!)E+jN;i=prI3;}PnmB8BQSkxy
zkm>RF6y;9^EC<5ok3>{<=By^G^@RuhtNSi7u9o&rgjlq5T?o&#yEBeIQtHY}EB;}{
zIg_4f8J>7yJv|u}Z#1mDC6q`duAuMPmm(I6ZVf|z-Ch2)#sg=v!?Z!j?`1*yZ+RN&
zPgMWQmI(actla$vj$8;O5dk4K+Cbz~$LhjZQt_yxYu*xSKo-@jtpqC}j$k$M{<JIG
z%*C7Z2;KJGe9918imwiL4zhI90Zrd>eMkAQC>G6Qu1M`D7o1DA^vbDQr7yRmSfM*L
zwYe?aNcOZ)hmOOgOQ0sYuzvjXGHcdgFk(6%dElHhyv0XmM5@G8Tx?K>ue7iwwm8&s
zr$O3LLf|&41O-IFg~1&%xJoMq&;?2El~A0K>Bst>%`7%or8iBvT%uPqYM&fJ*2Z5y
zakBhCV?*g~mO;fnCyylQ)(^<bs@!CIH*u=#LgvAB#&=)b%PS&O?jt@#{c1nQq2ID~
zqOa$crOwdxVLv8D^z50uy^sG9fJ4mw?XC}n#4mlTRQSA9q4R#6{i|+h;C|BZ0<wS}
zpEh<ET9q}!^mPo;s0t7ie`_)8AzWaHhuLWvT!R!ceM&4v))S<|;n=~JoB@9!6q&%n
z2yhkyG|MaoCI)aG!^BOLM&XR8w)$^4YTFh}1VE0rp>_PQ2?^6=IxPiqY&(kzDs9X0
zv<Wp;B7c^O-AJcU>pQd1DLEBMJX8<M`T~lF&{=e1JHLBnvJHPv6|);MNt6b%H<os%
zTTK$_9BnWsCv)W{iLAKLni2BtO_PKInzTV7s>fq%RI;3;i4Dcv0dV+2i*2BiM+Giw
zbhCx=R<}dqB%Bui<b%#?o@N)EqIfF(WR&T-d@8%OZ(NE+LTZ9vgn#J^sEUd&&PFE>
z-l~7P`{%x=^V7T+S6A)3qUt*{rqF?|{$F|vsIY{Rz^ZV@;8gdoH&=J+yBVthk0%d>
z6LKC;q2NlXwoag3EZy)kHGpn3$==VOk|ap+h)5Ff^C(Rcn7-f*3e`991M<67-&x3g
z+uKb{IGiaMZz2sLNt+3x(_?wex6)hI%#W(2`wLFUb3f<Wv;qUxis~DuOV=oeY*l|c
z+FX`}AGXz-R<vR6fReSrTGC!^!9V+P%{WR$T62gtd~w9bo&I?QMxg)Afc%f?fKgN&
z(k}NHZsS<}UT~jFgL2-of8xW9K(WR4plO9=m7sk%AF7f;fABPs_Tz~1vou=mzVR%A
zbV|{7=q4e{j0Cv;6$#mV`N0N#<>{wi)jz~^S;aX927jCF4L-bI$@)w8&;<VT?@-x+
z{;hx0%mYKn2JV=@%iq(-L+|!{x|O4<_wRQ_#wPRd(Cei?RyU4r&cFIR^lbg#qSre^
z1EueI;};B&NW%F&58)bv=3YnoE#jWN_{exE9tmnCAe;t94YCV!+ntT98bJvpGCL9U
zM%R%6iSebivW7dCO=j}=SN2~BRu!D{=G4umv3?<wtlZKF5a$W4;?aQkY>S%$jOGA$
zYC&rCq*=D1I)M2BOWRNAexwJ?J|A#QeXpL>ZtsHUiLpVaQbEHmKYho094@RHq?X8i
zJKoW?_=ZQUBVXEwhzZxT8m5f6Ykr{l(Fl_?3@cVj3XRTB#N_Jg2!m)gKD*nf+(eRm
zJG5`xwLC{=?yPZ&OP9#nFZX6kf+B<9@U}sshjL>sb~0*g)u1UqcNk=@l{Io2>5K}B
z%*xika7gm%0XWr@(l4R(nXFlbSo@0A9oc7HO(Jv}-&bTis^eW?5I=9up`2{LZNWCI
zqD8&vI8u4ZckGvpgvkAQhi7N+TrrinoX|bb$Eo*1`LF+n$XWh1{%195;Z<_0r9G)K
zvVJg`<ZX$l&QW*sl<c>`v=m8RG`sOjyRC}3z0=c#zn5M6?5`*RNpn}}^mnhRV%*gq
zEsw&^-EoqMqy@%kcLe70^R!cVFDNL{eE2G@Q@VqgDkQCBB(evIezfwfmaF2(k~S63
zAzNeNj|dmMzTX;`XnNWU{LH@cBpQMKi+=a^M?>AYn2^=uK^*gi^|ANOAx*$D)7|gH
zGB#|B>G)ayTfa^%f0R)$ozQbxPju&rk5glW>Ktg$)kNOW_|jRU_X#N06P5*7ZapOs
ztx?<31QQrq;=I9K7#hZQe&+~_SGNsZVyjPlTF;+L^w*HwzbvM@R=QC8jN@GiBs8qm
z+F(=&NyM6wrnr+>#6??t2CM<jRnV|NPGpb16vLS@VC1)Wb*r5s-ApStprh(5^tU+#
ztR$NL`XlDG^G{6quccgyL=TobIhUn~&b0Wm!uHxkk-8`WJ1Gas+2wSrd$MtsF5_rA
zZGJJ6?UcX0B2*qRp`Oe`eQ9@@Z>F>`{G>_lB-ZrlUqKlaWV_k4e(YObdbA3Eu6dy<
zh3ZXX+w0`e;yMkfjZX(UJTD_gI%1mh*L_x;n)Kc2u5guH4C!e9n?p|@%SQL#W=NZM
zw03}2Z+RXz+9*QoRy)WsrA)lBc8aCff@O_(kyj&<U|3N5jS|VV9>$zzj<&_Ue5xa!
zgTI9MvGSHrt9-VqeV>On1y+jpJ050~Pva?1@*Re)2HfDSvKC(Ii?Nu$kTTQgz<;N9
z5)%Ck;4X~;Ss2nxV)>vu-8)z7Rw}R74OFl=47q#g?e{XkAY)HZRlT?<mK5d@{RQg?
zTGJcU2UR5$I4bcv<y~d4kb@6Y`}&JWJCrQvpMYEAUp}>R^ZCM852`|X@4UOgbu;+e
zCreg^>yTSPP3N!GzU7%ojzHhImCaB$mH4@T@ZrTMxo02Os><i?-nhGZaIJeig!%4t
z|M%#avmy`vEpGp~Q8nnocMSifi!=L_(1&vceh-AG*!@FgYpQ-7`Dh^gW$@z+``6LX
zh9lNk1dfiSDmXj=AUD<1Ho2w%D1_YR?Z`_*M0tX7P#lb#Ojf-jXzYjL5j8wI7Vf9@
z1Vo}@IdlJ;cBL>L8t>j5FbyL>-KJP#Izh39s+WmPX}bVR<?yI{W<tZ1a0xCcTEY?I
zy|rS2x#S{YJG)yFky37E_Y*v~LH9I;lAJ3MWtDGglTzMbL%OOCEk}_WDJ9gD1wZs~
zmk_Dsftei?J#B7Hv`OKxhnCxHV|*vh+dDvAwNs6D-2woIqk5Pdz_SzL`WPx6oT@*k
zPV0dqiRZ9C`I5JC|JiQjwS;F&kb8kHQE;B~)b8|(wAF>9k4{xxgGF8v4sw(9&R;K;
zdso`Ky?j!sCMa?P9l50yTpupC-@Q(>$cS>7?AmcA-;+AN52nI-7XjvINegmvs>Bki
zco+!7_zJFgVIq{F)|h}bXv_8iqf|kqnR0)U*%j_tc~svWZqPy&i7W?M5=Il6aIOP&
zEqqmpcX!vMuNV88Rz%gv*}_w*FYT4DJ&||r7OD05%zL-CYVDOpLsR|SJ>#I03+c&{
ze*53+TPam#wN1{7bos#tn$HF?F3s@`azYrn%G!q9y&;M#c06<Me7NvKY~U}JHYKUm
zV6a_QU-Z#u;eFUlENeY<nw+6Xx2!^H#eiEx3DknefrD&fm(az_*a?dhdXV-Z*Y1C#
zh-bXIa7#QRY#n+og+tFT!VoSN^0@j0t;_O-4u2tm>(yv$YT(<4SwoQDqoT?8KaE)x
zO`QB{&U*a5s*QQ*%jo)iA4IOzlZXArsiBRV7dy5wA(KDvszc(^Y(!(Tv=gfcviVD(
zl}QRO9J^iGyx6`*+!F}GuViPfAr(>C!LSiOaYiyIj;eD4#7P7^%e^G|aUNwvcaR|$
zA;277a0X$pf#=F$u}t99LKqxz<SoUVn~BtK=P$(za7C!678vC|!B!J7c>vo9pu7Rp
zP%$L{t5u@Z67)V5>z=l4Vf~25lwhy9?T+QS)Yz#bLN*Dw2$2dJA^mP~pBi<UB<Qs2
z0d4n2r(DzI^*L)fQPpugE^U9@<-Y$r<?w*f1b&36Vf=$~yhF|2ski1foLeulokX?k
zTzgz%Hy~Del67fHXL7XG^4WF{CdC_U=AjPnxEvI<WW*QNa*x6E5oq~gK#589QSl~3
zFh?=5Z3}vuTDFSHc>dr_%D`(Ovm1e#N~L-6kU5t<82i1&rH-AoErHaFfwSsq6OV&u
z`R<uk*iN=-@Heqfjc-b^bk*}YKYHn_;wEnh+)=}A{F?0Wk$jJ@@6m+6EDF4gY8ss6
zmlnpK@3?ZO+Wc~kdLJKc@k1*`U*odb-OAl#tf>y>FI;n9l&pjVTJ1F#Gxb{zKWA5x
zkRbPcQ+rE7XHv#xCs$g5J?ZPE5!F0T@_&>llVmzD%};N%=&`5XU17bA5ZKI7I_wiw
z;>x#a+Zi<O{7h!~bh|5);hE|9-@7t@n?An!<0$>j?UYXBji<2BqL24n7hWZ|wsb{1
zJ|ic~!ueaDD*jodm;EPueo^}={->Vt7vF|(wgPI*i35g<fz&9KyWk4o?XvTb-g$Y7
z;hZCTbf=bNQD_rR(Y5i^JhY}+qC@i&fG#{WwhM;I#AVp>UnUR_)b8bNg&CsgIP@TA
z0ZvGwfg&BJ9>n;8lDM7$&D0-DPfF#~1`Ls;u;7j>sXcaW6D4(gUYD?P<^ifk?JyOw
z05Idk`udFhCdRP>np5o-B9nGGs44$$o)h<`0^?GLV+D)bL9i94IXc;)vu`Zd=-K%j
zF5Rt#>=%%!?g$gJB!2Gj&hXY$v%@p8p_7-h8(#SnuoLp)Oj=#n9yxh9pO@JYI8E$b
z027UPt(^j!<?P+kc5Gm(dXVLTo%$2nDXOUUjWoaj8|0FDZ+>UxxV9-Yp(EgGB;+NH
zmH~;?!t2rI-~Rv2KRs7@ZdP#2V^KYVAjOyZ2(9E-9WE)H$>62f$1V2`ny4ersi7@z
zFJCZLb3}<rr$eVR`7Alv9bf0VJBza_36f4TC?3IeXlnEN>I1gk*-qR0FkSE|=EP;Y
zD)mLa#twU$aOJX3b*$_sq7FfT^Nv9*?~BMO9e%Ghk25k|qfkCDNl{%LHPyk$=CS|D
z>ZQ5}<Kq#fR=ryUxXAP+^vJHf1N0QQi;9EF=R!Gq@FK^_j5TlkMYX?ZX@v^2KyF_k
zx4f`?oA?mW-Qg(FU$gqXIi+r8{N#QL<Xx<WP{uPGzJat)n41)ZD~-U$waYpr#~ahQ
z&(|w;%Rf3u9VY&K@NoU#^w7c2YdWXy@;(0gb7+I^AMlW4`S+UM-n}@w5ruK!U?^)!
z^%oxD^%{5(G9$;Vw{R%?w<`NLv~$c)=02UC0!D&H4@a`_p=kJv7vxw|I0V(!4iiSj
zYO<U%4l9MzqsXdhLX$|=d_dlMi@T2|k7kHIK5nd&iA7>yf&g;cL!MkS<HsUFE#2-H
zGuvMW6^Sl-qX4`WsF2^q(30>_v_U@i$IuVa<oL1(hZ-ihwRWYkja66;v!~T{ac1d3
zlYSVXxU(*zOBJLodJCY9?dVcy4b#3=NEgk3A6diZ!h56lODc@(X$t<xS<}8NG+}&q
zrAw9?PNb7GdGaSs_#V%n08k@x3R*J$Wzg5vkrG)CI)3>n>Ggt_C&T+Z#jPV0K&F@D
zULlcCYOlO~6u1Nx_qgt8w?pZC?q72!@cFa%KW<6$KFRYil{-BhZnCs<TONJ-&)Y<U
zZ?D39o@Irfjzv{f9;bn+P~e(<osk~s?aoL91x@hd`GP6<`h39yOPYpDLvUaAS?&ee
zoln-II-66YDMJ#UWjEF4q6MKA6(kHKaTYqdowb`$y$|>iC7LrR(d+q{2j9P@)>Y3(
zwNnmXm?KILh!J}|k~iXc(+<yHn0`Q68C^D`oY$<bIQX2~RsF%Q_Frk8o#tpexnu*d
z5Eo2+P90n;Wl=_zjh9q~n^nZ6sl{|Yn|(hhM5pGrE5kP0rY$1@MDAKVf7UU~_&h3V
za8zXNTLwo=;>M~na^<u$UpnZ$M=pLIydJ>IyqTRKn5<o#3-br~Z=Le#mk;3G?N`#q
zeKsUseGJnLjnPc_-7+-GdQ&EXg<Tq<@b3FKV*dV@Td$M9lc*T++aC`<5A3qgEh1sJ
zCEi1ajla;5Wa(o+R?zF^Yl5#&ps^gy(7Smw+b{B>+3uuVE6KbNNk8Uh27uzK)vbk?
z(Fm(L!A6`-zK6Mn=|Y<#!3J8iv@vd!J(H=gjC^OSa#7v_-K=Q^ED3X6M9c2_=Ek|+
z<vNZ9;7|}pfL0c8vRE5%8c8#X-g9m<)Rg;_sPlH=iooYgxl5FMwjCS7qfOdHr9$g5
z0oouc-E!f;k^nHbKJ?c782zp<4%_uG_vyB3a*C1B*eDmq!AsXgddr;C@Ov+HASdV3
z(Un%!!Xk0m)1h^S^70d<Ur07bH?Wn-gNa&brvQnI%zd|;_gEC%RSSx+a$vLJEPU0O
z-DBy4`Sg#NaS=Mci|KxqN=v+x_E%0_J1ovu&V^3VR8&;@0H~tOqfurM;&Zt3=rn1|
z#j6dH8HP>k3N_d#_HLoB^ieGMh%Y36SOv~yN_0bB2CWUkl<esGvmO2t&VLTTWRy1;
z(!RYfIMstP5s8{hxCiirb}6>ap@Jp%t@8)b!I?~n-I7cMUlW)w?D+%&Unyd5LkYj~
zwLFLD^R{d;1~qr90uP7ceUazFzsQ2;!l6(q4vQj>gZEMA2(NCmsmKq{J?V+)-#?gd
zMA5C4qUfASW+Ui%1Yl8UI(flF@Oq>mgATp?FZVY~ZtXmWI&~G_EW;+VTdhhR)1b*{
z2%mPS&$w><ZLfwYcwDO}^QxH~lQqMiSifktp=-jgvSoSOT6#gJK&7+U_hvx{k>40I
zO;b6pkFjb4iEHiF%!eNQ2?o4K02-0|RvYJJE+sa}l7`KVP$^J06&ibRJERb<lpF?t
z<I-7Nm1RVne|_vcsREBz@u-}W1r2SW@}47nY&m|ku#I&z6zhdH#VE%z5&-es%o;Ar
zZswA;5Xn3k*VbF|SlnJ)kFF_;b6?j8L8lp#=Ud3~)?#LlFOg)siV1fhjtK~l$fp;M
zE4!sk*6a^??{RstBqW+buj95;t&78Rd0uu_o#V4`&u$;+Y&}l9*s}J!kk!H8eCq2^
zyUJ`ajYh2KE>1#!l|xU4J6&?klfUCy01XN%wQ#4qUb_f6{Sk9{4gi##!*hHXYjv{m
z9oHo{0Bgv+rph}_^Xcl(=taRayB7lnG|Q@boF2fi_z#i*h_+rcS3|NS19aJ>1S2_U
z%EU0IQ;?5>XaZHtdNn;2CdxGF9bPOpCm;q+Z-?)$2zm(S(cV3A<---Ngt=Wr>`J=G
znPlx+n>8nTf6Llsnya9TAflgax5?woin)2A)&5ZN+3C|=tXIZ_LLVh44YEdz34Nlv
zYFxJHA0xD;y`=eTSPZ2Q4HIXW6RM4stkT)(5tsz%COuK_5kepVcIZjc3&#j0Fnem^
zB1NGbIs2-gGVD~*oE+4ziWV`Po2;Q@YM<U^3L?&5-~8^1BJ&E<CjeUtgC`nCtTAJv
z3sViRE1!Y7b%Xqpp}_B?vwdU^9`rAZ<B30tchB`KZr1+TsOZh=KYdbtOS?k7=iv9Z
zhoKB#e?3HC85k;CI+-EEt*+cBcRvOGgQy1!OC&ucJ{V5c4;^P@8EV;<P9xy1oP5>a
z4>DuMx4Bv-5D@u-2@_!koU9YO%h|N@0ET>8OPm1+(n0l*3B-0tA{NcA45dc{swEgx
z<zhPQJ-_reF>-ExK4=yTNdnB4k%*yKBnUe-v$$QFz@Hl`pL367`kC_{>102FfouCW
z2%(NXy5EFDS0wS_JS>kglIx*5(}|Z6Ng}2JPeCD)2TcU^^So>EK=q43vX(td|310^
z2V6xb6Zpd_lf`Ab&`G2G9prr4#3AR`Lz~g;o-p=RlDFnNa-sgiBxkFAF3;X8b}cey
zy!D*^W>NiO{?1=d57auV{VBRQ5Ctj(pSQWWth!d@I-0QNFe)Y~Y127&gGx0lcdu*i
z8U`*t6Kn*WtX2#?>Uxy9z)fPt*JL;T#GW%#BuwHBPf{=jxKQjVkcBFid%*7gO%wAd
zmd^jUTG($o3OGX%3x9!5M)G@BCe`zYS0?q}Igd)3TC{W2#CV1Kvl0aBTVzs&`zn|F
zjSy|S25*zak{}DDeV0)&odarkc=Ez|IJO|jT=a2g&;inNq=8>h&RxuONu*!h<nL;l
z%&&04a>|3x7DMBHgo+v&=?tT^deh)f2_M!puSdWC-u{SO9l0gs3h*qFlK2mD`2#e~
zYE+L7$n?LSm>+sUsv~vGIGzXoiD?G|purT7k)-FE!0e}Iwuz>zLd>DkkA)Xd85p5=
zXl+5RarA(Y%hPzOen3*U-_V2iJ3or)iYhfy<u^iyOfg%%AIyq!pId&uU%es!dC(qx
zwrfJT_T!2&$y!;(2s#C18~+0~q*-E_VF-S8!9<h+9K?<=ih1*chyj7yp$Gu8%g7`k
zvkzJk7~~z0G1lf-9jnAIVz&F&HWnfA3!ni0yD_u-=Fh~11wf4Cbf=15*CfbZEfzA`
zO~jm-u`FH}r~Ub3@NB741xmHtk9kA__AP=g`AdwXm*w4a^_yI+y<ZeWaOnlkHQNCK
zhQM>zN;WGp;ER~R;u&+^uu7`~P=j6>sQa)(f`*$^E|fyMAYMF1+p9kz)?1F{+MBk5
z(p%6*D5gC-mla?;9jwnx*NcuHyr;|r=_^v6bxupkX%*w(awxMhT^dnr>pmkb>{{=^
zhE0>LyD+|mZ8c*txjMu6oRhCoD^+GwQK71oO46s3I%H(e|3RLF>vQBE-r|~W7g8g2
z2wmV36zA2*f9Pp?a3NtNwzaQ+1pR}p{7PM9d#%$n$-UaAT4`UUN<?IsK@Z&otI8(6
z6E7=(rYCUTS8XqCDYd>Hi49vryo&-@;~*Jw+2HA_(}r$+Y(d;9gAbgZ8w&-};!o(V
z?o!L0H}NqTd4qcHCG9txNS+Xbtzr#N;SE<bB-+5RH;<WptIYHtW=S(y_zdQyiJ?v=
z)_E3Dr2ej)p$e6m4W3miz(70d$Ohu7I=L|7+Vl8^xtZFhy&Wb{6t_YGChfrz^dnVl
z1eL`;IN+E`pfkCrQrk*eQO>o!(3OBHC<b)e-2*S|@y5)P#vZ1IlZIzLW4;Dl<E&O5
z3HVGWU1R@8+EegqjVe#vLL~$KT5$XVw{BO<(O<m>L5Y{`q<a+CS@cevgDczN`rV^~
zW79XZtwkQje!`y2HWY=t-&SbfrakN(gm9qC&U8CECAi(W8fj(!0z*AXlbEQ{u=#D|
z#Z~Jq-`QH?(Akb&6pfG|CUe@dIt|J!{gv-rRkaDzsE&}EbT_rCAf@|fMAV)E7#W0w
z(ywGzBV&zoRgpyZGITC|=Hw_B+D!wJqepLGHz{aqZ&R<evI3gu(Gjr2K%Qk#5}T)h
zBuT6+Uv|nm|GCMTnn!vCSFT?&+bchl0v-)6>N?Ft+xjZox!k#O?S5nAL{2;QsBmds
z!kl27A09O6b|us2t+VT|<LZ}pmb;QIKePAMKD51aSQsYXtu_cf_9**NDxp;JT;<)Z
zFvTAP`O{wfQ`84?C@?)q*Cz-sxo1J<Nl&mc)?C<bFsd{_*Y@s<?vcci>HXn)6=xg>
zc(;hCaY(G(f9oZUNgO@2u>rWs3$Q1RZYhR(6Iggs3^x}^)xrM{U2px+)C2bYA7H>X
zY8#!N4H%=x2q~3~8YwNMbc%?C3Zl-#=y0TzIz|YHNQ)py>L`&A1q?()#6rNX?>_hU
zxu4&jU(TO!u5(@I{eHdV_vBHr15b9(A(T?@ymaA2q~EHSOoqBj3raZS^Bh*@>Isq7
zy!>`yg@$^Kp4+A5@Y%@2TG=*5<FB`D<CkoKiic|tB57Q=)WgP?NL{8k8q_1lpOS8c
zw0V4JzCTFZki%>OBILlBqkSmgYxhI|@eWLj<LOBSt2_zmXaVe?b&T_Su`ph&6IjJL
zxN`m~ro5b&I*fyV9vIqnvfmZ;03Hvf{FKsq5IQ?QIC0$M>$fCn?#b`OThCwrQePb;
zGWq-LF2j<?e-f=9ovH)YXnP87eT$3J`8f1_ds=YM#G6l)Kd6IJJQI7qjP>mvyn6kv
z=J&z={!a%hV*lK=eYg6Xv$BqVU@~NXZeQK<`T2eShh4$uz7{&Bco1%?&OJC*3B1_b
z619)Zg3*IOAIK*HUx&hdop~*1G+f08H)3EV9CSDw>h=dv9KL|sI%K_}lm<`<ppXLC
z=34}@nXt=yY%z_JisK~HYf57nu?o;0u5?C+)6I;9WOO#0FE)l7J%U$l;J)U9v>pKy
z@B*j6Hyy`nfhym6brcsjzO`@8RQ2*LF1GPHK~``436E9|Q}al;G0yU+?P+H9IvU%)
zUpl26M)5dN8;0v8Gt=5U!juzY;W5gQs|b5+bR<04Tzpefa9IEKDL+60BjWFfUP6Qb
z62aAB;|o!L^zFM2H-7MBj9>D9QbH06@<N1^rg&Y_yczDzK0Kx4{mqj@*n8~o;`A~i
zsQs9cgmQeLIYIv?%U5ftm-b@8*5?#BvuUJQw>?>wsQ8tIICH&u(yY+BEBK7!D^A=f
zxh|MQoCnh}9&-TqqoY)8JQCp=@<q)f{(6%X$11w`AhLLUE(uX#l-MTtx=tfdLcF!<
zG;jf*BvF*&7+0W?TGvy6XnX+jwcePDh;B(aid^Wwu4dTd`?R_+dazwNYhj`A+J<*~
zH%$oVaEp+2-R|Hmwdn69s&Ub~z`=@=jnW17J|#dym%?t>6mTa2n)SGqGWNSwWbbyp
ziFj9d#wiNqmn0k8yQ5)?6#Ej+abvU*6!8$LUQvsaOpS?|?#jTwFMTbI3x3>rBRwgl
z>p2Or#(a(u?5qk)iPgVV5!SSVk9{?m{3JT%Y6S<c{`K8ly-C}<VV$E%e}<3WdL})P
z?>ZrI;B%9E_VDLDyZ_E@z4`g;p&)R6dhnN`A$Om@;`V|1KH;lMD}Gnq_`!!HUa}Jx
zFQ~^unf82vOTPs$JEYFVi2WH-3gKtU2wGW;$}-Hjhi41?s1q|<%3|F`Gn=lq5mxdy
zFoMGcoCz9{CdIJmzJxIjKAg=Ct~6j-+XO{|P2bDVhF7db2Y_tV4ao2i{Ay;e)_<1`
zB&J6r)APD=V4@{qRBl4fH6P6KHfp$q|KZN1k$jU;-0!wD32@hgDT(ga#r?3w73v<Z
z6V5jCW(-nsPtUYqkE1*g%2}Hqy(TQ<H3=yyY!hkkZL@VuXRcyR`M+Y4X}XsFvifUo
z=i-U6ya~(ZJJ|B=Vwa+_3pnI1+}K)vecfvS`aR&;QCfgsKiq8Nqr2*R<*41midJ@j
zj;D=qU^<4s$i`AHv}1-lqKttFwTKw=!D9nRmR6)*sT$bl(5MgX?Ozl3v3-oed}roC
zNO)sR*9zbvHunr6%*q4sS-EB<BR}Ql*DE}-ghHD2t$H4fD+Wc2d3E)L@%gcerBd>w
zN<=RSFN*$AimruExe$SIWTI{zubPSIUQz3}`((in-&4D9N$B_{!X!tUj?P;;u02y{
z#lM@GzDbgF_3Q)U$Jd%4sc>H4`VJhf+l`)W_<J*+c9xQyI>YG-nWB3~f4-y8RNSRf
zw*BPkn&dkS+f6{hZY**3cXJ=k^NVV{9M2>*g)$qQ=s14Q_-&^_zcS@o-K*zi$Nvi6
zZ43P6Z}+=hW%Q>4)n;!1&pStv#@Pctl)q6=g=iat()|U85KT3h7NV69$|oSwd0%5!
zSK2@!!8xp{>T}mt!?&k)x?CUrl-)L94KY0Z^a5sgsap#;3%#oet7>k&2&+9esTWQL
znv(2VrIRd}Tzdy#4lE0>dF00~pq;oUb4^M>B^C+K_-~8$hahifd*1+cNu4Wt_qvzJ
zVi7RsHIBonZB9Oh2B=1#G_{_KQ`qWcBy$WYR1h!_%gGo}66L2MXse;JAr~-5#>Q;y
zZ+XfrmKF?s2=X$)zlJTAjQ=iXKlRHFA@j3!4V>59U+2EueOT_f84;8{or~FLqkMrB
zn-yP2rcAyT{c$(}^*+N+Cy0CW_PL<ohRc^i^Bl{sTkU$<wAjUAb*6sQZNKqi0dF+q
z#A+-@?okBygjN2!I&$wjV?C-_JEd@jU)yTGTv8>*kdV+J2XR>CoN)>8Jo?8e0b-q6
z04-^ebPMd(lS&IfFx9f^M23>Lk!0rmSDy`o^K6iX=7Vk0rkF#TrGP?AV$S8adZ}rl
z?cGm@jsRE32*gIY9x>so&Tk4Oi&n*TFV6*hK@=N%ua~>yb+_h+T46UR6>)ZQ=xn{9
zDakpjzP9jJ<tF%xNgX;L5>f|NXr)dr;>qbRm^r^LuU<m-4r(!eQ+Xs+2F$h11T(IP
z$)^gg26sCPi&>|NzqI)7>{1MybPK&GU0o<7QbRTXh3qT$A(sbvwr)HX^%4nxHz2#k
zleJlz`0|G<uqENmiNdTNU#$KZ{-b<9zVNh4ocwb60b<MF4$)TFXY!t>1i$7}9}B-=
z|B0k`MeP3i>wgvGUl9AZ&#nn59EkQl%6)fBbT^HR5_rQ)ei$tAo1)#Jh&*spP~!Yh
zjJFNnz{ANs=qkpVw(yXSo+X0PKqsT>&@MrOlo?-l!TsnWQL*8fryi;Iww9li0t8nk
zWK0DGQ@P+y!gt4JgISPGz2aqMtc!1}njNWNk6K&7?hphNRNe#Z%wSg_hd8$>b($zW
z50Gf<(aWkPlw+W}!lz#g95+sDq;hXKS){@;oQSEgMjXD8n1PM$DY2>YC>I^8@+}v)
zsfy{Tu&JsqmuaY)aK)z|b9r|Mar6PO$L>ZT<l)bACtmYy)t#OuCRbCr3&?8=$*HpX
z0r_{0^}95QkkZt}2U9h>-r*tDk;?5F>{K5!*dWoT>qCF$B-{sA+WIx9GYXJAT;B6+
zq32+9)v4OsI=-S9n;2GH>JKg3$9-LWRrX2LxM8Z|c$(1tarnWBL$Q*v_ax6gNqu>$
zL@n*e6S-FxeCAIgXhGk1Unbo?dFE1H)ur7k+FGJkurSuzXzMjx48iQVtXh`@g{^Qs
z+RqNR$|#u*JCX-zQSn8PPES_&y_K;PoEVmDD~7D1O61<G+35IZ%A$T`r#Y$eO+tzr
z+fF6NGFM#+u4QS>7k+%QB++v1%mek?RS|!GS1VJ?&n?zF7&|UDQo~TGCtCK(_AC7^
zZ=cv|b}lb>BP4d0YiwZio$+abPa#?3%O0azoi2MC*EO<L;;QVg>}GCe-;6E!nxPV!
zN}4h!O|>`uqRdozvEbrZviIp#{i*E;cAZc~WNMfH8Lg*Sa;z8}YYjyN);25Vq~Dgm
zBZywxNPZ(zb$ng&(6a|SckcHtg!u#KPTQZ$E?57Dee+p_Mfb=<RQ)!K(S(Dix$n)u
z1T9+}B*<<@VNBzF7U_ebW~omY{x#?eTU?j}+~RO>Tm@vX1O~*8{49WJr8v2LH>*kI
z8>`W<W($icmRgz5s=^#2l=bjROTn&@{+UXuhWMh{Qe4tcdn-%5GVxyY>}HhxSf{}?
z*xF;)Rw8by=6Io1+7q}>P*ja`CykenEM^+xj3lIrvJ{5kidR8sqo<?|u8v^VyV}N-
z3vzGjUB<#Z{kdEjHZp*GJi|Rb{-xTNNXnzePLItsNWD$rru&Z+w~M(!qLu}2FLq?G
zc4Mcs#Tz@6ZHD|-xK7|zL%U~xh@Pss9Dl=o{$GY~bKu&x*Q967{jwm95WzBr{q1mj
z?svif_c2Y4dlHl<5h9qkbwMm?x1rh`V$ht?B@*S_t-yBeP*=m+wrh#Hhu-&O7B<Gx
zc<@n+?S^6YpRXRAMM%%Rgh|xSk9Fc1zK5M*5p6d03dYjr_fI68_ot>hQ{af~w@A^C
zi_2Z5_ZHbc5yx)y8x*KfpTW=2@L~;<vuU3VYsup=$DZD}bx6=9;^>pQb@mm*q0Lup
z#d#w~&qoscME8jCi}jyZ0HaxH1sYEkAe;lKk>GDLu*VX_@acDN<0vl{^G~|g-zglw
z>?w@Cc*D)GbfRa3LR%Cy=<tVRxZx<clOvXFpUUSVCPsCg1TPw1?&I;;T@hHo9S`}y
zFpmWkKEsk>=+7GFbj_O8Aw%=swK;D5FX2Q{v*Y77&5yiq)St-pXuOl!_~Qmwmai0^
zh>;%%HwCaAhPONtmxt6(aRsRDPp#Q6JZzFST4k&5#rVqil<!N_4KX9F_MSCG&jchh
z;SP3Vqomr7ML#xfaG~WV>S_4FC~Z?ST*L$Hv_X&rZcfRep$<P<LMi#w_aiW(T-opF
zOZ4%q=mFIst(r)EDVt+^a$#yTt48i8K1LF`E8K=Jsq17hFIe6Psm>k#-Tc)Sz<^=M
zC`$vf*jpH5#57=>gdhY5=;hR4sKu6%ua#~i3)RNwuBTnd$*y0)mk#>RZ!%lizSkNg
zahuwzp8raVu2(88_)I<8G2Cu79edYKcs+4VWIdT3jG0fLFnVI~y7Eie93j;KgV2`@
z@y0^75vW%2y>j?~Bf&$*?@nG5)YbnuZ3D$UXqaKP?;}pRj-;1~y{s;SeZ!h{qzcqU
z+YCF%0TQS*mDTXMi4HDiXH0=gk3>+oZ~m$H{nQ!BBZtFVfCc@BfD{ODS|BQe|Few#
zr$453+mj@PGRRyE&lqkMk<9<}$M(@e3aQq?M6;+oBgKNT8kc&-nSUB7Y&@#LzU?Sq
z#ZVZDN;OX4y=qSY`HT96Wy@xd#E0ukSUUnPE5*p7w(EzgjW(tlgT-%K**jmF4-=BE
zu!@?gj1M~|{p>K^=cULQx|}F)FZ&Z|;;GDA#k(}3p-Ge4uhS!BRTS;Lm;<lkAQj~j
zFLsTWG-yk7b`vwg*Sl%P*fysNn9)}U#E9pTkQy9qNY4}Vtj*7z(f+xIP0>GhYPL?}
zn4B1p)wySW@zJK}4_3*}wX;hG{8wRTov-da@)Z5U>h<T_6^kMUBJ0Jm0Sd=M9xp#%
zJ9Fopw%>H>x8F(F12>sG-*@^CHo^XBHUIkgk2#$T|2y!|S8iv|rw$Y}6fOFXvmet1
zga%@=f5H04+|+&>5_cw0Ll-;jcgU@JW`9R#><>tTpM9MmD+0((=Xe{FILZw5EJ)km
z1Orh2gZ+$wfDjgJOG2mu7~eW*oNd~!G1j(Lji+wcROpPul`60_0EcvBhc}94v92|4
zCJ^Qd5XMH+I6gWzn}tL%tHimX6fvCMO~SpboM5dw+nhs$QO=`&l$GPJRp*`a987D=
ziLJl)^GUT@^8~eLp1sf2jrqu~4v<Ic%sDk*<IX(@rX_V%t8ZW@doF*$<?;>I>t_Zm
z!WCKHy_{CKg6c$F5D~WZnhlY520fx0%!O5la6sbll?|`Q;yK$`k4M*z9=_y_POvCp
zxd$?Di4P_z1?O+m(Jm}x`E#lxH{BPmJA7j05+&?3IA{5iM*UhXa%!?cn6D&YjFg_k
zEVOvo_iCQRQEQ~mY~|<bE(wZP>9%lPl0SOAxg^BS7+tm=ZV)=q5pKL}%FJePRe_JV
z;X_y<CnczpiV3&`&c*+xJHoSsv#OKs)fK+WEa@qGH|BD(v}Rexy`@S&sjveN%zZ5X
zDAJQwu5yFoj5&^MEZ<hkGWePQiK=<tm6`U84jp3hia}EEFq~yn=ponDjpO(Xs6|8X
z;uK!00@hQ0v^jtJrj0>Key4-$eu(qE9SUV~EHqFKoG=Nc?i&&)!3vQOS2}uk#%{l@
zXaSZ0c~9U;@Ue2tGFIsR`7zf_f%A9Hz)dp#_EGl>?}VNb_q*KVVJke)+ik}6=}x|!
zpJ<iBrx$`&`9ZI^2A%rFrK`~Y+J1vPJ7?>80q`{Z52%MAv-N)$1xNh@)&HG$B7dtc
zQlRI%>?N~9{tVvwOI_*UhTsu=W*Z`it0OS*q1QJ|*zdf@F@OU?udl{!pm^3*+?Me{
zLwOrj^0McV{lgzhcqyn%>ODD#2AJ2)+CQS2Yh~HYSL+sF#xqD~6#*_YlqIiZ847T!
z2^k?2OH{ih<-0_>+cgv$+!!Z5CX6o*8SNdXhzIbxM>4C`R&!dYxOUDm>U|%Rqpz48
z0u}%%?GkynrEDk9+C8VD5>=HlT_t_gyRi5&!=B(*lD^wEF<ODhltUfzMX1r!#4*ZO
zWi-N1Aa#f6V&8vh^{)!!o_hf&$|PmlUJ~~5G#(NhUVC{a(4s8TJ`_d?jT6fxea;%`
zqU7V70)lB=g&1Pn$S{JJb6?YNq+pILy1mAV)&sg}!3n<ke6cxGRYtM!nafH)z$w*L
zhE=7VOo~sWrdNYy55F|P<DDHGordY^UR+MZADznWlEXx<`1GER8ky>zXoo(-&4UQ>
zw3L}P{~({?S>;qpvfd#&MNXQ(PT74Jl^Rc$^8`-RpjsJ}aWd?s6^Cbd%UCVTcUtiZ
z_=r$w^oB%<OupFYx*69KXus(Y*Oc&JaM8p%>SKoDZ7^>urI`Qve%T)uF>Cbla+aXe
zlsf?sFSv~gJ!9J_bD1!1NS7!;$?(_g;pC{ql=iNXJxz>F+;1Ez75CG4VZMHro+mMA
z5l(J0MM5!}zXqjbXfx=w^Y>r=^XfK2M%AdDYl)m*+|UWVktBR!i+j9)jpo0aqtXRm
zT`(gB9J6u5;!k!Vmqq_Yy;MS<{iXpe7N!WSt?5#?maxN_#^u=83wESUxo1zZPZhkk
z&d3J=5~5u&@E9L6r%Me*t$QdepoVLW)I#}ez?T+fWoxA$K7Sof=0jLXI=4Uq+&S7o
z5pbyjCBRzsliWL>qGQgR`{i@g0L9&G4@(&eD&uWR@P5G)BqGNFv!hz8)H!NrKpopV
zh4*-Rv$*tLgw5NA?BF{cqiwZ!Y@deYUUj=)xP3+VRhe~O8`;Ji$~?<R_(=is%$1J1
z1b8W5CO+|<E)SMJtz1?#8Y`w#Zp5QAxmY^tdFL@(@L1OKKsX<jvoW(83JawBDJlq9
zWiaNX<JgQ*rri6>(+(B$b}JiiT7avfFHgA3j+0|Ev%nhI|IW()4!2t|z8YLYsUuj1
zXAxCe3>sa@51%|KDFaZk4->`mkV1uIrIy<k=UlZ*$VXxtXxu*f`BQIDX7g9%gf9=)
zaX?Mu5qvB_Rkz_R-b#e|;JIGnSsD22M)&pNaFg-~UC!$$FR@y<4G5o-+Oe{qNzsdP
zkkz$To(=!l-uLlag{hRN_T70iDXD#%u4zM4vbPqWW31p1omq9KQtSlBWo+Hz5-c{&
zt3$S8ICps<9HGbRV%~p_`>0mD#>QV}04BWRgD1Cc$<1Da^$f}^Oze9g5uW6K+Z|1i
zfd6g38+x(7a%Sfyun%9FH;?`i_HNJ=<e#6beSXh2N#Xi|W|223zxo~?FPeMy0!rS0
z_4u0uKMVIqJMnUXEdswQ%?4niynQWEBD@DaHA*5VsAN@vyKjE|%^%A1`Nlu|R`(0{
z@Ci<v&D@z=9yUUJHCsMP+|@yhqH=5@D`HKM#K?4ZwIuVQ3>>JMMFDTX0?r8lYyw5_
z%r;jLve{57QwQ7bT+(2+m5XTGxzuxM+q-hG*Y>)mRZ8Q!k#KIXx`=V1me43iXBG|s
zXDoMStuh@ml8^#ml@K4NJUHYAwT5Hs+RL+J!w=(AS>c}e)Xi{zjdfgjWL;WM_#+xW
zPfHt^w<)|`l@P=(cW3J#J|wSN8@YlnTt&EoX{!-bj$SIl4@^&bkWhDV02l!m+%E?&
ziCEU8T$RY+3h`=9UP)c{=j}bt?AjF41|ReAMlOZx&qR7JoZ*f0c`Wog8v8i4t@7|f
z&veM)Q{E}jMb_gImUdKvVq#s_hZg9C7E3bu_9+Qeip?SVogtq#_X&EC=j(TS!S4@D
zQ&(^EaH7Iq#{W^}a7amJ)i~0pYIq4rWLCXW)TaquSAv|tD~V<g)r8_xHa-e|{qQye
zx0O0yto%4Mtkf93#IzatosOd#^jhM=pARq;D=>#GKOk_Xg~}16vi1+lN1msAjDQBa
zyM#S9FMT{liK*&TH!i5Xc4k|+o@llz*Nm47M82Sj<GLT=vr$~V+mFnEcI;7GcqcPE
z5`K%Bodtgd$aoZ6S0&Y528qJ1a+kj>cuxnJPs;ddhy8eG5Bz6EhIGRfjsLemsc3hd
zd1o(S0ZiSVo={QU4!m;e_aP>90^mu4qQ6Ksw(rt-Gg2yq55|3~iH>sn-BXX315D5K
zxsB+E$OxXEmTBHEs0qXEN>OHHRIC`PXAe*%QvD%$%jkikn!o^FW~GSb!w7}y`)^>*
z9fB5w2PiYPQXVRHKha4+9E6ejU)ma5ZFLC^y@|~mThg#@85bEEi4&L(mP^@MNv)ws
z<d;!=!Azqo%1ZMkILIeUH6m8|Nf98banSxmgpQQO>_cfO0EogGS;01+z{z?C9pv&m
z+i+wz0Y5NH3L;Q9$!52C586HjG`KllWN-)%ZIugz;2jj&t2JH`%LP}%_hvF*;Pb*y
zUQTsu$~JEIz=S$5s6Y0=NkfN@3i%b<SOEM8J$cK^fAnUB`$M1)8t-mo5Jx-i&z*rE
znSGl%G3%<STsi0+J71}d$kmb->^iKgc4z`8ao3~u6)k%twGL+nho1l=XQGN)@q2VL
zOum(-S{-+w2#<w~!2;JlM7Kin#u(Fppoq5kX$c!CFSp%Ot{WmKCl@kVT_-f^QAK;n
zDa^B9ec_tIhH~{!$lf-3vLTnR(U5B7V#RJipg9*6amzY`P!!(0ZbTqF;LvkaqjH-I
z3Xq2le7yLM-3;j#!yQ@nS%h6vD(juI+H#9{wjaDV&+HDgdMQ+<ovbueE&5H87vRYQ
z^T=Z|s~w(>!zHX>R9K;<n@%Q^jfZ}>T~f6_+}J0fP1nlqReS1x`Fdw{jZs6%sZh!r
zpQmS5011YZJ(;}7%YTmsrM`oa$z{AMo$vD-t%+Y|68A<2KOgN~g@0>m0g|<ICmN+u
zKb5fpXKxM+a`PFx9Xzph?$i2B+c!T?3%4zt+iEE_uTQ%8Qq1JQJ2lvl&)H+l3lk|Q
zw@b@**<4kXDz~=MR+j4>;;>l0j7tV0%bu?MRS_S1;v6>^p69s&(fZwzj_)jF&z^e8
z+Sex;xpqWMG!d?01-^|?wldlcP@nu-ksm-;u5S>3qT!LHlX%7Q*AEIVE6_<J1M-WQ
z6#KAGF=xKc;ChjtIorn~3UH^?pZ-Wz98dI^ElOnrTwsP09W0Q7RIR6R8NNASfgxnW
zjZn&#)+@MKu+aE4$);uo#s;}_ZMg+hH<vd@I3_jQOm}siU!CQ#_X36muanl_YF~<5
zNPG}6VH8`rQXU6D+HXtkNx$zLDQhZUmuv3T)tK>Kj8Ab06flsbFK69u$$xD#Y`7;a
z;>ym#?>AOxlMcD{D>7QdrL*FqUVJ%V;VyJ=8(~IOB!L$=rD%&+0aSu<WYgACsdZ9-
zAnpP&Z=siy{mXmj9;JQEtlYWL&}-ut-+v=knd|?oqvXMuQeV7)5MY6GathwBDnYjl
z(mAaZDWZ^01({Fmvk*$@)&Zmp!$?={Qu6CUDZK|6;c~n6xl+B=xcz|R$}XI^KhdJp
z1uGUYk^2vp=sF-GQ*6Z<vGdnaR#P@pF30AXlg5QIHS7lMO%L1GHk~f99*vIg#Kb=(
zK!;Td0f&!&V7)_oBx!swu9QnJnU5Dw;{<X5JLZLMLKhMb<*ih2sAeILObuGa_%e7@
zwQtZA65cDHL%kVtVh)xPu2}QV4Sq3R;XR4!YS4*vcnUVWJHJLA0gl+C|G1o+znY*j
zW%I!$DWTCbx=FDIIGbSXu9d7fp|>;KNA&X*t$wf_VEWIvaHnef`|ks0B2jyOmeOq2
zxYzGq9G)Bkr)z}&3~Emz{|p}7m4^>8D&<PAZt2blkz2f7;lo6_CLh=;xJPJsV)-A2
z1FAiQw#ZPj|9da1C`_OZt!E;7n<&M#$0!5jgV-PscpuE6XDb1K@&6=oS<qNP0h$9a
zxV}eqjI>q`jm>T%G!*CHTsU^?Zn7%7UX73Lu#x4T)N6C5`O*X3ED*`8LK0KKb{0H=
znATVms%Dim-XMw0KiS@Bl~vOSW|QEnU|LVj@kR#-ezI3ku?;d+owD6hbKmE9s?pl+
zzlni_N2^)Y8Z^kM6~pQJ;fkoC{w^fy7gz#v1$hZ77Y07DsbOAPT;XM<c*mY?qI)+?
zT?$&P*U$BP{B_qS3Tu!%75fY^_A;7zOK9oW<MHBi-)c!Ri10C5xgo#}*Zc`_lUM<9
zizV_EK^&y8NRW}iR1KYdYvfjJV1WTY&U<ISN;<PNPbcr0;rwWRaOqbw4*r~jYfU|d
zL8^c(7Sc#}gOHHBVk8^O=Js618ny{~7^D*)ac>W4oAJ1$>@5yNT=RrmT+C%U;RG`x
z-$Hoks6V%9#?iN0BgIh}Km9KfY)C1swW@*i#mKSi`Q2ca%by-XME`mXvoX-MSEV8C
zPdBHIgl}s6ZB^8;cYx<~7%@#*0+JlhFKLie%V&0Ld7A(qcIE!QYKV`rL!64Y-*>;B
z`5uzTlcd>PSC~0%aLj+OSp{Y_H&TUJwThG`eZU1A`8<9p{~-89VY0AJhy`Cx5-i6&
z`<$algACnzvtI2syej#WaANmVlF3dt*PFABui&5NKmHX}uzcowRE&A++9%N=1O1J>
zhmATE%Rf7!aHQXhB@;Z6kL&J;H4kG~@O%u37k7WlZpG%kIb9iL_MJd+o|j1xS$s6;
z;s+0qLtDeg#qXMb&0RqYr=rZ(>+1FclI==3L>b<k9bJOsK4J%x*2EhayvJ;ZlF+I|
ziv`CqY%7c}vjbAW2wo{HPf*sbPgf~GUd1)=!eC+?!}DJzjk_1z<jhqF&MIomaUM6L
z3##A&u>s&exDU#@bmpoz%bi#R-B}fEcpm5EJ_5!_aMvnKZQcU2UXx3$88;_M%f9x6
zbm9=QYi+mENDv*SQ(PY_@$-Fv-7&v$@oq=Y_|~G?sTA8=2Q{Z-w=cP@T^cS?A_G4V
ze($!RUZpbCCr!;o{CUpyv%iPy$o#=$A=x9lWqJ!abJRLYRBKrmC)4#T^J9-JL_!L@
z64e581|VG=d<qU)ozi%Dfyj;gAl0sTRWv`pw9P-D+=h=SOi$^Ut(lh*Rm_B;8+!~|
zuCn6}AH9*<_VDXCIb$cXTC;LFzYVoylM<~QpO$y&4RBC)@J!9?zr|e(-^p^aNy@uj
zr9GVOk@EvNrde}Xwb0Xts>r|!+Zx`s)>xODt307~I;Epfh;mr&cd}MZigCJ^dp(-o
z=!{81*=&8F@pOZE8<5Em{8NVF0C*7&nNr}PaBMAh8hmbgo7cL+3+4fne+wXg+NtOB
zp14VDEygCu12B&MN)M7464u|t>xu_M)ljE}Gapy)oGn7{mRhf#E_L0x5K|y9v;zEP
zaM~>kbs|(dyAXClOXS1Q*}>faA<fQi&Sv^-2qz-=K~A-U-T#cjc0V@#Ubp*_)??1!
zoj9l5J6K8X7YBNaBM=Vr+ADl}7@l#MH;sN^;SuZ+&m0Pl0N4pR<kx1DlC{sS_S>+I
zJDyP&FHAsTby!-cil3F*V=xYI!SEglAsu5Hrbc-<#0d{M(ReDr6r~31ux__Bo_sZP
zy#}Gx8Uxkuz{O2Eof?R2V4oz6d}jdEYD5sg-FER{=zewRjdJaBBY5#ah(?{r#oCTe
zqc%q0gmRV2>yeuQr~jn7NBEkz>F~1)=9Jek!aXnW4#bPU=@ajU`YSYsOXH*&Q*vjD
zGYm`<5)Y8ppIp@*vtKPQ>;HN4^zpqa!lx>wXH{O<e-M461G@|p<pFrCjoBQIQdK>a
z>b1_^QTlp)^5>b3eVPZT4m?<yhy}fIk&M_uRhvvAjrRYXp`zlD1vo>4#Tv<GQ3D>D
zMP-R7)2&~UXs|XREoc%eswyWXuU~1SqT8+XJkjW?UD&Qv%>Hx)bEdAt=C}0PKe}lo
zfJ{c;ca7y-wfXuS+Fj$^p12#5*6Z#xv~hBRXOrrd8+xxxP}QGbI|&_8@h?C26`frE
zocV;HzWT{^*sp3JLkr-_f%sNfR6@!B%+Ok@DiUc7<&fH?^=77B=Y|v#_SVk28VU}c
z)U5|2Mg5>i|M$k_n|17lTbt3yE8D2~q}5il0gWwwLoTvk!ro4YopdN+<+7*wyBjZ{
z18QgYyP^Lyr~G)Sz~pZe{L}D@**_hYJ@t7v+4R80yEh-E@24uVu9zLy0Cvr^?NNWp
z2j4C8?PmmXNbwKtRr&8ejK}}$k1YEqY)-Y;7OHFMT@)E6j<}<ltrjx<gE}~CsnJ%L
zpA4mC_m~8ECO(9xu60)s00`>k5WrTy0_hAGBiVppe3c>%W6N1Y+d8hRRFUR82{vpT
zKOSrstk!64R};{oiRuZcIcyLV)f~d@7Y(ovR=D;Z)am#LK^X*fV`h=uNH$g&hl*(o
zjjapURQHAlHiq&%3{(q^tUIn2Iy5k^7IxHgSHJmv)F(GKr;RSZSvI>XYkOG=Qm<Ex
z0Su|ip@5cyE835F5d?mLUvWZ#Ug$RYAh0Vq?dk;Fl9j)7G6ooVa@;aA%1irMU)0To
za`}j-RL$}7<*uG<M@<g#&eIRr$d%lm$iNllF`215Mn~%Ru`3VnwG|5RUf5S0=MA@i
zOZPtXzTrHxfaMi2o)agR;vE<A0I)4h&b_#57|QwfgA<NGso{ihD7~Ig4&_n_jSpK|
zJDBh(f)X<&OceJQf~byK(BpdJ-660za>pqm9Q(4uuO1Vbp>UI;m*x!jXvwKr>VfAS
zeSELKP)Sx{GQT#{H9X~mjS01^Hex8e;@E~UROj_}eVyJJ(fqacnq&{H2D~GQ;871J
z)j;ZzOU;nFKI<u@LBQRkYp`(-Ax+=5#W9)ygkWtO>$cwcBK?bdH$w6>(B~sh<+}1b
zCmTP*%o&1DfjO)2X)u=?J_F`-E#&Df1&Z|3#J%Oam{aAa+;^$_Kn|*?EV-dn{}TB=
zjgv!HRCre4n)=)^rDoGq{>=S3=HtjOC;LjoE=b>x@ufgVh#gX`=oR1ykqd!+JV}12
zYIH&Ar5x_o_TQCQBgn~8wu=7?FS_urMg7KIa*f-cm2p9;lcR{)6Knnz-R~(8d;&vo
zmz*k7tV#x6YUq2^g~F^u!H7x-A3QKzXkl!Fh5?{k9~%2JJ#4JAvbMXVtS~S-J!b8G
zO`O~nF%ZaxVfU#FiArWr&VyOJl+j`#iSjW%XAAkm^{a5T@zOc9zz@^6haZyWBP$Hm
zlAktWR+Gfd0EUtbnNh}r9j~wjkay_n;_+_xUNQ3rfCO<PwVN4=lRhF(=r=-nRStPC
z6*<XOwszhdP)5G+A3;ak7Ko@HLKp@Le81XU=1+bcxqN`Im{<#A_OVxgf`y31n+rsC
ziEne^XT&#D8gkQfd4uEdjHor>WT~2-WF8X^wY*(-xz55XXp#<*T;LquB!`&Q1d(Rp
z6VKlA3PdsJlN7<J)uSeU`5Gxw>(FL~i6!y)0K{hPMrY3*#EPjtMupZ;m#RHfgy@-!
zEVU;YnUb8aZXtTp_^HF0m>SKVd@(g!yRqTZHMviq*#;Ziv586S)bIS+H%MnT*(PQ!
zcUbh0gu96O%8AjbF%-GfZLRDcxakB5%+`^L7=UIC#-`6B+bsJAtafc7dhaD4@<sDj
zYV-_28V3(iP`&hDza?cAiJ}rTb=SlM4_R$4yB{Wj)m1LUlhf829A^8p)9$_);44*e
z8N7JX`g&m9_dRI-<l?BjDIDewJC@he=IxLDUn{iaO?KNKHqN4g1pxn@KhQ%*_W|`o
zeAwQ^3&9JEOKz52&i2Zk8|6>QcFqFFa)fTJ;Y63~5Aiy`m+X#Vt@;Y&UHp*A*WI#D
z@)<WGe#Ty_=bOtr1<wY9sx|vCo5s%%sF(My%D)CbD|}I|mb1V>4w<*c6`osJ(F)v5
zfL3n}Mu^1))`xT8=Yp-eNyG&>{W&=P{sdvWPA`ZP)@5=%V4U6}bKX9#bhIyxUkwm?
z^zTps(7<*<WM-IVjBG0(Qs=)zR?5w5Ebx)Bp>g4F$c>fF_R4F$J(%~*V!A|~6=J*h
zFzI=;X}M@xsAviIuVaxThieV?=$AqL%lm@Ey-2%SuQOU|`*_$wZ?GYYc@!0e&<fpk
z`PT4?vRCFpIYuW8LW>rCuSoOq(#0f6LrC=G)9ovC+!>VT<9%%Ve!uor<<~5<XVVYi
zd>7Ryrzx!ZG@0xFpP6Ek9Q#RtQ@4_WxbS~_XG^e*fOz_-q7!w1+hk8_N>EIhn3@T^
zfYVDZvq+QXByd*>*o3+O5<TUOh~)28Nn_|B_MqzfpSkeT$$GI!dwd1Fc2;GlM+28h
zW<44ZHdZgmT5F#1D)M+Fp6<@MKwe)`SIZh=wZAX&cqabL^2`!=snmVj?_BlFprIU|
z8~N$Z5t>Mj2L^?Pp#P${T^u=Px_`A42fR)mAGCIIx>s;WGPRFz)6}(Eys?_C$Z(p}
zRLzQT_Stp+BVJ+INtW;YC5XPAWz^UDWW`^ygeR58yxF7v=DOv*BR{vAZ$%=W0SD@D
zPDY9U%KGg5Xa64SF2oxs>93n#rTW;XpXbgUs6TkK1TwZY<)#mW2mOWyHry~@55%~B
z|2fFW!SW6rUqvJLO~0Gq8zT0KogXIZX`~LJc8xe9y)_5n5yo$GnB`{|NilP0TR%ap
zbIl?77NWPbyp_mL0j}F@Mdp7qlMkXYd6XdFeU<=zi~z!+@wQk=jR_LRAUzhIQv=zP
za>kUwb|RO)gG&@YXRJo_sng(c4^kcHuKztlXn$3Lbbu@En1Mw~qhu9+WY!5n9h4Pb
zUss-WxW2Y0+Y(H-&59({HD(>(Ypb3H$$i1RAKhcC=>oY&ob{%=cOFICx7u#Qd%DL}
z+nt!%-0%b)hd@Mt&T(0>O{3#Iq9IrbcTUJ?8-oa6a%K62c}};ta_lFr?a|961(HX-
z*_Pv#J`xqTLY6P`AoT6nkTF~zuDnrJ6DKsS4&I`21$*9d`6cvJQoJF~H{7=(R{8PT
zCrz6oI2Uo!&=Ljt!>uHsX@ic6yzy{CFq8)8BwbV4grsRED-fv+R-NI|#!A=R&j`cp
zVAJ0u4xx`%36jLUXfn$Er+p$yXi(wK)W{_koPtkdjTtmv#3to(NHpHVVb}d+0igWo
zTNkmS_Z2cHIvPG0Bf38Pis9;3-prDy(iuOpb=|)}VuP-GNaAg8G~p1O{wZ9o^Nzah
zD#8a4-{N9xgyY5`jSWSAKx)E!sa;D;`M7H@+mQ~${}lc?0C^`5&oi%o3m%O)CDuo5
z%K)hp?HWr+X=c?Cdnlbi(3%5twZ^aNvsKFJJK+?Qjd$T;dmXS2<M3xpjydcF2eZrN
zysbq5UR60dUcGj^-fbn#Uy5h&_|3<$$gOiX*M)bibnaN$GcIgVsHUSD)sF#2Bk627
ze4~&a^Dn<c<N{Gh80VJ4i*7=t5VqlW=Oq~6^?xZuDr*Q%%$nJi#G=ai2S+^f{C8lf
zL;}5<8YZx^XqcMURHj5hR*Ldq!OX<d7E1kOk2YUyM50!!^6H~i0g;2Mt$HKD&lzmO
zQ^(BLS*`3ll;TB__SpAoOLqUrD?`zo4~Eb^>oF;gtQ$a5A=M$cZc#**=FB6<hA*eT
zkE_)hS`m61N8Pbw1vkOQ5!OYQtgmppr+Lcay8!&9Qf0wFc&HMj`KA7zJ#xOtE;U@L
z=FZ=R`3B{8@2hQ%+}0oOUMWgHr+_N+{IzP1@V9IEZP#_~7Kq;ODBzh_SG&IOxHl5(
zs+?t4=C)b844kY{{%PmsG-w|u7%+A(4B&0LlxaT|@NOfuPORKqYFV;kMw+3+X(h8k
zeUMoRrL{?f!xjI}HkvoZlCuM=Ay?hRf=l&e-tyod)g2|=b_>7fBCcuP5LHvF+8C8o
z?1;GDhl_da{Xrp0^Tcfz{FKeu(XfD8<KkPCa$BG3Ar6}6%bfY&RbQP8$KU+r5b%Tp
z<N~}1oxJz^16rE1tFq#>PE4byUja3**h9@30QHbl;N^<i3&E>emA8%zs!bijx@r9D
zjl+`<FT!+T=m8p!Az6V7R%gal_#YQf!DK8^=ybK|w<{Ed>W~)w&+n)^C-w{vTE;c!
zK<;5!_jFwt=DymELYj!ttd610&C!G#I|F}et83kxxCKO2Q;_+Q2C2LfFf(cd*Nu>7
z`d7EBw!;X4V-|YQ$&ZT^y9B|i9Dwez0-1M52;9&x+;>Vw&b!x*9MC4+%~)6&tg*0f
z&K>HdV&)6J+nWPOpY*qZY7f>=ppTQhGWRl4E$&5^2A*-V5wszWnv9pPcD^!$iTSJ8
z_EV~zA?%4f?|TuC&Qi6w4!YLUtv2(66w50Jtt${=RowMe3*b6}cG}lz-$49<Xq+XP
zQ;VH(04&dgu4ZGcs<t=M!4>Ng!5SOiqd%#{EPKb|el>ehz1Hg=8}rxk##o5ff*bdJ
z%m#fTlVda)n(885SELCz+SkeYl#WOQPbOJW@Ijd3Y^S>#4K;)K<^%p^8mSHqj}zPD
zSNhAQRa(`Flxy}+AD}Pe>!km4X~M41QJ|@W*M6@=UY=dGbY1W9sSVq&vx3jt^jKN5
zB&{-*jNX6o-2V*_LC{N7w~`wAzsZU655`N1r;Wa3c>XszbIDw7C|DjsKFx-?U9Z<B
zpp{P<oc&QG5e5?)B=1BtMGJ4ApRRZPDI{$0nKi5u?+<%kM!DwFur+Rhx!}QiWF{WN
zQ<8lIG9cV2SC1~_4VdwqcmE(>Y}qj=y?aP`HtQp+@BNnDqwSqDDKGq2$}1%i_K1Mv
z4|61t(s{#yG^tX&6PFRh53-V<a%wH&gO&_uF%50beU?XB#ZvsOWzq_`^xYz1u9?+y
z%aE^Dyv(wdtQljchxrU^0Ss684aSirXM)?zVl+U0%I4sncus)m!&6p&vZ08C2Jmx>
zyl39;u+A^q*L#17$1Am8eg5{d?`pUC&UfH|$gj6t#<r(<x$jcV-@%w$zQDi!ExIxv
z`C*PYx5~qKe!+g!$Q}V^`fn3H^O&f8$of_>vPi$P3CBO3ui<pn3at@UWqz>34_!Yq
z>5*pDh~iGAvDKR)YCR|n^@5y+%UG-h6QX5k6acmxdN<X_Hk5h*;tDrk&SW7E4zkx3
ztX8oNR}$tVc=gH;kGJbirf^3l52X+t2>=@_x+F<#o0EH=+hQHr&#K5$?O~Na;8mg@
zoZ3WKG1IoQ)|o{sT$~wN0le<IXael2JhHTh+#RxW24pu?J6PqrrCbNJ>doV!L;hS_
zth;Pw2r#LCx|;f=J|wMbF<i4EfGFB160o?bUmLk-&ztVI1XsT8=P4=F0~`Q(TT-8H
zaDJH{jufwd<9kwmyo$ZTTaz~xz5(Wex0c}WnoPIh2+xm?M&q<U>0dwAXK4QGv@goC
zHgob@n}GJDs$2gx|AOk)i;kF3b^MZ>UIj62gK!c`t>^5W9LrWblvz(mgHkp$?;dB)
z{=BBW0atyQ;WuuOToo3@mv}i4E-IE;<=>`}{-RKgkh+O5zETuf|F!peo$)NH_-N*~
ze?eV^482%&*|_qWF2G$<cdh=3F?2nvyd&*6;m9u-IDV1q?iMnzquGtB91EOK+r00k
z(ZrR(Dz{ZlW|cRpmb03}vG;o2H_R`Y9sHhBlHz3Z&DFf>vUmEdPItP^V}gCVW_LRk
z|LGZ6ML~+|35hztWad(VN39N#2$x6fXLc=s(A|<}k8@w0^)XE7$?~ticE0!1wb3)x
zL-hj^0!dRFrsdzCH2)QZOzRK4`rhL1J|SS;ow^R)JE2E#_UjS#jzI#fj^rP3-M29}
zSE}x2aH_mw!j|m(zt~l2u9YFv^Yz8x-Q9C}%o?nfl7#Tx4Qu`?67j-T6{t88osJ9?
zrne|amU@O$R$M_q;E{S=$72zfwqt=~wm4LL*XSxG9Bh=z)Nz{~b@ldhI9?0UVC4kK
z2w8ir$uVQjM{1FhU0|IltC3%>c0QKjaUYh_;4bCtHhbCiL=mfdR<Hr7)(FmQq&C+7
zjuPl;iG`uPOC)itIowx_j-EZ$|0RdAIwpHqNj|Wp3#)ud+SU8LS&B^>ZkWbh8<3qF
z%YK3uz7%k<Xd=`_ig(jo25EcrhWJ^O2XM^CHa(~YXWMcr2=5O)4`@p|D32$QB?@^m
z)#P(U#~AFo#QnjA6m^ne{8;Jr|GQVR6*+F*{{hBMu@#NLT;Tvz`f5BYXCizJ%_=fl
zw_37+N5pmHX4Tx=q3U{pyGVSe)rf_SGFO!{qOEgN#GLvrjHO3R?TU4`tnhts42Hfn
zap@!=KM`AP=`bW_@byJ$s&b4lobJ-#xFTno5HGA_$)K>w;$<VTj?NQC>3VOJq<|CA
zmC(dD+aN)huWH_nC5vlruB)E~8muIUWiH4ohhKXE!rviNZg@BqTYdT_hHeKXHyp|5
zXTrqa6e%a&bkBslD4}BJk;kwD+@(1yT%S<3zx<?9R%_RO?8pZS2zwy~&o&vHn)i_T
z;}7G$aC1|V>?xSe)KIa^78Y-&s`#(i8Wxs#PmxdKx^9>f&UZBpcxKy0XzE|0AN(q=
zBVcmueIvGO;oOp!x((OM%eO7-)Hrde%$O|sW^bo$toip;6h6m1V@^}czGGNac?{L_
znSxdRHv1FLtSC?uzE6<q@mHTQZ@5%YV=P+aY;76`rd;qj<W6w*{T=SAEOyeGf{?D(
z;XBb-a)6QX<3jaV_sx6UhBRa>pk4~dB*4}fM?ai-vpU?UYwM=<&RVffZKG2Ha$Gg!
zPm6J5=9gHWez~2i^hp7yGU5K`NnmmZtEk=YMly7vEY5Q4?S7(N@}P!(!JJOysZvr3
zrfBrK&c~v$D;8oCI*_E^dmGtt3$!`O>!!Z6xmpNaOs1V1i7Kn&<)@3U0f<EvwtlXT
z>!`MUn^48;3zRE2uo(y2ggJ>?wf|TNoW2Sj;EE{?#z+dK4^_1iz&tdX&*G&{={))^
zf10&oo2rtdg-{_y8T}BJC90lgul(7ls1YSxK~@oMU;`#_VG7qyVsOH1kp5smEJ*|V
zikdm6+G$*4Q0t!8*@#+fLcj5;Wvgvv{YFoKZ8W)Aep8NeDBM%FjnDK#0~o@VZsVKk
zF<WDpG>IhgVGwC)f$Cxp?GMTd-KUWZyRWEamq5Xrw^2okCNRX1%9eu^;0zZT;%>Ph
z2x>ObdA|}f;~fvc;-7#IpY=IB0$&WuenrlshrzHHN<z1JV##5U$(p=YZkyRwi-E$T
z?EqZBjcW*SO5eTk4NU&g=k(P23jF78(tv`e%ADla+yha2E~<Pq`?Y_1Zu8$=HueX%
zS$Gue$3XLH{pZ??Yl?i;PB2s&<)OW_kU4QgTeyl?V1l+#huNkLw~Dzj_H5lzmpsri
zTOnSXALT4%&bV_-=zhLZAYgsM)U?t1VdYU#>&=nyydg{=YKuw3p*}HbBot2-rVXKC
z+t!0twIx(`09=@HG>KVAApl@_toGh^g2mF9-R#o`pKW%A5F8@Zok4ylF{IR#qehC_
zzziI>1Zi?`c??+uPhQ&X4D7x2+q>iNlQm+m4O%oc->rOW5|=S?b(?xupC&u84PT}4
zUG@cx;URdy5ailom~ALN9i1-x1Iv5N<UN;rC?X^~@JYBsDcx&}L=L7&sGkX3Dp8+{
z@F`JZq&=Zt9uD#O+BnEt?E;w`m~vV5rgKZCglDFxZu*z{hQC8c>PTqj(S7t^30%N-
z9YJuWiG4qvr0X`Xag`8JtTTSv_bY4qea@Q$ui{G7DKxGgluQ*@v=yD7ZAPcV*=DB~
znR!EqW7S=C5f#4m4$t(lYBGqc*s@iX(~Tvy`LQMK<yje3guJrEsv8B^dZkG9q}=)j
zHLn{00y@}KSAo3UidVzIOI6L5E!$O=y=9q-wn?(LvDq?SJ&Hb^<@8Tf$1VEtMzz^o
zbtXUmfQCD-{;-l*IF}_&APL@Gb-f(k!_4l1-(hAi!TXt&3}67v!N|@5g2R}>CNZ4J
z<C9V$ypNB^?>%O$2RElc+?QujbIe>@_#<X+B>eIB+cd6*!y6GbEqnJhqFyqKIrInT
z-|vdYZpRNHCIE3CG#hJB7o28tq`>o&@+G(OUx$9wo<HC6so3F%`HyiG98J=q;NQ%+
z3_1M5pSwlD7hmk0IiIPDG=rsGbc$4^u|H$UO3@*pFE6x35gMSt^IoheKnx@0{2Goe
z(_5a7a*!PSF3YzPEalsBSyAKh_rgOi=MQG)p2F*WFRxmbz-H#^w5)xs$iT%#j14|D
zeNx$5w!8;G=+T>*8~uK6UNW^8sz}^={SPVwz!WB9AA?I8t{5MhlEF}og~w&<X1fk>
z8|BxpPl;n*yN%%YF;bE?;kGy8ln1F8cvp2ccjt*oUcUn@&#UlxPqphmVmksk=mpjf
zZ}qb1rgh;?iJm$C_p|nzs|`wp2w2iPVTUnHRu4;$3J77m3JTzTR1l5Dylo%FVg4w+
zYVcpjKGMug8(iCdR$S|zYRll#4UE7A!ca$AseP!7nf!xkR>w^6W|ixUa6SuAp5)<3
zmbQU~1fxqBR!7JRL2p8H<;%uZH{EgB#|ajWS~T8h%8}qdp2w6L^?rnqSqT6x*q_do
zR{!YLZeC;!YKgJX{W$ZTr*fV|p170VvMd>X%qqW2#?^M(R{eag@`ZuIA%vY7uUd46
z&7EfN5PSGsdQ>nfM9ZbdJCZBhcr@|jJ(Ah+ATuCw4He7|G;0#ezN-DjVYY<dX*0vF
zF~M>6xbcqsq0mV$C*e_sNeh?P<#)HiivVD1yA+w5aN_hQLeoCU`7h#w{)`o_X@K7h
zkYfTaOm0Jff8cwgv|#f6;13H*PTjLnhFG{(kme%o@17easayM{?mn781_Z!MQQpF4
z-$jKQsx0X(#>hb_l?Y9kyfv?*;4s`d>H`*$8nf_dAU-o}CZw50*nT!-k{;dktg}7U
z{ja%&b!0$;Oq;7QcrCbRbk5$z{^&Z+B7W>tMpj&n1{AGNtvXe>=5|mCMHo*Xza2U1
z-{ktW*{e7~>M8<k&?&EB6vxSmbsSUTX<-Q8;ky<?7Rvh~$FQE2O`-9jF207y0KdM!
zk}B-xiBkIYM2X~DRYW-qPu0?}zpX>LR|a#mvsutlp5ViJW6txTY@`6`UjD+9S3<^<
zMlVuBK9)#cNJQC0SkYkJ0A@$I@Tr}mr#g>MyQtK31avpHX?!%e9sTSrGdQ#m4T!og
zui~=z9#?X?)2gEHs5)kpp{NAXM(8|kD0JMJ16aYCp1`HR)rfN*p@majTJxI#(i!34
zz{S+s<nhda(}@2Q^$`@{XfQ9F*1*99(}rkhc(Q~lCTmdP;&-=bojvr?SLj3~F<FUB
zn^z=+UY(Byl*VMfJ<S~f6B{INIs1y47-u@JiAb&CGtF7|6x>RS6-4b)-3aqzbWAN@
z;v+!(baAY>SL5797r<LEWf}Km<3^7<tK%uFqrRzJ-6Ly()%D*_S@6s9lt&BEx*yq@
z%HR$uy$-ik&Y`c7BxHpTN?wEpRxTK7#UjdqFq#aoE2dsIPv(Aj_(-FG;Q0SX(s}q(
z{lEYJg@fZ5ImfZraU2|b%jTHpSlOEpLdXuOI7Y`l_O4@wBq6)9_YN5)p+ZPTN$UMO
zpYQLFc)hOM^SU07`yJz=U|a3S14J*8j_hofTHjT)e80mb*$@6)(O%nlA->sqRB?(Z
zLmT%Uyx7;d-itMd(68fxjpwW4CncXNUVppx*p!_aIKgO`KRLVgZ6*F*FCUKf+i}Gk
zb4cs&ZgUVrQ}4BJVfk7qPcM@(J18WebNdqYNBgKs9yDOyz01%)yRWz0U*{>pG7wA&
z`OAOgtU)>OBh4*y@I{Ohr!H~(HdG%P&B!|_M}Jgk@PbPSp#>9jI|2X*<PZ@{7J4B_
z>g47L`OhGM+sUoP8*hLHBqtfhkux=Q9l#i(0WU3%$cY8=0N9N|JT7|_5Hg~RFC}X1
z3`h!8u?QmZjiac433GwfJUg||is9>|l;Git`5l4fiZVY*B|LAvq)wu>8`W~+|6jTK
zm^!8j_ELMqbJvU3R1`}g>GWc0E?zp49?DPu)&L7`Cd{<#F=`02S$8Gc?+j&9I+BYA
z@s4%Wr9}ImqZInfr!F^w&@VOKc)W%zaQo|zKWnRBJw?*p`EZiQu!aZc9pkUNQJLYd
zZve>R8LNYyU$1_aB_;Icy09lawLO@-Ey!jhr0X$?dXG;P83(&gDn+w+BdzvNN6pef
z7qh11NG&IV9H8MKl}D3PH07%hB&mEp7VkVBzGYNfpIxp0{+%47qWRn0&x(izxdgy3
zG{+4P9aY$lzuv_`p^Ly#1*Z8ObV<LcrgPLw4)ED~=}=wbr`uZL+xS^E5IeXNZ&tGE
z@R~$gz1jO@>?;KC=W`WS^VD&MgD5^;T=6VKJ!D|CfpArD%B5!leO(eeKR3H%eA^_^
z9w4Kk>_*A>Ol96gnNOtD8{l6uy#1^RGQR?7bzd~}vnt7=B`GJGU$YoY|IRTW+t~=>
zMtG%v85HudjNMNA6{h2u?}7(!def9nNffS#4+P*78JrI$*AHZXZA6(p@WsOH9%8ri
z@YS{#?dqD@?jjg&%@xNkx2{cq5+w9Ii8=k^E=}5h%M*gBvt>b;+Rv=W)0AvN_BXwW
zmwbJ>zO)vbqVcnJoE2~tY8Z+IZ|=&a<LN+$7=$O(F~KUyRfJKbn$zr0o~lAGstr*D
zj@FwQ1}e-cD0Visz{%?CGxS*=+nKyDlk91){lHS;r}Ra8)<CZwV=Wh<_QRZ`nl|Gf
z)?(qoS9e`*jy+@etv>)n9WRQ6{my-%A2S~FtZtV9LyxQ$XUei2oo{_Qn*AaW1YLL<
z0&ct>^*TJ|vLZtytjjje(Pf+S;|p6OV~)yU54lG35fvvi*d)PyqP)r&%h1K{4oR@=
zNa$6G)F~*3xz4!Xf7uUtV^2@v<_T5RPfYvn(lZk`QJN^T#7>1Zh80`(d5=d{vICU&
z7$+n58Q64kaW{lbs}}QwJ;244vLNb)p`sSsz-fmSY8p>g{oJoe>nEoVttawqSgZQq
zZmB?`dnr^jS{D{q8+^)9LXCMOW{pwAgaQcH+-}hLK6W4sF*S}O78~O>)xuGr+(c-^
zk*P2DgBrcKUXu|p2Kb~XQpNoE?eP7@D+Bo<HjLBohr%`m0Xm_c@MeXZILmE)(a@oL
z^KAMaj(({Xh{UdEV<USOE<Ql`2F00XwH&}71Vr>wUVWpYi7DIaRtc-dz=jw+8G<3t
zrGd<GyId1K0XhW(eBX34oz8m6`=&27L-z05R>6W>cb~SdEwM!0Dhnw^S0+@XZKf#Y
z^|zrLtKFH6ngFrw1&gdb6@wI6a`uKoz?@MS$#DfEyh0Nf#!3<>yb86;W+@0-h3lM*
z&`ZfdlEDHI@0IT`e3Iqpl&N|D0sMeI@a0frfeuIIx6V78)N_cY3m)h07g6PN`aK1S
zyvC`$gbJ6<&g|nWjcc39q9%SN8<Z47k*xsB;=ebZoH83u|H{*;yiKJ&B5OK7=%RC>
z>I$YBl%wx$i_f05LEx8$A=(NY4huf|Y~}FH(kEC{yiY4c2b<mMu;|qTz%bTIm!8k)
z$Mgq5X!HE89sqSZSCaYfjR_U{vEx~nyYI!p2j?>4Rzg#LXrSX~-0hsDvw=3)`M9%s
zXY82gKrlSz#urO4jlbuFS+GSsLPaav%X&>?UnBXeL%UdrQdFt3p#~8*9N|FWUUp4O
z$Gg4kF<C#|v??4$)V9W8+^MMt;oS1`kSk!c%9B%?%9m-jHP42qhEZbi9SWOzwJwYc
zy_5tCrDBYug#_i#9&!Lk?*BTS4D5LP25D3RNyiY6p<?bx(-Z@uwKk9JCvDZBnZp}%
zbntAPAU@uMh_kl6#-2Y~Bios_1mVU%L~hni!qup+==U9?pAIV`D=mDEZstiz;|Oov
z^2AVV<<{vvJ7-4KeoxF&-qkl5vwCNGyfA#b5cy|m#V~9cV7ySP&sePNxUCQ|b)2zX
z+39omEguH{)v0!9pUS6%q^rjxI>87d>HL(>D2JzfAy12EFqFaf$m+}r{BAFtS3wv8
z$0vr*Vy&4asoODE5%e!vZe^^*DjgYAj};l-v{<u_NauZG7&!NS;f@CHoMDV}w~S#T
ze>W>g<D5^qdzp@vqEFl6&Kd8o!^%HoPy*G@jLUNjzt)3v{{7#L*D6Qrd$HpG2)#`p
zR*<&z$wkU*Vj4GC%9sa6=-p8eq8>1Rx<cDWbiT?Q3+1{B?f;>mIihp4uZ$m(DK%ym
zym{%%I+&%d!QP)DxXZ0)L3M=3LQC%o>SarWQw%kNO8Ag+*0d;z+m7H<Ql@$%z8U*J
z>GY_?YrHWDL?$L+)B5qIZlE=#$!AvgvxO1y_(ge~pf%M*rX{@u0Ba>z3bW^T4#_aU
zHbCvTSX`g<R%`@^Jgw&2R~uI?*RKc5k!pFv<}=M3QdTn}tiNT?SC-YQ{UocRG;Y{S
z*%V_O?7#xUl_d}rQYTzYrL9vHEihR5BG(4?Vha7CL>AE&Ev_&R?q0_`)(bTckNTu{
z#XIdtvIowEZT<Fl-k^2`oY5#b{D*e0{Q6asfW7otu;O9rOb!xKUNPFRk9Tte<d#2y
z5j?5uZ3m5jgVHLa=UPX4R@hp{v~1LbvR9?%RxRRvOt07eTC^irP&@af#3N_C8$iRh
zM2h2rJVL~I{Lc{54B<tPL#jL<_g<Rdxl=U^s}Xs+XU?(oQVvSOU@31vShdSzMQKz#
z3GlwULm{_s-HfE~(WYov6uPTQU*d+K6zQlDjV5I7VUe0-&F5X-ZltK3v}#cFW6JuP
z7`b7_pw+J6MDN`_@MBWChhSkt*xsYgP;C8+%FtC+!Rc6@zedk18rXI4s`pg&dt#aD
z#aEDuu4iumrZ<5f?$D=*>$azM{lrUSk$<+N_vvZ+H+P=GQ&yz8vqwx?#1+8XWUlM@
z=M5UWJY~K=T(f^h#f=D;2X^x$uV<Cl6Ji<vJb2z7uoj(l^*i_d4-16k1dCx&Lrz-d
z9e8k0I8$?x<u>$#R5ffB9zvx)Qm%!H;2W6jzT!UK8TMF@DQ9lnO6S`vG_uwxzFZZ9
z-R8ugI&|=#kl2JV3rfLA8-}c0dHvZ>f|c;X3inb?J>m|&8f}R!2T3Jk`<PMI=3O3P
zDh3u@?1jrY;H)f~lvo<%AdND~>JPv1SX^$+&UlfP>Y3?}ta|C4_e<P<;bz~%HJEKD
zsI@6$&2gmxnH69LhKh7SKPe1`bvBZT%G*-NCZv}HyciX%a-Pyy0I0R!@`}~@*4Mok
zTOl_VTgz>qVVsyfG#f{euMmkFDJdQqim5p``*HVo=+8e#v1z`_%>F4aDtQJA;<*vo
zzoSg#wmXI4Ooj7&9+P#&nDUe#t|A#u*`oEBT##%DLsTITjG^x)1-08Yv=N{iUwk*m
zUXZ5a2opx1*jhX+l85<DvOeJ;q<EQ<Z><<MlN&ikH)HgM)HRko7N@L&03a8SZ-zj?
zXc+`)10U~*d`A$kiInR6moHo<pV!hPEKxQ*Z)tzY_2;_3w6|I=7O`4#BN%b;s;ylH
z_#~QTbDMhX)!UIE`}Y*+Cz5mh^C8ZgTy+`2d7MQo5e?f|0o9U5lX4zfticB%E~}yO
zbXKG12A<n_i0RteGEo_avZ9cP1kN?+!(b|7Uqd#bu|IF_TqthO0(q=EmG3$|0dWBu
znYT@N^<zQzTqm}IANTfNy=z+@mCTgCl6U0bgudZlv=x)&dqVWP;gud&>m5ru8w=Y0
z6CnE;y|vuQ-OO-%|93joHro=^?jHt02a5G%{z}wO0T3#n2%?hC<S19y=m%-yuqxf;
z`46UzOC^BWC9JOs6K3IgJ=xQiV-S7k&Zfs<>eVDJA^JY(;ofe~uY9qAj>lI5^A5W#
zAmWwZMi{@ld`DSWeiw`%0ZW$~z9Gb3NWQ0qegau{-^>B72T`zX9{iLYr_(m)SSD=+
zax_CToSA*zC0WI<1Xi))bKqZjB{B6TyN@&sES~Ylf?_SGq1SUO2rD(9+Y$jO`pcb~
z74Mw0tu15frP>eeQ$9x_=A&xI(u&Y|<*ef{{A2Oz$RmPKBqCnZZUUBx63H;_^e0uA
zS}Cf&qfB;H{^C7@`zrg@f6ygVVBMtUTdDjD?h>47%v8Ui;xx^@0*0Z>O;d(G5tgIU
z-{<Ks95#Ax#vB(F8!AnfaXIH0ojtbtq&{r3=Uvmt);#QoxBerkBdHH5Pk=pSHWJ|9
zDPyfTh<L&5oH2heSXd<{!si}sfbnzo4gtaDR~^Pz1lwu`q7u@{=TI^!7mOkYiD)o7
zN9KcY95sQ)gW25QOp-b3%|<46mQtQA!(fYi4$YQkNf_J6F4A)q5(A8pP=|!q?(kHy
zq@`{)kPO_LA;m3A!XC2IPHkS{I1nphi?Z@KJufpT=dH)>%_;RnXxTiscS|qMh0X{z
z$4VTFyHU*f;57o|3{4_F2Y0)AOxGS9gTH0GbITb0mP!8*e^s#2$+%1PS4Q`N3R-ZB
zpLfEtxvfYIKTmFr3J}fH;4IW7pw`t6;YX5<$@`*Cl5SM)R7-t#xP8!;5S9<OBW1Aa
zEO)vI8kIe@FV{tefDn0e?MwC|7I#CdA4{d-1-!Qn10r?~UP~NJ+6q1|-G-a|+P(BL
zJ3`shQd8)EoO`!_=O<okr|4%!uaCg_e;PqL;C~gf^q*;kkMNQ-$RHvGy)L~86$HsV
zNx`U5mq0}}5`h?5bY3t>v+5BqUEo7Ut!mWzLH-t;nlh*ETqD7y0qj*@FD>{rEjIIb
zu|$SMj$xuIozO_|La@k?B`O6!9{lZ0jMSXa_y_U49!y0dN%a433XkGV(Wpth3AS_<
zFbl1R%p!97BN7zQazS}*bvH8QW(k6acrE!5$Sr;>!tEV5z47c+#rU|i29jai#zgqU
zQN&Cb{&vL(VCA4NwoW}W29Z)K+MYl4TLY)^K82}gEY%8}X1ohujJjU2xF<Y<NUf(M
zw?&^)W)$u0wzj^&3JzF`AOBHQuKL+8wT$ZpK>(|3dEJ3kKMxd5oktHw@gLAarokWV
z(%8vX&3%;!*JO?Fx6H?0y;Q!QLn*0Thi)OwAqurA5`}x~=^r9uL!=*6M0`WwJ$}`{
zO<QaGJ45M8zR`$p=46-#%;b_`jgz|xTUXoD$H8S+BlA50@9ESe0OUiKge8E{=J-T3
z+7_Q!%*L16M8?%S*(tud_e9_NSU}~8RiUB+qH`h!e`|vV0bH{_(<q&>gGj}>tzdyf
z62TAruwty)`~2yy@Noyy7im&b<4p6OaD;6R%Q+l-b46<=su=(H$DS4nKrgCy>^}9?
zFwYA`i?!R?j?cffI}LAs-He>!``2<iS#UD@xznE74S?xRz;XqvjkUAj6V_ldYfBD1
z9O>^kAF6>CM8AweP>Xz(RI|bUzGiP%KSxTJ3S4M=7Vle9!FOTv<L=5@;OZVaK6dNs
zZugz0uCo$F%=pI1rha$EN$(H!H{YgDj4EnSUth&O7ToELrA#&?Sg^%X{~RVDKWdBL
z3oLK-$s>u17|Y1AVHWNik(#xQ>6oD8l`k#KQ#gJWPv38yPEYdZN^Gi@)Df)77DzxP
zjsB3R-f<yEFbfio4r!{v+|UUVgNGJOTv^KZm@VMO1W>)m^C2dHCFPMf-H=M%_LoU;
zg0t7NtVWUC!!5cS5LM7lzQIz&ll1p?aTYFxjEY=O*8j{zo7v;lK7U}&%&|?|O*hU7
z*E_XwXCeOhXJ9yFi{V8P1)q<m;grtp*4C^5dOCL~ohrtbEC5VJ01_2ow^+(4M5=nK
zBs0+Zj;dVvTf^=PZ2y~rKDPWM-z=!YW;+hK6V-eWF?uc_`*{yuBvc*Pi<FjHYHdTj
zaCK@@re|FiV0YU+ODImt$8dA26kF~RWfPAv_T$AjW|SauoHLU4r8hj1dh_)=H%&xZ
zu9`_~m&xb*1(>c;16P*u^2gv=te`F;9gbHB0nhuGd_)E))&X=#fGhdBz5abISk)I}
zu7Ly74B4}mha7q$n|{@XCbxKz9*+rv8i6R4VdXpkOK(zC@$9^_y+3yYmk@{?d@;1Z
zWwx=WmL!5=eMZzt#MKAu-+GBnmCtMVYTcmo6mEZp6RtH8`@oKb5uf+q!Ac)Lfx@>j
z@49tx5=<fWNLtm-ZkE#UZ*?HmQ;XjgOBD;Q%s^y5-4@huO2XB~_r(=$y!x3u64w1L
zZAqcE9YNvsXchn&WaTpJT2C;QY>L{(EYDKi!kK;$LVfS~hPzSo7bXdkzqWzrod@3;
zMZ6NMiGFD_FKh!}pwP!KpJQ?qDMm{dkVh5NlVjtVB7V}HS!P?he+n2j{jH2;l7w{$
zgmkHGGRVo`*k$21>^scj6%+?vkJO52>|;)8?-Q>s*=0e*u=YTO_g%g<7kE-Hy*j5u
z4$RL3UIXF)Ltml(`FI&z;&tEdkcNoD>7&XUY1Inr1)m1)@CX;34@%Smh8_}>eCOk2
zl?=h;+fzhCAutThG9NAB7YyzhwjMA3WV0vMP&d^W!FoSs9EQM=xniS-lNsYU6TQLZ
zb73k@r3|+75s{b?O{)wEx6+qVmQoUF{xNND&_BE1)%bXN`*}HU3#ZiJuTK<+Q=8g9
z<Jf^RmxSKY1StLcV_VW5qv}$f&wr_Iq)wGtb%n^VUBkC>MSK0oRD<24@fTU=y6OM<
zglGgZt(5`l+I;XUZC@~Eb)cG60v->8*bQB@jnPw@4vJ~Tk1YySp<vmcnnj+M$o@be
zVio^y*pj2ZeR@IvH*9J8bY<+T_(VbEhwcV#sSgDw-tY!=7vyRzls@h@U*`;y01AeX
za-%)3Zz|?aeI^I7;EL4b5Yhq|9g(P?JG&LAd^!(4u6bJz?J}pV0U}50kDo{y_}XRJ
zR4#F&qNex)MoNEt)sLhIz^i&@eL0Q##ux6?_@jH7^3T1yu9eFif{p6?naY*Z%f}b$
zhnaS6>1*MSz~?WW<Q*bU`ZXNznm1Mm>nUZTRDGC>TC}bc!!m|a#)2#=;|BHVSe8vc
zqI>mG|LR?WG$rjDe8NUUYM52r@MfRuxu*Y<JO!kY?tI?+sPaJHkv!Qimr;Es4Nk29
zqsSB8MLd-6lK;t41E^T*#>~#i;roAx%wswLpxwIo+XogrBa2#Ed1;ho9Hl80jw;G1
zb?L)|=yjM&xfuqU-g8sw)>bKh)caxlAI;#Lvl^>jt%Um(%v`B=o*uM9>cMLO6nLTc
zW6*_P*EkG^)c>&`#W9xb8P920XI&tI4ZamGLdq^i19da~bkYDo?zbRf6^8MXZzZPF
zCt0u*G88O2^va|h5|BVjslTluRF`m3(6IqJkOKht5&-y!mfj-+-ge$<WE~(<M2Vr6
zc*|Wcc{QPHel}Q3zi^#jEqMqEtb|xMbNZss7Tb#@uplD6>dE{ECtJetHeRY$puQdU
zq~GU;sR-l?UeyPcDOi#3$$!)Mc%R>UbZ)=*=c_tNr)!AT#QaRB(KAxMjvem^omF#s
zRj~=Un#Jb>@!rR)F9~b5c*S=Eu6U$x2!vjwR){X5T23Py(6&ywn((UQh9T989qUuy
zb@m$ZxM?4WzBJR62xUN-1@v2;)C#GupH*PI06=voz6B&gH26vd&ged#L%LXs&McMD
zFVha7!M~ka<@rwqk=zwn@K&_vL(NM_aGdtXlO5;fSWY5E-fmE|(aXxHvy9AVV>+dU
z)uQ(oN~^^oUYZBLer7)rDS5OHpw(WRRUisFC|+%*vf!W9?%maVX4sGu(-cg{c*72n
z)&ywm`&382k(wA$=s!UVf>Td+Dk=C*&TeZN#WHx%k&hye9Y-rCE3S@dbW^UMYBU^!
z&*s@an_RqA`}sE}mJi8Q9C@+kzf&2uN&gXfu2Hjr%xQT@jEbdg{B&0;KFd=zgT67J
zinaw$*>QNG#Cq8aRIK;Z3Qpstt^@%?Qdeou2881c!Zw`22W}~>Xe2zT-yCTe2Udil
zmVp%L-cD>2M!ftqCC8$bEp|+Z(3iMiFn7t90B)x1E7Z%#ZiO2WAHn;Vh4o=`q6rcf
z5~zf*49jAbgkDOXYHO)X_j*eL^93ymtf{{s&+6*isiQYc1Xw~&kjZT#HQVpYa+)J=
z@6$-OM=ZV1`2jK)W55sQ_G9ddQ!VOd{Y23jM_{ocAtV1M8kFc-q-v%0zJ`_9LoR^T
z7j9FXxWs1TIm^wB@fn2aL%*7<(ChTt>owhMu;ato8-0v#un6vzlNQOW|3cX1$?mQL
zq93oY=U5!jO38_Ws#e7EM&`|BD=d3HKV?<;flF(jw_<yQ>hpdP7kf2AOM-Ah>pm$t
zGTz2DSe-0dfNH)bd%8dTCAZDZ_*tsKo!0=%+KT5i9FQwY0yxV-w<S05aUn#5b>nwl
zUWxA%VAqh~_o+h3UDCsxmnFrP_x$MUO!WIjd6JE-1Vh;#C`Nc-9Q@sGRn4_P+x^!F
z62DMkfwVKb7y9P+Sdgd5UGvJFxSP(OJag)zhgO|i7yrbkZ?jsD9HW&O>b(8E64$gG
z6#(8GPr*RvH=Rv@tMeyNm}kH&(J|kcSNs#eRKBD-;{2`D2E+NJ!b;1*)RO<jHw{mV
zeirN*=Br>|*)warI#GLx)1m$D6@}c98`UP$LScwZ=dUFdR@X&XR@}}MOWC$!VL0Q@
z%E-a>^#1oeA8&m};4e?CJ<!pM?;Bcda#P>zsSY8a+-aj<>~($Eg%8xpG{4oE94sCa
zAbOV6G(C^0HOF|tAZ=s3JXL7%TNNm!he0*#{eRM-?!3L7O3hpECkhw>ouz`5rUV93
z1DHM?RCKnz{;bpN5GED=seS4F#;|7}Y(d?Ol%=C!TJnyyBF0{T?(B`6s`}hoSDbkY
zG^!SpR(YkVCTh^Ng>(rqSeOE1mia05Lf80M<uPJELooHfY`4BPBtHhZ8W3^(DC}yV
zaJDsG;wG;lYI0V)Ig?dV<%z=QqQ2auuz~T14+3Rv8H#8>CudK3+<p<$USef_VLbTF
z7+ckQ{Wctr=jhbsq&tnXLbaP*UQ79sRx<8+o4+{fDe+6!!;~mS9avSLlJH-##?br_
zxg=wiuO|D|39viNI_c`-$CoupT8wXhpJ^srU%p`kxBCcxj{$!WB2g=ocfm+9Jr{7K
zqmJ(*;dhi@Goj++-98m6%UxkleZ~KSFz`qTh#J|FJzszs0XZW=mK_akbZi+WLOIIY
zmUOU!@h8ya_dl(<34ML@^*kYW*ej+GvFB>T`U+82yXH6sSfy<pz|C5qaKBqPb5R?*
zBUpjEiN6{EO**}IlmD*a`iozl_ud8dV5DS60=~%J1vI`dqlJFbB>*Zn4)>k!eROSD
z0oZ&5`?YcbFH%PBUsbwCfDiI4y5vg>JJl@dqS%>7nl3&GvB{Xm*Uker<>!jjJiPfj
zpAvD27J<oj;zNYuI8^GH=;G7)7F9f)$~nHpGH7U4)!ld(Xlr(Fv_rRXBlD|ZF|El1
zQ_xMrkFI;;xbGh>@3@e#8xW=kf?wAn@%)6}7n!}xC`ORrkr*8niHN13(~@Zrp{ORt
zpFo7nw~-C~=DCW@gOEvH`hlYuZW?`AJ?FfEF;9=wVc7-;PJ<sv6<$GTOC9H%9}-Sn
zSc`t}g8}HNB=4A<#0^GlHcz~$ezRVY_*ig^8iUc?eo8eC=XCjx^nH`;2HlWjqMSmj
z&u&hIirL5mEL975vklPkmU6G7GayPpW|!4iOZ+P%`Jh(xP2Z+~Z}6MfnQW@dJhjia
z-04osn?IeD_|@9fFSTLKD!!fBnvRs}YO4LiK(v|k1OIMdEnZ4pilzL5m)#K<&3pwG
zXv=Kdpm^O-jQ!o;w6voR%20-?+Oe#fx9;Sv?%LF6Tp<&1G1t)QW;rd|)b?o_+SE7}
zOkmY|ni1Qgez2>LP;-Xs2K9RCbivfen2))B%3J2>e$UTT$(H_rh>tn2zrmhOce6=s
zA}&S3wt9uyBXaGyaym2{yuWd^*+zf5u4*?|P34wNR_Bf;WPc7pb1N}^NBt5{S!c`h
z1reK1#ueGrQaHa;T+g)XZ--nVU2{bJr#>T^eWzXoXMI8xB*5q%`h!(D@B*VqL`g;H
zg>5d0Y;Jjhik7?9W{jX_DD1Aw&P$vv9#113@PH{WpLhDRQ@@pRcjH@eR`8<M(nw&Q
z8~wX=v~oX3)+qLhpUR92WIC@J`Rkg&8(%xs!&lOil+t!*qiVolM0hGw=CO$fxhKWC
z13a&6=+X*#wj+YEt~{i|bqLugh*iyqnbAM`b`ZjJ1#xKC`aze8((b#E)mBp8UEfC}
ztDcOVmh8Z~>$61}fp20`b3Ph@gE|U~q-Vne7|rjoz5rQ+od0MpGu$!M`9(!_a9!Bb
zAZ5-{QtwDZOgqf&m0bdd3Ee%>>TM@##IckPwhW2^>t$H_*=x}yF;#&aGhx;e_@+Qs
zon;k<jXf9q(IS6et!l320yIHp2su!DAM=(6qZ_o&oE4c)?H*>N1r<`2Iz1j|8UL<4
zS%Dfd&|9$T4={tK0rGA88$4EcQPkKjbA+GS<{BlyK)NmpZ${*<ELaVOXvBh#tCOp!
zuGV^BcnwSEH{d{smse>0nX7%?-5xvCBV1m7_6@?t8$>DykdOnvuvOv7CBLAa_G@}D
zQ4p+yihTRM+5n;Gzp3BclVL+$?W<u;;z+vNu->4ucMd%GAa^pvZA+&7ghwb%KL<LN
zD(_%2o$sG0|C-Iys8EI09krw&vl%fCv4YFO>M<|o>BQ`Ru;(4w7flGey%sJ`D~8QS
z$^5*bwq^43F~juLYxCg#brL{Q0HwD4tilM&>kBc;>512KG+rC*1Q;>uvlX68bQ9g;
zk~_sL=nMS;qvW*VSv)0-iQWnN(toO|s3TiyEoDl^XKFC*ssW>+_dt}|v*S`_<h^U9
z;sl*(J`yq9`V($#GKA^Ve?&tu3{NMIoS9E%=ZN;d@ret>VQGe{$Mzoh{CPw#&@1-K
zS9E-$2YHgwsulv?E;|d3^zxjvvr>)5{u!xzTK{UHLRMgAU^eKv$uhGpIbPK_c2`Lv
zgAOZm&Q$dPfc_j44Na?FMx$errtfB@3Q;_clDIEdn4x8<{EuP9;X&T}25ccEfI<VY
zEzPUYzA1SDiSc%sN1^&CNw~WsN8t}CKE0|aFp^R|fk4Be3%VU--e8erd%WkITgD<#
zWt#MkZ5Mj}_;z`pd<R{C1nTJ<s5FC7&x!DXYwnHcLkVqMuu!gC#}LovMO0=>m(MW$
z<)iOC-hPV${ljC87{R++kDk6(HS{dt6OZArs+#}H)okO{)D!115BrG?4;9!Wn_FI{
zz2Fpye81z)?rUjj;?CnD$|k^{>*;DSZg<z|*5^P$20u@ehNz73smr2*v+P*lKsumK
ztl-OieWSN^#N*)KWue1kpRrFBYjRIge;rGTT&Q@|PO)4&6|Lnh>NOQ%Q_hR{8Fu4+
zT(*&g+~kSQAFqb}2zc1VjG<>$i?Qld7zHh50$<#Yj`a|@$9qnXk07zfSwUA5`eu?;
z<R<)(BW&>ITS?n8W+P1u*?Y$Izv(Fj2ADB}GQs{lF0>4oL1NG(pClX2(AKr9m(X3_
z)Xr__`2vwbINiw0Jjk<=PO`wL2G-H=`U+>d$yb1^yS`0JYjQJ`;}wmfL26GG)KBkF
zMGudzV)CU5d}<uu=*g%;W%&Z6jF||beR;oJFM6e7$zHG-6Np#y&!*3Xeh=+-C|kPq
z>dPhYI+mxI_C5<EDAJ(}#^B7t1w@YWTGE=}^FW?hewADD-xR&4`sC*~6nL(PzU>nU
zTUNGZVtb3}yC7&Lk<MS5V7ReMhvY@f?N}bhQf3QLFy@ntw#k!3wp%OhY!L%%=~H>9
zn}<xTE>1IeTGDTO<ev67qSfYRp&HZV=-rin*R?7O>7*WD`I~9C<B#!LK7#)M?VD?t
z?_U#9%op0e)>NQs8NVy!Gl04{ATi6MNDo}umjcL`H4SRrqxUQjF{q9tT%WjYk~;Lm
zS%p*YXwpC3K&F(P;_;6zJ62&iP%!Us@H`#gXov|;sM<m=iv!~Z7v#q^BNau)o2?Om
z5f(uc|9S@jiEnLz_sJ=t7y1FyQ0H;L^yF^hRnl0CHA!R#Ni3>kLIZK6oYOtP#0T|J
zCzkrfnPjD|T5nzl6Q#}7dC9GKW2NbN>ouz&M@cip0SARSBCP{BLrx@`b=jn*Xuql)
z<HA@VCrmx&WcFd&eWZsF?Z7HpuF2c)!Y3AbyDhk>pcQkxr9>;NM&VbJ9n?~)<Ih?^
zjbFiw13vi@DeYUmG(HnznxChh`@R?S=9UXVV?A~5SNKbRCo7jPpM-sjCAOz~0c1X6
zBl}8RCqV-mqlW|OGF34^B+lFqm-xN=?&z=Gc4bjm46tAiREqD03*&M=$uC)GeGa3s
z$9ug!{Tqus<@41ukDAhHaX({|D*O~`pbiL!@uilVaR@P1T_06xXfYE_zL8DZShZw9
zT=awaNS`X0xXTI32FXa4Dcb_nn9h~f54SXnOZ&N_rHXe-<Rpp?>>&}4NZ&1M)6sXE
z6I&FEEM2VZVkw&I>UWS0>Ec*q2VQ)1b~&1a{|mI;AT0Xlo#feAcg6C2!cjLV#sj+^
zN)H|JG7UP~gFjvBrAC^41cu%rGIhYCzn&XwS+>09l3KG(HWf8iyesdFR{{$rn_yzn
z4cq3K6V_9eS@xKiNZ(=LholTg>0;zk3f-Ad;M2x}dl{Pw-z#-WHdE85e%))S`26IM
z2_AJ6+Qd-Hf8*$W<|Yqj=M@F%)RzKQbQ7!BNQ;8kW@S4w8jik#lI&s0ZWkta86B$X
zT3M<M4=Man99O@m7DT|&9J|ZLu8XDx&G{J_GpF<PpG)Me52_gM+NTb5+yadwU0f%_
zbC`7#MjdssuZ#QEwstg-_1E!Ug9pWEIpVKedLyrG)<lIwW=m?FR81oAv8QVO8ocF$
zFSXp&EU&#k@C$lpqp)-_p~1R546Th}-8|ODwIMAS5xVVQp>k|;0Dy?Wl9yy8Ah?Mz
zKZwW>T7EN>1-5nYcA@qCDMdJdZ@P<_-U@-Sh4*3vSNA0L(+XtkJnymlop>6L#>a(!
z=l<MkqKtcX-$3|qCHbwl+Z2<dEG2kg$->(s;|H^mu$)7UpOa|-jj){c8YeDny_CK>
zcUZ}2%pq(Cu|FJMf@Hb?t0|TGlc7IFPx&3Tmd!mKO0OvolGfL-j{^j@ha&5ZR*Sg&
z27R9a5UzJ%L03e2ES`BDyiBf-Arkb}h&W1|r*3<d5VVs;Sb5#)X^_ywS<6*ij-fV%
zX63YOFRXylxMVf5h8mHljMv^AMdb!>P?l%iY}oFl%0nKyOm?<6Sy0D$hbA~AGvC3g
z@J4qDP1iKME^dxUV<^jJO<yUjL}7W?zNv4=Z2+`Cqtlyfkc$-Go+kDjZ+7;I+@!eL
zb1ZE-=6t{I>$oUeH05dCEO2cm_K3KZ8$xHie=o`FwiH6@??)2AO^{G~6I6J+jJv<F
zmlB_S=eu}-Ho!|?#7;V!qbc;`$2VOp=n>3fo|Wnnj_MdtwaO2L6MKqgQ@B=3l7lzl
zqKU*+4xP24fI~-zq%ln%JwvtdfcNqVP2L<Ikdn}6^9)_@bdVq3Zue|jucggds*Wd*
z&ilw?l`VW*^w(H8B&<EW#+mN7Dj9Q`oA@JtvYqo>rS_1~21METsa==LJWW2`bhgd6
zpP)@UrdNoyAyLfpRr2A=EQGWC_wm@h@q?-|ABlN4zy=SF0;uZk&+gATE6Mt6gQ|)L
zyWH1ztuddyhJ`|2cV%~)^jQ9&b*-w4+WPHXUE|-K-tj5P_401~GJQz(X^pZKT>;DO
zC^5Y|pf`&~%`KyEwS?XVU3fS2<vfouPcCXoH4`nIaB$u$ZRplCTkbb_Ugc2gCrAT(
zYFv_RL*;YlICAFvt9E60C31nE&C!xFmVp96MlUl6;8RFd1Txr14WEZ-A_%}Bg7_o^
zX;guA=f8|t!D9}E41Gj4_C|J*b+e05E~YworL^XzJb8O7+ad9VP7{#vVv5qOxWQLB
zQE86r`58{lf^iKaQk#L-a^*iWj_LB%zOLt8Z~h~cKS@oEbzL-kWwC}1F_LUDl(b|F
z1*8rw9}Zdm0g-9)()6F%=>;Okc`YBp%io~~`*^b-=3AOyOkl+3-VZ`g3c4L)0UtCU
zeHGsK$%TtLTpUGmkS8(xEy^R!_;{^8tG0^LhTuTvt_;jKsktyZ%;tJIRHD5nr{)0-
zVN$>F;!z~XcE|T+fJH4zS?G;o2jIW`n(1--2Z=BWl=QCQJos;}me^xA+D$UTJ9F#u
zS1*zN=Lg`*))R_`%xyTnZ>HXYhAaVP_^-d0;^*an42XrHS|$dP512pwO4WaKc@5q#
zYsARb@B*gBq5nhAI^TdG`X962+#r<Fz@ov9vl_ZO#WOS(6VF#Ir$8vcg_u-jjLpL#
z6r>ko;NOT?aVIHK^9FDb3p>_X09fEd3Z@yJuK@}lv3a~1TChMBK?*y@$L*kzPZWJn
zUu8+ngS~i!kAV3-iDB~N7?0a5(iW@-gz@t`s59+Y#?Kl?h=pkEUPB%da)x**r?a=1
zslzbd^IgpRM65v$oWnb&V-j7TUc8C7BM6c)3QJ<(3X}-fMlP?YeyDvceg9E!OVBan
z72WBSx?3hn`|Y<)<wJB-_+@z~GZI>K=qc8c!-jOtQx!LICyj;nmYjZt;O>%Q|MudX
z>)aW`5jjFEk+F>FLkXk~%`5WGqD%se17Kk^pgYlj_-eDGzb>_*FoBMU=w$sB<iTRg
zmF=bC!UVYgj&RSOwxgyk`BICzAWoC0X&-HJB!LU;WfmUf`?3sQe>BDAojv#d<eoVm
zfeS$c0$Ak#e`Dg2WQTStg3u4Hj-{(76063HOxyMpil;hk_i9qx#bUc~F9WqHMx#;4
z$#0$jL}{gR|1|%tk%$kn(g!iQ+JqO!k^zt6V~}2AgR#tFe5=)k4&yv$fp)=jwn&P_
zaKq@o88R;MRlPEQ)00;brC0qknh{iNc-dM(KKz~M2nzei?}o%F(o{+P%2LLohh|eJ
z`ub+>*X}&ZwK4hHIVkw-YuCq)4EFIY6((D@_z0ha)`B~YRm?2vpn>-F-(XqD5q01P
zpJ&|BU1G5@F;_VlV%RrrwXwkhFT#iqu2X6oj3$8_Tba{H{hV>Wj2r@6=R+H(3<kUU
z-1&N~ac+}5hJ<EDoeqO@0o0W^b5p)bR2jtLl-9`Ckxy`;KD#^&^{|dnct9@`S}uxu
zxRV|!j5eFi6<_pDE(;RwsOq!2t8w#tT{S>`l|5(kzGWf>rbd5bkV7@q;<*d9eLlz^
zJ{W9eWO1!HPy039jSoW7TpD6>wLcdftRsg5{_918_sZtG#j-`+U`Z)Wy&z(w8zrto
zhvBeneu{Y5JM~PIVI-L@(n)I3pVh$nwS8KhNdjkxZWW{p2ieCkoUR)+<5+O0wmj;r
zs3Du)SL~p!JPCl9d}1L~=tOIhNpAPi@WMaiXgq>anAK>MMjQlr^X5Bc<yI`N681N|
z#gINGKV3<0VeuXe8^o5j7>vjKZNxZ3?o@u--6DD8Vn$V|SIVUKR=QRI<*_@nxr8+s
zt&Av_NF<uqV=xCyh;ns&`xBUPNx4L&4DV};I<^l)ea?!(xmRSh_J%W8n_yL;IwFM$
zBN`p_2yR<^b>37s7FZXeLgg}1*2+UjI7DMubLYwX9m2ONMAplqX`RVE+i(j+MhCHw
zwPrtAdpZ1Y6I%r7@nG{9zBB9zsNc*&%;-*9|M+Xi_p9bMi+==4h|-%^1-Ok1oyJfl
zK{__+uO)*Ks1#3tHUvV4L;idE5c56z>*{54=qVy|#WT<FCx(Boxkaz6Yh_-ILiqif
zrYMGC0?keDl}<+e{ahH-E4noy2Y)$R&&D6-|3H9YS+}hMwWfgWtbi-F6Z&~{2@jb9
z4#whs!RvJ+vw}Br4co^Yh2E!T2Dz`GlHO@9J6A!&%~2hQERCzrAzR8;F}8?;GC9|X
zyXrTJOH)s}q!;?V%XV|^-`F<#KorR=;$ToM!%?<%N|6<}ZaUd=vf0#67jL?K9Wi5)
zJ1f%7@5#ieq1Z%RO1%n*<s{}De0Uj4lbSrHN6coUY|eg|hRLfc8Wh?z>!vj6woOCc
zJC5gL_BHEgVWC0ARmZM!>wt@S!rehNKcFj#lp!}n@;G&_U+;;*Xm93@>}C<XMh~&}
zsvf1D5uozjdJO&G&-#6bfYpXH<1dFcCe8nRywRV<Zs^TEN?X&A&sI~bSj9hbxQKpn
z3Qgw7zU{$Cc`y5PPV?q#?m!eMR+<9%->hWACzC1(qEcjtNS<5eavJ<GHGQ8hGr4L^
zo|U88(BJTUV^jteYF-7P_$C=PDb#!}SnotrV(RtWw}(}LI?vDtK`S_KXNa$+u=LS`
z6$8|Lelpl8%Q)&>6Swr;Tf&YTkNP<CnW-qgVKcbmWb|KV>r>%Dh;}r>k3I-++$cz9
z*>wD#-&x(hbI1%FHM>9WDCKE~f+8nEWZxH3wgTZ3H_39Bmn)Lfeey?!r7zDU=SRYR
zyqhlmAh0kG95X8<a0`_0Z<N>zH&8rVbhtVj2v9u~-&*C%j6@jy1~FrGf^0sLvvgl$
zsg9hjA2JRNjdgU|%e-qS+;0|omMUoJ_%*+~x;fc^(gYjhsCB(dw*j2PU*~(C-;3V8
z%<tVn!N~b^oiXY+$<B}r9k7Hrwc-v+xU&DKil0`Oc|nUgmYIjVq|5>tvtokw7}*gd
zm@cP|%zglyAry5RrvHO0i*00XKZ+hJhXg+unM-&G9<{hkr4nk^nd2RpFO?Uo%+784
zJDv>B{in5q>L=s;B!<`jcg9pO@sSS$OimQusXPxQQ7DM{;UU$e*bvCtOf8xZKQryu
z*imo%3QiWgNG#ytHy4mrQ>R9%U=k@$E*(J{hwPGKtN!5^u;e#rWsXk9cKbKMUSg}y
zXIH%P%Nr@nWpx1|nVO1w<UyTWVV|*V%d!uU4Yqs(FDB1Q$>A;PN}pwb;&4{BR;Y2_
z3B7F*5X&IX2-gQI4DV0F9gm5PIVRh92rb$1StU?ybDew#T<;OxQy+QoVOL@;cEeYR
zLUq#476O>Fo3SLgkV`m8LbsN9!%2l@lf)rrKf2r19mgs`GAl6sbXS6~X@*3hqkgJ;
z<Ri&+QsGOB<jJT!UX3c1=?MI_!W0$E4(BGF`e%;G{dk)qHrtF{Rrp-;F6ZTGj-M}+
zzOAbd%;bhd91_AyD(`}bBFc{;@}!D+2!d3>1%b9-nKzFQ{EY?5t8n@W6U{$aluG4s
ztD^=Ie-@C{g4M7w(hj0A{b^<L1$g2q0oj9>0PhWFB>DP2^<J?(_|oIj?sq`&c&EcK
zV80IoAO5_DR9TJK_W=8^Jx4;Onj6N$=7e4|1ItU2sWHpD>7n+`w?&oCiSu192IYvB
znAwlEf%1uVQR4<3_}Acr!w;nLlBzw`>HfLH9#P5J?+ux9CNff2JXC)n9;1Pm*UBFl
zef>0~ZE5s?NJ*)kEW|2@>ZTxUa%0EbrTdU31@jf!dN>SAMU5J5)+i89=fVXYH7bC`
zAma#o%vEG!ADsjjp7>0%PvIqfcGvYpI!6wn?dObR3|AgL*%JW3am<@~V)$L>zfEDI
zy^a$*?%TmvpK#O^v*==^=?{!m47lnADX6VL&<X%NUe9sMCdCwNS|@m34c?0i^$cPp
zT0XMTrYuIpMw!)}U%J=`E}*3QF@ya&&mL2}%97@F!7fI=M#cXpd~>o{!XAWSGnx!*
z_6NjSJuvyD8QI3Z{%Q``b|JZSp@yu$MgzyLA`&1CP0L_U3c}c>G2>E~TlI$#mBxBz
z$%Dy>lX7+}sr_HwP@GAVcu2%%X}hmpxNM7@&H`2*>B&%Uldn$>tIP@e9@qQv*F974
zcA)l{TzM-0#cz8x?a;@Rx^~!3fZZ{bCFXl%M(g%-O{~OLx+$GH(OX3M%f!bLg8p-8
zpQi&W8t~VHHZvPcVz@D&06l0Mv%z}|cN{2C589;>pL>du^+9{P^uXT90W`lQYf#WF
z=9^j~xLZAlRK0lM(}A)bNy$gbbhJ|yf^pW@X7bvd8ym_$ys|oh7!1&}u{2g$YH8zT
z+70TyxoPpSs_xDV1E9_h`5{_f>u@ZmZ-iIAU4`L|M&<2}9vf@E)m=>YOzC)yYCNB&
zw^vGl1CR4|{^aF}KCLSHo&B!sFJbE(kvj=6%CDdMt$<okUB<E08JLFO)K?<7NY)gq
zg>E5Y_&HJ5zHa**XG`9fL0;mk9rs}F*`{qlc&62DbNs=DSg`pcuoKoH_`qv8Nv4KJ
zmp;#5pjDymh>cf|J25<2!7!;#>Y!>TC!#U2<Aq&9cE7z*;TJ0Q$0pRZlZB+R986^D
ztvdB5g|Ew{`#||w`SP#sh(>=Y1k&qGKG0{6c3Vw`iw?S*j&xjLSOK74ssXToe-OBS
zZwvAA=eXENr;vCUFbK<-!KrQ^q#sY@h)cS2T8WE{ik3TC7I2P7uh<5+bd-FHur(?o
z^l{KXL&>d%4XN~{kIJ~a>IF%2_nUWO#haCkYkOR?Gi^A!r3!zu#(65Ssy0{Jx_81t
zHaI^^_zlqrh57}~_5_V?Sn&^-aQyY6oKyai#q-ES(wb~uDSDFR)T?+!yVZVlu_9Fm
zeL^mDDOGB(R=p#Mx(1i*F&mNBu>itc8QK8_#Ze<Xu-xhYfdN`KHv|KChDuWV1MwUq
zMk5hNAbBWr&#N%!YOgm(ndCcA(ipp0`mjVRe=Hf{4LPEcQ{j$Nx{<N6p>iNsW$9~<
zX%Z7Ki?aqO?0K>5Xaf!3uxvmWw3A^k{$pe?_(UB~@tHZ2wFzL3OG4ZMv(7uY5;V3l
z=^2hEDBr|N=SVP$P~Ge`<dYu`6bskNP;X`yDPvN}i_@$us!c*rlmey#T#)w&M)cc3
zMO7c(r#9Gm?URMFG~HX0Yki7=yBl6VstMu&^XWlU51lsq<d-|*OusYBIEgR|mj=(2
zyk6e0>|lAVvQCH;pndfp)zsHqHEp8E`{2Ka8Co|VowJ;59TDznFlR2XY!_)=+oPpG
zT)W7l1eo<Ge?K8AuwA?Xs17JAiKQ%%ei_|a3auazM;zv;`-Gt%jnc^-Fy}F5)nmTY
zXCGot{*PT>)?bZ95~638Yd`{dpBOeXghGc$_3r2QS%eysuN|AzBES_#5CtiO2vCv&
zoS#V`>BF9hD~3X7+9jvOJmSPobZ8Y~fe_lO2<;RaT2g2>h^Enp=P1q$pt6&?Hp+>*
zr&~#t-8JRIp%mAHMM%U|Sfy;?NZQ3T3@;h|+7_RQ>3YYt!vDxUvPN&|kw2`>n^uB(
z=D-g%g05a)%ePhKySCG7JaH<ql(7kNSQsJB9zmT1NSjosc&^YRqr7z{84G)x>h#`-
zt$|@jTarG!6yk{NR<d9kjOdT4pgggh)0sJy1tVEJU-^%V={LqXLp~f<Ul;cuiFOJV
zdZlrFhm|5|!-tixw&xc-3DBrzEh4PcCP*fK__awJ(R038kb=nn`z&F-HsMdeO!GQU
zT+Ck6FkY<Az^RaARpS<S?Kd(f#q){=MS&?UKQ_T9YOCqwW`}$Rp#025A`S$%q=C<a
z2Oj08)MwLVB5;(TECf!-_QY1qSPyTqdrQwjFsWYomPk^FvX5ZWs&b%6l1u5~lf>vu
z?h0!Cwm3YNw~g=nB`wg{txY*Ja@&nsFTwoRFNyZ~Sn8(})#9O4Bc!})pPA5@d_H_w
ztx$BhNWNrr8Oi2v>h(o74f9#0{S!>f0<UV8nX$q+wVjYA)ztb;$XZBll%JFBN8Lu4
z=jN`To%mb3<gL;jRqR%M9Os3Wz-znfNazy61<!2M(_{C(nwRXNW$&GBIim`KnGk>n
zP*1o23PS%FpreqLctkug_2&)gMHtGuFnu0x8p~|@w}L~G>iQ*<`!0m?&DYb^jZJCn
z5fekz!x+m?3^ZEAGJc&=%S?d~afEpWhc+7hXFiGr;cLnBJPF&g(%BFtb0JX6F6B27
zN?{=h0)z)I9$Pg5-o2FQC+#w2Z}0XIgr8x`X9GVQWm!rMna3Gu1O1~Z;h{ofMs;@*
z-ojS~t;UHhhPwWSw3H40e$}>FLmobPaq1o^bM~Xkm(LcL0XwFlr6NwA7i`X1*)&34
zP#UzGeRo#4cmxsv=ZRpEYuBp;9?FU;+!@<g&cxzqUgT@A*ozD0Tc#)ARt#3XDy4H%
z=N-p(J)A0<V<ji62VhBhR?D}z-S;ihjJzdLh4GxCa=Cvea#d$nEaXxq%ZsE9?oU_G
z$KEo@4wf^x$@UeLA7fa)rC@+9;o=q$V&t@F#Ya*&I+-E@)B!~psA81G8eyadP}ejX
z>6XMJFrIM<{WNeahZ4dqA#ONnzT}FoSF#(WR@TTYz*GnX4AON#X|}aX$|(Z}>3X0v
zXF{xZ?*&W0>Vwik+6dePVI9Xr<4sQvr}Mkg@O&a~4z)$z-m4HVp*_(WzeiZOzf(qt
znM!@yj?Kh!{cr>uuDw~i_BSYeSUrxBMHpb!c@krsTX4<hmJ>m6O4O?LgWCCeG@qV<
zsLgms|42&pN8hi}+JIY5KU_OQ!Y6vWYKC)9%L&Y!>i1PI&Jr4KkxrFKlrF?u{93^9
zdn&i;y*!_O@{0;4{#Uo@KC5FbhGwQmsm;8RY0d68&6Nd+Mw&PMnolsueX%V;>F2w?
zS~@y+n}H-dHWp_eV@9c9T-;H`vXj~q`T&<j?O&207qBt?#}IQP<|9CeuEqLu-d3v3
zu8eE*2bi#UfoZ|?y`elItM7xB^>l52T%DHu2BQ<Sg716E3_&@uJl^SW=CGX63hMVl
z-=6<~FSg~_X;z583klqKq*iC^lNZuh`MfyYshL^;yqy%&ET%X5@}8h)Az=TM)mvgH
z#mzrqt_J2^XtN1r7X2Yvz&=<l;EhhKqEJ^bwzM-94JQh>cIVQs+%v8-$gBFhQz$p=
zVTBiv9o5?swtFl%Dq*D{6f>1qSh#TwMss<<cfC{y8Srr)Z2ep_aN6>YEI>E&_=&>y
ztEg|hwkN+_6yPT29J%>#RCex@=cIh^2Dya7NS#0@z-*-sx1wh1Grkt-OkS;~<hxak
ziW)$a@C3JVAG92VK4sj?-0<l?adf_M>=j&2z30awMQ>BzTmfo1x5=WMWpPc<jUsj7
zYg{O6X?R{gA3&(}ptmKzA79oAS%Q(d32x<`CLR^;cQIEYe79l2pfF2sI}RbaZ*u>S
zq_gmA^6~!mJsdE4bmJH`I!8-)cPou_hlGk7-QDUyK)M?gMvjzj5D}14LBK>m&%VFs
zU$}Sgb?$S{`&?JdlDC)*d-yLx;?>g&yNf$ydNun1Q;mA{rGBxJs7dPV7xQpHvDr3l
znR-JaZ01FX&@HVtxWud4xRL(M3|Li_Rv=q_CoF(oj*xh>&AF3{`dp=mX!;Rgy07<3
zEPZ|Kaw4C#rNyM`9|<?gG5nVaaj;)qO2sM7>)G!o9X$r6wEVu-bcZ-q@hr<V<?<i5
zdOLeMT;HyrG;4oz2f?xkNd8wRp?`)Z9q55-6JG|@aG)X=L~SZfARc>~#seCqI#&bk
zdgcBl$BMI9QnU@xm@_JiyYW+xv>KSQj081U)ANXrinFRNRN_NKm$z=W<ur<NcPAxW
ze!wTG%cS<;t;?11Q#20s0y;D)mP`OGJ^(tH#61N7HB!qdGd|xSJ2RiR!iVR)<Vo8B
zz)W8Of6GCp3`?bf49)xAAyj&IEQFOajs-y;3&6-aP_qbbyjh2?2c%~1i4atmzm^5g
z0Xir^hQZ{m?ppe78p`wy5i*@uiR?o+dPP2ZtaTpsxvjmyfOfx&T9NA&Z2+LpE+WS~
zg%5eH9_JO?oKLyUZQ?k;6x)Zd^sH{-8jNWqWQ2zShX@u+$(gHROY2xhpYov1ZdOO<
zipFUCV~)@;d|D@|Ko%~~#FZ&o&WNIp6RI^LRA?!R7ehVksOAQY>FTT#@K&(8IM>zR
zf>8(7JY(KY%#xSsJT|x41qR7RPAM}#SLM=R9jlUwE;wF~{G*L2b3<3#f63(!C-u`d
z^mD%FzbKFV_ax38$5UT72%$~ME7z%^{G*5H6ZEfg7x-U)X=X-WDdi0<0vJdwoxUXZ
zg%>mVEMSUR^$6nLAaL-kUrPLTFFw|J*Aq1?8wd=gEKkD)73m~C(qm((6vUsjL$u}J
z^o6C8el|}NXo<fT(mTLIUb%W_r}Nt}bCTixI`a;|9X&v5kLXV|k|M<V!%v8P&f!W_
znA48v6YZ4kNXxe+d(#>ISZ;Zp`g*&t={2{MAB|+#9sPuWI8;EmzT@f_!KL@ZZ#}ur
zj{jvYPqSd#d`+pw|CI>*Gd)$;p}G`Su>t-wD<jdbtR~>H97i$KL7rWZ{Jk4Dqz3Dq
z*VXwA=7z|DOgHW0Oy~tw*98fk@pcO_zyG*tGcwGA5j?u?$CWk4fHR9;eBd}#*<!dN
zNx<)c&{*X_r<<M&O=wa!2bV_WHo%H1Hg==#K>#57e90|<Y^Vt!62RD0G~$5{D8voC
za$g|Q<<!fFe@v;|k}NGlAP@C+qMW)IY@VDwj}j%k&r~5f$LaCLftKruDkE0XHkDK}
zD?S~*O1fyn>+Gd#?XAi%TI|-CW28`kQ)8?+0?2OKMDlvD#aPn^WVnx0bee)|-#+D$
zSA{_>{|IfoQgk}uENyE0-d|xGZ%j*)IyGrl0=L&pcB{?GXLeBAfPeC#ReCs4K=9zO
z{>J08k=snq>e$8~q&M--%l4kdh2?gW=V0Ng7fCT!Tn4M&JfFQA`6^e+y&$wf757TJ
z_6As=L5RLO+$Pk<s%?pb@$|h;YO8fs5;_YWVQeo=!D*ZRBS07f8pyU@{91YmMz}$Z
zbNSXIhQQ&l<|IHt?G=5JT3O0e(4v;6yehp$FhCu)ywvAwks=oo*c7Z^8Wk1(oCPB`
zC%nuKiZ}J<+NskH`>M9``^W0^K=`|KU&^%*>z6G3F;{(R+1431&exyi;?INL71k`h
z_C-Da07ogSf=|7%kQkA}ot)P*xT)Al(R#ug+#;z<f_=OsWekc^;7nmL`475<Eq*o&
z1mYYZ<zz6U!z8g}o*xjzXQk!aUYmkHjlI^f@?zXv$|Q2mjz)i<TN&1gB<Gy1=(ajq
zuhc>0Fa90svfAaaEwZ%FP9N(rDasq^*QFFfIT?_9X?BA5W@_7lEQ92$TIH4^Dp2Vu
zo&4tfxLzh#5IFVfyZGqO8NCT}{jyOJenje^;nyKOZa2DM#wuOAk0U0Tqx9xZvVUM)
zJHfCv>V1HOii?AEl66|sr7b>}yQN~@wxMnuAe!WDS#BL!^f$(ThfP_Z*+|S1O45`>
ztIiI*bF9xW&ayQWf@pM)kDZiGZ-hP(zwRBNDUToRxD;<wV0;^rB6Toqozl8fTS9r{
zd8inuYB#z))-Hd*8Xl1IiEa1ouV)!Yv{!O=XqM;R-X0#MoQnE8-m^35B1rVZJkn%P
zEbsrXgGB!aLNH)plZjEUFqJK@p{0GOyzhRD*d$d)r8&JiM%d|7pwud|tSvl&n?jkx
z*~k#-`;@Nw#S%vry{RIe`p;=T8T8qMD@J#MKc)2^%B0+>i2W(7M`D3eIlVB^U}yoz
zuxbnf2WAuCrqOr#kkb8iAZF_@g4x07gr*S?7$W`qWa&{GR(M{2K)w`Jt>5&s{|Uoc
z(y;x>h4yp#`}ruhA<FH6e!09trK)%O(W&aMQJ$?|`wNusXA+!!H!<{>W3SuN*t@(p
z{^>Cxw?+gu^%j4)LV{JxHXQd6FYW#obg~iBw|lUxe>nFmWca;4>Jh;7l!KRd|3kpv
z2d}eey+oeszwG*PIQR+tbM!w2(QSn<$;ttzq<``7()R>PP(_-JA>$D{luC;Y=ngEi
zf$hieMHvs#8z%s~rT%sKE}$MHBuT1nN+*%1p6#Yhr=G2GpMEINLgMMzC7Z`Ttp#pN
zx~A4q`S+SNm988kIdrl4xW-#KXoYbwKI7Vk4hN(W+|B%OGaN8DoiorhIO8cGf&!UK
zm;gNHE_KZ?OxhlbGYLKifSNSk3v)ua-w0;$EpdeDU0K|w80>);QKF%CzvN;G|16@i
z>Zgo$j4_~^?u8}aE%|8(f*Uox!uHF&sqvI_TPg|eRH=Y`1bl%dw!OENZd&m!w6g3B
zM=9n{;}SW}+a(IEhrmLBO$|?}2hZx`7m2pvj|<&foCt%rrxy=;675&uc>I9h;}GA?
z=1Ohf&209;Ae<B7?!<2JDSu!^#1KehIuFm3bBv%N5z$}$0#3G;5#V>*X&K8abQXL9
z-`K4Q9Z0l~NU=u4A04X4ROE%xX1YdXh@$t3?y-xyKT;w~hZif{RXdVZ5~uzsY{dDz
z%eAgk@bE@U7`aUAdw?SDDD_}7MF_~ExNvY46t@ms2gSWPx+Y!aeJ>Q?)q#7WcyT4c
zDEx*o3Wd)*p-_l06iG8b>`tqbxWO66xcK=ZyFPMyx=;&NKlS!a`P6Uga9Bh55B8w?
zsBdqY8_0kTSl2tYZ*2h4RF8Q7bR-V$yZGf(@76rXDwZ-@d8NaDihWgvy(wtXgE>#3
ztf~5IP>DxGy2ZxS{#;)2T}1$(>RDPO!B!XrSgw_iZ+w|3satAD{<3!`$kr<M&OjB<
zJ6hZ~R%s+A8O1l}Ty(D|6q@`&r;LjecM)e8_cZAJm&_d7CM~Vr|6UO|w7uKxjm0f+
zRuSMMqrqC*q6;KkJq%fQX8^q8f}A@^F>`MNtg9=&&Al#_?hHo6avM><y;cDB+}Pu@
ztVoouU%bHySiq8P(H#VcqP!WE5K_!w?LH|k$sREJVyYt>O!~sSEgGOE$sCg)#TY!=
zeng&)t|`hYf#+lhevnN;W&xYHtWsIk&xic0N!S%B%M5Xo`~jJPbITwND?UFr!SF`_
z70sK>#t)9ML`<qyQInGTx}F8UnA$6S>2e^JVY<3PzJ;}8iKOh$vaC<<+Y|x!Y2?Bt
z%}(Fg)OO_y#jfji9tu{VLAjOGPw^V|0qz4!2l92)6pc0K!9aQ8;`cif-xGzV>-!?I
z`UIM-6mf?KIQ?0vHHE64S&v+SQnN!$W5s)9HTG?UF~NOS_h5fshtY=(e+&7GGfHzS
zd_4C+m8IW{v}Oa9xen;)peCNGssx21jgJCpJ`5WiFadshfIZEJ0z*BJ$YXBkl8y?n
zmK0Q?nDLytd-(Ky8Gl!xsqTm-AB|6(Za@(9pNE;Ankr8+rD(9{M9=W~u|eu9nJ>jF
z^0_Li?5<6jl-TFgGM?eutUb@AI*YVj{l7p$4|u|Kwk$q6Tt1MajN~>vf~sezQsy>>
zS%QzVOtV6bplyPBldFR0wl+s*nkLbrLdq$cvjf|J$DLv~nHp6D<*$Bge3}}n)QASz
z<o=B}{s%3zk?=X!M5_ym;D&pQ0`}4rVoib)HatZM7k0|yO-jZ$Z7e+-@?QvFbTO6u
zboMS!<L%79B(kwLNMHC|)0M2nHl6G!;{2`1lG;>aRI8hMod9DhF2Y+&@I%PoeWc(3
zs*Hf2F%*4%`}{I87IDZ@Nn~gXE6+dFsvu#76dekmbShLG&r&CG2bFpBeqELxXlTX;
z*dluqb@)jm84QLV2Q%O3+!D2Nr|pI`?CLj_v5MQ2H&qw>3oey;)Ft+e#?GV9IH%WS
zvdW4xKTgkXT8KZ{Bd)7dqMN#*J5sD=PAta^6|nUw^iAN&Y4H^Nvk_ZB=thFvgWN8M
zoyp*7kAn}$D>MT&|75Osl_u6!V*?6}Wf|H700t6blxjJ&$Bm7GI3ACh4eVeOC8orV
z1tdA`(Sm)IKg2P~F;FU_v-b9rWEUx6DeBIYwNMZi88~ZqWuwmG-Br?d)UZ*n=DktU
zQ{Bwmu*meSYSvNG>PFA|cS*1J70!2;>2t{&!4L$G3sWt+#3xIojE$M1NFCG?7?wjq
z)&Ib^(1x4x4{`5^Xe^+4O8K*M_%v{;ftE3)b@(elCU%G8s^my~m8F9Q3vTt4EJ1iA
z@*|@Tg1cYu@=-Le``iTXym<ROVe&k{-XV{Jk0eobSP>y-MahD0e{rEYP6sLhy!xPt
z5kkP{K{bM)A%dyDwTplvK;c*5{k>#BT&aeu=@mFU@~HQMPz~~<q%R1bICpkC@FErt
z`4&t3&F2nh;yD!|Cd-Tj%;`ky4B>>vaT2SvxJeR?=&1fI8G%+%ks%jYX{kjP$dNRv
zCk@J!nyqQlZJHFz+zDFeQa<csf#-(In%R|)VH$1R?xeV&vaAFh@qMPJqu+$$xh9Ss
z&^!$}=>LR=gAJMmrp-EDiRfq1;OpHLURDAZo`UURG6I3gxg^Dd$?iRX?mg`IH;Ljq
z!6*o}#VH!-7D<xj)HD_Bqmq(^aNhz{6N~3wvp73=oS{VtJZaW^hdW_eg^<q-l^aE3
zR!(QfZ1xTrOj`8rY9B&kX?S`*HQ*=df4NSsKaWxj<c3_XJAvkY!7|$?oq3x_zIxy_
zxFE)jW1dzYl{7VG0(*w-`$1a<2FvSn7Kg#>Ls>Vt_-!G>&5<c<Gv<)N>&>eek+p->
zgvHSFC8fPE4hmaLuFKV9XT-hM$f1bGLnZr<QkZ>tZ4#)TE+X##O*1GuM8-PR^C;)I
zc3*dAA%9xQCoSL1>gRr}(PH*(*{C*1Sd!h9z<snMuXOBBV(<y;A}`@mtf+yDptull
z>3#yb63QG{vIHa#mTU$ou=Mh#gs#4$DJdzLT=aHn<@xRH@Y!3y+lyZ``gF9+_v}T<
z22P!0s@_=QyqPoLmBB72#h%2MyCPGRU6?!?hMsa?2!JhT4Q76EDNztT&B+e_FopT@
zj(y>Jdiw651VWv6s_lI4dP)3NQtSBpu{Py0(ky#2s*7pSy|gn;Kk|cfl#yC-5IftI
zbWajj0*bh4-|YyKfi`#`&Zp+{9+MbNdkHHrOy!r!42D$4)YgBz`T8=w)bn4G>#xI^
z#Ew~rs~NdPKJ~LcUEJ({ucSnBtVRj-AuB>Ei&r2s?|WJ%s{AS=P8nv~cb*06igUZ=
zshiz$@R}r0Vy}w|%Qb+wQDL!RS##38<nZ|BeHPUoW73>a$#@ex7Os34Ks_yD<eOhA
zFZJbkrcB$aF@dMfV+v#8Q}{mzrrbkrr<I&GF%P4=?F5un7X4+O=SXyWl^T{l#?x4X
z-J*5H<`tKLp_O*J)dT?}9n)E}OUCx)i}9JUSHH+?1v&1$-5>`T@S)HdFSkbbMlPG2
zulA{LB6J7`<`|Q~IcMuTA_aPvWYa9Ug2Q^ya3t^abVXX$O7zR`mV6rmxkh>@B>!>J
zB1KGvxP)OU&a!FX1t`2=vCB$JTCvH6P7`@Wk*#Y^KA+ky<8dWkG=3O7m%5B`|H?C)
zwb3z0-Z0M@07FTsK^D@N*F*h;9Xc*R3eR*|V0#^Cv<oF1=cxkwAcc-YQBo$!D*SIp
zsIfHex1|f_bHnclC!_$@c=G@42%&0uw|9%l19<(;2f1K-vG_&YMBJ(NM6?ds@-1hE
z3LpoEBF%Dhg3anU>$?@zy{e=_z7)JLT>Q^?nyVgmkdOzBiq~OBcgwr6Em3~*_exSf
zkUFEmAHXVzdv|Qh7<hf0VbHq&($Yqnwk5$z1-Skcy#N4C==yjzbwn>&^eFWOKw4#C
z_3Bggs6@-#N$8)_wz(GL74Mhre{9mDe`^`vP^fCqT9=2)#AXGtV6^`d!$M{CDyJq?
zd=N=Oqp&uxH2K}1k>o;ubK<f#X*M6}lAArRNemn)R<E}!`kenF!75`E4=W=3+`-ad
z@)JEuAMCbvHE$%&so=0xfL)0*HYI1nt|m2<?>CsHnl~==!Y-;(;YAMh-vhL2<)n@V
zOp8<Ui`(H;p*q|aaMKN2Xwt>_$K_b5g_~U+$g%1H``?E&Jl;ChpCv0%OIhSfy81`9
zq6b}+_rFzGP7b>bRvRD-&93|PBwYnRRKtXdQaHu<t%lqia09RZP-OK+@{2hP8RSZm
zsoM@FyN@V|$}VKh@g=6<9G2hw=<}QLeNxP4>HaW|{Rl^xmRB@Cst^bMUrocOVJL`E
z&z!|#0TxCu;m8o^k&qdLAC<CKv5s%mQWK!C%5{D?-WG{hFY6!@$f!~G951vjW`LPe
zk?Oh0b}ip8rJQ&#S-W?tblB_*$%lEH6pARNhoO`0r?0{Fs_gCF+ter0&BF-#la8rY
z%MNwUc<r&eZ9Mt#^AJvW5kirmj!Babr3(m17!if0F@Yi;qiFUTwlz2ofPO%%$A13K
zUGG}k56ngP+(0kpY*eTe{Zqz)1y9V}A02|(m#c3td(Q#Tbo%Cb<3(fRU>NcH&91`d
z=d_Vy2Y!#PAj%@7)G1ec<w*!VvV62`Yg2ODl<)WT+3s65qPG<Ks3?GgbnOYAeSqb-
z3V1of+7L(mUh(At`#Aw8@i#vZEb*N9&QOaQ6@g5P6u~$)%Z&|C6Eh=!Kir~6LXHhY
z2cod47JSfAsD&J*nvaDp@nEYjKh;R!wJY=3rJE1ym`R5|i?;Yd1nZdJIhuCdbeI|T
z@^aaW61E6`&po!7_KH{g8;c)8y)6&58LNXJLJdLiX&C)uE`f8nK@Ie5HdlfSiNVug
z!5EC0qe2B)9hp2VHl(uuodqEnFvh`PsQCu6bm=w-;^MLJQOtZdK0lMG1iNC*K$}x4
zWNIw6H)?7w$AZsM7JGM!h~EK(RO+zTXjbb5LloqF`|Mvgl(o~HCs(j$_n04?RP4#l
zZ*e?v-(ZP{8NXv0&`%#)opjc^*-5vv`fP?T9gM%Ejs@wpFmjJEEO`nSb=w^yHxulB
zM5l^xFXm52o0~`Rh%Yf7{CK=e?zh@$UvTxzZ@`-VVE4VuJ^iR&8NK%j+n)JC2@g7M
zK9hZr_8UFe&+Fqcu#{cVPc3K4Elq{nTA85Rn%Gk_w#Y13U4$_Ng#2zB3HHh>ItUFE
z8aGAC{Hg|#fl5k3w4suZ5&6~s3%x*Lm^RY2>Kb?;icd#1YCI!_B?E;YO6FX@IhMx&
zlBLlf?E%(k#ik}QqH7ehkAUwja_gIGW_jF$;DaUv=aca^5mzqV)fzGVk&m9DG-y;L
zj!cd-EgC+#kk-dE=V6cX^+q=Quw4G?r%kxVN%7R}Vzd6Ke)?Xd>z}Y6LW?^%VO!JL
zAa|UHB_4mQUu-82;VS_mzu?`_u%<<*4e#dJ+NM4GDV3AoORBE#8}|O6JndLZt;Po(
z>ZAS}pU*QW)d&{!p_{loZ}-)kp9ebM=jcCqcSHb9JiT=+BY67mlt^iRyI@a1>zRoz
zOoY1@{C?>#DjCV6s!{fD52XZ4cA-#xBnU~8w|O?!1C%9MWbd8djlaMg4AX2E>cHJy
z8ssDp6R@Bq3nkO!NjWC-qB;lf|6mE48g;k=RRe)qu{xe31THpA2N7UK*sIIK3;R;O
z4#=LZSky5+nluLyCQp?h1!ULN=rQz`bqhu9n!$$n<(2gj(!C*E_Om0)x_{7hoN!p2
z*Xnt!v&UagQ3f!!U?AOT#xJT_J!|U471-I#FS6N#!O!JZF%PUOrzM-uWJ?Wk6|5Z=
zKcVM35R}T*zDd!yJil}|2iz`EmeajJYNdX@8-6!E{&V!*hW@x1`Ay*f)6i^HUW3O&
z`ZB1>l*0?~bQ+Q6i-D`6c^sD}EY1@pg@WK6kNU85DU{SV$g&QX`Ex(Z`p$x`@478m
z-VwU-6u7SdNlQ2H2!DGl(JH{PbPB7ELU+^JZEX#G3VR)hVhJXfzaJeN?oa-&$E{M3
zohIE_{GJRn5q5xs6;$#rA2GZtIv`K8u8akpm<f2Q8pMPNf63bDn}|7`=1~nqLeQpt
z2|EmuUHR4D(PgiIAFnpXq8C5Byt`Q|>9Ii`slgyHaP-Vj)jp4tK&ba#RougbL;ZRh
z;Kpig)_(Q2$(|dwcsKV4>2%;Phv_EZRn#v#OzKZG<S#k51s`y?lH_S%5Gx$v>eAo_
z6YnCW7$LoMYw)&gy;z)8LA@s_M`kCiCc`&b<pItzKJ=;pt6iyxDyP?2=jw|^E`4*)
z1z+hz=ywirb%8ftGwMmY_@h+M9U2{YQF3-y@fg!5YdQ_9yz5nAhRbwmS2EE&Ne+jW
z-<{%IdNqYA1jxG5Y1SmEpx*1i|5#SyIb6SCZN5>L%<}!4WuQw{N?Y&}^FrS}D<*c3
zH2Sl~&m-h=#WcXp9>3F`%ZC$zHd(zD`@n;Eb8uRf*CYI-O_5&0{TE?V_4ohY`wGXW
zTIA>5G@Z{@^Qv<px6LIHJ6V&G<_E=u<u-#*S0R^4G5d*PrpdyYnZ#@(O<kv-(9KyB
z051VyR>8MuEF{EupCru2gn${0R^_}nmK6~shG0ipg1FiAiJ7qzEy3jM!c6D_rvsdv
z8YL1PU*7oRmKu$WYkbk7nZygJDHHxgUOVC)cvdF=nu>pv^G3T&=^^Fhm=b>SsPK?7
za%3^^xJ;q8CBoptnQX)yQ<vAsX5gPztO&HGObQr`xsOSNu<;6m3H3w@>+>f_C<b?1
z#hG*@sA$JsG0zZ6I-oo}ExRSE^Rx;)w^Y}j(3MGsSptME^sWW1>$<k2_Q*fa-E{Te
zclwj|goC;-K0sDM_~K4H+5V`UViH#=#bCl-q^a;%yx$X|`THk~i2Hfu8~E4y{-?@?
z^B=Grl9`Tmg<nzTN|K2KAAVhrC|-<4yn6{D@1gPpI^H03Z_Do1j#6%vL+$?(b3*m#
ziPa#!Y#)cS)ISrdo0Q2>!Lk}u05uaSB|u%Ajg@v-NJ`_4=GQAKy3rYKFUE0%Vi1G2
zY3O6<#AQP~J&r~wo<(G#{6E+v^?5pPxtW98Rjfn_HNO_MVmTcj&1lY*ocI(31lwbw
zBry0=FCA}4K1De$9;@+Q4wIW(jh{Tu11A7^$2Sxoj!TFbwiiXEWPG)KeU8a<0;w4t
zmkLo}J~CeqGtIcLO83SR%El?3^C%-Z>Gt_d@q0Id6cajmDjk4Mf?f&#VJK(~%OCiv
zBs{A|@*tKN736Gvj(p}i(Bc|F(G^ap7T5Kz{-Kxbs(T&Et{~d);W9b0+s9tIqtR?B
z__@g&Sq3zPKWDcqNrdw9;a;4wxx3EcQsK;Eyrrk6wSsvka(vkSLih;1cSApI)e_O3
z8@gS1uoAkBH%xupY7xwgE0=9pv8)Jl`1Q=W)59>dB2tev->q@BFx`G5HAhUer;Xmw
zC8XN;o;YF(l8#9_g=AuqNg>&oWC4hIc`d#jgCz$;ieSk(bS1nq5}_|lxT69c_=xNR
zgK9-%K~d$RSwI6o{%^JHe+PjL^=FjhBgiuWNF^-AG6jYFekbGO(RBY%zi5&<ewnj;
z`Z-PPD4L?o5>ZMU+A4P@fJXgVL&RO3f7KWX^?Rwejw~HXc0zt`_Yt%Ep~gBa_UCYX
zD=^$}c{s3Hi#U1(q49Ivy;!pKg+j&;%8{h$udsu4wrH37^DV0t@2DR%R2@S0TjVmz
z-E2U-iUVU9TtB^xoo4#y-8Ei~i&0~mjsIe_*>UfEb${x9qq_e0(uE<M>Mws)_r-4~
z_2byg8+z`8jQ=p9q|YW(z+8`D**T-csClvds!vbUXzK5v-|Yx<Gihb*{h~04qn3nf
zUe{SGjKa=AvXV7k<T&}{5K+zqzLf{%mcuDx&|4HDi;tStp4ZIVQopb~_>k$&TNk0C
z_u@xvlND`su9t*1g4Xd+`WaPgQ=SLXjG-;8&XAaC=aX2%PGBR(-@|Ck8yY|S%x1dv
z0T~uG!R66DflYW|I?O26xvzRaZk<x!`5CJDVuIr8UFLLpj!q;Q&+;XOV<r33w$2~)
zoVr(>rk~e!dGFKj&*Y0NR_*B)3i9pe4(6C%UbQ4c0Ir6T+>=YoHVB$-R}k?~oMP7i
zDgVXFVltffSBWAm<)>XC2}@p~FS4t#0B}i67{|r}4fItVP9~<I^+d-HQ!+qg^?12w
z%4mb>R&hEiCc?2OX*gtq^~}Q$%-JbDzP-ho1o%4<#1;@heyD(data^R7#I9I1qg;%
zDNk*S+?~GW*Tope8Hx96n)6p)$yAE6SdFwnJ{q(?H~JD2Cr@kUY4D*|dfCv?ppDXJ
zo2<UtY}I5c%1<&ow<jZnb3^QRRNsTNapR1i<61H)E~?qY3ZJ;Cm@8Mz4t-l~s|-)O
z9=1H;4*KQ)?oqrF5z$Ya{$WMt<@%so+n=m+DxF1L7Ptl5fMA>AQ!y=K>mt?%dk1ul
z^FsSr^~a!k_k>uJY*w2hQ39U>w#QI^oDGDU$T#d+!NqeyH74_iu#&UZ@2J5hSu23k
zx2aRFEMXP~qBwDoH999?S0Cx;W82~D=HEjXSN?2(OX(DjQrpeuKo}~#!jlgK5K|i#
z&dUeAHRR#h%_mBImZ*BGh7>VD<}@_{da8784&-#w7VDhB;{nRklLy`-96YV<*&^q0
zxF2DUn<Jxg7*#_YM{;_<j@@Lx{+-f-l=abH7VUIH#)?D=46%t&_Eoc|Z}O4|pRUy9
z7aH;RpS|Se5VFk?8MYJi{1ingaAN5l{J=im+)AAAebJ~f6aQMu*YwWATHd)VXazwm
z!~b<*<b-HdXQ+yl06Um)7_U<TOGBtp<FIGzh$vDaR66XHCq6+kkPMJznT^ur6GE{M
zntyUV@gh-p<1S_f@Irb@8$G`C_SgAel@3&2Je=8?)-LTm`m&j}GbR%{yR+OQn3Jek
zI^gk@q<?cn=JAFfL+>{AF9+0}DzO$oiIlW|jG)EdK<^~<?bgEx9Vr`NuLlsxX1z9N
z0wAhEzhp&NsPgmPHfEKrlF0)@pDx6_%4qd~cCSVgLxupt=5<Y}Aj5clSIq2Qc#!b!
z1WCx&aKMiuK1z~D<0G){w<7YSu`z+JElP~E^{Dq)F$E_=Gsut1pH6vAf7QWXOM6?z
zTthxpfjQMSf0oiw|B<Qw-X3r=cy(Udc%1uXYw%qR8K_)Lh;Fn+%>EyKVMa;|R-M<U
zCBV!p{2?1j$`d5#fzgnWV&*j@ZaOc)Borg_$9S3Jr3NP>z7p9pPe3~Y*tN6cbg9R(
z&mErP#BNhzT{K*~aj_Eit~|o6zjqj6i)d$-$%X%o5;1UoQ8^<$ff-n=#-#WR3_L4=
z!T9jVRD>h}2LO50ISQ2PO?qk+f`*~m1XD;~%GJQQGcXkk*U3~fZ4hP?FM}b#%zu`j
zPvV@O&!Ak2M^vp`Fo(q0XpiuoyhW#AT3olG4G=EQr#`x^Y%J^zsOd=Fai~IWGXq+d
z6<Z|M`F#{FlHnTFSexd@(t8`n`LCQ?rG0rEZT-^oy>-K%SEJ1$$UITAp}%SMwhP&-
zbY&^}qe&DQrYIhDueq;@JJcW*pT7_OyA@&?p;55Ae)A{N$?5wX$<s9h!+XY-5i7SJ
z_kGRG<t-o0iL9;EaG6^n=Saclj^{|L3Cv8?NiG*&B#c7SsX?+}$utmN%qtpFZCI@x
zqyUo~1u5#(t*0q#QH_UK@*b7E{a>Gzb=fM4174r%)j_@jl>1F6K?J-jSr7+Lt`Rzh
zPiFc_Gbm@2%O92ox`q+zq!{IWBzo(HB$P`xvcLVSGS_}}SKES31z4>x_#H7G6nzZ%
zS<WiGxAvzS4&U8ZT-o0KX5(9^A7Dgbod)aq9U*U&;o6)E*Vw99e-^UOpLAc}2W0J2
z_zDL^{QY}U+t{oIxq%x8DE|C;ZZr-kVTc-g)LJx4X*H!uBPg_k^W(p&<tW}|nh5HM
z2WjP0KKu6fDL3DZX0YR%ySjIMs(wFy@%4=Dzq`t}*)JCr^q`<0DjGT8)1wSSMn4PE
z6dwJ1A4+b+@3d8g0SX~p1P*ij`PE@l&gBFrhH=y~d|ZgK+=_5m?O}@A^6#=Tt~d+5
zqBl?z9bF4s64Odu9RcL|5a;hAk&M4@O2ht8VWzVEM;Vx3YX_0#eRvk8K_A7$_+%Si
z4{;-#d@%M}qltlx%wAAxH#32PiHim6A1K_%U$d@f)i#+r{>Cr2XN|S6*U!uc8Kj;6
zv(SZH*=*s)U#{xdmz(XG1=$e~!+OR!qeL0nC~K`zc9Qv^uZvUgzV}d4P7Pft6+Fd`
zuj!#F-@!AYyl<*Q(1I&Q%l^9x*-sR*vM`^Ma#Qk8QM6N|b5q4$VD|&+qAWGVnMo4D
zhhXaGP}PfjCFK2{DxeBzR)JAwv>q&73?+3mV*`OLh>QGfQj7)ek@S4vJ;k9YC0Y!3
z#Yy~}2qND}&v~=@1V-@^3AGe0aFjM=`JbZhPv!qRx<At>l7bbN2x*u?MDkY<Mb38*
zb9?Rodg=PbX0|sho%Q%PCMPcz%{~&XY?$VPnM;3KD^GfcAZv6ywcTKi@Ghzn<_f#i
zsm%>LUgMxX0%$k{jEOqb)LXv*K=LR#Ee)~r^b9~-gjW1falJ_P!RqS+W0)te;qIt|
zHq!g)s&+m2?%9zReFDKC1pc=krO=pJ{AcCImHoo3cYSAd&)|=x`+L#1F}hmVAq8R(
zChY#RN*Y2lbwZc3vFsL}E}XSSo2t{OilLwp<*?Mk!@XO0Lh^?ZzGGX@&uUd2*TalV
zKV50Ly+Xt0w^4*l!K&!oBtl7WT$3&B-B;zw_Fs;avw7|z?!Re?Uo1fNcdcvGYFP6g
z!3{O9s(9VDL!5%@jNTo8w0ZqEBfW#*5`fHFO}_c`^4sc-n0bJ309#r<_=}p3{zQ;l
zT5Kj=V~B&oP-tQ0mAv&Yq$37I;ucPn{r#VgL&D!6F}zsdk<qz@7JY<a8Vh!{g0m>+
zgs66oT7GPauo-W<uPEu3sdrk()3xb<|4j+WiO{O5CPgp?QnEy>^ZYtqaRE)iQ#4K7
z7-Yays%~PczI2I%x>!dlPuM0SiKwcE7*1FCOsa$&B&JdN4>J1_U=L+<r$G}%n%wVT
zL=v8;2F~B7V`L{>QjJ*HYbK=#CV5b9@?e=Uo$!Z8n;k4O+Jg5T_=y#!;DT_Gf);oH
zWD6d+fuaYE#B3DiXdL4Qoxd{mq|s6_#eF;~CWnD;{L1cQn(gyf(S<0Ry<!|!_*TU`
zbxGU%ZoHSWYwrW?`omqwe0x8IC^QyyF}Dul_3DqL?<Ny&-ucn4KYVi87=t%H3`B^K
z4DY#cN)_=Gtf_th6cu9c%%Km^k%D5{N}=b!P>?<oE6Py$-w6~y>7RU--;Al@J4HPe
zu1{eG$;@6uuvjNBG;RoIz)Fm5NkhzGYoz!<-AUZ6<d|0QJz#`dh?!Eu(@mL1<HEoU
ztDypRWgcC~Qy|g0SoUGpygcxu*QCA;;uw`6_hKCL<ou6IH%B9Y={1cV6EL>eP{pJj
zER@VpE-_GtoyzDV{Q!72j2{``VAN8vlzFQtL!4j@z*W=F0!Yx-faK=<%}DNvT}3Cv
z!!RYJ#4rp>7B#OjOiF{PP`HT8FfM`6y_&27pZDk><%iXzt{_(;<e!xijC18l<eKRX
ztu{o>)tJ7Cq`sQ8A2>hroLUx9SAeXwHHM?sVT37tHuQh<MK{uTj2UfC$>A|}R`tZ*
zoCN|we!Lm<gr;Kt^kbF)!|45h?$yVx9xX=nx#70QNbm<x1NYGfBcC^Y8;+Fi9Xp*?
z_U5kER_#7!{Xeyc!8;+_hr#j@AKk57bX@5ZyKnM|u$>U359))SJrNhoFI);OK{f&#
z=`(QqTsW6EC6(=w3^zb!KkQ>F<cu@(T;#1CaB!YIC4iJ;Qbcm*2IQcCU7cP<LsJc(
zxqJ(f<&Md+!a7H{L$?AstZUtu!(nihsqw#Xw;2wR(a-Ys#L5Z=1;Wx2jYL#$KF<6x
zY7JziHQDs-cfFJQe(Q`lvpkVk6%{z^_p#^0*1+hXf9iz9uv}BL&G8ByAJz2d$E>3r
zAMVLRE^GLL(Po@7(pi8&4QJ0>aoX7_im~Ast2?OU`V;V}dol*Vvfhqymy11xm+1WS
z^*IvB3aE!o>{_%6;~vbkf6?B3IKqI;L6clXzesz0^PA@B;FoE~xaY7Z!FLZiV;nP!
znWiq9tN*-Lf2V&E`F&9PGvMf=r_KLZtSeEc*!&qGV4{O+v)Ft^>k@?GuT7I=aB`z9
ziHZ{tR_=oVm<)+{lT%(AwP4a$N$@OKiXL;V69B;>V6ggCN_ovM?HHN?i|I|u=Ed9k
zvf54O`V-qOts#JZq!u=7>&z9F(hVmA*$a{Qjd?OMI0etz2_afU+-DhWENpD+?}f^1
zzUbM3?;*20Rz<2gfx-|XRgs~o+McgeuB0J}+3^WDOWbR=4^q-enr^Z`#}D9WqQGiw
zutxlgF?QSMS##O#Wn3BRlp}dT1{A~N6@imXGqs#TRSInr2shek2>=`e&`8h<qvQxl
zQh157OxaXzFUdVA05qXToV#X4K!kgIFEpn_l_m5Ya3{GYOWPaZV0KE0<7uE{=-7L}
z^;Amy-ng{%8299>raVeBlQKrAW5!Hya(Wk!9^CV2u3k*Gac@|rR$F=H5*(np&^#wY
zh_taAO+D^Y(5u~d<`a5#kB&%3i)3}9s8~wussk`ik@wB?A}^SbUCpTRt+zMbsG^oO
zSeysE*`FOKieKd8al^=+UUGWvAuQ>g*sn88!f-#tw>(Dt8=`m^^)={|GOZwZ@qk42
z-CxG#E~0-<;<B~PD>o{3udN(cES?gDHx7mf0^VmQk8kw4EKiqg_9XvA(<gLVj(Y#+
zx)S<ir7M+5hs44=EJ{_3Xs`swY5@=?$(yK=ErfIuHTY$lSgGN;>I#r@?<ixF3eu4t
zn;9HDC9aZM1F9nZ3WrcKbLL=GN0Cq_aibpuF~F?XyIqr{9y;U&E9>GE*lMC(Z0Z*I
znr=$@T@Jfa%}b7GQ$yFwq`ZTDT3woa(wQr+y~;g#rA2!_`hUE2?w0Z29B2CQDE+#+
z+*)9FHyK173r4@f|7CYRg2VMIPb)giKD6+&3^(6~Nt-StjQrYpHn+m&+!hnJSM}Gd
zX-Wjonx^=_EFl?Toa$Z{1}w%%NT}+3(IZ7i0ZrC&dd7s%lLG!%deju@iU}crGgNJ4
z(+UXEaQ98T9cZ{IoKqoF(ZG-P0QqCeDKO`(O87kDLYp46pkg!6K}SkQeWr<edralM
zL(mC+wC0q5ap&78&}NItpL|?r?H1<%>pGdI5u`)iZ-%Z5pIVd0?R-)p`%dIYKI+s$
zn-#+_B3kSy`e=Z|s@<mb2w<4jjCz(ihvoQnK38BJI_>Su<iNnnQ`d7Md`-dj7WZY=
zp^LTu_ULc&z__f;K$5XAxv!E;mbQgsG#I?seO2^O%drzstYNRe5Bk*c`A55P%AKg#
zz4D`i$m)1dri#BuR%Y#^E_w}A#9n3z@s$Byp#JR{i09Ke(JT^18$rMm=n{4ZmgUBc
zQ6m$gM5(ouae-kib<`99G?<K<=fdqCb-O88nQT<tg_?G><-lcXG~3sUZh|7thh;2q
z`ytWTg2rQ3o$PH}E}bBYIN<I##pkROpA8-zwzc_&vEgV~Bl$5o?FIR;EQ{e(7={g3
z*yqVr2JqEzZ{m4iG5e^#p*wJ*xg-R$A||Qq^F0&VkT|^bm-_-m01-?>ftDd4Ellht
zzW$V79RrXMJ2G8Z>><0%T>X{JhpI5vf@QsQ`^vX1=26U2v9ygAZ(i%`OPvd44DVPV
z4TrawrM?rI4a<ZbK8mwBf0O9g)`=|W9q=X$$0#Q8)M4v9d2&6F`q#UC-7D_8w(IHf
zWjo8(^wVM81v}`6udSx619mJ4$Ygfs;=fuphe;x^%`1n#TXvTHQr%90?S>H!N$w|4
z-WA?|fVEx`v`@m1^(e72Mi1oIU^crNYvs#qw|hp+5=t4cr0d_=9wcysJOhZDU12qU
zBqfGX8Z=PG<Fya5LJ$i|UBeRPV*_iaKz$f?d4bA01nFDImoo|oFfUnT)Q>uoRu5_2
zDkLCD>0G;6dAc9TvANc{Wh8<#@pR?NbNtSska6}|Qq`f$gov`zhUwqYr=2{G*?8$k
zj(1wbVvu{M4nohDajTSq8K_=~@)1Lev~>3=M;$JBoMgrxa4wm-W`%L-TpB{L1t>`A
z-xWzz!JpJ|mS9-k<%>KAL#%rK{i3>kkv}3$8zHW*ly>)UbUU%08z&s+3}2@P`+S}>
zW7*-3%o(lvay$8MlB3hnaYZQRW#$Jw^*ZwngZ}%0&L2}k9X$t&uE$Sn{{EL+Lps2L
zI)Nn@BWd$i)oIHpP`a&>SZ%$R%aYW_f%lb}nPiAK3F1*hz<3RcbQ&N?hGjk00SILQ
zx^gg{8OBXp^x$>5-5=Bie=2YM-Uy@GEnW*fD|X{@Xc(h-ziD<{ED_cVw{1xxF->dw
z*<e%{!fqD`a&KWI>MBF6rTUAv3VXhAuP8Z7T2KwssM&%V)P3VKW#P~?H|u|;pPP|&
z%2k8-d)JMi$<L>@r`U5~<s*CwpzJxPt!g@dC0?D@ax)5zdrw7AuI8tg{d6){U~4vr
zJXovFG;7EnQnoQFW>N>`J5kfdxgp|7e$zowS^)5CDi%~(EV(4`Zl=b`u#u^-BNkXp
z;xd56EB^nb6Za@UhO3JuYA_&QdhDYW*lk$b&%A5lB>y=JTR$niZ?hDbWcJobUK`p#
zvXJe6N`5Pw@U6~%E7M1$6(`F-x|)7al`mNVmF)uiR)l0oRPTJ^(N$H*fA%EQh;2~K
z1R!EN=#$BKa#!91Zj@1<0&A+~m0vH~gb1Y}D3?(NF3>N~<UE!tz-!>nuqrWU3H{Xl
z{Q`oyr?G@8FXzNBl=8-XIpT<F-sPWj%aod{5E|>`^OGw}VSYe`t)fITs+KKk{(Dch
zs2gVbeTeV;uMdUSlYF%{<;tXgKDL5URDJbtLu(4_xy~Cqq+2zIsR^tt$lbZ$5@Rh6
zoiUBH?$=Rl*w8a#i_mYdmSmX)FI!TfUc!pSZMrNITMEur!bGw;B>}H@jx_`qju+p!
z<mEPNxmx9f7gfi_iZ<xd=C7r#Dx$|GwYm4lVmisPUF8UNn3|*<0Y+Hi)+>SU=QQ~9
zkv`=u76pK7rzP=>K(QH;&h~2mXJ+zBOjEDoXocpRpf=uk`71%Lj}-lOj1SayELS48
zVm?{Xm3z!9_&a^ITm``3STdFWi$M?(V4cNOECfiQs1c{w9z_}kAax8rZ;}>F#)20*
zlpiA=U()C4IUN<{9R4Q&$h>I31Hd7Nh1iv)LXE=-Isdu)=7my+YjTf_9xN3hMFfoi
zrB6r%kLoYI+BxqVyA=sCi<&i;=BR5LEPo?*C@1Mn6XXhiwm2W{D=$FOrRpE_NTcEt
zf9jr7chW9{c=*VSxJmTAC+!{$szUiVo@%Vqv_nvUAYI*mk^TFZbXyq@1%t*<zOe{<
za4FOGeP{}T5z*3mqtJWJBuZTZ+jo4$9;J7FC+T|YiYcBc&;SJ<&P72Zp6Wf8^EVL@
zQb@H5UOhLuFImVl=I|Ru;{4bF1G<!pxfS0d)BnEz@6Wc1-TUp4i<H3=^K+Dnm_P_9
z7Ai$@=e3xbC4eO{xI?1aJj0NJg@z(Yhse_4d1^s-q)^!TN|6N((Q9p5%>1?jBUY@#
zm3C~Q>@KC&f^Xo{32GrtCh;Hx|5fdDw|Gu%Q`g5lIEdhHK=-WdMQRVC?mMa)$K3|#
zVZhxY(`{S6S<<nt2_I+nz%zJt9|2K<J|u;d2>(m)$zzhCzz!@K4xz)8yO6?pX!{_P
zuy)^W;m)&MRbFvJL)Zvxy5y5^twh`P>LN~Hu|u*((onbS{63GFRG6Bjc@^DXyPh{E
z@%IhkouwKbYvi9@ep}Ih_MyjI{Dy#<mp7e_DmvUbwH8B%KMJ>oyWexNpwEBiWtA7&
zDZ3SZ=HqXF3_sekr58>Q;Uyn2nQQYhd$O{Y#1rArSupx0Wa|5kU`S85LfW>y5Mj!y
zoGcmLGgxBEM^x8wzgOa`b9HcXD+vJ%1=jhdmDAm6b&uyoNK_Q+{vH87SMCW<K_QNL
zBme;l_=~dVX=fs3JH7p-k6>$l8o!pq+9<4Kt)^Z29Dch0-msLa`m4PkLCKm7>oF6o
z(2X$PY7~-52r;Q*8Gq{9YG-br;356|gT0w0E5pgA)Z-to4H+3qYvpLLR~E7?B9*Bp
zes8bpUyC?_!s{qsQuM%@U0<AGGNwp!F<<rtzQQtz;g^b;lJmel9X_@i>wpxJC7<pj
zkh9jJnkirF<G81Es@au6<P&ciLN+GvIRw#0Jnj(R{(0Zm4ITFU>$APET_Lo0#PhOD
zX012pKBvF$-|Djed_hWe^ygn(K5s>vsMog`iHAe)b<AR3@rmDkaz0|}DKzruDZ}-I
z+^lf-ZG{sEY`6z(^0a$Chj1|hq)dPc2BE4_OK6O5N1&E~D(EyGR>vtVLYOz5nsVsx
zR$DG`V+tvW4s#;&ToAF*^L}pb8YW<vK$Fis{ed;eH|+^iQOyI@54XyuEq}fHs*a;e
z3BFk`fG+W{ABYXJfK93gowDpBmpt!uJ8GA^8td-Vx_aN^yzd99C*1>_-5~K9gyJ-4
z-!;x$hljIu-0DO<1N*u!#7&+sP(SLXFJv=yyf{F$6-CpRH#pdf5K%unCfY8k2+T2|
zPAc-K(1A=n0zlY|u}H;<1ib*a*ZBpsIYIn3x^mv5Y&HD6p{G^Rjy@B!FG{SQ3FF7L
z&Z$QXvXjt753p61-*#RPL&mZLL8h-(XOrHlZ_Hs{qIfpaUe@)oc%^B#Pp-c`gX}=t
zLENB}WD<Jt$MNn8*JCee-Tl}6>+24JvT}2XvYvf%g%nje)`NpU+UyPoE!!YgV*JUI
z`uJ38Qgz5)&CNSxXiPb}L3<RtVX9q`{d^G_LB)_>DYN+XB|Ar?u^;WiTGRJhA;T!o
zx~v6xP0jcB?p6jc6y8hmyx|`c<`XR`G?y9peZnIe(^30ziTEYeU7w4T1w#><xPJ4y
zeY<r?DHP9=Z^0x8RyMmY#p+3_>cmEDw2}eNvf)sLr96hUSSn|kXsN1nlswB4kP!0%
z!OA7NOE1j^)M~Wv)t?Kc-W=L%@GFyZk(QMTMZvYz8KJsU{W1$2@X_EU4u)1dcttg8
zorc1ki;cg<ih~}f`G&c^GYVNCJ!;c=>9cxugKo7$>d@zu=tu=*3KK;N{>*1jJFiSG
z=wct9I#vC6VCikeyI*<|8B&p!=5PRxC64{SAs8X5-|@oJ2_V6@ByboIqb3Ya!GcUo
z92jIMAllp}>h=4qe0h5AX1#9DbMFAMADoD}XV)ZyXR8C4x97;e7r*^9Z%0Ry0z5JC
zGSRU>e$`Jf-^(lF(E(}KSssOz%;E#UdFqFzvd9#6#`aN6m*?}gHNG}Gp4ig(w<{5V
z#+R)1=j?~-ubg<0DG4+rDz(c(G*Q=#i~{wx?k)P;5+5nLor2`*G}Q)CxP!w!*3q`v
z;*P_nwIT$H6cTe>`8iffM$jX3L<Z{6bd>p|c%>B}E}Q!g9I^L&NhWS~P%_Y9%ZB#>
zNvouPIIn+kX~{#w1cP2$rMryMK#akAWN+yY!psc-aJ*Z|{1-KjRr;^l_uY3u`RkwU
zeC4n0xBrH+<XH(YE#%aIstNiR$*_(a7|R27<G-WsrN}lpV$va&KPpZxM>^V)rc63|
z>0pP|4AO%<9W!N9U>!@RbITjg20OEA==gjh#8FErVs!#?tw5N@I#xb_#)Sj{qKGLK
z)C=zC7Xl!sc<T*$!{{yy<AzJuPmTq)pVc7vWQ-s%1PTqoXT{=<5qhf_d@d{tgAa47
zASLR>avLTpV}DmQ<1INz-y9O{MJY|8gtz2>-Wl>pRWTdr&|QEuO>P9kjN3w|*<pft
z_L^2zmT38_H&UBM4MSli=9U)A`C3@1>po_FB)|J3z?$DZ+0;hrdEAE0^<lcT?FzC~
zZ92b>SrV2f1`4oV@~c~JJ6%yp*fNAO`w7apJoGa!NVWFara5djm8d7hZ=>^h6dvyS
zG>)S|A!gRWYq%2Lw9eFVrtKMuDGD_MS^|9wS0)@#h{=Qk4zG3Kk5KHHT$j0uU#FPf
z_XvF>t#`*$M2qgF9lGb+{MbK?Z6-tF^=ypO)^U!HXf1<2Sk&uvxa+D@gm{xqCo~;3
zu)g;O7_CB4iYxLHRnC2L#`f;=W93c{y)2tCq9rV(bked*o?FOA1!t1y2+7#GLrSi+
z;$DQ;%8-H%R|QJ$m}>yVPT5TgC?Mees6bE7Ic!ifE9DDJrX7i2$jTyx!>+DHZn{P(
z8=7=YJnb&|#6?vu*948^yVdcQ$_VK9V7|vI-xoAe*O&fLrEmY_&~V%wR&vaJKhgT`
zDedp9fW(u^e<`-?e}c6=^L|PgquO<)xiM%xNLj6j{bC}1$Z2&YDE0)8{+im#B~|RO
zpPgeWr~UQ}Ry%{@GlM+G!6RYG=O~tAuoejuV3I1{qN;7uv2v#A<a^mH`i&*TGS$*f
zud!1##BFuffC1f7LfsS=0m-cUXQfeYD@s6m?Cf>yebGjxx^AIbVArfed7U>(-$<w<
z1d9*M`uP%)H0~nB|2R45w(;<^XN}Ix%OjYrDKECa?2l!_L|MaV)pgI>09(44zqI=L
zoSwCiGCGdEMa-Ocfz%a$Lvx(p@@?MV34R||k(aAJN$!j07YlX*Wyccf9pSvRy?y(1
z@`*gnOK(MFPfv;^HpKK^vHANgu)ml>;&z#IZ!GllyQG=z*;#!ZIK_tMq`n8bJU6$9
zpI>MfLYN9ptDSnC235Dlv)Qqn2Na$`D7wDykK32AOSH8P07r|<+#AEZe&L2aG)#vo
z;N4&tXe5@H24J%RLb%2Ja>e@$#xG@gL!M1%ZLG;;u%(BJpzM&ukDLlP5ux8$p7vM5
zzi>EHlv6g+j{d;2KS;pk{(W(8c(QJGACs3~+pgb3A79QJLSG^J(iinKu4*+|qoh#j
zrq=yBI<A}1*Wv1mYeO|Uf`_kp8?URX2I&@ORbPL_iYQxj6~1qO^pwA5dyw%5n2zOf
zod%RF%eEARLvZ7-6j9Gc_cdW@w1qPS(&L^|>g@c&c2@$HR$(70&opJL+fvM|ne;VW
z97G!VhM8a5{drIriAtmvL+i0Djys#nHTkCuYoJ)^7gL?A=a2p$S%3W&h5LQ+!UGI2
zLk<m6Gcdr=-QC??f`lL?E$JJE?oL6vyFrkSp&J1~8cC&9M1AISzSnb|AI?9q?tQPd
z_iGVArs!&!;_s>fm>j9*42qPuzmhyZwkp;E8lKf&HbUMDTdXhAU1T{laWfFjWJPhl
z(8maq0Suz)4rnJz;e}o|l$4MdiTNT`FNB-|0#M!!S3~gNRfH%V8i>QQwEzO)d)jZF
zNG`bnn!719k+2_p!85D|riW|zqDVae*(SmM3x!C105N+MhYzqyw>b0XyZs8X7==10
zNZJM8vN4R)AN=R}h(nsmVK%j6=@@?VXU>}-?dMSn6$a3*eD}@Y%25U-xttYqwD@mU
zFBhiAY`|f|hYqG34~%U14XHcg?6h{c;D*uSJdHytB>=7p>oT6Bjvrs(IV*LpVZQBo
zt|%)JO+@f(KA+Xkw>aM(Y>#p!x+&f$4Zi{gxv>%#hLF|h4ahUDzPYr*WyWxV|LHb;
z)4_KGkd}FETH16P87u>_DS8jShz|PCI6(FLw;wX|ZkYdIyw!jwnUoL=%ppBIbu78*
za%Nz;9vp@}m~uh+Gy-u$7ql)E1O6Lvw&ICWIw@d-6sN&C{wnE`O;9V7lzu@<aX+J+
z4<p$hVPKX6K#&t@htWvYI0G<q7@>qzZ2M9?m<0f^!{<;FD9?jVp`fft<T5568pN=P
zM*=Nj!Xqgz-bW-Cw>nfuswdF$r8THa-CU@sDP6QT&u~2~z&u<rCsdZtS|rO?=y4?$
zW8X2-E@?gnD(R+>W>!cD&9XjltW2HVEUnU0XmD3^_D?NjNpmW$X53J*!YHiu)k=s$
zZ8X<F1rVgkBvY3-wDls&d`?6IY_EI&F<5;Cp)IOuA(9VvUYRcK(phb<c=viiSd%M#
zadm7&tzpgG(RCup=&_-YqgN-IOEjPR4NMiOXlfJ3WRUftCZXUW46oEb-N5xDR|!YL
zs=!(pVKJb@f}^6oWH)ht-d96SD=%p)5KB`IKFjhcZDT`GU49R*=d+ewfVD>J@|2A2
z(D=H)zL&n&lG6C62J1)EA&R5sJ&X`qJ$vGIZ?@c?Q;WCqbwlAz?2CqU&>In#UN@vD
zh}jn@8p|BoEbeR=jie*oY<$UaczTD#KD+eu5aH>>JU;*#L}8ikVk4s_B<O?<<h(fl
zjgI#z$&Or~@M4kQK$wq+t_u>)u|~F<BuSLLaFynKrFkic$N0KrXYTp#45@sq>FhUd
zLn0aK9_R-MiM(12nudQ;jRrF@lVWG>O)^Mj989jC>nu-Y3$<|myeZ}6)l!2GBeB}J
z){v11gsjNy3B1UD%3pcH`!J5Xr<~W#=w9lL%Qjnqv-HlM*Se`tcF)=ek+)~q>Vu*;
z)kSfvVf9108Q8V6TN#4kpvi0m70y2pXNX{;)z2YpbL<^OWEdn$S<bNnH&!z|lHl5o
zt1G#XK|;%HWayV%HGJpA2vzvS>R7FxTQ{Rccr3wv=0K;xZziVC(RBgH<cduP^QAkO
z>w1(h6M3oyJJ3e=3_aN%%VXbSxN`q2qrvfgMYCcL<7NP&>({@SCt?UM+|UUzkD~I#
zOG33uUS?CT*K0=H#@FIVNg+7Y681kjlZ;EWsd>*9OR+f}<vq%*!PO~;(j(_Ll!gV=
z8W?XSp$*(4gsAsZOeJ6!LhayG^y#33Jw#lIRE(S%DbIq&U)y4|hGC3rJv;0cQyS{*
zRQa-HnG5hJpULGO_8-$JSoB&Dic)CB+p}2?^C23W?rB>)+m_!dAnE|jZj6XlZV(jA
z1%8<u);I#wmiRA^EtllTOIVkG^%wjQ*pm+V<}`EYZ`eU587FaRy>(R_yYITNR!8ux
zRwbI3<X>OTvT_OjeBg>3k|oaqK)SqwE*}H1{AhoW!$N=2bCImWpr2~Y2hg~lTWPb0
z;=QLV%Stc#8gXKsBfR*G)J<+ZAAH?%a-|f$BtHdlQCgEp=5TN2Cv}ec@}vA-b{(9W
zc8UCE7D8FaX6-p+HExE}NxV$Zq=zvITdbC8+hkGz0!LY78R5;uD!nICS>rJ%*-DA2
z<!B!rDIwjb*9>jCSE-?Z;V|JHphE?TFtn7TuU^mw_v@cpaC|Uu*s2*NMvC_oFJHP4
zUQV(5GaBXaKPgvX#u{Q+RP8gM0_B9UG^;0$6B6gD*tUrooq?qyRG=w{3J~BnN12gC
z)tVKo8L3~r<gDa|VG@DK1hMF_U<Ni0IZ*`C+tC}f$#eO+TEDL|Dqb$WbMl+2kjc}3
zO-9=nUj-hRY~Z&zGAQxv`NFIpV#J0EZ%}Iv2;h&pjZN=@dIr<KReawPQEwq`WDqCY
z^J<W&Zfp(oHQo)Hger?h9K^d8|Au1y_d0;Nzc^ZwjzV%6Ku|?YUBLvptI%Xhd0)8q
zJ1Ej`4<JsB&ma4rU+H^lwshj-OMuiDXVEg+qw6oy3n>m%=T}cbrSFSuCT<=E=9iTI
z%IFaQ9_1!!zhl6gEogCJycdvv{R;$CV5uuE#MAKA3UvhI)g3Xxi02d2kI_kTh3{i=
zv{<C@#+$(<=*2pqWqc+UgI-mC3ZIq*0S`e6E^**9tHmP7ktirIyF)$Q01vmg7)eS4
zEk45|hL+HEA|p-^0?2S@E<e^fF3v7tI2SELJB%lSU$o)cMWasnc1E94`L+x+$-nHd
zEyC}tbeKAJ;&wM6Nuc!nZqXKt=x))-9Wl-!__@D%)~Eh&dQYYuSWTH+8yGoD8c#wG
zojc&yEY1?PyM`~H;hU=ZeIR2nmIKg;m|hn~wnAeZHTrmqvD3?pvhs_xb$nTLkZ6oi
zoa;j880L@;`JEqnO}I_i3C`ZZFKJ;A6=Mm*VCMCGP8ClCQe(Gx<-kkTk=es~qCFeD
z6&D{?_`xJtKVnHI86$F86mIURHMJexWa0I3S+{u3TcLA#{Xkqqtox(G`@J5PA5j|!
zfk3LJOm|LwezFH>q-aj&W!h&^yqXk_nNGLJ0{-@)HwO|FD*z!2NvTy~^?kcl;aK^Z
zco%2bLOdO)l=c5n@d(kRhweU_lakIp`p02*z3P(+qLHfPO3DaUzTrI@r&9tdOr@p*
z8ln}?9{_*}Nu(;t2<`E|A$h(c@c8Q%l}l`|-cKyf%2M-eMBaTn!gHf~P4pwhEjF@?
zcUD3=Y4V{WH{TCEktBWGv0CnQ8fq+oFgxjeFzfitogW-PaF;)?z6*SJSpU|lieu_#
zrXQ(^f_Ede5KwLb(|Y<k_s>T6mcF{vG1CcOZ^y3F8*v-0J&uJ}V+svdX!#4Q&XZp}
zg~oE{+nEWbyd-RnkiAj+M?FFne1?%L=kxG;=ltFnqHUa-<!MF7t`fSSf+3ojZ@c88
z{pL0Mmj?frrHqzrQJO6OGVKwwB@%1lwirE-0G=`QH!?&yo3z2%?j02!2dwTdOh#d=
zV{;1t-6JfO_*tNyQ&Qpa`ViSvnnXfFVCaZF8QzQx*rMhnNK_gfNdfBP@7fi`Ot#Pq
zqCAsmrcYE-HWFmTzS7Z<B4X35Ra#e3<9QZIDXCs+t#_X(=$CiHNn{jB)lP}Z>RF-n
zKLKF>L;#kSwjd!e662SU{$URert@M&zVq<1_GExdF{W{cH)u@_uI^fl(UfZFD;8Vc
z4a1_Tr3yzWv-q})H`K@TR_+ggIQ_iK)B4mV4EFR!-uj@Doh5KF&*|~gWO+E8OPqLx
z7bD*L>u_agi<+EOac%hCM!V`vc1Tt!ohy-bEa}`ailQVNVNe_=0Qg?IP-EjYi330~
zKohih!J-EG!#r#ndtLns7|0h*+P4qM0x_9D!`zT*pX%l1C6@uj4gd_c?(e&XRFT(X
zE=u<NglcZ0MKvvH-KO_=RGF!ke6x=#RfAR}K{1tAVi!G8IY8(Im)V8*Kcl#3PwcjU
zs?4WBqgUF79PF&7colJVoBnF2tvpeD6f;yHS!@_QuEDs%YIXf$hEiy^`XcM!GWMcY
zd3vG6S&N8iyeVCoR&q?eiE1jk=|XUD5KY$n2aHsZBSuM4(&9xbGLeXtBfuccVdx?#
z6A}F@{{NJl1A<oWh4n4=BjUJ^$0T4k14-`WnNqT9Uy^IWz!9g6pK$*O<>(3LGz+U_
zUPx6Khp?Ek5(#zfnr)U^<jWGW>M{0?VgT#pi%9eGj#Ky=GgtYRZTe@rZ%6pOpKC}a
z3-=Z2<BdEa@gLv*fcx8c3b?8;kVQ0X@CP_|ASc~~Q1FfJh&fyeoZ}Lhx*Dbpa5;=E
zKkwkf<8kx_G&$DQ-k#7;(QlD(9b=4eXO#MLKtwuiJMy*jE^eZTDe%3F2bt#iZ;FD8
zmyZjo-%V&g0|F$Qb#amG``;h!7@Sp;DK74R^vkHd?wvln`YXfmuW9<{ql|Oi4~&6%
zDZC81RP=uA03X&r-2N*A640PdAKew>l@vXeqGtg&PI=1bHj*&N3=7d1UOEJW$<<y4
zz@3P|pizlcdXBAvQq%w_pBuewpD@4#Gpd5%Z<w3Y@xV+aazPLf46q5IEx{m#KwxMH
zIdKUf9fDl~pyfi9l94r!kLkmgre+O=RxX<Les8@~yg+gUjokW(3UOvw-L<tE1~c@}
zEtkT?%hfK)drNrzD8K2n+iynqw3BKI>0VTDbuHXwNu<v%Zj#BYXtD@I>4Uj0C3VT4
z)D33BfZH_cAh@H<%B?cKdh=ZndGaF4wL<%o5FS{RW0~F|Zr$y0A>N!)Xf1Nwn7+al
z8cX_B?X&BXtaKga<=ZXZ(rIPwZK6TSrJnN}3jfl$eZ)trH>BSERSBhGC9qnH#NHac
z$Lt`*+anCKwVzUJowln9tnm%m!_!eP{u|ZUa?{e+VG_2OXHjgir=Q*~WFKu?FZ+is
zJF=q+P5S4%-?HTGX^qXL=X(V~D35nBRtuR(ZWUr#zy{C!caoGJkaTt5RFHHhu8AAn
zdAGj(9TK?!ucnfoJ0wx=AYcPPB!^KB&8*A)3hQP9zdX>rFwE-Z-8O_Mq_kaQf76Q;
z0(e}Lvf;f>^}oj-u>8G9_MP@0KJN8X3ho44&p{mGZKd*+*vD!@9Od)W$3Ld{#HKh|
z-bboQsclVXxC*nj#n^!HCyg+u3)uK<nJmsE?bwB0#IOX&aOGKqk!-)(E`cT%AxZ#C
z5Jt3ocSd4<+)jwm5{x$gZm2V-pM_pZd!GL7A!bNK+a;yz;aiR$4$`=*=_rS{_hYEa
z@~1aTRg51}`RB*-xxAI8G7S99;PU?c#^uRti8v-4gXyyO9|4mDl<-vjACg_E!^R2U
z-#VCJHy$tSGM>_#S0`z4l`kako^M5K>R_ciGZVt{SM3rIR_!1!bU4IjdFb5VJaMIF
zT6!Fo#ueY#nS=wtykg~scjlyj>x-3_LU`#_6m(+h=I%jk7Kv!8R(^vRku9;~20+Ch
zuJ~zgl5p<WC{gNSl-Vgb(Mf@?<yaGYpIj@c84{qPx&hkd@<`VED=67oni9b&4dXhD
z6&!}nPvR>$HBchj1FBtgEVY>k|3?G`t-TX;5M^Vj7iL1YGDSRh!wz^erZNluZ0KU$
z&<zj;{llZ%fcPhuXo}iDeX-KO@Z`Rn@Al<T({mX2sn(y?`lNP1ACZ#Z_;CsKs&l7u
zbTaRoh=&f9!CMl+V)PI`E}ol<1i78QJJjFM5&&IR^$x12^V2Z>dc0<^91f`d#mL;1
ze9d4CV0tZvWWfYw#xgHK!@*~%O6~H1gY{tDP+>Iz)|w5M9R7@cT>6-KYzINcasm2p
zb(FMBncFO>a1(6JM^|!6EY`|vK>V|OlgA<gQiJ)^r&gD@*RVcOWMncS!X-dy-sish
z9*?^0$A7?8flpx+0`EjOb@6u0!J$|E_2U*|3N^+~G3wdRD$yqlJaYeYN6@CYEaO!y
z^EXOBlspq3s$7H-ggI2&B36w_PtMhE--;fowb|0c<gO8Yl(Gyl6D$iq`a0El<K|d1
zY=LX&3L+UcPYrUCulzBK&O&hx23Z2>MgT}ag&qKuB+s$>VOEFJDqY3ZcwIRR!>-9}
zH#<Qne~~_C_*Lo@@HkMnWR%CZTCp{dY*3ele2c!8#-v8QcHQh755B>~i<VBU%4f4?
zBbc_8=H&cISkYLvoj-ZvKT*ueV=U+y&HYClthl}KBYTQbwN!Cy;rAWMBJs`;z209s
zk^`yDA-dkl-I{xH{!;)^KyV|&Rm}Czg&%Xn|A=!2O1^JV3V`=-RYM{3aKldq4`I@8
zyH%SGZr_JO2ma$Ndi*7e2);wgrrZ$%<h~mLuLpU3pGC{PqP@mdNR9Xh8q%xP1`jW2
za1tq|)Kd{cn-lp-M{P0w;g0ZKkbo7w|6Upg=sg)y3|>pHKPyh#r_L%)!kk04;q{s`
zL0icqSHegQ@S$obcpm%YjRY8>Nl2F(FI){}KqA%9BXvY9;w>`L86hp8MOp_(hPl;I
zqsng+W+en(DP6_CPfOSMIqs&(@LrFkV0UOPqx_<MF@ftyk=f@K<Uer73Hm8{!9=17
z_Z<`ZGjK;*o4)Al&2F#+z!ujvY_I#LT)P^%m9`415F0M_`^YTK)R7KWMztavUg+Hh
zvFl4`PTgbjxEH3V>-clin3cA2yS~~qoGp!4tG;Y9)Eje%QeANFko76aR(hzCJ!=y;
z`a+>$?8|bxgvhA|Dc^iDe-*V*jKDL&?3=G=#edsG<yfS@m+8{WRKK2h*jZZllC^Pv
zMnuqkr!MPTPvh+D-}0tF9POx`P6h2O&R^~eW70fh2Z(ZCja~^mn!c`lOm2=|zTzaI
z=y+#?OX_y@eBrC_&}>h@Vc0Wo$T>7=6Z9pFh^TenHF=Z0T0E$?@jb3i0N|uFxfEOr
zph*Q>x_>*)^X((i_RE`4m%PMXO(W2MsmkvtPwbf%$q5>436ZBBygR;~xKd~RGk9Z_
zXiYiRB4ju-CiMw_-ut=>pUzk6PsxHx@RT;APb{1IWVzQ5b90oYQUcS-5?3L+y7Wa;
zVR$T89?W?uG7rH`-^y90d4lexK{qO{A#vN3Rz(6=3n{!Ef4w70-?qgb`2L*sgRX;d
zPRw|`WDVPhP4N9cRQ%Hnp1N_P?{?N*d56=_DX`$>%i6RYUJCDOJ-aG>e03LUhvKe`
z#|7qQx=eu)0td?U(eQb%YcWCUKiL{N;oOVAsweia)a=&6noHbCU+$`BpGJf_6ngQk
zvu>Lzo>*5B^vfCVfbB5o8D56fP?2(c%(<nHJ(`2_mfj5J!J43nba7*9Wu$rJ@VESK
zqh!PNb7ro4(X8e!Ehhfmcx_Wc=g%-k(gR6pz?u$T4sp%d27gVt-o$(DLnoGWe(D$4
zCwP^Jt6(?F)AwLIHDDLDovbO6mZxycQm>{e+LPx8wu>CE#|zs6K0jMFqlZgt&@s1q
z6TRrqOiF9RIVcw=s;`4a^czmTJe>F>7$5mMZI#sIA<FXn-RrZOxfwRVseWLTKj~j<
zarN|*%8&_fB4bi*^)f*#A(WD#IpNH!7Li4tg#5PWujUjOPT7E`xjz10Gu3cq>vu5)
zw}E|SM~&Y$X@%YpTy9IMh5+D6fB?fk`wOD~F(2p?1je-^r6GX3K>!R|e0`oOC#nbl
z;D?nn(M+7UtE^r!z(>}s>$+tncRnnXH6c^ws?iX2toxMINPvcg^F&SW9}C>Ef3<5b
zf1kQUBQqcE3{L*GXYMD2-{TZeo6Z^<YayZ_uT+fHVEn6Nf>lwoPoyd(__mHOs+aJ{
z2G#yR<Agckk-#v{|HrZhV+RlHHk%%zlQX0e%=4!&_Ya~tr2{#TlnN(p)VOVx6MD7c
zf03nrR{;_GvxLcGfh*vgGGC0;-a_u3OQ29uKbz+nImxL~xjdpxosiX(HhF>r;*4Ob
zc~-sRK;bg>oXsjO+@(W|MP&@wY<_0+)8Q>xBvG-_x7&J&M5ki3N$4c=%d5AQpZUzB
z;FTSSpwAU@5)Ta1&!g5kL<kbeY3z|$wk6LVTm@-?L-GsO1f;9_*z$7TdH^6Xu&A%(
zGXj1F`ZgwBGS}@>N_P<>@nUY>r<U$|%pE`nVtoXUCJDo~5z85GMq(s?UVUvBzm3nE
z*;)8bNAf#<__I{&Yn%7N=L5lE3<FlYpAVnpWtwerK9m2ccwP24Mn;e9?y3K^0zMX#
zCl>|QM_vcWKLt<$b*dbs9T%^Hua62m@;!uvSTT)2C}T7g9aN5ZW-0@QF31HzW7m2Z
zlu9jAtQgF0cAy*pDdtKu7oo<&j2eQ|hOxL6O|n(LNz6y0mLjt_+fiK6NmIo^cwi{n
zttNt&;NdYqi|<Bwi;FE#aTD{J1J76pe5fQ;QqDvMS;!7>rQx>;_>%nWQkP<LGXh1j
zteKds12SC3Oj(PumPhIpMzuxa<tv&-!}=@Bv(zQNq@paPE{O+5aPpg=A^k~gr7UBO
zd##D$Ohp*OKo-+x5|fdixR#0Wy_cBD>b4KusNO~2a-N{B$ezW>QvjLijV?866b{CW
zYXn=AnKw<cl=(EP8n<^X>0*BXu$^K$y{<aHKP#y*9RyTV|Cs#~y8ZBYF;piQYHd{W
zlRG7f^G2!Y)r)F)ktDaV`+uI+C+Wvywr`NvZ<^jb{`UX8{F&m#W^TtqM$>Yq#jvS~
z1XZ*hLB2?{z>TB~QZQRm3gC2N^?q^RU`Y?u5i@Lcgm$L0f{T-vKwn(kAL#<bct$)f
z%XAEL0pf#LB`q+t7wyHbiXVSood37~TXE(-SC3MlcChofmoVK=exp8l{qbf;d$QO3
zF<6)$&Gmv~W4+ro!xj3vECZw0Z#Yp3X&njVUQ#RCGuH)Qi7CdKCp0pB@cuId4W&&;
z)2n6@R22fq5)(N5Z83+S#-x}-?9eRLaLxdMK@1~qv-&ii*;tlFr1o<p9`TGb!u`0^
zPuVrk(Lb3e)q>$nS5+L6<fQ3=`ZPQo{P4wYD71$=eAF=y{%q`3+XU_}00n{F-x;#=
z;FKCYSvx}j<tsMh1eQTk#}Z5e=C3++QcWB&Uw*Gkcp-_eZCm{@w<iVKJoNZFdW}03
zc3bDnU;VkOkNHp*eO+(K;`f;8xK%fOOCO@?^((D}z!kO;Wv#T&&=U2$Pi^zo?ouu;
z<Dq<z%0a~Dh=)S}_X72a3@1U4h_t$r3NJU|gHYlZ0U@*Qo01YCD}JGV-bx9SU8Gks
z$&i|?S|G%{ARzhrm}z1JIK2zEF*{akf-9w>51wS5;S@z#l(E;Kd|hsBXoXNK-Cv8q
zc$Y9t?a9vmH4tpr!rcvtFktmAzcqm-<r{bUFKuqtxo62?bOS)D<mz>W+^O!UGO}+c
zi*0XJb;QS1owSlu47hVnHgF`UIF4V!KL$5y(=+8IpC3E)`+mP((5#Z#WBr_?+}JZS
zXCQL6)gF{@=#V6FXmDZc)5Aryu_dWakio$9Gf_WZO$sp~OQDS6Jn4{xuKmu5WwH&N
ztc-@JR?CyYfI)XGhQq4+Q%4^H!gPmZ27VF(U%2&!;J;6pa{egw!a>-g`7KUQ*Il9s
zGZmaX9uHxM&;4REu`(SU{&j21yt?3d``sUG=;Y9{r)3q+F`Tlr#>-brqs8of?JV7`
z@p}?mJ+i~XdU!Ki!(k(e0jD`EEDRnth^7aE0GR0RXVrth#JJHTyD8nCn9P*%BReFy
znf}s@sfznIh(i*BU#U}$23-UM804v_-CI!R^H(D2<IF8@`i{MTNefJEs^um~K-rRA
zOORALjb_2bo)<@<4x@3DYVB*hq?eLr$w=q}f+ZE>Wz9yoh?sOE*rZ9`RUeODK~lQJ
za5pDj={QBJ%=WeE7Tq+?pCVJo04?ir<LFc?R>6YIvl^eKiLf`c*hS)QttS8sYlsMO
zW%3ye;_25Z?a?ETDG&zi1l`2<mNrR|tM5>t2jn!1L$Y{)|JZZM_eMvT6n<j?A&9!7
zItnkH04PyA$s&dCU6+^X%-3n)f7YSY^=>gFm-kO{3_o+Kjvjt(hcU+fx5V%y2daBY
zk>iaxD;TWKI0X)+IFSDR7-#r%eE4-AB!0MQiyu^c6(UJk&Nm}~lD!5J<1@FAYo(0L
zr~Shp<MS1VjC%^X<CL|a3seDWQbEaWuE9zX&^S*3wvd5Rd<1GxQkhwVGyz?FOzI8h
z1dxulFo_`2XVGb55D^%<J4HkU-8~~BoE3BdEs3b1F%T%OUO`%2vYBvcok^ft#Go7P
z<`$b*{V&A|w^~MtT6Jye@dfsJmq!8e_<~fuOl4+Go)kYqRTd#*5_#s811eRN8vI;h
z8n0I<qUXZuZ}JK%OJAn!wzkR|dhd49y>@EMtkHsCK@_at-RsN%h$UFKOf$9`CrmG_
zGo)($SyTc<V$qyi!TJd3y8$h1e90xl)tOXM?FyB#&dJ~nul24BoP2}355F`k;|*A^
zdAS^)#tlwBx;$(OUMgb=)Sof5*%6X1v$ri()EG^7_<j2WJ3q$hw+>*yO&v7l{+g_^
zZpk#h<~MAB66lG_OA<gmwZt9JjB@}mVr7EG+PQTQs~V;X4C_pH)98RO7mwkx*CQ8j
z(EUvVbX1&ghEdOC9E;H)=^<0@%~ZwV?J4o?b_k6SbI-q3nyk1soi~#LQ!J1Okzs#n
zPxO2mPfH}EkkN~LGcfjUEJ^oDH&Sr)s`(%D<cL+d<y1M{0C>hHWaY(-H{HGuy)Rvk
z;JEdZJMlcUkd2oyj6RC8yME~~iO)^~guy!bvoap==4Lf`Z}mC6FJgv_6f6ME5C987
zGYr5YL4rOi5lF$Tx*U6Y&CiW5^RiTJzYAxjt=P!?Sjlp<VSfGW$VR&%=xXx0<;rrP
zbe2-+tR-bzQ5vTQiY9W<v~!kh<VQ$(!{7eZFe~HRF?+o2Ch8w^JzE&rW}a55oW>pv
zQ~3*ayBH=4qK|)U^<kELo>{^bF3jotURHR1!93Zngm`X3;gC~Wo>rM&8*>I~zOe+7
zm2TBfO#h6cE*~L&KM1KT{>W;@erXoLo3x>0eldA{C93*|YFbd%$>R`!fx}K68)W5Q
z$5)i+yg4yBscg%yH`FZLIIY&l?&uI1^nTo)F$*Zq@8$lj@1?JkJ=`u)*Kr&25c_KT
zR?sd9ehiUs|7Zyw%}^qhDX`<Hv^Xq`5Qx*7%&3;9g#q10{n{al9{@zZwcOpU;zu8_
zXf5-XQZ-gTc9m=GKu6x@%yAEvzM=cNFf-_@qfgr7w1~NF8K37vGduH1Bl)<kB)>6l
zO*^nkxFBRJU#DmO0}U{WH8{z~Cv8d+*WqDfxib(aUO;lf*A3q&&BBX<tA(u#X#ROp
z>s6Luswa>9;5`r;<{VK40Jyv-C*yA#%cfBa7-uAVV&NmAol&}FVUvbgLg@7tjR<j8
zONCREYVj7k-)Y!<t}*u2N-^ju?%fq8Me2I~WY0MpW@m?0`kwJFqg%MvRzxiBO0XGF
z(o}inO!g3r1HO#Sp*&?ilLel4#-%c7J`i)*|G5;HL&Z%KPb5DGkwdUFM5GEUAVi7j
zIv~+s3XrV2zgS&QDM*e~Kd7ymiMQrOHrskFXuT!ljhO<F<pHxnKZ<niS3q|jFpl>c
zr8l(>@*Aq@szn{t7tt>mBPQwucIhaqc<TV1-W(UCU6w$3`BY0S(oH~DB(%+prBPb6
zTY(eO9eI;PRIOo0EP+3Q2?%;|+N?Tj@E6du>MS6!YQxRjvLi@_&toH9)b{13<>FiP
zwp)k1heg1Zs3Gu^JWm*5ES3|~%>0!X?%SIPsxL!)TZk9}!2SLxgFbE&ZVvEJUVN8;
zO)~@NGb+h{Ab19{eb-}06dO^M2Psu`G+SqW{GEaP>htR<F=6{M*5U0g@y_?V;(?|q
z7+?Uze3mlcoJcw^@5v$YPtG%u9%tw(ljB!evlKtJ!ot1{9v9#3FQVdRCTlV!y>flh
z;YA~La6d^T-G755A-dEfUNiLc%4u6c0F|_x=fBXckmPFs{Na}X3~i;<u1Q;|g3lW6
z1@*`&?aUVTI=72NBrkqQJ`&>ujT4U$RKO!dPsJ-p?N}%Wva=P1A*oq^>qF1QvGs^7
zTpSH()APdnC@*M4Pocb+E{4Ix@~yy!P+%sBphlHEiWW=sdJGY}d`By(Eed+%3FXM#
z)>vY`<lJ9mS@~eChk6zH5WC{*XN5HN4W(C>;EIyeVN*-eYeooFI7%-YPJty&$$c-M
zx1D_C)<tk#HZholVG!iCPm@VCSE$8jz3BJLK$J*pR7@c&vh)CX%(Hnb#hv$pUkar~
zEg71dW+Zan*d|gE`WsPblhMuleYCMN;UJ=a4sCRi>YAGlx&YV*Y%;Be0TS!jo5=2e
z_%nUiFwWwumn4g5vBlcR<j6MilKr9<-(|bucI0lx@#`C6am$D`$;SCysVeCZKZt4b
zz`f3?#_5dKA1MaKY4ba0-G}9bQMW;2%|r;{$O}~X#gV&CH{kN}4FALTK3>|X+m?8u
zPklp^`;!IbM&;}S`h1a5Bt!F0jxQ9i>-njew?s)MNA-2-X|Kp9Xh+8UxkxB)uBy0E
z<6Ga^mc8k!WXA@)33yk1LZ)RG{oJ|?Klx|bN%Nl6%?u(ZeHcUcrbxF^HyUpGI*ukY
zy}vXxff9$2KMgs+g2frSZNjr{ZxvRd#c#q7-gG9%4O&~LTgSDoX7&0_NNx!#Ir|lz
z#<o7#OB)F81j%%+)aHfiPQD}k@idCp`}DT9!FRxq{BPm%NAvQbyPjgeQ{rPj^7X28
zc}F2IAM?PRvlPJf@-=7*4QvPc6F1;6d^c<p44GdqlryCv+{D9tt{GZh_Y<LK8jQ=u
znNHTO{CCK_90V<0$u6r7(9Fp5(}|j6%*Fosfk~XfmLD9hy!L`B_H5`a0XJd)u#%a{
zxfpZpeWFm?^9j&zsDmzjT77K23bA~QV`RVqX&Gv0;~jq(z6DcH?kt6krHtU~_eydo
zk?*DicyH?6y3-ycDVQ>5L@N4WnPjzoREnN2*=U+YSO+&OUog1CNq*6b9<fclf;>MA
z;HKXN(8hJ|F=6FfcSVn@7G(&^dQLK-DvWmH@gv!E)K_LE<gruQa{N8A=_LZoQ?|7h
zLpA8vXEyLfD%;9Eza_0p+-aS$mLI$$lZNW#O^Yl}_xK$>cS|+()U!rERoLz<<yF<0
z)4^lr*Q<Md<<++gWE#%jl|I5_YV%?dl);mu01R}WDF{Io0wlSHD3uWxTSJZYjDB@=
zJ*?Wg;e;=HPP11NEx}4d-d;5VGr%$(FBEUzlQhVWIAQ)d4M|ToU?5MwPfo5Shaow1
zvPxow>M6?(oX;Lqm*a{{ZJC8%Xes0jWm&2vo-LaiCub>|sSCfD(a=Y`6pa6yjG6v#
zG7fMM0X_XUvd_03NpejVAI8jglIcxM8>M<cDt3a&%aS-?=Q&n)EX^BTQtW+IPe+4`
zGc?5a#;8%8CS$zxb>Llg1ViGILnenXPXtZK;;Sq%2c#xnuEIIwtQpgouW_v;Oq_s}
z5Ex`sd9@ot2gAkFF5aV-F2E3PlOJrE*wTqmO3?eh$`+}!9Nuobn?<LT&t_9I<zj4A
z_?jEo;<%s1_0nQ8Fve-G=N*nIzgdHGNmgBF<rJV}$bE4(dHf9!eZ<$waisp0Y-c1<
zU>6h5Nxpc`i%&ICdzmcgj_I{ajtY3?{H~Wz2ZZsN?EOejvr3=#2$@e1(_20q2HJIi
zlfCG=CtgeK>|1Zh#GURVQh@j;28K-PvkDA3J^Vk|CG(r&7_vzMHbA+H5E?Al0+N^z
zntI4LtvKIyOHixlt4=h0b<+zPBXOa{V<m}{NkP}ub5bd%6)=(ye|_Sm8(E}_f=%F!
zM$nI*xP>wmr;$Dbbet`{1n@e;E3rl0u^c(s)TAo0r=eIL0Km-|n131spF(O>E8Hb(
z*R?MJxY4AW2qa2OAR<Cdj4vW0R*Yw(ILt?AQ5NK*&cu<pq7~h46x5~E?oDSBv7|93
zuuzs6+(5GYDpVoGAc0m`85MuYsJ|E&rK!rHi=;6TZKa?xU5<--Wm0@Uh@oLjaT#bm
zAe<FtDtxCvEIf;1$z0wb_^2=`Uuy|7R#@E^*AO!Pm}9@u@h3f*L7!9HZLzaqW!m5r
ziOtPvLg?yS>k^b*uAp&wS5Zyb@hiA2elchthP<m%0N@9Kqj+u!kAiC4utdgpz{-Cs
z5AfbeOmSQu?AKU#Ik32*mnGvgv`Ckg)=M%PNxON}m`=HQNw=!|`Q>on178vLnGaui
zy^(X(ypA<NmXvFmIfr7wO1(xahQgH{R9L1lR)luV>w8J@7rifgYTJL+%#dYAe?_N$
zm(Cy-*{%;Bjr*5j<iGdg=)Yr@`_(F{-^BpEYqsudb$t%w2CE$k)lZ$>8ENG8eesFC
zN%~}&2kBb7ro0z^Fi+p=?I=>$T3N-+E9kf91=+QP{I{qsH6c&sQ&?s5**DEwkY&Pp
zlnv~=<(5YBpg*4&@grsS!>8E~D;?sEO9fT@)3*c@FSUG>2SfrhB1goJmOp@{iZeFB
z^2HhdrXr95N?%`^=O>NUU-#fi>n-u6OWPZ_S5*GZ940lL2iwV!M>ZAHG^r&WSqnHt
zmY_jfYI+pOd47G4t9RrAGalIT)C_-ifz*~x+g|lwJG)JS_Ke~OmxkEr4Cc-o1n?Rm
z1H#bN5)KDfP9(6gmD<T1P8c)x;y20Qoz|=yeN?$nS<8MFwmIpoeYA2xd%*j1!eA`e
z40Q}2s?Q@Q+>G|ZI#wA`&~Fbnb-ors2xYUD(GbWdUA_7R88?{Cv&3LdY2OkAJ<8M6
z^so|K?25F~1?Q7QGkMUBqlWtinIe57F8RTT))DY#AFG8P7}f#i%T$wk1B=>d6r^-p
zXU5!fOLUB5I1ZV!G;}ij+S4s^d^~Z~<||k=IM)?@x3Qi^Kr^c4Ny6W!v_&(+p_w^W
z@`luefj}hPTY9xeC=fp#lgj603Ifj&IoU4YeKWWENiwf8mP+W@&%i)X@@tCZd%E*?
zg2O>N<PP82m|Oh~RbKU(#)ux@qN1B711j%J*in)Tnk(Fb3xgQ+(`_4CgBU-C{c_*E
zjCa?09sa-;FZFBrCaA2QCvwqFtm<$^)WxtvUnOo%b3HgQ*0<Fo{bx`aEWWUiPv+gS
zUkAYnT}D+ijUJ5cXBp*wm1!C!K3pQC`3!HdLP;o;0%34-xFRCF!jPwf*wMC-{1>&b
zM8cU@L1f}+7QHIt1_{gsaMa$`$MajG0Wb{zUx_Xvp>|VXBu@aKCHjy-lp$V`AuSG0
za?6sW0ZR|KXb0d%3>AlUiwW3^BDIR!VwI5eG{wqHKEDVa4S9;kPK|=>AEJ~iMwqWO
z{76Cf)yl5I=I(F|tHe(n#<^CVFNoSkUS^0%*JFqvfyd3B6>}iMUI2!`F4mVKVBpRm
zshB?*E^<VZJ(CZIV}v|l&F#%hr73r2iw(eQnmc=$p!N7>BiBEHS3KEi&!X2s!p{=`
zWJ{e~Ir{-@-Khyc%?T1R)$i3ow44Yq#^SwN^d`Dk*=I9+_H(ON46%UzXb1&Dk`l2S
zFSegNHT?l`b2`SjsJi1#hnf7U{W0ikBmE^ZG>oxnE#u){4zC>ZU+X+Nx*kw-AdMJ^
zpCNb}T1bS($a}uy#Or@uPK-0WFv8lvvT#TOmWy*!CPP>F;F2ioUEusT#vvd{%$pJ^
zusohxPE0cz>tLG$KpeWg?*fV8MHD;3(*YRj`+h8w?%fzH4YRGp>>36xp!Vr>p(tKx
z`bus@_=&_J5)&o0gv2}%LjxWS1&I2z1Qg8xGHQ}qttA;V@pS@Cce1n#=}gb=^x6sD
zDE0f?(8@!<-@}v^QJ0)5%(Z)aShc=fzoZ53`UKUN$wv04m#=0e6^x-+#n~%HhN{ZN
zS&6^(SY`>m40Z31Laj~fE9m|i`^M`v8pTdGHr4r+(wfctZDqG^`bj&<Nwj0LI2np~
zV?S@J|0vO9mAT($*PSDwm)P>jqV=ftT*J3Zgs{HgJy+K=J#Q8b;d?pDTI;D8vm6s}
z1ZSs9n*Clb@LO_+!KPT3UBb+~A}lFG4{0w4JSp5&EAt)MM)--QABJHij`PPKTMThg
z32WvxWwCg~Sczr%<m+W8MOd%aEG@0ucQt%NYG_aCec>k}DyVBVw=ArdK=?gBFuqgH
zs|}+>VL0|8m^)*nP_V}T8<Y>#D(!OAoE>5;^;7EAdpSgetnp#*Ml~7a^tgg?RQNjo
zP>u$F1=D+)A2pFFhj3|<d%gQdHF~qv#6CG%utPFC@${8qs{MBg{o)N|hM&L}ae@3X
zcg~f<qO;s6nyL|6d%vyGIuKe8y(O>MsJO+X-4Dnhdyz7G9)A)sTa9K#>&SS2-~C~N
zT_ky~jZLF-{P|O7j!|}uuIlF^8ISChJekg^ZLquL{nAE7SJbhe$En5N9Su<3j^(ZC
z>(%9cDxJSOc5`7<zh1}*iZ#yi{}q~f#F0Dp|6)Y|jv(}lV#58hV{=C#RI|0rZR$L~
zbC9BV84%6Yfujnw#DLX>DhlT84pXCj%(|!S+))|R!~{enK~w?N@G@cCV&(IxyFwbZ
zK(ciuUfXL(r3|Pvv54FN)`CNv`SQ!}nGo7&I6cp>i0`$wGr;b`2uWokrb~U!$fg{a
zmg^iMzvI>K;zXaTx~5OHw{aS(udPtC##B;Vwi&%dFe^z>KMp=ngkDmV^+xAT5uTcd
zqC!S<naS-5^y9_#FR}^|0$%ZZE176b6d`>kZYd9>J=F6ek)JbB4J6Njlcb@5{TDE;
z2+wcU1IjNSj%|Xtcw>)wda76iif0z`h#Drvz%&Gq&uM-{9xmElk|Hl9i{9zPznT-g
zJF&W^%Zt`u78GAQS{#XhCbvg?xUooGD_u%&;ogFG9DX0f@X!##*psqbx(3;!f`KGQ
zm4`>P&x4n=(zW`sGqhp51u4N(toEF26F6CsNh}N9)8oR3e>Th>=JTV;gX|F)6r4X;
zTHRF;UuPVPl@#@w4RQ@P^@UO1SE;csSVMNt{uW2VYd9X>am2RZyhtyMN_)vGzo|}B
z{Tou_RbnHL?st1-L}S}tp;WYw`G0Cmil!XPLl<&2um1sK6q%r)0ux5Ugb^l^T*-Z2
z%4iw;8!>ng^`CTQ=G5vHC^2r-knL`)7?G1`wmfV4dS;_B?fW6Rw$nowHttM4W{8G1
zPoQeVvI}{&vIq%Tjq<tBq_#<%GWvcc8=<v1f!m6?yC)`H3-$nDL-#<^S{S~CT0E^e
z<<EZ;421FVn)T&kFr}|mh{gRCNt5X{6EK|W)QJGMDzJ*Y$~?p<$enSyf2_1*D@k%@
zHc?;4+zSA>`Wq%CeJ9fxQDC6Kzq6r<-ud3Grv)dY8H+q%SZ`z#B&G^g{t}Al?F>~!
ziZNaO?2YI7S|qvu7s0$!m=+OM_Pm<aM+@+v%eZ{@dJg;m2(|kct9o$vd*F3BX6XR&
zzR*-jDi==Vkh&BVmK;PQ6lW-jc?Z8$PYwf50kXwNJgk?<2^m>{%ae?@Xo+JFv`GG;
zgkF{NmG<+$JHl6P9+f02I=#YdmSK;_c__Z2b?KWVmC9lkjuj*AM78>sBQXr}hufjM
z9Kzh{0`=7q;pbMPNKC~4)VK}vI3hfKG`6iI@n9>zOq*2bM!<9FBQ0k}{E&F-3h<Ur
zH{%{(x;5Dwayqoa4a{CJBEM@k@VO*b<ZAn^t-6%S6h9{+G)$r+I3K*IxeC(eR9Y3L
zJ1L!84wf}->nL>8TNb8xMXP9h9;nG)yKSs2EW#1rs$=Nq?%3vg7GE#Euyk=NRLgA=
zNn}84G0STb^x-pq4Q4@h<BafVB!3Q>cf@<+-#V*v9c6m+=I!p|WtMl`I!`-X?iKhv
zYh<dywzj~lxR!WV>WS7<?;!^bvM>x@;q_z=q?Xv|hH!CGghlR_7CvUAgIaIuE?<*m
zjOXyD$AqZCyIZ!<&$0G`7F;=nc^)mF#w_2Ph<|b06uvBTf_GHsb!RrtzFN-g-i-SB
zx#uxH^XP2Kzoq8v!#C0srpIk+fSa>oGeA-eHk@z?#d#l1pF$f<!WfuwAg|IcCGl@e
z^3eaQ@|$)^x-l__*Q6@?NpYh}Ppton7CLOO$h|}U!yi+MbCc-xZt;CV;vM{zK2Q4h
ztl?0ZwUW}LpZZsA3zWc2DN=2{G8v$@BviC%x4n+B^JcrgzLhI|pX@-myOKRI4Brm?
z6+nhIFv0-YB1rAle}U%0Mq|wHt@5%=T{l*z0u4g0-@QSF>^O2Oy`@Q_*Ir+v<G;R(
zm6_HyHRksJof+{I+Vm7HL<XOQVhTDqDnAVfV>U4S{u`Vyg~`=nb*M)Mw8F>E;&n7>
zBuJk$(UQNcGvkR`Bki}C?jdMnX-zjruMS^vO*fjEbyIB#xuQ1$dAh#L=L9Q;HR}fn
zj<FE#)d|oq2)}PIz4*BAbIq=WhodY=>5~`vEN<otjH`9moVvs_QN1QfI$F#cC3eC#
zIy$1b3z!1I!o|29BS~{>C`%&|aVe40Le$-@8#5}-_+i;&>F;Oq@lS*+J=ey?W}K3o
zsiEH+MLSsL<j3@WLBr``6JU;t@|#mPt_EaBC;8OoFQ6%5>=|Ty?z5%&2GeC<WEUyu
zJpuTqN2Pi1`>w5oZFAfl=aR)5C$29G?}|Jxz3pde!UeVp#&YB7jr(KZ2Uf!zaYP0V
zGgN{w*6BiDGVyt>SE%3gK{<YnB(s9-pLk1Yfc|ij1Icuv%cSxLXbSe1idR1$3LmEe
zYCM8dJ)hkV0*Dg7%b!Y<65W>v0D`%+H|o1S{3=cqcv0@6frvr<W>h>0=U_<FRud%K
z;^+R)<t+yDOXQ%H6}XIVoNP(-H()iBEs}f_piq6U%RYk{shh7f1O_TT+d_LL$Xs#@
z|1~vbUDwRF__7u;VEU0=WOFB{YHS6Wn#M{6&>0=;J^OzrW4-SGzj@I$szU*ZRsoPy
z`z&$ehK%mFAebMN8Plg=UMRO{YOLtt7Ymb_T*QI}%0~vyGqv=>1>;8sD2E$U#UlBW
z<r=MYDwQN7ChuLWRyKpuC1*bMNYd9<5uXMrIUyXx!fo=HdY+-tM37GT-39gfH5EYb
zaO1t&9cfK)+<0vk9icOZX7nPbXH_Q^RZ7FGYjdoO&Lm+^m_Q(b(gB5R^zki?WhNa}
zWkdwmqZ!kyTcD`ichVq#_ObSnZ-`Gp!iJ&AQLBtrc$E)LWP8g!P}~CT?;Jj(pRYyX
zHAx(G@{&;e6O2EGBY^f)T}s7S{O`-bOI2HS%O`QaPrm##9XQqa5#qV?T#D>lw9NeS
zQ{ey~9l`w|glzY=Waf-=QCfol3u_=%@^1+nq<{*@>e(lZHv%~nCYINK#`<4337op1
zuf<B@Pw`csJXi(fU=hXBU@?dk<W2>&7gtM#GBhmi69GH$?owEa)0Ofg!m6dh5hdxR
z;}PKv8br9<Ct|D-NP*3Lq_9B{dV6^$Ls7U25+PJDgfd7H8=JA(iqg5Q?flkCHuG6b
zF7B+gKqp1&kLX-2CefIVSN$DQ{PyV$Zk3Rfsi?kDKHcQfN~pAg9<MOovHr@|h6Zbv
z)V7Y4v(>7)F}P5mY~j_r>?n>{V~-~Vp@tBosgJa!2~U>WRrJt;ANAj{HBEwWwKZ;8
z8GR2-p)p=|7uIjhud!TTvS<0#j;;QnKt<Q<Tj$6NAYOgetLFL>khkqu=3NDw<Ng>N
zMzwk!xOBr!!Z~Ti^&h?MxzPj?fR6-qZgN$W188duz75&9VHR5LQayWLZba5_q}xtj
zI5l08I4fyUGEqk`X7WbEKAg2d0{l)*OvrPzG|=xalByv9KqsPo;8)i4Y5PO8m<~(c
ze*hv2p`(^+mTIH~g1HhYQElHCE$Q+3!;gcs4olj@Sf3-zk(76Y#9zFO#Kqm;8ByRW
zEp+Trk2V|0cRRk*w$%Ur`{CC|dA=%Q!f`7b{4u$cwcDQSkas)e?4Prj+dZwy1-llv
zTu}uRV(-|Ppy{~!1z{vNqcf9(IG!#3()GCjjU`M9PK{~uHUZ70GcaFq`qzZ0nPOaf
zp%)a^x`r%wI0^s;3SLRGYT=^BhNHJ*X&=A-IHWy?<@e*-ui+7@I!Rsm5q~u_5iLqL
zWGpKCS(1(CH0+e6y}nKU`_Y|u=sUmY!ml;j8{ervW;ImZcR8&|(yBBT1EUvp>a{Xv
ziG<#R0f32}$6Mt5<rSoP)UA+{RC%1;wc#b#i-i5(_V}UC`$=6;p9Hb!M{6#rfAr#h
z!rzFQ4;dSYQu$~KHe+>HVC5PXx_?n4s5*10KGdl<LZ)G>&NCU#kArigflu{HWv`-X
zU6Yjh_P{}frZ%f<t7fvsPGDX{LiXt5F%M0+B1+3w?(}?Vo{3{W^9zz%iIjf^13HgF
z{1nli$8I?^aRRYR*PW@KY(8s-f8m!V=&=UYLrDU)E>l?Fzes#n0Q0WtrS4qYw{(rp
z23dD$ifNprk?wl8PFZ~v<=U}|Ob>pkt>29+&HGYn+{0ksae9LFu+n!hkLU%FVw|S~
zi#<I5n^^R04^p9Wnk<S{8wwXRJ7=(a>DRbjAW7wXxsB$@x+(9lutkp8El(85VI1~#
zSmG45V~3N1=Bb@k9=;$iOMr7CRV&nwN6X>y95R@CxmS1l7%55L%OeI)q-rykzlR$U
zr7M2A1`|v2{nrMTmSbjAttMo8dp7pC6d<bbs-C!vAPCGm$&7m<MIhkm5HV0Ks%&;i
z8lop3Fl<1M4)Cb|C%2?vY_zv<g_c`Nv{!5gQFTzlAOpzQEymPaQ2tDiwOd?=Bf(5a
z6&A>;L#H(^#`IBYO=hRPaJ0tW^=ZhGqiDfAtnmzj>BpO>iMYulq%`G7P$|EE=B|@x
zQl{^iD(1EnZW9rHAtJ`&O#(y{UH$8z07kUc$p!!mJ+A@2-aZiy-dfARtr+3`A%*P<
zj*4D@`GPCr!EzJkgVfVe{`3*qyD$9M+kbEXfKHd5+6)?@;E2z=$$9itMgTwM#@3I8
zXC)zX8n>w*S3I~LcjUz=lDe^?0fCEwe5zLF)0E!&=HzDe23$9_<DTn3#qeyt6v*~^
z&QsV?9o_hc+}Wr$(sDiw{pHgw{IU4s;;+nmRM)3~zk|eEK!B_s^Y|>^m4-Ry$9XNB
z$Dx#rpV0#-Uwr&QK7n7Lp{p6Z|3<h*X|Z7`K(&$K=5jW&N_`=%Hl;xhqJ{dF0ym^e
z80JhH20|*J6Pk7gLZRe<2}~1Q@sy}ZgxIr)a5XU)JM>%wgbTeH2KA`%-^#S{YdxCt
z^(MA@k?@m<pJMg-FwuyzUE33j!In#{ft+N+tv~v_n8JeR+sMhYngy5tj?Qvfoh#4O
zU((95oMV%ymn2CevKi(LY!qQD+?WN1D<8g!>dX|<h%TrxeKcHY&*I2gX7>))n|HSN
zzF5c_{j659j3sNl*uf$xqML<{{&Cbe*Eh0ew!!m?FNXONd8$Xmk`WEHpmd((=>&1s
z4({S(v(rt_d~GV}KtYx+ZI?Xj8Nvaf?v4-OV3S6!d$U!LE3>K1xywwxhQPB;RI3<=
zC0#{!8}ibeYfP+P?}z*Ji?N&oekXN<d!x?Q%wT;_hb#eWTOmU7uw~d}iFZhG_+yU;
z?Bz>a0Cw$BhesXd2ks=x*~f$!G0j!)ezCJ4ZaG+^835gE7z8>gY0Q5XP&!wQQ7?+e
zr4@Dli%Tf`BLM+STh5D)#o^-G=sU3@Z}eW5{CwxvmJ!;wW8#YM=xF<|NS?3cyp|mQ
zK0H)Cfe%sa)%iqrA-My#zwFB%{B|%1w5Of0I;Nn%6;L2%YHwvzT`tZwfyAgv8<ntW
zr1!OLJQ(cgt2%tT_-eRCU7#y%+-yA75)j05mw$3GxA^8~;nzaoum2wa@<0v0II3j8
z)@m(<N>sXMb#PVHbjHv4y~lmudQ?p1T2W9;-R*rn&)fIao`2nu_g=lj^G_-*@#O!N
z-DIy{1N0~h@%S#q%wgX_b9y8>WwH1itJylyXly-dm!Z7=i_+X*3`vWsC=BI`^2c2e
zb_hFy7PFu^N;>Z?hO9#bu9`zZc+2J%Dl#0vRBSi^21S9o_6-l=*==D&v4$CQY+i_4
zP&m)|<{+3k2qH4-U(f;v!=_OQ;)B9t4&ca`*Xb3=b3S4*9O6WU=?tP-5-&#`{jm0u
z!{1>CY0)XE00GWXMp4vjBw4O3h9HSZbCG0-rLneTS0Ru~eUS~-2Dc|1fXCVyZ}Ler
zxLGVyp(I&x@)Aq8NjTt;vMX|~DZ|M`2PqtA0CA2QN4Mk^Af(fDCQ=c>lGy}{pc9!a
z$mU*4r$Xl)A#F(R-Pc3{7VBaYupo&|+JFSLGnrh&STF_;KY6N&A0i4(a%teXXTtrT
zE5}bzEb~0su4m(l#(y)Zl1<lfI1`iIRWH_gm`AEkBk6>ka%KD?dEEEe6#;&4VfiNI
zocE`yrzLZyKN7g#I@PS%ClktP(O0(1VvGor4?-G{LIQvfz`y_iA_)hCK>(1LWAXnC
zghQZ_AOqGO0E#|h@km5!2_A~Y<PX@~UJ)dSN@4&xR4Q99m_r?Lc|@vNEt<?G@R(e-
zcL<-$=QJrazBv$~#O9Q`R1x(|j7912Nvu+nF`!Z?uu0e+4?C{c;PvX|j>QDBLTxk}
z#a6ZetJUi@OMPOS3AtUbRC#Rl0QR@oAouGG`v+&0U!^v{1}ax}$60L{e4M*10LDjT
zu*-CRH*n9$B>DX40;T|vMKtm`6+&yJ)4lEQIJ>$o<%_xZF?Th+*%#lz9ya>5*AaTr
z!ynveCobvAxAUb{ew3%B)r0jqJ-&DYuiOAW;{A?qkIkI(!eX8OFShZ_<W8$y4|iMB
z?1cV4-$_@Ky!4?Clh*sdkK^$Hpojbl{Jsy`5&=Fiidz3c$%H6~LI`9s07Gmd4uPPN
z0t$nm$RtVtL{Ul(1jLbA4h6-`r~wurk)QwtMll)~w8sb}dmEtOat;Q_G8zsBMe)pR
z!7@!W8oM(tJS6PN@|>)f$}()Ogv&DH9)>nf>yr1U3WUcku4%Nf2(|Mvi8M14TMaES
z4jbaeNs{!NJkJwks?a_w%*8)W6739wwCr5In9rzX8$C`lqb~_Ks&idS&~z&iK2vmB
zQ!u796sEY%$YKQ(CG3mT_#%j+EW5QUdO(mI3X_Pps`BbnRaZ})R<^zp?L{=96;+8T
zOtu0Tc~=wtD?nNndrs9<wuB`|&nsIIZAMHjA^<HYMY!ZNvplGl*_6$`y;x{E`8Yx_
z^qYBH6O$WcCzrL0eN`xfMQx%NqEk^_>8<{|s_4xygw`~(JnprQ>tKu22t#OsHpnAz
zf;U*=E_64}0xo(Y?-mhY%I?$ob3Uj%FLh<_>;ZG;PjjT2zZtw20OoIYaL?yC?X7rG
zPecs(XgO|_|K)jf|DI;hTm`8-5IfTaYC3AL|3i8tC79+qBK;1r3-nJBMEguW%0_W)
zYb5RQlx-VF@w88IAo2e7BA}8Y2_bK?KHU^;bKUV?&T~}ZcI&_fD#US@TqDGCTuqOo
zOq|XKYVs@956dDI1l!DIoYb$%TNj5@M^n5vSwPSA6zx#y^L-g8UmU+HNOxK<dBf3j
z&G#!JnIpSI(HLHd)$o*;ci#5eB}sK50Zc@*A&d3j(&0!U@Tg$zLt9u@$Zea~xfSIJ
zU-pS<-`epuH(JD~-H+_7S^XNL<FoBQrj*`cJ1XmqN3bV=UtDYeePqGyz>;9S9}DVU
zZsAFh1VsFwL&JY>(Qdtk^7EZ35q$57^Dd{lv<kZoNyHJ0!MFVQ;X~yiCIEse1~LE{
z10EoQsgJS}2yj3H#ylpFw}4b4%vt+dXl*SDvqUV_VHyrU5hbQYSg3!G(@s0BO{~RM
zp%UG@i;4(Mszj);cni}>imh5M#(28B&XZV+(P}xs=<I_dBzJ>Nk_oDWLg3p}e=_aa
z9HR!}+hjX+Z!ma-BlJ#^2qcC84fp^_DJdkRf;xG!ZB9X#LZ96HdUgrkCojoJRvki~
zSHvPAI#>SZT~oQ0PGSGT8A_F<OXzZvv6K?jMDU-Qk0uU9{UIq}vSSR%lrKUWm=`B3
z*8Bm0?kX6uC}ct*d(?FA1;V?zmP_So@KjIetB??Y3P8XJBoq$?gaIJ0$NVB30Di+^
z(8wd^B>0I$VzDShS~moW03*N$)N%^zlR+a;;2b;w3YS4;u=$+E9V?j5A#$0t>TN%l
zO=c5mtkz>gn8;`m3FPJ^@21P8bvTrUsRWYBs<E1lYJWzVL1=aeB-RH6gV}5|OBJqJ
zL7-A8cRQrT?OmSUDL0Fi*4;6(Ti}<_1s2<Mp-?RH`>qCuN|#XWI9M&-ACth}r_nm(
zf_W>D)<6|$<c?P((P(qBEnKff2+IH-@h5ai(GqjSolz-7J=wI0Jt9xO97+9py7A}o
zzEnG#2+i~89V+$Dsm|kaI=tTIt`Q;T^E)t3zjwFe0CRj3&kt(_e!Ro{({G>75#_vf
z{{Npx<2>>|uS53$K#d#%f*<e_3j00~Y#jJMP<$4i07?WZf<o+K)PgZ^>Iwuy@eDY_
zfB;NH00C$$OoBymEL4JlQF;j&!w4t~7)0P=3lyM$Y<7aj$Rv3eL}(;$BE9U&{|8A>
z+$ATuZ*mZtvu&EcC@=~G117!<+`P}b(!(1i%JPe<=psj|@T<&H<g*&d^5nS1u*+nf
zEv~HEoioA5jHIA5b9BhcOs{<2qDhN{?LtW_bn!yXlVe_j&(5sB2GQ?(7b(kBRK(3q
zZEAQ3Q*v_%$RX_mR?;Krb4-aObz+!@)fJr$5>WKw={+E<-D^FrZHoM{(K2;`UQdgq
zeyLPX{cP7&a}83wQ#K_Q#j<u42VAHOopoDV_YH8Jq3k6EX~DKz2PjTd?be5+aC;n=
zF?TZR!cgo=(3dk3g^>d~wXIaFD|Sm*>a`duNQoY8TT-4%*jfnTAdRXBs33<+c!?dU
z<9~{{E-DD(K(zi#Ucp&D4G6wd{#N#7FN@Oy+!rQh^T4n6Z(BaNR&|q1PZnFD=Wu3;
zmS<^>k)G&}UUQh~m<Ec8K{`9#q%Y9Ufu-fn!XK?A(9APy01)(%5FndI%M++^Zq*^h
zyNqN2Mt}r$7A4zO>%GSaARWHN5&rQcNc&wEcFFjS#YN~E1^;<*wlzJ)!c@9$iDEpq
zv9Qd1WF5;ud6yr~z6q|uOkK!wF|X0o9P7r-lE+6~r<Xk$%5t1V5QpNO-ETY7-G5(h
zVAP%oOIf_XQO`-$FD2%7@`n|aOx{(WQTf-?IqKF`mEwZc^=)NeOb8{kx?a+K|8;uG
zUu>h>eN4I7*_TV)cv@b)TS)w~Z(LiOBf@lrEpHa(0_4{k&LJ=$hp^TR@gMWcLr(e#
z7v<(d*Q^XC%i-XnND98lB5GjH+43-GO2OHy0b&Zki7@w$t)X!aVkU{+v0wnk00SBz
zhV%e2(jN_>!LcN4k`$d|__`WO6NwIlrMbA55}Nc$P7y_+H8yb-nd4AqkrAl17b5zi
z(^hHZ{i8(~n#>tgSUzn!D8<%jb6TuejOu;UrY5-;+~iY3FKM$lX#CoYTWW2nowzqP
z<c!EGcSwXey*LK=-=rcZkOjHIt@qym;M2H|4t=hax2B<AbEJ~-J?gp#sT7Nxb%GL&
z_R4seOP%Ypf-q5&JEu(RUV4m`g)RC<0_f`5l$wfgRrtLLi6|fZxqwdsWIR%MXrJQ-
zb24^f$LVb9Qo>YJF(O`o0s?>p0KfnPo&N-cLO=jWL-G#_e#7E0NOTe=0E)t&Q20yc
zB^-oDqS6T*C;$VKLZuR!OfpR?mP}zWd1Pi~C7Zz}6PLV_PdA;=B{6xW4t+JF&FHg<
zoPG%+08uH?3QXo-2%yfZ5Q%jDwO_A7o|RdRX2S`yS*4b$<Tia>t6XgNDb2EFFt|!?
z*Euu7r)a%a;Fa4>2IG0KOaS)SBo7C2z0+jTnq5YH8K`Aq^Jql&pAU`2G!;Dv0{te^
z*x;Gq1I8Zjhv~o$*nZMX?$^WK(E5Y!|8jp>I5KRn3<1W;Lb)6ZB(sY?<;=Lzem7R9
z)`E25{f<{}y#(KOd;EVNa_I>HdF0(1r;FX)_ri0YzZ|pK_UL_K9nR!u*#11Q3*LXg
zPgD%LJu8dul)-O|4unA<1S1Hq2tynILh#BE1VYg38iWBb+7JcA5h@7~#OOd2gaMH<
z6&0Y7fB_jsQ6dQ&01)JF5g?J&Fu?!-D-6Ct>;mr2tc;3;xU#9tn<A>xQoy>%5ImmZ
zH|k;#daRPVRLDqfOPrre(<_@ONXsnIFvw|yBJj;fBqKDglJpw$%gb7ftEN+&**;1$
z1Jsx`lhYG6$P3!`sV5X;EiF<Md>*~R^i-=q(r7~})jhP564Ie3q=he}%IoJzGiser
zP$V+Q0L;vZ^(>hlYa<agzEe77FG@&VFF;C?#Un8$^*o(eud1CLM@(`mV8YNAgo#_&
zH6^V&NokEtx5^FOac8BeO;2i9^~E(=Q4D;ia@V!Jz=AP%RDXF|t%R7%%e7UJPNMep
zUeTb9DlVvCN%K;NwT*HKdo<0fVxKlxPytRK2zw~~SV@cjS3K7|9Q982YvGZnF4O4u
z<j*Eh>%T1%-&Eyz{#2_yllDP;<~bYLV?TJtNakeuniBrNx*Q7tXD&1eoZQfUZH-r2
z6d9<!3A`f0LJ;-mt;f*RKd*ohL~{|v+ZN3hMNx7S8f`I*cmPKc#YeI18=Nm}U3<=*
zY|zxr?)6VMHY&KUdVOa+XBitc#M-=V%QIs+7bJvmbnfu;*RVeeA=0uR4FgWvqLtBa
zR4fh5+`H@#r|-6v-)nXJK1F5ORd*JXa;&cp)=@OaQD9GLjhKX0h$ThfAcsyZgD2I7
zGniA}4T-&S{B`N3t<o)cxoZd|{p)hr_rqcI3nfq5QMGrc%w3mHVQ|)(k6-Gsh*ifR
zcT~r~x_*DHa;aP6*it6RG?N4__fw1PB+JDjK*$cDim{bvkENGBHBfGi;{X5*6wj~~
zT*1g8xFCbbhr&h{L)EDSfP^84koG9@$-;MZEd=T^)jG&p1Bhl#83i)Na}8npJ~+qu
zeKeSC$w~9PNUWs%L}r-O;oCijtp$yq_=wD!Lo<sfaiztlNZ#NSO==N^tgj^8)|&Hl
zYjLFuwj_Lx8#*bDKmiuE;!N0^WOhbvy||Eu=GSBMT5{`2G(@C`LQ{b(kjNe!IT*<V
zBvblzE{$EoXKqYgG*M1YoVgtND<hl&=vmS|>PA(p=TrolTJm*$%0&j09sGxqMaD(D
z*;6Qs3;kLzN%201qRgBMk#}omLP<9XB_0t|ONvB*fPgmupa>)s2?c~gACLG*0rd=o
zzG4s9gd!;fi+~|<h^#aL8H-0?fH?FbNhOp3B=DD{ic<xZ%j5Ftl-4~lo6cs_h-`uj
zK8{ajlW0u>6C0w$=}<{+25Uy6No8}$O-^Atn$;+hs=PL}29Hu95P2N_T~x2iZ6F7Q
z4$CRBR_m85rPfVd00FEP`9-d!9s}MX7#Jn$32~WUpszTEBB4v8+op8r%i19if<&O1
zh<0B+Le0b;`S1hEA*O;m;uy#!wi~PfY$>1z#6kCsmquUPnRf0QzR7?%5RM(13Bcmr
zrqpgsCz~bUZlWCCk56$<>UH>84o6#$-tF?dQvR%`yWjM9eO(U+vTe_T`TcNON6U%e
z`F?*M2miOo$tFM(#;v{&Oa_v`Ogiwmu)qWuf`JGu8wSECBM7|02q+eUfiUDef<vev
z7=nRF^hXOskxB^?ppd8m4WN;L0D+;ga;&}~5K5|-u}buLAEhzW4*0+b@}nWHNn1v>
zFiOM*$EnE+K$^gy1cahTt_$vz$_`SD+NW|u5~4>@ymo?2vUHl|x04iyEX)p6uQ$AF
zMAo^=%f!sJ%+ivQ$}$dQ+NZ4OESA2=6T<F>&U8b?K}u>e1ctE-3S4zNNh&i<&@7V0
zH$hTl={8gg{Y5jTv^_OUxYPPkBd(Q&T~10Ve7iBn3dH+6)XS9)qf0gIg+$SCJouN=
zvkglmOx0C&UP};KJqD$4y^Cw3)=D(UPs%kvNU{vGLd>*`<;_8}lsh=oG^l~th&9cM
zT-aPd17|q4=q0}3xwM1k>^H9F17*r`+^HhHE|b#oy-aib^uD+j7X853B!eP1_U16>
zRhXO2fu$0wvyU=4CJg;#nM2h6;<)<hM&i$VB`W3!EOj*0FvfEVLRsuug2MUaV-J83
zyh9hDu{skL02)4-rbXI>Rh<A4bZM&3Q@tBwuA1c>lu@&a?O9cIB$B%8+Vr16*7a&-
zDzlIpSGMCgF5Q^h7__sTQA@`8yw3-&5mzQ#C7Ec*d8@@>%}eC%G%pv*6UA@4w(8*4
zm464rN8Ike$MSqcy^vd}ai6R1_W~Pz^t1(Q)$nVzwZJA#g@?RU=`Uw!Y}elD!aRKE
zF;1l|oc&yD`~<VcB`|G)I@q3#bH&jobqlWgQr8dW&g+c#=r5Mlsi1MZsuOKt$RXO7
zeLe!5&M6l?#?Jm~GdaxA^NVM}AfMvcYS)<%UaQp200$)Gm+KE-kST>R#sKBpTQV+*
zRf9m&<pqxd-eQNU1VE$g?iq9rdrY;4vIa!y2=pg%?2Pq6_ELBlizZ`d0hTfsV+asD
zDr^M3o2?@X&zhq`h%b^Kv-XCFA|p*hO(c&r#+=mJiWx<50jotN{^(uHyj2M0?7X!~
z@SZxhhVM+Q#wC=SlIm4+FyXJe_6-<ad$?>*Y`HbG?n>MIcX%#tF`THpT^4j~bI#p7
zM`s+?T!dM2(22~eCxsXz(FBfaG4i~mkt3MV4Im(ZGr*t-Bo+Gqg2G_XfCL^C0DeOu
z@JEa?BLs&-VUd_jawi>$M8I+R6p~FNlSkzenPj$GE02LCGYNDmGVqPgWYOq+@<}V7
zODEBq3@&>ilg#9>`K2anDFacVQn;-?oiBpZKozM~TD@GQRpb@vWr7J&f>x$hn#>k)
zU$B5Jb+|2}r)`2<>sD#B>J=A_QSd+q)LI#Zf?_b3d}Lygi^pU@4|rqZ81#lcb1=(z
z0q2j;!5+}f91-&jed-~P?0jcggV%sIHa(U<W3Jm{?3fvD&dI)q@Hd+r2I2{^+vD&Y
zjb5vd&EWJmeK`LL6T;(ddHuLoGELsz?x4O7ze~Zw^YpoXZO?K4;_h`Mz%P|Q(eRRe
zK5$SB7Uuu1unNwCfe;V?y8!Ss3W5PBEE@(Ouu2F8qHuTs1|YD)2?c-<kN^chh%7*Y
zAaLA5wnUIJwEZXQTCj)3it3n}uZa67@4pSAKBBO3lB|`*Ys6am$EuU8sYj9{Eh0CO
z)5jdDQe>qXERl?ay`ZsVu^FY&yR?!hl7y!eNN$|1F~{qQ5|zyBq{$#iQ)@LYFRYZE
z98SoblPS&-gk2v^a+H%Wq{cK#z&4Z`{X3!y^$9wqvP^3sDfFD^%q&SwB{fg!trWr2
z^902=Nwo7@MbA|_A1_c8r8`W?l(ffLPpi6JSvP8>Q94z#MSD_EGkmVHFNs?916VaB
zk6u$%-5SX=tW_S2+DK%*YR@c99O*Gq^p*fy^Oyl~G$>O_09^MyzSN+XWWMCSGK`+?
zJ1;9F`^l5t=IO}yjqbx=?&GlaJ(vyN_SeaE@b1-EJ_UqdxO2liOZT=Vi(=0M*N5Yn
zx>EPw(>4;6;c9%^gvF2Q^8aO!9$f>(`G#K)F3{vHo5Cn8D2Kw()GwIB(HH>^Xi+YQ
zn5B9$?5$Omb(yDt6KuC&B|4Q&iYQOT^=#JDOv#jKzy;lr-c<wfi0gS8hlFc8BA;<r
z^~xtLPBC?9s^4+lowsV5hD@6774?f3Zj+?hu-QCJk%pz0g+;ztQ}PLK@ml(8lizg;
zSwkR{M;4vg$@7Z8Qxt~Q>Q@{^U%tiki}AtM9d|*qAe}84+w40|^{#h(e_P7yt1ks#
zDzh(zsBZn9y;4(sj`^wCHLYrdo!x%yxYnz+m#(jjS9*l77Q=aJFV?-bhx|^>y2|~C
zCB<?v_e)9k>`rzidhEZurHg+V3+j?siS}&e`1F}3Hlz+C1vqdij5!0N1K46kW2cdQ
zu@*|+NSqgfZ|(}PNH+)As~>}hQOrQ-0+mnVS!Jcl6_;pSfuVT>W{kz0LnIu9S<)Fq
zOyq(zBshnV;v8trB!a{?>hqmbmLQE;!#Q_W_sE-pf)3&3DO9Nv-CNLC%7y?nrABVt
zqxm+Fo!%cObg>eWL2@lc>#KI~|A;8^i_W4nLP#9))%%BuN|HS~$ADI&k&=w@!ZO8J
zlJT3G!*9{Nk~rv~Nr(u5Er6g1Bo++)f59J+m@onn4Ti(v(8xS8B^8K7qVQPULOl%u
z$K+5+42}&YkV&KRhrCi%FNjJe5=iuFT^XCi;E~vj<}o6fM<>%cw8D8ml|HC*8Jz~1
zMy1T^QaW`4Jyoj8<kWdIqES40RBKf^?NYC2r+_QaTFrLRYPU@;Kmarp32e99V3esG
z@)vuHLmapn+%j8*!~h?48Z7G^{)S%w4~&d4?F@RuGLa0t6FLCT#Gf$CUMC?0)8nK1
zn3ktmIoDwT8kttoiBa2GAlsO3zfla>;&1yo&bArD-fy^@{$z`J1_42N`>sy|8P?|T
zcfF2w!@VQw#5-NB-*d<2^mca>m;H-%@A!1R)=rEN0|okkF7Qv^>I%A`kOJucz>o?G
z1HhmF5Q2d)WDf@-5NZeoprB$31wrT}FoHp7BoPiEu;>7Ru5lzrye@Cb#*s9sd{q=K
z3rnE4#cGmxt~shhZn7^)N(mRPX*_6;GEuYEoG-4s!1=0i!yhI|&wQfz#w?tel_e4c
zt1PcA)U@)+2^6NYN0Ow!Dl_kV&biBrO4lE*G4z_*Do&ddBSuPUO#IE@1dA#)ZY>0n
z!iXXTB%xFs#X2Zcy&AHB3_Ddv&=M5Lm(R4dD?mrFtsf^w^kl+=EHvDuQBib7F+C;`
z?K?_T&wDbMPm_$Wn#%GW9U#UO^2sMF71XyFRp^1@oYN^Z1wPr9BDUF7EK4MbT5Xji
z#>J@n8pgFOlQ%yy%))bTv+Vu97PM$2TH`sgbCm5TmxH+4*EQR>SjR7v!qm8TW0r=+
zuRGM!Jud1-{Lado!++aYEWw6fScI1GJNPywf4qn*hJ!g6E(MH8=xf`2WRj*6HGmJg
zApF0t>;;!ZFiu|#z;OZz2>=<x`h#Z~#4Lhm`OGhv!`dWHf<)R)oupT<#cg3mb9IF+
zRn66-iy+mVQ<Bd!g?TnldWD~f(im-3K;(($wTI6Xtj9{&wO+kBO}nJLlQ)$EHHxG8
zMSQ95I_CR0?zGhVlvp+UWwoQ&N>rY3JHEdv(_8bYMMp@1Tzw#gOi8P!`+qY@-rWUZ
zP3pC7o5{WQx}(JH+{JLj^j$v%vviu@aa*Kz{WO(w%T3*S_dEZA)y`e!%H1lwg@sbz
z4drV~#eIUmWJ{@qm!;Ty1ysiLEUl$zA%{1~igdL_*3vds{}_T>_e(*&-AHHUY|;45
z_Ss=~^OV{NdkJ1I5qds_Aip2t^B}ARj~&JQ+Z)6LCTuPUFO!7DSX+)_Fjb4N_%i~O
zNf2YOS&%R|I|kuFBV=yflCpA4%7|+$F32UAGiGZI&*~qB=!zXPgapqJfB<MM1)L$Y
z6wMJ6N@<ZT?zDF()1IQjPwjOoxQDWw+dH#f?&=m5SEk*VJ7OXZ`P)0VQu5cd8Y&TH
z5;*uIsN(4}e-VZ6Ll&Ouqg-f!B}L%8mzfI*RAg3<ajGK;3l$w4ZfuI4L@H>$@8mHv
zkWd{Z67g*3009AjDnOtB1NHv|gh62Nm{bxC42VQw@W`AdD+r6jp>UU^ZZRH+!T@p^
zBoap@lfxx48C;4>B#g?UGTD5-V=;?D;&X|#f*Bo%O(C;cBl>+gqEP0vDqSQ10;fV{
zGsq<xqZ*{sA}@M8&RtiSPpW_l)n>m>uu&?|%7mI<WwKf7R(XvIQz>^(s6cBy@*#ei
zL??DB))ue;eM0du2i_YIj+ol!_~=eI50;_6A#YjsY6$s-=YSdrCSygI$wKXRxeQNJ
zRiA6X8oZ=OSufk*wl&*M+f{_va6lT3{xgfS<Lxl}o<%2_y^!-dTfNi~`PAmoc=KL-
zBc~AMLp$IOzYD|X<oaa3IjjG9(D-w`_Sql?5%l^2As?tG@BqL-2tW{oFc0t#0zn8Q
z4uSzNN*M;hh(H*GLI4CNf<SOm5P`!G+&qFlkmBOEL<>6L`YvfgBNQ%6JXoNs(IT#~
z#*V^5&N{Aw;FP|R+wT~#E`r9SJWqQj@IC5`Ya}3z6qO{WuZ&vwE(lC}DXkGw!78Ki
z%A)2+5(|45q>|*MvnevXWil@D{9!k&D+0$bN)tTA703}v*te}Mbd5TI1+nyk67dox
zKuBtIj-^Yeqky7J)4bt5C6mn*pDU8pAx6lQ?Kw}d?dry%Q7>Hnq|~t*uAx&jl*>>k
zl3e3gDYVrhn$dKusTU>l45diN$b{KgF3NKDzM?DzDMrz38xF;>?lqpsC&!y=f?5uW
zeP}Iqgq(ylO&|eqp=~wF<ImO;yzE^xW6Z}qH1$n+(asd@cgHUaiF+Xz+p_dIakJv?
ztPf*+;a#sD6+mE)$`ggjk}ItBU-+gWi91)N?vg#&-T8GSue3{%fDiUf2S70D2?C)I
zTmJ`T%mgZg!mz9^0A|n@TMvK`$N>>VQ4CEtRZ%@HN$8BK^*m`2l&q&Ym6jhz*AxYH
zSLBuqvsYI1Jex#4`i-xQ#(M@MC%zR;YctX{9+^$!*LEE!?hb^>tL`$|Uu??}=7*4D
z*ly9h%?Txkrt5pcooh!sQX@j7wND+$(e=$>nAh8W*R5+@CfBv{)ekSKPp^7X(Q<p0
z<yq={2R}Y>8oyJiYt^S<hFj|c=-G5S<&C0PyN!X{>GOi?qw5G}`58^RcWWYA48jv=
zS;`wR>h-;15#{3^n~!fN*E2rQp?4E8>)jg-+4SNqmE%}jSk)WBb67qrfZq-qw|?T-
zqayy#gY-U4o%=rM0R7(SxP4E_{Jod5d0;`|f)Bn1uDBTjT<`#6uw=BdCboZ%qby}?
zp$9YM41U>y4rXXl77#{m&RJ7GXN>{iGsM)^-V26!iDDb9wn-0_Q^hS#kuO1I>a0od
z_9xBVAQUGL5L!xDhz(U#K)8;oU=&A+kYJKJhOH5p+)Qw!)*ifw#O<3)0&<QO#wWPO
z{auUCicsyqLzr(9o6K2@WYx^6r(Wx#X)lcDaZ|dYoIpT;AwZxABo++^f`H(!*Zd+C
z2!li5@W@0^0TzryVDG5ZLJt0p$Dp7o6gm?n07Rlu>0F{+FNi%PGHHZb4K{+vB(sUE
z&S5{7Pa<>Z^$K+dl1wGCT3n_I_n%W}6lm27i$tJF=~Mbh0*@Jr)*zJHltLw4vQOU@
zD$RCTW|UcLbjXFGwNA0!>h>5UatQmMU!wI~CI1V9gi-HtxYe#pU&GPy*vKR1HGqO-
z^4M(VBE5%?Vx<`fMq&>9$lEkC>ZT`ED639mRDC9X3AD@X=h|(?-*vu%Yj)L{c8iGy
z<45=WIlnKHz2#%=oSsDcq1D$xH5?8fL$}@Qa{CmGmu9onpTG6|e`FK`BZ7fG5I{G^
z3-$Va|4=|j+6V)`zyt6AKo80Y1fmc`4uXM*EEE6%NGu|PAaJ4!3V;y8?F%4~*Z~hB
zDMH-1CXXWX5}?ZKCbzDO;)dxbZ32GuArTx}yhO-4RJz2866BFKC_(D+HW8Fy_PMch
z#;Lt-jFBEm5&TlUA`LQK8%NSAVC2XyWS=ZWi4?&qMlnKvAH&kK=NwEgVudcr^NiTI
z#8aeCF-h^GJc~thA`XE#h?0dLsk3_yuri5!^n)r9yh9SqbG)T3Gt+F&pU}@U7bZ?g
z6&po0l?;_TPEp)zQccebgC@=ty5@t(bsbk(OVvDYQ$zFhA01P%%`H3E>dgr#M+lVr
zS*7T+1thq1LM<*(^;CgTBu%ZJ#;_>UHUKo%>vGQ8_Cr?}Itmoj?KLhXp-numY`WyT
z_Y1(zx%U0%d)bs)A$>cOjP&STcg5R!$m#|OCtv8(zVA8MYt4k+m+WUIqt8`)Uaa$5
z<bq>x6Zelm*#<=d<XG-W0pyT+4FSQhS_ziGkd!Z(!<Ee;KSNb?@kcK5#fJ{Zwhc3C
zt&~O$Cd(E*d7H$lZ6i6;w|!Y7Xqt_wCDRLKw`OAWnoFnY)kN!8(`_EFed3!K0DCQ)
z4ta~#+Fp04M-c{s<LG;Zdvoqmo||G=DhsEoYn9RqKqN_(UbM{1?XzSywq}i;SUhHJ
zEZ7^z3ByM9ji*{uo7S^+Z1)luChN(4aldl_29;K%yXRqK?L5yLO7!)l^{_7Z&uzZ*
ziG4noa^5D@IQ4z%@7dW_rLA<u^WCMg^vFXe=evltyw3Y}<?VTDS0k73``*t7E$F{J
zLGd9MjvV2)mY3r0I{nRS_5U4av53jf9s>PeqD^4yjsCDV)WXhO2!Jd~1+`cT=^sHb
zV$O(|9`=C++`AoPs16P>_&R^s`zB-Tkgl?H7=xif1VU`ZJE4Yh&0X_!h3={2w{-5$
zl_N=Qui64QS3tX56SpJI@uoK>-0N7o<Ri{{A0E~C^kRHUbZx=Z#5TnS912x-uF?KI
zNA7zXdb@v6+6p;%#IYQ+b~7bJh&Pte8CCO?jF92q#6q(966%8`1O$K`fS?E@77PP}
zLSaydBr+2YgTjChr~E1@1b)Rp59rip6&?UcBoQef0wEHM!{SnSY!+E2lFVdLs5Fja
zDwxaWvq&u3ZxWwR=u|kw@`m+|M<euTO$LcDr^{e~DZJ`kK&DgQbjo!`i(ac#skFzH
zCb?OnL#mc3eXgNzw#2DaD|G%}VVp^Bwd>8^uVRo{W>MTE66Xe#&~9`Z?i&k2#=>!U
z_%&_`mH=im5)9O9F^8;7bQS!~n;oRN=kk{;L@P&Q(Q9&6eLN-kv;b}Pv>nuMAHBKj
z@_Ox*lV5z_Y<XKftnX{W(@?q<?w=>E2H^B=d=A$~vEJ5kJY6m<7mpHu`FRvD3>Mw*
zf`K5QZ=co)^!xpQAAkTK^!Xr<@(BO{07@kRK&UJXf&pkO5CuWY_yB>Rke~p!LdbH0
zwL(gI_JyYK`ri>Gi}I$br>&D-$g(dwmk=m%A_~kzuX0xoqEVC9<|;8`)bcCI1YFo8
zvKmN`9?|;ngGWg!;+)4Z+TST8DrAimymF+Ph@nTqnGQ;d9G@pf(S$D)OwM$O<45w+
z_`D&>Ot`$Q(pqlCHcVvNo1jvHhZ;<)f{2v3U;*yjNOP2NKTPvY7eP_V8fO=(GlFix
zB(U7EGE6e8%^xssZ8sy&bc%&AQ<Y5AgG<#6*7Ha7RT)mwj{R#l&?t1rT~YBAuM?&<
zZB;ZOGTd)j*z*l1Pdex;6Du`sB-v?B@%;-xG>q#m$J<RUMO(`>+f?1W4Mm|>R?-A}
zbGpsrqL4e5q6v7}m!qpZI+ZQeX~?%Nn{zyM{r`d8I4xl3-Hyv^dpo!cRd!&=QrzmN
z_!12C)`%<HiBKza2>xUb)<XQiFZ?3`!B9R@f<bUjOP9Z>EHjx`Ijw;a(dqn}yhK$(
z?PSTaO~HaRH{GjKB$?d<Ip?{~myF=|UC@G`H`0H6Q#gf2o5*a8=bcv=<&m4Y3&w=V
zVfr2|DQUYD>8NaUZBw@HQLN0QTlQunxJo;Ym9kOwMD3~XHD0$JjS!?vyzBY2rkl`=
zeGf`eGWye}tX9Q~!07TMSAA!i<nzm3Tx6w9bi6kx+*niA;-}1fYQ?^9Tt0}4NnNLG
z*Pa*F>A^x%t&^nhoL+yob`~b>Wg}F?gxq=kPK!dh8uP;<TfU9GW>C9dOL6;*eyehC
zI_|XPsrtvLg5H;cnaw+FeiZ9|eO8R2;8Gv;e{=sW{J$I9dw*~FgfO-mz~7t$U#o@&
zJ9Kzc*uzj`Zf%>f=0tvgL>N9SoB%QQXvrCg3PB9E5<$cef*G0>anA{xkmod$9Jw)X
z@KIB@)rQ)U`_^!+71<Z1h}{+xJ%$d=)3s!z_nMRshmA%1KnRvhVSB?UjnW~*cXZX;
zO0S1)!YVm}-w<MvURco)w>uSj=%K_{c#ZlW#usYR9O~(NN|8>G5P%v0pa1|42?v5f
zp%9o{G8zJdzn}2<XaXe!eMG<y*wkhS`;9^)k@*Z(Jqv|OBC!Z;rV%NZLFJHGti~H7
zh0Y@LnJiXoKABJ;lezQOdH|WwrL?MKijO{`MQ0T{Jw^#Qr%<NU*?g{lORiXFkV-WU
zk65Ww>{hsKS~q8nTdY)=Ji>zxnn5OYy4)h^Y@b%_v<tOz_g}zXF*TcgUgL1bS+X?Q
z)N0Xe%hPSsi3F293(n(W*=&AdkD;CAa&Mh3isg6F+ig@V%p+g2$<FmLr{=d&8qn>x
znoO3*jk?HM_Vf;43b}6RZ1~!)Wml!k=<#{nZr>w|z2f(welEvftwQF~yEwQ1N_+v>
zU$y{9I2sNJ`u<>_XeZ(b`XGQh4*&oU-~fTYZG;d2Ku91GgTe3$4+g=AJQo5bFk242
zE~@ht4=Jnb;>12m;<mq~O{2~L9?l~)_bE-RI-x}h>^QI~5hOtoG|XET^q_~rj~+Bm
z+cM3@4$==J$tnbE8Le?dPNhhZ^0;})vO{qzB9SzA5vx+PpByL9+7~HA3WJjt%W~w#
z!c8nh%``^ygnIGC&Rk@+3$R?F8%Ht23cS#fLUxm<2|I%&xDJv}KvEM02|!A6RPRTk
zF>=K!%ae?~PROrQq^dG*+Zco}bp$^$Qc@zfJ*;!WZ&As$Rb^PQk^&bYOSGJ!Gf&g}
zbz4`eWd^29)QZ_oE0rT$jUG{3GgwzuJgsU^G;L<$(RSQZSgp+sy3sT*1)pzJ)XeWS
zIrhtnoLtG<e$U-D<67}vuHwUNO|=uz72h&5TXt6XCJl98PYcb3VU9iTfugTo35jAd
z&H%tYt7G9h#Iy(j0zl973jJhn>?e{y(7s57L2!&K2W0S;Wdp!+gf(B-Q>=*;HI^)?
zF<5GB-+<AVY<*Z~dHy+^Rgw&vl-3p{RaeMyjirm!xAk#^F*AO9GwD)xgO1?!2BWTM
zxGhzo=Q#3HwpCySc(vJ>^_9DAyN1=Q(#&4<yUqK4nQzVan<=aBvSQeo&$?|2#87k^
zXSvZf&;d+1v_~y#ZZxgGW40WN0m{pJKOL!O$Zf~FtW|}bf?3VnRn%>~2FJGa)eic>
z_cpCn*X<PrlflD1zT?_&Tbl!*@H+~9+^5$cfp@t(FAuX}cQ^s*H`SuqaPxaj%Xe#g
z?#bZqe!XwmINu%ZzVO$UAHL$2Z?mFbzs?fs^t#8=c~|@NVhl9$KQ<i0UbFyza44}q
zS7_c7sWT>QfcvtBNXU>{24yVO1d!HR%vmS^0Z?(8GdBc577IFbDtT|Lg2LTXE5>Y1
z7@!%H*#6*oN_nj;yeCLzot@M<YGtLzH#D^RPNSTCF(|~hSa%QIVv>IbE#8r+ZsMSP
zK8MV8&nsx0^CJV_dat47L^o3lVO%O#ttInBMQ;*YOjCv=5P*Py6+oZ}Bo++@0l`2}
zSWFTc0Efci(8#150R4Z#A5oA4?l~KaKcn%;#1cIglELBdh_C`BDvHD<viN+~TLyZ^
zAkvxSazzA`&FEA)l@g5#mdfcgx;+YmEuPTl@p&Zrt3#*I-xH~AP7N=XMX6C~eR`2i
zqgkjG%Uw3rXt+u$@Oyp6X9bhYAd^Xrf|)|G!Rt314f=<H!qG4`8g;Vcd7a$g)j3UW
z+d{YC;rKhX+bLGLK{NU+c5;cCj$U+e-38Mfk*Q>$w_TpTiG!)?>UWpsw^fec=_K0P
z{?<c|+Hv_+O^+`pw&82Jy)I``Z*Sy)J3F376OG!|_c}cOk8iu<uKD~N?@v$3ox5SB
z)_<H32?YZ|K_8Fg6Y_#TXd(vxKcD~(g1+b^4vRnF1P22^pi~fo!SGs&`oYV(CcUOE
zD-N_J=gXk@DDRSP#i|h+;OxYV>`cC|@03vvBFD5;@5QSl6rx4(Yf$zw@%u3mw9B++
zgT7G`hQ2AW>#WYmvKp}UM=BI#`Z197FD*V2QsXPfOB8s&w+$?BD#P;&0S!X(WRVWd
zkzxvYxDuiYl+IFG3V|u8iej5aki7uGBxoVv$0QA7F1*aMQxP^u^Yk}CK90(w>a}x3
z;TFsjS}jD&Exhc@whvpYJWS0D(6`gI4AoXsRW(alps}=DD8+S3k37Q+O(!=}RP}_L
z*Ad**qcAM8k0Z&<+|MV}le@iW+LnW=##ptBzf@IC(`5HWE{gKVT2DpILOODdogX!}
z<xG^fvI;qP+V1`7cRW<ZM_DxQRHE!(Z+--{U>2?iAXLvY%JpA3ZKs6c_&y<UVR%L*
zi^nd@<&DB`v<n8nAOstcps=<{0b{U)846^uEG-AY>3(AvP&M?oHdWTt(I;m%C7!gg
z)w3HTOmj>zoY}d?37^CkG_9Rn=}v`-UD@_697_?-lRZ*uHhHWr*3GS2Acwx0PDVP;
zyR}$%D)kbt^5r)-%vP+0ZEAat*%Q8&3mG?S_@(OM;To)5f{Czxm9^4#4IfFRG)>5j
zH7lJNea+fN=fBVSrTJAJS8a5-Nc|>@x8~d8n?~IoYgfZsyB2_gSxbhu!RGVrMJek2
zUe76Yy`JI5SF6Jv%<p{Op=I>_R@-CDo&N8)dU!Q!%z1W3-Kum>j>Sti+^xHStsQ2q
z5??pM=X<rkW0&`0cYYqS;y3OQAy=3ysf9Y1{6_wt;`u%7If}XX4FI3A)_^PR{V{_)
zh}hz4K_CJNvTy*%V6qKl$Q1{{G%P`&(=0%UDiI--0?gI>eNqoK<rimb0#AeVRqp-l
zwnv!P780#_FA>0yapMgkYjk%l9w5Uaj=P^T)JTt!bHmoW(}}y2Y4AL^L<LlJ;<Gz#
zDh16usIL%Vd_0N}?F+x>eAFFFPH+)zHnEpR@rWSwS4#3HkU|206F{H{Bo+(;gF!%W
zm?SP61%yN4kl+MS`u~DHqW}loZVeiOM_`e7Bd$FRltiI0IGjRL3YbA6FpxC986=m^
zAJMtQ8hbW`#V8awtpaBTicqD~Xrtm!Hl0)9Gm4CkHzKLjr|?PL61`u8&?40;ReH-=
zs8p-8N3}{*TcTR+_Ui<q&ohc%DRfw69_@9&SS<D$WTESHl}c{#Tb2f?YMx>tING!`
zC;-1;a5g-(YBy`DfG!f6#2YW6tXwkrdj_i|aF$E-`TD*>xwYHnKo8Bfi)jSjZmoPg
zj(2l))$eE9`;RLb%#`an_L;XQkEY)BfE-RgbGOXxbhz0*caOE$@%l4fxt?b+1p<6N
zP(VZ$YxMhmV1R$n1NeeIFaiqyp)d>lg0?Tb4u&=<oEn(D>1t%Xu}U-73%p7qX3N8D
zlrIae2fRHBLJT4j_b_QhP{%}TGQPbptNLvYtIZ2sf-SDAJnN_p97@2w%0w)$NGb#}
z^+WCSbqk*{YF_t6af~Gs$uT1fDJAS&uO&88bbTf&QY#X@uP}t82+FMkpn|lhD?=ov
zViRK-N|Kx_EImmA609o|(hWAn&}8W`FO)0Y1t@a-xiuk&oSdQ1le&pA%xEO%IyJLe
zZsNP`)d4O!Z%nG2M=PvpI?{1czYNCjT@>iG^m@N7P}4LaQl&A4P|ZV-{8JgMRt&Kt
z$F$?sSGsFM$dJ}jC52ZxjZHM1ytG?!P$d>5c?rpCC6R5~)?>|U(T&@B(6~1ZJ9X8u
zHDOU)4^w8ZJJLmx@YXi%<!auyyTO0Eww<+gsCUJ?4BxknS7yx;gXRE0&@<|SL0Fgp
z1wl|IGl#&?A`Ong7<*#_LG-lAD^|ATvmw-YTyV)(x274E=9S!km0j7qpG+(E`V$dZ
z(#2_7Qdex;=4jW2)kLyY{55CSF?tGmESf%Ln&6f!FQ;lV#&t92FufxPNwx;PJ0%(O
z&z9lR+)YYlDh^RzQ;fYnoNAiBY=TZ(c2TEjmQ4LNTKpZvm(kLlbzM?ZKH+z+tAyVV
z?0ZDF#$-G4Y{TJA)NO0jI?Xn~V0ooau;qI^y6RbPT}z){-D3q+aaFf1RB$~c!(T`W
z1#z8q8KukE+ZtA})kskVqq@t@?(fiF)*PKeV10$NV|q7L%XIWNO4h79-vz6+Uzvrc
z@?01<Thi@6FVlfw7hApgyFWJdec=DMMg6Q88`{bVn-O8`S&SdnHhvj00%DG30-!h;
z$r%%KB@C59wb&}{8LQPv%*5urcsx)WD^GPzIYKqYw0hwr7-(=|qP{kewO%86c(8p*
z76l09T}(4_4+-YAvVf=EL@9X>9w9+@RS97`1%?h%77$oM<5@dGiLc$OF2~~NVpFOh
zAb<{lpa>)u3<HBfKu}ln{u2*{Lm|-!Blaf-d_>~VN8C;u6^q3rP)I})7bJ+vByvy$
zo)avVOQMna96~n<o6RDTnXLAGCYsD55IB6se<GbtqfzQyazh}3QepFYI0B_rpFHW{
z31wnaFNjR2^?Ib%uN1P+?DVSjcE1O&(WtXobLO{VxzFu2$*u0)P?2Bdv>T;*u{6Ee
zD{yOE{yTuUPvzGvJvOZq$AB+$Jhn2oH_Yd%H+Ysy4UL~-bFf$Dewm}dYie;lmR_B(
z+W<2)d{wJSxZYOgnvGOHgSgIdxLCL^78Aqe@-(}ihXY^K=<azinRcU(+UfT6T>fWE
z4PU%q;DAUkv)2U#K|tRxq!Z@^1N<PgKnJ)0`=Kx6{QRg0Bm{z`kMk(>zl@s-l_XC~
z&he+L+8}$iaGW&_!;BIr^t~|h-iSm?)5@PjpaOWXK&#|i2SbblzMR7^Bh1Z2@rwwr
zLQxvpf}T;#bkxXD)KY^)5hNb&NNMa-7(ys2?9(^N8=WS}5!=-kOLBy>2utj%X52=R
zye#D)hqAVoAW2%n@X2l>w-vMwyw4OMtqgrGOe~`!oK0#x?$k>XS}36*@8tg;%Bd8B
z+%q#QaWhD&L{B@zu-rp8tIg_tLOH73)if&<#FUZC%Z)iMQ*mt%K~OTaV?a&FeHj-a
zH5{0quvIk4O3qcSX+BAF^2W<1l#PKJ)6C0jNh-B%3sBLtl|1pW3f-RT+h`+eY`WDX
zD-*i0wasAM(4(gBJ2zBKab0e`u<P8nt>t*pw{_EeUw4FWc1KBe1h2o?E9(E?@B{z^
z;P4~*_M#Yc4;JEe#1!-}%oLKm!gce(e<WD-r;)1>Os6Q;$~=!f-3qQXRkDy=5h2<R
zoh3ou?1BpnWEH#5pH%Rh(@<oV9!Hhc(}a1V%W`s$f#Z<<k6C0AZEs^j5nUIV<vL7m
z@wwKuE0Jq}1lo~LTBRp0PPC<d+vDl=q=P}S)cZZxwVlS1=!q3|k<~G3Sw(ERmffQ|
zc2?iG?3%5ql0_=kqg89N#lcU~_a!QWaka+{&vKRBS)i`5K7qb-e12U+XfqC-G4mPM
zQMFb5g#LqVAO`En?M>Z~*HoFTWXy9~UwJHJ%D;WvUyI$k-1+wZe}C+LFJp7vynDX!
zUzgXp*={$rN%1=u55t_&+3uC|<R0JG?=aejMT0%>wiElHm}l(#Vjrvm|6S7<K+p+X
zF}MQPodgJOW*OwYX7)l^<8f-uF=)4B_UYhDC||B2UMjZk+#viKC$3$2z6ad+AA&c7
z5LF+criO{fqp5PIoun0oZq?t5OoI^B`8#qT$=u@0c5IRxx<+8=Qw%SNZH0P0SXuxG
z2!IVhpa>)u4F`ll->>*2E)WBP!+;OCU;+IVgG8e6*sM|s8-&PUfGA8t5d)FJUeSmw
znhzv`OQcgde9~<<gvcioxJ<wSGJ?;g(uh3*5k8$nWs%54f;<3u#bgnxTq>(PsnzE-
zsZ8Qs5|Gh=6e;ZTcUhcLVHOJ95%E>ES*MfeHHyhclF((hsC|~@2Dr_wmFcC@^LT<v
z@RK|oisOyIS1fn?tkQ{wn9kx<`$Sr+jn3ohQQVe_Ka$K@tXT{Eg99|GY3+JACUa>M
z+uU@}eSXV5v)^QCyM7g0g9YC2_nb~fON$w**Xpm$F6XVj>TkDP&K;)V-|q7KJpOFY
zlXJdJV1P(3vIqu({5}64q!17F0DOQSe_R*%^#BiW0L#5FBD$ZwC^CHmD5xr)khLtT
z<kmbb+CuZZtfFrS!%*xv%&`Cg^5-zCJV_40Ogkq7rOEQ{kwfng0MIt^qsFB?&Z-F!
z!_h>1n?^9gaTun{6nz-RGBOJuLvYMe<E~9ilLX3(>wufNG88o=MN*RY9mMbiF3-FU
zQ)0(VGI|byL#L~62car_Z5gzU^yf7~Py|~q#uMb#B}y^+<t;H%+`5FQlgx=HM-Ee=
zJ4UYKjSVxYvy(-{6QX99P_z5fKvHzH$sDnhO&LK#5mh}9(Np76u}%=|&XCq=-Cn;z
z37s)qq)?<~N6i(~QBYAe#Pc($_Dx4KI`s@QSk(|aDAm-}B^=FJwgR1Iw$g>JrdR82
zzfD|o9lYYwu*|bE+|~`<U?9%T+}ByD1?74|vDC44CXU>1Z%bEgQ&v4N5`6t(55x5T
z;V-k=guqZ_UV~uM^V1hq(ye(fQ)~TwbXj;6`!LCtjiX)96;+KAyV*4RD@A#X_XFg)
zRZD;(vpbVtU)gq7pG6W4s)Odah6g6tR24wVDGIKe9^jdVIFVYD1^;p5^+iFcuGIZ)
zk;?f6!FA9mKwn~}_Jy{Y&QmKlNb2>xLz&l_JvpZ@`s)_}LfZySxn?Lf(T&8?yLQm$
zExMx)=XchDqt7*7Wo%`(cCUDFs69KMZ(61`1<>@*0LoUpHiyyc+DdPttlcjy4QqW5
z3DHGXMC#mh+Z`o2+7b2BOxMnq_Q7%7&9%4y7rzs8)b|c=ao-mwqq13-&8+fr{%^5z
zD;KIO<vUi-wdVSs-=p(io@Xn_bNGA@hQBB%5%)iTm+1e%SfUGwV$bYYt{B6#T1;gS
zy2m0yS>x$aX=#AC_#Da@)CX!H0DGE+#RH(4FMux1z_Vp~q?|+Qdyrl6s3;8IpJIc8
z&^`|`msJW~gFZ=+HK-;hQw^XTDr)Uzr830z03Ychb8O+<u2-868>tB(Ab<<Npa>)u
z4F`h0-|&a@ECC9FJ)zKOgdOn?hQS}Ph@2)n2#^2(P*5C#M+B0{V3KeIrVlKGOQsU2
zM7j+#noXdx$jlx$G=ooQ5~rMgTPBiD=`e|8>W3PmPiNo?lyad2p+RbN`2}VXTdvWo
zmO4b%%MYVdtbhvyGNVSe%B$A9jjn-eoI&PRcpaKoOOZ$|QW%wf#RaO?F!jsL?)@sf
z%x=@_w5scq!P2qx+|?fcZmZQH(OA3^D?!oaYIi+G8y}aP)~!1&j)sSs*IcoCt<3g&
z72iX(d%b2OcLw3;G+G`GDrf}bK_8a*MxUbx;`6tiy>3!z&E@!deXj?DyCJ#ud_I1^
ztI?JB-Mv?zq!*!|00E%Cs3-IC`yik?3Isq1Bn12*&$|MYGpW29z%MXUB?L39<S7fU
zN&{f^y6Fp&r@hRY41Bop6ilWpZNgs3uQ3cxkiM{E@e4t5szS9eu;aAkGOtSD;zY50
zg$}Io6XzkOi_6ByEb&r?$jK2jT_MIXTxBau5;Ld(9`XF5yU4J7FuW)cY_}dfk^^$v
zr~y*?p|LLtD-WWIGfMEzi1O_kLDIZnz)!Ht@Onrwv>{A=g<BJB-0l#N?k?$WMvv|q
zIl82~L6A@c5$PC>AX3sb8b)`+=#cJiCG_xK-*?V+{)gv&esu?*h7Y3sj0u0$g9Qd-
zdaA7g#&K;*;pBhkDTa=^=BarRI}UW!6N7OE5$3XTz2j?Q96E&k%JQAqK9XD&LVBrM
z+<weNO!HjKd{vUVlA%=w!@j@Lz?TrI86h6MPgaAPeQ-UUF%rRWRUd^;k5Qd9!K>v|
z93LgUd!kfPc>G)&gCIh3(*<;`f2i0Y0w4C15CTt!sQcfchW;95jx&?S*ERZQm9VCc
z9E#8Q5Vqv>n#9DwV3r@lh8V|zm?epEH;Bh?XH#y3?6v-9W?kRDpGB!5*SsSQb;eS5
zf57MODw9rkefQs+vSwNYl1$1>-bsQB9RyK~3)J-eN|kG4q@>fd1#+G}oJl$|u??38
zEOK*ObN2la^<0oN@I3RV<9^e2MHo%?7j=z8{d(3kS8>hu+7q$)n=bdDxPMiO>?r5(
z3;qz^Jg*0AEelhTJ*W5@@}24=_CG>R1qby-SUy_susLJx*A#koO_v!n)_Q|1ggSgz
zG}i(TO>dS=wqj4Sap<Q<I43llVv3rsej(G2u79g)^Jrgq6S8drUy$G1|BXg}(pHVi
z+3szL8<05cr~LE7)0+h7?K7DL#=UJwXMgby;5Bd5KiWo`r!8`4^nMljEW2pc*v!U?
zJ*rnaM%AAtL0{gE<amF1fBIb3YPi+3RQ}UHVXnIRZsd5~#{Yx1CRUGf&%_UTSa{*Z
z8mvUTJBP9&@OX=a#RxZbru>78mThT5QI#=;FGhHr*lH_Uqa2lNnw$1w7(RX|#(zBl
z$<x2E?iX%2@)-qMn0#JhbF2UnMsP2K_F}LJ2~Et<+dMWlTxABRifwYKLNpO4K+SI2
zzc0{%AKJ~L`-dx<o$RaP{zVpcYotib#6IvVQxB2APhI9&if3I{K)EbijWb-$?52Fq
zrH*4Qke9D6l!XX0f@ejfNX1nzP~r>138rx3@3Qv(JR}|B1Y3PWn&}+YwIf=gP<L&c
zs9h)Mayl@{J!p4EmR(`d9Xz<_bj3yb#wvEe=>jVD<Ss&yc>E(_$Zn!G^t=AmG7Yo$
zuHmAGC`7FzUAc2gTL9!Ta1Tw``&DkZ0=E#EJG_&jSjJQK5IOh%0n$vFY=aKHYonyK
zB!cdr2DMm}b;g8TXC8jk)@FXWeRdwayjvMMloysLzMAh}u`w6H!tT;-?CtKU6zZn#
z3)q*>)#ov1e}L5~A_Tqef-0pV&I%gY{Z8LNM4jAbTn!CguBHT1XEr4@P53ufw7myh
zmeeY^#I8;3UitFa@;VTeN9B>5ywkEP$|M$5{t7CpQasnNR^^(ovFCc7ly4Wx@v27;
zjWA5JQB|2&DEkEm<@Iq5<;P)5AY~ZCxkfAqnMpCZHEpoJrf(A{QO8s&E{20chMEVx
z8CBz)*jc<P=k1MTZyZVaDLVp;<!jVF>2Xj8-GoToAb{T2yec(07-;&~K=RKGd@-MY
z3#c$zYL6Y=)SBv%FfIwsoo=hEj%oX77u6=Ik4gb>F$v>xRS;9B3IeE=IIBVxg<Dgz
zaWrq0ArWJRM(N%&#|V!eM8HzD&{65S*LbJFA}K13j%8gu@<(_8aR)%g&mE0XosXF0
zi?t^g=79CM1r^0Sv7(;ss9*aw%dr1v|My^+;(F@#2GXsug7<Dtn)2-!t)5*A^Ucm$
zNxQ|lSxYg9XOH~d=TemuA+5B<2O+Ff15RNQe|m8&+TwjuMK9KkRv)8|-9K?5m1eFV
z!b>|T8xYxfdAI&b9d~_~9!$IpVj%tc*@7JVd4tE=Ww8{XzNtZedY%;p_`0mF#_fj#
zEbA1cx%rUN^L^Y!cy@g3JrR+0E43kKwLrR%6S-$Syl9dx^(>{mzNxjk%`1ByNo2SE
z%Xhmqt@g@^lT19*$=`mn4W904BNVfLO#2UW9b4h+<HbQZ=0uf}^98{-51>f6#k@W4
zgRazBRGF2B`Q_zJ)|($#Zg^?acfS9s3pQmJU9q06JD9M;-(Z<&bJG`d8Pv_M=@R=^
zoW!)G%`E70hWX>YC#?F-6h-sEZ(D4S+|+LmH$HmOz}E>Avk5RUe!Ta0VYeV;p+bB6
z3eUA-H=^q$-^(24sbD+2$2XsLluqT~tzAkkBZ`@@MJ(pQ#FqsVkjENgM+!yzZ)xzK
zJf+)m>c0;GH%#Fr$u?Gi@F{ZOZDKQ%E+HUZg+3vp{sa0eD1ezTgAEspGhvK5G1GTK
zpFI;?a3RVxZ-bf(-u5;Y43x)694sYPdFIfQ`aH%#_)ti2sF9`SG%F^=lBpbFz-k~a
z^r2r7w%A4T!Z9z>2WZ<tvmwzSji9CCxweYpZsE?g>L$}7%#SE}D?2w>UpVF>y4GK^
z$2MMFheJ(EuVc>0&^~qT@wc>(_k{mMsJoLKR1m6G6}xQKR*;U$TTPqY3rluH|7q*m
zbc2HS!fxA^4q(d_g&*TmKQDRLy(t973{Eclr+xbls>D9Jd8CRlL>q>82L%1%iA+v6
zKeenBH4Y`jx(oXN066kqq5lKmP2oOv#hPe*mx|qTJ!?{TQAX}Q5vbc?a8aMV@2URG
zR@>rI9*Gl*lOjvdt;3|TsZcQ!L|Ddo{2d$4S9kHdR|g0v7CHCvz7g>}VH#kXoP{(L
z@y?o()O_V&#%4}z#i3BD4R)jd!8?oJta9)q%{p*dkx4~Y(T1DZ&3Dy6sU&c`Ku}Xh
zh>894E3|691z5(^)u?jaddR19Vl<<CAuz{1r91jMuh}VlTCB7hIe{8p<4ZR)6O|pQ
zBv0JuN2rzjpi$nF!idA6LPVpe$SpVv2lgfHib(YxYA7_TosVYf%+?D54w_HxtW;V^
zn3DK=6ITmY=Y5HF>>!nju6+i#+Z;=(j+}PR!fBF*n936<Q+4c-bDccu;Bk7(ajqbc
zU-4}F521xg{hjj_fB7a%Nqn1ODGUEfzmKd2bJHJw5&L}#Q?34{_H31KM*Xj=Jf4wJ
zj*~|5<6@FVt?AG^(<GnavC?Dz$r&&+pB;s!NUEUW9H+o)J&!@=2E-oaWEW~)lx5~$
zdYQW|7R->rV-y={G6ww%xYL0___KT85x)xCoyEe5`KhmeYB=IFZxy4Tz=M<P<f0EP
zuOj+<sKj9ER=4;o6y?5lZn$xhYj9TGr+v?RB5}~M%1x47@ej&$f(_MHW$k_d_><4C
zM^@&6UOy>Lv-zyM7H?0_UbhG%ve!9=XH5nXRvpdTdTK@Kz6H%kRCSrOUdEGRBioQ1
zFGv53)bHym@y_zDO!I9{pmw)Nw-w*^cT&YJ)I_@rJ8L4W?_3Q~%ZqJ+)r`AyWK|de
z3msZjts6QY+b*Brct82Kn_~gbNaV3-vQ}_Ov=ipsHLj_((QYT+mP5XG2g6h=Pfl7~
znN=*_Ex%&8T+Ylh#lMnXf1?r(?yKb&n~R;IVLy0Z!wl^tLD?|L8VeiCiKYLthj`<&
zrdQ?W4g?aRQppj!Xh-Fd1F_HyVnUQ1w{SY{)n;S8_+EyD|6~#WQMo_LZq)o-<sd?N
znGN`cAcW{o5wiK0D)lCB011P(x_kgkRaU4hvw$Z6SCunOeNSMqzQ+zFU9Gs*Y3LOi
ze~_ZhbcP=$fkM*Y4p2<tm!f1bxa3N7wk}mE0>L8K3bqEBRm>M&M$xFLGvzLYro|A~
zmA@@E;vs6yX~fWnu9lO5>W@Z!0me+aEzh|aE-xZ@f)$|;_bQ1t=XGQ5JWJ3qBOm@%
z{YPv-)Ry&PRPh@wv=}RgRt>*-2Ef9g#pO6EDSh(znZF)r*CmK`YR6wBZa2u!eqt9$
zVRAYTs{OI%d?gj(5!W{StJTy6&n1FkeONH}O_NA>^Wpu4MaEyu>(zm`%-(Q1DrM3~
zr%t`|j-ajKmUvwoKqYVTx6~Hi$Btsrc%S_9rgCHlq|%WXo)dZe5!KhF7l?Gs{7E0K
ze+sS-TdR`i0w7$8L^B|1L*;cfnF!xER3g?y;N62V*Y)1tqC|DTugdc<l{-#CE`995
zLn<tmLG)F=bEb<O-Ht8}8z@(0If^mG0Ke-GRHg&Mvons$K&gdhp7K{ie0n}JZR$x^
zTp=15-;TQbDt3HlU?*HdTVogA$GlTBeKOy;%LTdu74d@&C8qg7BPTMPz$C(kq+=0g
z-nx8lG2Nt8%<GwYXndFItqqdqbj-7Dxd;i0^;*RQrWU>Y*esCy#UWW`uKpAp!PpiT
zyha6112YT{%$+(oDWxicwPtH0sIYow;VjdTYwXz5RlZ=8Uz5&my4NDLBzO+X<qZwY
z(>ymx>~grfYc5=bp93bP>0ZB@d7CEauUKxV@ffj=4<UHF?91$=Fh8e%NHyhI)a`7O
z7Tcz~K>CZ3&_Ey_47T|V_lPlEFt314QutQCR`QpU)!#`$$(ynKGyBg-zIkpd27><A
zI?1t#<~mjW&qP3%Obd;CfazWP!J3%polol4pL+g}`Umx6fHy#SC!rsPYbj@L6n>8^
zrkNxpm57-y`G1{tTf+nE$cTJ2V7Op{^xfo!#FM?2jtD>S1M#4iNH8_Y`z`k#>Fyp0
zh<lIL3D=seHDxY)k$r#?FmF4nLg=kQK%n)E5C!*_%yG5eaN!@CIOl0sa4j+?A5Nmc
zz-#RVr1nYGtRUR?9!e=U*Q8?(5vEIO#vxbtk?d8#2ecN>Un}6-OP-Fw+zJQR*RP*c
zC_ktpih_%E09I5@@`#%zTj=#eX?*+>ZFz@Dyv}L;JQ2WX<r;wF1Qlg1{AWVt?xRhh
z+iY(;O8SG-((TidF!6~jiHok5M?_40+8>PBo^M*GYfl7@<o9o|4Bm5xv`D98pl<e|
zg`fMzyo>{Y&==JI`G*oNOIzdlQvET$^wK5{F%i;`Mh@8Ee=RhS#7}t1@DyW8Q)Wm8
z>{Mr*WSR4%sBYDj(h+j<X8~A%QCtJk+yJ@lWR(a^W&A|-J>t?^4oR^Dou4;q$iIX%
z^X7JDvx=<YBDsriQ?ZDTy4kA{WjZFiS&>fZrq=hAsG3;V1-fjcD_~&^P;}S%o#$)J
zwTNOXZjn;vy<_GM>xw$(hJ3h7`i{%OAq^Dmd!HkVs8c;Y9x(b)hT@QBVudTrl>tQA
zApo-&HxM?&G^hh-%nb*St~G#s+J8APiHZN?)WNJY<qZLS5LYFtS_4V6u!3ZV3bs#g
zJ4x2xcU_n_uTcfd*X{v=p8uZx?4A4EB`ai-CusQaEX2iVG{M3H$~bSS0pY&f5(Uw3
z;ML{PLlWgK?Y(VL&^C1W#Lw7av{=2PxTPm*7EiruW+Ei8Dq^KWAaD6Fc~guFN3;AH
zqL)%3aZHD{Wdj+KG33gKgVvv5TDQf9TZ4q(!}B?v4CYC6$=RRSo=P*QyPU%<D^R8q
zBvI$nP2D}r72kuT+J>^iHk!E;r3%Xz{w-UkH&y);xWY|5Sf^2uHRWj*j@@wuJtYSa
zv_C73sfPzwPhZ&#N=hd0ILLGpALFS`s|uoFN?a9*-ZTNjT`0r|<?)2-Ny6olnmi$J
zCEW(WZs}%{SIWxMoS%7h<^}rg=LBzB(R(?BOjKGL!O&j|#>0M}^qNv{v!z#j=P0O~
zgen<+8qWE449m~i=C91)#m#vbYxhsfa9)pZzbYZ?3#eZ{-OVWCuJdvIqq*>6<neaF
z#fMnh<BkTPhfw#t>N(kMct1C-NbtVachn8i(=?_5Fw=2oj5j6d2Zzp1osK50PU)Mo
zDWuPBMrgEH_)+dQuz0-ftwUB+VjS1?VK7?e#zPemC1y_GIHM7`P^6TMi|zgT#VO6&
zT~kj-9tunnl+{nRX8Z0)e=;*?B%#>E-^^Xw%t>}+E;T#P=x6Q}NNrs1u>{1cQZ{vM
z<C`(Xm1i%ygtsMv-qTEW?r9s`=IK#B*Mw*`nXw$LJ!OY~)t={;$n|}sIyPJN+2XV)
z{8=0naKL@?YRZ$4IBZTZ7B8fE&G9zxi;0T;-ft&(HX@6+1>8Mxae3-L-EUZ^mEsUT
zYN!4l^ECnZY_U`Cq@S-z|M-gEemLazuKcj|ZFauIpBr|rm>KEk1=Yug1~|&UUj_L#
z?kE$IcXeBn;kqH57^Y059DC{N=7zgSdjc7>08*;d5VWFfF0y8ir6nU}9<M9*W-tm3
z7rRxW*h)`YiqR@mL2(k*qXA9kD{UFRqju3~*COdA?WIwmYYQ%(k@w{04G-U!l7yGP
zKr%G+|N9HBD6n2u14(=+GJxwZQk#hxg`^_b@qJs<GSpwdn2D-|H8RAxtla4uwg>=9
z6-Y2kkp=41*P$=<S<Q}!e3z6~4j_3*#9k@jpqa1B<n6elTTWCyWIdO0>fOTnZLC6I
z=-aZNR^sAj?L$N(rwYup${?$VN6|Gw+e3d@;!CG>UEjt#4Z2s3sCnQEQKLfVeT#P0
zBY#jF?tXf?IgEVxi3sgW{{e8B`Y)q03IZU+>#QV)5#R9AwT$PSiJ8vtRSO*dbvJij
zw>V54%ef9Gin6ZqS;95^IGE`rAQaQ@1MTfVw9I2Ej>L_g?b!=gp2hfQT)BI%W^5l(
z4FA@Z5;>jKlNOx(RAUL3&eFV-)YSd9A;G2I+|x2)i?ZEb_3-OK4tsg-6<rj}dA|(5
zAa=aHnke9*6?^NDpxgCCoj~E>cn!a~(pMPTKaKTfO|}+A9#t7zU8;)j?8Jwnhu_Bo
zFr<xbd#!vfNZ7?e5X}<U>J1uXSH!=rWf_>f2|j}z{Fv|VFBx5D3&jZ&1FFO!qE8Da
zI8>2k>QK-`<tj9Ya|O;)!Ue#?DNF24h*@d$5ri{&u*TQIhjmpiCRWkXx&o$VT{WAT
zj3#tu{89NSUl5Bbw#Fr~)F}=vM-^<1pR4f9kBnW<L&8pPRO`r1w*haL0U+2gYXxY;
z)!@bIo*lD<OnIm9G|5Ar(T~)LQJ6-%^@4$F%jl=E(TaY_j6xa-Ttael1Pqzd{lb;C
zG6RX2+yJ_?<Gsax%}PS3A;}iYGhfq~V3z6vmmczl4@?f$Pe#e=K5f|M7HPT|j}f&o
z-@*0McwVDAj=Ld_Tic49hbU2(OrD=_>5L@F+dRN5_Fz<Wf~{vhef%k^K@2;&Nu>co
zi?iP-DP*9TimUv|N-bV5%C1#y*M5(Ct}S-`YFTb_^kx_K_Qg}(qMhAXe~D^_0_Www
zEjd*#FbsTWoFd{&m~p%ukL_G(O^&<msg~l3YT~*mzEX3T(TVJ1SATWc>;=9_&N=06
zP|RObVa2dKG1`&OR5IJg8CuY9Bb<djlX(BVMnA`>64s+y5Ii*gYdonL{5L&rH%HUq
zXA91pQ<E-D@p;b{sQ7YhRoyk9@o+&gz5X<09qQLVFSP&OZ=s5Itng36J&i8`LyOU$
z;FpZ4`L=((H>)=yEw0^bbFhS`e-;>DbS+3$R%T49#~!H*m_D9lKqc<drp!mL?J6!y
zDoKh9lD9YPV>h5ws1-wr%hg^LioZKwTWn{+4!vuvR%L$vQZc%AH1|MS>>6}!HDUk$
z(&&rI{4dPq=r+(+d5BQH%5T<`6bg{xhxKn0H1E6vzDZO8=qyoxQKv&1X5#<7nrn`q
z(#?E1EMZNHmmt4hbGY-xi!*CvdzO;e`=e5vEUTCCIE%4x?!d2`BS}(zVA@C?l#-he
zW>8|pssD#0dR!%XeABmiRq|5j#g8KCXxD<?2->wX4d9NieOGQ$BX}%XfE8J+!0|5d
z4uHg3Wx;bl6md+O5{7iu2#QrBeTwMtxr4u<Eca^ZbI#CE80mVLH@^98o^wuAm8H$Q
zyxKw8-;n0XQEi@pW>^WG^J*LO7h!D%*!r8E><fVOb0EI>>cK>ffb-rbaSI`n+O!*=
z`$qHj@YJ~#W3iRoh<|g|t$o}%D{jgt`OR2FX%8L8D6jiuc-o>*M62rP7NrtWFb~)~
zfg86Sp7Qa3;Z?*`Xj~%|^Z@xY1)g@eMzZKAC*BxaY8(6YQ?roviQ`>~4mBrPS7LMo
za%oke?SKM4u`J4v7K%EyB2Nb36E{aR%g=$5`us38muSBJkz}sm47n3;IC34%O5UMz
zvTfD9Ak-|Zsse{-TckwWHj<}Gr(H}!U4!#faI9MJ+j?`|&bvBA1Y{F5er(f^(|IZ%
zqtLHq$}L}A6Z=mQe7+?yJ1lThwx>#Lx_CQ|XHu&XN?^kW0ID$aVzk%Qr-Z3Pla0=2
zz(VwrTJ<CmybxGL+Mwjs6QhN;TVXm#i;IQ=RcRWyLMH;H9JH66@3)PgNu|2QDZrz|
zAZWYWKq8pF_RS$Sy^qspg*1Zaf11wfH-I(fgfJ}4=W{Cc5MB?4G$u(n!=D!gZib1*
zgo^O6*WDW=IhOHn72_t8A`+yn$x=D0^%3Iq=0<b4{FckGwU|V0lA`P2&88}8pS`n2
zr+8D5Q(PAVzY17tsTT8(AG`X5aL*3WO1xr8|3?3`1=xV49oLkUTj6|3KMm?4VI_8r
zzGyKsY(Hycuta5%_`bSr$@a2#C{x%5{XitE>FaUUU3dLvg;W*5`e8tsB`?)p4dA68
zfaA&77G})ec00Gybf$6EOKl9xGHg}&b}#z-*XU1>rW*~Oe%UV2_AGLfhKitc_C%6n
z&k{6|S2j!3KOS@@g(*U?mcWYK=b`VRxBPBRbG+By!oP^Aays|X3oI)5qHV}|M^a@%
zzDN7bh5UQ^*+$2iP`OXQIg$g%TC9-OR=b4Z&WGmvZb|8XxL+qRKvyBWc4zJWc1VfP
z^Uy@LV^M*c7dfO*Xz2ee?qxR;niWWia3TP-H48g(RJdPz>8EQ@l&F8l3^M78ZeU`u
zBoEUihoaO`Bd4mrtr63x!s5#Y+v`yf2jb92b~E^ruRgpNFBtj`!IY#Fk<Ul4&xzK1
zGisLT+UZ5Dx|_4ssFmNAGpC7>)Ee2QiQhVxDl`o`*K+2F4|Am%a;LX^i@p@;G%ZaQ
z{~QK=2UF!Iaec$`KKR>$#~*y|d-gPd-P=$n5^oyq@?IMmEE%M0pP7Rj(Giw3nu1uf
z%f%<$Ns_ps6W5s$zdB3)A*Gv}tX=z$6)V&H%O|9?Ua)oRK*4W0@04zeJ*`&X%+K#~
z%pEf1in0Pncg}ye5K2`WIL)8CwWgK$@gP(lOj_X`>2LBcc*>x;YuuG9H<jWZVadJH
zN3JNhKWnHyp-AzLp1-Q}Eb~L@Ohl2m>6p|&@9)z}5yKJVbJ->AIorWqh2gotQF5n(
z^#}$=qu&$qPy6yLi%mWGV<=%jDBkpVnPIOeoJ1Sh7bjrFHVdJ<$WjkXF{M)U<DU=%
zfD_m2ye}t?)@$~&z^#i>9l^%BzT|JlV&mRDVq*N$*u%scz~D8~(WIv~^cEtUXLcEO
z$StvkFLt-Gw2#0rp>o2_v+xk4)pxTOYu2I)elpWKL6=G**H?z4J~fjs(3UC>#|b_K
zYs@&SGS1A>2%*eR9Dfa)(3!&!tAHv*(uES6HJPdq<zxayY867TB~!<-^jEMzQ(x;F
zY+y`v`m>!R3kx(ZPI^~Y=PpL65f=Fhf|aNYtCRhQenw6-yW;91qYr<6EmT51e!VL~
zc&g79J^I5%_0vCgPlZA4#EBy&S>N90FIia|xjASkU5ETMdn@dApJ+#%>J)6-Vxb?(
zlqP`1rWQ@&;I1~Fx<+CiwRyH*nR17$((3|w^xP)LO^`58z!i($?Rx!DV>1&KC)?zq
zD(JJ$k{xZyF!3wDXtRd;qf{Y?rX&j!#63F9@iI;DkJT{!ahO9^;U<HsL2g7L1f5$U
z4H7%bufvt9uILIE^4)*U2r|g&P)I$a4{@Y=4If{_658%irkr4$UVNIzX|zfUIdm+@
zWJQwPfOF8ek2E4ohdoHEposf)RuFbxHVb#!owz&M>%)7CNX&o?(xZgm4^6U3W@Nm6
z8$mQ-@f&~s0+SYO|2n`--b)p)FCZS%-^cxaicI*q{t-L<*Q!P5?z>~d_VtCprNEJ+
z?^JyBeJG*)fd55l_YVNN#0$y)L(h6*bFcwB+2;Xk{N~wrAiS4(I!}}<T8Jffgqe_n
zs}X|<IvPq?a`J-GSoro!3YrunZq5|rtT;2Nn_~VD;|;l;0Fl}cJsZ8>V<rV}N_1I#
z9XXFOl`D;04LDKL>_k70vk14&*;mSk3~(5NLgEpqQPW)WqmW`{fkFeDIko-|=~mrF
zwIa@QXHC3et8UP*)Py>z0DC}<SsyFEXpNk=q4=#*DPH@WwIMf6r@3bD)D6(gUMeyM
z;OtKpQiY%hNz>mmg~zq-ep=LeFJ5^<r2b;|ByJ#?a~;xnuy`z<GhZdN78byhGOp!N
z-M)7lL^9R{TR1%q=3RLJ1LG80CmE&8tIMaZx#JOOt1=0&w8JjE>~+*u)?RG5#0d&B
zpplCs|FQ7`^l&eWUw%{Da-mNBuUP%~6Glr-&v#@o9p@DsaRRC}H2Es|OSFONLUtNB
zfB7?d-dAFClAZ&Tl1rg8!k^^weBnL2eJA0&6lz`^?#FOd1uQbbOIz<HrOH0XmKyJy
zub2JP5NVta9i|oD3Bm%`6mG&g^xzhsvadbzGhEMtVlnkFV*Z&}(kXsj+duThdeoCx
ziYE$rbt**PPF4h>pnT)NugB8Fg6h!R?>t&t`Imw|)JeReNn1&@FC;b!VQqcnNNN$Z
z`qwS$E-)`{)l?{zm4SY^o3eItuM1O?z;%m-=(Sl6p8+Qhnv20q*^-{D4>ZVPU=}CB
z#^kOj#C+gjcF<Edt$z(wJaLy(%&b0w?rGSN5X)9Mof+mcV+fA?@XHsDV`C;5bo)UA
zdU~s*Y((Q?ZPgs%5DlJpEBBEV?Wwl}HjTAOH(;eHNAHrsio*Me71q;d=BJP(?L~wZ
z!`?K;{!6eT)<hN9nzx-4vJ{DbJ7|@09YplDB@LM6Na%)aZ%F~(`Mqtl3oY1RL?OsN
zG6HiF7#k<+xHDs~>>x9?Sz|N^kM(rBi(N^oU8zjm!IJkq=1c?3o4<A9ub<^S$L$(5
zM&?-E#JjznH)CP%JXlOUuH1S^UU&D%Ui;Cn+n$c}b(<&da7>a$0#xHA4x@1QhQ}1u
z_@$*W8(v8AR8NP}koi@4;qp@S-;jR)i0aVtc)ZJ&6R8vK3g7MY^@3N$th}=j@d>$l
z<3P1D*q8%f`~9FR{-U$t{lA6szY90Y8w}hpy$pCmDFS48(LFW<B|DT9G>IxQ+Q!u0
z^ccwlMFL;ha8-H3)b;=vDFgDtQ048qfE#xq{9N#h&ZdYk4kK@JUuUQp>6YT_gtI<n
zVHi}UXs7^YGWP~<S)k^s-^R2RLtUkwmk>B^9j6)%ITQ*)DI0xPZ@!!6vPO6)(z$TK
z8y(U5>;~64P6G)GTmU{Tg~Ip*{W(#Fm+FKA<_3hc!t{Km#V3Lkv8RtX2(XzRku9pU
z??kh4IgV>qEL_Q{v1VY^1s5Da+?`}`27O+QK@~e;EtOR_?m^Ey)s<B{1K*!BRM4C(
z`_JCPK<AN}>{Gr0x5n!}E1@S>PeG<=Bk=a4V8+~BW#IfD89dWrr0V#`v2O80%+ROu
zldFe)p7XlTPokdL+sJZ!jAF=e&-|29IN$Xn9$dYG>7R^t1>{dZ&*Mm2_au>fO0$-A
z>?cJR>)_7~+*ptkWv632;;s{4(acKiRlz~ovnNG*cGe0C(*EQ^Wls2IeFN$czbW>X
zB1XF=K%AhWo3OTwI1YPEyYAOfmF_oF#;y`38@_%hb;kxA5p+w)=2daaT<l|-F+LDK
zDvVtBl~Z7d#d`w~QigqV1^~_$p3)+SD<wsTk_7E^#!DD~P|U^#gTz#bkrRL_dJ+{s
z6o9AZ#$&$J#}ecEaUnd21~+C^HuvS&bH<dJD6_mqg=uC(Z&!q4|7gW`X<2Q!T4HA^
ziG$?m+!KL>KItmG_9O;a3)>{gOyaP`pwgekWMX`ie(7kp&8hajXgF~4s{fOx!PQ*h
zh3OR04;xbvV1k1IgmDtmL{42-O?t8%@dj2Fu4Sd4;fl&IRl?0nw&lnK{ixQA(imea
z6n^NjA=+N|)u7=uR+xQ8yi365!tSJ_Wy6tskinSHd9G%lQ2IN4y>0)(@g+i{LhA*^
z>*NE~90DAmuQ}xyUF)-{onXqcm<A%HL8?smjseSWFQAH`^>FepJC5*!*^k7rvT?#~
zKB4*ZoZY3OjE5tf+onv8G)jsu%9*&Q-iHS)^d)u~k~m`pro}jAb3y(8%p6)LmFKiM
z##4Wkul@?%n#hXV@33Dp*q!%~9DKjBw%X9oAtUuVYEl2bDb@4m;AHyjeL0$KzIV=~
z>(;{u<+v0}8s4_eypk6SvaXlvM~d_MgY-ti0ptx4N^q~$2D&W|6$wCLWBRMaJ9eA|
z6<Zm$n`8`@7r8kh1%%X)CqowCL!ptTD3Un9m}t~{CoA#2zo0rAdke%{@RCp80vEv3
zjGJ~sl%<jus8$%*rDpjNHYjI<4~b0l)9|=!4Nvt|PBkl?lP0+Q+E55?6<Zd#@7mT_
zmKZu$*+sGiDW`4wjCS!sLJ8!6!B6`Dr#w<gsW63Y0?}ELdz`jCYZFP*G-)}+k@IgZ
zMHdXO3YUwy9Gs7KR-Kl9eweTG<1|w|IH|ea>kw{hvhP}37YO6J+r66GXa>b4KX!H=
zw5;9sdeH2S^JHU5M`I$bx?Xg+OYQxc`L%IoiEGGuc$W08wA16~2izN}^Xq|IiLbz(
z+ivVv4_yV52xEXmwTnf+_)&S{bH@dh7jmns`rc#fR2!t|M~%1lUwOXRH_Zw|>?a5P
zoaMU|%=n)%FyNe@<q6tK4y_0K;^YEj>f_Y@o+&3nUaj(7_KY*h#X8s3HploXto7sG
zG+;{;NHp(B)e&<utJlVkEhzJ+xZlZ7y6RT5Uzov|n`dINXf%BoQC};lom}mYL+i6v
z`A>sr<?|}?(TC|$^zcU+D|&zof-~|`Z7LPJKsA-wov!TO6y#E!iCZQ;ByjTeB?n>#
zTVGV!Ph#S?0dEY-0a$m%HQ~)Q)gY%v8mbeyk{_48P(f`2vfb>Q?<VQIyHG01vT(&#
z_A)8F0s+RQ`U5fKPyLBIG5)msChu;f!E1QxByq1ZZQMotCz)*~UPpQk$b&8&V)ob4
z5H*gtnH2UcD{<exCTxD&Nb_M~F#w5nJ$(^L73jNbrKz9%gs@pkJWV38A}yAVUxEDP
z`mKpS*Hb<CUQu7)NV|&DZT`KxypC<g;|p<v{LU*$E#L_~+(EbXHsa94-7wok?617+
z0EEL!fi5<LMhQUd_mN?n?T7&(Tv^5s)AJ#XU^=)Tc;9re;tMR?Arf}+wKNotY&`e#
zmRCQ#bAJqUnqoP3^SoY*YSyE)U1`01+(7c$PoI14E;MZ7@A440<PY~*wR?AWZ5D|H
z*D^}eANDrnA%fZ;_U}qBsN$)G`#wf{jL0^ln(}yo?qbAEM0xrp?lV8A4>i$r?6$MP
zbHBY5IZm1XLE6U`q%CT{B0~8fZ*_6ts)RultMH>HfJ;-1EN-w%=eJ<I7)$s|pQ#Fo
z#$pRmvAt4#p?1GXm|!9pp4D}Gky|o*A7Re~F%=BjYX@qa1p_k14?UyJg}<v5O&#eu
z&%JIlPt##DVCR~Tv4YLyrBcGP5FX7MXTrx|lUlRyb3bdd*5H?VwMN`Ci!6kCzTMq#
z1I!GB(b2;`ZREN7#pjUz+@qV!s1ROqjXD6<qR)w+YjoK#rj5MD+!b6fJAHg<%wvMe
z&fYBt0MGT>!T<qOO@IG>3SPSSV~}-fYAm+q%(7}4Vd^BkX(4p`gp-W5v~yVTtcJDn
zapBbJpd+4U#s6&g$>!-_I-bqy!qP`ctW&**b7?od+3@xk<ETsN3;Cn(9T&FL%@UZ%
zL)-ObSc(6*_uJwCvYy!DO+9?Uub%~`xhOY!W8@0~z?fstY&vi&<rjgAGL|th9IOw&
zg|&Kn32O0)Lr8dPR|>FI@lF2WNhWi;k@b74y=PSO)F_}UnH1m3koUF~y;JdCzN+Xw
zgJcu`JQu9Q9l?<ZI9AY+(CBM(KYAHEVSN`vbg{|kBbvv)X#gNW5qc5qlrnEpuA<LN
z5nR0!dNY(n!YMFw9CWb6tDEnHfxthJ*Dgp!F2_XpF|G>;Puek}(#;8Sa|_Q=qak;B
za&-#o#z0`K^GT=gReR5T+c@V8z^SwdAT}x?9YWuMqhnMexK~dg^`ot^PikG*oG|4+
z){J()o#xoQ`FB6MlR0i&LtWx+^N|I)6(hPL!At(q>2^1zR*`y;1Lr~^8m);zc)H=Q
z-)=A2#su5%ElrI`x-qxvBp3J0bO$i_Y#L5W3C%=T5O2g9B^!io3<}rZ7N)G=xJ)dH
zuSqZyAy{*257@A{wCi<o6zXHlDJTyrCitVx>iRni8(W0Rlk`&VSao+H1M2p&??bdo
z&HU^>=l3`Ms&;q%y}b}_A<RPWzfKiboMngwc56Bzw}7>GGz>89Ze(|CmhHIxk`-U`
zyfX-!3exQM@C^Kj5$P1|H(AaD;=nPrdP2h)8l`fp&Dt~rw*+Yiw~D^thW)?h@TEe%
z;3hG{j1BM<DGwlEOOeAP`r7*<iP*6gp-q$p=u$AH@iHck00>J2>qK~<s`=VqXGtA|
zv&I~BG4!^4ICG|K>N%*lY>2;)Gh7F93rA`F&|o8^xXD%3O&dMBQA@RM(5^AEgADJu
z4>Jepx(b*md|~?6V6wGA`K)<roMi4&V+<)ndmm=mMNTJ3hK2H?R%#krH3B?G+T=~D
z_!-dRhS>UOYrGR*L2K5lxNXr$J}hc+cF9sj-rIhYXe{{dG&l{e{QY!irQBiyuOs1t
zuJzQA^w<RvDhTBM@ED<)bIzaYo`;<LxO%hklq=|wu6A};2%jWN@7+)Kl6=!?DIFKo
za{1Q)J-uwHdhNs84h__G$i8s3wbU~U5A<P_7w6I9ykhuKYDv};^YdRBt*dTiu2L)}
zxO@~=az}@w_#lQbei+7U_I_lPWZ8pJF*0F16pcM0zMh3wa?@jk;oLlB{uJ$mSiRUv
zBaEvX-P4THObjQm;N78Pq9sgaFYL0vU2bN<^Q^98oRo5PF>TD<wGt{jJXO8rczyiH
zk0N}IoG3kfh%)rvLAJ6F6Ll4fNW21<A*eEarB6SY$^^DVIOt%IjysjQmAnF3HR65(
zE>4Cb8TOl4>m05mxp4LA(-p|htjaSIfJq3<PW4l}aFerYB$fPZR-eHS+=(aXr`r#e
z{AATs8FZ>QUe^$U0-tvaoR#OP7vI(oMWwNZ%&OU#PV&V#77Y+0!>vpm*W%9*a|LdS
z)0*X*mU&RyTDvv_AB9Lm_9w_HW5uH+xe`R7D_s5^4X^*2q1(pdhAs&!<$@y@fvZM+
z6@{k_Ib14c>gpL`)Q~P7{@FYwjV#%Au_&B(<`3a_FN=G5WB*@}{?$`WqHnt(0MS5*
zrIt$j?=XQeubnX^Y#;iJsg~3qT)Wz&((g;B&jgd_7PuozH@)qY<;^nczH91SG5*~<
zC2^l=ktazF@!x2S{rF|>ksDg((i-Pm1`>}061wmRuR)b5zlfBtu3$=c?@do)MRz>j
zHYDwkeo+ka{C(Enx-EECP^CB5NP=?4-|t{)`c(v(Vh>&XRVWMK1v<p2sGp^2FO+Y1
z@Bv@C>9vP^85r@Sd$)B0W_3Zdi7K|b=sWQ^G~q9Em7_wHDQd8MVU4QU1V2^mkp0Xs
zcB_s=p0=%?(<*6{SoSarByyY~M!Zm~D3yc6z*4SEpB_N*RHkTLJ(pKqW**(Il_-5k
z>SFsYLA2S_>b6v3rWH_USS^EL?tS_ZAag<3jt{^=?d|3lb!j(AdfzTzM}eNJBmo%d
zbglW_@ovGj^)2HA#cMff$E0P!)Sxu+h?217iiK0xYeSll{UcJhh(VXmTDKd6QDst^
ztkJ`JNX~QEJ!ILZkeX{3RM`dfZOAmH)%^sAI#;fC+*|K%ENR;d#k$S!Gc0>NOF4cV
zT&p+@@}HG5`;hg(s0A~5`dPhYG|8x?_Uow09dq$tl{SMclDG$A{Z$ul>se}#cD~e~
zc9&Svggo_S!tsW&KJ5ZUPnVP*f+OW!7lNMkE<}>Ub<W4#30Mzh^$8q;w{85GVcXJ<
zOfozD$+l2hWz26#>`VfTtu-T@JHlbTQ;07MgJ*q?i(`B!l`Mmm8l%iT-hI5>>O9F0
z_k7TN%${1GQ35QMv214Pe8{WhK$xcquQ12VNvB8&!KyihL>z3KKQFR<lz1BE`xBe%
zrakr?G12P{Q;|U>uGJxa!Y<OepytM9tcrCFoAs7>&m<=lcy6p0_snh}R~KZIFkiol
zP|+aq-8Rlk6@SShnvP3X>`CRasBdvA%G)rWLCsXsSDB@kr$^W->yKQ8RC=hhJ+~<i
z3C_0g0&o>poDg-MNN$xV;aGvP1ZT)TbBz*s+8gt)DlwGqk&&eOo5B2oQW-X5Wrnu{
ztjx+unu1|Hn5gTbMoZk0=9WY`L4go5Qi8M*l7`d>>vTCqw{m|dS_=4Vd$p%0mM228
zR<9#2e2R_fxv*|p7b8fZ8CTMK_cq=yQl)8D&t&`Fz1mWvd4eEdXVF<1xE<&Av96hf
zM=8&Nq6*7;mB+@V4zf^WRc$Ovl-{tP2|}`9BCCH<8Mfpuxs<ICj<;IiMRYtEo)zwv
zdt|28B(4}K7WJP~3B0c=6_f(C*k_vG`V~rqAD7<<KWoJ}c#dBVY>8~Ay_6xQ|A(dT
z7b*}23M_!UH9vX^3q5YE!Zx2#g*%WYLH@grO1rl@UgC(=)O!e_F=y=Pwhz1D7g^lw
z@m*LA%8a>0o*a`exzAs%H#uM%y=aNOY~=!-7YOY<6d6{iTM7o&ca)M>br%bBqS_}Z
z!;JTRGaK@Y3}R$C-?jNQ%6q1ap5v=M-Cf&uYHcKfb{x<Fy_;QAW)`?uk*NyblQ7AV
zIiX>8TX>6KtkXqW_Rs36u+Q#g8})dmPbDT0?6a$O)gf<MqbkGJ{qCIOJ8jxFG_U7#
z`iVaJCZ5~)p5bB{w{*$z8i<c>(ZlzgD|OendUIM2HbCw0O7(>CLr7g?F60{Kbp88@
zKLHIiU7qJ9LoxYNuYFyjO8)O-OQi(%wV+d*V=RMe+wv3Fq&Vc6W3uA)pPpk5c`Qjj
zMDC+FR&sM^5sqALKArY;<!gmgVq+vW91S}_sauLGOfKODgx98`Kt@?`=@Rxw0cTY-
zSN3dWm~i0AI+Z1qPz|!#m$bvQsFcXqh6SoS@!e26U^1u>5`eC8bt14+fmO%Be5K>(
z#5-YX@jk_20M{T2N&XQ=A#nz@6_D%{0ax<LIJh48Mv9<7cdA)HLQ+WkTHzZ4JiNyI
zigEmyQvs29D|aKU6UQx7KqS~#A}b`p@lSnfB_LVx@+4dXqL+chV?g-8&tIeu0SlK4
zt|_z4o(D5hs>IeSTN~ZQHd*rVA-~I>Ed|p%4mDP6thEsmrlRT0Nu(_dvh^MP>(t-%
zQc^2feneg-(oe}r()euUHJ@5fA2QzrVkj8A3fFU8sa><k*KK+VU0Z6FGX<{%pgx_N
zWnh0d{e{zBu5Dwm^X_(`<_hj?kr8N`Xyki39=AQ)8vd49032KPi?2WX{em96Tw*d0
znX*qS8vF0d{)GgqP&?X!DwgfRfc2Xp$M|`=X=YoFnwe3ZNc76EW<bT3zA#M^C|geV
zubZi`=_h+qrO9tKEy8LEE+#$cVP^v2*KH?3xTmcxjihUe9FHT_HE;biEi+YuNJ?o=
z8yKIl+|ndZrcc_d39!A4D<8ux+t6^he3r{-{LQ2GjWNzL&J9IdR`!Ty#a=cJ4FA*5
zp@qG;aVht`NZURu&=ZKJXyTE?$XQK0_7Q^6J`QZ_91<rpGKYs22(&=zu(-oR?YuIv
za}5cD)L49)$9iz2qNZ4(%hXrWuM37Yuc9g}bX1`_Z1rV12Bp9f6=oui224HPyhXay
zktYVYbotb=kR#(BjZ6KUJLzZ<Y4+<nb60&%?z?I9It@O<rrCTe<S?YXW-9E;Ke)x4
zZ!@00i9LFZfv<(i=k^>dU@MIGe5k*o?<@Du4LNL&*sH3hz0xajtF~1ZWlv`%D<9p0
z)-M8#%NiV%D^t%7XHH%VKk`lbuq|PJG>r0_vbcMTbA~(EvkSTTCV65yTD^6}(*d%-
zFIWxhxR(k{!2M})x_<uM4Q{jdmo)+F^OFoZP@|;z&<uB8rZU>#zKyHA@L6ICc^$LI
zO>gEezJ`3?v6y?->}nHkyIPh4z6#*W4}24DtX|C^LlVX$CM5JuH`9ag{@5M4oPM2r
z2Qmh)u(5raN=8-Gq3N;sFey!E^mCD!;Ig7V@39;%1uZkT*HvXMY$wX9-?BN>zVf}^
z=R4peJ|FRxW96@VG2!E{yO8&eV!UFc;T?4fUwnWQZuv_y0wua>^Xe5eS6B#6_<qX^
zR#I@wpXSHVbljK}59`gs+2t9WT_fb^QsiK-)Fu%>z$GOqWO{?K%l-xszu;|7L!A^6
zUWfqPeOen=7p81Phmwt{sqgY^yVR)n$!TebStdpwf{JEkLc~?{dfpHA2zgoVDpDdy
z<08$Ag^QFGO&YgBM!K_ET(f%GfmpZ;H-V`xtNs#^_?E*Bs*-Q#`;qLHf#9gmQ>Ck@
zXsq!kT#aTTngpK|o6Qe7$DDJLh~5gBt~j=N^`H8y8X*3C<%X>x(b;>!p_=9zW;TKh
zKIQ>oy-V0ja+9(3C*u>DsXFo2EfSS(%?rdNSra%2>`iBmH%hI)NZuF*Zipz5mrb(o
zwYi_-J4?%o2nnM%uLyi8_1_n<rFR%y<W_7FN#?k>7$iA0kF07G-q$?A+k+9if$kpr
zj)!<yi*>1et7A-Wq*8gRGW)}akMCXnG@l3Mn#NhE?|M<Yv~gJuoK^ml<gP~Ncoq8Q
z|5hV8+R5J<EMz!e0rZ#Tz5?t7ANzmklyGS>5+x{G&yX`E6H$aoF(w5_)bRk)dhGfC
zm>KZ0q)#}!2*$6dq;fQ61SuruSvkIpu*|j;5k|>mOwkjI`?0uaLN%Fmk1BfTWy%1w
zFq0J0UV^$|=L-{k>jm@TRr-eV)OQH|52jtDOCh=6ExWX~NGW3}v5><67eWWjuVfix
zj5gA<4K%rfTXtCPOUKvl13R^S2sng}VBb0ycxu~!#pskzjpemG5nI}nhdG`>E7#;6
zC09KIAmAckWbBNKW6f&OIk>uP?48JmG@fnrHJ_s;BP~)!M6ZqOVn=n%c-^k1f8d1F
zJiGth+0JCeLeskE%I^<P(iU4?$I|NZuk!^RM%B(ldBaape_*v;&DDF@GBt|hXQ6L(
z*4P5VB=A5Z&n>u5g3YpEGc|zBq<3+Do{lcvA8`ypzEg&z@R`b%u`vQ_x#5-2IoZ-+
zH_DzeCOv#MohN0wflEZ%wz9X+^18$&Ve7C;G?wP-fB=^{u0+Bt4YT;cXGVJUkup}s
zD(%seBWaA_i7(w(3do?5YHEpRegHD@Jqv5uP*_VvaPufAN^ozd&r8Cfs!brp61iC~
zj7pe?aG70ku!b<KM~eU(4RwaIJP0Qg*EJI)j9*|V0*0xB-srk76Yfp>g;|_K{D?a(
z0Yqh~U>?D(DBKqOXG6T2hG&pTQ&jn(L9?%)lZtM#*|pYKs@$%InviV>pW*pqq*+G|
zxWVrS$<|3ecp(<yYo1O?WNX}royy;H77@oSPFjEHzHleAEwxZO{*P;f-0i#PU`dX;
zwaI<MYP4pB-#(?emlQB?kvxR_gkeO%=gWeDpS-rIhkV3))kO=vbe%tbS9vD>#G_v|
z^`TywJKA$xWfM)POC0vlDWuODDxd$^4nsJPikuB`4wWX@SmH7l1NJ!m?DRumK=m?Q
z$>N1SV?w%cBOIMHClGFwA!O+e>BA0rRh#FuDb4z)-3Ck4B1SUJPHHP}&C7JBI}5{l
z_cpjlo5h(FYEl)IHGP5J@a*x#ynn^o=#oCp*rxM-C;QYnFjzIfwK1-ir44SXH?j-o
zx=*{lV0FuNH#6|lo>O7N4a=horIz_`;ed_wVm+&M|HMIq2ax_zlaa!HNffSV#W(a>
zi61vk$>u^8n^YYuY0yr$S)v4$E@fmp2}7dJ4KqZ;YUWRSYmjvOxUKG2$b5uIt^&Kj
zA~Q=mC=blwF|08`s~o1!)?hsQjsWwgtSg_|6U+Cv+0*5<m<>v6MxQ(EL@;SMLNMz_
zPVSwUtJWmp8&>m1H2K-MNQ<>P%jy%XlyMqYr0w69S7h2}xJ3@$sk8e`X!99fppG;t
zL_69ntAxZNW%iy}fpF~ek2zbdPZlL-Des%coo)rOJE~m<m&bh;ei2i}v<!IACA3*2
zIB@Lm-_5ugJYdxqZh3a$_yLFEB{!l|f4QW=(#zgb+Lj(Zs-1^g|K3RFSm+)}osYI(
zcDe=COo>&)9+A2|T}&b+k7umV*s#Kn_T&-+FK&djkbU7D?ClH>AXS?=3~|0y3|yHZ
z`q+@XaCIykg^3fE&4H>_A{w}--X|KlsL|i!#Z&dJ*$V9Sm|gN5$zfIW%I)!;71dl)
z=gs^h##n<Tx((D~Gv6eZgb<W;A8IeTKJuxSru-D@oL2jNJghsWg$~xSrFgAY_Ic-0
z;)?n^wwU*c=dF5_8C0wZ{Tq_JdJH5qciNLHomLN>5ymin3`Ztya)XXV4yisf>xtK2
zL4o?dH#)-d7^fn_MJa*nC;*adMl(&KTe+_qh}D|-d7o$z3w59nlBwD;bGFvc2FW0>
zUPb>4MOL^3$sVGrPiCd9J6A0R@v^NX;Z9Su(J1=bepGGdz1wN2l$JB-H{)+@=q{~I
zl6!4-i)yK$B}kQ^Zi!$rs_VaubqHkEYY(_()cU~IuviN(aAW6;;C*8>X;&Zl%O^z{
zd;6Ybao5&4_R)RQz;8X(C_2Y0O`bG4|5(4)B19hS@`V{wM>Luhy73!KA!(!S?DARS
zeaJ^jZQKSbIR@`ER>Ar#9@{iKad<*FIPmt9p*0l~@%_mXBJtSS5H)Y!ZK?RDr9;Et
z>%csi0;iw3$iOGB6LI&NO~b%n4lQPtsegKQ$n1gri$UK&4o60~{dwCI&TfTX>F`^Y
zT`^wnHbf4QTFTLC)$Wi1aU@{53l4O7@8EkcJEu?h`b(1nRqu7^&El~>kBk1O{`z6o
z-%TICp_dv){$C?VjEwsE#p1cymi_O)FzI~!j1&$|oX?8e1mxcYXi4LHwso4vzNlg%
z$=Z5<<=g`krw=o~#rVdc$NEjv9#Ohu?ID?@zUL$QbY(6YK1v7u^UQ>#5iz-$%7Kz5
zYnY{eP#Z`+-~%b@c50v^N$#bN)!8G7nw>AF%3pDr{|ydB(o8n5ni}<+!T;UT<3Z|D
zK`DY90XV5I-M(s&8#3X(C;x>T@}XSLlPLPkhWC4}r3;@%p$#ZEXPbSYg4tm1XR?!3
zi}MGb^Uo!64S~zAdp3#IommZ=InO_RikZKn!%2U=-UmCq%*y@{9W}byb01h)|Boj2
zK=nA|(eAa^=E+6%GOwX7!9c8=<lH|&k)_jvr;Ti|9pc+w4g1XOGhcPD!i_-5D*ltC
zX4}ZD6uR@tX^#f50JTPi3y|BwhpCSzh;GYvq~Hc~{3N1nEwSBwO4ZlZ<@pH4r{_-I
zB>^TC0BM?%`Z&HR@%Hh*5zu9Fp<(6SZo?9;9(65k*7O+&qb<qq*c0<S3S8#L$-?64
ziyCvI;C91KY}N3}@(dbCIQfV@JUtR5Sb3iEorv$iltgs6!ev<C2az@P4!HKZ@XJI(
zdyK&U;pr_HqTt#tTvF*C5RmSUp}S)k8fNJ3ZfTM34oPW{?(Xg!8dM~t8$?3SJnwhT
z`49Wvcdm8Gt7AneNYti>k<bv9#$OF3=LJYq)p<cm1qm!Gx95oVFY;vg?k2JOmc_nR
zRa`VN9<Twd67-~X0qPP<j;+?xvuZ)!n3EZ__~2f5t0dypyUl8}g}GW|EX~UukAgKA
zL-BFP*C2iuHHa~b5?_XQlH<xv9I^wfDMHgz0H$LSuPGQdcMkQ$j^P+q8}j6!X(8Ma
zH2S@Rt4Li88*~HCzrM=%uP*ti(0xeP{iJYq#}lW&`t4n|`EQDlhfQKOXYu*!_kWOp
z*y8$ZE1!|PIi;ByiI3(s$R1;soQYYepucncLbcsFhVpHXC3uWC@G&sct#t*Gg4cx~
z=zA76W*W`*(HNLOE1Ro&-{v1&XljoPDQd&a8aQ{<I^7aeQ|((h5-MO^aUYwH_?u5=
zj1x=bW>;!5?4@Y@)mjCWp}pB2&ZL{im2hCw9}G}e!Tj*V)<b@;=i>vLeP7I3eGNZZ
zoA}uI17%suNj8af=XVP1fJG?aErq$$bHs7=VBYLj&%!g*wAsh-ZNP-M0pnWw&9XmI
z2=V_KF1Q2Xe{p7p`0_78c31r|oRNVzcGyxgTKf=4o-9jygANx7yor^wys>95AVJFn
z9Sop{&yiB*O8?@vcM`E?NuIYN)H8d7$)6K)bl1T^hAx*ix97Rui{u9gmbW8}hL14#
z%4KX?PEjQ=ttu5NTJ1QrteM-_4!zi%$8-gPc52#~cT)Ee+EnXO9hHz4%_Gh2Ar3CI
z>W;ngSH5V~-|jySeZPeo_6Q==^efx~^CErZC1bxi>j(M>0F;_x-uGe&r;?(>do-fC
z+vFsHJDQeWm|ldsCgInN`a(7zWd#<G3%dh<(Pb>foboShb0k^Q<gC%>zzro!A{&0U
zn^b=<s6RMebD=Z>fB7;H`Pc&GwEDGuSvcQ)GTY=*B8V`6iJ6!srIwo+Th$PVi_-1(
zo(Z%6s`?>K+N~RsAT=k)I5b#qXt7bEF3N;7gk9wn$f!xr+}oaHIroW1<4h}}zmkpt
zU6{v{pK`(lCXdUTR80J;j3*L+HCaF8^K*hFuCQ-#LfiLycioE$i%5NLa!ohSok1v7
z6G{xGO5K*ZAkQV;0e4~5zAWRY2j3;TqPq+^SK0Em#4U~cvd?M=!QN8D0>MlYP{Wh(
zJM$DloIL||6f8lZgV*aox#frfRd~Kd?B#(;`(7q;G<pSVSG#uEPp223uO+Yfc4k^}
zEe1KS6NJs77dwV^9==U~>kPh-On*75@42o~d)V|0xTTihN2Q<|o)<4M({DR{RnQ04
zB$z4@wdy*Id54Mfvq8Zrq&k&++1?N?)QKTeY&vHexeuG<d^>KiSAO`Yg2EYe--k)2
z=^4t$T<BBw2h7$~#z1Lw*CiWJe-A6s+$A{)d$&KKYAUDeZER@%^T7Z+_9ylVAZi6~
zN11-cGou5+?DMp?Z;5Vhf9_%Jg}Bi-o$%`P;|<5C#NAa3^neXz9~F~<r#X~S3lRwR
zhZBL|59Rb5sDqWD%|KF*;Z=!QWtg1+Bv<001wZOU^G)S~CVoqA!=-(};%6EG&iVcg
zjK9jjU+D!0JuZZA#tlIJ6!H-UWKY8-5Ph4h3G?j3k0IYW2tIV5=Z&*{aCk3K<nLr$
zMJfx*6K}4&hq?x+Xtl@-Z!w8mIMOQRXZaOhi9cW0{&tPb$!=&7w9os_Wq}kz1i#lu
zz(7Y08#3q0`mcfv?=NaZKmnl>$12cpJ?3eNV<af<YY7ELQZS~1EK`-`Vt!Mk&sYG_
z(^n<m<jm1~tczLrp=6BNq$tDak#Ro_GU@o${+HR*Dx0$9EW&_sBi|c^?;Sd26V|Ek
zG4e#bki%d#J~^do_2NY<c0P|@yw1)^#!NnJeC0*xO+$>$Pk?XAp9U5SZ=TNx783@+
z4XLr%Q@4Q;rdP0<Z$9GbGcu+*q<tTxnU)}SWS?ts34^0Hi#BTZoAv#>_kaSkwUU-S
z-(B7CUkj1hJ}*yj3X0nV#PMt^J3jTw5qkR%DYaf-aK8Og5O(Kng7X_GYhZVfJ<XvV
zjOeo$P$MBSGTojss@T(qyI*k;EphSWutSS8{CTi-jxKm((1EV>q>nocex;9Tc(+bB
zzMP9uUU$Zw(tmmLV~akSk9n8o(vbR5oNbG9zE2%f?`${`ql9c!v&E3O3P~#sZxm0?
z>`D4adt_49?oOX}ph+czu%Qv9h<VDj$thn}Gw=(?WYhc73n8ok+UAlIFLCP8rETzP
zVeHmeyy~(AbFWm2$}A=VhUYEMh^Kb)LD7<!-Cyoh5gO@!JZ&1031tdwmI3X(l{Yh*
zPM<iHQ_^EpCuXhb*h{_QTK;yj0Oaw3-t12);*In!QJT@E^Uzi0Rw^_dPh|hwCcpt9
zpSo+_f$YHDsIZ1abuZ`VMS!j_|Aum_t=W_#Eb}i9;fSB;lG8Sm`7#vwtGiLg48<Fn
z-?9UXQ(qa>%?u&6aJ6sa_Su9%R`^0Z6@J3F%;ZD_ovG1MZC1c)2EWD)&xgBTlG8H}
zxO@h7e-XL#mr7K;wbq?s&uf#gpdfSR*Jloin$}~!oq9_i!##axyyu;4<7_6)cbtL-
z2LO{Gsh<RYeJ5sAlMz8hr!da|-4&<yoJx_fo%nK|*&uOsG67v_6CXfyDumxUuKu9G
z`8IWBlGq%>%mvjZ-H?CmW5$8O!!Hae_6w%ZL;RScLd~1yzvP^qF4$6y|1@0}nL%;p
zOVk4r|5zQ1T>Pp727DWBx!&&m)v)WUjoWmS8pWW0vKH&;+9`Mtmv<+mkWJyVQ?LBN
zN9bTbRDv{;bLTE-Rq6Sn9XI%wZJ;_|)Ez0~Ji-Twx1^)z1~`*3px1QAw1d3!FQ@)u
zA9W^<-JCq&<9fXRKLn+?KU)d>*Tj9lTDt=y0%GNhc$mX^dEh67RvJgoKl$m>mG*8v
zgUR%<BE~IKCyk07xiY617SZFS@0mhXf8L=b5D?1b$xx?lY+q#K<cIvYTNjrg7fm1E
zyA=`B|3jR;_9IPXg`iWYpx-WoEFV%t-z@(X&mT|#RL<J5TT|KjDX-VA=!Um88~RK+
z{JrYxq($}(!q{?cM|U5swMVf@Q%8-L@b8#0TRsD!V2QMxXpK6zt+rHh@kVuyrxTn|
z)1-+X<{7HN>K~|A7H<A@A2#Ds&7U*mY|h#2QT&+l+9ah?&sdCdvI1_*a@m3=K0GI%
zJG<t$HXaInOQ<j$PXDYV{_QB3q`LI_Mc7E)C@yrhpZ(PTRrDx8=T`_iaQ(IYh;9d=
zFR}O<t$V(QeM9ELfvi>9m2NIYIN8!`sZmv?>+irUe3u~a7UnhH$zwq9NqY8%=(XIo
z8&=$(0G3D};jWga9?=wI9F=0;jCnW8eHKR`%R`1sD|IdNW3frIrsa1J&Jn^_6_4qG
zX9YEs4-DebFG9PDA;_&PGsiIcZz|_^#*{UQV)%?B+O(86^D1{NyaVniI%g_`xd-wp
zP7Z|p6B9wyKWEeA*+9y7^j*oye6!*lMK{(wxC~&i7zq(w3TZW|S_ZsKLF~IxE-f#P
zx6pauD6)E^uGp-x2)-_rIox1fq#rtccgd-;l6RJ@fvduLtGw(G{$;c2wjHwK0cd4W
zYN2QwgeE=XqDn4m|3atd@}6~AIom&q+OStm>~EenShOgG_V)D~pS5C}7!fUL1*m)U
zy;;;;3>YQXj5hQ7wCr=HFFAf!R^m1O6)WZ=1F;e!1&Hs3atV}{h#m*Rjjx%4LF%-n
zE%@Wxxy4A^*z)HSWTrw@eM)ywOoYNCL~Kdp1`+nW<#rQSbRKdeSOkd^vdmqpv8_#0
zdM6$QMZ`NlX=(foir?QNBP=uWGuB%C!GiNMS1)xlLVEV@ucWZWx^u6-g=o62(OFJl
z5bf4`322Izs1G(o*vP`&;FQ?s>GHzW7JbOhh`2xa%x~qNjq>q#!g{6W55S>yVGjOe
z>zo7*)?&i@jk?{5^NaD5t$)UC2YqkdLv$wE!otV9*j!#mHp_~-Mzh!awx5_ikl-%1
z|I2s(BW-l(y&=W?*uR@}l+MY55<N(Ht=A*L0|<xHaE^%*RWZ^)R!!O9Vtua9($?I^
z@#R3ez;Sv)`)vYy{*>vWJ605f0EI}^{dB)tJVeQK?J7LC_mm~LUxyh8Kj-t}3MP)*
zTu#)6dQ*jhjvAh7c;8A?Yy7x<lxM|tNM5{H&AGo>`&d6}>f=XoiOAjswqJ`mq#`t^
zQE`EysgASoPAJ#v4xr||<`0=SX1_>4)ICSR?oY-xwuTNtns@%&0_`f*Tgq)+F%c%D
z`jPv$c-{2FAB<bHgf%ZD4$Yf?8M`(~ME-gkRpDi=6FgV(F|*>Th^$j#Ufz0Pm)%%&
z254DvdD>2iOBHv#ney_xKgKq<{G>2yc>P&|C7^4Gr_O-#u&;g_6S;>glmv}#eknAp
z>k8<KB*Y1O*IlpvY>n8BI|Lh+QiUlMiY4qbPPEL=2$6DJwJ?7kxh78@8V8{uVm4eq
ziDR{<^jKo_W8HoQG%PgbObX2Angb>@_6^^4pvyI7cc|Y=+!rRYNptn$$vU+aIR?`1
z7NN_<;0fQd1Avk-0l8di4Gtlrqb{kWf~AYimv6`L0~>b58c#ppt7)VBQmnrUZ@Hwm
zJ$n{gCM<bYscd)!B9$ctNuq(S&at&&%_}5Aom|e;1Ix#OaAbx@tvxg3E--;w2iAN*
z-;m*z!Jz9M$e=4m1dx`V%_#n;sSKF$ofYxy71rM5Ua8#E)G&hDcBiw7eFd!y<F&4N
z?=%ZcC*M_;z_f?5>>1lyJI-}l!Nyt&?zJ;Z<2*Y?Nf7r}aZCPo-OT}GA;WwNn)Cd^
z6}~EOpD#TRn=~Ir+Keof_kQnWTPd(XF!7r2Z)1eboadx=4}M!3Q!Q>d^ZT1j7y;vV
z<BalJfBY6>2v)lGvD$^TsG+(7f3;~Km<h?#xyz=lF!)36+X_o&)DK%S&HlWW6&sTA
zXMf&EZne#uUoyTFIei+Vb6gfV^=yQSGA6YMtcOXOuZ!4}?gDea&8%5vI&x;3tF~Q(
z&+`C>SwH>RC<^6tt^LLv@)U7LRX<QN7q5A}nU*@~1;w^H2)79)xM-*|W<p%NC)Ru(
z)?}{WVo!{L|GRfWl-`wkr$h1gUxZ{a0$MsN6?WvP*>&Xz85c#oB0YgH3ZpU;f-F65
zWgeLhLYkrtPfVbNEozDq-R<Y?HcYB?nIoW`(>H0ZLN%sEGCia;>Y$Nd=^&y^OojB7
zQw`->7hJtmg^IL}+=rZ(nB|}8n-p5jD4%B>+c7v;i5|<>&$AqzA6^4f4XV7zQ4)WB
zj54`wbEpl*jH)pz^<#z*qmvD4yJvQ^f^xG*z(pOt3S_-5VG{1zSZ@erDF%OeKMT#(
z{d351rqQLqG1O0hS>C;{*R6V2_bLep<-?+!gEOt~1J38Ea@W?5MM-uZWO{qrmbe7+
z9;SuWZq9_CtCI-j2X@!^cRK0&OA-(K_n#A7bB12|YL{D1#g%0<Ped=5OeR{K^2pLf
z>r3(e2sMP|6bQ4NNUcb66^9;_;O>>-%2bN>bV*>eFux$65wVw)IwQ{|Noow`Jda@R
z%M&n6*XUgJ@mN;<9#-F4`XxC@K1exwEJhou=wjr_ER4I<h}eEJlg-|gN)-BU#KjCD
zpeGH>nx(eE3W`vB>SRqO+qh@Sr=TL|$F8R3sY)<%ZPmy*n<~3dP6OWNP2eH`Us9>4
zdn3PXM2}xoY}YPDC>&0ujZ!XZ+_DwpKfkMg%hUQE6ejGxLr7Y2*Yk60WyP``ePl7g
zQmi!9*zJOu+n5GKe+fXBUWL%{&~>Z~Z-zp(jl#557Krx0YbP_H;*GV&1%W$&7*Xm_
zPv=h@v${1OIZ9Il8fx@gRz7l-daI1Hs_`bcVy+4{dp)lco=^0vA2BO^+VKJJBJC27
zn)=SPV`(eE?oTjgx7q=_$iUxuaTE=wy3$1Ini0q0KUUgEY(%^nW+gSVbj{7zZw2@r
z_Jyq;`&ExPpkd3GWrMC(i;R*{2EryE1vcN+er4VFi<<}T@oztu{(Nlp4K>YSh*JIb
z)U}mkn^YbduQqXk9JNkX{2XMJO>WzuTYd2(*{Qxxw@y&+VUOAos>YCPOjgBxWh?6O
zr$U!3J}&lR@%5GZ`XSx7#@6liwp?|}Wvri#v^+`dM%aW!!A<Nfs-jm~W>l8f(~f9@
zbFKw;&3Jkn&*aW4AJo9B5#EcTBm6I%-iFJ-ZIf00{6qUVyeoy~rN%-KA!o6um%GV>
zo*-Y;_$bGVhL|E<y5MJ2tc@8uU6!0}L}tyJHMSpujzuOc5u<Jn^yR=%M-PU(5*bhs
zXP|zWVDcuNMXE+ESGI4h5_dz?4_jot0`ZxkzfT^g(qxmD@YJbaJ#=>jBu+!ZcFw%F
zwqx(nIux7--p=7<(RO`z^1(#eY>XZs;t?j#Q%BY)+yV*&Zc_@oC2wBqvj9R*%a>N%
zyTq+3xI|V@3hR3){+iaWU)YkUs-<$ocUrV<#YK{M`=(!AZTmv%{>=7Wus-N#ZUVd)
zJrPKS=SBABr@T+vj8WD3t#0MSc_e<vedG!6XY%*2_axb@_bUC+2faOfp}tK0c2L3D
z9Qv(>TlRguv{SWDR^Pm1;<J=SsNG&q4f5(BFi8g?MN(bvEp9oQOzJoBYZ;S_LM7Q|
z0w5j?W3U~UZZ2kEF>U=(w4r;DsN4OP4P)@aVvB-${>&$@Z!TPyK{oylZ5rejU{Be;
z;5kew$g7jYBBx6B116m;`m)Z&oN_fd*|Z%JAr!Oi$t}ovxA`K3uX7eUa$~^wtgP80
zQlM}H7N`)^h@ZI}1O^iIbu32UHH&Jbf9?m>@6hk4+tJ5xDzzBP^bYfuimNUqg6+Zc
zXyAH<6@ad#>fG&JKJ`q~tX0^ohdkpx^g7-h0s-`pYpxN%bk>(<$)I>4II~5<>i}m<
zJE%jiBQInH<#DjAHubkbXX3r6HCQ_XS9*${Rhwj(Apfg=IpiQgQ%UOPt^V4YH4h(%
zZN!lu<U9oR-1;mR{-k(k6+gGx!iY%2z+dyU{==P;Ay{9qk6diM)~PpIPZ`^!`Ufm1
z%j|n|IdULW;KDHa+lL!Z^))r|TzOmJgmXbFezDzD4Ug9ybB)P4^$g1{)TCJ6cl+Hk
zzcg~>eZfy@h6Q#)KIZj0Sr8`fMxD3{@AUwReEDB+d3`&r{|I~UuKsDOw5qVnb=J1O
z4XK`s-re8Tjh3AX3q#CHjMYsD!*u1L!Nrz>q2u{v)#8LgR=3lig6}yFxeT_$7w_TU
z(d`FC)^K<pdi%ezAspt~XL=QFhWHONGK6XoEuNSOEo#)_`W@U}_)X%l<+WbA);pxo
zDcV9(Ft0vJhMe^e^msB$V6L<+!8+Z8Dt(;#kL!*1w1PCLYDf6`wVe5HiWiQwm?LPv
zK(l12GDK!*bJ=R-9TrajiA6>U2Jm^v%M__i;ieO$Vy9~BO_}t0D=8|*AJh&#JCZ!W
z$(Qf~>1#rCaUWcX3e!CjxE={!?i@6<#yKBeinRU5JOG}oSWeort+Sg{mDR1=ef@mI
z?}A^Ox9{~(t5#uI-=y8ZYaIE3yuH?-q5F&k_hZDi`}z8uqU0yraQmj`nYokvFAWYa
z!n2vKgR7}J=Eg2I^NXPm&*#BHu3yAkM)rY0lq9UF3tct9BRFKzF|x85E4SvH#l_R&
zv2Xr)dz9tWj`LHWhUiPEG|tb%64A;yt$<qf9;DnxPT&Xvqq`gwAO#xMMi~KD<^5%G
z6(Q>yzOL~<Q=F$$q~OChd;^E*>4X+aC#6l(SL2Wyq78YVU?bIDZ5h1^UXPeP4Qa-g
zSM+hnIsGV_2p7duUTQ&nKLd0EvMVpqBmJJN2StcVXunSonms|W#Z~rnx;dXs?0WZD
zNl@|&1Jn@MQa!kwichJA--}G!x)fg5si})fwko3J<hC6`B$5TM?3~{`Xn8wx$P{6>
za&Rr+lvMCQbd6H<=I+l_jaPKa(E4D6Kt<4C8?qo8lrNOFZFL1mfjQ`@{XtWO54^*m
zM0AVDq31YENZcsM2lb-Pq;NN8#$77m??_F^_lA6+=yc=cFXO#sxf|(PGEzUkSz`3%
z$VD9W1q1||L*y2!M6aqXxg-f+!;IA@5_F;8I?@7wRd+U2YT$Quq+zX#bUkavcfoIi
zt?w1|)1l2jZ==`+Y@}nApbreLvx46ct`CKqlndxmS5*tXJ<f&EEeSe*EU~k**0r&V
z=S;IZ;M!7_;MafpV$n;^z>o{`-T6@Fyy#_7XrIYDRJphnuAg?YYMd0LB%m!==V*|)
z(dYakbCV6<3!_N=f0@J__ty(6+P}!*A~?xMh6F!MZg<g6%t06<LAvBx3i*Nu9Z_0}
zTOUjSMadjIfCt2!C4VNUjIdH}3|k^q$*hGrP$7oCc!@SmL{}jFF>Q$64I~y@0;-y`
zU&yh#Xg5WGIst^vO;W5Hq*;lx>BpL#I%F&1+%yqeyP}Q4HWgo_XUPhq-6aQ;H>Vhl
zs%)Pf6CQ*W$G=sjkdJ8nb56A|?PSXs7bhv-YR)#~3^tD|I4*jZPfvQE7iQsY@P5u>
zuqoBil}DA)R6BEV<=oBp@V9X2`nlIr*9x1v>W<ZeTS1b@(#yI!i&297$LhY=D<?_}
z1<Wr#V_K~bI0u6Bkn%qwewVIuz>NkU@%3QH2aH47r0Ucak^1i7JE<!&oTuI+w$+5@
zcu(f)CfuP$Oo@eQr~01DYJpg4jpfoP=_@`O{QB4)t*<hyd_33<m?fDFoef&<q+@3~
z)~MZ@@xG5$XLK<nf+_w#yC<ga3~5{NMf&;%)K37+g(>WEV?%Q<sq-ybJslV0OGH70
z;KWkqYfdb1<*7h2c#v$oF?6Y~!-Y?u8gy$!R6DPAJIFaXp&i9Ffd8_=#ln4nFRam&
zvotcbWW}YdX$KMq&Rt@DLV$}rW`!hABj}{sc~uTbo3EUFZoaPQM2WG$t@L=nFo%aZ
z-4#n+qyB`hd=9Nv3JTD?cW;c^3b8!ai0ZAkOerLV41lzuP2P$##}<FiB<~I15`R^P
z;j2n`|J+mG7(gKYGy+9oAkrqjo0cmjpaTzT5V!asc7AOY4~F5FE!FPa=(2Dr<G#;g
z((vX;(rxp+po~%BZXH-t@y*9IFV{WuTG}Vh-%C+?yxG8A)T3y&-k<V###sK48Wum>
z3F=R`N-E$o{kc)DAFVGC*mJTe^q2pWsJ9c)Oy_ex7h@j+;%!Q=fR!!3^#?w=lg-S`
zs(IZt;@Xnq>WG0R{){o&jZ+A*VT4mwAAie*@M!Lrz0SIEV_R)`Z3TPcv5*c2mxw)=
znm_kR;oEHti#_l(f%Jdv3cSAhUsSwDh?JqySU~>Ef*&?om=ewBfk_snSeS}-rbUH@
zDD(eCMF=Cey)VLB_Rv`y&+L&QKZ$Tvv*k`PMtRb>QK}{fcH$(}v`PBw*mRd{w5k!-
ztCLxoUe@}x%1n|`mtouFjp~&x`L-R`D2*Zk(F`P9o}eX1;;PmQlE8Mo!z?U7s3ud_
z&i<o8ghak!&B*TKNrWw-^O#}N^$Th{i(ASs_lFOM$bvo*_IG3*5L<oR6^+v^a=9=D
z)%h)k&8L~ruZG{hpZ&>({R4Cto}Rboo(p>0?`{fw_K#A}m|L_A;11$)tp8rH?frD}
z@Nsah?9Z$5M<!Z|kwyy%@h8#m?WSEukKeK7hiB%5l&V9{J1-69a=NP=9FeMAyYKdk
zj$<8=s2%p-uL`m)B_Ebn-xgsPUOrcbb<}%UJcQu0Yh8_k!D_=o=feoUMl`WF&qNo9
zkRsT4s?XNNoO9O52WE{NDV5@6U1=L!;%VNss*oj-@#2y9{pDs>`TV<2BfhI$I{9bu
zfYOl)2X*(kP8LtiEh`oGWNLgrqbizLJMAbo_?SXLI{^F*ROjR$t6>*NP(Rub2>CmC
zp;I|EcI#nv%z-br+CJ!^vK^*IxY?@CaWgEf0t0Z666ma`Se+DcfpUpNW*y>d%ekkS
zR8`WQ^seT0Ma7Y5<JiR_v_NcM^p=_kW!l$U_Y(BQ7fN5XR$-5bT0-0}DHH>kSVpfK
z_nYV#i(YxEZrZMxxA>K9wb28*Yfd)jTij=VJ`C`kVET?>qngven)?tMHrkAJZZ`np
z{a!l>@1??2yf`kA%?0^;M(4bA1Gr5xk*GeI-A)fGn`#q$PwmwaA2v0+m(`DL#*8%E
zYydEbE6!)tQKWOUfE59jUS1RDZMR{PAFD_KD&S>8ez)5afRAQ@R_YtW>J5=5iVR1r
zRUU}FfKIau=bE=oB^5jBS{8^%yOzjlLx8D?-$x9a$TX9D+Q`|%0Y3N&kqj|IKz{v)
z5-uZ!@J@~lfDtMCQ$tTojs0)z7<KZIjspuZf?~1z1UQx)kSzO?yO4+$$`K0M1)>>I
zqjRQB6%i^2f~-aJKo;7{{1}4FC0ce3U+K%mRYSFp@t%x9Y3RYQUG=UN6MdF1Yc&v2
z`owOv<|PMWLo0@R`)J+6bT%|3KiMxJXLpenAGUzD(YhA4eABm5QETkJ25OYjAytsZ
zRPx9-8)?(fukb@Nt(A-x8!x?hz@B17;i3D$+ruzT)3t@>0CG-QajLD&%dQ}&(;y$%
zAwN(IXD$g;>s;E7L;MfDrOEJ#gz}4^tI2^}w+M$xy?XxfZ%&BX?<n=^Q<=a9(0s0`
z<N4^HtOB3Z(+{Oj=I@=yIVlrGK)|235@$eU#C~TF_Q|^HDwMsJhET@){;Dpqok3=L
zr0}*WBfQ3n`YN;+A@o?hPb01^x<n(ItPZ2YY(x?z!1{Qx_;I{c1}s*#7XYP5{B$jb
z_zV^@X@4w{%_f6uJoBsEM4-}7`G)<ER5UkY0NN>5;3BdDKGGyPujY1JjCf7=FntB9
zRo#4TvsK5Ej9pp)m#DT_Rr(YG)?vMRt=N8?+9h8|Uw!4lTPcZDEM=OOY0E#nk|!`h
zfw^WZ+D4`Hk$>VXYON&d)amTApxTyGQ>o><y>T8EexTf6^#%@UN!{9#U2(2SBzu+V
z25Z`j9{x>f=}G4VXEBb-{beg?k}jP+b<~I^64(LeN;yQlb)X}HkL^w@f9(NvEKi|d
zX4Ql_>l-x*B3tc`b4=~ZPGj(Wa$sIXF%CA)ico7)CQeZHRj!U$ZPL?pA%h7*XKFub
z3s{kGYkcN2v}){-AT+FL_aC)0QN@T4{MJ#<r6+)Cahhn<Vxu>0Iz<mi(eX&P`1J!i
z+8wy4w+eM1YC7vrHaq?KW6Ofwe^=M^&Y`!`hUCF_MLmvDIptf18cPbtCUIStC05&D
zt%UD|cd1G>hc%*|!RE|y*#}}oz_Ryhx9eEOfql~n<O?!|-W9~$`g=55hy;S!e`3A=
zvVn#a7U%naABU(G--*ef6Nbu%;78YJ5k18!6v53&iOI22LP{6(b&AnB5R?Z9(bJdo
z1;bU%fnV7lf+Qmpsh6194#iQ6<@b4%4j6G&OW+kGS&-HzX!hJL5M$IwMW#l(5N3ZK
z+G|_8Nu$m3>H!sQ+2+uQ^_Dr-NjBQ`nO?CuFo92V*QW9xS|h?{zOI8_4HV;x_G(&H
z>a@PvB#t!cIiXsTekzo>^_v{YqA87-0piu7q(LQC?Cx6EE?jvsMilB6ds-v~<L9?7
z*9(Z7aTywAydR?>KS|Vrof@=M#rmG7VKV%k$Euc%Db?eA8W*pY?a)zOk=}gp^V*x=
zXB;GXzX~jqF227e7pSTE{|&)If@L$R#lByVii-`oX`g>CLv40#{_!GpRl++~w7`e_
zcF3&?52>({0PTsx71%S|4l*di?8fjy=^Ny}y*TAoZm}oru-}UelU3slK$mn2<LjPq
z+lC?t+VQ0x$i|&YQ+BAw){_j*(^IhurecAYcoJ%?38pVtpNS@7Yo^YoKqz=Es&;b9
z9MZ}!OY7qo`(*V3XWaon;oBv=>p4Q?7?Fx*Ritxqm{<^d2tnxm8~P^Xd!dTunb-ty
zD*wAH^-9lAsEQhA?uyiR=@Dx4>3ca_tVu0P_pn*7oF$!f!>^dS9{KvBmHhd1?^tkk
z!VEf29TsIJwAiqrP@<9w*#$uQT<8+McaWwbHd|=_mbXYZy9)ZtciS1he0)!4f>HKD
z2%3^fFm+Vwn`ITB+sATwot-ei8h01A-E5|ltEQqJM{C&j^$@l;gij~i+SE??I&kvC
zA)T5QKsWKF-jm?ot<*!V{_Rg+@8IUG*1oqE8|ymbfd;<KpTC%4Gm@+83)-ksEb=kj
zdldQ3ai;9}v;QWZ_xmR51z9KP&1vvE-71sxC;pV-;4M+O)9&IU|HYa&(a`C@D^OF?
zbgid0yvQJu<g2JK@}mDK5#Z^|>Ak#vs9sKav#^qY!gRjPy1z=_Lcm|~QTv#O11x9Z
z%=@B?4|2|0MWUXz)F)HFsf3TWK9aNEyj#K_(LRFjPsBq07d3B!`hQ%x5phzWAPkyb
zRR-$562%PCUMWo=iIT^TTQnS_B&woDPW<HV1XDr~vZigg_48yj`J%uT+)$X7x@45f
z5s{ErEGBh{Ld7C;8EqCQV#yM=8ThwME=;4sqmu2W+MrPP$iu)YaZ<Btu;xC_y3N%k
zSB2SAPxPUmF>{;wUhlPh`fcmC2Hl?bLs5R6lhx~bwV|Xjy%wB8hD4leK`lDee`2t2
zUZo1YH7@*FL1u`D$=-R9k5w3BE*#$e#x|ZJX&u({+^`$f67K!xdbTAbkVVy==iJz#
zq;|5QHND~{_(Hj-UbS}CEtdV4D~s;*L?n<C_HTS^&DSyKxwUykZ`+@r!r3NA^*YVk
z5x0Qhaw4jWBjEiWU*g`;9}e}?cdw#obUi=&&cYIF#Ey8&-pGKaz&_OWFSxiOapWNg
zEc0~pSVGLzn|FPM$ZA~-*P-9GhuoG2&ie5T%kT%;XO!{#6UqOk$<)ezCzoHCiXx(&
z-Ofqr*J351t|kzpEtI`_Z-~^1-%W}zeiQEVNBT<6<IB`cjyhjkf>Xnw{HYTpYKqrO
zg}cg~&o@<hLAIYyG|lLxt+NH?+r%J2+7xW&Hg2_Az5eb7L4bYl?nuDP!)QoFN9>=7
zw2m<YCfMne6nCcOeZf%gtyl`DIKDU?4JQ%n4phZS^p_CSTf1FnEF)BH8%juU&!~2V
zYBRLlhAk}BkdP3hsjK5uk0%Ub0ARud$htPlt`5Ya=Db(zwbZr9`Mq>~@=-Y_1;Y|Z
zsgvn#p$*YI*DkXw30ZLdGkcJ=G1LiPfHU*%{l=m$>S{5p-G)IzLfeYmiMGA<)M$(I
z5LJm8kggr`qeFKmuEAG+=F95ZY`wYlALDDw-Lw`((WQ;Lw<vn=`Z(;YPUt1mI5wU(
zBI_SA{CsUb#9^jrxt}wI*!l*G^4pPl=N4OryfIAa7ktf<wDZSF;nKd!_*5d3kk9=~
zXI?%|wRh#DvEWo#rMdZd^RT|<IG!jwis`1jY{)9j6*=MCJGJh@6aM>}h|}wO%C-%1
z%G-d=BeW3V|DxnSHLDB)H=LwPiW@%o6HoE>@4J6-2z2;LGcJUHxG@U?w7g7J)L^C3
zt4B8^2K3D7T@5?-Hhiwk+0tbeBY^<I0`*@vN^<`A^d-`!O<%Dxt|`hDPx#1))Vvw0
zbWd7A3=nk6x@89lD~CdoO+5mYPYj2HtzM?=F9Q4c)el}@maN*|sU9H)_M4VI96z6I
zuvHmIy;BJXl0v@xbP?Jxk)Y8ZmED7N9K@z*gxa#ZS0#k0X!hCIWSrJD>(;L+TML7T
z4bA<APrU!cdr!OO$Bfr+{6(!J?P@YE%AH75XY2cZ!_^fmsr^xMe1&^?r=6*O>hR2t
z;_;(OR8-TXoAztLcdCsW^*-^<?_>Xtjy!)9Y*Q+prFAKVA;Gw>$b)Oy)GQ4q*ye!$
z-WKDQ9Oc$yKd+v2PljGxP*)OWVGM(AAJfE6o_K%Kx?#Gb$#59SWC>5_N&Mk)y&Z|B
z;u^8pYQPt*xfSGWQux|8U-EgBEjC`^hd5^*_mA;K;0lF&N~yu77+z`ZyJ@bOx|nW{
za3D38M+s+|ys9<>`PgEB!-G=7$rg51gVDEwktWYe7QO^5bP46gvx3W6I~94Z(mVF5
zb){)A6S2(gW;MfL64nlh6dwCOiFr-AC^1%U6?k)fo9D9*=5>}7*Hveq6{x*rppsH*
zx@{DZo_nPeE4=`i%EQ{Ql`dE9jal^I1RDV799TZJxq1Pp5DsPLpyL}Oc1|IY2DE;0
z?T2E7XRTkO(t(G8-D9bFXcTK^6X$_9%aC-61Jh={lJ8(K9BYh6sZ-U=f(4xnWL0oG
z2vcVj&?C&Q_m0Dm5vY|QjA)AcTCKg2Ib#~4P)Tp?t4cCjW(U?(0|uMS>P~(C;X^D6
z+|n*<&)NGNEba|$+0NzumcA#bx_`vcux@&dO6UgBb`dP+hl)Gq?en!RrXLVzh(qbS
zf?f0XTAWqoH*Wy7)O~`9NTH64)^X;?y5#l7zZU7FD`|v-tL3bde6rgw(Jw56`u$9i
zyR2Jn=EFBO#(ulF=mXLs*)|e=*Uq%-iD!Q5_&7zuCsjiKtHS=roQ6jVT&o%>XCd`=
z`G1i@wfLFO0-Zc&!mRO_C{mdrU8xvf*hqwsBYK9Ow<Env8<-+tnMT?JQl<`{|K+AE
zrVuBdv~Y~)uZLvMnxgu(RRsAWLn?8pnlFdJBt!A*x*6=z8ia0zp#OEDDxrzoC2Pc4
z!(rr`GN5ksRKu_~_Eoxf%9-RjU&fuK)$Br(<l@a*^e~8nxcaPkP^M?BgjbnSfK+OF
zt^N;|SQEy}xXxAQi%(rjY}C<%uIQUNrh=HC_T7dD!N80;yQa<D2%zP7x?>x;*_#`5
z-vN`>yb1UxKYilVO}tI45CCcPEE2=|j@jIKsm|V8@Ev{~vF!I3lH**mb=z%OLu&9{
zf#8T=9W)nOHs5@8+cjTp^E}8I5S`wmE}EG|u0+0w*z0m?H6uB{cXKs(P6kDgO42@B
z8cZe_epAPT)QQtR$tM#dJBg`LHp(fZLd?`yopVd?x*UAd<$8yc+Uee1O~~uchtVOZ
zu^n=(G!Y*}SGkx6j!{-^VFC!v#~WhKXl@U3j5M+OFN|LO^qT4SpyOwiVY|ah?o^x6
zr&3H4^NQ|6KfUCiRtDGe&3Iyr?KIBQoBW;Q4wu>1RO4VTo3AUwndz@C!zk5e4>;hQ
zo4%KopGIdtRh;143(#3a%V1F%x#ci790rW4qN*P;Afv9-l8;iUCQ(R&)>?_aSgk3k
zDmBWdQ~23THI4Lz!p)uS?b;lB4#@QGEM!$XQxhCcS7T~kH4JP=WG#erjjIj~AeJNn
zvYghtw6hl!#t_p7;4c?LHMyL9eIK1g5;LWo!Qkb#at8k<wFBqC9g@*`LjCh#4tgU!
z$ER)CM65U*EfOq#xo?lX`5BPMvJ`(&!Sn#U?FoKc7ErpEd8F05QTuEN|H?$FvVKeL
zXPt*U++vPX;=)qP_ROH@A0+vxNk@Jzz!+T?<BthU8<8xbO2;M{q(&Y@aFbb~v)M4a
z)~SDA2hsbky?d_d4xc;i(BR&y|5sAMx9A#XSj6O*(Q;-<zh6Ks1Rv!sG&}amxbWlQ
zY!o3d{ykZ^!tRxyQ6erwmYj8CXZRQ<P4xJW<rr9EfIuR6Uwu+e<P(2}23^`86bDJ6
zvgOJKP&je6=+~uUzUT}LQ`DeU8^AG`z}zxo4bBGY`PK}s8ad@T&38EeP^(YfqSAUY
zs~lk(LL9AIWgJ>KOYa+JZSxZ$soy~tTp-i*C&6;>VQcIyqG^gHuuI4!w!JjWlpjzN
zT)bkktYfb89K6QjGiPws(5nL^p>KCN+}w7G^L+nPsbRB&aZE7v(Bsc%|F=|OZ_AB1
z+u1JI#J`8_T^Sv<yH=FkSa&zOWS+E|=lWYv)N{cQC-1=>l;|e?N`Ls0h68U0qEBm)
zT4o4P4g@b8mA2biL}jYk*a-#3#TG<vTAHuOS{}NREt~?Hs)lWDIg?N@0kW|tNpD)+
zmV9SM(b#whyJH>C@ydCS5{S93Jm{MXYv`n274erFv6Q)Tn$hJwh>Dj7aqg0T(-QDb
zW0z`$x0OlZw%ER(aqV_`Y1U+iT#r_s4TZHg<?acC(n?&BB$tqAAC>KNmKnQlJQrT4
z&cO0OA?0{f457s;v=vP^_8|0Lr*kGz1YA(I(A+in>1vqIYllsC>h4yi?rMd5em=wa
z&4c7s;O9IT-)Wo<jn01Jx@u+kcUiiY-XJK5Ukj5<Bl-RlM-DTEB8gtgwpqE%vaYj~
za0Y*proMJr_)=GEzSxEeVF}7Z^@{T#NGdtp-bsf*#p6Y9Db-kIEgjTz1ohxpGm;df
zKvpo}FDvm_@p)n}*7FHZa+wf9&FtRQlNH%i6dG9s?&;jGNE&9GeT*?(F2I37AB#z>
zto}gWmr11mMRhz>=~nd;%plg<w&Iz}P=fpGvPPR{d*kEo-~M&XvGLeH)!K(PVFuGB
zxqR|J=$i2hsC(ixH7q96{pHB-vs%{N;C5P(TlZ87t#J~*2opDNg8O~&N&VlWM;P4I
z_^bmzdW1jvdibL!dW{_|TvXD20^%e}@AFii1(P8~57Q?r>(m)DWepxQDZeKcM+sCe
z2X?T}@iQl=hY+D3U{EL&$dtEK&U}*>30p34&&E6{(k>nOO85u-Kqr?0s=+g$r|kx}
z=r|BHpn}Mq^7~HiPq1x1SVwLZLE~y<A_5Ui&u&h(yJ8^S7E;7hUXil?t<pSg-Fca5
zAw#=vfEo>?1I)Rp3qT&Hr)))>%|+TI<}a4asH1x#KgMe5;EkiEZi;Oc>X&a9jyI~D
z^%299ZWiP&Vq2yKl~zB=nS*W2*&}~+lgIAM(oi3Gx*MUbX^+~J9(JTqmdjFfAG>u+
z;nGhxw|>6w?obymKfCO&FD^V3$HJ^bAhjbW_}S^=`LHk;8(?-NIT?=-Dx4gPH6q15
zBKlS^`S~nsAdz>Xruj$=bRnbEyj>-M<KHkb!j;;LH8Q;gX{4?kc8O}h4=6txQwbRA
zmf>C&Ig~jevv?Y4idUza0(aZOKPD@)P@e0!G)|?;aG~ygt{y6cX)Kjl-d<)Wt!`D9
ztv>`@KrDkW<>q*2;Apg05kBqm5|nX)0C}~q<-{B9aMTGLK01+x5}DPn2|p%Q)K0TY
z&~(e)pj)V#MLC@1t!0l^KT+8uohR6#-mBEy|Ef|1b5=DRI5wiwDVuj3)?9Lb|BFcz
zH;-UcrzvOiY856z`Lwo*ZHTc7z&gfP=fX6W5UC+Mo6%pXR;JTV3@Q;V_KKwv+*}2W
z>MujIhW#L!x)ojqnZ~ye;l}%}QT=v~dUMzs>W>05=9Xo+<zY)#%kF*9H5s3fs>O>O
z2;heyzw!prt<4j*%r^mSeGD~pul4pYy)yPpHf~qZRNJ`s@VVC{HF*<dlQ2h+zj(8R
z9%-!~vEy&T{g75N_%+zx%!auwCg70I3BL4eHL)fWf2jEwCZ$u7lG!q=+H<&llv$Jr
z#kb7<sO>|I03W{c4*b8Jv*eGf4)~cb7NYF#z`JxZIzXsAjq9V1@GC{+IF0+`b|?>1
z<X9QOhwU6H+KdneZBn@*Hu@a7;_H|^-T>jyxgX1qc_*akQ47V|F<2dl0+HI(%^2YX
zWekN7M?R&Nr(VqbsWQkr_9{cZs_~MxT(CgBLE%Vc#^fyPE^7Oz%kqTfL%wmu4!2++
zJ;CpYsIBvqZQXj(#2SN!`svp(b<Yv=UqZ~IZ6z5aatx#swO?d;mQ=kEFlW`FZ{}r>
za!E63V{$fR`8I<NYz>(<<vnwwB`V(X{WQEuR>Miu9Ld=85NE&W>z8lP4>0~bJN<>=
zU`+7tZ`<#Hmg_%7y*N9!*utgWe(9&+JraYN4??}c0CJD%?#pM`PV-r8&OJZwp4>&p
z4<sLpLN}Qo57O1k`N-7u@i>z-b53|0J@x5?D6+FwGD}})y9EJXd8`z|hEjM<p7>o#
zPF6-L9f#3H(S|19DBwDbJyhbeos-dDf=(`_8@EX)WfL%?8<kJqb1}=zu-v{^s0kMt
zAI8m5By4YtyBSj8I%!2>U6N}=m2VDk8WAVh3rU<@PPN+;sE6H4PO(fai}g-;%?h`t
z%w$vX;FOqiQ(v>1%P_d>d;<1*Fz`!q;!*&l_`#rj0T7|8phUkreXrt-CxDJurd8xO
zSPa|PR(@6s>!Cww*-KCZv=Vn$%r#*|Jv6N8?Fk-$V4`iRHRgZP(S;KY7ACcOD^%p~
zs$X5E&JYkesx^Ddpes$U<qPvEua(of1n)yKwo?GJ`1)>G{^?uys7;@zyv5N&c0L#v
z-x%F_8hgQr%65aHF7j&;>c5>B{C~dV#P_z;PXFV=Cjf4I_x<jvJ$MZ>{4ryst!FoA
zE#rYWY<VRD(;buv@c!`4qgGVRMjOZV^OicXGn{ZC0m*BbD&3)*CPl7KIv~C9j^Bpd
zPvh;8<_E`Et@`lp9_zxtbngF3c>r7+IrN`MQs6yel*~_jJR&(n!UzRgxQ9F?9VLP!
z4Z_>IMBz7S$_HAVVxjt6QOY)WlU{SqC<)S5+P+Q@b>Zj*n&qPd3_ptWK}L<^8iHK<
z(xI<+zS|dHu}h@wl0T1P#K_bPvbh-;xk#%wjaIo$W8-i*w{6-2{N)M`ydu`llc(Q<
zu0IaQngYm;I4{*(bj(|xT~H8%;%9i;XS}K|o+@`PNioJCrfKssZtccc9KU1RXZcBv
z|5m8fj~}#pbL8*w*F~K%<Ommpa(RuI14YI6d27XcPk99k)c%HbExHTzY)TU6FPHfs
zkjB3l3d639_FrEFG3zmtwVqZ$4|_q*#qK}4PY$(Q?^>O%URhinMSk_Dc)@Ks?O0Ln
zBqud*D%<DjGJny4Lw~&V*Rz`3cVQV3q*cv!&*jQuVKEL!FVMzj$kpeVE!WNO*<-RK
zZ}XP);W%y~jpN0V;?jW0BuNz#Z`}#yd8Tac$K$tk4rsXITDm71^L);Qc%1OrinKtQ
zjDlL87a0mQx4+qGc$Zw6`>}3#h-JBqWVwcq4Z`nxZ^*rZC$O>n1lyTQ2Sq`uTJ-{Q
z^H2X+M~~xgu6wv6FlrTG$R-sF>DVZV;ymep6gv}~5Z{(~#E&h}04-rD%U<cj8k$W`
zSrTO_UCw-C%n6EGD?~<38-r745ePMC=02aQ@ShRi%wNwiD2s5{`b77EZ%I|kyj7h^
zl<r|+>Lp$^9a)vBVGPP3=Iw`D{rg(>sx`hlb+y6!47+Cf4`Q~u3rh^CQR>9rx$-jS
z`POQhu!wqJV~<uIFB+a3eZyVmo#cz1Il0kDl@F@DTBg3O`q5iwH9O9x&J-nbb9(27
zL{=ERXAjULh<c15r0F02!sEyuOY_o$Wau|y9GUDU(qKbNbC)@!tV8c$@xaA)HtYP8
zOg80Q2Lb`?FF!JTS`qeq3zEVPh>QIwp<SNmXm(0kNFkg9@I@LNYzAMX;XaDNRa4RM
zLC<fP01aWN0*w@Nx&%LZqJjmlFg+&+ZkV#QRwq3#1y#68S+a8aoC-^X#QxQBO{%*<
zp~68L29{48O7fts83uSR2BTuxjz|CfgP%(H{6TB1IHZ#{2YNyz6a*STs9daW_><vK
zkCG3yzxKz@S#;=@aq;<@Z=g!o4ys?PW)Hx|=^0|eod(qXOB^5J=FWY<(f>4U1%ysA
zGNQ*9nw(u@-6`^@ObvVZ%(zg}`7GL8XgnuQ0kXGT95>17lPnpQ_S(8@1=>9uS}$({
z5*d~#3J*`L{rjAO#^EVT_OCrVTVGQ$y@c{66S(^Kx6iz~$<+?smm&goo4H7`f_jTo
z_6ugXzQ1G$aaK7kx7%w+&h{se;hh6xW$+9NU6F>OM33{3UZpj>c)J_%9I3ZuzS5l{
zO3V<9;tf=v#+S(@pFj>6+}|6%lVpzNCzIC%X##rjo<<S`@l!z;^3(6Rw<K`r?cYl~
zd+^=&yJBjxbh+k1Y5JRZlNg!tPgL;OwLp4-Q;SY*twT4khJP)wSZG^QDOTVpsavb2
zU*M(qTY`Ca+Lt44-te=t`4%<?w(h!~GN1~KFNR)?r5A5j80!VfG_S2KDyZd_!8fZ|
zeLJt#IOX?LHtD4WxzqyggZ?l$k80C)<G1oAL+-TzG#XQE?xwI<EJ7TG#-3(08P9MQ
zv(=;#Yp~LNEo;eQQ*D6N^tE`lI(;*3x4tGjtdq2^Wp>=D^Oh}+!th?+*l|6h*4=J+
z#%rWdt+n-SkY249DM{^z-Zd9J?cS3YT9Vm?lG(E|gHU<&)@q6E8%N)i?*i|)9QAeX
z<)fN#bv=D8n#<Z_=1E%HYQA~_9)FmixB6MH3Y*|!TmMO<$TxLPB+AqgZXFHhBBj_<
zt$dgGA#V@=FmJ$$0Ft79oFfjUsC~T1(o^%^9JT*u0NkolwTGw_=#g0H9C59$cz--u
zr9}#1NBF<RF&y8S++jsV3a{2JGzPHbP~p2WOYV*y8Gc+iz7szNj`m|nm!;v&HY!)<
zh#V^Ci3wLwC&-;X)H-HA6c<ZXs>FZyibMUJKf<&mLO~q!K2>q=u8WU24L4`0>fXRu
ztzEZ7lI^-<6yu9ox-JJntk+*{X9T4>f_KJ43_e-PXIhT$kPIFAw4JV=1u)hG`X(Lu
z9aaSzV@k{rh~mZj5+bH{PK3MJyGfYJmTv!GP+WZSojGL6iJwVhs9rt2T?sUC;Himm
zXzlLET*J>^b*agnu+dsBEp#o+W6qzC!QC+PdvvtHehymoY!;{dLaCj)_}e~3Y~-UV
zLx(3*0cC`#bj!YSV27mQQjyE@bUR+302M5{Q{934f~l&zX^uOthUkS_Egcz&c^-_H
zGyr#qxR%ACThA_b^MAUKV%dAf>bs~>x)g4R$2*jnOT#JaIWipBm%-Wk`7DV&gLgbu
zQ)97P9OL<mTG~w|@^!%N>8XEGtu!=R^1Rz1NZG>)m90#x$&Ydpd%r6Z#9jjjDtpjH
z|1QGyS|OZndgg4Moq9h_z=?n2PClhQqg6D+BN7=nk7Wf;SMt1x5@iCP<<mCY<_XJc
zUXQ-{0T%_0lPqvg$6xTO0)RA%Ue;`y!p=^Dq^e#-O|T-Dp1VIfK)#2UB?4d?nTm=*
zSH$=w-J5d+%ukR{jo029Kf#XiOax1?GDq8@`_m0|p5>m@9PNh)fb^F7L9>A#b8GOg
z<<#na*Xqn?eUnw<(+aDlHOFx?Fai4>yWrIcfYqQ)yi;=(npS~p+Qzr+SV{2HSW!~d
z`JzBRBhHVHYekjznv=xpRRR`=a_S|o$2usd$ea*hYdlXR8`$?%6*Ik~5~8eRl8vpK
zsehJnNxU`@$**sIIwwtT^e7Lx_$kbpVbR@I%oJ;|{($Iqu%5n^E9m$4Ys}UoV_8){
zG13osWzPCvW&R(vUUC;t4-B8y!(TVTyL4g}6hwKm6gLzPAYo`f&7vK9Fh5f|2>zQ}
z$>8Ee$XYK6r$a0Sa%2x~Huj~Tg$t%E)8AR+&<Pf6*=Z?-=fr`_z|2IxMzLjN`7rbJ
z*cHYE)(WVT?y-0vA|!p?(i<(^f5SBc%6hH8GlA&SZRphAc~CWmkl$A!_*nplBs}N)
z<>%@Mf?3Oc?LE!$9X08gDjVu7qZUd0#L+$4P6}++*)Nd>ywJe&Q+^hhCC}r!O`3dr
zm#sIPVLs2*zJB_fSoQUd@R!^JYoj1nTxjdk=8ZeWy<^ziszL+w{kcs*Thyh0>c_W`
zFrQ1KKkvVNAsI*c@*F5m{x3}au;BiuN#5@!))zW-43BFm=1<^H6<|@UgO-WdSET=k
zskd;8g5AEqQ9`;yO1c?p=#-A3h8czy>F!bieI!J>OF*QhYv_=!VaQ>SRsrd51o_Q#
z&iTIA`#<b!U2Cs<-=D=%F0gvjQ!^k~Tqj~l881veWB6C)2lfG8WliPz=QbUS6~ZQO
z9QMz$N9J`8T*-1ibm~IlD(RiIg<MaXW5V?CHLCU$Cxo2FT?iCGOqORO6Z;bMn!a!J
zS4Qg>C7vNf>>|QSkJP9SMfIz!1O<%Xu!mMp+t*TD_3)X&ZlFCpS|2iyN!jP3HYfdU
z{LW8uAfdljTK~9^MF_0t{YZ9N6g!Hai&Fkdy~LL@oYvrL+_Zv_v)zsSI61S}DfG<Z
z^N^Y!KZo+1F2V+cGA_=R12<4|V3&G?lBu*)nM=2r8dCPu8yPJc*q70efL<)r9^}za
zi~k}pn98Uw%z(h))?Sm7NNnB?<EGL=XhK@0hq%v$WMd=TA=(>Kw=tB43hHw0UYX!{
z@5^>-JW>6Fg7`txhRz|}(njp~5p{p9k)Pwsl-+1p^R>d7@!V*2RtK2YYP?#@<xqUk
zN;2ia9VP`;eY)Wr@Lv1df?d^v`V5}M8@{@3-XWuCxt6#&iHYD?OhzL>vSUIUlmK}e
zz<RWy3lsH5?!2%ew)eL)$>AKq#}JACH`<Dr(dPI|NBmI@bNZ78<9{5j&Mx{jS4WgF
zY?v*D7fz{!5pLGgthmnt0vW1o)AYG2t~XHwrHy7Gzg-lwrfeH@s6WMOCeQFe`8IL?
zJTIOuCfDIKw^Gj1aj9F3VCVp0!FB>Mpw(WANa+5BfTaqOw`H*)Js9e1;FB=!6mT8s
zNEG~Wtr&g-_?7-4&zcV=Nb}*w{@btW76FBIg4AMLvFz5SUxWFP>te>^Lbf{~__Uwd
zQbaH?E8CDGo0v4whP-3%fYGB{lLub1Ds8dggl(6K`gz)^@7N#D_GM?cQ%2}B{b=L*
z>s->1hn&|L{+rC6AAHlxr?i8=kfP{Dq3H29Y2V&8zMdX;{h1wX?RPjMEgZ9a@}-0j
zue<)}Pfv-g7Gi8!c`|%FTA>m}{ke+l94lQ}WSNh&<2ZIZO^!Sbo4o1}dkb%16tqVy
zU4i|cu#iH~a9HD@<KnSY{j$X)A?M`l1vRj(d$&e{d?FcZ3do9}isIaR39f(qh*1?h
zuC^)LF!NzWA~D6VUV$QQd~_%|q$j4!HO@e!k5In2n$2sS@<Qd%a$sgO<vgPqSwUFI
zHko>!R%#%ayY*yhJoi9xkSupOt)kJWmq+RZmpp4K8I}KNjNA%Wbo}h@`;wj*PKz;A
zc$wz;q=&KT@u2ujxG9t>lE<J2S#=x+-gDd2R+y%g(LuXgVb*9-oQhZxtj9jq)AhF&
z<kNvomCnMk{R}4-TAhkT2nkt`F2*^uJeh?TUl`~^@4k5m%~`S5s?0QfF3_58ns+rb
zQ2_Z_1R~Ylid)X-gH~M}1pnxu<U*R~L=Uv}&4M0RoBfyw@L+24%9#&DA2gxH1z}tk
z<n#2hi7D2*8Z1FCDbdE^^DzFzNLwLJmyBN1gMBQmxm6RxF5utSo`$WSdC#1L{#t!S
zfAY|bGrD*S{OYoe@gqFX|2NisFKe&Wc(DmFiv1jmTAzR{R)Y;}8sVxyk)XB?b`C#f
zBu-KJS<fq9%ElY1%azgw?RLS*RxUlGUv3W-N*>`km)66>|C2XeFr!U85v`UxxRcBP
zeH*Hm2iXOVG<i}1GL@VfZ_iVDF>B<3gn;SebdP2*pYg|iS_$r0D=zeFF3)?ph7~9D
zpAWQL?=x+MQ=?bdJwA+B6=%xFlMxsomddUw0{j$SO*;6SE6~8^*ehHBvczgU7S7|-
z0VX7GflkrI8&`&Uc&mWe=mY<b1DKyp=!f;@V5SU0sg7S*(y-5X?ZZ}kyF6&YXQjZ^
zt%Ga2+j%G87V=!evL^0f`t^m{8=q&j>`U3}mNp{1QS<I4-)azo*M$T`x?xI!v$^nA
zT`10t2q|J1zkVBXhs9!8L_8*UytnwfLo1hxsIFq^_*gE6lQL}}x-y(Z^?>jlZCAay
zHuVs1YSy(XGBuBsH@T?fsrp1?2S=fooz_itonJlAFdu03xY=Vbl(`p7iG1>HU%riZ
zoIp42p0yH1Mm~%%zhKis_G=1)NI9#*^kO-?=hV)KALnZ)Epo_A8Zyx?>L1D{ix-IK
zuB-5#3kuMJk+~kv1}J8G)mo<Vo~7vesf}GdPr`Om91{D)rLH*YXQqWp$0ZF*<z-rK
zn)YY*;McuS_bjON-;c;r{^~CLQcjqi$9hH_PGmSuek(zQu82%kRTCTJu|w>u$IF;D
zBROOj+y|``%*6-!#Dw^bA8F}OUM%@6z=;=sX-|i)X{<u4m8{lGkdp?Mk^?uMldov*
zgJi{U*98}hDQ6|-Qtlw$T<C5_JafNNV6bJpY?S`b0QdG*+jK?JHluUfX#GWL<J&LV
zuU07nt-m*LFgaXFGUry)&IH9sn*}^XZpX#_NixrtjQVW#^cO^N!z}Ve80ILM``=Oi
zml=;asA;u-icRuC$p(yHOpZ$&)ypCBYWW2pW0bnRVU}eI>EGy)U%oC-N-%XQh%J?u
zbH;!#W;7oXP3<bc9A~fvmmwBXAWu?u5!c~d%+@U#=7VabE3)Z_A$HHkR?LfZtF+xx
z_~*<4j<r)ZULDMz)Lml02bUG;3@c{MNH10*lG7`auggNmE~}(}!UvS?(PnuMRL&nP
zD}3%kj26Osl^e`-CFP#Pjct1^3T`~B3_wBs{FX`C|3r1VU@^3R=ujY^!eYsJCAsC`
zQ|RbRD|l!U>bR)x`{(vqi$LA);~%%()WVep+sZe-tAj_i(R*iw%%D;pYs>X<$v?St
z%>tv9Z(iQ#(GvZ@x^fB5ZxuR1{T3MEB@9dX%FvD$7y85AE^p8=(84|1=(}0Uzdn&E
z>|ObHS!HU^{;t_eP;IP9hpOstWnw$S4I9aZ?oO-7<MS9reZ=-?%?{9jsOKb2|6UVW
zQB9*sC}4;b9PamerdS`W&e)nl>>E8)-O;?(nS7hfrxwHcF<-jwaTBd5*k6ifd@O{S
z;u|GDk?_cwZ#6f2^?2ZshS-LM3=*hzf!i)}OGIc;jwDi=CRc->86K$KtLuwfZu036
z9PoGd>JFS~{X>@8x1<AHezT)Bj+6IX^o-MS7=B74V}wk=*ln=bG|pSrB*96@tX7GI
zq;k{7lD0{Jl{)|TSwSg#@c;(e<vwv&M$Li>c^1<d7wUOiQ!fUju~{vyVOzyaSITui
zI-tD;BAg2h!kC_HKc2=&8&+MKP_VOhP`LJMae;#!S&MXu*dQoBSI+=KYVORukghoS
z`Oo<m*e-mngA+MJuoY(N-VB-}z8@#>lNf{ZE=H9X5?qpXcBUF^SmtSxV4vT<heX;d
za65<hzhxL!q+il*Dv6*ojOR?W=Kiv{JH670N_FKAt*zLqlyE9xT?~-Rn!v~Zhx7l;
z2PP1Fj=2>h3&XOdkpA+Rizv3QNaTC2Bh|C85q7U@HGXZP)PGn{WdutpZ&JoKW=uoc
zi6ql>9g}tjCIpl)-5JmEH}OQN3x>DJD+Cpz3}V36sqL1h<@}ZXJ56RJzFi6x;{}og
zT`y8WjZ=b2nk$agFIqKfq~uCG-@CMdk07^z7{v1|kWglf@!<|m(}1XSE>{q4Oyw_|
ztKAC&5|v^{@0*UuMK;5PEgo6kx2BI$Vr&l<Wq(U@WOZ#7NyV3@8dt^q@<$zT4TC?<
zEnodvlx+T4ldqEB`LqTSJ-Ky!M1L4%lI~b?A-XK&(4W_KINwI`0&R_aeeQdBMX+D?
z?cIdN0e$%cN0i<1&L0M0sOs3SmW0^TJE6okoeu@Plyum9adJZ?-zsKbo{Q8K5>CpD
zf)+>0^l{`j`h|!khkN5Y)(Hwk?4CbJ9~HwQn)TsI)Zau@w|C1b-5sNp6RO<LT7C9@
z(#xN+zg_8YU$&-Zn;MXAQWEaWni{FKf5KKUjH;O*i`mc^5jb6b#Md`d71#cy9)*zT
z-L@-W6FoRr=N;Eq%M{}M=+Azu0idEHANCAc{3Y$1_9YUjQz15_UV-xtIFuI2Spx=g
zKE6dz=S(bgh;~+OO_1uaD0D*I;)VJkKqZ3#svNZ&1?N1iZq3vZGAgvF0~jQ74!n3=
zagp;z<4eF`+~<iI_fffVKYu!&`3tHa*{ZPc+@3{YZ0~T`L}Tf}LdHEvXd!iXi_^j%
zHAyArM<u1%SI7Pi&s4`MG{E?PEBn;~EOb_cXGc%KXBOY$!EhBe7Z0jvxOEdX89`3t
z&HG0_;eyQeRleeTrLy+Aq>Ye=a^?F3pQMUMb)Lk88FN8!w=*{n2UI3v{Ru?IQ4hL?
zZ=sjt)Qo)dV?ko#pVdLPA%&Q2OaT66%iLBV-NZbG?akU{O@8F4#BT}xweKVyaV9dq
zooZIh6%JA=F*cjyzasu5Lb>N(gbNW-7zW|;F+!Gw;E{@TvNV)i055K^aB_P?hl43x
z`yWbedH5s#bmfwi8#P)x+8jhF#MJ+{x>C62I>jq4LITDJEju{A!_!ClF*9Gqb*#jy
zWh>Ec#;x+V5c%-#Dw;8SSRO29zC5D>w_(()74ovS1NSq6wp?r9FLuRtouosZ*Lx?=
ze8swIg$-3j;=2+V@pI2~X@c;<8S$Hqo*4eXn4N52-C(g_3o5MK<XG%Hl=u@+VF{Ss
zx%5*`pHh{aLTWIAVDpE5f0!rU+zf8$wOn=qWRp#nwORsd5*qS@3YT`4mJ{+b+qU{;
zn{vG}0EP!Xd+pjs?;z+l)mCqP;)k}kAC|gbk#Yz9+JFoD*<Xq3{3L(hir~0>sPyO`
ze_*X;c$-~H^w^kCA-d><n2pHi^hiLsz3oZ+;W%rU@}#Z-<v$d<x)p5%kiJc6{e9I(
z8j0gR41>Fiv&>ztIF5JxzMPL(dxb-&JNXiFmUwDJ_^au}N!0R4w6)}({;EI0em=*0
z5RtGic_csnMcG$gZB<o_sct}YmO7Pvh-|+}PH?=#iAa#L{U-tIObQ&O=LG?ZBdIut
z@ia!<_S9?jm|5?q@OAG>jp~E1<O`@n+E^t~f*HjPCtUP2(^M4{%BsFNjKFV+X&0=9
z3GT+U&}{{UXT!i^Y%Hjr*aP;o1d~R~SmNDs5{#q@D6LXk40Qf(r0&Kmh04QJNr7F(
zuhddX+N@Q?BP*p1&pjF{CtY#PWX98V(n0-A0dMw9MVMm+3({BYq-V4gb~u6B_o-%I
zDLd85_)JdKSyo#~=A)(!B9WGp7h&{Lx=-6{BMmYz1p$Kri0PE&rcLp-Dv}|*MY9`b
zI_UgS(?PQEzQ>xcH74fHuQYq>^_I0Y@O_K}y>VCZSQDOVqRmG@W(?YxJ2+M_J6x(@
z>=nA+vtnV@;ZqHbPEmzL`Po&-)FNr?ztmZFtbg72^?x6FwD9N&9;yBUOBeGqQ^gJx
ziDpF@i9JN9<j>IK2<c#@j;=Lm+fy2Fq=IZ4qKOqQB(f)L#9meTDiFsF*!Z;L>@q5a
z85T~NbCJ^P6hU7$;zKKZjAG{48%G>`d0tdO%Vs}$Xr!Cxnew8E)Q0+vnwQpF1ek?3
zU2Eog=onUZgQ?SJ>x>z3e|>w?Z&eR{#f4^J8L+OE5Y!iQN+?zKmJ<AJA@{jhqcENL
zZw6IJ*iT=hz1brZ@ma9V8(q&RK)^EO=cm{Jn1kH>zCTr7V8c;wjDufxER9_@aM8P2
z<ZUK)aOjTTb*M!tLcQ?_+!#_L4x*~qpQvhnhIZ<;Uvg5wA{CtIJy^U`g8fZ6d)X&L
zwix%N%OG}a7kI=Gi7Uv-fQO|-+@W#!eB7e5z0r;Eds0QTu5`BQB>#)GN^-kRtUhu9
zyzMS8-@*s=+Hs@ePEo(so4)vE!?R%@RXH5C!&nwZO%ZGmm5{+{KbyK1^gX-C&bT*f
zc%ss`x7t0H51?W+V7b*Vr28g!)bNui_ejHDB!1i3;|qMGSWYcJyVrTr*P42Jfu}>>
zaOW`Y)1^196K9|nm5JC;ibpce;+6EzeVd4P^bI6fB!qXy`^6+q9ayKv4)5l0HB(|k
z&cAubA%KVp57R%OgeDsp3W^k|QmTH_b29E0vP9Ss&}gSV4Q-Eq0@=>lV@1-sfvhJl
zFy4eeY1hF`4UD6B+7IoLd_YPNc@d4zbvhXKq!<6BL<??aibBVb!QRVFP4n8{KkBXN
zm?Fg&UWx_e8N8nfGNSiF0ILJuqGOB;I9{5hY#JCeQwH$-XsLN*KZ(u&$}DA(*hfu_
zlDu{rYF%%UAA5Sxp}f+1kMw3wQja!nye<_yoSS`NA8kVoGRXRo=0We=EPToLZapj1
zePH93pHAgPU?BgBgPFcl@aBRmiw-Jd`6Pa?exjex=2Mr;%e^IbmN=&yLV{dgjKZ`0
z>t!%D{oi|)ml!WY5T*8<0zVDLLKCBE3)b@6c_4f@{No67r}&@pL9zwGW&`$c9c~~#
z9PI(jnXvI05j-WJ&R5&2H$|bt#qy@MPGWQyll4Do@dLj$DU!jWwE1atHVL`ZLs8q}
zPrCxkO>4{r=icPil-rjMI13}(Y3KOL(S=gM{?EeIBdk5=O}TcNLvq%3O!&HHtTCJ!
z4*&7bL;%ko2hnVEaayy?oR#CLHW=keQtr#DAV$C~oqDU$>GXbRqg#3RzCS&)xP@j%
z_-~J!kbs;3(x3XRcO>wHvY=5X$M2O^QDv4Z0TK&HTnqKd)gRa1)#!dInzi583vZ*M
zh)Os1-kQ}o9Icdnyy))wwqfz>R>_;N>b&+VRZR|WbrGtkVZxI1W1EQIak#LafNOfm
zWPj)JbN&joU;Pzl*Jtwa_Ii|qSUBv19inpkjqr&W-+P5>6#;rVig5^@ESQ+PMH8Px
z@%Qkg+)%GZjV~8#mopTWqogZ$#93K^Ej~9iV{y2wX=rKLJ>n}EgoRmNzmayF&U()1
zc(0%SK@e1F3ria8PfAok#^1?nH&QoX=>YAq33$XgM>Yi#?^u*rr;As(Wku4~l?-Pq
ze}`$G6(>unkx@$k8cyNEApJuUGL^36*rUg+aKlF;sILxBBNc?=vX;>j?(I{MN-H+W
z(*C|DGFsF7uPv1uB{b>292*&SsZ@>w3H9c3FLZ=}c?p9vXYf5;G~qp&#g`_BS}0Px
z+QU%mLJi`*gi^a9U(VF4)?-IiXZ`A%;Zh(UY&P!*Se-e<rm%R$PxI;prp_ev#pNNF
z<?Pchk6tW9PY@`q#~{m;4O(^CJk?QoAb!>|q%r%~GfW?p^#mRpWZwCOX!Lj!x9@$w
z-`qR(shdypelH{s$#SJK6+;s@v$+EzZ07=g2CoNd#Fp8=`uNEFJ3)=tr>!7lEdL6F
z9_kN9*Ps2X>zEn;?;)ZPGf9}@+VM72g^w;$>6iB_xGWV720KUYjB=Fxn>w^n_gan8
z1P}9T{8XkG%?$kXjl)-lIxGY$*1(zE^lp(rIcdh}oL9{@Qo0C>TTC`mgU&EdrNSqs
zinaq*YE&U^>JU?{S*yQ)*?wR3NT6z|24X?{H{K&1B@C;~lf4gYL$O0FmS02$V1d1l
zE!9?UsGHZEGUYvW@b8B=tLb!0K23fcnLCr^3sh3Y$yycn2^kD~pdS3e4L4uT@kpBc
z-RK8EW)gJOXdH^%qXRe7=Y8!`JvBf?onhVw5*{@?*4<yvUbo5rMr3|K`kgGkNZ2wE
z|339+C4r|_Ver%IyZed1>p$D)Ja%(Qm4J#5N)?Uf+lm0-^(WGgj4|h3wW@u?^+X&`
zuX?K6ZF{SCK%>Op^w^hU6}`_c51WZ7lk)q(ob&|c&X#z8DntpZ?g|qshyIA^&rDKx
z8B~c}Y7oggJncCpe)L4cOAI=kZz#KX(3iaNWvz+QpZHi|goB1k%cy~RM{FiHE>1I*
z>CG*`$099Um!dBYHLBGyu+vvt5jS8!UOcd<FT&|1IgTO<$C}db_<OB>D*w;Qv|odJ
z=OjF0ll@Cg6@nH#%{jG>MXe{H3vQ(LOwl>s)T0;{+h_z%LZMbVs_@)$PKfA!fXbAg
z{|;0be%7cD39w{*VIsi+!0Dzkc9VorU62UE8maQN`Hd{4>`e5bQSPQ6JC__lp63RX
z9W&|5PV+Q!M+|He)sH9)8)?!lRwVag&gRjZlN?Z!2+7@UJ>(w^-lW3Y^q~>KCAQT7
zBQj8FtDYTnwekh)Cu`wuyggGBs~l+<3_0iay=7Gtdo|4?D&<$;ssPa8J3EIXQgl@y
zZ3B&AJPc)&XoL-)D~uKtzv>QBF_mz1C;`moO68l#k|>oF&PxoMn3D~3$?gd2dsiE@
zG#nYNnZ0c+aFJDv&VV#Gm{s?F#s7!#zos1{>6o@YC_5wGBUPKE>tY!G!@I#R_0lL`
zDav?NJM!|Uk3n4TH0)=}HYz}DS-Q3jSykP3l3AJs-rwU6X=Jg~w#j$yPL&mtrheA8
zK~(_a*-)Faj|xU4&`R}63BGiGf#-F)wq6SXl7JUA>ig&%`x+Z4+`_qq7hr2_Sh@J)
zbg6`$#inh|EpWGb@5#HO-eYP8iRKgYF4g_4rw5nX$pf1W*%e$3!7&qhn^44C+quk7
zKjGgkn}&f-^YiECdFu5Sl>^Q}Mu4IIAeM5ct3O#KXUc?=zpj%jo6!Zxq@P#QAdPl{
zzMU;z27P&!)cRd7$(Agbl0j9Be`C-g7IyvqwH=S7KqliT+nIIfgYfU5s=v*iv2PX?
z$T~h`bx+B)Pn1vQp!;NEXHUzUoIl|{QTD=NB(Biw#II@hG}=h5s+BvuP&N=Sp?*Gz
zDR=FtplS%~I7nlmP~HPwhO2l(p0G6Oq_$D_euM{Jws|c5%~r?wo0K6+&OFwh$|}5O
z*;<^85!KVip|CL#RMO=TxS@y}aR%gHQKwuihU)@@C8VT%LrnNpQbDr<NEhNLQ9WJP
zqGtnBggdf2M*AC)`T_?OxJoDeE27<E02+Ky4|cvhD4K+kb6C6>t9kl(KuQ}*JxF9c
z3*9uMEvy5riw#LozUP>M;~GEWrsj|zsP~U)<26X6B)uCU5iG&zQ@Rf+G+_BOeDb{g
znK)~}otKr;SrhFs+8w2J)=tc9Up(O3HT=S?8CM--6oBmam=bVPa-9skw+UHG-kh`T
zIl9jm?732)3STr;Bi*+4Q{gi}qc>+2tY6h|_Mp>s*_)A#k1rf;ObIuWtZqn<COYN0
zpts)<`r5B8@k>5O&3_MM3b&;v4X|=pSlP>3B}3+xO<ZxC#2TMMo!)>^VT>Acw9mi+
zi*MAI;;iO#x=Oa`J&42B7~r=LqSDVmZnlAn;ChIt&UCmyS_Xl$c8-;7JSK5u_^-@A
ziNL}#Od=LzCJ5`dC6D^^m*+`LFALbYGmsN2vA4iGnpi^)FK&n<aeJfC0yk}R<J2sz
z!i_I$*a>J>9HxhpVZe5_!x^ag<n#FNdRg6FYn2#Gd1V{?S-4n*_F6i>vCuo+M3b+9
z0$dqLr}BBh+9l&cHnWI1A#zaG<2|cl)4dZy_tlbs9t$rKon`h!yu?Yb=DiCQhu9%|
zU(@S@hUugh1JCn4Zag=WNps&D>A!RwSwkBa=3S006d;7_nW><oFpcWK>X)ondN0<P
za=nGG1y@0KwXp}~Z{h=}o=k4ods@nsQI(g4om|n}chPmdg!{NQyrVg5Sz8UdN!e39
zs;1jP`E}^cOP);Mg@y(_>w06f#>T;bHzp&nOdO`<m9w$0j1R)_)`Y`rv1IGay9gz`
zsD7Qad7<Pl6w9eq>fkPw%W&S~{=2K1X}j;gdqpBN@dm0bbQ?8<fktcHNT~SVZ%Jvc
z;c|M*PV~xpl+~;nb-}(z!wrxlN_JntB7sUw$<gA7fZSLXKUls*Xz*mCZX8LR_6=Ad
z+F35CBQ`U#&^-bDawu>L5BaH5?+y^rr&PzqZ>I{yR+jsMvm!SFVY;0{dUFvm$jpTD
zs2R@a!cf!x9fhAqXXH1P1StGRpQ|P>%Ly77{^?cjxe)w|ey-~upbJhK&x#&5?jitz
zg*k7aQ|GfXwnma9wx9cHjrmkSKm|<-Ua}HKpg2%~2iDVkU~O&Ms7r4Ko^Z3%8Xt)e
zC22zY(trWzF`|G)R2qwbOj3yOn?kxdp!W~jQ>6Z4TpV<CM)bZGY9&ov8u2pSD&W0M
zMg|ia<-eEhFW3@51v00L|L_AHz_}1){WL?^#9lHq@x%0qqfCqgs!+*sGd->-kk75^
zKp84AVc}~E^_YYhv<sQMTc)qf&z}wCfMOo*pZ5IUBKq>nQ%ink;xIKfFt0xhK=ZDT
z4V=a4E6jocKqY;TOW|RS7%rO0LKRvGM=F)nN9T4MOJM01owng5;6MI6S^)fkOBv6e
zrT#VTlb+Da`&<Zj(#}Spk1-aEE9Evor$hbA@ZN<0mtv7k>Vj*=>y?H%^9D=S)UPWq
zR$L2SI31fuzF7|nLkqzw?w?sd@3azZjd4g&e$u?k*CzPc=qe-khwxMBW<P9(ZnWnD
zgCiMp7Ro$8R)EI9gm-H1(xm!x1^Dy0P4kR%#-2@4`&_Ff(?lvnZOEvaGG1{$TPtbL
zW&x9=yeAlRItb3u00wt%dv0_dws?U&qum4Z9#~>}r_rv*@8klBBu-8p+Pd;1$uOzO
z#LQ|gZ`du~P;=th60V{?ws)p%v})%?HCfT=4*T(&<~!Ef^@`0eHK4vV&PgckLn*0S
z!l=4-`2LxyPpsuuEqNRj`JhOLHSNI3K3=&h*w=^gxez0QY}mkV`S)<rMl6pK7<-9b
zVHzdMpxmHa&0RPxX2n`M*}?Z@@OXUi(R0d%6Jb4w`-&tY*_J=F{UDVvX28HbU2a@h
z%=1ijI<+;*K&;Y*pG{l<^1PH<@H+Nu5<KbGj2|xRVJFp$fQIJGAg*wCqih>B1bnV_
zIfLv{<tY#<X3^K%8>Asrs_&8$H=)GAkLejpE9YXL$`uvk02Plle5V=&sv6T+*<KZ%
zl>~5@i097YneRw2^O>b}u1KS|ITz>`D+Uzo5P_RiObh$$Hm1r69-L_}2yh|3=E;#2
zcMB3?%EFeEkxCr1^TrE-sEqRlEK9$G4)-bVt|OQvj035r+C(XAhOr4;5%$ETqDem?
zFz!t7mBmkmAM-Q#p~@q3TXHtiA3zQ=^A$=SW7j(MS=v|XXE?AntIW^lv_oX?SJD0^
zg)^Uw?`_uS2bD9;QLB|H4rZODxh_Ni>*@k6d`um}e+>DPuzzf@@S&89L}6G3qWHNK
ze;;C$e_?7;Up``t*Tf_)A&P92VM?4}Q+pvWb;8JRh)gkqHF*}&c6tYgA($wCl+D+b
znU+H%2FxYaR=gTu5Mfjrz$;<orkpq{AQC-(+hqC`%0IPjaoI#(|J?)D=03xs{}sUv
z!w1Z?IaRLmQxZTO4=j?Vs#DC>3*B7WR_mVT1U_5vz_P~NtCt$Ey+c^JnZ^k@v3(BT
zXrz?;n}{1bj26CF4($h2jqerG1MtMknwC91ULyz3Xj03)J&h0{iLc*dxr<Pe2tBa$
zjg41JdK}3WCA6-dm{&XmJ;~eO5J`JS0&)>bK5a%<i?b9wnSXV^N|t*BJg8NXsn1-q
z@PDYABz}yg2msw(2=Y=66w4-7F<r{JY;b-9diD?etV%-sIR7^Fn0$e?PKEredqVZJ
ztm~Me;uatzYe1%M2)F&DR6E;61q2J~PpMB0JX_ZwKTG&kK<-xjMTM{EgsJ?<?J`C#
zUKVwu?wj_Wy}!YhjjM%7r~X-^7^8d?;$yMa%D5X*qS$!-a?;VTp}6~BkQkhgb&#sh
z@p7D!c=D-1HEMBdF!^kgTvdDK`KHutu8+`=L2)9Mp*IbOS}m}HgJAkVjZRMGcy{Be
z^;|Voc6P=CO#P(}AX3tQHsb@;KC*JKnA^Winm0I0ht9@)Wk?m6H>Tmz9i!fKV|gCH
zL{HcO8N{~Nn~kqpnn5FDJq9aDf(^c1IQZd%(UqBu!>Ps}KWd?67j(V{;FLRRqOBy<
zVTq@Ir>&?!2A(Ja)I8@rT}iFt(&Z{Hs9`>$?45y_@oC)nxdS+A@oMnsBkVh1IeVow
zz1K|mj9Xa6ZeD;TVyyl??sTd-HaXZ>3RLT1<YqbfNh2sB^wH>dF6671<N<k^+oP3M
z46_X@f21uK^!}bZNGLj#*_+`RRBW|*3@jq!jGkfCo8~|M``_}amq{Njc^?r+_2<`K
z`@@(XVpQzNwU%9h%&}TFqWBTdh&WTnY>dpP8I%E8S`Ml7UmjaC#r5W=W(ls^0Ame#
z4R@K)@tWyI+~Q`;{KE3N${5^blS(CFzR_38MSEBad+lt+B|ldff+T&U(2rp}X34Y?
z<Z-e?e0$*2U}`HFL-J|xP5FYLiA>e)g?T4Z<UEcm`3m;Py3V9C7j~ri&bYqK^n)kB
zZ&KaEe^K#6s!*r3*kblypxKMD>6+IoAt1N<WyqE6lKvGR_ngZ$Y#jB@EECK5>S@=7
zUy#U@$JGrWVk~%Odzs(T?ZGIOxBHcB2n#o&esIFJ>aPM%BL`uqf7_+f=bWvpRqK$>
z5q-Rkv#-Bm@b1cEX9r4T$Uz&U&ujjCxD@wlJp8M4IIDQhJNc{QNxxSg@uuvqJnuy-
z8N!4}?WkC#UCf8^@`%sb?{2GcYIYvGcVCdULZue?vAv%}wRxrg3-;<g-?YC1OM^S`
z^GDIc233wx7dNpxon#KmI~jtinusy6ihmXceRNzGb-?FhE80|INq^NSD+`z>AeBA@
zB9K@<_TC!T6;@=b-wBt-Ec{hiok$w-g@~uZBWk^qh^r^^A{+3Tidhm|QKKV!AMq&^
zv5PEd3RhKsq_Io)Qx7=q#pyuph7%Fb%v~g{T1^ENM<?`N0SrAsSZSHYGCP)mB!ZW$
z^k<oNxJ)Fe?`=CI#W0Qv9%E<Ul@V|A@;j9W*NfS5k3@@MBZrw-3)&KG4>M;#o0p!)
z%Eb3RNWYy*;e4J!V2hZ-k;#jufd|{+7PUPmJ;|y`_m<hY-soRxL;OiUg5psP*7|#)
ziW*md{wPa6PcE02t<`5vGcboJqMuLyc@(#n9?z#e)0{h}_Y;z#oH!jLArU(j@GLsN
zd;1_U;U9ebf2jWd6U<6TjRA_;ieNTK;)i#=9OPx^j#P|^!(UFEBNUm5!Uh}+uX9UT
zSYwnpjkJrK0g|7vxGAsC4`qP4Q!g7{r8UG$6&N`-j&NMQr^#1$qLfwV_XAf!*6S+b
zxy!hVP*oDHB(ek+4d%8eo#G5IVeMiW^rj>*Tc^Y9Fv%P_hSW)47J}{Lk&y&+4c1D(
zvC7EzsW+|<%*}1DjI7ibtFL(LscJdU=V&_q0jO7oat(mw+*zwu7H>7vyA5?mHq7d?
z%dV8FVkduf_FIaZ*m7zbg`B@mz-{ImXxo+%XBL`S`nK=o9;o{QaMe7cEw$1~$Aa8y
z*_8=q!Py$loLloS6ps9`fWEWiWWL+F3j5VrAu#i?`v{bqB1`PHd?>BGrvev2ZV;b}
zd=w|_Z?DdgziO)nZp`y+LvC<u^iaGbD*NKB=j?SIjacyAm<O7A1Gf%F<$BwGDz!>Z
zF_enIA51GnmH0bSfhju(NvAevIOk9W;q#x34FZWQ!v^9mf^Dab(>Q~v*btskUHL)M
zK0govsWwq(2Ons{e&iN$(qH^tuMzruL(9wVYgCulIQrnZKj$>N{#i()d=G|yB`D&<
z6rce<fD{0GP9pf8j#YTb#zbo|X=@rn3vZZ4d^#T)8P@7r=^;nCu#F3oI$P8Y0>*2(
zXSLjnwM3&(>pBzh5lWCT35MBc#z_@DD1={55wS@|fMwk*eyEq}g2Vs?R1>y*p{#|j
zYExUFR-NQnM&U-ej_+IP`Jgj4BKfVz)Jt?1O!Oul*Df!y6_-q~y@HlnxSzPxp^?FE
zwj{SCE@oLYsJZWS`+L&XV%Vy24O!xCj_T*k7OM465Bw`(uhng$W?os{WN+?46f)pf
zIp9}tUi^P3|LZ3Hmv@Mfkw&Wg5^=)peK;|FMXdNsmw+engTF)xm}r?fqjhXgqp3Ai
zxiiKcl3r4)-?L;P)~P-U?%F9u_ZLkq03K6l6dO7<j#A&Is;AHHLR#m<WMDBcR=8}g
zVu5C%sS8lnDzMhE4(b5Qu8zrb&gsifuPFW->Q%bJNmriEU}oR62oSHL`P63CK5%%2
z(0JU%K5XMk7u&SQoLO$Oy`Z%Up$M+nsBLAWVK@6c?Y!5*sHkL7K4@I982=$G6~46J
z4A05K_R-3>ccdr$U<c}tJ=*W+;-)jJA9WJV$`0IpKlIz9ojDK6m_L$wE%Q`PBk*FZ
z(Yvixf1_oNF7&+T*`ND&!<#p61iBJEyNk#0w;53RokDVjXGMo8pZA|v`&~b@kN%Os
z7O1jN`P(#UH?r4H0P((Oos5&A=yCtkLM2<zfpgoJY&(=OKx)Z-popNhBI)+D$i@|`
z>-guc+i-R{M+-px?3Ze-*cpqISMi5Nsk(g#W%rT!-JQ1Ac_X!K!UjXQhEM+*pBx~U
z<7xRJae$bBx9)rKk-DNkWD}zMK%TK^Xabfl&~BEy(XYyYuS`U3lc8xQE=XC=SM~B&
zCj?c=)N{J;d}z=BzZcV<Hs)bOP&JgP8~F^35($*@U57mv_cJy$s?l8mAmIuUIwmv`
z1R8_K7V1z?Qhz=DEPMs<aw_0LflxC;{WTje_4wiMd1G<b#fBsY3OYR+>M|qkOp?w~
z)8@!XdAVBmbVZndmxHerU3|Q>G3pOPxtdAn%1gsE5)Vs-)aBvVtJIC(-bjfDwD;;~
zsg%7(^XByiK{7KQX<1)$W%PAj=jEB3UWUZS>iavY#{HUdpR+)mgjEMjwc!cJ|9Xlh
zvig!afgNm~PSg@PO$$%W+~~oD{(B`w@$ZM5i~o`9pp^6^QT?{yr%8_<p2YMOo@Bvo
z1sD@l*pp?5HS{=>#yA?XHWEy+-f5Pcl&485vt~~?q-;Y^sT9-ozk;&|bYhu`5G;OX
z`xEa~BaCb$_-ItZO`>3IzUJ`7Y^yLWRutc6Mw8Ol`3(u}&;}E=CL<vU*HB3dwzrnM
zbQTh;9Tdes?4AmE?B-c!uDD|nO$a?WRjk+0@n6DR((;VFvSL<liF60;%S?&IRn_@g
zG!NhCDG;v2BlZF<<n--a_>en|^9zoB0tHK3fpULIy8R+X53YdKS9FP$1xK70pUa4%
z=TH#t+ZPgy%G2paXLE9&%hY?~Ze`6FTPzj~TLLd32o||0w?lO^;Sn}Uyu3;S$Z=ou
z#2IC@Vok^LZWlSEVMRf3{l{sY7s7F~cSObZkBl&f;F+KZoQ$HWHulS?VzS&KT;4m4
zgtR}&(tcGLjD0w-@{uEld`Q&a0$;))H<VsIw#bdKdRI5_umQ2eMy**ZpU*B1@*CvV
zCRGa?R`A7MO;@guHJxtuwaDkKn1(lr>!r9`;~*RU)J>?8;jGgGW{G{6<&wn-#6Z(`
zHgf$_F{2_13Aql^hJe^)2{o|ghh_ECr91^AD7BQTG!z>91R*rX@koz4to{C5?fvuI
zSt?mGh#~bx2ZzD=UFkVEWst~f<kTN?sp;>QCo!BpC&0%VHT%ta0wzJ=IFu2|!#6nz
z&vgDSzQLm~cY@64H-&k;${e4kDZz)PF*6e)(ExxP1mIzwy?Ch(Us!BmRt+4bIa_kK
zq~4|QU6TAmS~E2NmL$?jN6#-3DS65E@}-#Io@znsTkan}B@PlFFJuXpg>p&Ht%$z7
zx~Gyg&vckKU!^V;!P2>5cS@+gA$E+wf~wIcuP-P@I=wijq)K&8e-Zw+7^A-A|9XZ0
zR7kQOXW26_Vm=u>C&PSrC!&s3=Ag)eYTwgD^lgYNQ2Xjpri^k?zUmBPVT{n_Y8Ykm
zg%Icb!z21V_S8x|Zs<3-{GKwNykKCvo?yAcLqB51nMQj2x9@$)T!{~ZyGE)}swoc*
z!3Me#S~R+E%wRc`sR&oyYj#T?0=Tu$3np!IO(2Au$9G`4H0{CfKUvqzcUl1%WvkbJ
zX0-q1z{6^FIHu<hy`oR5-*&%2a%+^m3j6K%#sW<_osM>Q&DMo~uCK|kyfV9Am7;0*
zVHjBVeYG;We_*ubmdG&`T)c^HTv2q`stmzW+zBSs^h$=V{<^`eZbpEaMyv{odz!yo
z@X@Q?kZ*y%=2mM%zSIeOz9qkuuVuoymmiOvy6vnbs`^t9-yz=Dx~E3HDqsEP`Ju>E
z@Hz?HdzXA&sh%ie8sJsTN1@_Fd3Y!3`DdD3C|TC*QBh(9|GxYIQ0Jt8+@Fp~i(KIA
zW=9OQ5JHtWM0ZVNXWWKmP&dSbc+h*@gt0A2K5TD9*O#8>E2TwGUB^jr-EYmr5YzJE
zS$Q|Ho@AyWG-5ig14q$0ZAnV}EX)NcrXQP*-}rI)N>Tr?PwjZx6(?0dg)}07h%Ev2
zAUz2zL}-lo8!Pb6&=8AH=xUFpia?D%>9TzBKD+y2NCiomXiIv0AgU=o$Ajr-ao>_1
zqg=OB9owgl<>*h<;}RMKrXVP5#OAV4l@iO9*5>EL4ll=zW6!)QGC2F)K!JiQE~W-*
z(X?8VO&s8Y3-w4r6FN?=V0k08c{2-qH=u(tl4jc96yUIDwlKe0`}$?$X0Vz>#>MH!
zWn&cBWA37qjd^l9=i3K!ai*1jz|;;1gVmr-z}w}glKMkGuOaUpXHzD?p9ZfFkgu$t
zdI(FQj0$xzI~rNff1d>ZefGv+8c6To_2r*D^@s7$#3|dJWNlE=(<Jt>Hay)I0`Vkj
z*e6F@21+r8tFflykI#SrQ6MKu8Tn=pUMz#n6D<i+C$)HWo^wL`XG(x<EiULHfZa>s
z!$c+Jm<M4sZS5SVksGICzip|}&*rb`pKG1-#@tdY915GUz7F%9Apk?+I<?bZuKBBM
z_hdS3z2<Z+Kc7JcRsi#Nl23R7`e(i7I{*oJK^1nVl7b%1v#EVk;_Wgu7H0I*ZpUw8
z^z$7mMx4%C#?t%E>JuCT7oO2hdVOp;Ic<*mJ+sxig|2OQbNR_?V#Z6x%JfL8dTawr
z_66X#->C1|ZGiXwP)EVhz)A3eL0Mxx-m~Xd7Hd!Fj4G6nh`vb)&&T`7N&xoPE2RV1
zthr8aCY;5uWI0BMB1y$!$4W_Ylv_Py9ZdvX#HZ(BJrj)ko1Nt6Ad3DZchesOdW)je
z-+X|HSiQ%0a!(Zu{q{%{CN^MKebaGV&zg@T(h0Rr<GwUC?B00~X-=c80pF74KkpA3
zT43;1wGm~ah%;ULI$g0lA36mwlpn6fOvtM}xL@aCxVG5FCRa0L`Rkf?^4;#4M#9}h
zc(*?dnXF#wAO|1lTonQ8B9jm=oo=X_Q<6v!lw9W}8{bnx@fO*Eb*n>uT#OJywWZPi
zdfE~NvwIvk(2IDZJyl}d_coAY&PEoNIrqTPxr^#VN%T3Bg%dhuGk{B>8Sv;ij5gBB
zl@$#tGX(1>jA2>A;*r|Mv~wXFONV;3)?aCnbHSr950+PEqkyfqGQc^7KudCk;xzC~
zNNxDf<h*61d1~a6(2u&!fQX+GgFC{u$+(&F<JIvUGHb8738E}d2ZPhso_bmYOeQ(9
z`HYAoML#XJAw6`oZ=R9wi<owfVWyV!`AaM;Jc9q0JPC?16TkcRi-I>qh=V3por9uv
z!;^+6v5!;qyROF{h7>I>F<$uukN}2X_=6zV*G(h^RAM-aYVZ-r881v6RVR!Wk1=$i
zex=U;PA+D6%b3^yGT)@s*bOprURsS)sKGZC%<NcCR@?8E9IN+Pz@cQR29~W*Q^eS+
zdL*%Xuo&vvW8t1=5if=Rx6i0H&>dH?-}l=vKw9tvge^SRQq)ve@&#EU9JWh$*PN-~
zgs?vG$)WXS_C`31pXyoGLNbwVrE&_JfA@+q);!O0JKwpzTG4!UyK)FrB_8m5-P!TE
zC^GrS@7FR*?|9x#W<k%WNM3jaNgiDMk!G<nvfjVC?f^%vmHMPEht_o~Ze1F^S2*mu
zOY59~;IFkOq&>dq(c1I;rFd-5c~c`SaDG&Fl=}Wr0}0}3_5dM>ibDQSAd*B?E4UCX
zeh`jo7S}zOKWsM`kRd6oPLreP1qVklDc7Dk-;06I4KMmh?M=JXk}6Yi;6`C}vl<lp
zv?<!c%Tde&4f~l?8mIDXPjp5G8rczJIIm7+L_)^?>Qcf302;ArC1SeX?j9^i(#qWY
zQor0t0STAcas}jW=V4hHl$u9giyG(Ss@~LWIS~leM+clfTUx$`Qs}WLkH+VbH_k$t
z2%`{e?N6BW<^=c5U}?JXPitcaWdtVE4!Foh&vVNxO;Sj}FTWGYI^k&7=Xh9FP|<LR
ze?P4bb~7F8-{VrCkthJeX6_!u#*znvBrU}+LT#JQtQ^Rf{YvPiO<*)-Zk^&q$%KG8
zt0hmdD}~}?3-Rz^)9JIWd%zlTz=8J?8L~Wjh13IqH~5Mx`UCPZ;L5_V<^JsBmrMn+
z$2Qj=B;-*6f1y2=*8mcGSWq!ZjDx$Iy1`bWrzCuiPv0(J+N?x7M}HgtACiA1_g|x+
z{F5AmQ6PVK*Z(CMKj|?aWo&<b{nM&I1%iaWLL=Tg1`tn}HmAq}v=odRJLE_);y-3h
z6p!36`fhouOq`>+4K&>tiWScu*gi35ssYL8AvmW-&n0tka;M6vD*O|?gewOtePWs4
z2J?KK6P%vk%rH={Tik{I$@`4KCeQCS?EQFL0ExD6ff5Ch4t|LG>3#ZVRR|nEyjpWZ
zoGvyT)`@QTuKU|DJpS97%_8Fqqs>w*n5Wbm@ih|~lw+V-UVQg3UDYb!&Hd-TJ*5tB
z|G@ZzxBZ>5zquGESm@{nCN}IlCjuSrV|Fj?ZsTiW-)T2E{tVUC3@;(tJF0C5=>>O%
zZUhR{A8_R)whUS-g2Bb4cSqIwIcJaB0gdpV&&gG`x1@w~uns!~eXAdf*M$GQP?%)A
zB6+S0v>+Argp%Q^oCxgp$apWxpQ<Hbe>zaywpV{#nPjOoB|Q!0VUW|`pu(2`&mtH)
z+$tBa)ksi^0~okk01qH(*#~RDw-~}dTECofCK1b2iW<||XT6p5W31#FB@!Qx8S>dj
z05I67=6QYnas8`-8XQIeWg*0c;BX>*j~Ya^A<C^l%}iKVUv*EUIg-lmc|Jp)76lUZ
zvBN+=$=thV6algZIy#Mazw4Cn?f&TZqHqn=U{pGc3eBkp9{lR5mS<0z{nW9Aj3jbs
z<Pa2b%GMBZr~to0eE<PzwVzwCFcOBt!BYgE%O_r#9c)K(oq5E&4o!!C^P8Ya4_BSf
zaG29&3D6UT=+Fd=WAORQmH6rzX_K#EQjI(o^Fe-jR#pUE_trC)D6$B>gOt~XEpE)(
z{y!4{(RAytxY?XvN`zLW*}MX{>1-^h=4n`nojIkfw#ml^I+(e?hS)58){E^5a<>Sx
zptEWVHEG4u4El<3xy1iw4F8|Y{m<~SlW%kCQ_;q#+EHX}1kw{ljBqsYZiGoa!_wv=
zk2=@j<4+sgz$h=;N+4IWl&Wp~k{K9<{88`9MJOQjMb)8kTRbC&{xgIf>fB{iqW{(S
zxA<#y?zU%djVlemdT6NHRqGW=?4W9F>=V%(Xaq^2pJS`pp@i%Kd%1eIu~YN)T+CHq
zL$7tK8(NCOx!G<fql#vdUbj=#$2ShSL|``RP#tvFjUhoHQH{6mqz&)lMt%VsZr{po
zmPfg6(IOL?qe|AC#Vo3&C~E`eeP3G$0opecOOVp^=44Mwtmo(UXGmA)o!auf0<*_;
zfj%F{0^a1@Y!5xLxPHa|H&@tP?pMOO&z)DI@YqT72*>rU9GTtSjcAR^8Fo*dTg^r9
zgazzS=AdF|Rn}Q`;y^h8p-$Q(ypQuRKn|QE={;KJ@9*^h9{*NPf;&)jPgX&&S=`QD
ze~P&l-Oy+x;aE`iPgsK42S(Nh={n<>Oeg>pca9I5eEZq#+SUHWQ)9>NR@9*KbMA+Z
z(+~<_ao<L536KaR4;$e-uqjhQ<uCJ0Ep^;ZUXO2_fU)v;;BcrfPWJh|_F3FNvjYJ8
z2hDz7ffd%N^O)N`L&HC2>IO#C(V}&yvJMh`G=g0Evx#|H7se?dRW$=2VuUdU$YL}i
z>qmqs%A|HL)0CtoQhhOg@n>oXs(S&ZgT~>41kgKqp5r=!x&?yf<n|Aou==pW1Q@lw
zoD0g&e#e3TvgspyUc5<y%L-k5eGT(Ji}dcku(EPmgLVyyDra(}%31EGd{5OxU&NY@
z1YFWHa<p;*VSUojGL<np%Ww<5YhS-;Q>{{-&;oq?e~2)-!M{!*LYZ|6bC{2TeD||c
z@Wx3$qKZ|)Y+W0^bOdh)?dm?>CFs#5YZjlHx;M}OQin=Jw3eT_QD;rq&TI?zJ-y2w
zEIs?gq!=!iYQSSS27UJ0pk&y&(N#f%k*yMDe<6+Z_0!87a;4=*4RtwvK?^o6adFRk
zwW_!W3B()u@P0CM_K7vt=nd$B@XYIy2z8NuTkxt!tn4vcReX1yzbmdK;z#s(9|x^!
zN_<WjK6@<yn6OK4**>DPGCor_pMAN08~tpiD86I5Maq@?n>Nqj(uvGx9=iC-0WZ#!
zH#mDv;TuQCmZ9i(Y#p7y17(EX=?6XeebO3wXq}4fyf;;U;!Fo3;P`PWW4NYq^=N(Z
z<_bNDY*{@m5GE|XC?|s?9DLhdy4>yY0;%*AZJo(4DeEAhyQ*X<dPI%*aLyL_$<r}(
z<J5W{Y47nE`Wk+sN>AD4!*N6-cC4aAs}T=5{?$SX55<!uwB#fIhnzZ`YAC6SIC)?)
z#J^RULhBKe8aXPHqSq&cjHByt?;C$RA5+}E&SX^%t2X&~5N92b$qk@$$ca7#CE-L7
z57)--5o^p=-5qEdZT$Z8T!i!I<DvrFCpZI8xh*j;rO~=_!AxN_uzcEYMXtC0p2fhB
zk`pVTu?kmIQYn>0Q)z;4PQQK92mvtz$66lV86_57DU^HPdq9oF)5ZTHPVHZI41uj&
zjC*GyabenGq5n*nUf@(Kng%+=d^7hCU_CYq+*|RmOdD&MHVGp6W@2TO=OS!uA`z~E
z0!0LCEzo%!*Va%s4ZfRzT~zgbqq#unElKMFRILKLvDtMbBg_4M8rN_{_6O-R{98{&
zg!O--_;Tv2J?8Z`Oo^R*3P+KT?t^*}#*PLmup|x@lecz0qv4JiV{7<&j?un&tuKvy
zOsGeKsmL<$Lh)fJORkPh;&$%ecwpi5j^VDJkg&oR$d|OI>XdTO6#WY7nDoHN7nMji
zzbs?(Ui|t&m*(=wnrY8ZXh9$%;xIhugJn_Tl2z%XW|x77w7>!S0TTD3)-Z;+TPC)D
zNaU4!`~yw&xYkkHDD$mC(u6v2j!)yosMO?dcdF%p?xor?=e--NQgm0&!px-w?`k-h
zYtBwu_EQFBzFVW)*QJBY%h1lw<7<YubaZ@{F`lyI1!`pPsDE$Nt#r{@duX5$A*-a^
zzZ(VWZT#h{YjQkx&L-~^4Bl;RKgerwq+EYXkSDBD^0F(fmiMx7d(=aETZCV>pY!se
zZ|C%US*6bR*LFR*TpqScEc|gq-w24Ay4=&Q1XspKQ2#^c$E6c2#mNZT#i9DgG8n*P
z#LL}2@VWo+kQY>&7!V()K;Aq<olP-1ofcEn;u{i0Hxg4Z&onk=yWOKuJDU$sO%CZv
z?iI_$&y#D2NF*&h>Fj4gXf40F>EFv)k<prg8b(h{^xsQP9G6f$2Pcf5NmY&!UncQX
zU+^!2U*be`0_)s?2E`Qq)i|?!_e6#oA9LSxO*W|2YfjE^$ruh?C{Xrv1z6@l;i&*@
z=CWBmD&;c&JTf7rddg4>ksbATb<J7J2f!Sqp0bUJpZutRd^$hSly6i$+x~l^fWZHw
z>Mi`5?xVMHRJs|XK?Fu`#72j7ZuIC-1XQ{W1oS4QTU1Kvj*V_cGnx?+DuQ%30{YDR
z{yx9w`}-F@pYuA`d7pEIk?y%(f&H*AMpV#zI=bb_SEL)BK|5Q3dfWDt^bMKv|9hh2
z=SBSOsXG_1#xUccjlox!){Z6(TWmKM-c?8nPM^|~Pitvor^xjlZ=fifar8hu6;lM$
zS<H1-`ha~(Uu;VrNt~f0aMF}{Mj>B}cse=Q3Meuw9dO2J`#lpjCotPKas}#lv7o1g
z0b4hZVNON8C1MMDB6)<#i^Az}f9MTq+5>wtat89Y*EzN|jO=z&oFUP+jxvG%edkd_
zYwl;|(>FJkEo0nf;Is^`!h%h|mkVz6g5?A*sD_d>)Mfd+oqYhaR69p)ht4yjoBN{w
zyzBQ`Yo_`pl{U68;A1Jkq$b(@?ZJA`eQ<YE^tNwq213-2;Of{799$9}lhBd**uw`*
zgK{211+`eUcJQo~2!nYhFWQQy?w!|HvA(&!m!LYc38i&7>rq9_pDig@prC&`3~flL
zlnm82dZglq<sww^8pF|sgjM|r^)ajoL!-DaK~CFPX#yFKcdQE205#(2iU1i{=7+X$
zcQ0K{0>$?$8DsXledv8cF3v$3n>w~nyTgtf6*Mpq?I$lp$~}2sV1byHwqpVx41j-=
zkCd<6?Hci+R4>#%bj!>e^9F6hdg+H4cSfLrUo@a5CoOEs^gT7K-QZ|NP6?A=k++Qf
z*wKpOPZvw)#>aNjbVq@wWE{9U?$^>y^s$@5rfzN~Is$tmY}!a%kSnBnBG9L83UF-I
zG4)Qv`oZ*>jj5hsDiss`#N^M|c%C7K0%XxaT7&4Y#7>cyO`PAX3tF;~0RLzv{kM+(
zp5yHe@pXjvv{HGne-W$6&k(AyEMDyB!*n-Nn}@bo-3(5a(zSf_J}}xA7&i>^988UO
zm(J8z1`L8kUee`^eyU{!ZeB^|PCfw$AuE-%Q{gVOgNo1~^GKt6X`cuu)eOtb*G>n*
zlQX2yW;=%;Gj$gTt1+$<u0fUm$doR*r#mk_n$VA4bo6`mvx>;0^J^Ys#_kh}Sj-!-
zbT`Eq9;u-f`?}#ZyUUccCumug-_Kt0#Xdciwndt)XU`$)VS%Hnhv9KbHI};~U&ED>
z-0Of#?waABzAQ?nP>V$D$JcyoQYd{IOfpsH4|^62l#UU_YCV5Q?Z%WezI*n>hJr+d
z;pa|ZkdlmaPLA&0nUa!V!jS%KN6f;;rhwaRzXKQzTV`g5yC1{tW+8g$g;H7m?WwB0
z$$O0`_j1xx6)zYS31;uOpRDJttKV{)TOj;SuNctoNTkNb>RX@JmBauh8Xj;WJ5DHe
zC@a2g&W<{QvR`TH23jQb9}Lq2hM*zQXKDZ-3D+QC8-P`fbwEU5O<0Qy`UJYm|A+$?
zJ>Cw+r3!QF`?_u9H-c@bi2HqOug}{`-ReZOrf@21N<dI!IHzm*MJx6wje*Q)0>~u|
zdOA&_GnGV{BVi1~Z0mLFh4tjN?6a4_r*OSS5}!>$DHfX1?wrEC6A&l5sq!{Y4b!Kf
z6<rIuX={hE)6D3@zKNet=cYxeAO_=S`49%#wEPhcgOrvg=h<>d2EQ4)td3A6ohZp=
ztkybC5QTrn5_kUFmi~=bsU{$1p$SpTrIT?pX2j>L@X!t{-jyd#z&MIK10T}>qB<PM
zOVo39h|^UZAChH;m_swL>n2UBXPn{@@JEv+CrAHiL>n!eSU`jz0)?7I;&T4nEb3)?
z#jsxxvj;}yb8cq*r}{c}*%n}i;;c$<hwyp9T5Eq@I*YgF#ph+=@!P>@jjHpHkat&P
zP4<VTZg)i)UNo(sWXcidOmTzi-V8(SB(R*wH8EDQuhDYXV;1{<*Iyr)gjMV~o-Z+W
z-YO1rIhWI0_0fZU_uP7AlZlq~p6NPg&{=vFMfi{BV}=E4vl^FW+r0VgzH?#K9HR^Q
zy_poQn4!6<ED`+_;fJaAwf?_yioS&4eyw+Sf{N@Kn#kc#|1`LUUevcFayt`=ZfuVZ
zcWxtqc`|z$j#sr11SgpqRjoWh_cpuKrtI!2_gc;9uV<Is)Y}3x%0`q<1pG;?f`ICM
zR#CF5im4B`Yv9+G<Zb1CHY@xFcb?qSbQ_Wj>Jk7Y00&}m4x9yhelNo01gxtdSPF%Q
zWeUczj7NP@!oSYBC)q3b5rZJFP4#`s()1=@VCAU<eOY}LpYJiR!WfzzoIUERj(3oz
z0^YD8V2D{s6<}tDGzy7O(bgJLuY*t4-HC1=nF7U$Ax)`F)W#DfN;IYzSc3$or<Rzr
zrmMr`^&rV^eg)H{db0Ap@Nf>G>Zu}x7YojUt4+IuN0`i#f>JEzH*9mPC$vA_enb3s
zR{H<!5Tqz?MXEodV-7TiFoa_si8dndN-`y2cxqmNBXpRehPabIDAX(RMCjzF2>R7s
ziG3V(GAXt}y_C#RFFM$wq7J9a=z9YACGk5(^P`F5$%PYR39MiuC+&l+Zrg+G(WiAR
zaXw^@1?ukfLx{bX=82YX7@OQN;v_A|AL($y8Ci?GIT5ChL?^exvLA>|z};UnAbP(-
zW!P=4%F;{BcFw^;Y>@E5pMK(Qd(iZP!TD25>^tff`{YNw&9MsW&Tsoiq!PdAH<RT|
z95|X4?^a3rPAxWCYKz|9XYBEiH5_2P%@H@gUvVPy8}E=@i){$FdUmX*G=1?hxRv3R
zlkCcpFGp*Rt4{fuV^xbmLd(E<OZ*l0d56%i@4p+i7YV;~P{w$C?u-p#J*+2fsUh#A
ziq>E3at6z7D`2;G{(ur&DQJ}r6@VWxQGVPJJ!2_bm)%0Hli!=LQ{jXHiDAf7HLq(9
zVvG;yX@vSHp)X-mGU4MAIU}{s!iFlIf0+9XfMHqklbJ1KT8BGgYXe8FRgnDexg=t>
z^z<7&jg`7gcQv=dCd9{RaHk^LlbLgQa6mv8BL-??v(qcWO<F%%t7alUga?2UjS}Y@
zZG!`$W!>^xWW>kGEnjyolN9V{Xlsqt3Zn@?!&(3?CJBS6=`-A6Gd;ClFfueMe_*Of
z%Rr`M=8ssAl8P!&jZ{lc`(T3}%c7zi9B04tL>`^m<uIK5i29Z(W1Ph@{A{H3qgFZt
z4NOj$UKI~%ME@1jzdcB6<u~oE@SaDs7qKGv>xC-k!G?kd+1>d5dy}NdWEfqF&a%-<
z8#7CybPN~jB_hHU4{1+KntJ+P@+9dOO+=m&lc}W{^Pd#g^0_Lf8WkOFWj@N03&!OE
z2T!48=Fz5ZX7ar6L@hs><<^tcy<>6<H7`xc2$!3Ne^zm4+-RH0^XPo&gnE^_PC?T4
zJ-^|_eNKjdIxJAI0LM{SeAuk7Y-_N{)QhMmUPv3sF%6lIt9xHAMzu^6MpPu;FRnc>
zFRFBZ{OkSNkz{Pe?vqZ)w84{50@vMZQgCRH`<Rc5O(k`@n*D-*o<gv@E(syo^V7v)
z-u0T)0l)M1ifh=Q!WIIY@!<x&Qq=ogq2^Cee{)A&xx>c1!anr!ju5pCK@YmT+u~Ze
z`t2<eNt!@`T&2>Cy+wX~lfG-HkRuDJS~d1;ryRb6*ier`YTfBM1c{9Hh`abQH4D|6
zvtuX~9_KZp2tgB2a4q0OZga<-tP4Q-?yNT^SC$>(87NKF>3Na@(Eup0PQjyEz*ma#
zDJr*h;xQs~ee^J1VMU*?HUZg#?n5bAddklHp<-py;8q&o_nIEv)YlSN^wxr0$5HeW
z_`dJFfsiuYN*#Y`jY%C;<;d@pOAlB;r!ZJf#G;Emm3W3in)Hgp$6^DeCRC${G-yoy
zgZ#;>>5hbrv{Tuw)o{s+0VM<DICJo%Nsv79e%;W^2lw6)De_yqLl#(QKXZRtZa8z|
z3~hLp1|ho7nD)WY0`31SYs8f&vrqOU47YsWY-h}I;W=VcW@0)kAqe?=gr2#6L*n;e
zn!=qof4kRT`q$^bGZgE;7P!oxZu2w5Xx_t<9KCMc4aL|SlY)~au9GzLeBS5V35&ez
zdQ8WX8E?s&qK7{`l<x&8MJg2@Zw^cOYJKi1LJy`vdBr~(3YrR<a(nB93<)Kc+Ipzj
zeKL2DxFUKN^eD2Iv+nIKN?ZeNE+TFnrn&9giYzv{YWsT3T4N=8;_CmJgt|)w^5WGy
z7=3K=vh!S&+5P9i13ysaZi}zwLpx_{j&17xh`WdXI7(xaO1HKzc9f(q<qH<HigZ3+
z5ayHWFZ+f(Pk-Cq;>+E#$ForKjgk3wVVBIJ8z+>-Z0XEvsh#=u`I9+Z^YaTv-^jf3
zh30g&U78xd1-GA<4~pR1{W*L0>Hj>)L5#NTVBcJ}k8Fp1mo$`H>u;-q9eh<Pd-VfV
zu7A~Bs^GH7Le>)N+d|lUuu4QxOJ2ITtu*o|@J2n##+pduFyxrD)X3rDpq^q?{ZfDm
zXZS;lOfF&)>I#n}#w1gUGj$6fA<RAEtNtWPUa8jH%GGSS-#?)O3uAR^^0jWsdZ>@E
zV4uQ1g9j^8RfKfK0_EoQ-9VHB5@I-?)>}{6E5s1qNG1V|skGa>+;KQ}QteuI!YZxt
z%w8H{-@r|o(W+K52wXh1aklh0tN=b)v+$a^!Ni?R-;jP%60TcktE2|sN_}mqJz&bX
zg~Kg*;d%XlHro-XQ&`2!8F-&<rZ!~pr-dYyRrfS<H_dW34frm^;$5B1fn_>TjcKQp
zyOzBCn17zPxy0hJ_q^%xzRm;?jC}=}4rl*{Wm<6vFq1$S{IhtX$m3K6I9+z08g&<o
z##i0#|3<C<EQADzDaa!525!vxX5=^F8lRG6aL?#i5->jq$k-zkX%g|q%Z+Zj5cW6(
zZ{kZZR2-J6@8DakFhnMqV91yJ;RaxSKic>nF50o(T{Bxlz_^q_pT#VEGXJzRj`*|4
zJ97e>A<5{x$JJWy4D)HhUu5bAAroKNzeakt+IgBp_dZ*BMPQpNy~r`DC*BEnn7#T6
zMJr%b+}>Xo@MXCT4CKjXyyM$<86yy%7iddYql<qcaS`)4!4_87>*Ko^!D?jJ=2HKw
z+3tageDsdfEBSCw2HT|Rr;8cczTsmlTXJ&jKW8&$qmR(mgQhWBZ9|8zyvNmP<QIDn
zTLZ9Tu1b5Q?s&}3+-~!%@o3}p?0v8rS~=$XeGVdeNBg@}48_@Lp%)wKPo-#H$b6R<
ztf!}LAO7NpO4YSUs}kZ`n65}H*_^if5X8JFk{GJmn3p7BN~E$q6}GAB)}yf0T$bth
zM{{iZlD)$n@ti?Z+%ICf{V><}Zf_D{{Bf@Mbrw5xTphyFmf+BFPd9EJ(Oy+_v=*;N
ztpLp%rdTb4%F_XRMed_hLy1~T!JC8)$F-=MAvdsDeUXv++CfpW%XN(5K=}yK1cH!5
zYz$cd-s?mow<U`JwbrcTDJNRm-6#z}j0v{q;i=9E<$Pb2pX->`G<reHlE<{yQt?yj
zDZib;QSTe{M4Dfem4&GyiL~W2Axm2$fQ`x1PX|8>$PH?{t(|n@PHUr0rTo=wI*!95
z@5~7!JnbwQPhqM}Zc8h=)$|CuS9-~9H1s1I!*_y*#ItndJMf!^TmKhMRle<CC==7;
zUx@Y-%n~AkOi`NL;!lxKarQV>5P-QnfRmm;2{g%~o@~t$2VbtXWKVXNey71_@^-P3
zOp?HmCt>cCJcu@H(q5t@ONvH58Lto}k5!E`E;s&>u%)Ofh%BFTonhm>?<-zD$Ag4J
z>X$eg7DZq&999YT^)~zFjW>6v1Iw2@P&#I}>muHMuRhaR`mJN0;qX~H^Bq6a+s=pU
zW{bslB=uWukIoiN^Yq?!tovID{ziSXnOfguRj7Ytn}J%Dw%P_x*$Pa4L9=E(2Y8k(
zct`~Xt@F{}UYzwyX5X=U{xW7&Uct`YpvCixN=Z^YjTw4wbJz1#(@ylwx5JD25BoWN
z^lM@EK8j<{8<>A82!@`mE5zOzpDKz=y<M(E%X5o3cOS<=hDj`o$io<-mWaCbfdb##
zwbf~E*Ht@wZI>la*`@B)&ez<Ah$PogOP9qe|J0<2h5oMCgAR1`B?3c2G(2o9Lh9*U
zuOnLwnxpsg4}fYknnyhDoH_|V#D;XKjxMQnQ4;Y4a-?AoNJOeb#%@*WFQzl20dS8~
z6_NZFIT2s-#WRtKA5<JY$@v9bB8h?fQiD^cfIn>}w<%yPQ><97_N2_2Zk1X>ml8c>
z9IrunZMcet`ZT-S0unwg$X+-JvD7aXm!H|MI@Kc~L^4>f_4gT@WXWm)!5t5$CWltQ
z`t$&A;_2xnJz>6ML~ZHZbR6WV+T=$gx-K(Y8khO#8)vl{2FG<L13HHG@=y2P5d8mP
zl7ohT;1TWHFf%SdjB1Y8`(b@qj&KdGi8qANY<w{TykeK2QVK`{{4<)&_m`6J2Yq{=
zx1b|9dy?Vf<KkGTj#eIg%|sUj4Ppp3TtD~~BvqjuJB2qot9#~e^;XHnB!dvu##m{&
zgM;gN^y}4FmA<=5Eyen`>g*+#xk2+@rrYjaP=XRDLh~_u=gi3DE0!E9?}MEzyYz6Z
zk@$xwV(!^zh3nOG2;k6EM#$=sdF$E(29Cm~wN~>Z5$DebaCHH?NP2kHJQjs!p+=kZ
zW#bAL^r%*z)c#|$e15&Ql4QGW`@*-agR_8f9dpV~>4EVMsCwbYKDyozGkAf#Np$H;
zz*bD#oxhgt8~VQ85u2NTDw}{Lol+*<PA6#FS!p0qfw)nJs5lSbjmB*GR#7&@UNgUy
zi`~BX)_y3wv9Df@Cz5s;#+qSzjpP@H+l}(?e=pnh@jL7nJ09VLB)M{NY3@+qzb;B4
zhE3rJDaq1Hqek$nCn=c$RGqMBv8yp@lX+5pgz<Y#V9b7hr@4|_s83T^p^if*?j-n1
z*Ce_{TwVWK?2|zXE>A|fis<$S?9QEIrF%(~)5@^@^5a`@tnVN}f8xg&nywSnvDEO;
z&Wl8oq+Ja?Y^_bGem?MqMNadSvf}n2I*nhsXxK+q0}kcYxu-sDt7eTLKlO&={H=ub
zWT)eZ>{O>GS?=l`g*!d^lFF8$J%47sYd_|%o_#u(7L=x?$>{r|4k>9G>Sc7M@Ee1N
zKczRy|Le)fazjp_YX6Yg&5Q{Uqv>GOxN0WOk*Mu(FvuQu3z(wjh<XpowG@2^&!-pI
znG~gs)8sda0v(YFf9U4xBn{N3P<wCeBtf?GRTdF%EPxEi|5%`uqy4mobwtS5ws64Z
z=%rq~<l`!hQaWpO$mPS@Mc0{@w8@_)@2wr4UFps5+oU`wn=PCEon;ri2sX;NOHY+h
zHD7})vy*~F4LtTW3(#jgj2Xckvr;(Ds{PYzb!wV<%9<D3IJ1n}0haMsx32HC1oY`v
zn1-&@&1kPB2$rmxoPT^JD*d|Z>(YMW^F``?;dxg<c{|5YBGkxpISG!j^cK;XL+(~L
ziG3Q<w=eSil#1?5t;c>Zcp>Ukw6*+c(04DS-5~VfM%IhMYx3UM_8|#|Hs+YHKqRB}
zXQxLPFIne7USA_VhgF&`ceqw}O8u>ZYIWxX!iY(eMuN3h4b1j|wP!5Tfu?N(%yGMs
z9O*NvG!9Y-9Uya+TM`tjl?5mpJEjpRi0CT(90Z4UuJ=vI2X2>%l>Xk)9%aws7@-VZ
zz0@MzY2eUGP*0br-k(REi|k*8T-`&<<Cd^Hi)b!glk1E3aABKojF>&7q$+%>02DrQ
z!k(k79t(8kRu#8-Wsq;I$ghTUy-_rls7Vdg8Jz~7)lPUUsdt-r*>G7+9Q!}98H&>g
zOvg(08WEYNB$MGCZR=w6#m|r}NCPt-I{6`7cY>)kqoj%Ou*=ZxH;SGAmd(G9!~gc3
z>W}bybB14kqPrg)z8!uBz4dm08*q=%Ly<UHYxVGbD^!mp9aD%;G6lHOC+j{$7posJ
z-A^0&juW)uivqsuUKjgtDitFVZ?Z+FtANR)`lR8CS2o{GVJXFxm<sJi7(6aD-7)N6
zDxI}W)!$}p&b(jtH2kYb()wvLhgbC1DxVVe?5xnD*|qc?+d`H%*#q9pJK1pl=z%3D
z&}QlPx$WpgO}aHvi?{=3>&d0fU2?_Gl}=9QbDAwi4UH?CX~OOC^7S7)(YUxXPnDMW
zgKGH=M$~4}=OeVuUJ*%I1lGLS?And<A;F4fi*L|A{|HCHntwpCv|8_al$U(t{rmmX
zP5W*wJ`L*$XWM0l6oLe@lq+9D>!VQ{ty1F-zc=d)$}(G(s#$r6yY{WTcFGM>Nv{=0
zroP<n-pIhx%T&{Qwe^Ur!|CJ`Lo@e3BNm0JG;4TX{E>(O$u#xR1+cLVXy=1wx<%N{
zuCyqUvom_Dd!n~mp_Exf5SNmTe>%i!!)X)&A9)CCQ6~;m!v=TOg>#KdkM9xyKr<OS
zB0cY~wM6m;DTnC^(-iW{d}&0+Q^3{RB2(p?5(OT=T4Z(hmN+;k#caM`Ol~T2QE4AL
z{sSIMqa2Pjz@2D@A(A1wv;EVG6q-gfu0f_F#*z=w69{!@gBeqFiS6v^Peg9l#MUKn
zdYCuLX6B^e0sLMXoL}02kzL*rXXbi8WZu~QV+J0+6bbN^^DoK4UYX?THxr)9`^fOU
zo8T4oTMc`$7nwq`9EmDCM=YRslAx49M}RCP%9JT>C>O8^o-|^P#9x<n7NOqIPg-j!
zPbF`wUgwMnihs0O%g{_v-AER6XUsE?!|{)o>~ObGR1NT$kUP1In8o9EPAI4zo4SPA
z@R=-tzJ7KuGR~EbvA7)$_&n`dZT-_Hhq3#M<6I_3|E;(IdmqN=J69~J{mZDAF^mr+
zpEQ}lUdZ#owXEu%`mw%xhWgCeye`?);*r5$I<0;#4yjX3sw_o3l5Zzs^EO^}KC6kX
z+gG&C^L!i-J31B<6Rz+u$n}Bl?nJ?%=t6L-l0(4G%U9WoO3|EqU!LzPlAk!d2CnZl
z-5+~U_F^5Se@ythU3C8bTAOF+{mXWPE4g*0u|VS4`#bEwT8x+ra+4o{Afl=fk=MB0
zwXyYzre_RG#sG2Gy&~@x$$P+}Mr%Dtt3`^S&Bp9cMO0|esXG9aVi)PS8lmbbWLkSj
zMMm+8uuq4oM}RH?m^Xb9@4W<hbGkvVk5|;nfF5(kzT`*rqFN;s`MU@Tg$h!E$rH4=
zJ|s6Cs_jO>#nZP(cyX;rQ$Aj9FuAhMEOIPyv^4_1;S`%pAl#%9*Um^6nHYsSNll(m
z?rU(-*N!1ZrWUi$I^2B22?yxGZgsV_yE?bc{M!^+N`XZFwpwwHwwiNq!}ab@*WLtw
z#Jyv_<uFSJZ_&pAmOebH_SRr=ou4?(bLy)L`8qi64>9+$K0{=v=;G-K^2*Bp(G&i?
zxp4jd6Vdax2bn?WLh-o@bBlJ0B$1f>dt~hK9JJ9od579Ncdg0Oht`auGJ{PSG6s3+
zKMZqS2}Jd+iABXx+e&7sKS?E5&(0vs(-ShZQo!(vXAj|nxFOIcyNH=JzvjSzJa*Ka
z8=dS;A=8swjY`H0hM8Fa%Bo`8b(K%ry~BFzy!raazHT?h%>wb5f=a6T(XO;?r3qX5
zu-+5V9Rv*W1nXFHl)3jrG5*8PLJKMeW68oMJUyY<uAaXh=ir+`9=t+PJ?`e&f)%-A
z&|P{6V_iBIzJp#?ef~@#p3nDovA1mNM#@oF*X)^3jmOv7vb?^DXV%nG>vikB&d*P_
zf+%LaONJ{aMN_3)zbYC4#!K2q)d?GvT;M-WL=r-nH(L^8L06arEVM>4Ve2SDHeoB|
zTmj;nbF0d0n~V5fV#pzlAXPpMMQ;L|I1`_z8$pGpnAPw`_W_y~)iaP+c+`7%!ne|I
zv}MWD<D)8qXJWRp5BDV_-?#BhatpGl8~Z8$$_GT79q3ddDhumSE1P7xsH;P0&k=k$
z9RGYXy{GF2s^iDhfEKv1^lHnJ`o4EQ^GHlx75_Gh<7NGze;lTP-=z(&UJjoSQUG*-
zi{A`#k-XwY6F5q9&M8R*uaS<Q`iLn)5+n=P^H-$O(b0+Hxvv4N2)IEY6`p;CJ)^QW
zv^YW!I>?qOK9{zXER%GAoy9${ymuCKOL)Rey}3`#CyyCD9S6%$9QNI9EzEnat_Fgq
znl|U#PQ#jBE<9^%kM1~WyZ*~Pko${r|Me=vMw6RAc;O7UkxZB|gegSjd%{NSBYsu_
zZ7u*AFaSawHN*uiR!1p{q^RcOEfV-##WM+bQa-FQJ|xH)cR<g9ero|g4s!Vh@ZOHp
zC>q+5aK@hH@f9HQra$00EmqaYJfL8&@n?s!u?mT~<D<NLXmfW)Y`)zG&U`zMh86}n
zaWnL{YV%<b>#0=Ln8*2|2(f|d`=O#23w<V#^kJi8v2sE<Q%3lrPihu1y@u$3D)ia#
zZ%%mh?96_EPG6N;;?&M#xg}hKNyC!fvsv4p*jH&|CNG-whR<GSt#pgOLTCU60gavI
z;vSK=MMqLzNWF03^_A>vw+Xn~=xtHKZNB&=%bGJI?ALhmy;VuGSzxWr)p)3(-Czs)
zMfm_;M$~%XMz~)J-o|~abaQBpP!bDx(Fmm$?zwENT4m3ab%TE)<rT{#OjFy7IoWM}
z+tNa$0O>(qw(kbQ*?A^1S?+3zyIS7Uf(Vyza*wZwa%m@B>+lT<fRhB&hY`<K2T;Q$
z0>#N0Dg>Gbs=K$b$@z9s@NtRibp+T?6w=^%;>=u>NKQruB~c|2@AN90lB!IAGI0vW
z3olbC9U?8RRL26#m=(%<Ci^kyIyU7YT;Qy(2C&sCXJ~(QjII%<z+iGe2~kI1ky>Xd
zT@H0*(5a~-qWys|0ZaWfqwaCn&4u-vB4inG8<{wFjYt`D><z*2(4J!Mu#R4|iWS@+
zO(8sA>6AS=0#mHT@YSVGKeoV96={o^^@`d~`;SaGO-L=RMlDLNghEXWVk(qX-P)G?
z#*%%d-{fkOzLEP2jQ?YjSAFN^4`FaO-h15<@5d3TkthBF#0R;H4|Sw{3l7m@O6j-v
zZd^r)vZbr=iOW)&@kl0X{iKgtg_<aSLgXf2q>erz&KrLM6$Fc@NGFahr@F8UrKlE|
zDbxQ-lVZU0qJ_^`ZH1~NLWj42(d7|iCiRQL=JNSxPwq90yVlEA++T4lvVB~q+dCiQ
zj{fS2#%}`9uPQLReh7}=rr}*qzOqeyYO=n)Pd4P3*IsE9HCdoPkepe|ex7s@ud(iJ
z&{NLY@y3;BW8ty$p7{7mDr?)0Wz~k}-}jC4vW9mrS~DPHmLu3LDb_uHih}8#Qy8Gl
z_Z>;;$gn(<jX@RHx6WsdtY2m$SD5$gFTS<t^USm~)07qNOOINUHYknJ@11vwsa@bO
zG=R>K`w41yZh^*Ifxq(%;gp}`z2;w$H+tHrUkMvJ5-uz6@l2DT41Dug>OF6xX`4N^
zRp`0*te5Gl^lRT_YUwRNe>TzdMBeJEQ1Irh_vX>y9Mst<5ztANsG}OBQ@90|G8UQu
z4JQM*?rKok@KPblud|QTjd#q6hG^yH!WAaxH<>;ir*epCj%Pw7x?#9wq0s>d;IJFD
zc*CG)RMx98gl?5q;U9OqKI;P#LHmt?S3IivRU>i)1~`<p1OjlaWMDYbon>K=7{@|w
zPL8aSLW~AI$}Xh`F4*+DOxs$Bq%BT{no4B2I~toR)CkF)rD&OfX9DrATxPtvILmSd
z)vI~xf&`yWlWCrA3*HPZ9&_%BVlhklI7Mq)Act0v3NrUG{-7iC@67%mM**)~Wg?2e
z<l+U_P!5JzO$Wn9bs>4Ogszpu#!L@AP)zTqBw28-FeFO%A&Ny}Qj!1T=o9)TKi(^*
z_XGm4Aznr|g1mtuKbf#0bNyVLgWroAKJwqirhL9bwe6qP3r#ltFL!lEWIoN>i(+}R
zmze8pJQ%MePz$QX7M}F1vB9sUn>E}Gf6Wk|M^wyK%odLniofo$uJ%!gHxrD<+6vRP
z;*->62D=0<7k_@;e4l3kdua=DRm<*NtY)K-Qgd!-!udBYNv(Tpr?}V1bNr4G{4_l7
zY26MsG0)j>zcl|yq)HT$?ei<C?SU&CowDr6w(}>3XR5}2`DUl>aTC!pa@}e_D`bBo
z>==JLN(e#JiyOF}{gD~x*}UEArH0vQsJtf3yf2E~C(bjN9IaCwXS<~89NQ8kZ6Bw2
z(JoLuEHhMhaAKSyY8Y!y#cLS&@CUzlK=gV^O$LNb-5cjNTkG1Yyw)?mRdAOF0H)`-
zH%>^y-&$3kLavisuAfpYvQr}1R~DGKBQ|+u!h;~iMFO;HVkdjF_ShB1l<1;EyzV{C
zdqAOM;5bX9YqEO7I|SnnBJ0A|PB6kfQ+V%>;v8#KRke{kirV|2DruD@0G3$+*9-j4
zWz<VVV*u2hFwsLU_#=&LCxszeo<aA~2m@O##Wv)y$c1TI7DTCmO1;9y6f7|YG@4F@
z+s(Q0(CK`M=5c!5Y!;_z``OsPgn!VqfVuRGgnW#{LOt@E>%^%)e!BCe;B>W(S09R<
zsY5!kV;aKQUR2FEhMF#F>)ysElK*XD|JInn`Lz%Sew+Toh}2EziXl|}k$0oIzC2sF
z*7t*r>u42FTwgw2iQ^**p_DE@dKSWu&P1=_c~d&IUxSoB=)a+t$=8okCNOaHZFb_0
zRE_O@bj+%FJB>YC7bIDtXxBhdtm`JR1(B1Kt+o`Zhdb5hctn|3N@j!)QC`<96sNb0
zU~Yn2RGu`l-4xO*mMm`r*}^M%12L=K{)<GYsHn7W?(cQ=bv@(9Y|tMzi5+zCqJF=m
z8{#~@2cumQmI|lzmLE$!#2OU~?r_A{TAv!;+%=Ds{;|_gyKwU(uKC+u342z&(4gYX
z@xl1n2@S=>)P>-J?C)cHTtQFMB6yUow}0D-Pj*a;lPjThd-t0`*c9pv=7QR|XcH5X
z`rc9U>eHqq%`Z+86=CQxq0Hd%HYsX|exDLjg%jE89A7yb#86KLQPqt*U<umYQvLui
z<LqL&s<Dob*3^xL{Ruma76UKks<_t<iuGDpOEe65(z)u40+Tzs_ded-#gvpb32;q>
zWzkEB(LbETV`mPL=D-W$ke2e8Z%m@X3jK`z)W{Cr3e-H_#9Z`;18b}I@XlF-cWUsT
zp%ieHTCZ>B&wIM#h(AgKNU(wQpfTuJRBJO1RE$j>hG}S?R4A+>VsHyoru3$QBF!hj
z1U(hsI2Hf`GBgbuFlAlVn>F1HBF!(s9NSDX1YXIQ`Jwc`oBHE&SH?^M^E)~M)4V{L
zXS^!L^6p1M{lx2Ar1%ac{T~G4&DA*fFA#i8q<EpS;asdfgeg{)0MFraq34Lz<`I8i
z&U;B2rS4cmrXFt#NYVoNvIJsWrK5&^Lg#9?D1T?`bNlKtF#4*bkA14umV`dxEY@_M
z*ub78$rPvvOSr6}TFA>rw)`(39=6IK=XTz;II=c8u3vP;i;=7%p)C`u@0fYxqh9A(
zZ_<~A%~IIbS_q{roDOM4Has9;ERBu8Y7d#Aj9$+0iGJvRiccot4c?8-SQUX%60Nxx
zj6OcR65tCsd{^@DS!&i-%$HAfyA{n#XGiD0-yHm>qCr{)A{fU{NE;>1j?&zN2G+~h
zfZ3T?U6gH{=K2Wp`cL~-s;sS+)w(O0#!{eG-BGj2e!adq6X9hU)dJu`DGo<)s(hfk
zc7irgU?6U++Mda7a9IQubdhE<|B@2UB;9UsW%avO^{^p0Qgik_SVQR$-tSjWd=&)j
zGb)R~HB{J$^?t6L%wW5B_=@MQlKz6y9Y_KwFj5Ob`KY#5|KuO8RzWobE^U;89-9KG
zYhh?#Vn`EM<^Ul|qU8f8UdrE76S*8RQSE@W?X!Q(Dm$_%qSbeSDbFaQ=EKFh6Z6j{
zRg8nkFUO;QGEjF+Q2qnu-ET$nA!>w=xy8MKvxt!sTOL_$For_2TVkR4tZ@Wnj*w5y
z3zct5m87wrC2c+F*Eqg0;T$(g6Qt`0gv<OI#VI-|Nl_6Q-pq-;8Wb6lXiK;=9zB2i
zjg<1gLla3z2i`K?QHq8f&v-GM3)6?tzgGRx$=vE8PaLoQsM?ub7fzb;hFAR3kx`8#
zy?+fl=ht?{8ZrD6)wpxytn|t7DU$32<t~%1#&2T9uA5F@G+vC-)sU(%&mGx1oR8fr
zv#!>66Z>@&KS&t+rNZnLk>4$kM4j^LA&AJXe*Qq&(MOz|uwcpj0g}xhlcw18wOs1`
zY2=;I5c`T}JHe{2gRqa?UJ7}b^)j{ecvai<xfDe8O_aS4sAE$65WVR)bN%&+7CLJC
z`+F))=t$<Mhv@uUVEt9|vbC)AM^BOuz_u-+_hpZV79<u<PgnzJcqC^QXE%fP?4~rz
zar;+{iV3=#dl=PV_A2n7EZnkZ+d)EW+1js8^6uZ$ts815RZ29fKL$Dzpx{|aimN<=
z4v5;X->qQZp3`naO0kg6{Syk3POp4D`ktq-E+8MR@xgD6sH;I@-tr$?A604L=`2kZ
zWe{5Q!JVYLiu#KV9QW3mNdUZb;BbEMBw^q?D5{)qO7-wnEv_b+{nMWMCcIy?$zzz2
zK*JmE4$(9Lsn_uy9YZAAV2<9>;3?ofPYEi;T3Rbb!j@3R7wJqQL>=%Gq@j%i#SA1l
z{6h@}#%3_{qK~2!gbr@Rr3b`@FUqF0Zy@SOZ<w0g_!j%dvVlQt9OKlf1dny6&~TOE
z=yZAE<}}`YoFBQ7Q30NvP!qy;6v=;Qo__;kzLO$EC~v~ma*br(6H`z{|GVCPrpO+n
zwVcF^djto?4Dlq90V6c1)8U=;EMd=_q2Yr?072Yd5Xn2ub@VR~t0zg`#QI4o*bIh<
zH(Wt7r|IVCeV#5sZ-sLQSyl9}&}<_0yX<R?gwetZj~9TIiw;uSypOTa{OLWq1)?(^
z&!l-T6E~1Z2~pjQ$c6k@*sa)*M~;B^wo_dGy(>PKtwGz;)>xhOmx9^c545VCVTZbR
zOI=gKzdn5ba_z~TvdrCT)=Y>zOYwqHUW@M8Yx(z!ZhpUhBVG|q|I@WUKT0KK+rNgz
z`$V*%{ik!c(dMxyuD!|!er>RupG-BRr)M5EC0`?(3g;}_VeS3drKq-@(4Ak>V<w9?
zO=1*#)n(PJ;RGs_^p0`O2ceiXZiCFbTWaxJ?7Ll_E!uy050K0`>Qva^pFKj?bO6~(
zkRD~bo{b(|2kF(D!xqC>m+RJvq594CBOdk8u7MN2BiZWmHv{r?9~&ce#4wY`J)>dB
z8@1x&C@R@8MC`Ukv7erJ1)$WLQMo1`dj}R*YcB(zIAIc#MhBAJ!=gfXMEk~Tu?hxm
zxlw}RQ`ftSTCvfSEqX`RirP99*FS}gDj?*m@Whh_o{9KXuWlnx#|OgWQ<UbCUEaE0
zGdR(oa0JXZbF0=cvB3CfMqD9#L}90PLe3DjE6Zn&T=eIhnsOlzHuK*Zd@ze;V8oY;
zM5i*r-=|lY>ci?6HbM-<f7{6?N_{eh81-Db_aF~R%6M&ix+uJ-%e~nC0%#Oi)08^B
zmlrJyOtzMKJIXaxl79xcd1rt(%3@%QRLL`THW|=^!t|321?tGMjFogF23)42hxwvx
zGNzURFY1D09v4`F8C#rOd~HzX_KYPqGjL?ZuppECqTRP=S>Kk==SOlkIWPu?QuBIi
zOR?qlYi5Idx&=N11mbgB?!LT`_v<jJkJJy?Y$l%WHbm{2GX)QC)|tNoTD>esEsM=x
z+3lU0#4bLv%A6S!3|#T?Vcuh;FyJ2Rd1w(gW}IM=?fYV34{5-Wym1<Ev&-oco-zOY
zS@7L`6z|%po5G3ndb3t>_Dus`OoZkB)B(5l8mxA8ydAH+DLg`0O_OowjxyDc%HKSR
zY|2Y&RA{SpSsHeJn*3^qAD6OQKqfLt4|o?)hSml1r&Rn=*m#fEHu8szcWL#7RJ9-Z
zkvy576I*}HFy}qBNdV~|iKx6Ly844fzZ}WI{0{;W-aXO3)S;}jPw%<)^wP>4;&KE_
zBUfJ1A>uM?KPuAs6~^lxXST@8`m9=W<|VBzGwAu8XpgDFdcaqor^-I7Abfk0he!6n
z@zs0+^D9z{V%#Zoy09C5HXUT(rF=f@ltWexuE+wYGqQqc2%(k?zfSw{-aj<~ScU43
zV9S%h1}?{B+UTlzYYQ+?A;C_s-reoP@V@Cw6Wuf_1J@Ztpo#kIvtc?G{HjQT;D0Jd
z<ssgof{%*o?^-*NQlI&Ec#r+z(^Y^pYiz%xu`Ebei8o2-C!m_p%#td2;9>H{&ZMkV
z`Y4_)w~8`VOzIN=H1$N|Q+qT7q0VVgU#+*pk9;qAjUk3vdcyScwS^h*e1k1xt3_R`
z?uR)py5=3B>Bny^SD$_1y&LY>IKO>JQGidPYF{`okCxb4c;5BH(^US38|xFX%6q{A
ziB}0vaPYSLpn3l|8nxhK(yH-5TK=Px#3B2MeqBzHmcL<a-R;+IBab(J{nF!nQkL)T
z%S5$*C#t`6&%S()9p)|Y4f`~?J^q11U$$4pQvA;uEqutUPiqxZddqU{W$tf1#fH3%
zFRw(u%-bD@#%=w$c_6#q-5GYD8ZNV)OFd;oAn1wZAr#yV71-$7PkwbrB|+l$O*f4n
zD85C{g@vMSZ1Opg>==&;RXNQoM2JLHzBq9o-78BvF~j^vSA2&yx*Hml(2whwk<ewv
zq9l+LlPo!*a=&K$o%fwwUSU#y<f2{n0or3gNuCbpe7SvW3T02%KrDMY4B@jtTK(Ar
zhgxnKw^6%`_j~_Zr=9}#t5w@yd=syKygK?@d0+Q|qJA=1T6vhpl(?a9QumuA*Tl~E
zIfH0+{Yv8j`%8KqG>51fZ)#vj<^%(SDa-iS^*Tp>?ISYn`u$#c_C_CDJxc@>q-Z*d
zOv(R_FAM1ZG?Y*kE<+0ie2UZGvN)hb++v7P{VXo9_?U+yLEF(-W>rdw5AUrC(Dn-<
zV~&9H8EuBeyE0}B|0Qy3nkxRCPh>OKq7?HqpB$I?1!U;GoqTFaimOZ@FHzr^e&0G&
zLKr+<c4XD|ZbbC0+0K#nQaqDK>U1gWZ8#>urs>NbZadZLT<yJuQ+kXYV^T!e18=db
z>mhr1?6R;i*v&v#JQ)GKh*~ODOYDCl)&yZp*Z7n;Ec{V>)MM75_c6Fx4|&uSGG*#F
zKh!(rt2#aZXd#-Nf(f<cx!oj76`#>l<!1kCNJ+jxp>VDFBECq3w|L+1bnd=+`Uqgd
z#XV>-eqFTs)UIB<!+?7tE9ptAWN<CI@3@$PEu=@(b1UjrLFFR#c#&smz<IluarAd(
z+Op1Xog>$DWKGq{H#9PvtndC{iu1zhMGl3jKXqYC%lF!nd>gMDDjo08ijC#=a+Dt|
zx<qP<BUMK=Ny9|x2lO+72xK~MUEaNql?eRQEfVwNr*4U(HF>8|zS~)IO^?hZ_GwD(
zr<NL!;N8+AiV1eO7@|WMl4ycjfKL(zqefxi<f9Jwu&=x-j8a}o-}g#T#t@b4_-mk6
z&RIhd=O#(hS{<q<sUtyomaToLC&<=IIaDOB5`X;wgmhk5mNlcwOy@<GgKso2sLYci
zq(AOr4uAiG{;x(+jQFb*Q6~^*#wTbqz3s|PD(<{>MG-&XP~!{)$TFu4+5<iy>ut&2
zso~v0GVi+r@oT<=&DcE+=^VpH#!W=T{|Mxc7gTK_hSIq5rnZcl-7sb9nTAhl0v3-l
zK*8qg$4^_6^GvF&-0={@I!IV(=E5-4`f;13aQ4Zw7f!Xe0^1+t9+xa_RfP1I6g0B-
zB+$DxE_)oaOL6~`-ant;WNqhlD`|M@m=!|F&r(pgEEMo+FPE+yZGAe=Ua-cL)b^yB
zO^9POD0^h-q@^Xb)E9%b^gr5lY$tkFrzOF9*WEP<IqM%l_mxmdvGmJ$<NF<^=Q_)g
zem@$Qqyu_8wtvZ+cW5Y))%{xDXDNNx{o>1g7y5&&RvK*bX%{uLZ%4@)NsOOlC`R9Q
zia~A06voj3Z`%xU)cp+*^wva;_6~W08cnPs118~D;AMBY?mv{hiPlXFkc1O@dQ43z
z7fIXrqo+T)#@Hh+_$TB~>IWv!3!K_XZh++e;?3xP>Z%-$h46<lS#~v}{7~W{fI|L}
za%^U20X*JnCYet><Le-z>=zxcZ1Vi3sE+5b>Tgxwo+4?)A@>4anN)^Wz3&W?yPTj0
zo$II>y93h=wcUB{=+$zFl9l=m2Zs+MfPOd)iEBN{p_Jhm_Wr3fWEc(!A~G|bQjq4K
zadwx6K#XE-<z?!AS|#F8!{HVdD1WyBb4GVXTMK_yEBr(vuKd4oMT+$%ygOH!fKs20
zA)+gHq7mt%$eD<5Af&i>4&EqT9?y-~kG3MQT7}R{P`xwPhmm#I3-vxYI9r9^YY-V6
zLz$zO>nBI)@j>r{=Ef1L{85@=l8%dy%M!Uib<u1&GP)-_O{;RCn9<rXIMP1aq?`e(
zXg8wSq5_c`C#v-JX}1#-m)BS)dGl$JOPo?sjUn#i*Xl16S<Rn=!>l}C>IB`5%=m~0
ze`}Fw{`aFUzOr&R(kf9s>J^90^)ydHFs?i@?t%QC@W}_~fZf19{-WOw@~zwr_#L`R
zyqCeuch^n5u^wv~i)*l|$UVoFU-R)Mu8Hf`vLCRK5%QgfFQ3XKq)Y8*h0iowj?J`C
zp4L9)+4v@rcspc7$&;<}r2K6y^FB|4AMGyBn9svR5n5;s1>e2`@JwxAT{T{n%DRED
zg<9>?AMWv0&5O>&T;5x)aVgnDYDR$;orU6H19ow`;eQA)6{|*MWun(+7b?bT{lq+p
z5@B^3w8LV>5RrUqpiJ`0J}Gtr*K-Z>^b4YshdSDvw@-3k3g(OFkw|vCAV2fglwV`c
zJ4%3Ka&jW9dV)Ic*Wa7r9?i4L65_2UY$}sI2|Sue%JHeuDR#uPQR?(P)bNS?G;MXw
zRXS@?nF}2DSz=d_W(XI{SZo@{YinMW*5<^A)IN2U=!5Z8FpVDjcJT8Zr`_!rr#$^&
zH0u)wios)z=>K3%*#BtI2no7>7&c1$9bW&wb*6sNut)TPXd7eiYp|yDIYDI!-$6to
zum#>tna)?7ao9qD>|#ByWHMa9NT-)Njw!c?D`n0v0EWmLa29{sdz-;5Y(`+ZRD=36
zpwPk|xEbiuVH;_FuTCzO+sUo|i~X5!R>+EUiVcK8c%*OT3Bg0rDfkREI=uDafrV>Y
zWm|0Ck0Yi=^~yn)j)yQ~S=%!4lyOIJ3R`_ddSSMcS96@BFMUP4trQTFimJ#Nht~!K
z1pAr`d|O6YbBM?>bVnWW)G1buGTk2Xt!4?PVasxF*p{Z<A@OXRUfOe8B<HtVTJQhm
zu4so8?cPp&cJig^+_fxn<67wbgV$K(MN_<x&sUiPAmveqcE<JB4gokmgnMsgyss)@
z{)+H(qBY+yF1<iL46i|pHI=L=EQU&q_9%_Gdl#NLT3rLOB@?M8q$3jYdc~Yp5{+J<
z!ia?-3VRR|PF#bOlw_c;NpR|?-lWzK0-A|=1H}V-JWKo%agMKV^;Lrl?e6(x>Th91
zVW{F>6A4jvy(vmK?U0_sH);fZG`(oY#_D9pfPmvZ#|U|*z4UlATZa%Fh3nP&T!S<F
zEOoSc(rXCKi`W}42F+iq0o!0gJ+!qeRK~`R7l*pgS9p>7DLDfY4YgX)9Wz>mVy68t
zWXRcy@KE^J5y#V+Dd!uAPC;CXqEj6`rk`Mf!aYV??u?z~p1xUDgP{OkZzlSG7jOTT
z4(JZi>yG7W^Hp7brdTzu!}r6QviL6JkhIa7tU({#`)Q&{0cAjzqRwl4(TWe74AbXB
z>#XYg3&bECfPy>G{z{RW&VXMap(uk77{OXY!lWCcP~3+9TT>AxyR?x_!v#XAI@3y1
zh>=_?V9Y7jXy;vXS|Q65^pZ$Aslt|tb<0wfg#sVoGCXtm)4MMo+|!2rHqO%UK5~uM
z18Wc8zshu#R2a1tPZl11tyk1;UfZ-89N&w;I(IVc>SZ|Gn_ls1-gU}7ml)qU^1hr|
z<7@ioPzt%j#75T@b)3)gc3sW=P5JJ*ZEKK2!Q$*2>1@g|Yw!8c=LgTh>Gd?Rr;qa6
z_B^MSHct>r(hu4a7Uvr(b5$>z+(QG#Wn8vt_wy40i#MHR6u)v&)v@41Rq9fCN_4gG
z@D!A6yPpc?iW5rcOl*EdgVFs(PuHt=1q1d$fHLV-NqLwYwF;0&w-$<=jD){tik`L6
z2m0$fNc52svzS14VxmTLzJ9nag$=<sc`=DML-IxxGZ9z_vYWGJ(kJ$dZY73+TZa}I
zpVhga8YHbhHgzcnFOjPt)quH%%ho5_Ovnrg*q{MA-Vi!*1EhL5lgQihHXVa_8(+0i
zH-9D_^hp@^J41tBK+@D(oA0HDhBvkd(^w5NOmP~-p6s&^vO$)BA&c!k<^)`#fJsAG
zBTRNRRCH#>1H|x91bOrS4wQJ#7d;h$(zip>R8{<TR%7|_?V=gY-B?UMfVt%tKPXiV
z<gKGnXu_6^;PPp7o3xdVS6wqAllb8+pJV7`JkP)gQ~snTC}u@CPkxiD>ww#2(Bv`8
z({xFWUUWfg7iyIFd<@sEu`EFBoJC89kU7-r@TE27pRGKv{dzBLuI}hmqftj>_8PQJ
z!PU7e@~mZsy4$nm$F>Fboe521{kXR{IkCKG&bVFShc?P|NP5Ml@RwMkGWg3#cwXc1
z$*h6EwClME8zYuVGuZw6xX^vTvfnbi9G$t^lJq!zZ>ve6eckfe`U%?lH%_?Xa|eNJ
z0QF5vOMTn!k#z?ptY$Fh<*N)TlKPIyrQPRQ`$|QHztMYFLa|MrXx&R?T0+849f()d
zJ1xbFjPH~-S}9Kis$GBd$h%(i6Sf{q-oKEnxii<NwxQ1cS>1pq@Hb}HMvquMrhE9N
zh%TDk@{@Z^E>37-3cA)z+e5s_JK;jXC3FbW{(?!$i2lt#i7dFMd>B9jEE8pam(mJ-
zG$F1;38sTIl()7CmnD^(Lo|%SLZx}2wR7bdy3A3`=v4C;e)KW6fb1T6r-M9S0(gyQ
z!L&AU^8h88=&=%6ygY`7N5PjPCz&fCeQjP*nu(2`f!}=xKZ5x1zykhviT2;&@P!S2
zWDUb_TZ{kx_;96MuMPa+kUF}@!P}Ygksg0Y|7x<VIsguNugPmfdfiUOnhxhqDTzIU
zz!G$d>9=?p<7hL~a?pcu)UPQ%PZb}Ltwy9!mEl%TKb(5qk|8kvTqi_1^w}zPQt(}j
zV`Y$Iq=n$w)1YXIXIT?_j0-<m+w2Oymin{pF;PXv8-8jKcK<#5HmraDS=n$NM`G>@
z<m{z~0ay0$Q}<-{X*QKyoP|r{0dbCO((Ll4bX>%96N?$0(-|AhDOgkeqV)4!M{-WJ
z&Rsw24ck|d4s*x;-tmu_R0c!+9WB61oT3de2Q|iD`$LW9a}N$OMyN-n^UqIq>FtQs
zI+_nJ0u+bx9dsAkC|b5=I)pP$nmavkx6im~u7-DW53G^fVxEh4!xhT%MW&QXGxkYP
zI$_Y?7||4`TOHKfX~1p|)-EzR+C}a0ro-fbU=3<Y@M*hSuHmWXcq;E*VQ;K3cQ-=5
z=eRCBnuJ;>dfSq<E2@=Vc;N81J(u<{0-4)#;OF*Hb&7rD79@$?K~ygh>Oj++Bq5a9
zXA(j$u5}E44r>Lhra+Y@w=U%cYY8vGU4Y~ux>|tEJg$#^ntG+Xc(_GXY%(uOW`bgu
zN2pSaT}^b-?FLR(JcVH~!TP3*a6=^TOmW_uHhq*mWvu?$#@Hy!>Ro+&YhkXrAnYl=
zCxDgz+rR(KZZ+TjO-~bo`9b^qT8^J7vKMbdpXwpb5sQ~<2}mR>@~3L?is?uI*o5CB
zlmVozlXQ@``h58JEw?)?yZ76K3l*c2OqS8-;|obB4}XzX33LdQ?JZiM>msgOURg*`
zHCc4p?8Y}}nKix(u-f&Rl|)-_OkYu{E(_Ob@1ocikCK9FEq@xxeI@_tpJ4Z-(KYR>
zQ$+Ja5euwM3<FumJWPUJ%7jEYYrUq{rRTL)(bPsA)3ygUO~L=E>MX#jXxfLra6qNI
z5tZ&ZG#vWSaA;9VrKH;g)Q6JpkWiG8u0uB*x;q3!0g-MHDZk-)eXsxfo$LBtGrK!`
zc6Vm(+1Z(U?z?8hs>bA12@PUGOZ1DwQMG4d>T)K6$KsDd?i9~Qx1!3#?DlUeCv&TT
zR_677$C9;|Q?7XpX@|T_oMIMvUw7uq8s$`6Q&(0VuSoH4q5W5nEIz-W7iiNhYZlPl
zooJ4ngA%IndDa)pdJXDvGh97Y(wm4fZ_Fx^(EDDKcZqY0Yvu9O9n9GE?%u2{lm2jq
zj>a!s5bK?-OUDS{NHesT8M^IugoHL73-6+%Zk7!l($A~votqP2+*tKjYjc5nSG8cx
z1oldD5vrS_aOB<i&eAm92F>V2v|v|>lJae4{JESjUEzq{i7L1APXb5{!-^u&*t56H
z^1Fv++?k%UTN1;ern8a-aPs(9>LL*o3bMjZk)0SWjIxrp0lHd5KL#zN&c<I!Cx_9!
z$=IcxKsg$vk^sEY!3krfwN>FZO6s}$zTW1Dvhj_FD5@WP_jJ&>&TP|%MSd3zsK(4k
zpt)56)rIGO`WgSOYY$ccGkxKA`AFX?FY2k89!rvjbY_83vP965sMwIcAS@>0^Owuq
z{T&c#grC}%A)HLPe_!~$f^7wzSH6u*h9aluskRu2Y}n{`BW^0}_t!H!KTq7K^e`|^
zGGcj(PxMlgDdt{Qm7S;yg+sJau~=$Cte#82UFXu@M{RtsvS#@wW@Pc>>{>f^$9v2L
zu;>$))+U|I@{NMiEIsensPIp4XV0Kd)J4}*W2@&a>%up`5X1*89wc?YN@*>8zi}9U
zAzMJ_8N4lKs>W{hm;kHwp_%Wvv2*l-CmTaVm!{~@#?vR_>SCD3n903o^A_V9twiM}
z4T;2Ew{DJX^`8F<dqB~L-~I&Qy(9a0`{<KXf#pe7_=}Sh>BzX1l@@{W2RIUfe!X!u
z{RB@3DoEqX4_ZiH#xKYe(9;kXMr8<Hm)+B#Bkj;$s;6$(<rO9A*fRZ1c_)Ir@Uo$Q
zTy3}UMxj<rfq>y17O9b?ZFqX_9sPf*1sWvjI!T%N@axD6z4@g2eM;*U+{!WBnh_p!
z0<EEG-OU3Iy!pRcdFuG6<u#Cjx44H!X-5VID@cTtM?*Ebdhl6aoequ?(l_+RBYN8h
z3zy-e0~J12JSdTN(>q;4Em*wj^0>h0n@FXS{wE>p!-5tn#C6o*!Q3;V-owIH<B=!5
z85f-s@v-H~q4?Nuxc}3H0L$v%|23U~AAm_8I$^owA^~bCdNtN2qD+x$*5p7TSerDW
zeR;pux=Ix{sn0rwj&RflH&ydi*^7o_U%7at9O2WO*f%9ZbU4TF2XY6ZG8MSUQqtfB
zBn86`f-i5v@uWXbwos)~$~MCDX6}&pA?Syk{ia#OUcPV3W-B*wr0xw&ynrX%-7!9A
zr_OY3>Rf%sCHjTIp>gELJ4sn-BFB#pJ$4%c9mT2IryXB7t)OVKlD~V_p0n#0#l+aj
zlM_yeQRPmW2(Q~QZOGJq(<z$`wiVVX|JJ&L_+b6&Q+v(6NO{u(@uI1G#gkEu9Zl1*
zxkbtO-|SPbf2Qm{_vR09G!E)|k#)Mk>pzR$QF2Lp{bcE3ztGy)eq@W??AJ}X5+rFt
zy(7c;dDDhxI(F4=X8af&mD%F2P|8Z3CO>@khqML{N9we#yPX6ViH~#lcim7N;i(c=
zoZEK0`uPKjOzlNiddZmB6X2qSXNs|f=w*AY(1=e01M^;k0a5cvVYGj@K5Nc+Z5bx{
zHd}e$9)5jvK=pNv(Lxvz5-qGO#LdGRy3&ttmMPq{xBP@$ny0~2vTeVPN&@MA>MgI%
zdmhE96+CcbSa(c|f|kuoF?^z@+n4FEFW!nrIC}yo57o2UA=WHaLGCNn5yxi+>8LHa
zYfY%ZvEOk1+r=Qda;qlz&Kgcb6o>?xmS7boa3`2C5<LEtfiYChN@xbh;TBVf`nS>C
zxkwJS_})8XoLYX5s8YLahLf$Cy(!a&R^p}DsPQB-b-09PsF<E<=4x41a3NP>$P3zd
zMDXKBUtNA~U60vdjKv6`2b@J|ofE5_vZk3S>Df%Cu2*+#|H3~s>ErQR*oiiAPU%6m
z8Wg=tb;i)yb+ozFNOf#)Bo^5U(B-~Qy7f=H9>1REpvv3y-iLyR?|ENK;*DTLD+z5g
zI`Y04KcnFNuzsO2v9kK4@q@8{{j$<S6JH`w9MgNHvrYWmnG?ZlReM`bKQq<89Dbai
zC9!-wxU?#HJa}1uKV{aT&YSObajijiw>Ua{D%B^Q%jppNyy+vXJ4&XceEQGrC_?Ca
zL&+xVvI0ML1eU^%Y3Zh{fPp$TIyavCusp&+heAQi|2k1i$?SDPmNI|$YXZeqTlAb|
z2>CsJvNpZzD)}NRXB}%!2G_cF%e5*MZ}^+i0RQLuKi-i0zb5R08)m}f%MMPqRqX=I
zi8XaF3&1iC;rknX+m!rmvJ^ZFoEWr27^TWk+B^SlcZ0$C!P4_qX?a)Vf|3pz`AbgE
z(8>O`o(RKSie_&F_HUg-OdLl){{RCQf^u$gLtAZ&Ia9xcu^z9{J-$S;E`+g2w|`uQ
z4_8&!T$;*ot6!aYG7%S-rQ0$4RkCtCo1Cs7SU<`6-&e<#mv}Swo4_Q>BfT3y+xz;f
z=Ea?paSIB@DAlEM+Co3|YjJ9py7p!-O*!J#R>)}%dj$lN-vC3k$ZbzKzd;Vt^_$p6
zDxoTt6zj_LVH`OI4-@E#G<bBfjBdx7q<!H7Q)O%kb~cGUkNu|E#e%gxW>_mtc#TY)
z{gyOKOqZsfSK^!NH>tadpG{JqdAEMgr+w!;=xx!3a+3%*bEPA$HszsXVB&s~lxOW3
zqs{o0D|yIFC|O1urU6a$ADu>sRXejbs~-yw+%<V!G5g&^kh77Sqq4JKT%ySK)$4)E
z87b;_JJ?aA^YdP042dvfi?x|WbH~ru$WnJK5c5&maS^DaUqa`XbGI-(%eIBXui3wz
zoHg-}xs0$3ZKR#a7CS-vRob4%1&#T_sW9PEg7!AWVI{A3T;X9~$6H<Xn@*c`ti^@|
zqO%we>dO$qc+z`5hUqOa-sRKEyS?r(m9>;)(k`KLgwAa@>0(U#Ks!@gZRt6(Rha}w
z?Yl@c{%?j(_x2B=tV8EH=NQsL-a~n{m&v@PBMnpng@X8QXC1;;rc6EV)(-^K1kwg*
zhqzhuPj&SDUuLOnhQE-)@SxYH2IDpz-u4V4B8XAC*0st*6)8gqE@E5;bprv@!~(rA
z<cNl*^AVo7mZ%LDvMl~QN=|MxLD5fNK-Bumi8{xd|IX7s0+VdK(4{{nLs!k|-)R*V
zGO+xJ<64*+%Lo_cQ%#n5#pPXfvmA50V9llIwX`O6vQ*>>`dx#qsdyraW1MT5``OL+
z{dVIf*8EYN;X2<ZxSj8ME9a}O#`PMBrQa=8aD2+ZYTJxJ&ny~!v9ZOtpzraB6Y#E#
zX}!9){-i#8QuSE@YRh<?G3W@DYATo*R!sZ{C<zM1N^eWaXml?w%_QGtl1YqT6*k^9
zT>tcN#8Tvm;|Fgx3$Tdf<$IAnV}ZggF)};MFLSXO=T!us@`O07+*y$BWYW=8dExQX
z{rRM8OI2T;o?V0%Cwemf+~eNgqzOYS=N@nBUmlD9vmS6%bygx?+_QChbX@r$hau;L
zJuH^FUmhQWxXg1C=4_HLp-9ZB7RW#Sjfm<)mABroBwcM9u8_g`q?;LnuNa1qB$AHm
z<6bP=#7`#78Ik__M`_EDhrV-Q&Lv5XaE`uNg+!`=OmgH@kEu{lDT+{S*Fl`1*<Q0P
zPDY=VdW~(gLcX$R&=Dxl4j;4(X=>v;?SFEQ<vi^cI&|mDCN69#)-1OQD%V2$WJ3L1
zUgLW@yyn<NYI<W$M>_lQKgIIMe16Mopws%p)JZSs1UTXuk8U=Jgf~s<YXx){)fyn=
zsD>&Erx+W@D1UMd8FBN;jqWJlV(muzI!fGia~9Pe2(`q1!~Wj}Lh<1nT0?3&qF|uv
zL7OIai_*Utj8lw1!~)7=w})2e#FUw$dThv&4U5fhg!O%Qzi;-zMmz;|hg@yBicBoN
zZ<T^Bf&8(Qzg`~Mx)DE>eD)CY_}bRpJmXLUd$N~-%=LzOcX<u(4@3^Ir{7<j*u@Rf
zbIP79GGw4?WAP}n;1PURe>~<HWuE=35H}sqzvRKUs#SJ;^S5c%f~8Jg20Ua{x|?wg
zhHV5XEk8Zn3OW;izv;POHnm=5s_c$-usfA*`p{J#?kJYpa2zGj@Xhk*<OcPEqd+Gl
zG95}oT+|$WkR4YXgvlHI)+tWeF!1@)$L_7X@uG>H<e4Nd>8BSo#{NAAFWE1566yOK
zP9Dl8S+w^ZpH#b^;_uh&<lp|$($kLfwcHih+?R27oL=I03+=#`EIK%SFOI)B+O6Q&
zkN+97lfOq=IUH|9rWpQ`g%H7Q)t=fmf}|i8Bnxn<EgLRR)V%GyOa`ggE4f`{k)i2h
zTl<uhI6C2>aW^|7@!#Zek|V`_sEsh_4)n%)nGN!K^j8b^jUm-WdI~d@Z)uiVkFR#_
z>tfv$iwj8H$d4PHxKlXhK*5LDVqGa36j+A)59|68=cC3<IavC4-*X7c3Lh3w6>hJS
zA!H44Z*g@RBnbY|N3*FKO+;yCb?fDMV83B|^Y>E}jA6bZ<i{g=tB?U*Ke)U}5!?y-
zkxG}mP?YK&I31;fI$d154O|z;@;+Zc-=~rLK9kNQsck<zbI<9YF{iiN>eex8crvHx
zvBaAosQr0!46M<0z8XcMS)C3-`SW+FEDCx*qZtS-dTg@pZBg@>k+%|8&pLV$7@;=o
zgG`+&>))>{Jc~55n{+%>K?kCz-R^}Z{Gz+pqRB5&KTmK=wtL!r*k=y)_HEz5eyr1A
z6oJoh=OI<UwnLKSn8Q<33HD;g)cj44D#I*dlDc;mw!i0HM!(<s=60*v;+mzn`{E<v
zGzS-#wV@T&<MGYG`e%iWUd|2E0w$8Pk!#+KTFt3}h@O{wX^q+>^JB;NebRhsx}j6n
zl6w3LFtOH<Eh>mZ_^Z{K5T{CnTjSaF&!mpwJ<WZlx`{GG=kR8P79$w}dnrrW35%}&
z1AL^Llj45m>fC(_W+6khW9482w<GzX2C1)&{0yPFDgtdJ#6`~Zc}!fTtU`2ZW#d5+
z>ZE2*PrCV719n<=M+q1txXRzstA&kul5}nbjEG=Hfp!LmQ2%(fv|z(}yNGApk!aPb
zR(_{)-1w{*(vZbbR>Ps8yIvv2oUpRZz??cpSErFfRfE!5BvogB)R|3>xV>sXWqnRs
zLduYPgk$8$>9P7<N|tT#b;&CKe>Fg!ke`g`tzri0^E4!wY*%?S^y}P#h%rFzjtA#q
zq%^^6_3!%Xywvyk!n!hnRinX2;%N#wqqT-FEui$aY(Wje6*}?vBN_2|;B}y!qmm~|
z3On{TEWGPzRJW}1i9J(ymFoEbibf1=z&n}zB(~W+YPK+8)`z--yhf!MO<=R)?u(uh
zcu3HA%J(Mc;SQw}a)HXPTswbw(`ock6h;qCRaksofTL~2{#iFaOxmk<s8XoWT=n{=
zk)J$tjUtm2j<XA;FKYSQ&<%5w2Q`EXFy;6ExUU#S5+zbJP1t6gHGQa7Ms66_OQIfl
zHcIvkdYu)y8U*$?oa|4d79I?Z{_=Tc?CLCD_Ts=y>_bGvGq*<GD1uj~^;&1kN20^b
zS<O-;-aoJBM=NS9v2%Li%yLI(-62G1c~KEHMZ+`kkc7fPqxA&=0%Y<IbE3pNLM+>f
zRmG=ExShk?o;2-cDdWY+RgVrP)%-_H|8$oH=rQJQvEC;EUKWCLlpV3*7mZG4Jgj|i
zji&RAVJExKQak50GqQM29pv3Haql@hhMaN&Dz}{aF(t#um1xY`@`oR{<4n6dwRR)$
zi!{)lTQ`T$Jdag|_f2QiMq?XqUykMu;3{gC+X%~|MDf3+8hD*oBh=B}GDaE0S;x7(
zWxU&k67h{%QDD51rRD$XL5(#lj5rAMos827MSt<S!%F`8xveBqpz5Po&bdVGYq82U
zf-`0=T-TD?Z^zLgCdn_OwR0%)c&lh+<57>}da^!*DFx|$tH`xpjM2$8bTE>bz;@Qk
zH^@)0oA)5G$k+PxBxF9Q4^gVTYDAz#K5LsY!#jCm)cak#a(sL0#l*{B?9FX1Nj(=U
zF|5rB&L<3!6gQ)??v{EXY?b&P_ai-yINMn`Qitu_l4^2d?+1;TR??cIT*?wczy6#k
za`D3@92^&;tmRMP<L_`-o{lj0mUFA#e)<t#Ks?E~&GclBSI*#0{kNwtF6yFqUWhKO
zp2ad%l^6V|QI;N)rui(YG<0J9>Oo>1L&e_Z%RePD>@NHDB*NWIl6_0Te?(DZb1BWF
z!o#a@k^<<G+s!fg2Im052A*Pg1s`#t_CE1w`;D{K>xF{lKD{lx{iJc27;X)fkM8gt
zs9ihk%dgIE1J=bNmslCvcKeMrN}*le&<myQ<=~F?GN%_e*-Ol%GP<MFP!}3%bA_Kj
zkNQN{<&hgwm(`Vj5v9^{mNw<@)ZHwSK#?-K5^}qw;oZ!1KWAKM8X8B-@^AR0$_yNM
z4R?-&&xutIgb}XiX;4Ja5VWE$1mlKxDI66F3e4{1slwrzYJ=;c*l$?=J5zo&hh=(7
zLKuvchiK!uxu}Aa7u|!A4w8ia$SlD;MoU$;s4kX~`^eB+T!C^n2RAnFit+>wvd00N
ziN5^lZ8l|e<CZrSKIqwtgxZL`X9yf#+ru00{cK#QXs@f0aP6)6`)M#4VaOb%Rj$Pc
z$9r#^>6~+~Xw3Beho8?1?-jY9&7J;GXliqr3RZsgowod;ORScR)Z5UUW%dLownfp`
zy`yl$Gq!a)|E7lm5?@wg?cbHH*wTlw8yJPeZ{AMj{*bX()n&$FLcsslH(<_m`$df<
z_x1eoovnKf%HG;6Q;)pH`O|E7f=Z8Gm^-J<#u#rtsJGx+px{{vd$uTZOQ2wW=j5dw
z*^sqIN9(e)aQtS{V)Uo#sDtrCSwWue4CbQ6N*K%LA#EH#3FdIRDEM>;OWL6U55INo
zjDIh0S3l{RaM5V#p^^(Z3!zZ3lLA@G?gsCUl0fVy9PXWhKQx`1NG|#UAu9}V&EPpQ
ztvM#k`F0z>cNd=2z9X6d;hd$vTpE4)np%;>Vdb=UZL@7fDSZ6pukJ0x3%33p%#LW~
zzP`6etzF!iXkUo+cdY({%$jfN!{e`oJ2WzO2!_0P<8F)d)9(mX<eq!=w>dZ+2*{7{
zM&}F-ZhGGtj${3-n}LUzY?`1rr_tA}v@HH=B#ai;oM^$ue#88?6L_T${)UhlXxBh@
z%4oBgI7q`<GllLgGgC1IwzKR$+YFNC@az0Rs?I2DN}1HV6q{R-V<R4fV&8kgaH>uG
zZZM+)C~)eCWNL6xnwXWphNtU081}xu8={&&X+Orr`?SO)bj*qx-%!7U|8p15VG5B-
zj}#hH3@7l>*tlN#<tL4F1^N851{1rfo{n{$pyUUFao*;Cu*8BFSMI&Jrr%@P1D1T6
zJs}Xb3I0(|m3HyfKcmP&=nyflVV~b|mqS{{EM1y;a<KBARMs=0%K2%rqkGh-vFza8
z%~`JEQ;OP2b75l(6=To%Z&TIMH;Z%%W)e=M%X0aic8%|y`WnbRNcyzZJuGwX`Wd78
zv3eh@j;WqH`6|R~wGt_(xn8~tk1XguY|xC;nrrCiy^fphe5QogyvoX3T~Er0e=dl(
zEO?@TG82FbbkCCUIJKp#-edSvzE0KcF-%{yjvy_ITJ|cS8g{aSZA`b`mn@~*P6Wne
zs>EJtE1f$VUKN;Pt<b$JH%A%9Ubu)hS64K$L_mGBp*vIA03lMUspDif-r^QMAzGlv
zn{r%f;BZT%my9s@aB$SHmA|@_wM0fMj`9hqW|-oJq^wBz%Fp(Q!ekbm;6|9LPDKjS
z&hUQL1LaR+i$y|>hWPeGBT)eahzat!K3c=b3B(`$U_b0PcdojFRKQ$#8)y!!#VCPk
z-aEUinWt=c!OFmdYQKXFj(^*-&~L_9+8ps3kMt#Z^UVn&+ic2zi*(wHruH!JucfW{
z%0+cqj^ndY+lyuDTOWp;AIm5u41SC6*@7qNrH<tv=2xuLbLZaOl*gZlOtSTx+Mo`e
zBgY`}P=$AMk4C>cHJJP$nT@cRxS9XpQKE_H<$+|Ip7VisWf;ZdgonIO%w+s=ow|Ow
zSA_#FY^hCJEXfzF2I3zY)eovS1DQk%>eo4?X&uO~bG6y;(AC+lbIA;DvY*r~To<rj
z`d%^HWGfwox2W{oluP9-Z)(HKbKaquL7BPcmk{H{>sX!@%t_h%S34O8W9hb+lsd-D
zMh9sx_ZMD2_1-;`c>_zp5_DC%T>n@+wm&FWiqClZd01-n3)@gU!*xZcLe{LNQvI7m
z7?M%;AMMdTiu<43_H787xR*?CD3eKjnUmN;|0ZwerilH+&wqg5pgI`;j8VkB!G<}n
z%*u#OZMXL|m3pzG4J{|x;U{J;{xk*7p1%HENMkLlYO6m)q_aDF_~0LQj;)On;oPBf
z`Qi34#hjjjV5eq*%rSo|L`9L7bq^<LMNM6%0pXcEU!3P_f}wH5VUR>LqwOSu^a%^E
zcG*iE!7;CB2z32!|No^8<X%Buh&Eo4gECy@;cm^GzBFB^^7o-t<QuG8aSCkY$vB~=
z)QMWQ@U{4SP0_$++k=qh<43YU&(c7mKlZK2r*<1)6|yPr+WStfijzQkCBs5J9#Q;x
z%NXLkDW|(q=0WujN~T*V3JfRv^m5F2^zEr~{;{p?<{s<4Se$(wyx?;0hV$fo?=~fY
zVa-Y=_ptUhXEMS7_M(I|E30XQ&12EH5=(UAtSyJAPmukQP!W?#2X>_6lj(bduLy$E
zt@(ak7-`y&^lLvWEvk^yjGb9undB#7KTw~u{XHjpv!FnIrR_x{gR%IXsr2WPqeC>e
zx=i<;H=IdqKX@&3I3siEJJ*NK2={q@3vm>MbCe&s$h{y>oCU3MZ&19&l5<Ig&awsc
zYu+mA^wO@j6|;2R5E)vhzEB_$A>L<(on9wvBGzB6Z`>&;Oj8IpH2Wh~dN3KI6au?L
z3$z^3jJc%n4kmf~c(+SQN1Pa@Hutdw!V-P6WPA-bgnUqUD5E5&dXY61xn!(a`I(0P
zrZ-PBu~v<r3h($o+`ZAPuJpN{^v`$lgw052hV(sXq_rsK>`4Y=G~bJ7#4(=9=k;un
z9-zUR=v)6^PfqOSHK+~v2u7fYFJHIa+IyEpElwHLWvv@r5vhGWx-}dA!a9+GKT(N!
zBqSL7k!WP^@>pIwm>USuy*;Lm^iW0wYp&|eG#saDMd^JO<($w@B7QZIzpJ6FSpA^%
zp8e5Fwx<~N*!x@5xxOj4tYT(A8T7K{f5)#McTOaT7d!KQtF@tPI$%unFznIeDS}Jb
zO`O!p$Im4%xm`oM#ut-AkNo=LGX^}9@vn((rF8u`C@m;VS9KcG6+Ps8HAoRLxnfg#
zkC7aga&G8zsxxb<WA(7vZ}Ec1<W_gSZ9X51e6nOyw`OPWLq}<f{%2nA_w!d3eNQG<
zp1&{}ViagzKelTkz5F@Yyn1q4Y25TwbMr`My*@HprqFXeh;Nvn;<HpJ0sB6WlP1*N
zQgY7wRzU|QeqCrgoV&lgf0O*URhJ=<q*zGNo3J$6@iR@^PCwysGajST4Hbb!e3e_d
zx6i*~$n?((Bu9knmhXh0liiWmm_<_~hT>CFdWn6KDZBLIN*G)1wBiFhOL(X!`Xa}v
zxU-`UtpxkgrWyr8#f;TrgW+Doel2SiKW`6~P$s7Lmh(yqWXIKqRu5HZJ-*&g<kj6j
z+-v5F)YNOEK~$_aX#5@)@^T6qqcr3bQ+mQ%*FRwBQ4GZS)|EK_HDvtkko#XFF?tfW
zuW{jqDJ}^;4`63w3hri!m1MO9hRoEKNoNqT;(U<iq9<2?hP~)JjmKo_<J&a6>ADZc
zUi3=^;(Qoh-NR?CN~O;n=F+Xxk}XinG%64dZRBs{FPVOPR8RFX-6nE|_n1JribcO#
zY4d1SyVFXwvehNYzTmhA(Y(MHd%tm8?w>RZ%NkB!3|&;WvAeF%2ULelm%_f`=JE&e
zpaDxbZQZ3`GT(<OuSo`36Yb1tC($PtPm`1_zjO>mOA;?w(e%!Jepy6yM#4C^^Q+ng
z)%=fh?N0uCKH}MgvX#Tr;lXT_zxd`4DXKh`un@(W1@8u~%Ec&|)uY|?+c^GXpHE-;
z&b<&=kb)$JD8P!Y9t3`;I4=*fzh#qQ537wdlnZL@Nu9w%tcH{SDRtE%!B>io{Wv6`
zBRu&<afCUbza3?@y8C(9frFZtZ}ud+oy=fmRBB5zcUXlabBtaUwg2dLW3i$UMN_#I
z7FKtRV{cavL4(RpgCj%5daYf94PR~<(3qDY&}Sdk;H4}O#Hi0raS4@svPkk#_*@tG
zxYvJUU84jRRIYO)(^NuJXERxvQ-n}JLC-A!9)OB-O5n+gIm{^^A@C9_)U60GYj5^q
zwRk_I>#J<=5h$TC{afh&wf_oie+4UnHaq}@M}4(cD;LQxP5%nB<X+Prsm&48^@F^|
zI+1`sUW+Z}K4UN8WsHK2fduQexa<d@D=)?B;i(*?%NA#s4W~f`8gfzM8?Hx@<`03@
zpA#0s`dRm!?n+Gf^~jWHF2;q)d>C-9(d2nX8xNoHYEUf_KAR{0>5!pSw4c)GvZGl2
zZAVmkYl_CF?7`ZhnPDbdRC}Mz6D?<xkhd-OZ14LNr3(aqTyvkiiMzoORi`3ydNXL!
zzbLLHLt5tRQ<|EIaZy9YCXYQ#gDX#*tUL_2a&*5m+4=ZO*+kskWApF@&j%R^E7nQx
zE|<bc)*jbQqFjq-*TCnyg<RS~<Nk@ZZ2f~Fo;Nj;1>UD;1#1Pz8O^&=s~_cgGrpZ?
z4PZ3(q_#87s(GCm@{e;1MQ-G@3`(h9V~$~^9Ook~h0ZZCQlW_3E~YsfEW{Ki>ot9c
za=40Qu+<e|ZU2m>Rw1*efvRO^$6GR*toIVSi%f{{mC44yAfdaLVVrUvLaVfIf4n_d
zgQve<sA3e6jU!O5H73H&r=`l$Jc@8wMHXySOGu63tKy+_(&o7uga~;A1kr}Ra6K&|
z7>nXgcpijuIe!cv=~=E<7#>^vG6z1;nV|m@@8??ou8Mf%HZ5`OCUtNpnD#SN<RXdc
zT>g(5K%@q1Odiftab8f;1`>tTN+MuYgs@uwzTssOuwlQ}K^y|lP{^YU_Jv!iL#Xc)
zoGix?Hwt>W6X*vN1s<dr6pSVpMb;9ODXda!7^Td+)@W^s(`y%pm`2>)j^%les`rku
zC?(abY#Q?o{`T=*L4YHfRmE3M11)~yuJEL>O-Z}2md510t%3(V1FkZ*U0;vH2oUf;
zvcv77Ps3NH^bj*$l?MZnv%cwp??1krIsTv>mEB$~<@kqwp>d(T_Dg?%x~KE%%=6Si
z*JtSIFNd37`4Z}TYDZ31rw45~W-FI_j%9q(UoE}J^p%~Ks&YN974MIup0DTYJ6&yv
zI*mR2tjkh`mUS8jIxM>y_=k4}{R<BifOmOFA;dsqM~X*_1z%8beT@XOr$|G@x#wj=
z*QO>Mh}Ae^C>E1PO5qjP7-jYZQVDIR{>D0TU<7r0h`hS!$R7`6l=Cp)BUE+e8LyJk
zV|~QIw~JP$uhnKvWo{s29Nug=F!8>jwU4tTt`wJ{E+lP$v)-affyScf;0g8Qtcq$l
z#)uQ_hsF%(1X9Q;>>__MHH!rN*5nyIPv*_a9mRgb`1kGocX2Mz8-pIjaQf)q!3hI%
zch8Y7l1%=<eT{^Z?d{9J9yWb2vrohx(|`Mb%ge%EG^RT{W(_I(L?UFEQ-AG(*i_kX
zV0H99BIki-w*FGZ8tcm#$$Z0)2`|i5l7)hF?4I-{ysUg&VPf-CjW2Y}w$j)|tZra%
z>qfOMU((BoHb;2lfSU++B(c9w*!SG2O`}m3yJjU<p>UPE&aB;SE@cJ9;&}4O*8AgH
zU;Tc*sax@S!bknUxMcb+Tgpcq6I|)h1$HBbLsL`2^eN$)LW`~lbmF*|DN06@0aNPf
zKJ0tG^yzgsQu3WowX8wJh+DPLpPO<q@vHNv`^EG7ML)hdI5RcH4{a2GYJPR=^-rDC
zG?|aE)x5f)FF{{x1q!d#3Xlp*Tp7{{{Zp~omXeNe_0F1ai%Ppm07kZ?rdr7uqR3bU
z&s36$IqAEo6oQu}x^f2zg=v)qQzsaiNlp7V<;mciI|>ETc3}$c`$tS_aN&y5o?tH+
zWAjir0iniTJnNTslKxD54F|0v?neCa_?ixqbIcv>kb<_szI`9NOKmsGOyOSr&0Ata
zakCT>{Uw&4I8hFid$+R33fhZ?Nh9!U2ly~&QbT2|0<^>94t;Fx_OYOt<^K-1U0Jrh
zTG0YwzU|Q>^*PAH71B$)4LukL0@PUGlE8uuTTssra!JbKJN*6{*`A!zr!s<RC|mMr
z-u);1DJslkYaQgTiBdIIM>QC~@~Hah+9yD`8VRWI(QgU)1D2u|vBvBoc))0sWxk1>
zk$SdAoI|(?_Yr<&+m==Rp!305e8z@vgWl>p0yfzLi4>J>S}jBln@HV~$9*ZJC*94Y
zA8@hzOfL5y)}hc7(<<CIlPZ6Dq||-&zfaR^add`wf>Cv!a^^<oQTIIw@^ATce4&h%
zBpzAasqAO^X<XgaBQkr5z}y+%G&`X5J;tE3c3NvH>9*@XCATB%(Pw|8ZZY?r`+j+y
zp8l+5eB_mz>jQ?iKMe$$ojdF#8*OLuBoxPJ*^rdn3@Imv4IBmDbMBU^QkDtawymsd
z#fs5h$nfG~R*8nDA#~!Dyj$M^G4NMQIiW&U@Q1Xuz|f>#rVO3$7qQBS{Y7dAX%V7q
z$Evjj*K7CL5Yam$fhh={Td($f!+DykC|H>0*F}ET{i_!YvBt})Gu9n;MvsSg_PMi`
zZ)=tHFF{(GhWkC*o~aL{`-qX}0lmTJ8=Odc-d_r3&R%(>iuz$n!u3KL=B$I`h-^v0
zLmWPoUdZ@qTlE2fu@2|ndX(Nkg+4y$sWJRV#{XkJo9C2@Fj%n#e0b@TQ241ZkJPZT
zOOr%&Wk!eKL~2vT_1fs!Qx;<J1*v3@ad}bOi>9Hh5AY24y`TuCY>JbBQ*~sLULIwQ
z$X7>H-1zO189w49jlfA}(R{wUBxZ4j`D2bmaIS}by4hk}7v*slXq`vLVG)zUg!%W2
zM(gLv*PgaKwAz<89OUz@Hx<}-GN&NZZqgU5;o&>|&f7QXDMCmrYo!)(H|yT4#ogqL
zfqBm<pKGz^FFyYejlIwkHg}&G6PLv6#Mm^~Y#l$L<to445@{)USF4Ix(rdv3_{scw
zt#ab~pf_26x`vX)cheV1&Z;j7vi5&SKX-a|-Er{!*~ahs&rbznLRA{QqKFU3e~F<&
zRW2Kbd2;r|bO^R!Qo774RdV(i%HOQJ6wyDL1+4~dRE5P~d)w+VwtLpPb<P-!A(CFh
zzY{~K3BzbeugoX~&ni*N5)y3Dqszj5iDbyyn3tsVywq7T$d={UI^EA7jv<E2lPH@<
zz0%T=J5~~b-Gk>cJbZhZjl?Xv73?bo9+dugNDs}f+iGee5p0sY?F^zQVeBo*YT`q*
z3_tpGbfDfd^L%$$d;H^2*XZUfB)OA7Aubhfy@7-Fdcs3kINEN^G3J3h(lJ2y`>Kz4
z!O$=R9erbDaU?LnK>y~i@L!F(T2sRCfQdB0>8!iNKyfFNG<eR0{4%^NV?>fNRGlqI
z)rP#r>!l7(QXjj1*JcovP^d<Z`~7idcb@cK+mW@+ZQ!1*oAa;^SM0F@q-T@B$)_L4
zS)gp`m20lpsFgOnNyft%$#B2Cm5=N<QSlfm*|gwpN?_9um6REmioV9tXWpMDob*k%
zEEYWDAM#ZB+-#;Y=xlSjcTdK{)h*O&k-Q;CiXkQ9;f9F$HRJ0-qsxM)jW!I@TJL^T
z)?R#=lDXGCDlYgksP_@>Kh9Ot`GGX5_5O~>b;(aY$3qL2o6~7OS>8^3nS2qq>Z(FD
z^mC(bPT)37bM`>VRYHgHVD{Vm@8d+&P*rQ$nJxT5$^%Sqq6`VXiJy!c>@r4P+jIm+
zE{wkVTC1Bh&bp$G7H6_VsRQATtRn+w4@P%gYo(MA<47#NTM$QDqBXlg#o@Xig@Yjr
zP-oyALGtdaL^!Yue{`R9rLS{Rq+Swt$I-bVu-6_%t)@rmdQFwo`;1d{z}g1ZRlHfo
zA**d>&&Dqh`mI)xZ!GZ-Mu4}<uS-{&ioaFgvsoaUID8D=R)YM+uO3{n#MNt1VN#CS
z@d5q>+<8OjM%+Z)rS$g9=GE#BjPA%5i7>1w-5zu1p4E?~$9_WxRvG>46!PB!r>7$c
zz<k>Ibq<ul13H+=a@#$Gu16^@>YW5cs^5qM_VR1NLT8-Un%Sf;v@9J3g4C?hA<Gs|
zWIih99MZBJ+fb*EaOwAI^S?)a8eEN|oo9L>m(vD5&B9gesf))pj2tP+G1U3Scb<f?
zCF+<~>p1Od26o=EPo6I{Ht`*t_o;sHiI!lJhDa>-Vd-H58xK|l#<FmV_enJgdBdZ9
zT83-<rXM~ozoARbF_BCCXjk^Ku#rVO_?_csZKLw5yEgx<THi~yEiWnQ+5{#9kD~(P
zq83VKQ8EQF+>1lCjdZLY)s>^YhbH{(k5fM`j2(U;ens3f+xY8fQbw*Iu3>+7cdq9r
zP2S$-uZ9E@j%UY1&93!`KjOKjogFQq6r6urqZ#H(__+!xZYX?&`Qx{R^fSe^=%(rq
zC^?eXjmWQCE9N4IHxVa_W!$HO7*fa8pKRLgdbl#%<67r+I!i~i9eaHbBU|^(+zAx6
z4Y8Q2^(m8y5c~o732p9PF}F2h5Ty-W`^yrCRq%eF>OTF$hi`LADL*Dimz1mXXvNMB
zaG=zEBDk0-0*0gqSDk)H5A-Y_Nwo=)Z{cYb&Ep~)V_9D)sqLTpeH<!J5mFfGeorSN
zHol~wF&LH7ovCHep{=M-d88*^TM4^r?P>qpqkHp+w1$<N?#5e%MPNcq1Nf!tcvu#k
z<w3#}0ahbQvg$KmhGP~z?m4*_@cOB-ljWIJDT^j(W*Km$eNmDK(qY+8p~ClO`KY(*
zmD|vitdpphPx@OKRYZ_Av3A52A@$7S!=&Y?LwjU_OtI3Ipd;mIC0pg}auVGUe8?lm
zoa^|Dp~{hGsaZ*kW+V3>!Pc)kv<=oPQExLB-D@R^)@7n&2jRNO49BgO15*wXj7Vc8
zyHUFmvYWWgGV#f4D^Jrpt#rz#RZ7bTI;K2b`&}1H`F^lOkme2YNuAxz!fO3)>s3oP
znMsopGhA^&+$i>Gu5<}ONr&3Wd~k5;Bb&#TZa=vHg#Fk;mB4X#(5L*B>n-&8L32q;
z<N;q~J`vQs;n{R9r{(w(l8@FLovbg7UAR%H6ROy#u*<`R&rfQZ*ufj>|1LrSk5z$6
zei$9u)3`=)@>6WjbK*LtH2yKe9e@05T{0fnbc|)&URF#B#r??6?merGJ90dXN(_?v
zrrf1U#PLpNJvTNBk2D26PIm;zof25v%i|FReL|XXc-o_zeF7K{k<|jVH`wQ;DEJ12
z;Gph%?70p*J}icQ51!$b5jR7%p3jt0C9>Tv5u9TN#JuoqeeDQ7G6aDZm|oNGcNZTi
z#@Sx|c>4{l{8cmmpCurrILvf7Z<VeJ{w^O5@D3j}J&Y|(^R{c5T#^;e!4lWbT3*W<
zO2;3s_(L!`ZIb#jvX2Ak`Ex%4`k6L{+^p>W)R`Uj@H%tq_u8KgAE9U7D|&0EsjyT!
zB%t1kg!X?s%<GS=H81_bVMsqf--k%mDLN*I_v>&#n|+(~Hu`Ez7OCf&6c$85^tP>=
zuae=IIWVG28BU%2<HL++>v#C`&3R2>>A~Sr;d<GpOEKfdM|6%8EB_P)+S$)?Pv$g!
zoY^R$^Q|z!rR?Z0PeQT7drFp7tH^vVZ`oIuE!KK`O%srd*i^7YTx@^7KRRF{+4!~E
zxW}c<z`k*iN_e3ES9inn4{sgcvM=>mn=uqGu^wazgnbPY*_chtZ``qJIgt@$*n!w~
zy>3r8=*DBWD)1mFV=-?2A1)Q$zA*o;MQO?4RjL)rSGEnp>&Zp(!nr6}V{nl3T*PMT
zG@|@IPZODi%x=-uk2v<x-?2Qs_J>)=F?6nd6d^#Ku}|U1s=jkxG2b@W5VR=mP%p%y
z7D~~44JlYy8qhny+9TRU?#Yy>y5Hc*Gk{L3TJ7U(P~~sY$=nm|Ullo!M3HW;i4U%`
zQDzJuRD74^F!cE>ti-)3E;Z`8B`uH@cgAy*-zz+sRe!{4$nay~nJSZh0xo$z3LbWL
zL&ocDL{)>_(HVtIfO~=FfaY&=aHX|te0E8J7l`>5LmN-eMHPy9M3x)xaf>Nj_50|R
z>Naq)&L$XQ_JQ~^rZtQFewv0UN#dX-d5D*bm5l$uD#d-u(bw_;Kx40Vi#u5-dn}(y
zDvSA%(fdh`iCQ)d8`UD+j3+5X*jyG_)10N=%KE1IWe{K4tdZ=J3EG0ET>Aa1Jl}?|
znY4tIi96h%C${5fi-bvJy4p%>rBqa6LVNGKji1I(;TiPVK2EuP&5|x+)NWlW%mV*O
z>iB$-Bw`z_Q&zSqP8oi_Sl~84D>m*z_^=+<V_HUL78z9gs^joktc*C#6Y3w|J%7nM
z$CdF;cr7+sRJ^UmBzaadd?ktd^y9>$QQzlR#?SP9pL4k;+dso6_1hGu;munqIYW&o
z9H>Yp(tG*15ZFSQ5s8x8y^yzOUi_utHZC>AI^4EAE?aO;KvK<L!CHvaEjK4t;yY-i
z6xQ9}{ZbQwsVhM4n0Z6_1mVbBW#@YQefE#^_)8sdf@`H}E`2-XvLl<JqoQkQ&PdIe
zb1wQ8IJq`Y2btHsa22ww2$KFd{YsH6p70*7yvXI|+tP5>h8z^};S}GHgW1z01p}`K
z++{IoDjOKI({@XR+VZKN-nan|>F{tPd~{g2gLhC7o|&LMH1Neyy|gf#O`}gQ>^G&Z
zov-d>&n^qcz42&K0lkTeAKIwN;?b6S>AB3<Z)l*uy~2Ohx%vNXF7zaLl`sE<DlqQ>
zvlr5IV4)c4vrS7?j$q`X5Uo>&`L!6$AN$>f?8e+N{kHlc<8W`uIMk{kQ0KlSm#LKp
zT|c@O2Q)Aq9;hkv>)eeV%bp0)cBwS@G{JdvnxNQWmhpw_n9GSd)uu*+_gDk>B^h;{
z`L=MqA^U=K6^fOX4wrw#H|Y_JM6MY8*Xzo!`-WO~r3oHjEL;o`L8zk-?`$im2^$H>
z-h_OCPZ3!ZhrAuIUp}3`_B5hAWu&}L0=1;+_MZ1asTvi5J^7T@hV<{_uq4C)*K@c1
zc9Q#kfluM4TPm;BXO_(!@R`yC8z)1bNv96k9K7FLL)JAdWbaUI{5<tNMliA&?tfw~
z7*6FCpjiLGT{8Osmy6fnGDKcinzgoK*ixjVgk+QTYrzjEC<I@SfdWU!fdRh4jnZrg
zRaWb{Px5Io9+}ZmI?KFUwTZDJxU*>}L#pD;=t!X&okp0{d?V%&?H#`We{A_}Y4&NB
zSZ-A=&Oonsem%WmwRbu^{5M#eBe9#lOT_w!l6OI{9tIRX!^*}$(JUgnNjUs439+ZR
zC6o1owOU>`81YkaI9vHTQnxhR8+&MM?v{#vu;@*V9NmnkMk8Jwc%(HWtoNBU@^C73
z^vhy11r5k9^i&K<itc5r2Kq`H55hNR?rUt!o15t-jHU~;7LTeLOp`cZ6G9Lc48nrI
z4;MuL#)q*X2>WlG6dZ#6je{i6wZHMJbI7o8pj{AJ>fk_B|FfU^ZyW}W(fm!5p@{=A
z{!bd2zj1IW$l!nWgG~tkIs{!@gCI^2QUJIBHvoHJ-#LH`#D9Qr0nh{30uX?G(;!R+
z<O8w+Er4#o)L%Y>@Hs#izz>iG2m{Ok&j9X#w}95aVH5}x0L}nEz}0_$1bhNC0V)CG
z01|L*M!<EzB-ow<V1xM8y<Z)>y8k1v@9I9T(*O4v;9$Q4Km#BH;00`h;}8hR0onio
zfCb>{m=ypC5CKR5uJYk3k4ykQ0A~OTKo%efxH?A|aFymN|NeXZA#jaqz}2&O18f1T
z00__r3b_uD3`hWk0|NiXGeH>ncb_W=c>wb`5Htp;2RsGX0lWZa08r$R5<n4v1TX<e
z0XG1I0J6XFV^C49wts@K8ju6<2Y3S<01^OcfEGX<pbRhr*!{%=gjeaE0oni^fFytn
zKn(!Z3nBuD0oVcD02ts1RK9({cfbZ<7w{D@3z!1*{f)1Ka2s$4SO)9>P5`}tZ-A?F
z+5p9XhQIMD5MI&vmuL_M0xSS(03CohKpWr=cnYusr~^=dt7B3C4S*%U8Sn^T4Uh%E
z0k;7HfU9&g0P?@vJRp<?umOGpL1p;6pAv+)0A>IY;A$HJU;}OdjzQY1<M@DW5T62^
z0FD6bfS-T40O8endmy}uUtNP5Y|{al05`zltNKeq44#7uJOd*Hd9r}=<c1(8A@H0M
z5cEP0<b?{T_u8P^8A8y32?Sj}0`g-AL9VXgzn%ga^MRmO{ty^1hoGA=ApesfDD?x7
zt85?(1rQWd20=sB;8vO-=s`O;w-<s`M<8f$3WB1(f_wS_+JOy_Kl>2$`2@(}A5c!X
zSdb+l76hZff?DaYpr^O7APo*I=srId^h6X3@<Cuh`bt<3zXld0sD}k9-2>YXupn<+
zEXd6n3sUvMf*8E9AR0d`h%*EWGKt25+LN##;&d!XGY1QLR)hrsV?K~gJ&3n~V?9{V
z!7vu2H;o1PE?`05R<Iyekh%9jrpBIQL76aYs1IZ~8z{U_AOnf-U_(Nj*pMqI*mN;)
zOa>deL}EkiTG$Z30XBqdf(^|)#D;PoV?z&Iu%SO**w9OFZ0OrR*bq)IAQBrIio=FN
zlCdG?bZn?P3ma0($A-E~up#qGP&oD2P-F`>MBfS0fC^p#8vB?DY{>cxHgszN8%kKh
zhAw_$L++c{&;Sq!S|B8*zp<hB7ue8uAV`03VGs#E45A^1K`i7jNCXHNf*uCJnP8AC
zGYnz_0yf75gZ%hl5UvmmdLsgZ*2Q5EGZ0{NSs0Y30E70CFi2bt2E}Q@pfeN<veJh^
z6GkxT-hCL<0R)@;Aq;wC1%py-VNiiR3~F_PL2a%uXb1>8?o$}V^8yAL`GEAVVUV^T
z43Z0kLAQfp5J4FDzX%vK6AgnZ;$To%A`H5l41<KyU=Tw(48qHRK^Ivt==*0FG@K8E
zx{F{?Y$*)#K*J#2DsbIe7}Qb^gM6C6v$VpXLktYc?}R~*dteZ2Klsl<7$i9YgObKz
z(82@^;-7{=HeX;+`5efL1sIh49lT4IVUWfO40^W;?spwra|;F;?7*PPJ&*^7Fv$NH
z2K_t%`E&+@j4xo&vp+B>2fQP{!Em5oxHu3VXdET*aUd(uAbubQc4I(;*+7m1O;O@N
z*JyAcdO95F76T5Xa1#eY-NJz^?*LeEAZs=p$cY2Qxp1JzJUEa$9}e_D00$Bi!h!gO
taUd2^9EeC92Rf9%fo7#}plJj^76<B)$ALOQ1KkW7=m`}ZXj=^j`hTubB?JHf

literal 0
HcmV?d00001

diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index e55cb3f0..902ab88f 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -1,6 +1,7 @@
 import json
 import logging
 import os
+import cv2
 from typing import Union, List
 
 import matplotlib.pyplot as plt
@@ -12,7 +13,7 @@
 from kompass_cpp.types import PathInterpolationType, Path as PathCpp
 
 from kompass_core.datatypes.laserscan import LaserScanData
-from kompass_core.datatypes import TrackedPose2D, Bbox3D
+from kompass_core.datatypes import TrackedPose2D, Bbox3D, Bbox2D
 from kompass_core.control import (
     DVZ,
     DWAConfig,
@@ -408,8 +409,8 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw
         max_angular_samples=4,
         octree_resolution=0.1,
         costs_weights=cost_weights,
-        prediction_horizon=1.0,
-        control_horizon=0.2,
+        prediction_horizon=10,
+        control_horizon=2,
         control_time_step=control_time_step,
         max_num_threads=1,
     )
@@ -429,11 +430,106 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw
     assert reached_end is True
 
 
+def test_vision_dwa_with_depth_img():
+    """Run DWA pytest and assert reaching end"""
+    global global_path, my_robot, robot_ctr_limits, control_time_step
+
+    from kompass_core import set_logging_level
+
+    set_logging_level("DEBUG")
+
+    my_robot.state.x = -0.5
+
+    image_file_path = f"{control_resources}/bag_image_depth.tif"
+    # clicked point on image to track target
+    clicked_point = [610, 200]
+    # Detected 2D box
+    box = Bbox2D(top_left_corner=np.array([410, 0]), size=np.array([410, 390]))
+    detections = [box]
+
+    point_cloud = [np.array([10.3, 10.5, 0.2], dtype=np.float32)]
+
+    control_horizon = 2
+    prediction_horizon = 10
+
+    focal_length = [911.71, 910.288]
+    principal_point = [643.06, 366.72]
+
+    cost_weights = TrajectoryCostsWeights(
+        reference_path_distance_weight=1.0,
+        goal_distance_weight=0.0,
+        smoothness_weight=0.0,
+        jerk_weight=0.0,
+        obstacles_distance_weight=0.0,
+    )
+    config = VisionDWAConfig(
+        control_time_step=control_time_step,
+        control_horizon=control_horizon,
+        prediction_horizon=prediction_horizon,
+        speed_gain=1.0,
+        rotation_gain=1.0,
+        costs_weights=cost_weights,
+        max_linear_samples=20,
+        max_angular_samples=20,
+        octree_resolution=0.1,
+        max_num_threads=1,
+        min_vel=0.05,
+        min_depth=0.001,
+        max_depth=5.0,
+        depth_conversion_factor=0.001,
+        camera_position_to_robot=np.array([0.32, 0.02089, 0.2999]),
+        camera_rotation_to_robot=np.array([-0.5846, 0.595, -0.395, 0.385])
+    )
+
+    controller = VisionDWA(
+        robot=my_robot,
+        ctrl_limits=robot_ctr_limits,
+        config=config,
+        camera_focal_length=focal_length,
+        camera_principal_point=principal_point,
+    )
+
+    # Get image
+    cv_img = cv2.imread(image_file_path, cv2.IMREAD_UNCHANGED)
+
+    if cv_img is None:
+        logging.error("Could not open or find the image")
+        return
+
+    # Create a NumPy array from the OpenCV Mat
+    depth_image = np.array(cv_img, dtype=np.ushort, order="F")
+
+    found_target = controller.set_initial_tracking_depth(
+        my_robot.state, clicked_point[0], clicked_point[1], detections, depth_image
+    )
+
+    if not found_target:
+        print("Point not found on image")
+        return
+    else:
+        print("Point found on image ...")
+
+    res = controller.loop_step(
+        current_state=my_robot.state,
+        detections_2d=detections,
+        depth_image=depth_image,
+        point_cloud=point_cloud,
+    )
+    if not res:
+        print("No control found")
+
+    assert res
+
+    velocities = controller.control_till_horizon
+    (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
+    print(f"Found Control {vx}, {vy}, {omega}")
+
+
 @pytest.mark.parametrize("use_tracker", [False, True])
-@pytest.mark.parametrize("point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]])
-def test_vision_dwa(
-    use_tracker: bool, point_cloud: List[np.ndarray]
-):
+@pytest.mark.parametrize(
+    "point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]]
+)
+def test_vision_dwa(use_tracker: bool, point_cloud: List[np.ndarray]):
     """Run DWA pytest and assert reaching end"""
     global global_path, my_robot, robot_ctr_limits, control_time_step
 
@@ -473,7 +569,6 @@ def test_vision_dwa(
         octree_resolution=0.1,
         max_num_threads=1,
         min_vel=0.05,
-        use_tracker=use_tracker
     )
 
     controller = VisionDWA(robot=my_robot, ctrl_limits=robot_ctr_limits, config=config)
@@ -485,7 +580,12 @@ def test_vision_dwa(
     detected_boxes = []
     boxes_ori = 0.0
     for i in range(3):
-        new_box = Bbox3D(center=np.array([0.0, 0.0, 0.0], dtype=np.float32), size=np.array([0.5, 0.5, 1.0], dtype=np.float32), center_img_frame=np.array([150, 150], dtype=np.int32), size_img_frame=np.array([25, 25], dtype=np.int32))
+        new_box = Bbox3D(
+            center=np.array([0.0, 0.0, 0.0], dtype=np.float32),
+            size=np.array([0.5, 0.5, 1.0], dtype=np.float32),
+            center_img_frame=np.array([150, 150], dtype=np.int32),
+            size_img_frame=np.array([25, 25], dtype=np.int32),
+        )
         new_box_shift = np.array([0.7 * i, 0.7 * i, 0.0], dtype=np.float32)
         img_frame_shift = np.array([50 * i, 50 * i], dtype=np.int32)
         new_box.center = new_box_shift
@@ -505,7 +605,7 @@ def moveBoxes(boxes_orientation, boxes) -> float:
         return boxes_orientation
 
     if use_tracker:
-        controller.set_initial_tracking(150, 150, detected_boxes)
+        controller.set_initial_tracking_3d(150, 150, detected_boxes)
 
     while steps < 100:
         robot_x.append(my_robot.state.x)
@@ -514,7 +614,11 @@ def moveBoxes(boxes_orientation, boxes) -> float:
         tracked_y.append(tracked_pose.y())
 
         if use_tracker:
-            res = controller.loop_step(current_state=my_robot.state, detections=detected_boxes, point_cloud=point_cloud)
+            res = controller.loop_step(
+                current_state=my_robot.state,
+                detections_3d=detected_boxes,
+                point_cloud=point_cloud,
+            )
         else:
             res = controller.loop_step(
                 current_state=my_robot.state,
@@ -527,9 +631,7 @@ def moveBoxes(boxes_orientation, boxes) -> float:
         # print(f"Found trajectory with cost {controller.result_cost}")
         velocities = controller.control_till_horizon
         (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
-        print(
-                f"Found Control {vx}, {vy}, {omega}"
-            )
+        print(f"Found Control {vx}, {vy}, {omega}")
         my_robot.set_control(
             velocity_x=vx,
             velocity_y=vy,
@@ -549,9 +651,7 @@ def moveBoxes(boxes_orientation, boxes) -> float:
 
     figure_name += f"{point_cloud[0]}"
     plt.figure()
-    plt.plot(
-        robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path"
-    )
+    plt.plot(robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path")
     plt.plot(
         tracked_x,
         tracked_y,
@@ -585,8 +685,8 @@ def test_dwa_debug():
         max_angular_samples=21,
         octree_resolution=0.1,
         costs_weights=cost_weights,
-        prediction_horizon=1.0,
-        control_horizon=0.2,
+        prediction_horizon=10,
+        control_horizon=2,
         control_time_step=control_time_step,
         max_num_threads=1,
     )
@@ -688,30 +788,31 @@ def main():
 
     control_time_step = 0.1
 
-    print("RUNNING PATH INTERPOLATION TEST")
-    test_path_interpolation(plot=True)
+    # print("RUNNING PATH INTERPOLATION TEST")
+    # test_path_interpolation(plot=True)
 
-    ## TESTING STANLEY ##
-    print("RUNNING STANLEY CONTROLLER TEST")
-    test_stanley(
-        plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
-    )
+    # ## TESTING STANLEY ##
+    # print("RUNNING STANLEY CONTROLLER TEST")
+    # test_stanley(
+    #     plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
+    # )
 
-    ## TESTING DVZ ##
-    print("RUNNING DVZ CONTROLLER TEST")
-    test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
+    # ## TESTING DVZ ##
+    # print("RUNNING DVZ CONTROLLER TEST")
+    # test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
 
-    ## TESTING DWA DEBUG MODE ##
-    print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
-    test_dwa_debug()
+    # ## TESTING DWA DEBUG MODE ##
+    # print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
+    # test_dwa_debug()
 
-    ## TESTING DWA ##
-    print("RUNNING DWA CONTROLLER TEST")
-    test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
+    # ## TESTING DWA ##
+    # print("RUNNING DWA CONTROLLER TEST")
+    # test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
 
-    ## TESTING VISION DWA ##
-    print("RUNNING VISION DWA CONTROLLER TEST")
-    test_vision_dwa(use_tracker=True, point_cloud=[np.array([0.3, 0.27, 0.1])])
+    # ## TESTING VISION DWA ##
+    # print("RUNNING VISION DWA CONTROLLER TEST")
+    # test_vision_dwa(use_tracker=True, point_cloud=[np.array([0.3, 0.27, 0.1])])
+    test_vision_dwa_with_depth_img()
 
 
 if __name__ == "__main__":

From 960fdd92a16b4b8ce188fb84ed3732f63f77f190 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 12 May 2025 13:27:39 +0200
Subject: [PATCH 049/118] (fix) Updates Vision Controller key

---
 src/kompass_core/control/__init__.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index c5f9e46a..188f3cc1 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -71,7 +71,7 @@ class ControllersID(StrEnum):
     STANLEY = "Stanley"
     DWA = "DWA"
     DVZ = "DVZ"
-    VISION = "VisionFollower"
+    VISION = "VisionDWA"
 
 
 ControlClasses = {

From 9dde97488d26282b6fb4c5f6274209f63585d882 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 12 May 2025 23:48:00 +0800
Subject: [PATCH 050/118] (fix) Adds check on incoming dtections before setting
 target

---
 src/kompass_core/control/vision_dwa.py | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 8e98a057..716b8f3c 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -144,6 +144,8 @@ def __init__(
         :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
         :type params_file: str
         """
+        import logging
+        logging.info("INIT VISION DWA")
         self._config = config or VisionDWAConfig()
 
         if config_file:
@@ -196,7 +198,10 @@ def set_initial_tracking_3d(
         :type detected_boxes: List[Bbox3D]
         """
         try:
-            return self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+            if any(detected_boxes):
+                return self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+            logging.error(f"Could not set initial tracking state: No detections are provided")
+            return False
         except Exception as e:
             logging.error(f"Could not set initial tracking state: {e}")
             return False
@@ -218,10 +223,13 @@ def set_initial_tracking_depth(
         try:
             self._planner.set_current_state(
             current_state.x, current_state.y, current_state.yaw, current_state.speed)
+            if any(detected_boxes):
+                return self._planner.set_initial_tracking(
+                    pose_x_img, pose_y_img, aligned_depth_image, detected_boxes
+                )
+            logging.error(f"Could not set initial tracking state: No detections are provided")
+            return False
 
-            return self._planner.set_initial_tracking(
-                pose_x_img, pose_y_img, aligned_depth_image, detected_boxes
-            )
         except Exception as e:
             logging.error(f"Could not set initial tracking state: {e}")
             return False

From 342b9c18c438a2e4f440d14db19c311cadf15463 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 14 May 2025 16:02:42 +0800
Subject: [PATCH 051/118] (fix) Minor fix for loop_step return value

---
 src/kompass_core/control/vision_dwa.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 716b8f3c..98f96411 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -144,8 +144,6 @@ def __init__(
         :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
         :type params_file: str
         """
-        import logging
-        logging.info("INIT VISION DWA")
         self._config = config or VisionDWAConfig()
 
         if config_file:
@@ -172,7 +170,7 @@ def __init__(
             max_num_threads=self._config.max_num_threads,
             config=self._config.to_kompass_cpp(),
         )
-        if camera_focal_length and camera_principal_point:
+        if camera_focal_length is not None and camera_principal_point is not None:
             self._planner.set_camera_intrinsics(camera_focal_length[0], camera_focal_length[1], camera_principal_point[0], camera_principal_point[1])
 
         # Init the following result
@@ -200,6 +198,7 @@ def set_initial_tracking_3d(
         try:
             if any(detected_boxes):
                 return self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+
             logging.error(f"Could not set initial tracking state: No detections are provided")
             return False
         except Exception as e:
@@ -298,7 +297,7 @@ def loop_step(
             logging.error(f"Could not find velocity command: {e}")
             return False
 
-        return True
+        return self._result.is_found
 
     def has_result(self) -> None:
         """

From 9d73a569e07353ca9558e1c44db6602d96a649ff Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 14 May 2025 16:58:43 +0200
Subject: [PATCH 052/118] (fix) Fixes critical_zone_checker bindings

---
 src/kompass_cpp/bindings/bindings_utils.cpp                     | 2 +-
 src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h | 2 +-
 .../kompass_cpp/include/utils/critical_zone_check_gpu.h         | 2 +-
 src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp   | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_utils.cpp b/src/kompass_cpp/bindings/bindings_utils.cpp
index dc5b7a0d..d24691f5 100644
--- a/src/kompass_cpp/bindings/bindings_utils.cpp
+++ b/src/kompass_cpp/bindings/bindings_utils.cpp
@@ -16,7 +16,7 @@ void bindings_utils(py::module_ &m) {
 
   py::class_<CriticalZoneChecker>(m_utils, "CriticalZoneChecker")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const Eigen::Vector3f &, const Eigen::Quaternionf &, float,
+                    const Eigen::Vector3f &, const Eigen::Vector4f &, float,
                     float>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
index b6b633f7..ffbe8295 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
@@ -22,7 +22,7 @@ class CriticalZoneChecker {
   CriticalZoneChecker(const CollisionChecker::ShapeType robot_shape_type,
                       const std::vector<float> &robot_dimensions,
                       const Eigen::Vector3f &sensor_position_body,
-                      const Eigen::Quaternionf &sensor_rotation_body,
+                      const Eigen::Vector4f &sensor_rotation_body,
                       const float critical_angle,
                       const float critical_distance);
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
index d3766c92..1f1f2284 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
@@ -19,7 +19,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
   CriticalZoneCheckerGPU(const CollisionChecker::ShapeType robot_shape_type,
                          const std::vector<float> &robot_dimensions,
                          const Eigen::Vector3f &sensor_position_body,
-                         const Eigen::Quaternionf &sensor_rotation_body,
+                         const Eigen::Vector4f &sensor_rotation_body,
                          const float critical_angle,
                          const float critical_distance,
                          const std::vector<double> &angles)
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
index f4f8823a..8fd0c83f 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
@@ -14,7 +14,7 @@ CriticalZoneChecker::CriticalZoneChecker(
     const CollisionChecker::ShapeType robot_shape_type,
     const std::vector<float> &robot_dimensions,
     const Eigen::Vector3f &sensor_position_body,
-    const Eigen::Quaternionf &sensor_rotation_body, const float critical_angle,
+    const Eigen::Vector4f &sensor_rotation_body, const float critical_angle,
     const float critical_distance) {
   // Construct  a geometry object based on the robot shape
   if (robot_shape_type == CollisionChecker::ShapeType::CYLINDER) {

From 7ccb2f0bbb55738d295118d2d09cbe9a9329b7be Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 14 May 2025 17:02:39 +0200
Subject: [PATCH 053/118] (fix) Updates VisionDWA to use sampling only when
 target is lost

---
 .../include/controllers/vision_dwa.h          | 131 ++++++------------
 .../src/controllers/vision_dwa.cpp            |  36 ++++-
 .../tests/vision_detector_test.cpp            |  19 +--
 src/kompass_cpp/tests/vision_dwa_test.cpp     |   8 +-
 tests/test_controllers.py                     |   2 +-
 5 files changed, 93 insertions(+), 103 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 9c6f0df3..bde09e78 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -2,15 +2,17 @@
 
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
+#include "datatypes/path.h"
 #include "datatypes/tracking.h"
+#include "datatypes/trajectory.h"
 #include "dwa.h"
 #include "utils/logger.h"
 #include "vision/depth_detector.h"
 #include "vision/tracker.h"
 #include <Eigen/Dense>
+#include <boost/filesystem/path.hpp>
 #include <cmath>
 #include <memory>
-#include <tuple>
 #include <vector>
 
 namespace Kompass {
@@ -161,31 +163,21 @@ class VisionDWA : public DWA {
                                             const T &sensor_points) {
     LOG_DEBUG("Tracked pose: ", tracked_pose.x(), tracked_pose.y(),
               tracked_pose.yaw());
-    Trajectory2D ref_traj;
-    bool has_collisions;
-    std::tie(ref_traj, has_collisions) =
-        this->getTrackingReferenceSegment(tracked_pose, sensor_points);
-    if (!has_collisions) {
-      // The tracking sample is collision free -> No need to explore other
-      // samples
-      TrajSearchResult result;
-      result.isTrajFound = true;
-      result.trajCost = 0.0;
-      result.trajectory = ref_traj;
-      latest_velocity_command_ = ref_traj.velocities.getFront();
-      return result;
-    } else {
-      LOG_DEBUG("USING DWA SAMPLING");
-      // The tracking sample has collisions -> use DWA-like sampling and control
-      Path::Path ref_tracking_path(ref_traj.path.x, ref_traj.path.y,
-                                   ref_traj.path.z,
-                                   config_.prediction_horizon());
-      // Set the tracking segment as the reference path
-      // Interpolation of the path is not required as the reference is already
-      // created using the robot control time step
-      this->setCurrentPath(ref_tracking_path, false);
-      return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    }
+    Trajectory2D ref_traj =
+        getTrackingReferenceSegment(tracked_pose, sensor_points);
+    TrajSearchResult result;
+    result.isTrajFound = true;
+    result.trajCost = 0.0;
+    result.trajectory = ref_traj;
+    latest_velocity_command_ = ref_traj.velocities.getFront();
+    // ---------------------------------------------------------------
+    // Update reference to use in case goal is lost
+    auto referenceToTarget =
+        Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z,
+                   config_.prediction_horizon());
+    this->setCurrentPath(referenceToTarget, false);
+    // ---------------------------------------------------------------
+    return result;
   };
 
   /**
@@ -236,27 +228,31 @@ class VisionDWA : public DWA {
           "Tracker is not initialized with an initial tracking target. Call "
           "'VisionDWA::setInitialTracking' first");
     }
-    // Send current state to the detector
-    detector_->updateState(currentState);
-    detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
-    auto boxes_3d = detector_->get3dDetections();
-
-    if (boxes_3d) {
-      LOG_DEBUG("Got 3D boxes from 2d");
-      // Update the tracker with the detected boxes
-      tracker_->updateTracking(boxes_3d.value());
-      auto tracked_pose = tracker_->getFilteredTrackedPose2D();
-      if (tracked_pose) {
-        return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
-                                        sensor_points);
+    if (!detected_boxes_2d.empty()) {
+      // Send current state to the detector
+      detector_->updateState(currentState);
+      detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
+      auto boxes_3d = detector_->get3dDetections();
+      if (boxes_3d) {
+        LOG_DEBUG("Got 3D boxes from 2d");
+        // Update the tracker with the detected boxes
+        tracker_->updateTracking(boxes_3d.value());
+        auto tracked_pose = tracker_->getFilteredTrackedPose2D();
+        if (tracked_pose) {
+          return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
+                                          sensor_points);
+        }
       } else {
-        LOG_WARNING("Tracker failed to find the target");
-        // Return false for trajectory found
-        return TrajSearchResult();
+        LOG_WARNING("Detector failed to find 3D boxes");
       }
+    }
+    // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
+    if (!isGoalReached()) {
+      LOG_DEBUG("USING DWA SAMPLING TO GO TO LAST KNOWN LOCATION");
+      // The tracking sample has collisions -> use DWA-like sampling and control
+      return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
-      LOG_WARNING("Detector failed to find 3D boxes");
-      // Return false for trajectory found
+      LOG_WARNING("Target is Lost");
       return TrajSearchResult();
     }
   };
@@ -284,8 +280,7 @@ class VisionDWA : public DWA {
    * @return false
    */
   bool
-  setInitialTracking(const int pose_x_img,
-                     const int pose_y_img,
+  setInitialTracking(const int pose_x_img, const int pose_y_img,
                      const Eigen::MatrixX<unsigned short> &aligned_depth_image,
                      const std::vector<Bbox2D> &detected_boxes_2d);
 
@@ -297,53 +292,13 @@ class VisionDWA : public DWA {
   Eigen::Isometry3f vision_sensor_tf_;
 
   /**
-   * @brief Get the Tracking Reference Trajectory Segment and if this segment is
-   * has collision
+   * @brief Get the Tracking Reference Trajectory Segment
    *
    * @tparam T
    * @param tracking_pose
-   * @param sensor_points
    * @return std::tuple<Trajectory2D, bool>
    */
-  template <typename T>
-  std::tuple<Trajectory2D, bool>
-  getTrackingReferenceSegment(const TrackedPose2D &tracking_pose,
-                              const T &sensor_points) {
-    int step = 0;
-
-    Trajectory2D ref_traj(config_.prediction_horizon());
-    std::vector<Path::State> states;
-    Path::State simulated_state = currentState;
-    Path::State original_state = currentState;
-    TrackedPose2D simulated_track = tracking_pose;
-    Velocity2D cmd;
-
-    // Simulate following the tracked target for the period til
-    // prediction_horizon assuming the target moves with its same current
-    // velocity
-    while (step < config_.prediction_horizon()) {
-      states.push_back(simulated_state);
-      ref_traj.path.add(step,
-                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
-      this->setCurrentState(simulated_state);
-      cmd = this->getPureTrackingCtrl(simulated_track);
-      simulated_state.update(cmd, config_.control_time_step());
-      simulated_track.update(config_.control_time_step());
-      if (step < config_.prediction_horizon() - 1) {
-        ref_traj.velocities.add(step, cmd);
-      }
-
-      step++;
-    }
-    this->setCurrentState(original_state);
-
-    bool has_collisions =
-        trajSampler->checkStatesFeasibility(states, sensor_points);
-
-    LOG_DEBUG("Found reference traj with collisions: ", has_collisions);
-
-    return std::make_tuple(ref_traj, has_collisions);
-  };
+  Trajectory2D getTrackingReferenceSegment(const TrackedPose2D &tracking_pose);
   //
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 57d1d83a..50e0e3a3 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -101,8 +101,7 @@ bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
 }
 
 bool VisionDWA::setInitialTracking(
-    const int pose_x_img,
-    const int pose_y_img,
+    const int pose_x_img, const int pose_y_img,
     const Eigen::MatrixX<unsigned short> &aligned_depth_image,
     const std::vector<Bbox2D> &detected_boxes) {
   if (!detector_) {
@@ -136,5 +135,38 @@ bool VisionDWA::setInitialTracking(
   return tracker_->setInitialTracking(boxes_3d.value()[0]);
 }
 
+Trajectory2D
+VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
+  int step = 0;
+
+  Trajectory2D ref_traj(config_.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til
+  // prediction_horizon assuming the target moves with its same current
+  // velocity
+  while (step < config_.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    simulated_state.update(cmd, config_.control_time_step());
+    simulated_track.update(config_.control_time_step());
+    if (step < config_.prediction_horizon() - 1) {
+      ref_traj.velocities.add(step, cmd);
+    }
+
+    step++;
+  }
+  this->setCurrentState(original_state);
+
+  return ref_traj;
+};
+
 } // namespace Control
 } // namespace Kompass
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
index b0e44f9d..48769a99 100644
--- a/src/kompass_cpp/tests/vision_detector_test.cpp
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -4,6 +4,7 @@
 #include "utils/transformation.h"
 #include "vision/depth_detector.h"
 #include <Eigen/Dense>
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <opencv2/opencv.hpp>
 #include <stdexcept>
 #define BOOST_TEST_MODULE KOMPASS TESTS
@@ -30,23 +31,23 @@ struct DepthDetectorTestConfig {
 
   DepthDetectorTestConfig(const std::string image_filename, const Bbox2D &box) {
     // Body to camera tf from robot of test pictures
-    auto body_to_link_tf =
+    auto link_in_body =
         getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
                           Eigen::Vector3f{0.32f, 0.0209f, 0.3f});
 
-    auto link_to_cam_tf =
+    auto cam_in_link =
         getTransformation(Eigen::Quaternionf{0.01f, -0.00131f, 0.002f, 0.9999f},
                           Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
 
-    auto cam_to_cam_opt_tf =
+    auto cam_opt_in_cam =
         getTransformation(Eigen::Quaternionf{-0.5f, 0.5f, -0.5f, 0.5f},
                           Eigen::Vector3f{0.0f, 0.0105f, 0.0f});
 
-    Eigen::Isometry3f body_to_cam_tf = body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
+    Eigen::Isometry3f cam_in_body = link_in_body * cam_in_link * cam_opt_in_cam;
 
-    detector = std::make_unique<DepthDetector>(depth_range, body_to_cam_tf,
-                                               focal_length, principal_point,
-                                               depth_conv_factor);
+    detector =
+        std::make_unique<DepthDetector>(depth_range, cam_in_body, focal_length,
+                                        principal_point, depth_conv_factor);
 
     detector->updateState(current_state);
     cv_img = cv::imread(image_filename, cv::IMREAD_GRAYSCALE);
@@ -132,6 +133,8 @@ BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image) {
   auto boxes = config.run(outputFilename);
 
   const float approx_actual_dist = 1.0;
-  BOOST_TEST(std::abs(boxes[0].center.x() - approx_actual_dist) <= 0.1 ,
+  LOG_INFO("Got distance = ", boxes[0].center.x(),
+           ",  boxes[0].center.y()=", boxes[0].center.y());
+  BOOST_TEST(std::abs(boxes[0].center.x() - approx_actual_dist) <= 0.1,
              "3D box distance is not equal to approximate measured distance");
 }
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 9c7cb1e2..e2cca473 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -6,6 +6,7 @@
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
+#include <Eigen/src/Geometry/Quaternion.h>
 #include <memory>
 #include <opencv2/opencv.hpp>
 #define BOOST_TEST_MODULE KOMPASS TESTS
@@ -124,9 +125,8 @@ struct VisionDWATestConfig {
         body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
 
     Eigen::Vector3f translation = body_to_cam_tf.translation();
-    Eigen::Vector4f rotation = {
-        -0.5846, 0.595, -0.395,
-        0.385}; // Eigen::Vector4f(body_to_cam_tf.rotation().data());
+    Eigen::Quaternionf rotation_quat = Eigen::Quaternionf(body_to_cam_tf.rotation());
+    Eigen::Vector4f rotation = {rotation_quat.w(), rotation_quat.x(), rotation_quat.y(), rotation_quat.z()};
 
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
@@ -390,7 +390,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
   auto initial_point = Eigen::Vector2i{610, 200};
 
   // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+  std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
 
   VisionDWATestConfig testConfig(cloud);
 
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index 902ab88f..bdc2daba 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -478,7 +478,7 @@ def test_vision_dwa_with_depth_img():
         max_depth=5.0,
         depth_conversion_factor=0.001,
         camera_position_to_robot=np.array([0.32, 0.02089, 0.2999]),
-        camera_rotation_to_robot=np.array([-0.5846, 0.595, -0.395, 0.385])
+        camera_rotation_to_robot=np.array([0.385 , -0.5846, 0.595, -0.395]),
     )
 
     controller = VisionDWA(

From 6c5e0308ebdce016235b585dbfd7bed30c1fd93e Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 14 May 2025 17:19:30 +0200
Subject: [PATCH 054/118] (fix) Fixes minor typo

---
 src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index bde09e78..4680b7a3 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -163,8 +163,7 @@ class VisionDWA : public DWA {
                                             const T &sensor_points) {
     LOG_DEBUG("Tracked pose: ", tracked_pose.x(), tracked_pose.y(),
               tracked_pose.yaw());
-    Trajectory2D ref_traj =
-        getTrackingReferenceSegment(tracked_pose, sensor_points);
+    Trajectory2D ref_traj = getTrackingReferenceSegment(tracked_pose);
     TrajSearchResult result;
     result.isTrajFound = true;
     result.trajCost = 0.0;

From 38eed5049dbef495cfb3c73318d2ddd34e662df5 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 14 May 2025 17:35:51 +0200
Subject: [PATCH 055/118] (fix) Updates CriticalZoneChecker bindings

---
 src/kompass_cpp/bindings/bindings_gpu.cpp     | 2 +-
 src/kompass_cpp/tests/collisions_test.cpp     | 2 +-
 src/kompass_cpp/tests/collisions_test_gpu.cpp | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_gpu.cpp b/src/kompass_cpp/bindings/bindings_gpu.cpp
index 4bd75b13..18fdf555 100644
--- a/src/kompass_cpp/bindings/bindings_gpu.cpp
+++ b/src/kompass_cpp/bindings/bindings_gpu.cpp
@@ -26,7 +26,7 @@ void bindings_utils_gpu(py::module_ &m) {
 
   py::class_<CriticalZoneCheckerGPU>(m, "CriticalZoneCheckerGPU")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const Eigen::Vector3f &, const Eigen::Quaternionf &, float,
+                    const Eigen::Vector3f &, const Eigen::Vector4f &, float,
                     float, const std::vector<double> &>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
diff --git a/src/kompass_cpp/tests/collisions_test.cpp b/src/kompass_cpp/tests/collisions_test.cpp
index dc62c46e..fdd62ce1 100644
--- a/src/kompass_cpp/tests/collisions_test.cpp
+++ b/src/kompass_cpp/tests/collisions_test.cpp
@@ -123,7 +123,7 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   std::vector<float> robotDimensions{0.51, 0.27, 0.4};
 
   const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4};
-  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0.99, 0.0};
+  const Eigen::Vector4f sensor_rotation_body{0, 0, 0.99, 0.0};
 
   // Robot laserscan value
   std::vector<double> scan_angles;
diff --git a/src/kompass_cpp/tests/collisions_test_gpu.cpp b/src/kompass_cpp/tests/collisions_test_gpu.cpp
index 18d98cda..d554d155 100644
--- a/src/kompass_cpp/tests/collisions_test_gpu.cpp
+++ b/src/kompass_cpp/tests/collisions_test_gpu.cpp
@@ -17,7 +17,7 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   std::vector<float> robotDimensions{0.51, 0.27, 0.4};
 
   const Eigen::Vector3f sensor_position_body{0.22, 0.0, 0.4};
-  const Eigen::Quaternionf sensor_rotation_body{0, 0, 0.99, 0.0};
+  const Eigen::Vector4f sensor_rotation_body{0, 0, 0.99, 0.0};
 
   // Robot laserscan value
   std::vector<double> scan_angles;

From dd424b68936032d04cbd23ad9bf147868a1d7c92 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 15 May 2025 09:44:51 +0200
Subject: [PATCH 056/118] (fix) Adds  DWA sampling to getTrackingCtrl using
 direct 3D boxes

---
 .../include/controllers/vision_dwa.h          | 36 +++++++++++--------
 .../src/controllers/vision_dwa.cpp            |  2 ++
 2 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 4680b7a3..265388d5 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -193,22 +193,31 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult
   getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
                   const Velocity2D &current_vel, const T &sensor_points) {
-    if (tracker_->trackerInitialized()) {
-      // Update the tracker with the detected boxes
-      tracker_->updateTracking(detected_boxes);
-      auto tracked_pose = tracker_->getFilteredTrackedPose2D();
-      if (tracked_pose) {
-        return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
-                                        sensor_points);
+    if(!detected_boxes.empty()){
+      if (tracker_->trackerInitialized()) {
+        // Update the tracker with the detected boxes
+        tracker_->updateTracking(detected_boxes);
+        auto tracked_pose = tracker_->getFilteredTrackedPose2D();
+        if (tracked_pose) {
+          return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
+                                          sensor_points);
+        } else {
+          LOG_WARNING("Tracker failed to find the target");
+        }
       } else {
-        LOG_WARNING("Tracker failed to find the target");
-        // Return false for trajectory found
-        return TrajSearchResult();
+        throw std::runtime_error(
+            "Tracker is not initialized with an initial tracking target. Call "
+            "'VisionDWA::setInitialTracking' first");
       }
+    }
+    // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
+    if (!isGoalReached()) {
+      LOG_DEBUG("USING DWA SAMPLING TO GO TO LAST KNOWN LOCATION");
+      // The tracking sample has collisions -> use DWA-like sampling and control
+      return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
-      throw std::runtime_error(
-          "Tracker is not initialized with an initial tracking target. Call "
-          "'VisionDWA::setInitialTracking' first");
+      LOG_WARNING("Target is Lost");
+      return TrajSearchResult();
     }
   };
 
@@ -298,7 +307,6 @@ class VisionDWA : public DWA {
    * @return std::tuple<Trajectory2D, bool>
    */
   Trajectory2D getTrackingReferenceSegment(const TrackedPose2D &tracking_pose);
-  //
 };
 
 } // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 50e0e3a3..376b9320 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -33,6 +33,8 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
           octreeRes, costWeights, maxNumThreads) {
   ctrl_limits_ = ctrlLimits;
   config_ = config;
+  // Set the reaching goal distance (used in the DWA mode when vision target is lost)
+  goal_dist_tolerance = config_.e_pose();
   // Initialize the bounding box tracker
   tracker_ = std::make_unique<FeatureBasedBboxTracker>(
       config.control_time_step(), config.e_pose(), config.e_vel(),

From 8c92cdfcfb1bbcdf3b58da8aed0a23dbd60ffe21 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 19 May 2025 20:16:24 +0800
Subject: [PATCH 057/118] (fix) Adds orientation to initial tracking data in
 vision dwa

---
 src/kompass_core/control/vision_dwa.py        |  2 +-
 src/kompass_cpp/bindings/bindings_control.cpp |  8 +++----
 .../include/controllers/vision_dwa.h          |  9 ++------
 .../include/utils/collision_check.h           |  2 ++
 .../include/utils/trajectory_sampler.h        |  2 ++
 .../kompass_cpp/include/vision/tracker.h      |  4 ++--
 .../src/controllers/vision_dwa.cpp            | 22 +++++++++++++------
 .../kompass_cpp/src/utils/collision_check.cpp |  4 ++++
 .../src/utils/trajectory_sampler.cpp          |  4 ++++
 .../kompass_cpp/src/vision/depth_detector.cpp |  6 +----
 .../kompass_cpp/src/vision/tracker.cpp        | 16 +++++---------
 11 files changed, 43 insertions(+), 36 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 98f96411..284e20ab 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -224,7 +224,7 @@ def set_initial_tracking_depth(
             current_state.x, current_state.y, current_state.yaw, current_state.speed)
             if any(detected_boxes):
                 return self._planner.set_initial_tracking(
-                    pose_x_img, pose_y_img, aligned_depth_image, detected_boxes
+                    pose_x_img, pose_y_img, aligned_depth_image, detected_boxes, current_state.yaw
                 )
             logging.error(f"Could not set initial tracking state: No detections are provided")
             return False
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 6fd38195..c3c40152 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -263,17 +263,17 @@ void bindings_control(py::module_ &m) {
            py::arg("principal_point_x"), py::arg("principal_point_y"))
       .def("set_initial_tracking",
            py::overload_cast<const int, const int,
-                             const std::vector<Bbox3D> &>(
+                             const std::vector<Bbox3D> &, const float>(
                &Control::VisionDWA::setInitialTracking),
            py::arg("pixel_x"), py::arg("pixel_y"),
-           py::arg("detected_boxes_3d"))
+           py::arg("detected_boxes_3d"), py::arg("robot_orientation") = 0.0)
       .def("set_initial_tracking",
            py::overload_cast<const int, const int,
                              const Eigen::MatrixX<unsigned short> &,
-                             const std::vector<Bbox2D> &>(
+                             const std::vector<Bbox2D> &, const float>(
                &Control::VisionDWA::setInitialTracking),
            py::arg("pixel_x"), py::arg("pixel_y"),
-           py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"))
+           py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"), py::arg("robot_orientation") = 0.0)
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 265388d5..b106f465 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -161,8 +161,6 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracked_pose,
                                             const Velocity2D &current_vel,
                                             const T &sensor_points) {
-    LOG_DEBUG("Tracked pose: ", tracked_pose.x(), tracked_pose.y(),
-              tracked_pose.yaw());
     Trajectory2D ref_traj = getTrackingReferenceSegment(tracked_pose);
     TrajSearchResult result;
     result.isTrajFound = true;
@@ -212,7 +210,6 @@ class VisionDWA : public DWA {
     }
     // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
     if (!isGoalReached()) {
-      LOG_DEBUG("USING DWA SAMPLING TO GO TO LAST KNOWN LOCATION");
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
@@ -242,7 +239,6 @@ class VisionDWA : public DWA {
       detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
       auto boxes_3d = detector_->get3dDetections();
       if (boxes_3d) {
-        LOG_DEBUG("Got 3D boxes from 2d");
         // Update the tracker with the detected boxes
         tracker_->updateTracking(boxes_3d.value());
         auto tracked_pose = tracker_->getFilteredTrackedPose2D();
@@ -256,7 +252,6 @@ class VisionDWA : public DWA {
     }
     // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
     if (!isGoalReached()) {
-      LOG_DEBUG("USING DWA SAMPLING TO GO TO LAST KNOWN LOCATION");
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
@@ -275,7 +270,7 @@ class VisionDWA : public DWA {
    * @return false
    */
   bool setInitialTracking(const int pose_x_img, const int pose_y_img,
-                          const std::vector<Bbox3D> &detected_boxes);
+                          const std::vector<Bbox3D> &detected_boxes, const float yaw = 0.0);
 
   /**
    * @brief  Set the initial image position of the target to be tracked using 2D
@@ -290,7 +285,7 @@ class VisionDWA : public DWA {
   bool
   setInitialTracking(const int pose_x_img, const int pose_y_img,
                      const Eigen::MatrixX<unsigned short> &aligned_depth_image,
-                     const std::vector<Bbox2D> &detected_boxes_2d);
+                     const std::vector<Bbox2D> &detected_boxes_2d, const float yaw = 0.0);
 
 private:
   ControlLimitsParams ctrl_limits_;
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
index 84f86b54..bc52d8db 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
@@ -219,6 +219,8 @@ class CollisionChecker {
    */
   bool checkCollisions(const Path::State current_state);
 
+  float getRadius() const;
+
 protected:
   double robotHeight_{1.0}, robotRadius_;
   // sensor tf with respect to the world
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 0055a706..1fcb39bc 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -127,6 +127,8 @@ class TrajectorySampler {
    */
   void resetOctreeResolution(const double resolution);
 
+  float getRobotRadius() const;
+
   Trajectory2D
   generateSingleSampleFromVel(const Velocity2D &vel,
                               const Path::State &pose = Path::State());
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index 0df236ca..886e4e71 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -21,10 +21,10 @@ class FeatureBasedBboxTracker{
 
       bool setInitialTracking(const TrackedBbox3D& bBox);
 
-      bool setInitialTracking(const Bbox3D &bBox);
+      bool setInitialTracking(const Bbox3D &bBox, const float yaw = 0.0);
 
       bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
-                              const std::vector<Bbox3D> &detected_boxes);
+                              const std::vector<Bbox3D> &detected_boxes, const float yaw = 0.0);
 
       bool trackerInitialized() const;
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 376b9320..8bfe9e9d 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -55,9 +55,9 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0);
-  float gamma =
-      Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) - trajSampler->getRobotRadius();
+  distance = std::max(distance, 0.0f);
+  float gamma = Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
   float psi = Angle::normalizeToMinusPiPlusPi(
       std::atan2(tracking_pose.y() - currentState.y,
                  tracking_pose.x() - currentState.x) -
@@ -67,6 +67,8 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   float angle_error =
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
+  LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
+
   float distance_tolerance = config_.tolerance() * config_.target_distance();
   float angle_tolerance =
       std::max(0.001, config_.tolerance() * config_.target_orientation());
@@ -82,10 +84,15 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
+      // TODO terms to be removed
+      auto term2 = 2.0 * (v * sin(psi) / distance);
+      auto term3 = 2.0 * tracking_pose.v() * sin(gamma - psi) / distance;
+      auto term4 = - 2.0 * config_.K_omega() * tanh(angle_error);
       omega = -tracking_pose.omega() +
               2.0 * (v * sin(psi) / distance +
                      tracking_pose.v() * sin(gamma - psi) / distance -
                      config_.K_omega() * tanh(angle_error));
+      LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(), ", term2=", term2, ", term3=", term3, ", term4=", term4);
     } else {
       omega =
           -tracking_pose.omega() - 2.0 * config_.K_omega() * tanh(angle_error);
@@ -93,19 +100,20 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
     followingVel.setOmega(omega);
+    LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
   }
   return followingVel;
 }
 
 bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
-                                   const std::vector<Bbox3D> &detected_boxes) {
-  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes);
+                                   const std::vector<Bbox3D> &detected_boxes, const float yaw) {
+  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes, yaw);
 }
 
 bool VisionDWA::setInitialTracking(
     const int pose_x_img, const int pose_y_img,
     const Eigen::MatrixX<unsigned short> &aligned_depth_image,
-    const std::vector<Bbox2D> &detected_boxes) {
+    const std::vector<Bbox2D> &detected_boxes, const float yaw) {
   if (!detector_) {
     throw std::runtime_error(
         "DepthDetector is not initialized with the camera intrinsics. Call "
@@ -134,7 +142,7 @@ bool VisionDWA::setInitialTracking(
     LOG_DEBUG("Failed to get 3D box from 2D target box");
     return false;
   }
-  return tracker_->setInitialTracking(boxes_3d.value()[0]);
+  return tracker_->setInitialTracking(boxes_3d.value()[0], yaw);
 }
 
 Trajectory2D
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
index 53702707..8ccccfc2 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
@@ -74,6 +74,10 @@ void CollisionChecker::resetOctreeResolution(const double resolution) {
   }
 }
 
+float CollisionChecker::getRadius() const{
+  return robotRadius_;
+}
+
 void CollisionChecker::generateBoxesFromOctomap(
     std::vector<fcl::CollisionObjectf *> &boxes, fcl::OcTreef &tree) {
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index 1d44142d..b25bb830 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -98,6 +98,10 @@ void TrajectorySampler::setSampleDroppingMode(const bool drop_samples) {
   this->drop_samples_ = drop_samples;
 }
 
+float TrajectorySampler::getRobotRadius() const{
+  return collChecker->getRadius();
+}
+
 void TrajectorySampler::updateParams(TrajectorySamplerParameters config) {
   time_step_ = config.getParameter<double>("time_step");
   max_time_ = config.getParameter<double>("prediction_horizon");
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index bc322fc2..758a28e0 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -126,9 +126,6 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
   size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_;
   size_camera_frame(2) = maximum_d - minimum_d;
 
-  LOG_DEBUG("Box size in camera frame :", size_camera_frame.x(), ", ",
-            size_camera_frame.y(), ", ", size_camera_frame.z());
-
   Eigen::Isometry3f camera_in_world = body_in_world_tf_ * camera_in_body_tf_;
 
   // Register center in the world frame
@@ -140,8 +137,7 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
   // Transform size from camera frame to world frame
   Eigen::Matrix3f abs_rotation = camera_in_world.linear().cwiseAbs();
   box3d.size = abs_rotation * size_camera_frame;
-  LOG_DEBUG("3D box size :", box3d.size.x(), ", ", box3d.size.y(), ", ",
-            box3d.size.z());
+
   return box3d;
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 8d399dd0..6c80c993 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -60,18 +60,20 @@ bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   return true;
 }
 
-bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox) {
+bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox, const float yaw) {
   LOG_DEBUG("Setting initial tracked box");
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  trackedBox_->yaw_vec(0) = yaw;
   Eigen::Matrix<float, StateSize, 1> state_vec =
       Eigen::Matrix<float, StateSize, 1>::Zero();
   state_vec(0) = bBox.center.x();
   state_vec(1) = bBox.center.y();
+  state_vec(2) = yaw;
   stateKalmanFilter_->setInitialState(state_vec);
   return true;
 }
 
-bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const int& pose_y_img, const std::vector<Bbox3D>& detected_boxes){
+bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const int& pose_y_img, const std::vector<Bbox3D>& detected_boxes, const float yaw){
     std::unique_ptr<Bbox3D> target_box;
     // Find a detected box containing the point
     for(auto box: detected_boxes){
@@ -88,13 +90,7 @@ bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const in
       // given position was not found inside any detected box
       return false;
     }
-    trackedBox_ = std::make_unique<TrackedBbox3D>(*target_box);
-    Eigen::Vector<float, StateSize> state_vec =
-        Eigen::Vector<float, StateSize>::Zero();
-    state_vec(0) = target_box->center[0];
-    state_vec(1) = target_box->center[1];
-    stateKalmanFilter_->setInitialState(state_vec);
-    return true;
+    return setInitialTracking(*target_box, yaw);
 }
 
 bool FeatureBasedBboxTracker::trackerInitialized() const{
@@ -120,7 +116,6 @@ void FeatureBasedBboxTracker::updateTrackedBoxState(){
 }
 
 bool FeatureBasedBboxTracker::updateTracking(const std::vector<Bbox3D> &detected_boxes){
-
     // Predicted the new location of the tracked box
     auto predicted_tracked_box = trackedBox_->predictConstantAcc(timeStep_);
 
@@ -146,6 +141,7 @@ bool FeatureBasedBboxTracker::updateTracking(const std::vector<Bbox3D> &detected
       trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx], timeStep_);
       // Update state by applying kalman filter on the raw measurement
       updateTrackedBoxState();
+      LOG_DEBUG("Box Now updated to: ", trackedBox_->box.center.x(), ", ", trackedBox_->box.center.y(), ", yaw=", trackedBox_->yaw_vec(0), ", omega=", trackedBox_->yaw_vec(1), ", v=", trackedBox_->v());
       return true;
     }
     return false;

From 613e5e3d0c62f2dfde75d3d90d70425c6dcfdc45 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 20 May 2025 17:08:09 +0200
Subject: [PATCH 058/118] (feature) Adds detections timestamp to VisonDWA
 tracking

---
 src/kompass_core/control/vision_dwa.py        |  47 +++--
 .../include/controllers/vision_dwa.h          |  94 +++++-----
 .../kompass_cpp/include/datatypes/tracking.h  | 103 +++++------
 .../kompass_cpp/include/utils/kalman_filter.h |   7 +-
 .../kompass_cpp/include/vision/tracker.h      |   2 +-
 .../src/controllers/vision_dwa.cpp            |  37 ++--
 .../kompass_cpp/src/utils/kalman_filter.cpp   |  44 ++---
 .../kompass_cpp/src/vision/tracker.cpp        | 161 ++++++++++--------
 .../tests/vision_detector_test.cpp            |   4 +-
 src/kompass_cpp/tests/vision_dwa_test.cpp     |  63 ++++---
 .../tests/vision_tracking_test.cpp            |   8 +-
 11 files changed, 311 insertions(+), 259 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 284e20ab..3109603a 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -12,7 +12,7 @@
     LaserScan,
     TrajectoryVelocities2D,
     TrajectoryPath,
-    TrackedPose2D
+    TrackedPose2D,
 )
 from typing import Optional, List
 import numpy as np
@@ -26,7 +26,6 @@
 
 @define
 class VisionDWAConfig(DWAConfig):
-
     tolerance: float = field(
         default=0.01, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
     )
@@ -72,6 +71,10 @@ class VisionDWAConfig(DWAConfig):
 
     enable_search: bool = field(default=False)  # Enable or disable the search mechanism
 
+    track_velocity: bool = field(
+        default=True
+    )  # Enable or disable tracking the linear/angular velocity of the moving target
+
     error_pose: float = field(
         default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
     )  # Error in pose estimation (m)
@@ -171,13 +174,16 @@ def __init__(
             config=self._config.to_kompass_cpp(),
         )
         if camera_focal_length is not None and camera_principal_point is not None:
-            self._planner.set_camera_intrinsics(camera_focal_length[0], camera_focal_length[1], camera_principal_point[0], camera_principal_point[1])
+            self._planner.set_camera_intrinsics(
+                camera_focal_length[0],
+                camera_focal_length[1],
+                camera_principal_point[0],
+                camera_principal_point[1],
+            )
 
         # Init the following result
         self._result = SamplingControlResult()
-        self._end_of_ctrl_horizon: int = max(
-            self._config.control_horizon, 1
-        )
+        self._end_of_ctrl_horizon: int = max(self._config.control_horizon, 1)
         logging.info("VisionDWA CONTROLLER IS READY")
 
     def set_camera_intrinsics(self, fx: float, fy: float, cx: float, cy: float) -> None:
@@ -197,9 +203,13 @@ def set_initial_tracking_3d(
         """
         try:
             if any(detected_boxes):
-                return self._planner.set_initial_tracking(pose_x_img, pose_y_img, detected_boxes)
+                return self._planner.set_initial_tracking(
+                    pose_x_img, pose_y_img, detected_boxes
+                )
 
-            logging.error(f"Could not set initial tracking state: No detections are provided")
+            logging.error(
+                "Could not set initial tracking state: No detections are provided"
+            )
             return False
         except Exception as e:
             logging.error(f"Could not set initial tracking state: {e}")
@@ -221,12 +231,19 @@ def set_initial_tracking_depth(
         """
         try:
             self._planner.set_current_state(
-            current_state.x, current_state.y, current_state.yaw, current_state.speed)
+                current_state.x, current_state.y, current_state.yaw, current_state.speed
+            )
             if any(detected_boxes):
                 return self._planner.set_initial_tracking(
-                    pose_x_img, pose_y_img, aligned_depth_image, detected_boxes, current_state.yaw
+                    pose_x_img,
+                    pose_y_img,
+                    aligned_depth_image,
+                    detected_boxes,
+                    current_state.yaw,
                 )
-            logging.error(f"Could not set initial tracking state: No detections are provided")
+            logging.error(
+                "Could not set initial tracking state: No detections are provided"
+            )
             return False
 
         except Exception as e:
@@ -272,9 +289,7 @@ def loop_step(
         if local_map is not None:
             sensor_data = PointCloudData.numpy_to_kompass_cpp(local_map)
         elif laser_scan:
-            sensor_data = LaserScan(
-                ranges=laser_scan.ranges, angles=laser_scan.angles
-            )
+            sensor_data = LaserScan(ranges=laser_scan.ranges, angles=laser_scan.angles)
         elif point_cloud is not None:
             sensor_data = point_cloud
         else:
@@ -311,7 +326,9 @@ def has_result(self) -> None:
     def logging_info(self) -> str:
         """logging_info."""
         if self._result.is_found:
-            return f"VisionDWA Controller found trajectory with cost: {self._result.cost}"
+            return (
+                f"VisionDWA Controller found trajectory with cost: {self._result.cost}"
+            )
         else:
             return "VisionDWA Controller Failed to find a valid trajectory"
 
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index b106f465..aa5ef875 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -13,6 +13,7 @@
 #include <boost/filesystem/path.hpp>
 #include <cmath>
 #include <memory>
+#include <optional>
 #include <vector>
 
 namespace Kompass {
@@ -45,13 +46,17 @@ class VisionDWA : public DWA {
           "target_orientation",
           Parameter(0.0, -M_PI, M_PI,
                     "Bearing angle to maintain with the target (rad)"));
+      addParameter(
+          "track_velocity",
+          Parameter(true,
+                    "Track the linear and angular velocity of the target"));
       // Search Parameters
       addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
       addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
       addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
       addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
       // Pure tracking control law parameters
-      addParameter("rotation_gain", Parameter(0.5, 1e-2, 10.0));
+      addParameter("rotation_gain", Parameter(1.0, 1e-2, 10.0));
       addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
       addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
       addParameter("enable_search", Parameter(false));
@@ -72,6 +77,7 @@ class VisionDWA : public DWA {
           Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value"));
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
+    bool enable_vel_tracking() const { return getParameter<bool>("track_velocity"); }
     double control_time_step() const {
       return getParameter<double>("control_time_step");
     }
@@ -158,23 +164,32 @@ class VisionDWA : public DWA {
    * @return Control::TrajSearchResult
    */
   template <typename T>
-  Control::TrajSearchResult getTrackingCtrl(const TrackedPose2D &tracked_pose,
-                                            const Velocity2D &current_vel,
-                                            const T &sensor_points) {
-    Trajectory2D ref_traj = getTrackingReferenceSegment(tracked_pose);
-    TrajSearchResult result;
-    result.isTrajFound = true;
-    result.trajCost = 0.0;
-    result.trajectory = ref_traj;
-    latest_velocity_command_ = ref_traj.velocities.getFront();
-    // ---------------------------------------------------------------
-    // Update reference to use in case goal is lost
-    auto referenceToTarget =
-        Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z,
-                   config_.prediction_horizon());
-    this->setCurrentPath(referenceToTarget, false);
-    // ---------------------------------------------------------------
-    return result;
+  Control::TrajSearchResult
+  getTrackingCtrl(const std::optional<TrackedPose2D> &tracked_pose,
+                  const Velocity2D &current_vel, const T &sensor_points) {
+    if (tracked_pose.has_value()) {
+      Trajectory2D ref_traj = getTrackingReferenceSegment(tracked_pose.value());
+      TrajSearchResult result;
+      result.isTrajFound = true;
+      result.trajCost = 0.0;
+      result.trajectory = ref_traj;
+      latest_velocity_command_ = ref_traj.velocities.getFront();
+      // ---------------------------------------------------------------
+      // Update reference to use in case goal is lost
+      auto referenceToTarget =
+          Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z,
+                     config_.prediction_horizon());
+      this->setCurrentPath(referenceToTarget, false);
+      // ---------------------------------------------------------------
+      return result;
+    }
+    if (!isGoalReached()) {
+      // The tracking sample has collisions -> use DWA-like sampling and control
+      return this->computeVelocityCommandsSet(current_vel, sensor_points);
+    } else {
+      LOG_WARNING("No tracking target is received!");
+      return TrajSearchResult();
+    }
   };
 
   /**
@@ -191,31 +206,19 @@ class VisionDWA : public DWA {
   Control::TrajSearchResult
   getTrackingCtrl(const std::vector<Bbox3D> &detected_boxes,
                   const Velocity2D &current_vel, const T &sensor_points) {
-    if(!detected_boxes.empty()){
+    std::optional<TrackedPose2D> tracked_pose = std::nullopt;
+    if (!detected_boxes.empty()) {
       if (tracker_->trackerInitialized()) {
         // Update the tracker with the detected boxes
         tracker_->updateTracking(detected_boxes);
-        auto tracked_pose = tracker_->getFilteredTrackedPose2D();
-        if (tracked_pose) {
-          return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
-                                          sensor_points);
-        } else {
-          LOG_WARNING("Tracker failed to find the target");
-        }
+        tracked_pose = tracker_->getFilteredTrackedPose2D();
       } else {
         throw std::runtime_error(
             "Tracker is not initialized with an initial tracking target. Call "
             "'VisionDWA::setInitialTracking' first");
       }
     }
-    // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
-    if (!isGoalReached()) {
-      // The tracking sample has collisions -> use DWA-like sampling and control
-      return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    } else {
-      LOG_WARNING("Target is Lost");
-      return TrajSearchResult();
-    }
+    return this->getTrackingCtrl<T>(tracked_pose, current_vel, sensor_points);
   };
 
   template <typename T>
@@ -233,6 +236,7 @@ class VisionDWA : public DWA {
           "Tracker is not initialized with an initial tracking target. Call "
           "'VisionDWA::setInitialTracking' first");
     }
+    std::optional<TrackedPose2D> tracked_pose = std::nullopt;
     if (!detected_boxes_2d.empty()) {
       // Send current state to the detector
       detector_->updateState(currentState);
@@ -241,23 +245,12 @@ class VisionDWA : public DWA {
       if (boxes_3d) {
         // Update the tracker with the detected boxes
         tracker_->updateTracking(boxes_3d.value());
-        auto tracked_pose = tracker_->getFilteredTrackedPose2D();
-        if (tracked_pose) {
-          return this->getTrackingCtrl<T>(tracked_pose.value(), current_vel,
-                                          sensor_points);
-        }
+        tracked_pose = tracker_->getFilteredTrackedPose2D();
       } else {
         LOG_WARNING("Detector failed to find 3D boxes");
       }
     }
-    // IF TARGET IS LOST -> USE DWA TO LAST KNOWN LOCATION
-    if (!isGoalReached()) {
-      // The tracking sample has collisions -> use DWA-like sampling and control
-      return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    } else {
-      LOG_WARNING("Target is Lost");
-      return TrajSearchResult();
-    }
+    return this->getTrackingCtrl<T>(tracked_pose, current_vel, sensor_points);
   };
 
   /**
@@ -270,7 +263,8 @@ class VisionDWA : public DWA {
    * @return false
    */
   bool setInitialTracking(const int pose_x_img, const int pose_y_img,
-                          const std::vector<Bbox3D> &detected_boxes, const float yaw = 0.0);
+                          const std::vector<Bbox3D> &detected_boxes,
+                          const float yaw = 0.0);
 
   /**
    * @brief  Set the initial image position of the target to be tracked using 2D
@@ -285,7 +279,8 @@ class VisionDWA : public DWA {
   bool
   setInitialTracking(const int pose_x_img, const int pose_y_img,
                      const Eigen::MatrixX<unsigned short> &aligned_depth_image,
-                     const std::vector<Bbox2D> &detected_boxes_2d, const float yaw = 0.0);
+                     const std::vector<Bbox2D> &detected_boxes_2d,
+                     const float yaw = 0.0);
 
 private:
   ControlLimitsParams ctrl_limits_;
@@ -293,6 +288,7 @@ class VisionDWA : public DWA {
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
   std::unique_ptr<DepthDetector> detector_;
   Eigen::Isometry3f vision_sensor_tf_;
+  int track_velocity_;
 
   /**
    * @brief Get the Tracking Reference Trajectory Segment
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 9cb944db..b69ca1d1 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -1,18 +1,19 @@
 #pragma once
 
+#include "datatypes/control.h"
+#include "utils/logger.h"
 #include <Eigen/Dense>
 #include <cmath>
 #include <vector>
-#include "datatypes/control.h"
 
 namespace Kompass {
 
-
 struct Bbox2D {
   Eigen::Vector2i top_corner = {0, 0};
   Eigen::Vector2i size = {0, 0};
+  float timestamp = 0.0; // Timestamp of the detection in seconds
 
-  Bbox2D() {};
+  Bbox2D(){};
 
   Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size){};
 
@@ -34,36 +35,42 @@ struct Bbox3D {
   Eigen::Vector2i center_img_frame = {0, 0};
   Eigen::Vector2i size_img_frame = {0, 0};
   std::vector<Eigen::Vector3f> pc_points = {};
+  float timestamp = 0.0; // Timestamp of the detection in seconds
 
-  Bbox3D() {};
+  Bbox3D(){};
 
   Bbox3D(const Bbox3D &box)
-      : center(box.center), size(box.size), center_img_frame(box.center_img_frame), size_img_frame(box.size_img_frame), pc_points(box.pc_points){};
+      : center(box.center), size(box.size),
+        center_img_frame(box.center_img_frame),
+        size_img_frame(box.size_img_frame), pc_points(box.pc_points),
+        timestamp(box.timestamp){};
 
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Eigen::Vector2i center_img_frame,
-         const Eigen::Vector2i size_img_frame,
+         const Eigen::Vector2i size_img_frame, const float timestamp = 0.0,
          std::vector<Eigen::Vector3f> pc_points = {})
-      : center(center), size(size),
-        center_img_frame(center_img_frame),
-        size_img_frame(size_img_frame), pc_points(pc_points){};
+      : center(center), size(size), center_img_frame(center_img_frame),
+        size_img_frame(size_img_frame), pc_points(pc_points),
+        timestamp(timestamp){};
 
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Bbox2D &box2d, std::vector<Eigen::Vector3f> pc_points = {})
-      : center(center), size(size), center_img_frame(box2d.top_corner + Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
-        size_img_frame(box2d.size), pc_points(pc_points){};
+      : center(center), size(size),
+        center_img_frame(
+            box2d.top_corner +
+            Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
+        size_img_frame(box2d.size), pc_points(pc_points),
+        timestamp(box2d.timestamp){};
 
   Bbox3D(const Bbox2D &box2d)
       : center_img_frame(
             box2d.top_corner +
             Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
-        size_img_frame(box2d.size) {};
+        size_img_frame(box2d.size), timestamp(box2d.timestamp){};
 
   Eigen::Vector2f getXLimitsImg() const {
-    return {
-      center_img_frame.x() - (size_img_frame.x() / 2),
-          center_img_frame.x() + (size_img_frame.x() / 2)
-    };
+    return {center_img_frame.x() - (size_img_frame.x() / 2),
+            center_img_frame.x() + (size_img_frame.x() / 2)};
   };
 
   Eigen::Vector2f getYLimitsImg() const {
@@ -77,59 +84,52 @@ struct TrackedBbox3D {
   Eigen::Vector3f vel = {0.0, 0.0, 0.0};
   Eigen::Vector3f acc = {0.0, 0.0, 0.0};
   int unique_id = 0;
-  Eigen::Vector3f yaw_vec = {0.0, 0.0, 0.0}; // To track yaw, omega and angular acc
+  Eigen::Vector3f yaw_vec = {0.0, 0.0,
+                             0.0}; // To track yaw, omega and angular acc
 
-  TrackedBbox3D(const Bbox3D& box): box(box) {};
+  TrackedBbox3D(const Bbox3D &box) : box(box){};
 
-  void setState(const Eigen::Matrix<float, 6, 1>& state_vector){
+  void setState(const Eigen::Matrix<float, 9, 1> &state_vector) {
     this->box.center = {state_vector(0), state_vector(1), 0.0f};
-    this->vel = {state_vector(2), state_vector(3), 0.0f};
-    this->acc = {state_vector(4), state_vector(5), 0.0f};
-  };
-
-  void setSize(const Eigen::Vector3f& size) {
-    this->box.size = size;
+    this->yaw_vec = {state_vector(2), state_vector(5), state_vector(8)};
+    this->vel = {state_vector(3), state_vector(4), 0.0f};
+    this->acc = {state_vector(6), state_vector(7), 0.0f};
   };
 
-  void setfromBox(const Bbox3D& box){
-    this->box = box;
-  };
+  void setSize(const Eigen::Vector3f &size) { this->box.size = size; };
 
-  void updateFromNewState(const Eigen::Matrix<float, 6, 1> &state_vector,
-                          const float time_step) {
-    this->box.center = {state_vector(0), state_vector(1), 0.0f};
-    this->vel = {state_vector(2), state_vector(3), 0.0f};
-    this->acc = {state_vector(4), state_vector(5), 0.0f};
-    auto new_yaw = std::atan2(this->vel(1), this->vel(0));
-    // Orientation difference
-    auto new_omega = (this->yaw_vec(0) - new_yaw) / time_step;
-    this->yaw_vec(2) = (this->yaw_vec(1) - new_omega) / time_step;
-    this->yaw_vec(1) = new_omega;
-    this->yaw_vec(0) = new_yaw;
-  };
+  void setfromBox(const Bbox3D &box) { this->box = box; };
 
-  void updateFromNewDetection(const Bbox3D& new_box, const float time_step){
+  void updateFromNewDetection(const Bbox3D &new_box) {
+    float time_step = new_box.timestamp - this->box.timestamp;
+    if (time_step <= 0.0) {
+      LOG_ERROR("Cannot update from new detection, invalid time step = ",
+                time_step);
+      return; // Invalid time step
+    }
     // Compute velocity and acceleration based on location change
     Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
-    Eigen::Vector3f new_acc = (new_vel - this->vel) / time_step;
+    this->acc = (new_vel - this->vel) / time_step;
+    this->vel = new_vel;
     // Update
     setfromBox(new_box);
     // New orientation (yaw) based on new velocity
     auto new_yaw = std::atan2(new_vel(1), new_vel(0));
     // Orientation difference
-    auto new_omega = (this->yaw_vec(0) - new_yaw) / time_step;
-    this->yaw_vec(2) = (this->yaw_vec(1) - new_omega) / time_step;
+    auto new_omega = (new_yaw - this->yaw_vec(0)) / time_step;
+    this->yaw_vec(2) = (new_omega - this->yaw_vec(1)) / time_step;
     this->yaw_vec(1) = new_omega;
     this->yaw_vec(0) = new_yaw;
-    this->vel = new_vel;
-    this->acc = new_acc;
   }
 
   TrackedBbox3D predictConstantVel(const float &dt) {
     auto predicted_tracking = *this;
     predicted_tracking.box.center += predicted_tracking.vel * dt;
+    predicted_tracking.yaw_vec(0) += predicted_tracking.yaw_vec(1) * dt;
+    predicted_tracking.yaw_vec(2) = 0.0; // Set angular acceleration to zero
     // Set acceleration to zero for constant velocity prediction
     predicted_tracking.acc = {0.0, 0.0, 0.0};
+    predicted_tracking.box.timestamp += dt;
     return predicted_tracking;
   };
 
@@ -137,6 +137,9 @@ struct TrackedBbox3D {
     auto predicted_tracking = *this;
     predicted_tracking.vel += this->acc * dt;
     predicted_tracking.box.center += predicted_tracking.vel * dt;
+    predicted_tracking.yaw_vec(1) += predicted_tracking.yaw_vec(2) * dt;
+    predicted_tracking.yaw_vec(0) += predicted_tracking.yaw_vec(1) * dt;
+    predicted_tracking.box.timestamp += dt;
     return predicted_tracking;
   };
 
@@ -150,6 +153,8 @@ struct TrackedBbox3D {
 
   float omega() const { return yaw_vec(1); };
 
+  float timestamp() const { return box.timestamp; };
+
   void update(const float timeStep) {
     box.center(0) += vel.x() * timeStep;
     box.center(1) += vel.y() * timeStep;
@@ -162,9 +167,9 @@ struct TrackedBbox3D {
   }
 
   Control::TrackedPose2D getTrackedPose() const {
-    return Control::TrackedPose2D(box.center.x(), box.center.y(),
-           yaw(), vel.x(), vel.y(), omega());
+    return Control::TrackedPose2D(box.center.x(), box.center.y(), yaw(),
+                                  vel.x(), vel.y(), omega());
   }
 };
 
-}
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
index 646e2f2d..ec4ed654 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/kalman_filter.h
@@ -34,9 +34,10 @@ class LinearSSKalmanFilter {
   void setA(const Eigen::MatrixXf &A);
 
   // state estimate
-  void estimate(const Eigen::MatrixXf &z, const Eigen::MatrixXf &u);
+  void estimate(const Eigen::MatrixXf &z, const Eigen::MatrixXf &u,
+                const int numberSteps = 1);
 
-  void estimate(const Eigen::MatrixXf &z);
+  void estimate(const Eigen::MatrixXf &z, const int numberSteps = 1);
 
   // read output from the state
   double getState(const size_t state_index);
@@ -44,4 +45,4 @@ class LinearSSKalmanFilter {
   std::optional<Eigen::MatrixXf> getState();
 };
 
-}
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index 886e4e71..12f6b2a6 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -47,7 +47,7 @@ class FeatureBasedBboxTracker{
 
       Eigen::Vector3f computePointsStdDev(const std::vector<Eigen::Vector3f> &pc_points) const;
 
-      void updateTrackedBoxState();
+      void updateTrackedBoxState(const int numberSteps = 1);
 };
 
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 8bfe9e9d..0e995b19 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -33,7 +33,9 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
           octreeRes, costWeights, maxNumThreads) {
   ctrl_limits_ = ctrlLimits;
   config_ = config;
-  // Set the reaching goal distance (used in the DWA mode when vision target is lost)
+  // Set the reaching goal distance (used in the DWA mode when vision target is
+  // lost)
+  track_velocity_ = config_.enable_vel_tracking();
   goal_dist_tolerance = config_.e_pose();
   // Initialize the bounding box tracker
   tracker_ = std::make_unique<FeatureBasedBboxTracker>(
@@ -55,9 +57,9 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) - trajSampler->getRobotRadius();
+  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) -
+                   trajSampler->getRobotRadius();
   distance = std::max(distance, 0.0f);
-  float gamma = Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
   float psi = Angle::normalizeToMinusPiPlusPi(
       std::atan2(tracking_pose.y() - currentState.y,
                  tracking_pose.x() - currentState.x) -
@@ -67,7 +69,7 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   float angle_error =
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
-  LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
+  // LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
 
   float distance_tolerance = config_.tolerance() * config_.target_distance();
   float angle_tolerance =
@@ -76,7 +78,7 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
-    double v = ((tracking_pose.v() * cos(gamma - psi)) -
+    double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
                 (config_.K_v() * tanh(distance_error))) /
                cos(psi);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
@@ -85,29 +87,30 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     double omega;
     if (distance > 0.0) {
       // TODO terms to be removed
-      auto term2 = 2.0 * (v * sin(psi) / distance);
-      auto term3 = 2.0 * tracking_pose.v() * sin(gamma - psi) / distance;
-      auto term4 = - 2.0 * config_.K_omega() * tanh(angle_error);
-      omega = -tracking_pose.omega() +
-              2.0 * (v * sin(psi) / distance +
-                     tracking_pose.v() * sin(gamma - psi) / distance -
+      // auto term2 = 2.0 * sin(psi) / distance * (v - tracking_pose.v());
+      // auto term3 = -2.0 * config_.K_omega() * tanh(angle_error);
+      omega = -track_velocity_ * tracking_pose.omega() +
+              2.0 * (sin(psi) / distance * (v - tracking_pose.v()) -
                      config_.K_omega() * tanh(angle_error));
-      LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(), ", term2=", term2, ", term3=", term3, ", term4=", term4);
+      // LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(),
+      //           ", term_psi=", term2, ", term_ang_err=", term3);
     } else {
-      omega =
-          -tracking_pose.omega() - 2.0 * config_.K_omega() * tanh(angle_error);
+      omega = -track_velocity_ * tracking_pose.omega() -
+              2.0 * config_.K_omega() * tanh(angle_error);
     }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
     followingVel.setOmega(omega);
-    LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
+    // LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
   }
   return followingVel;
 }
 
 bool VisionDWA::setInitialTracking(const int pose_x_img, const int pose_y_img,
-                                   const std::vector<Bbox3D> &detected_boxes, const float yaw) {
-  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes, yaw);
+                                   const std::vector<Bbox3D> &detected_boxes,
+                                   const float yaw) {
+  return tracker_->setInitialTracking(pose_x_img, pose_y_img, detected_boxes,
+                                      yaw);
 }
 
 bool VisionDWA::setInitialTracking(
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
index 137106eb..1a8bd67e 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/kalman_filter.cpp
@@ -1,8 +1,8 @@
-#define EIGEN_DONT_PARALLELIZE  // TODO: resolve error when enabling Eigen parallelization
+#define EIGEN_DONT_PARALLELIZE // TODO: resolve error when enabling Eigen
+                               // parallelization
 #include "utils/kalman_filter.h"
 #include "utils/logger.h"
 
-
 namespace Kompass {
 
 LinearSSKalmanFilter::LinearSSKalmanFilter(const size_t num_states,
@@ -44,11 +44,13 @@ bool LinearSSKalmanFilter::setup(const Eigen::MatrixXf &A,
   return true;
 }
 
-void LinearSSKalmanFilter::setInitialState(const Eigen::VectorXf& initial_state) {
-  if (initial_state.size() != this->state.size()){
-     LOG_ERROR("Cannot set initial state. Expected the "
-              "following sized: ", this->state.size());
-     throw std::length_error("Error Setting Initial State");
+void LinearSSKalmanFilter::setInitialState(
+    const Eigen::VectorXf &initial_state) {
+  if (initial_state.size() != this->state.size()) {
+    LOG_ERROR("Cannot set initial state. Expected the "
+              "following sized: ",
+              this->state.size());
+    throw std::length_error("Error Setting Initial State");
   }
   this->state = initial_state;
   this->state_initialized = true;
@@ -57,20 +59,23 @@ void LinearSSKalmanFilter::setInitialState(const Eigen::VectorXf& initial_state)
 void LinearSSKalmanFilter::setA(const Eigen::MatrixXf &A) { this->A = A; }
 
 void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements,
-                                    const Eigen::MatrixXf &inputs) {
-  // predict a new state
-  Eigen::MatrixXf predicted_state = this->A * this->state + this->B * inputs;
-
-  // Covariance Extrapolation Equation
-  this->P = this->A * this->P * this->A.transpose() + this->Q;
+                                    const Eigen::MatrixXf &inputs,
+                                    const int numberSteps) {
+  Eigen::MatrixXf A_transpose = this->A.transpose();
+  Eigen::MatrixXf B_inputs = this->B * inputs;
+  Eigen::MatrixXf predicted_state = this->state;
+  // predict a new state after number of steps
+  for (int i = 0; i < numberSteps; i++) {
+    predicted_state = this->A * predicted_state + B_inputs;
+    // Covariance Extrapolation Equation
+    this->P = this->A * this->P * A_transpose + this->Q;
+  }
 
   // Innovation Matrix
-  Eigen::MatrixXf S =
-      this->R + this->H * this->P * this->H.transpose();
+  Eigen::MatrixXf S = this->R + this->H * this->P * this->H.transpose();
 
   // Kalman Gain Matrix
-  Eigen::MatrixXf K =
-      this->P * this->H.transpose() * S.inverse();
+  Eigen::MatrixXf K = this->P * this->H.transpose() * S.inverse();
 
   // Update the state
   this->state =
@@ -82,7 +87,8 @@ void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements,
             this->P;
 }
 
-void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements) {
+void LinearSSKalmanFilter::estimate(const Eigen::MatrixXf &measurements,
+                                    const int numberSteps) {
   // estimate with zero inputs
   auto size = this->B.cols();
   Eigen::MatrixXf inputs;
@@ -105,4 +111,4 @@ std::optional<Eigen::MatrixXf> LinearSSKalmanFilter::getState() {
   }
   return std::nullopt;
 }
-}
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 6c80c993..a845b48e 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -7,24 +7,21 @@
 
 namespace Kompass {
 
-FeatureBasedBboxTracker::FeatureBasedBboxTracker(
-    const float& time_step, const float& e_pos,
-    const float& e_vel, const float& e_acc) {
+FeatureBasedBboxTracker::FeatureBasedBboxTracker(const float &time_step,
+                                                 const float &e_pos,
+                                                 const float &e_vel,
+                                                 const float &e_acc) {
 
   timeStep_ = time_step;
   // Setup Kalman filter matrices
   Eigen::MatrixXf A;
   A.resize(StateSize, StateSize);
 
-  A << 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0,
-       0, 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0,
-       0, 0, 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2),
-       0, 0, 0, 1, 0, 0, time_step, 0, 0,
-       0, 0, 0, 0, 1, 0, 0, time_step, 0,
-       0, 0, 0, 0, 0, 1, 0, 0, time_step,
-       0, 0, 0, 0, 0, 0, 1, 0, 0,
-       0, 0, 0, 0, 0, 0, 0, 1, 0,
-       0, 0, 0, 0, 0, 0, 0, 0, 0;
+  A << 1, 0, 0, time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0,
+      time_step, 0, 0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0, time_step, 0,
+      0, 0.5 * pow(time_step, 2), 0, 0, 0, 1, 0, 0, time_step, 0, 0, 0, 0, 0, 0,
+      1, 0, 0, time_step, 0, 0, 0, 0, 0, 0, 1, 0, 0, time_step, 0, 0, 0, 0, 0,
+      0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
 
   Eigen::MatrixXf B = Eigen::MatrixXf::Zero(StateSize, 1);
   Eigen::MatrixXf H = Eigen::MatrixXf::Identity(StateSize, StateSize);
@@ -47,20 +44,21 @@ bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
   Eigen::VectorXf state_vec;
   state_vec.resize(StateSize);
-  state_vec(0) = bBox.box.center[0];  // x
-  state_vec(1) = bBox.box.center[1];  // y
-  state_vec(2) = bBox.yaw_vec(0);  // yaw
-  state_vec(3) = bBox.vel[0];   // vx
-  state_vec(4) = bBox.vel[1];  // vy
-  state_vec(5) = bBox.yaw_vec(1);  // omega
-  state_vec(6) = bBox.acc[0];   // ax
-  state_vec(7) = bBox.acc[1];   // ay
-  state_vec(8) = bBox.yaw_vec(2);   // a_yaw
+  state_vec(0) = bBox.box.center[0]; // x
+  state_vec(1) = bBox.box.center[1]; // y
+  state_vec(2) = bBox.yaw_vec(0);    // yaw
+  state_vec(3) = bBox.vel[0];        // vx
+  state_vec(4) = bBox.vel[1];        // vy
+  state_vec(5) = bBox.yaw_vec(1);    // omega
+  state_vec(6) = bBox.acc[0];        // ax
+  state_vec(7) = bBox.acc[1];        // ay
+  state_vec(8) = bBox.yaw_vec(2);    // a_yaw
   stateKalmanFilter_->setInitialState(state_vec);
   return true;
 }
 
-bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox, const float yaw) {
+bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox,
+                                                 const float yaw) {
   LOG_DEBUG("Setting initial tracked box");
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
   trackedBox_->yaw_vec(0) = yaw;
@@ -73,34 +71,36 @@ bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox, const float
   return true;
 }
 
-bool FeatureBasedBboxTracker::setInitialTracking(const int& pose_x_img, const int& pose_y_img, const std::vector<Bbox3D>& detected_boxes, const float yaw){
-    std::unique_ptr<Bbox3D> target_box;
-    // Find a detected box containing the point
-    for(auto box: detected_boxes){
-      auto limits_x = box.getXLimitsImg();
-      if(pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)){
-        auto limits_y = box.getYLimitsImg();
-        if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) {
-          target_box = std::make_unique<Bbox3D>(box);
-          break;
-        }
+bool FeatureBasedBboxTracker::setInitialTracking(
+    const int &pose_x_img, const int &pose_y_img,
+    const std::vector<Bbox3D> &detected_boxes, const float yaw) {
+  std::unique_ptr<Bbox3D> target_box;
+  // Find a detected box containing the point
+  for (auto box : detected_boxes) {
+    auto limits_x = box.getXLimitsImg();
+    if (pose_x_img >= limits_x(0) and pose_x_img <= limits_x(1)) {
+      auto limits_y = box.getYLimitsImg();
+      if (pose_y_img >= limits_y(0) and pose_y_img <= limits_y(1)) {
+        target_box = std::make_unique<Bbox3D>(box);
+        break;
       }
     }
-    if(!target_box){
-      // given position was not found inside any detected box
-      return false;
-    }
-    return setInitialTracking(*target_box, yaw);
+  }
+  if (!target_box) {
+    // given position was not found inside any detected box
+    return false;
+  }
+  return setInitialTracking(*target_box, yaw);
 }
 
-bool FeatureBasedBboxTracker::trackerInitialized() const{
-  if(trackedBox_){
+bool FeatureBasedBboxTracker::trackerInitialized() const {
+  if (trackedBox_) {
     return true;
   }
   return false;
 }
 
-void FeatureBasedBboxTracker::updateTrackedBoxState(){
+void FeatureBasedBboxTracker::updateTrackedBoxState(const int numberSteps) {
   Eigen::MatrixXf measurement;
   measurement.resize(StateSize, 1);
   measurement(0) = trackedBox_->box.center.x();
@@ -112,39 +112,51 @@ void FeatureBasedBboxTracker::updateTrackedBoxState(){
   measurement(6) = trackedBox_->acc.x();
   measurement(7) = trackedBox_->acc.y();
   measurement(8) = trackedBox_->yaw_vec(2);
-  stateKalmanFilter_->estimate(measurement);
+  stateKalmanFilter_->estimate(measurement, numberSteps);
 }
 
-bool FeatureBasedBboxTracker::updateTracking(const std::vector<Bbox3D> &detected_boxes){
-    // Predicted the new location of the tracked box
-    auto predicted_tracked_box = trackedBox_->predictConstantAcc(timeStep_);
+bool FeatureBasedBboxTracker::updateTracking(
+    const std::vector<Bbox3D> &detected_boxes) {
+  float new_timestamp = detected_boxes[0].timestamp;
+  float dt = new_timestamp - trackedBox_->box.timestamp;
 
-    auto ref_box_features = extractFeatures(predicted_tracked_box);
+  // Predicted the new location of the tracked box
+  auto predicted_tracked_box = trackedBox_->predictConstantAcc(dt);
 
-    // Get the features of all the new detections
-    FeaturesVector detected_boxes_feature_vec;
-    float max_similarity_score = 0.0;  // Similarity score
-    size_t similar_box_idx = 0, count = 0;
-    for(auto box: detected_boxes){
-      detected_boxes_feature_vec = extractFeatures(box);
-      auto error_vec = detected_boxes_feature_vec - ref_box_features;
-      float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
+  auto ref_box_features = extractFeatures(predicted_tracked_box);
 
-      if (similarity_score > max_similarity_score){
-        max_similarity_score = similarity_score;
-        similar_box_idx = count;
-      }
-      count++;
-    }
-    if (max_similarity_score > minAcceptedSimilarityScore_){
-      // Update raw tracking
-      trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx], timeStep_);
-      // Update state by applying kalman filter on the raw measurement
-      updateTrackedBoxState();
-      LOG_DEBUG("Box Now updated to: ", trackedBox_->box.center.x(), ", ", trackedBox_->box.center.y(), ", yaw=", trackedBox_->yaw_vec(0), ", omega=", trackedBox_->yaw_vec(1), ", v=", trackedBox_->v());
-      return true;
+  // Get the features of all the new detections
+  FeaturesVector detected_boxes_feature_vec;
+  float max_similarity_score = 0.0; // Similarity score
+  size_t similar_box_idx = 0, count = 0;
+  for (auto box : detected_boxes) {
+    detected_boxes_feature_vec = extractFeatures(box);
+    auto error_vec = detected_boxes_feature_vec - ref_box_features;
+    float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
+
+    if (similarity_score > max_similarity_score) {
+      max_similarity_score = similarity_score;
+      similar_box_idx = count;
     }
-    return false;
+    count++;
+  }
+  if (max_similarity_score > minAcceptedSimilarityScore_) {
+    // Update raw tracking
+    int number_steps = std::max(static_cast<int>(dt / timeStep_), 1);
+
+    trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx]);
+
+    // Update state by applying kalman filter on the raw measurement
+    updateTrackedBoxState(number_steps);
+    LOG_DEBUG("Update after ", number_steps,
+              " timer steps, Box Now updated to: ", trackedBox_->box.center.x(),
+              ", ", trackedBox_->box.center.y(),
+              ", yaw=", trackedBox_->yaw_vec(0),
+              ", omega=", trackedBox_->yaw_vec(1), ", v=", trackedBox_->v());
+    return true;
+  }
+  LOG_DEBUG("Box not found");
+  return false;
 }
 
 FeatureBasedBboxTracker::FeaturesVector
@@ -155,9 +167,10 @@ FeatureBasedBboxTracker::extractFeatures(
 
 FeatureBasedBboxTracker::FeaturesVector
 FeatureBasedBboxTracker::extractFeatures(const Bbox3D &bBox) const {
-  FeatureBasedBboxTracker::FeaturesVector features_vec = Eigen::Vector<float, 9>::Zero();
+  FeatureBasedBboxTracker::FeaturesVector features_vec =
+      Eigen::Vector<float, 9>::Zero();
   features_vec.segment<2>(0) = bBox.center.segment<2>(0); // indices 0, 1
-  features_vec.segment<3>(2) = bBox.size; // indices 2, 3, 4
+  features_vec.segment<3>(2) = bBox.size;                 // indices 2, 3, 4
   features_vec(5) = bBox.pc_points.size();
   if (features_vec(5) > 0.0) {
     // Compute point cloud points standard deviation
@@ -169,7 +182,7 @@ FeatureBasedBboxTracker::extractFeatures(const Bbox3D &bBox) const {
 
 std::optional<TrackedBbox3D> FeatureBasedBboxTracker::getRawTracking() const {
   if (trackedBox_) {
-     return *trackedBox_;
+    return *trackedBox_;
   }
   return std::nullopt;
 }
@@ -186,8 +199,8 @@ std::optional<Control::TrackedPose2D>
 FeatureBasedBboxTracker::getFilteredTrackedPose2D() const {
   if (trackedBox_) {
     auto state_vec = stateKalmanFilter_->getState().value();
-    return Control::TrackedPose2D(state_vec(0), state_vec(1),
-        state_vec(2), state_vec(3), state_vec(4), state_vec(5));
+    return Control::TrackedPose2D(state_vec(0), state_vec(1), state_vec(2),
+                                  state_vec(3), state_vec(4), state_vec(5));
   }
   return std::nullopt;
 }
@@ -197,7 +210,7 @@ Eigen::Vector3f FeatureBasedBboxTracker::computePointsStdDev(
   // compute the mean in each direction
   auto size = std::max(int(pc_points.size() - 1), 1);
   Eigen::Vector3f mean = Eigen::Vector3f::Zero();
-  for (auto point: pc_points){
+  for (auto point : pc_points) {
     mean += point;
   }
   mean /= size;
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
index 48769a99..42615c06 100644
--- a/src/kompass_cpp/tests/vision_detector_test.cpp
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -132,9 +132,7 @@ BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image) {
       "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg";
   auto boxes = config.run(outputFilename);
 
-  const float approx_actual_dist = 1.0;
-  LOG_INFO("Got distance = ", boxes[0].center.x(),
-           ",  boxes[0].center.y()=", boxes[0].center.y());
+  const float approx_actual_dist = 1.8;
   BOOST_TEST(std::abs(boxes[0].center.x() - approx_actual_dist) <= 0.1,
              "3D box distance is not equal to approximate measured distance");
 }
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index e2cca473..0e233f52 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -9,6 +9,7 @@
 #include <Eigen/src/Geometry/Quaternion.h>
 #include <memory>
 #include <opencv2/opencv.hpp>
+#include <optional>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "json_export.h"
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
@@ -19,6 +20,7 @@
 
 using namespace Kompass;
 
+
 struct VisionDWATestConfig {
   // Sampling configuration
   double timeStep;
@@ -74,14 +76,14 @@ struct VisionDWATestConfig {
   VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
                       const double timeStep = 0.1,
                       const int predictionHorizon = 10,
-                      const int controlHorizon = 2,
+                      const int controlHorizon = 1,
                       const int maxLinearSamples = 20,
                       const int maxAngularSamples = 20,
                       const float maxVel = 1.0, const float maxOmega = 4.0,
                       const int maxNumThreads = 1,
-                      const double reference_path_distance_weight = 5.0,
+                      const double reference_path_distance_weight = 1.0,
                       const double goal_distance_weight = 1.0,
-                      const double obstacles_distance_weight = 0.1)
+                      const double obstacles_distance_weight = 0.0)
       : timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
@@ -145,6 +147,7 @@ struct VisionDWATestConfig {
       new_box.center = new_box_shift;
       new_box.center_img_frame = img_frame_shift + ref_point_img;
       new_box.size_img_frame = {25, 25};
+      new_box.timestamp = 0.0f;
       detected_boxes[i] = new_box;
     }
   }
@@ -157,6 +160,7 @@ struct VisionDWATestConfig {
     boxes_ori += tracked_vel.omega() * timeStep;
     for (auto &box : detected_boxes) {
       box.center += target_ref_vel * timeStep;
+      box.timestamp += timeStep;
     }
   };
 
@@ -205,7 +209,7 @@ struct VisionDWATestConfig {
   }
 
   bool run_test(const int numPointsPerTrajectory, std::string pltFileName,
-                bool with_tracker) {
+                bool with_tracker, bool simulate_obstacle=false) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
     Control::TrajectoryVelocities2D simulated_velocities(
         numPointsPerTrajectory);
@@ -228,11 +232,18 @@ struct VisionDWATestConfig {
       LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(),
                ", ", tracked_pose.yaw());
       LOG_INFO("Robot at: ", point.x(), ", ", point.y());
-
+      auto seen_boxes = detected_boxes;
+      std::optional<Control::TrackedPose2D> seen_target = tracked_pose;
+      // Simulate lost of target around obstacle
+      if (simulate_obstacle and robotState.y > 0.1 and robotState.y < 0.3) {
+        LOG_INFO("Sending EMPTY BOXES NOW!!!!!");
+        seen_boxes = {};
+        seen_target = std::nullopt;
+      }
       if (with_tracker) {
-        result = controller->getTrackingCtrl(detected_boxes, cmd, cloud);
+        result = controller->getTrackingCtrl(seen_boxes, cmd, cloud);
       } else {
-        result = controller->getTrackingCtrl(tracked_pose, cmd, cloud);
+        result = controller->getTrackingCtrl(seen_target, cmd, cloud);
       }
 
       if (result.isTrajFound) {
@@ -266,7 +277,7 @@ struct VisionDWATestConfig {
                   robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
                   RST, BOLD(FRED(", yaw: ")), KRED, robotState.yaw, RST,
                   BOLD(FRED("}")));
-        return false;
+        break;
       }
 
       tracked_pose.update(timeStep);
@@ -334,7 +345,7 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
 
   bool test_passed = testConfig.run_test(
       numPointsPerTrajectory, std::string("vision_follower_with_obstacle"),
-      use_tracker);
+      use_tracker, true);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
@@ -373,29 +384,29 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
 
   bool test_passed = testConfig.run_test(
       numPointsPerTrajectory,
-      std::string("vision_follower_with_tracker_and_obstacle"), use_tracker);
+      std::string("vision_follower_with_tracker_and_obstacle"), use_tracker, true);
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
-BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
-  // Create timer
-  Timer time;
+// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
+//   // Create timer
+//   Timer time;
 
-  std::string filename =
-      "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
-      "bag_image_depth.tif";
-  Bbox2D box({410, 0}, {410, 390});
-  std::vector<Bbox2D> detections_2d{box};
+//   std::string filename =
+//       "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
+//       "bag_image_depth.tif";
+//   Bbox2D box({410, 0}, {410, 390});
+//   std::vector<Bbox2D> detections_2d{box};
 
-  auto initial_point = Eigen::Vector2i{610, 200};
+//   auto initial_point = Eigen::Vector2i{610, 200};
 
-  // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
+//   // Robot pointcloud values (global frame)
+//   std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
 
-  VisionDWATestConfig testConfig(cloud);
+//   VisionDWATestConfig testConfig(cloud);
 
-  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
-                                                   initial_point, cloud);
+//   auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
+//                                                    initial_point, cloud);
 
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
-}
+//   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
+// }
diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp
index edea65e8..64db5917 100644
--- a/src/kompass_cpp/tests/vision_tracking_test.cpp
+++ b/src/kompass_cpp/tests/vision_tracking_test.cpp
@@ -41,6 +41,7 @@ struct VisionTrackingTestConfig {
     Bbox3D tracked_box;
     tracked_box.center = target_pose;
     tracked_box.size = target_box_size;
+    tracked_box.timestamp = 0.0f;
     tracker->setInitialTracking(tracked_box);
     reference_path = Control::TrajectoryPath(maxSteps);
     tracked_path = Control::TrajectoryPath(maxSteps);
@@ -53,6 +54,7 @@ struct VisionTrackingTestConfig {
     for(int i=1; i < num_test_boxes; ++i){
       auto new_box_shift = Eigen::Vector3f({float(0.7 * i), float(0.7 * i), 0.0f});
       new_box.center = new_box_shift + target_pose;
+      new_box.timestamp = 0.0f;
       detected_boxes[i] = new_box;
     }
 
@@ -70,6 +72,7 @@ struct VisionTrackingTestConfig {
     for(auto &box: detected_boxes){
       Eigen::Vector3f noise_vector(noise(gen), noise(gen), noise(gen));
       box.center += target_ref_vel * time_step + noise_vector;
+      box.timestamp += time_step;
     }
     // Update the reference state using same velocity command (real)
     reference_state.x += target_ref_vel.x() * time_step;
@@ -91,8 +94,6 @@ BOOST_AUTO_TEST_CASE(test_Vision_Tracker) {
   int step = 0;
   float tracking_error = 0.0;
   while (step < config.maxSteps) {
-    // Move the detected boxes one step using the reference velocity
-    config.moveDetectedBoxes(step);
     // Sed detected boxes and ask tracker to update
     config.tracker->updateTracking(config.detected_boxes);
     auto measured_track = config.tracker->getRawTracking();
@@ -115,6 +116,8 @@ BOOST_AUTO_TEST_CASE(test_Vision_Tracker) {
         LOG_ERROR("Lost tracked target at step: ", step);
         break;
     }
+    // Move the detected boxes one step using the reference velocity
+    config.moveDetectedBoxes(step);
     step++;
   }
 
@@ -146,4 +149,3 @@ BOOST_AUTO_TEST_CASE(test_Vision_Tracker) {
     throw std::system_error(res, std::generic_category(),
                             "Python script failed with error code");
 }
-

From dd638cc9daef32db8e2eb6866133fd510239d2d5 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 20 May 2025 17:23:46 +0200
Subject: [PATCH 059/118] (feature) Adds detections timestamp to python
 nanobind bindings

---
 src/kompass_cpp/bindings/bindings_control.cpp | 16 ++++----
 src/kompass_cpp/bindings/bindings_types.cpp   | 41 ++++++-------------
 .../kompass_cpp/include/datatypes/tracking.h  |  8 ++--
 3 files changed, 26 insertions(+), 39 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index c3c40152..8df0719c 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -1,6 +1,7 @@
 #include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/function.h>
+#include <nanobind/stl/optional.h>
 #include <nanobind/stl/tuple.h>
 #include <nanobind/stl/vector.h>
 
@@ -262,18 +263,19 @@ void bindings_control(py::module_ &m) {
            py::arg("focal_length_x"), py::arg("focal_length_y"),
            py::arg("principal_point_x"), py::arg("principal_point_y"))
       .def("set_initial_tracking",
-           py::overload_cast<const int, const int,
-                             const std::vector<Bbox3D> &, const float>(
+           py::overload_cast<const int, const int, const std::vector<Bbox3D> &,
+                             const float>(
                &Control::VisionDWA::setInitialTracking),
-           py::arg("pixel_x"), py::arg("pixel_y"),
-           py::arg("detected_boxes_3d"), py::arg("robot_orientation") = 0.0)
+           py::arg("pixel_x"), py::arg("pixel_y"), py::arg("detected_boxes_3d"),
+           py::arg("robot_orientation") = 0.0)
       .def("set_initial_tracking",
            py::overload_cast<const int, const int,
                              const Eigen::MatrixX<unsigned short> &,
                              const std::vector<Bbox2D> &, const float>(
                &Control::VisionDWA::setInitialTracking),
            py::arg("pixel_x"), py::arg("pixel_y"),
-           py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"), py::arg("robot_orientation") = 0.0)
+           py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"),
+           py::arg("robot_orientation") = 0.0)
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
@@ -290,7 +292,7 @@ void bindings_control(py::module_ &m) {
            py::arg("detected_boxes"), py::arg("robot_velocity"),
            py::arg("sensor_data"))
       .def("get_tracking_ctrl",
-           py::overload_cast<const Control::TrackedPose2D &,
+           py::overload_cast<const std::optional<Control::TrackedPose2D> &,
                              const Control::Velocity2D &,
                              const std::vector<Eigen::Vector3f> &>(
                &Control::VisionDWA::getTrackingCtrl<
@@ -298,7 +300,7 @@ void bindings_control(py::module_ &m) {
            py::arg("tracked_pose"), py::arg("robot_velocity"),
            py::arg("sensor_data"))
       .def("get_tracking_ctrl",
-           py::overload_cast<const Control::TrackedPose2D &,
+           py::overload_cast<const std::optional<Control::TrackedPose2D> &,
                              const Control::Velocity2D &,
                              const Control::LaserScan &>(
                &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 7f21ecbc..02e5c040 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -135,43 +135,28 @@ py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
 py::class_<Bbox2D>(m_types, "Bbox2D")
     .def(py::init<>())
     .def(py::init<const Bbox2D &>())
-    .def(py::init<Eigen::Vector2i &, Eigen::Vector2i &>(),
-         py::arg("top_left_corner"), py::arg("size"))
+    .def(py::init<const Eigen::Vector2i &, const Eigen::Vector2i &,
+                  const float>(),
+         py::arg("top_left_corner"), py::arg("size"),
+         py::arg("timestamp") = 0.0)
     .def_rw("top_left_corner", &Bbox2D::top_corner)
-    .def_rw("size", &Bbox2D::size);
+    .def_rw("size", &Bbox2D::size)
+    .def_rw("timestamp", &Bbox2D::timestamp);
 
 py::class_<Bbox3D>(m_types, "Bbox3D")
     .def(py::init<>())
     .def(py::init<const Bbox3D &>())
-    .def(py::init<Eigen::Vector3f &, Eigen::Vector3f &, Eigen::Vector2i &,
-                  Eigen::Vector2i &, std::vector<Eigen::Vector3f> &>(),
+    .def(py::init<const Eigen::Vector3f &, const Eigen::Vector3f &,
+                  const Eigen::Vector2i &, const Eigen::Vector2i &, const float,
+                  const std::vector<Eigen::Vector3f> &>(),
          py::arg("center"), py::arg("size"), py::arg("center_img_frame"),
-         py::arg("size_img_frame"), py::arg("pc_points") = py::list())
+         py::arg("size_img_frame"), py::arg("timestamp") = 0.0,
+         py ::arg("pc_points") = py::list())
     .def_rw("center", &Bbox3D::center)
     .def_rw("size", &Bbox3D::size)
     .def_rw("center_img_frame", &Bbox3D::center_img_frame)
     .def_rw("size_img_frame", &Bbox3D::size_img_frame)
-    .def_rw("pc_points", &Bbox3D::pc_points);
+    .def_rw("pc_points", &Bbox3D::pc_points)
+    .def_rw("timestamp", &Bbox3D::timestamp);
 
-py::class_<TrackedBbox3D>(m_types, "TrackedBbox3D")
-    .def(py::init<const Bbox3D &>())
-    .def_rw("box", &TrackedBbox3D::box)
-    .def_rw("vel", &TrackedBbox3D::vel)
-    .def_rw("acc", &TrackedBbox3D::acc)
-    .def_rw("unique_id", &TrackedBbox3D::unique_id)
-    .def("updateFromNewDetection", &TrackedBbox3D::updateFromNewDetection);
-
-py::class_<Control::Pose3D>(m_types, "Pose3D");
-
-py::class_<Control::TrackedPose2D, Control::Pose3D>(m_types, "TrackedPose2D")
-    .def(py::init<const float &, const float &, const float &, const float &,
-                  const float &, const float &>(),
-         py::arg("x"), py::arg("y"), py::arg("yaw"), py::arg("vx"),
-         py::arg("vy"), py::arg("omega"))
-    .def("x", &Control::TrackedPose2D::x)
-    .def("y", &Control::TrackedPose2D::y)
-    .def("yaw", &Control::TrackedPose2D::yaw)
-    .def("v", &Control::TrackedPose2D::v)
-    .def("omega", &Control::TrackedPose2D::omega)
-    .def("update", &Control::TrackedPose2D::update);
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index b69ca1d1..7f94d938 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -15,10 +15,10 @@ struct Bbox2D {
 
   Bbox2D(){};
 
-  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size){};
+  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp){};
 
-  Bbox2D(const Eigen::Vector2i top_corner, Eigen::Vector2i size)
-      : top_corner(top_corner), size(size){};
+  Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size, const float timestamp = 0.0)
+      : top_corner(top_corner), size(size), timestamp(timestamp){};
 
   Eigen::Vector2i getXLimits() const {
     return {top_corner.x(), top_corner.x() + size.x()};
@@ -48,7 +48,7 @@ struct Bbox3D {
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Eigen::Vector2i center_img_frame,
          const Eigen::Vector2i size_img_frame, const float timestamp = 0.0,
-         std::vector<Eigen::Vector3f> pc_points = {})
+         const std::vector<Eigen::Vector3f> pc_points = {})
       : center(center), size(size), center_img_frame(center_img_frame),
         size_img_frame(size_img_frame), pc_points(pc_points),
         timestamp(timestamp){};

From 9623867232d0daa274058a6a2ed14e8bd276a417 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 21 May 2025 13:11:23 +0200
Subject: [PATCH 060/118] (fix) Handles Vision DWA reference tracking for
 Differential Drive robots

---
 .../include/controllers/vision_dwa.h          | 17 ++++-
 .../src/controllers/vision_dwa.cpp            | 69 ++++++++++++++++---
 2 files changed, 76 insertions(+), 10 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index aa5ef875..6d7b8055 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -77,7 +77,9 @@ class VisionDWA : public DWA {
           Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value"));
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
-    bool enable_vel_tracking() const { return getParameter<bool>("track_velocity"); }
+    bool enable_vel_tracking() const {
+      return getParameter<bool>("track_velocity");
+    }
     double control_time_step() const {
       return getParameter<double>("control_time_step");
     }
@@ -168,7 +170,14 @@ class VisionDWA : public DWA {
   getTrackingCtrl(const std::optional<TrackedPose2D> &tracked_pose,
                   const Velocity2D &current_vel, const T &sensor_points) {
     if (tracked_pose.has_value()) {
-      Trajectory2D ref_traj = getTrackingReferenceSegment(tracked_pose.value());
+      Trajectory2D ref_traj;
+      if(is_diff_drive_){
+        ref_traj = getTrackingReferenceSegmentDiffDrive(tracked_pose.value());
+      }
+      else{
+        ref_traj = getTrackingReferenceSegment(tracked_pose.value());
+      }
+
       TrajSearchResult result;
       result.isTrajFound = true;
       result.trajCost = 0.0;
@@ -284,6 +293,7 @@ class VisionDWA : public DWA {
 
 private:
   ControlLimitsParams ctrl_limits_;
+  bool is_diff_drive_;
   VisionDWAConfig config_;
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
   std::unique_ptr<DepthDetector> detector_;
@@ -298,6 +308,9 @@ class VisionDWA : public DWA {
    * @return std::tuple<Trajectory2D, bool>
    */
   Trajectory2D getTrackingReferenceSegment(const TrackedPose2D &tracking_pose);
+
+  Trajectory2D
+  getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose);
 };
 
 } // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 0e995b19..5f1b1c17 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -32,6 +32,7 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
           proximity_sensor_position_body, proximity_sensor_rotation_body,
           octreeRes, costWeights, maxNumThreads) {
   ctrl_limits_ = ctrlLimits;
+  is_diff_drive_ = robotCtrlType == ControlType::DIFFERENTIAL_DRIVE;
   config_ = config;
   // Set the reaching goal distance (used in the DWA mode when vision target is
   // lost)
@@ -80,20 +81,23 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
       abs(angle_error) > angle_tolerance) {
     double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
                 (config_.K_v() * tanh(distance_error))) /
-               cos(psi);
+               cos(angle_error);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
       // TODO terms to be removed
-      // auto term2 = 2.0 * sin(psi) / distance * (v - tracking_pose.v());
-      // auto term3 = -2.0 * config_.K_omega() * tanh(angle_error);
-      omega = -track_velocity_ * tracking_pose.omega() +
-              2.0 * (sin(psi) / distance * (v - tracking_pose.v()) -
-                     config_.K_omega() * tanh(angle_error));
-      // LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(),
-      //           ", term_psi=", term2, ", term_ang_err=", term3);
+      auto term2 = -2.0 * sin(psi) / distance * tracking_pose.v();
+      auto term3 = -2.0 * sin(angle_error) / distance * v;
+      auto term4 = -2.0 * config_.K_omega() * tanh(angle_error);
+      omega = -track_velocity_ * tracking_pose.omega() -
+              2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() +
+              -2.0 * sin(angle_error) / distance * v -
+              2.0 * config_.K_omega() * tanh(angle_error);
+      LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(),
+                ", term_psi=", term2, ", term_ang_err_v=", term3,
+                ", term_ang_err=", term4);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
               2.0 * config_.K_omega() * tanh(angle_error);
@@ -181,5 +185,54 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   return ref_traj;
 };
 
+Trajectory2D
+VisionDWA::getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose) {
+  int step = 0;
+
+  Trajectory2D ref_traj(config_.prediction_horizon());
+  std::vector<Path::State> states;
+  Path::State simulated_state = currentState;
+  Path::State original_state = currentState;
+  TrackedPose2D simulated_track = tracking_pose;
+  Velocity2D cmd;
+
+  // Simulate following the tracked target for the period til
+  // prediction_horizon assuming the target moves with its same current
+  // velocity
+  while (step < config_.prediction_horizon()) {
+    states.push_back(simulated_state);
+    ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+    this->setCurrentState(simulated_state);
+    cmd = this->getPureTrackingCtrl(simulated_track);
+    if(cmd.vx() >= config_.min_vel() && cmd.omega() >= config_.min_vel()) {
+      // Rotate then Move
+      auto vel_rotate = Velocity2D(0.0, 0.0, cmd.omega());
+      simulated_state.update(vel_rotate, config_.control_time_step());
+      if (step < config_.prediction_horizon() - 1) {
+        ref_traj.velocities.add(step, vel_rotate);
+      }
+      step++;
+      auto vel_move = Velocity2D(cmd.vx(), 0.0, 0.0);
+      simulated_state.update(vel_move, config_.control_time_step());
+      if (step < config_.prediction_horizon() - 1) {
+        ref_traj.velocities.add(step, vel_move);
+      }
+      step++;
+    }
+    else{
+      simulated_state.update(cmd, config_.control_time_step());
+      simulated_track.update(config_.control_time_step());
+      if (step < config_.prediction_horizon() - 1) {
+        ref_traj.velocities.add(step, cmd);
+      }
+      step++;
+    }
+  }
+  this->setCurrentState(original_state);
+
+  return ref_traj;
+};
+
 } // namespace Control
 } // namespace Kompass

From 0f24942f0fe1b67000dea067ea2e196577979507 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 21 May 2025 23:20:39 +0800
Subject: [PATCH 061/118] (fix) Fixes vision dwa control law

---
 src/kompass_core/control/vision_dwa.py        |  8 ++--
 src/kompass_core/datatypes/__init__.py        |  3 +-
 .../include/controllers/vision_dwa.h          |  4 +-
 .../src/controllers/vision_dwa.cpp            | 44 ++++++++++---------
 4 files changed, 30 insertions(+), 29 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 3109603a..18b1b29f 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -12,7 +12,7 @@
     LaserScan,
     TrajectoryVelocities2D,
     TrajectoryPath,
-    TrackedPose2D,
+    # TrackedPose2D,
 )
 from typing import Optional, List
 import numpy as np
@@ -257,7 +257,7 @@ def loop_step(
         detections_3d: Optional[List[Bbox3D]] = None,
         detections_2d: Optional[List[Bbox2D]] = None,
         depth_image: Optional[np.ndarray] = None,
-        tracked_pose: Optional[TrackedPose2D] = None,
+        # tracked_pose: Optional[TrackedPose2D] = None,
         laser_scan: Optional[LaserScanData] = None,
         point_cloud: Optional[List[np.ndarray]] = None,
         local_map: Optional[np.ndarray] = None,
@@ -299,9 +299,9 @@ def loop_step(
             return False
 
         try:
-            if (detections_3d or tracked_pose) is not None:
+            if detections_3d is not None:
                 self._result = self._planner.get_tracking_ctrl(
-                    tracked_pose or detections_3d, current_velocity, sensor_data
+                    detections_3d, current_velocity, sensor_data
                 )
             else:
                 self._result = self._planner.get_tracking_ctrl(
diff --git a/src/kompass_core/datatypes/__init__.py b/src/kompass_core/datatypes/__init__.py
index d518d01e..e9d4722a 100644
--- a/src/kompass_core/datatypes/__init__.py
+++ b/src/kompass_core/datatypes/__init__.py
@@ -15,7 +15,7 @@
 from .pointcloud import PointCloudData
 from .pose import PoseData
 from .vision import TrackingData, ImageMetaData
-from kompass_cpp.types import TrackedPose2D, Bbox3D, Bbox2D
+from kompass_cpp.types import Bbox3D, Bbox2D
 
 __all__ = [
     "LaserScanData",
@@ -36,5 +36,4 @@
     "ImageMetaData",
     "Bbox3D",
     "Bbox2D",
-    "TrackedPose2D",
 ]
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 6d7b8055..56160820 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -34,7 +34,7 @@ class VisionDWA : public DWA {
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
       addParameter(
           "tolerance",
-          Parameter(0.01, 1e-6, 1e3,
+          Parameter(0.01, 0.0, 1.0,
                     "Tolerance value for distance and angle following errors"));
       addParameter(
           "target_distance",
@@ -58,7 +58,7 @@ class VisionDWA : public DWA {
       // Pure tracking control law parameters
       addParameter("rotation_gain", Parameter(1.0, 1e-2, 10.0));
       addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
-      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
+      addParameter("min_vel", Parameter(0.01, 1e-9, 1e9));
       addParameter("enable_search", Parameter(false));
       // Kalman Filter parameters
       addParameter("error_pose", Parameter(0.05, 1e-9, 1e9));
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 5f1b1c17..b1ed6675 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -72,9 +72,12 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
 
   // LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
 
-  float distance_tolerance = config_.tolerance() * config_.target_distance();
+  float distance_tolerance = std::max(0.01, config_.tolerance() * config_.target_distance());
   float angle_tolerance =
-      std::max(0.001, config_.tolerance() * config_.target_orientation());
+      std::max(0.01, config_.tolerance() * config_.target_orientation());
+  LOG_DEBUG("distance_error=", distance_error, ", angle_error=",
+            angle_error, ", distance_tolerance=", distance_tolerance,
+            ", angle_tolerance=", angle_tolerance);
 
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
@@ -84,28 +87,27 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
                cos(angle_error);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
+    if (std::abs(v) < config_.min_vel()) {
+      v = 0.0;
+    }
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
-      // TODO terms to be removed
-      auto term2 = -2.0 * sin(psi) / distance * tracking_pose.v();
-      auto term3 = -2.0 * sin(angle_error) / distance * v;
-      auto term4 = -2.0 * config_.K_omega() * tanh(angle_error);
       omega = -track_velocity_ * tracking_pose.omega() -
               2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() +
               -2.0 * sin(angle_error) / distance * v -
               2.0 * config_.K_omega() * tanh(angle_error);
-      LOG_DEBUG("-tracking_pose.omega()=", -tracking_pose.omega(),
-                ", term_psi=", term2, ", term_ang_err_v=", term3,
-                ", term_ang_err=", term4);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
               2.0 * config_.K_omega() * tanh(angle_error);
     }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
+    if (std::abs(omega) < config_.min_vel()) {
+      omega = 0.0;
+    }
     followingVel.setOmega(omega);
-    // LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
+    LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
   }
   return followingVel;
 }
@@ -157,7 +159,6 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  std::vector<Path::State> states;
   Path::State simulated_state = currentState;
   Path::State original_state = currentState;
   TrackedPose2D simulated_track = tracking_pose;
@@ -167,7 +168,6 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   // prediction_horizon assuming the target moves with its same current
   // velocity
   while (step < config_.prediction_horizon()) {
-    states.push_back(simulated_state);
     ref_traj.path.add(step,
                       Path::Point(simulated_state.x, simulated_state.y, 0.0));
     this->setCurrentState(simulated_state);
@@ -190,7 +190,6 @@ VisionDWA::getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_po
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  std::vector<Path::State> states;
   Path::State simulated_state = currentState;
   Path::State original_state = currentState;
   TrackedPose2D simulated_track = tracking_pose;
@@ -200,27 +199,30 @@ VisionDWA::getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_po
   // prediction_horizon assuming the target moves with its same current
   // velocity
   while (step < config_.prediction_horizon()) {
-    states.push_back(simulated_state);
-    ref_traj.path.add(step,
-                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
-    if(cmd.vx() >= config_.min_vel() && cmd.omega() >= config_.min_vel()) {
+    if(std::abs(cmd.vx()) >= config_.min_vel() && std::abs(cmd.omega()) >= config_.min_vel()) {
       // Rotate then Move
+      ref_traj.path.add(step,
+                    Path::Point(simulated_state.x, simulated_state.y, 0.0));
       auto vel_rotate = Velocity2D(0.0, 0.0, cmd.omega());
       simulated_state.update(vel_rotate, config_.control_time_step());
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, vel_rotate);
       }
       step++;
-      auto vel_move = Velocity2D(cmd.vx(), 0.0, 0.0);
-      simulated_state.update(vel_move, config_.control_time_step());
-      if (step < config_.prediction_horizon() - 1) {
+      if(step < config_.prediction_horizon() - 2){
+        auto vel_move = Velocity2D(cmd.vx(), 0.0, 0.0);
+        ref_traj.path.add(step,
+                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
+        simulated_state.update(vel_move, config_.control_time_step());
         ref_traj.velocities.add(step, vel_move);
+        step++;
       }
-      step++;
     }
     else{
+      ref_traj.path.add(step,
+                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
       simulated_state.update(cmd, config_.control_time_step());
       simulated_track.update(config_.control_time_step());
       if (step < config_.prediction_horizon() - 1) {

From be5379ecc6d778d5cbfe91e012a69c083197ecb9 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 21 May 2025 18:29:50 +0200
Subject: [PATCH 062/118] (feature) Adds method to set initial tracking from 2D
 box target

---
 src/kompass_core/control/vision_dwa.py        | 28 ++++++++++-
 src/kompass_core/utils/emergency_stop.py      | 22 ++++-----
 src/kompass_cpp/bindings/bindings_control.cpp |  6 +++
 .../include/controllers/vision_dwa.h          |  5 ++
 .../src/controllers/vision_dwa.cpp            | 47 +++++++++++--------
 tests/test_laserscan_emergency_stop.py        |  5 +-
 6 files changed, 79 insertions(+), 34 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 18b1b29f..c3e129a0 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -215,7 +215,33 @@ def set_initial_tracking_3d(
             logging.error(f"Could not set initial tracking state: {e}")
             return False
 
-    def set_initial_tracking_depth(
+    def set_initial_tracking_2d_target(
+        self,
+        current_state: RobotState,
+        target_box: Bbox2D,
+        aligned_depth_image: np.ndarray,
+    ) -> bool:
+        """
+        Set initial tracking state
+
+        :param detected_boxes: Detected boxes
+        :type detected_boxes: List[Bbox3D]
+        """
+        try:
+            self._planner.set_current_state(
+                current_state.x, current_state.y, current_state.yaw, current_state.speed
+            )
+            return self._planner.set_initial_tracking(
+                aligned_depth_image,
+                target_box,
+                current_state.yaw,
+            )
+
+        except Exception as e:
+            logging.error(f"Could not set initial tracking state: {e}")
+            return False
+
+    def set_initial_tracking_image(
         self,
         current_state: RobotState,
         pose_x_img: int,
diff --git a/src/kompass_core/utils/emergency_stop.py b/src/kompass_core/utils/emergency_stop.py
index ba2b0e41..c2f2989b 100644
--- a/src/kompass_core/utils/emergency_stop.py
+++ b/src/kompass_core/utils/emergency_stop.py
@@ -4,6 +4,7 @@
     Robot,
     RobotGeometry,
 )
+import numpy as np
 from ..datatypes import LaserScanData
 
 
@@ -15,14 +16,16 @@ def __init__(
         robot: Robot,
         emergency_distance: float,
         emergency_angle: float,
-        sensor_position_robot: Optional[List[float]] = None,
-        sensor_rotation_robot: Optional[List[float]] = None,
+        sensor_position_robot: Optional[np.ndarray] = None,
+        sensor_rotation_robot: Optional[np.ndarray] = None,
         use_gpu: bool = False,
     ) -> None:
         self.__emergency_distance = emergency_distance
         self.__emergency_angle = emergency_angle
-        self.__sensor_position_robot = sensor_position_robot or [0.0, 0.0, 0.0]
-        self.__sensor_rotation_robot = sensor_rotation_robot or [0.0, 0.0, 0.0, 1.0]
+        self.__sensor_position_robot = sensor_position_robot if sensor_position_robot is not None else np.array([0.0, 0.0, 0.0], dtype=np.float32)
+        self.__sensor_rotation_robot = sensor_rotation_robot if sensor_rotation_robot is not None else np.array(
+            [1.0, 0.0, 0.0, 0.0], dtype=np.float32
+        )
         self.__robot_shape = RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type)
         self.__robot_dimensions = robot.geometry_params
         self.__use_gpu = use_gpu
@@ -35,10 +38,8 @@ def _init_checker(self, scan: LaserScanData) -> None:
                 self._critical_zone_checker = CriticalZoneCheckerGPU(
                     robot_shape=self.__robot_shape,
                     robot_dimensions=self.__robot_dimensions,
-                    sensor_position_body=self.__sensor_position_robot
-                    or [0.0, 0.0, 0.0],
-                    sensor_rotation_body=self.__sensor_rotation_robot
-                    or [0.0, 0.0, 0.0, 1.0],
+                    sensor_position_body=self.__sensor_position_robot,
+                    sensor_rotation_body=self.__sensor_rotation_robot,
                     critical_angle=self.__emergency_angle,
                     critical_distance=self.__emergency_distance,
                     scan_angles=scan.angles,
@@ -55,9 +56,8 @@ def _init_checker(self, scan: LaserScanData) -> None:
             self._critical_zone_checker = CriticalZoneChecker(
                 robot_shape=self.__robot_shape,
                 robot_dimensions=self.__robot_dimensions,
-                sensor_position_body=self.__sensor_position_robot or [0.0, 0.0, 0.0],
-                sensor_rotation_body=self.__sensor_rotation_robot
-                or [0.0, 0.0, 0.0, 1.0],
+                sensor_position_body=self.__sensor_position_robot,
+                sensor_rotation_body=self.__sensor_rotation_robot,
                 critical_angle=self.__emergency_angle,
                 critical_distance=self.__emergency_distance,
             )
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 8df0719c..caa10e14 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -276,6 +276,12 @@ void bindings_control(py::module_ &m) {
            py::arg("pixel_x"), py::arg("pixel_y"),
            py::arg("aligned_depth_image"), py::arg("detected_boxes_2d"),
            py::arg("robot_orientation") = 0.0)
+      .def("set_initial_tracking",
+           py::overload_cast<const Eigen::MatrixX<unsigned short> &,
+                             const Bbox2D &, const float>(
+               &Control::VisionDWA::setInitialTracking),
+           py::arg("aligned_depth_image"), py::arg("target_box_2d"),
+           py::arg("robot_orientation") = 0.0)
       .def("get_tracking_ctrl",
            py::overload_cast<const std::vector<Bbox3D> &,
                              const Control::Velocity2D &,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 56160820..928ec0cc 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -291,6 +291,11 @@ class VisionDWA : public DWA {
                      const std::vector<Bbox2D> &detected_boxes_2d,
                      const float yaw = 0.0);
 
+  bool
+  setInitialTracking(const Eigen::MatrixX<unsigned short> &aligned_depth_image,
+                     const Bbox2D &target_box_2d,
+                     const float yaw = 0.0);
+
 private:
   ControlLimitsParams ctrl_limits_;
   bool is_diff_drive_;
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index b1ed6675..0621b9be 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -72,11 +72,12 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
 
   // LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
 
-  float distance_tolerance = std::max(0.01, config_.tolerance() * config_.target_distance());
+  float distance_tolerance =
+      std::max(0.01, config_.tolerance() * config_.target_distance());
   float angle_tolerance =
       std::max(0.01, config_.tolerance() * config_.target_orientation());
-  LOG_DEBUG("distance_error=", distance_error, ", angle_error=",
-            angle_error, ", distance_tolerance=", distance_tolerance,
+  LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error,
+            ", distance_tolerance=", distance_tolerance,
             ", angle_tolerance=", angle_tolerance);
 
   Velocity2D followingVel;
@@ -123,11 +124,7 @@ bool VisionDWA::setInitialTracking(
     const int pose_x_img, const int pose_y_img,
     const Eigen::MatrixX<unsigned short> &aligned_depth_image,
     const std::vector<Bbox2D> &detected_boxes, const float yaw) {
-  if (!detector_) {
-    throw std::runtime_error(
-        "DepthDetector is not initialized with the camera intrinsics. Call "
-        "'VisionDWA::setCameraIntrinsics' first");
-  }
+
   std::unique_ptr<Bbox2D> target_box;
   for (auto box : detected_boxes) {
     auto limits_x = box.getXLimits();
@@ -143,9 +140,21 @@ bool VisionDWA::setInitialTracking(
     LOG_DEBUG("Target point not found in any detected box");
     return false;
   }
+
+  return setInitialTracking(aligned_depth_image, *target_box, yaw);
+}
+
+bool VisionDWA::setInitialTracking(
+    const Eigen::MatrixX<unsigned short> &aligned_depth_image,
+    const Bbox2D &target_box_2d, const float yaw) {
+  if (!detector_) {
+    throw std::runtime_error(
+        "DepthDetector is not initialized with the camera intrinsics. Call "
+        "'VisionDWA::setCameraIntrinsics' first");
+  }
   // Send current state to the detector
   detector_->updateState(currentState);
-  detector_->updateBoxes(aligned_depth_image, {*target_box});
+  detector_->updateBoxes(aligned_depth_image, {target_box_2d});
   auto boxes_3d = detector_->get3dDetections();
   if (!boxes_3d) {
     LOG_DEBUG("Failed to get 3D box from 2D target box");
@@ -185,8 +194,8 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   return ref_traj;
 };
 
-Trajectory2D
-VisionDWA::getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose) {
+Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
+    const TrackedPose2D &tracking_pose) {
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
@@ -201,28 +210,28 @@ VisionDWA::getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_po
   while (step < config_.prediction_horizon()) {
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
-    if(std::abs(cmd.vx()) >= config_.min_vel() && std::abs(cmd.omega()) >= config_.min_vel()) {
+    if (std::abs(cmd.vx()) >= config_.min_vel() &&
+        std::abs(cmd.omega()) >= config_.min_vel()) {
       // Rotate then Move
       ref_traj.path.add(step,
-                    Path::Point(simulated_state.x, simulated_state.y, 0.0));
+                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
       auto vel_rotate = Velocity2D(0.0, 0.0, cmd.omega());
       simulated_state.update(vel_rotate, config_.control_time_step());
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, vel_rotate);
       }
       step++;
-      if(step < config_.prediction_horizon() - 2){
+      if (step < config_.prediction_horizon() - 2) {
         auto vel_move = Velocity2D(cmd.vx(), 0.0, 0.0);
-        ref_traj.path.add(step,
-                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
+        ref_traj.path.add(
+            step, Path::Point(simulated_state.x, simulated_state.y, 0.0));
         simulated_state.update(vel_move, config_.control_time_step());
         ref_traj.velocities.add(step, vel_move);
         step++;
       }
-    }
-    else{
+    } else {
       ref_traj.path.add(step,
-                      Path::Point(simulated_state.x, simulated_state.y, 0.0));
+                        Path::Point(simulated_state.x, simulated_state.y, 0.0));
       simulated_state.update(cmd, config_.control_time_step());
       simulated_track.update(config_.control_time_step());
       if (step < config_.prediction_horizon() - 1) {
diff --git a/tests/test_laserscan_emergency_stop.py b/tests/test_laserscan_emergency_stop.py
index 751bba0d..4afec7db 100644
--- a/tests/test_laserscan_emergency_stop.py
+++ b/tests/test_laserscan_emergency_stop.py
@@ -49,7 +49,6 @@ def test_laserscan_polar_tf(laser_scan_data: LaserScanData, plot: bool = False):
     # 90 Deg rotation around z
     translation = [0.0, 0.0, 0.173]
     rotation = [0.0, 0.0, 0.7071068, 0.7071068]
-    # rotation = [0.0, 0.0, 0.0, 1.0]
 
     transformed_scan = get_laserscan_transformed_polar_coordinates(
         angle_min=laser_scan_data.angle_min,
@@ -145,8 +144,8 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu):
         robot=robot,
         emergency_distance=emergency_distance,
         emergency_angle=emergency_angle,
-        sensor_position_robot=[0.0, 0.0, 0.173],
-        sensor_rotation_robot=[0.0, 0.0, 0.0, 1.0],
+        sensor_position_robot=np.array([0.0, 0.0, 0.173], dtype=np.float32),
+        sensor_rotation_robot=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32),
         use_gpu=use_gpu,
     )
     angles_size = np.arange(

From 4eef4b02f8f29e12a91d6099df38d5ed210aa73d Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 22 May 2025 13:57:04 +0200
Subject: [PATCH 063/118] (feature) Adds Slowdown zone to CriticalZoneChecker
 and updates tests

---
 src/kompass_core/utils/emergency_stop.py      |  10 +-
 src/kompass_cpp/bindings/bindings_gpu.cpp     |   6 +-
 src/kompass_cpp/bindings/bindings_utils.cpp   |   7 +-
 .../include/utils/critical_zone_check.h       |  15 ++-
 .../include/utils/critical_zone_check_gpu.h   |  26 ++--
 .../src/utils/critical_zone_check.cpp         |  43 +++++--
 .../src/utils/critical_zone_check_gpu.cpp     | 115 +++++++++---------
 src/kompass_cpp/tests/collisions_test.cpp     |  91 ++++++++++----
 src/kompass_cpp/tests/collisions_test_gpu.cpp |  94 ++++++++++----
 tests/test_laserscan_emergency_stop.py        |   8 +-
 10 files changed, 280 insertions(+), 135 deletions(-)

diff --git a/src/kompass_core/utils/emergency_stop.py b/src/kompass_core/utils/emergency_stop.py
index c2f2989b..f18e8466 100644
--- a/src/kompass_core/utils/emergency_stop.py
+++ b/src/kompass_core/utils/emergency_stop.py
@@ -15,12 +15,14 @@ def __init__(
         self,
         robot: Robot,
         emergency_distance: float,
+        slowdown_distance: float,
         emergency_angle: float,
         sensor_position_robot: Optional[np.ndarray] = None,
         sensor_rotation_robot: Optional[np.ndarray] = None,
         use_gpu: bool = False,
     ) -> None:
         self.__emergency_distance = emergency_distance
+        self.__slowdown_distance = slowdown_distance
         self.__emergency_angle = emergency_angle
         self.__sensor_position_robot = sensor_position_robot if sensor_position_robot is not None else np.array([0.0, 0.0, 0.0], dtype=np.float32)
         self.__sensor_rotation_robot = sensor_rotation_robot if sensor_rotation_robot is not None else np.array(
@@ -42,6 +44,7 @@ def _init_checker(self, scan: LaserScanData) -> None:
                     sensor_rotation_body=self.__sensor_rotation_robot,
                     critical_angle=self.__emergency_angle,
                     critical_distance=self.__emergency_distance,
+                    slowdown_distance=self.__slowdown_distance,
                     scan_angles=scan.angles,
                 )
             except (ImportError, ModuleNotFoundError):
@@ -60,17 +63,18 @@ def _init_checker(self, scan: LaserScanData) -> None:
                 sensor_rotation_body=self.__sensor_rotation_robot,
                 critical_angle=self.__emergency_angle,
                 critical_distance=self.__emergency_distance,
+                slowdown_distance=self.__slowdown_distance,
             )
 
-    def run(self, *_, scan: LaserScanData, forward: bool = True) -> bool:
+    def run(self, *_, scan: LaserScanData, forward: bool = True) -> float:
         """Runs emergency checking on new incoming laser scan data
 
         :param scan: 2D Laserscan data (ranges/angles)
         :type scan: LaserScanData
         :param forward: If the robot is moving forward or not, defaults to True
         :type forward: bool, optional
-        :return: If an obstacle is within the safety zone
-        :rtype: bool
+        :return: Slowdown factor if an obstacle is within the safety zone
+        :rtype: float
         """
         if not self.__initialized:
             self._init_checker(scan)
diff --git a/src/kompass_cpp/bindings/bindings_gpu.cpp b/src/kompass_cpp/bindings/bindings_gpu.cpp
index 18fdf555..0b7b50a0 100644
--- a/src/kompass_cpp/bindings/bindings_gpu.cpp
+++ b/src/kompass_cpp/bindings/bindings_gpu.cpp
@@ -26,12 +26,12 @@ void bindings_utils_gpu(py::module_ &m) {
 
   py::class_<CriticalZoneCheckerGPU>(m, "CriticalZoneCheckerGPU")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const Eigen::Vector3f &, const Eigen::Vector4f &, float,
-                    float, const std::vector<double> &>(),
+                    const Eigen::Vector3f &, const Eigen::Vector4f &, const float,
+                    const float, const float, const std::vector<double> &>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
            py::arg("critical_angle"), py::arg("critical_distance"),
-           py::arg("scan_angles"))
+           py::arg("slowdown_distance"), py::arg("scan_angles"))
       .def("check", &CriticalZoneChecker::check, py::arg("ranges"),
            py::arg("angles"), py::arg("forward"));
 }
diff --git a/src/kompass_cpp/bindings/bindings_utils.cpp b/src/kompass_cpp/bindings/bindings_utils.cpp
index d24691f5..c66d405b 100644
--- a/src/kompass_cpp/bindings/bindings_utils.cpp
+++ b/src/kompass_cpp/bindings/bindings_utils.cpp
@@ -16,11 +16,12 @@ void bindings_utils(py::module_ &m) {
 
   py::class_<CriticalZoneChecker>(m_utils, "CriticalZoneChecker")
       .def(py::init<CollisionChecker::ShapeType, const std::vector<float> &,
-                    const Eigen::Vector3f &, const Eigen::Vector4f &, float,
-                    float>(),
+                    const Eigen::Vector3f &, const Eigen::Vector4f &, const float,
+                    const float, const float>(),
            py::arg("robot_shape"), py::arg("robot_dimensions"),
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
-           py::arg("critical_angle"), py::arg("critical_distance"))
+           py::arg("critical_angle"), py::arg("critical_distance"),
+           py::arg("slowdown_distance"))
       .def("check", &CriticalZoneChecker::check, py::arg("ranges"),
            py::arg("angles"), py::arg("forward"));
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
index ffbe8295..28560111 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
@@ -24,7 +24,8 @@ class CriticalZoneChecker {
                       const Eigen::Vector3f &sensor_position_body,
                       const Eigen::Vector4f &sensor_rotation_body,
                       const float critical_angle,
-                      const float critical_distance);
+                      const float critical_distance,
+                      const float slowdown_distance);
 
   /**
    * @brief Destroy the CriticalZoneChecker object
@@ -34,7 +35,15 @@ class CriticalZoneChecker {
 
   void preset(const std::vector<double> &angles);
 
-  bool check(const std::vector<double> &ranges,
+  /**
+   * @brief Check if the robot is in the slowdown or critical zone
+   *
+   * @param ranges    LaserScan ranges
+   * @param angles    LaserScan angles
+   * @param forward   True if the robot is moving forward, false otherwise
+   * @return    Slowdown factor (0.0 - 1.0) if in the slowdown zone, 0.0 if in the critical zone (stop), 1.0 otherwise
+   */
+  float check(const std::vector<double> &ranges,
              const std::vector<double> &angles, const bool forward);
 
 protected:
@@ -43,7 +52,7 @@ class CriticalZoneChecker {
       angle_left_backward_;
   std::vector<size_t> indicies_forward_, indicies_backward_;
   bool preset_{false};
-  float critical_distance_;
+  float critical_distance_, slowdown_distance_;
 
   Eigen::Isometry3f sensor_tf_body_ =
       Eigen::Isometry3f::Identity(); // Sensor transformation with
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
index 1f1f2284..41c050d3 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
@@ -16,16 +16,16 @@ namespace Kompass {
 class CriticalZoneCheckerGPU : public CriticalZoneChecker {
 public:
   // Constructor
-  CriticalZoneCheckerGPU(const CollisionChecker::ShapeType robot_shape_type,
-                         const std::vector<float> &robot_dimensions,
-                         const Eigen::Vector3f &sensor_position_body,
-                         const Eigen::Vector4f &sensor_rotation_body,
-                         const float critical_angle,
-                         const float critical_distance,
-                         const std::vector<double> &angles)
+  CriticalZoneCheckerGPU(
+      const CollisionChecker::ShapeType robot_shape_type,
+      const std::vector<float> &robot_dimensions,
+      const Eigen::Vector3f &sensor_position_body,
+      const Eigen::Vector4f &sensor_rotation_body, const float critical_angle,
+      const float critical_distance,
+      const float slowdown_distance, const std::vector<double> &angles)
       : CriticalZoneChecker(robot_shape_type, robot_dimensions,
                             sensor_position_body, sensor_rotation_body,
-                            critical_angle, critical_distance),
+                            critical_angle, critical_distance, slowdown_distance),
         m_scanSize(angles.size()) {
     m_q = sycl::queue{sycl::default_selector_v,
                       sycl::property::queue::in_order{}};
@@ -35,7 +35,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
 
     m_devicePtrRanges = sycl::malloc_device<double>(m_scanSize, m_q);
     m_devicePtrAngles = sycl::malloc_device<double>(m_scanSize, m_q);
-    m_result = sycl::malloc_shared<bool>(1, m_q);
+    m_result = sycl::malloc_shared<float>(1, m_q);
 
     // set forward and backward indices
     preset(angles);
@@ -52,7 +52,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
 
     m_scan_in_zone =
         std::max(indicies_forward_.size(), indicies_backward_.size());
-    m_devicePtrOutput = sycl::malloc_device<bool>(m_scan_in_zone, m_q);
+    m_devicePtrOutput = sycl::malloc_device<float>(m_scan_in_zone, m_q);
   }
 
   // Destructor
@@ -85,7 +85,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
    * @param ranges         LaserScan ranges in meters
    * @param forward        If the robot is moving forward
    */
-  bool check(const std::vector<double> &ranges,
+  float check(const std::vector<double> &ranges,
              const std::vector<double> &angles, const bool forward);
 
 private:
@@ -95,8 +95,8 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
   double *m_devicePtrAngles;
   size_t *m_devicePtrForward;
   size_t *m_devicePtrBackward;
-  bool *m_devicePtrOutput;
-  bool *m_result;
+  float *m_devicePtrOutput;
+  float *m_result;
   sycl::queue m_q;
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
index 8fd0c83f..cf3c92f4 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
@@ -3,6 +3,7 @@
 #include "utils/logger.h"
 #include "utils/transformation.h"
 #include <Eigen/Core>
+#include <array>
 
 namespace Kompass {
 /**
@@ -15,7 +16,7 @@ CriticalZoneChecker::CriticalZoneChecker(
     const std::vector<float> &robot_dimensions,
     const Eigen::Vector3f &sensor_position_body,
     const Eigen::Vector4f &sensor_rotation_body, const float critical_angle,
-    const float critical_distance) {
+    const float critical_distance, const float slowdown_distance) {
   // Construct  a geometry object based on the robot shape
   if (robot_shape_type == CollisionChecker::ShapeType::CYLINDER) {
     robotHeight_ = robot_dimensions.at(1);
@@ -45,7 +46,13 @@ CriticalZoneChecker::CriticalZoneChecker(
             angle_left_backward_, "]");
 
   // Set critical distance
+  if (slowdown_distance <= critical_distance) {
+
+    throw std::invalid_argument(
+        "SlowDown distance must be greater than the Critical distance!");
+  }
   critical_distance_ = critical_distance;
+  slowdown_distance_ = slowdown_distance;
 }
 
 void CriticalZoneChecker::preset(const std::vector<double> &angles) {
@@ -75,9 +82,9 @@ void CriticalZoneChecker::preset(const std::vector<double> &angles) {
   preset_ = true;
 }
 
-bool CriticalZoneChecker::check(const std::vector<double> &ranges,
-                                const std::vector<double> &angles,
-                                const bool forward) {
+float CriticalZoneChecker::check(const std::vector<double> &ranges,
+                                 const std::vector<double> &angles,
+                                 const bool forward) {
   if (angles.size() != ranges.size()) {
     LOG_ERROR("Angles and ranges vectors must have the same size!");
     return false;
@@ -102,20 +109,40 @@ bool CriticalZoneChecker::check(const std::vector<double> &ranges,
         cartesianPoint = {x, y, 0.0f};
         // Apply TF
         cartesianPoint = sensor_tf_body_ * cartesianPoint;
+        auto transformation_matrix = sensor_tf_body_.matrix();
+        const std::array<float, 4> x_transform{
+            transformation_matrix(0, 0), transformation_matrix(0, 1),
+            transformation_matrix(0, 2), transformation_matrix(0, 3)};
+
+        const std::array<float, 4> y_transform{
+            transformation_matrix(1, 0), transformation_matrix(1, 1),
+            transformation_matrix(1, 2), transformation_matrix(1, 3)};
+
+        std::array<float, 4> point{x,
+                                   y, 0.0, 1.0};
+        std::array<float, 2> transformed_point{0.0, 0.0};
+
+        for (size_t i = 0; i < 4; ++i) {
+          transformed_point[0] += x_transform[i] * point[i];
+          transformed_point[1] += y_transform[i] * point[i];
+        }
 
         // check if within the zone
         converted_range = std::sqrt(std::pow(cartesianPoint.y(), 2) +
                                     std::pow(cartesianPoint.x(), 2));
-
-        if (converted_range - robotRadius_ <= critical_distance_) {
-          return true;
+        float distance = converted_range - robotRadius_;
+        if (distance <= critical_distance_) {
+          return 0.0;
+        } else if (distance <= slowdown_distance_) {
+          return (distance - critical_distance_) /
+                 (slowdown_distance_ - critical_distance_);
         }
       } else {
         preset_ = false;
         return check(ranges, angles, forward);
       }
     }
-    return false;
+    return 1.0;
   }
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
index adbea3cb..c2e58f10 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
@@ -6,9 +6,9 @@
 
 namespace Kompass {
 
-bool CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
-                                   const std::vector<double> &angles,
-                                   const bool forward) {
+float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
+                                    const std::vector<double> &angles,
+                                    const bool forward) {
   try {
     m_q.fill(m_devicePtrOutput, false, m_scan_in_zone);
 
@@ -19,65 +19,70 @@ bool CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
 
     // command scope
     m_q.submit([&](sycl::handler &h) {
-      // local copies of class members to be used inside the kernel
-      const double robot_radius = robotRadius_;
-      auto transformation_matrix = sensor_tf_body_.matrix();
-      const sycl::vec<float, 4> x_transform{
-          transformation_matrix(0, 0), transformation_matrix(0, 1),
-          transformation_matrix(0, 2), transformation_matrix(0, 3)};
+         // local copies of class members to be used inside the kernel
+         const double robot_radius = robotRadius_;
+         auto transformation_matrix = sensor_tf_body_.matrix();
+         const sycl::vec<float, 4> x_transform{
+             transformation_matrix(0, 0), transformation_matrix(0, 1),
+             transformation_matrix(0, 2), transformation_matrix(0, 3)};
 
-      sycl::vec<float, 4> y_transform{
-          transformation_matrix(1, 0), transformation_matrix(1, 1),
-          transformation_matrix(1, 2), transformation_matrix(1, 3)};
+         sycl::vec<float, 4> y_transform{
+             transformation_matrix(1, 0), transformation_matrix(1, 1),
+             transformation_matrix(1, 2), transformation_matrix(1, 3)};
 
-      const auto devicePtrRanges = m_devicePtrRanges;
-      const auto devicePtrAngles = m_devicePtrAngles;
-      const auto devicePtrOutput = m_devicePtrOutput;
-      const auto criticalDistance = critical_distance_;
-      size_t *critical_indices;
-      sycl::range<1> global_size;
-      if (forward) {
-        critical_indices = m_devicePtrForward;
-        global_size = sycl::range<1>(indicies_forward_.size());
-      } else {
-        critical_indices = m_devicePtrBackward;
-        global_size = sycl::range<1>(indicies_backward_.size());
-      }
+         const auto devicePtrRanges = m_devicePtrRanges;
+         const auto devicePtrAngles = m_devicePtrAngles;
+         const auto devicePtrOutput = m_devicePtrOutput;
+         const auto criticalDistance = critical_distance_;
+         const auto slowdownDistance = slowdown_distance_;
+         size_t *critical_indices;
+         *m_result = 1.0f;
+         sycl::range<1> global_size;
+         if (forward) {
+           critical_indices = m_devicePtrForward;
+           global_size = sycl::range<1>(indicies_forward_.size());
+         } else {
+           critical_indices = m_devicePtrBackward;
+           global_size = sycl::range<1>(indicies_backward_.size());
+         }
 
-      // kernel scope
-      h.parallel_for<class checkCriticalZoneKernel>(
-          global_size, [=](sycl::id<1> idx) {
-            const size_t local_id = critical_indices[idx];
-            double range = devicePtrRanges[local_id];
-            double angle = devicePtrAngles[local_id];
+         // kernel scope
+         h.parallel_for<class checkCriticalZoneKernel>(
+             global_size, [=](sycl::id<1> idx) {
+               const size_t local_id = critical_indices[idx];
+               double range = devicePtrRanges[local_id];
+               double angle = devicePtrAngles[local_id];
 
-            sycl::vec<float, 4> point{range * sycl::cos(angle),
-                                      range * sycl::sin(angle), 0.0, 1.0};
-            sycl::vec<float, 2> transformed_point{0.0, 0.0};
+               sycl::vec<float, 4> point{range * sycl::cos(angle),
+                                         range * sycl::sin(angle), 0.0, 1.0};
+               sycl::vec<float, 2> transformed_point{0.0, 0.0};
 
-            for (size_t i = 0; i < 4; ++i) {
-              transformed_point[0] += x_transform[i] * point[i];
-              transformed_point[1] += y_transform[i] * point[i];
-            }
+               for (size_t i = 0; i < 4; ++i) {
+                 transformed_point[0] += x_transform[i] * point[i];
+                 transformed_point[1] += y_transform[i] * point[i];
+               }
 
-            double converted_range = sycl::length(transformed_point);
-            if (converted_range - robot_radius <= criticalDistance) {
-              // point within the zone and range is low
-              devicePtrOutput[idx] = true;
-            }
-          });
-    }).wait();
+               float converted_range = sycl::length(transformed_point);
+               if (converted_range - robot_radius <= criticalDistance) {
+                 // KERNEL_DEBUG("Converted Range 0 = %3f\n", converted_range);
+                 // point within the zone and range is low
+                 devicePtrOutput[idx] = 0.0f;
+               } else if (converted_range - robot_radius <= slowdownDistance) {
+                 devicePtrOutput[idx] =
+                     (converted_range - robot_radius - criticalDistance) /
+                     (slowdownDistance - criticalDistance);
+               } else {
+                 devicePtrOutput[idx] = 1.0f;
+               }
 
-    *m_result = false;
-    // Launch a kernel that reduces the array using a logical OR operation.
-    // If any element is true, the reduction will produce true.
-    m_q.submit([&](sycl::handler &h) {
-      auto reduction = sycl::reduction(m_result, sycl::logical_or<bool>());
-      auto devicePtrOutput = m_devicePtrOutput;
-      h.parallel_for(
-          sycl::range<1>(m_scan_in_zone), reduction,
-          [=](sycl::id<1> idx, auto &r) { r.combine(devicePtrOutput[idx]); });
-    });
+               sycl::atomic_ref<float, sycl::memory_order::relaxed,
+                                sycl::memory_scope::device,
+                                sycl::access::address_space::global_space>
+                   atomic_cost(*m_result);
+               atomic_cost.fetch_min(devicePtrOutput[idx]);
+             });
+       })
+        .wait();
 
     m_q.wait_and_throw();
 
diff --git a/src/kompass_cpp/tests/collisions_test.cpp b/src/kompass_cpp/tests/collisions_test.cpp
index fdd62ce1..b4526505 100644
--- a/src/kompass_cpp/tests/collisions_test.cpp
+++ b/src/kompass_cpp/tests/collisions_test.cpp
@@ -1,7 +1,5 @@
 #include "test.h"
 #include <Eigen/Dense>
-#include <Eigen/src/Geometry/Quaternion.h>
-#include <array>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "utils/angles.h"
 #include "utils/collision_check.h"
@@ -131,11 +129,12 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   initLaserscan(360, 10.0, scan_ranges, scan_angles);
 
   bool forward_motion = true;
-  float critical_angle = 120.0, critical_distance = 0.3;
+  float critical_angle = 160.0, critical_distance = 0.3, slowdown_distance = 0.6;
 
   CriticalZoneChecker zoneChecker(robotShapeType, robotDimensions,
                                   sensor_position_body, sensor_rotation_body,
-                                  critical_angle, critical_distance);
+                                  critical_angle, critical_distance,
+                                  slowdown_distance);
 
   LOG_INFO("Testing Emergency Stop with CPU");
 
@@ -143,21 +142,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   setLaserscanAtAngle(0.0, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles);
-  bool result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result, "Angles are behind and robot is moving forward -> "
-                      "Critical zone result should be FALSE, returned "
+  float result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(result == 1.0, "Angles are behind and robot is moving forward -> "
+                      "Critical zone result should be 1.0, returned "
                           << result);
-  if (!result) {
+  if (result == 1.0) {
     LOG_INFO("Test1 PASSED: Angles are behind and robot is moving forward");
   }
 
   // Set small ranges behind the robot
   initLaserscan(360, 10.0, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result, "Angles are in front and far and robot is moving forward "
-                      "-> Critical zone result should be FALSE, returned "
-                          << result);
-  if (!result) {
+  BOOST_TEST(result == 1.0,
+             "Angles are in front and far and robot is moving forward "
+             "-> Critical zone result should be 1.0, returned "
+                 << result);
+  if (result == 1.0) {
     LOG_INFO("Test2 PASSED: Angles are in front and robot is moving forward");
   }
 
@@ -166,21 +166,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   setLaserscanAtAngle(M_PI + 0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(M_PI - 0.1, 0.2, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(result, "Angles are in front and close and robot is moving "
-                     "forward -> Critical zone result should be TRUE, returned "
-                         << result);
-  if (result) {
+  BOOST_TEST(result == 0.0,
+             "Angles are in front and close and robot is moving "
+             "forward -> Critical zone result should be 0.0, returned "
+                 << result);
+  if (result == 0.0) {
     LOG_INFO("Test3 PASSED: Angles are in front and close and robot is moving "
              "forward");
   }
 
   forward_motion = false;
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result,
+  BOOST_TEST(result == 1.0,
              "Angles are in front and close and robot is moving "
-             "backwards-> Critical zone result should be FALSE, returned "
+             "backwards-> Critical zone result should be 1.0, returned "
                  << result);
-  if (!result) {
+  if (result == 1.0) {
     LOG_INFO("Test4 PASSED: Angles are in front and close and robot is "
              "moving backward");
   }
@@ -190,12 +191,58 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(result,
+  BOOST_TEST(result == 0.0,
              "Angles are in back and close and robot is moving "
-             "backwards -> Critical zone result should be TRUE, returned "
+             "backwards -> Critical zone result should be 0.0, returned "
                  << result);
-  if (result) {
+  if (result == 0.0) {
     LOG_INFO("Test5 PASSED: Angles are in back and close and robot is moving "
              "backwards");
   }
+
+  // Set slowdown ranges behind the robot
+  initLaserscan(360, 10.0, scan_ranges, scan_angles);
+  setLaserscanAtAngle(0.0, 1.0, scan_ranges, scan_angles);
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(
+      (result > 0.0 and result < 1.0),
+      "Angles are in back and in the slowdown zone and robot is moving "
+      "backwards -> Critical zone result should be between [0, 1], returned "
+          << result);
+  if (result > 0.0 and result < 1.0) {
+    LOG_INFO("Test6 PASSED: Angles are in back and in the slowdown zone and "
+             "robot is moving "
+             "backwards, slowdown factor = ",
+             result);
+  }
+
+  forward_motion = true;
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(result == 1.0,
+             "Angles are in back and in the slowdown zone and robot is moving "
+             "forward -> Critical zone result should be between 1.0, returned "
+                 << result);
+  if (result == 1.0) {
+    LOG_INFO("Test7 PASSED: Angles are in back and in the slowdown zone and "
+             "robot is moving "
+             "forward, slowdown factor = ",
+             result);
+  }
+
+  // Set slowdown ranges infront of the robot
+  setLaserscanAtAngle(M_PI, 0.5, scan_ranges, scan_angles);
+  setLaserscanAtAngle(M_PI + 0.1, 0.4, scan_ranges, scan_angles);
+  setLaserscanAtAngle(M_PI - 0.1, 0.4, scan_ranges, scan_angles);
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(
+      (result > 0.0 and result < 1.0),
+      "Angles are in front and in the slowdown zone and robot is moving "
+      "forward -> Critical zone result should be between [0, 1], returned "
+          << result);
+  if (result > 0.0 and result < 1.0) {
+    LOG_INFO("Test8 PASSED: Angles are in front and in the slowdown zone and "
+             "robot is moving "
+             "forward, slowdown factor = ",
+             result);
+  }
 }
diff --git a/src/kompass_cpp/tests/collisions_test_gpu.cpp b/src/kompass_cpp/tests/collisions_test_gpu.cpp
index d554d155..011424ff 100644
--- a/src/kompass_cpp/tests/collisions_test_gpu.cpp
+++ b/src/kompass_cpp/tests/collisions_test_gpu.cpp
@@ -25,11 +25,13 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   initLaserscan(360, 10.0, scan_ranges, scan_angles);
 
   bool forward_motion = true;
-  float critical_angle = 120.0, critical_distance = 0.2;
+  float critical_angle = 160.0, critical_distance = 0.3,
+        slowdown_distance = 0.6;
 
-  CriticalZoneCheckerGPU zoneChecker(
-      robotShapeType, robotDimensions, sensor_position_body,
-      sensor_rotation_body, critical_angle, critical_distance, scan_angles);
+  CriticalZoneCheckerGPU zoneChecker(robotShapeType, robotDimensions,
+                                     sensor_position_body, sensor_rotation_body,
+                                     critical_angle, critical_distance,
+                                     slowdown_distance, scan_angles);
 
   LOG_INFO("Testing Emergency Stop with GPU ");
 
@@ -37,21 +39,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   setLaserscanAtAngle(0.0, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles);
-  bool result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result, "Angles are behind and robot is moving forward -> "
-                      "Critical zone result should be FALSE, returned "
+  float result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(result == 1.0, "Angles are behind and robot is moving forward -> "
+                      "Critical zone result should be 1.0, returned "
                           << result);
-  if (!result) {
+  if (result == 1.0) {
     LOG_INFO("Test1 PASSED: Angles are behind and robot is moving forward");
   }
 
   // Set small ranges behind the robot
   initLaserscan(360, 10.0, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result, "Angles are in front and far and robot is moving forward "
-                      "-> Critical zone result should be FALSE, returned "
-                          << result);
-  if (!result) {
+  BOOST_TEST(result == 1.0,
+             "Angles are in front and far and robot is moving forward "
+             "-> Critical zone result should be 1.0, returned "
+                 << result);
+  if (result == 1.0) {
     LOG_INFO("Test2 PASSED: Angles are in front and robot is moving forward");
   }
 
@@ -60,21 +63,22 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   setLaserscanAtAngle(M_PI + 0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(M_PI - 0.1, 0.2, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(result, "Angles are in front and close and robot is moving "
-                     "forward -> Critical zone result should be TRUE, returned "
-                         << result);
-  if (result) {
+  BOOST_TEST(result == 0.0,
+             "Angles are in front and close and robot is moving "
+             "forward -> Critical zone result should be 0.0, returned "
+                 << result);
+  if (result == 0.0) {
     LOG_INFO("Test3 PASSED: Angles are in front and close and robot is moving "
              "forward");
   }
 
   forward_motion = false;
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(!result,
+  BOOST_TEST(result == 1.0,
              "Angles are in front and close and robot is moving "
-             "backwards-> Critical zone result should be FALSE, returned "
+             "backwards-> Critical zone result should be 1.0, returned "
                  << result);
-  if (!result) {
+  if (result == 1.0) {
     LOG_INFO("Test4 PASSED: Angles are in front and close and robot is "
              "moving backward");
   }
@@ -84,12 +88,58 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check_gpu) {
   setLaserscanAtAngle(0.1, 0.2, scan_ranges, scan_angles);
   setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles);
   result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
-  BOOST_TEST(result,
+  BOOST_TEST(result == 0.0,
              "Angles are in back and close and robot is moving "
-             "backwards -> Critical zone result should be TRUE, returned "
+             "backwards -> Critical zone result should be 0.0, returned "
                  << result);
-  if (result) {
+  if (result == 0.0) {
     LOG_INFO("Test5 PASSED: Angles are in back and close and robot is moving "
              "backwards");
   }
+
+  // Set slowdown ranges behind the robot
+  forward_motion = false;
+  initLaserscan(360, 10.0, scan_ranges, scan_angles);
+  setLaserscanAtAngle(0.0, 1.0, scan_ranges, scan_angles);
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST((result > 0.0f and result < 1.0f),
+             "Angles are in back and in the slowdown zone and robot is moving "
+             "backwards -> Critical zone result should be between [0, 1], returned "
+                 << result);
+  if (result > 0.0f and result < 1.0f) {
+    LOG_INFO(
+        "Test6 PASSED: Angles are in back and in the slowdown zone and robot is moving "
+        "backwards, slowdown factor = ", result);
+  }
+
+  forward_motion = true;
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(
+      result==1.0,
+      "Angles are in back and in the slowdown zone and robot is moving "
+      "forward -> Critical zone result should be between 1.0, returned "
+          << result);
+  if (result ==1.0) {
+    LOG_INFO("Test7 PASSED: Angles are in back and in the slowdown zone and "
+             "robot is moving "
+             "forward, slowdown factor = ",
+             result);
+  }
+
+  // Set slowdown ranges infront of the robot
+  setLaserscanAtAngle(M_PI, 0.5, scan_ranges, scan_angles);
+  setLaserscanAtAngle(M_PI + 0.1, 0.4, scan_ranges, scan_angles);
+  setLaserscanAtAngle(M_PI - 0.1, 0.4, scan_ranges, scan_angles);
+  result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
+  BOOST_TEST(
+      (result > 0.0 and result < 1.0),
+      "Angles are in front and in the slowdown zone and robot is moving "
+      "forward -> Critical zone result should be between [0, 1], returned "
+          << result);
+  if (result > 0.0 and result < 1.0) {
+    LOG_INFO("Test8 PASSED: Angles are in front and in the slowdown zone and "
+             "robot is moving "
+             "forward, slowdown factor = ",
+             result);
+  }
 }
diff --git a/tests/test_laserscan_emergency_stop.py b/tests/test_laserscan_emergency_stop.py
index 4afec7db..496ff871 100644
--- a/tests/test_laserscan_emergency_stop.py
+++ b/tests/test_laserscan_emergency_stop.py
@@ -135,6 +135,7 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu):
         geometry_params=np.array([robot_radius, 0.4]),
     )
     emergency_distance = 0.5
+    slowdown_distance = 1.0
     emergency_angle = 90.0
 
     large_range = 10.0
@@ -143,6 +144,7 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu):
     emergency_stop = EmergencyChecker(
         robot=robot,
         emergency_distance=emergency_distance,
+        slowdown_distance=slowdown_distance,
         emergency_angle=emergency_angle,
         sensor_position_robot=np.array([0.0, 0.0, 0.173], dtype=np.float32),
         sensor_rotation_robot=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32),
@@ -155,12 +157,12 @@ def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu):
     ).shape[0]
     laser_scan_data.ranges = np.array([large_range] * angles_size)
 
-    assert not emergency_stop.run(scan=laser_scan_data, forward=True)
+    assert emergency_stop.run(scan=laser_scan_data, forward=True) == 1.0
 
     # Add an obstacle in the critical zone in front of the robot
     laser_scan_data.ranges[0] = emergency_value
-    assert emergency_stop.run(scan=laser_scan_data, forward=True)
-    assert not emergency_stop.run(scan=laser_scan_data, forward=False)
+    assert emergency_stop.run(scan=laser_scan_data, forward=True) == 0.0
+    assert emergency_stop.run(scan=laser_scan_data, forward=False) == 1.0
 
 
 if __name__ == "__main__":

From e34401b8950ba9695ba3bb717cf37168499bb6e5 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 23 May 2025 00:17:13 +0800
Subject: [PATCH 064/118] (fix) Fixes critical zone check

---
 src/kompass_core/models.py                      |  2 ++
 src/kompass_cpp/bindings/bindings_gpu.cpp       |  2 +-
 .../src/utils/critical_zone_check.cpp           | 17 -----------------
 .../src/utils/critical_zone_check_gpu.cpp       |  9 +++------
 4 files changed, 6 insertions(+), 24 deletions(-)

diff --git a/src/kompass_core/models.py b/src/kompass_core/models.py
index 9fcd5bd6..927368d9 100644
--- a/src/kompass_core/models.py
+++ b/src/kompass_core/models.py
@@ -1163,6 +1163,7 @@ class LinearCtrlLimits(BaseAttrs):
     max_vel: float = field(validator=validators.ge(0.0))  # m/s
     max_acc: float = field(validator=validators.ge(0.0))  # m/s^2
     max_decel: float = field(validator=validators.ge(0.0))  # m/s^2
+    min_absolute_val: float = field(default=0.01, validator=validators.ge(0.0))  # m/s
 
 
 @define(kw_only=True)
@@ -1173,6 +1174,7 @@ class AngularCtrlLimits(BaseAttrs):
     max_steer: float = field(validator=validators.ge(0.0))
     max_acc: float = field(validator=validators.ge(0.0))
     max_decel: float = field(validator=validators.ge(0.0))
+    min_absolute_val: float = field(default=0.01, validator=validators.ge(0.0))  # m/s
 
 
 @define(kw_only=True)
diff --git a/src/kompass_cpp/bindings/bindings_gpu.cpp b/src/kompass_cpp/bindings/bindings_gpu.cpp
index 0b7b50a0..15f0db37 100644
--- a/src/kompass_cpp/bindings/bindings_gpu.cpp
+++ b/src/kompass_cpp/bindings/bindings_gpu.cpp
@@ -32,6 +32,6 @@ void bindings_utils_gpu(py::module_ &m) {
            py::arg("sensor_position_body"), py::arg("sensor_rotation_body"),
            py::arg("critical_angle"), py::arg("critical_distance"),
            py::arg("slowdown_distance"), py::arg("scan_angles"))
-      .def("check", &CriticalZoneChecker::check, py::arg("ranges"),
+      .def("check", &CriticalZoneCheckerGPU::check, py::arg("ranges"),
            py::arg("angles"), py::arg("forward"));
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
index cf3c92f4..095bc20f 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
@@ -109,23 +109,6 @@ float CriticalZoneChecker::check(const std::vector<double> &ranges,
         cartesianPoint = {x, y, 0.0f};
         // Apply TF
         cartesianPoint = sensor_tf_body_ * cartesianPoint;
-        auto transformation_matrix = sensor_tf_body_.matrix();
-        const std::array<float, 4> x_transform{
-            transformation_matrix(0, 0), transformation_matrix(0, 1),
-            transformation_matrix(0, 2), transformation_matrix(0, 3)};
-
-        const std::array<float, 4> y_transform{
-            transformation_matrix(1, 0), transformation_matrix(1, 1),
-            transformation_matrix(1, 2), transformation_matrix(1, 3)};
-
-        std::array<float, 4> point{x,
-                                   y, 0.0, 1.0};
-        std::array<float, 2> transformed_point{0.0, 0.0};
-
-        for (size_t i = 0; i < 4; ++i) {
-          transformed_point[0] += x_transform[i] * point[i];
-          transformed_point[1] += y_transform[i] * point[i];
-        }
 
         // check if within the zone
         converted_range = std::sqrt(std::pow(cartesianPoint.y(), 2) +
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
index c2e58f10..aea04386 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
@@ -12,10 +12,8 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
   try {
     m_q.fill(m_devicePtrOutput, false, m_scan_in_zone);
 
-    m_q.memcpy(m_devicePtrAngles, angles.data(), sizeof(double) * m_scanSize)
-        .wait();
-    m_q.memcpy(m_devicePtrRanges, ranges.data(), sizeof(double) * m_scanSize)
-        .wait();
+    m_q.memcpy(m_devicePtrAngles, angles.data(), sizeof(double) * m_scanSize);
+    m_q.memcpy(m_devicePtrRanges, ranges.data(), sizeof(double) * m_scanSize);
 
     // command scope
     m_q.submit([&](sycl::handler &h) {
@@ -81,8 +79,7 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
                    atomic_cost(*m_result);
                atomic_cost.fetch_min(devicePtrOutput[idx]);
              });
-       })
-        .wait();
+       });
 
     m_q.wait_and_throw();
 

From 970bc0e04e1d35424fcbc149e1e9af905c1dd571 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Thu, 22 May 2025 18:41:37 +0200
Subject: [PATCH 065/118] (fix) Fixes bugs in criticalzonechecker GPU

---
 .../kompass_cpp/src/utils/critical_zone_check_gpu.cpp | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
index aea04386..5908d240 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
@@ -1,7 +1,5 @@
 #include "utils/critical_zone_check_gpu.h"
 #include "utils/logger.h"
-#include <CL/sycl.hpp>
-#include <hipSYCL/sycl/libkernel/marray.hpp>
 #include <sycl/sycl.hpp>
 
 namespace Kompass {
@@ -10,7 +8,7 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
                                     const std::vector<double> &angles,
                                     const bool forward) {
   try {
-    m_q.fill(m_devicePtrOutput, false, m_scan_in_zone);
+    m_q.fill(m_devicePtrOutput, 1.0f, m_scan_in_zone);
 
     m_q.memcpy(m_devicePtrAngles, angles.data(), sizeof(double) * m_scanSize);
     m_q.memcpy(m_devicePtrRanges, ranges.data(), sizeof(double) * m_scanSize);
@@ -34,7 +32,8 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
          const auto criticalDistance = critical_distance_;
          const auto slowdownDistance = slowdown_distance_;
          size_t *critical_indices;
-         *m_result = 1.0f;
+         float* result = m_result;
+         *result = 1.0f;
          sycl::range<1> global_size;
          if (forward) {
            critical_indices = m_devicePtrForward;
@@ -62,8 +61,6 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
 
                float converted_range = sycl::length(transformed_point);
                if (converted_range - robot_radius <= criticalDistance) {
-                 // KERNEL_DEBUG("Converted Range 0 = %3f\n", converted_range);
-                 // point within the zone and range is low
                  devicePtrOutput[idx] = 0.0f;
                } else if (converted_range - robot_radius <= slowdownDistance) {
                  devicePtrOutput[idx] =
@@ -76,7 +73,7 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
                sycl::atomic_ref<float, sycl::memory_order::relaxed,
                                 sycl::memory_scope::device,
                                 sycl::access::address_space::global_space>
-                   atomic_cost(*m_result);
+                   atomic_cost(*result);
                atomic_cost.fetch_min(devicePtrOutput[idx]);
              });
        });

From 736927f721fb6ba22429de9bfc7efe0b321f8323 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 27 May 2025 00:32:57 +0800
Subject: [PATCH 066/118] (feature) Adds search to VisionDWA

---
 src/kompass_core/control/vision_dwa.py        |  2 +
 .../include/controllers/vision_dwa.h          | 80 +++++++++++++++-
 .../kompass_cpp/include/datatypes/tracking.h  |  8 +-
 .../src/controllers/vision_dwa.cpp            | 92 ++++++++++++++++---
 .../kompass_cpp/src/vision/depth_detector.cpp |  6 +-
 .../kompass_cpp/src/vision/tracker.cpp        | 11 ++-
 6 files changed, 175 insertions(+), 24 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index c3e129a0..92091e72 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -45,6 +45,8 @@ class VisionDWAConfig(DWAConfig):
         default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
     )  # Wait for target to appear again timeout (seconds), used if search is disabled
 
+    enable_search: bool = field(default=False)  # Enable or disable the search mechanism
+
     target_search_timeout: float = field(
         default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
     )  # Search timeout in seconds
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 928ec0cc..96807d94 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -170,6 +170,10 @@ class VisionDWA : public DWA {
   getTrackingCtrl(const std::optional<TrackedPose2D> &tracked_pose,
                   const Velocity2D &current_vel, const T &sensor_points) {
     if (tracked_pose.has_value()) {
+      // Reset recorded wait and search times
+      recorded_wait_time_ = 0.0;
+      recorded_search_time_ = 0.0;
+      // Generate reference to target
       Trajectory2D ref_traj;
       if(is_diff_drive_){
         ref_traj = getTrackingReferenceSegmentDiffDrive(tracked_pose.value());
@@ -196,7 +200,57 @@ class VisionDWA : public DWA {
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
-      LOG_WARNING("No tracking target is received!");
+      LOG_WARNING("No tracking target is received and DWA goal is reached!");
+      // Start Search and/or Wait if enabled
+      if (config_.enable_search()) {
+        if (recorded_search_time_ < config_.target_search_timeout()) {
+          if (search_commands_queue_.empty()) {
+            getFindTargetCmds();
+          }
+          LOG_DEBUG(
+            "Number of search commands remaining: ", search_commands_queue_.size(),
+            "recorded search time: ", recorded_search_time_);
+          // Create search command
+          Trajectory2D ref_traj(config_.control_horizon() + 1);
+          std::array<double, 3> search_command;
+          for (int i = 0; i <= config_.control_horizon(); i++) {
+            search_command = search_commands_queue_.front();
+            search_commands_queue_.pop();
+            recorded_search_time_ += config_.control_time_step();
+            ref_traj.velocities.add(i, Velocity2D(search_command[0], search_command[1], search_command[2]));
+          }
+          auto result = TrajSearchResult();
+          result.isTrajFound = true;
+          result.trajCost = 0.0;
+          result.trajectory = ref_traj;
+          return result;
+        }
+      } else {
+        if (recorded_wait_time_ < config_.target_wait_timeout()) {
+          auto timeout = config_.target_wait_timeout() - recorded_wait_time_;
+          LOG_DEBUG("Target lost, waiting to get tracked target again ... timeout in ",
+                    timeout, " seconds");
+          // Do nothing and wait
+          Trajectory2D ref_traj(config_.control_horizon() + 1);
+          for (int i = 0; i <= config_.control_horizon(); i++) {
+            recorded_wait_time_ += config_.control_time_step();
+            ref_traj.velocities.add(i, Velocity2D(0.0, 0.0, 0.0));
+          }
+          auto result = TrajSearchResult();
+          result.isTrajFound = true;
+          result.trajCost = 0.0;
+          result.trajectory = ref_traj;
+          return result;
+        }
+      }
+      // Target is lost and not recovered from search or wait
+      LOG_WARNING("Target is lost and not recovered from search or wait");
+      recorded_wait_time_ = 0.0;
+      recorded_search_time_ = 0.0;
+      // Empty the search commands queue
+      while (!search_commands_queue_.empty()) {
+        search_commands_queue_.pop();
+      }
       return TrajSearchResult();
     }
   };
@@ -219,8 +273,13 @@ class VisionDWA : public DWA {
     if (!detected_boxes.empty()) {
       if (tracker_->trackerInitialized()) {
         // Update the tracker with the detected boxes
-        tracker_->updateTracking(detected_boxes);
-        tracked_pose = tracker_->getFilteredTrackedPose2D();
+        bool tracking_updated = tracker_->updateTracking(detected_boxes);
+        if (!tracking_updated) {
+          LOG_WARNING("Tracker failed to update target with the detected boxes");
+        }
+        else{
+          tracked_pose = tracker_->getFilteredTrackedPose2D();
+        }
       } else {
         throw std::runtime_error(
             "Tracker is not initialized with an initial tracking target. Call "
@@ -253,8 +312,12 @@ class VisionDWA : public DWA {
       auto boxes_3d = detector_->get3dDetections();
       if (boxes_3d) {
         // Update the tracker with the detected boxes
-        tracker_->updateTracking(boxes_3d.value());
-        tracked_pose = tracker_->getFilteredTrackedPose2D();
+        bool tracking_updated = tracker_->updateTracking(boxes_3d.value());
+        if (!tracking_updated) {
+          LOG_WARNING("Tracker failed to update target with the detected boxes");
+        }else{
+          tracked_pose = tracker_->getFilteredTrackedPose2D();
+        }
       } else {
         LOG_WARNING("Detector failed to find 3D boxes");
       }
@@ -299,6 +362,8 @@ class VisionDWA : public DWA {
 private:
   ControlLimitsParams ctrl_limits_;
   bool is_diff_drive_;
+  float recorded_search_time_ = 0.0, recorded_wait_time_ = 0.0;
+  std::queue<std::array<double, 3>> search_commands_queue_;
   VisionDWAConfig config_;
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
   std::unique_ptr<DepthDetector> detector_;
@@ -316,6 +381,11 @@ class VisionDWA : public DWA {
 
   Trajectory2D
   getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose);
+
+  void generateSearchCommands(double total_rotation, double search_radius,
+                                int max_rotation_steps,
+                                bool enable_pause = false);
+  void getFindTargetCmds(const int last_direction = 1);
 };
 
 } // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 7f94d938..6a5dcce8 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -104,7 +104,8 @@ struct TrackedBbox3D {
     float time_step = new_box.timestamp - this->box.timestamp;
     if (time_step <= 0.0) {
       LOG_ERROR("Cannot update from new detection, invalid time step = ",
-                time_step);
+                "new_box.timestamp = ", new_box.timestamp,
+                ", old_box.timestamp = ", this->box.timestamp);
       return; // Invalid time step
     }
     // Compute velocity and acceleration based on location change
@@ -123,7 +124,7 @@ struct TrackedBbox3D {
   }
 
   TrackedBbox3D predictConstantVel(const float &dt) {
-    auto predicted_tracking = *this;
+    auto predicted_tracking = TrackedBbox3D(*this);
     predicted_tracking.box.center += predicted_tracking.vel * dt;
     predicted_tracking.yaw_vec(0) += predicted_tracking.yaw_vec(1) * dt;
     predicted_tracking.yaw_vec(2) = 0.0; // Set angular acceleration to zero
@@ -134,7 +135,7 @@ struct TrackedBbox3D {
   };
 
   TrackedBbox3D predictConstantAcc(const float &dt) {
-    auto predicted_tracking = *this;
+    auto predicted_tracking = TrackedBbox3D(*this);
     predicted_tracking.vel += this->acc * dt;
     predicted_tracking.box.center += predicted_tracking.vel * dt;
     predicted_tracking.yaw_vec(1) += predicted_tracking.yaw_vec(2) * dt;
@@ -159,6 +160,7 @@ struct TrackedBbox3D {
     box.center(0) += vel.x() * timeStep;
     box.center(1) += vel.y() * timeStep;
     this->yaw_vec(0) += this->yaw_vec(1) * timeStep;
+    box.timestamp += timeStep;
   }
 
   float distance(const float x, const float y, const float z = 0.0) const {
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 0621b9be..e9568ced 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -58,13 +58,9 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) -
-                   trajSampler->getRobotRadius();
+  float distance = tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
   distance = std::max(distance, 0.0f);
-  float psi = Angle::normalizeToMinusPiPlusPi(
-      std::atan2(tracking_pose.y() - currentState.y,
-                 tracking_pose.x() - currentState.x) -
-      currentState.yaw);
+  float psi = Angle::normalizeToMinusPiPlusPi(std::atan2(tracking_pose.y(), tracking_pose.x()));
 
   float distance_error = config_.target_distance() - distance;
   float angle_error =
@@ -76,16 +72,17 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
       std::max(0.01, config_.tolerance() * config_.target_distance());
   float angle_tolerance =
       std::max(0.01, config_.tolerance() * config_.target_orientation());
-  LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error,
-            ", distance_tolerance=", distance_tolerance,
-            ", angle_tolerance=", angle_tolerance);
+  LOG_DEBUG("distance=", distance, "distance_error=", distance_error, "psi=", psi, ", angle_error=", angle_error);
 
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
+    auto term1 = (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
+    auto term2 = - (config_.K_v() * tanh(distance_error)) / cos(angle_error);
     double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
                 (config_.K_v() * tanh(distance_error))) /
                cos(angle_error);
+    LOG_INFO("v=", v, ", term1=", term1, ", term2=", term2);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     if (std::abs(v) < config_.min_vel()) {
@@ -94,10 +91,15 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
+      auto term1_o = track_velocity_ * (tracking_pose.omega() - 2.0 * sin(psi) / distance * tracking_pose.v());
+      auto term2_o = -2.0 * v * sin(angle_error) / distance;
+      auto term3_o = - (config_.K_omega() * tanh(angle_error));
       omega = -track_velocity_ * tracking_pose.omega() -
-              2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() +
-              -2.0 * sin(angle_error) / distance * v -
+              2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() -
+              2.0 * v * sin(angle_error) / distance -
               2.0 * config_.K_omega() * tanh(angle_error);
+      LOG_INFO("omega=", omega, ", term_track_velocity=", term1_o, ", term_v=", term2_o,
+               ", term_angle_err=", term3_o);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
               2.0 * config_.K_omega() * tanh(angle_error);
@@ -108,7 +110,6 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
       omega = 0.0;
     }
     followingVel.setOmega(omega);
-    LOG_DEBUG("RETURNIN V=", v, " OMEGA=", omega);
   }
   return followingVel;
 }
@@ -245,5 +246,72 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   return ref_traj;
 };
 
+
+void VisionDWA::generateSearchCommands(double total_rotation,
+                                            double search_radius,
+                                            int max_rotation_steps,
+                                            bool enable_pause) {
+  // Calculate rotation direction and magnitude
+  double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
+  int rotation_steps = max_rotation_steps;
+  if (enable_pause) {
+    // Modify the total number of active rotation to include the pause steps
+    rotation_steps = (max_rotation_steps + config_.search_pause()) /
+                     (config_.search_pause() + 1);
+  }
+  // Angular velocity to rotate 'total_rotation' in total time steps
+  // 'rotation_steps' with dt = control_time_step
+  double rotation_value =
+      (total_rotation / rotation_steps) / config_.control_time_step();
+  rotation_value =
+      std::max(std::min(rotation_value, ctrl_limits_.omegaParams.maxOmega),
+               config_.min_vel());
+  // Generate velocity commands
+  for (int i = 0; i <= rotation_steps; i = i + 1) {
+    if (is_diff_drive_) {
+      // In-place rotation
+      search_commands_queue_.emplace(
+          std::array<double, 3>{0.0, 0.0, rotation_sign * rotation_value});
+    } else {
+      // Angular velocity based on linear velocity and radius
+      double omega_ackermann =
+          rotation_sign * ctrl_limits_.velXParams.maxVel / search_radius;
+      // Non-holonomic circular motion
+      search_commands_queue_.emplace(std::array<double, 3>{
+          ctrl_limits_.velXParams.maxVel, 0.0, omega_ackermann});
+    }
+    if (enable_pause) {
+      // Add zero commands for search pause
+      for (int j = 0; j <= config_.search_pause(); j++) {
+        search_commands_queue_.emplace(std::array<double, 3>{0.0, 0.0, 0.0});
+      }
+    }
+  }
+  return;
+}
+
+void VisionDWA::getFindTargetCmds(const int  last_direction) {
+  // Generate new search commands if starting a new search or no commands are
+  // available
+  LOG_DEBUG("Generating new search commands in direction: ", last_direction);
+
+  search_commands_queue_ = std::queue<std::array<double, 3>>();
+  int target_search_steps_max = static_cast<int>(
+      config_.target_search_timeout() / config_.control_time_step());
+  // rotate pi
+  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
+                         target_search_steps_max / 4, true);
+  // go back
+  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
+                         target_search_steps_max / 8);
+  // rotate -pi
+  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
+                         target_search_steps_max / 4, true);
+  // go back
+  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
+                         target_search_steps_max / 8);
+}
+
+
 } // namespace Control
 } // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index 758a28e0..272f885d 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -126,16 +126,16 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
   size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_;
   size_camera_frame(2) = maximum_d - minimum_d;
 
-  Eigen::Isometry3f camera_in_world = body_in_world_tf_ * camera_in_body_tf_;
+  // Eigen::Isometry3f camera_in_world = body_in_world_tf_ * camera_in_body_tf_;
 
   // Register center in the world frame
-  box3d.center = camera_in_world * center_in_camera_frame;
+  box3d.center = camera_in_body_tf_ * center_in_camera_frame;
 
   LOG_DEBUG("Got detected box in 3D world frame at :", box3d.center.x(), ", ",
             box3d.center.y(), ", ", box3d.center.z());
 
   // Transform size from camera frame to world frame
-  Eigen::Matrix3f abs_rotation = camera_in_world.linear().cwiseAbs();
+  Eigen::Matrix3f abs_rotation = camera_in_body_tf_.linear().cwiseAbs();
   box3d.size = abs_rotation * size_camera_frame;
 
   return box3d;
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index a845b48e..30c073d7 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -132,6 +132,13 @@ bool FeatureBasedBboxTracker::updateTracking(
   for (auto box : detected_boxes) {
     detected_boxes_feature_vec = extractFeatures(box);
     auto error_vec = detected_boxes_feature_vec - ref_box_features;
+    // Error vector normalization
+    // for(int i = 0; i < error_vec.size(); ++i) {
+    //   if (std::abs(ref_box_features(i)) > 0.0) {
+    //     float normalized_error = error_vec(i) / std::abs(ref_box_features(i));
+    //     error_vec(i) = normalized_error;
+    //   }
+    // }
     float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
 
     if (similarity_score > max_similarity_score) {
@@ -155,7 +162,9 @@ bool FeatureBasedBboxTracker::updateTracking(
               ", omega=", trackedBox_->yaw_vec(1), ", v=", trackedBox_->v());
     return true;
   }
-  LOG_DEBUG("Box not found");
+  LOG_DEBUG("Box not found in the detected boxes! Max similarity score = ",
+            max_similarity_score, ", min accepted = ",
+            minAcceptedSimilarityScore_);
   return false;
 }
 

From 188dcad189b13c8a386f9a108116833e025cc241 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 27 May 2025 12:50:54 +0200
Subject: [PATCH 067/118] (feature) Updates VisionDWA to use local coordinates
 and fixes tests

---
 src/kompass_core/control/vision_dwa.py        |  41 +---
 src/kompass_cpp/bindings/bindings_control.cpp |  30 ---
 .../include/controllers/vision_dwa.h          | 231 +++++++++---------
 .../kompass_cpp/include/datatypes/tracking.h  |  40 ++-
 .../include/vision/depth_detector.h           |   6 +-
 .../src/controllers/vision_dwa.cpp            |  51 ++--
 .../kompass_cpp/src/vision/depth_detector.cpp |  16 +-
 .../kompass_cpp/src/vision/tracker.cpp        |  31 ++-
 .../tests/vision_detector_test.cpp            |   2 -
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 182 ++++++--------
 .../tests/vision_tracking_test.cpp            |   2 +-
 11 files changed, 262 insertions(+), 370 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 92091e72..27bc1d7d 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -6,13 +6,11 @@
     SamplingControlResult,
 )
 from kompass_cpp.types import (
-    Bbox3D,
     Bbox2D,
     ControlCmd,
     LaserScan,
     TrajectoryVelocities2D,
     TrajectoryPath,
-    # TrackedPose2D,
 )
 from typing import Optional, List
 import numpy as np
@@ -191,32 +189,6 @@ def __init__(
     def set_camera_intrinsics(self, fx: float, fy: float, cx: float, cy: float) -> None:
         self._planner.set_camera_intrinsics(fx, fy, cx, cy)
 
-    def set_initial_tracking_3d(
-        self,
-        pose_x_img: int,
-        pose_y_img: int,
-        detected_boxes: List[Bbox3D],
-    ) -> bool:
-        """
-        Set initial tracking state
-
-        :param detected_boxes: Detected boxes
-        :type detected_boxes: List[Bbox3D]
-        """
-        try:
-            if any(detected_boxes):
-                return self._planner.set_initial_tracking(
-                    pose_x_img, pose_y_img, detected_boxes
-                )
-
-            logging.error(
-                "Could not set initial tracking state: No detections are provided"
-            )
-            return False
-        except Exception as e:
-            logging.error(f"Could not set initial tracking state: {e}")
-            return False
-
     def set_initial_tracking_2d_target(
         self,
         current_state: RobotState,
@@ -282,10 +254,8 @@ def loop_step(
         self,
         *,
         current_state: RobotState,
-        detections_3d: Optional[List[Bbox3D]] = None,
         detections_2d: Optional[List[Bbox2D]] = None,
         depth_image: Optional[np.ndarray] = None,
-        # tracked_pose: Optional[TrackedPose2D] = None,
         laser_scan: Optional[LaserScanData] = None,
         point_cloud: Optional[List[np.ndarray]] = None,
         local_map: Optional[np.ndarray] = None,
@@ -327,14 +297,9 @@ def loop_step(
             return False
 
         try:
-            if detections_3d is not None:
-                self._result = self._planner.get_tracking_ctrl(
-                    detections_3d, current_velocity, sensor_data
-                )
-            else:
-                self._result = self._planner.get_tracking_ctrl(
-                    depth_image, detections_2d, current_velocity, sensor_data
-                )
+            self._result = self._planner.get_tracking_ctrl(
+                depth_image, detections_2d, current_velocity, sensor_data
+            )
 
         except Exception as e:
             logging.error(f"Could not find velocity command: {e}")
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index caa10e14..866572a6 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -282,36 +282,6 @@ void bindings_control(py::module_ &m) {
                &Control::VisionDWA::setInitialTracking),
            py::arg("aligned_depth_image"), py::arg("target_box_2d"),
            py::arg("robot_orientation") = 0.0)
-      .def("get_tracking_ctrl",
-           py::overload_cast<const std::vector<Bbox3D> &,
-                             const Control::Velocity2D &,
-                             const std::vector<Eigen::Vector3f> &>(
-               &Control::VisionDWA::getTrackingCtrl<
-                   std::vector<Eigen::Vector3f>>),
-           py::arg("detected_boxes"), py::arg("robot_velocity"),
-           py::arg("sensor_data"))
-      .def("get_tracking_ctrl",
-           py::overload_cast<const std::vector<Bbox3D> &,
-                             const Control::Velocity2D &,
-                             const Control::LaserScan &>(
-               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
-           py::arg("detected_boxes"), py::arg("robot_velocity"),
-           py::arg("sensor_data"))
-      .def("get_tracking_ctrl",
-           py::overload_cast<const std::optional<Control::TrackedPose2D> &,
-                             const Control::Velocity2D &,
-                             const std::vector<Eigen::Vector3f> &>(
-               &Control::VisionDWA::getTrackingCtrl<
-                   std::vector<Eigen::Vector3f>>),
-           py::arg("tracked_pose"), py::arg("robot_velocity"),
-           py::arg("sensor_data"))
-      .def("get_tracking_ctrl",
-           py::overload_cast<const std::optional<Control::TrackedPose2D> &,
-                             const Control::Velocity2D &,
-                             const Control::LaserScan &>(
-               &Control::VisionDWA::getTrackingCtrl<Control::LaserScan>),
-           py::arg("tracked_pose"), py::arg("robot_velocity"),
-           py::arg("sensor_data"))
       .def("get_tracking_ctrl",
            py::overload_cast<const Eigen::MatrixX<unsigned short> &,
                              const std::vector<Bbox2D> &,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 96807d94..7b1bafa3 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -14,6 +14,7 @@
 #include <cmath>
 #include <memory>
 #include <optional>
+#include <queue>
 #include <vector>
 
 namespace Kompass {
@@ -48,7 +49,7 @@ class VisionDWA : public DWA {
                     "Bearing angle to maintain with the target (rad)"));
       addParameter(
           "track_velocity",
-          Parameter(true,
+          Parameter(false,
                     "Track the linear and angular velocity of the target"));
       // Search Parameters
       addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
@@ -147,114 +148,6 @@ class VisionDWA : public DWA {
                            const float principal_point_x,
                            const float principal_point_y);
 
-  /**
-   * @brief Get the Pure Tracking Control Command
-   *
-   * @param tracking_pose
-   * @return Velocity2D
-   */
-  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
-
-  /**
-   * @brief Get the Tracking Control Result based on object tracking and DWA
-   * sampling by direct following of the tracked target
-   *
-   * @tparam T
-   * @param tracking_pose
-   * @param current_vel
-   * @param sensor_points
-   * @return Control::TrajSearchResult
-   */
-  template <typename T>
-  Control::TrajSearchResult
-  getTrackingCtrl(const std::optional<TrackedPose2D> &tracked_pose,
-                  const Velocity2D &current_vel, const T &sensor_points) {
-    if (tracked_pose.has_value()) {
-      // Reset recorded wait and search times
-      recorded_wait_time_ = 0.0;
-      recorded_search_time_ = 0.0;
-      // Generate reference to target
-      Trajectory2D ref_traj;
-      if(is_diff_drive_){
-        ref_traj = getTrackingReferenceSegmentDiffDrive(tracked_pose.value());
-      }
-      else{
-        ref_traj = getTrackingReferenceSegment(tracked_pose.value());
-      }
-
-      TrajSearchResult result;
-      result.isTrajFound = true;
-      result.trajCost = 0.0;
-      result.trajectory = ref_traj;
-      latest_velocity_command_ = ref_traj.velocities.getFront();
-      // ---------------------------------------------------------------
-      // Update reference to use in case goal is lost
-      auto referenceToTarget =
-          Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z,
-                     config_.prediction_horizon());
-      this->setCurrentPath(referenceToTarget, false);
-      // ---------------------------------------------------------------
-      return result;
-    }
-    if (!isGoalReached()) {
-      // The tracking sample has collisions -> use DWA-like sampling and control
-      return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    } else {
-      LOG_WARNING("No tracking target is received and DWA goal is reached!");
-      // Start Search and/or Wait if enabled
-      if (config_.enable_search()) {
-        if (recorded_search_time_ < config_.target_search_timeout()) {
-          if (search_commands_queue_.empty()) {
-            getFindTargetCmds();
-          }
-          LOG_DEBUG(
-            "Number of search commands remaining: ", search_commands_queue_.size(),
-            "recorded search time: ", recorded_search_time_);
-          // Create search command
-          Trajectory2D ref_traj(config_.control_horizon() + 1);
-          std::array<double, 3> search_command;
-          for (int i = 0; i <= config_.control_horizon(); i++) {
-            search_command = search_commands_queue_.front();
-            search_commands_queue_.pop();
-            recorded_search_time_ += config_.control_time_step();
-            ref_traj.velocities.add(i, Velocity2D(search_command[0], search_command[1], search_command[2]));
-          }
-          auto result = TrajSearchResult();
-          result.isTrajFound = true;
-          result.trajCost = 0.0;
-          result.trajectory = ref_traj;
-          return result;
-        }
-      } else {
-        if (recorded_wait_time_ < config_.target_wait_timeout()) {
-          auto timeout = config_.target_wait_timeout() - recorded_wait_time_;
-          LOG_DEBUG("Target lost, waiting to get tracked target again ... timeout in ",
-                    timeout, " seconds");
-          // Do nothing and wait
-          Trajectory2D ref_traj(config_.control_horizon() + 1);
-          for (int i = 0; i <= config_.control_horizon(); i++) {
-            recorded_wait_time_ += config_.control_time_step();
-            ref_traj.velocities.add(i, Velocity2D(0.0, 0.0, 0.0));
-          }
-          auto result = TrajSearchResult();
-          result.isTrajFound = true;
-          result.trajCost = 0.0;
-          result.trajectory = ref_traj;
-          return result;
-        }
-      }
-      // Target is lost and not recovered from search or wait
-      LOG_WARNING("Target is lost and not recovered from search or wait");
-      recorded_wait_time_ = 0.0;
-      recorded_search_time_ = 0.0;
-      // Empty the search commands queue
-      while (!search_commands_queue_.empty()) {
-        search_commands_queue_.pop();
-      }
-      return TrajSearchResult();
-    }
-  };
-
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
    * sampling by searching a set of given detections
@@ -307,7 +200,6 @@ class VisionDWA : public DWA {
     std::optional<TrackedPose2D> tracked_pose = std::nullopt;
     if (!detected_boxes_2d.empty()) {
       // Send current state to the detector
-      detector_->updateState(currentState);
       detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
       auto boxes_3d = detector_->get3dDetections();
       if (boxes_3d) {
@@ -382,10 +274,129 @@ class VisionDWA : public DWA {
   Trajectory2D
   getTrackingReferenceSegmentDiffDrive(const TrackedPose2D &tracking_pose);
 
+  /**
+   * @brief Get the Pure Tracking Control Command
+   *
+   * @param tracking_pose
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
+
+  /**
+   * @brief Get the Pure Tracking Ctrl object
+   *
+   * @param distance_erro
+   * @param angle_error
+   * @return Velocity2D
+   */
+  Velocity2D getPureTrackingCtrl(const float distance_erro, const float angle_error);
+
   void generateSearchCommands(double total_rotation, double search_radius,
                                 int max_rotation_steps,
                                 bool enable_pause = false);
   void getFindTargetCmds(const int last_direction = 1);
+
+  /**
+   * @brief Get the Tracking Control Result based on object tracking and DWA
+   * sampling by direct following of the tracked target in local frame
+   *
+   * @tparam T
+   * @param tracking_pose
+   * @param current_vel
+   * @param sensor_points
+   * @return Control::TrajSearchResult
+   */
+  template <typename T>
+  Control::TrajSearchResult
+  getTrackingCtrl(const std::optional<TrackedPose2D> &tracked_pose,
+                  const Velocity2D &current_vel, const T &sensor_points) {
+    if (tracked_pose.has_value()) {
+      // Reset recorded wait and search times
+      recorded_wait_time_ = 0.0;
+      recorded_search_time_ = 0.0;
+      // Generate reference to target
+      Trajectory2D ref_traj;
+      if (is_diff_drive_) {
+        ref_traj = getTrackingReferenceSegmentDiffDrive(tracked_pose.value());
+      } else {
+        ref_traj = getTrackingReferenceSegment(tracked_pose.value());
+      }
+
+      TrajSearchResult result;
+      result.isTrajFound = true;
+      result.trajCost = 0.0;
+      result.trajectory = ref_traj;
+      latest_velocity_command_ = ref_traj.velocities.getFront();
+      // ---------------------------------------------------------------
+      // Update reference to use in case goal is lost
+      auto referenceToTarget =
+          Path::Path(ref_traj.path.x, ref_traj.path.y, ref_traj.path.z,
+                     config_.prediction_horizon());
+      this->setCurrentPath(referenceToTarget, false);
+      // ---------------------------------------------------------------
+      return result;
+    }
+    if (!isGoalReached()) {
+      // The tracking sample has collisions -> use DWA-like sampling and control
+      return this->computeVelocityCommandsSet(current_vel, sensor_points);
+    } else {
+      LOG_WARNING("No tracking target is received and DWA goal is reached!");
+      // Start Search and/or Wait if enabled
+      if (config_.enable_search()) {
+        if (recorded_search_time_ < config_.target_search_timeout()) {
+          if (search_commands_queue_.empty()) {
+            getFindTargetCmds();
+          }
+          LOG_DEBUG("Number of search commands remaining: ",
+                    search_commands_queue_.size(),
+                    "recorded search time: ", recorded_search_time_);
+          // Create search command
+          Trajectory2D ref_traj(config_.control_horizon() + 1);
+          std::array<double, 3> search_command;
+          for (int i = 0; i <= config_.control_horizon(); i++) {
+            search_command = search_commands_queue_.front();
+            search_commands_queue_.pop();
+            recorded_search_time_ += config_.control_time_step();
+            ref_traj.velocities.add(i, Velocity2D(search_command[0],
+                                                  search_command[1],
+                                                  search_command[2]));
+          }
+          auto result = TrajSearchResult();
+          result.isTrajFound = true;
+          result.trajCost = 0.0;
+          result.trajectory = ref_traj;
+          return result;
+        }
+      } else {
+        if (recorded_wait_time_ < config_.target_wait_timeout()) {
+          auto timeout = config_.target_wait_timeout() - recorded_wait_time_;
+          LOG_DEBUG("Target lost, waiting to get tracked target again ... "
+                    "timeout in ",
+                    timeout, " seconds");
+          // Do nothing and wait
+          Trajectory2D ref_traj(config_.control_horizon() + 1);
+          for (int i = 0; i <= config_.control_horizon(); i++) {
+            recorded_wait_time_ += config_.control_time_step();
+            ref_traj.velocities.add(i, Velocity2D(0.0, 0.0, 0.0));
+          }
+          auto result = TrajSearchResult();
+          result.isTrajFound = true;
+          result.trajCost = 0.0;
+          result.trajectory = ref_traj;
+          return result;
+        }
+      }
+      // Target is lost and not recovered from search or wait
+      LOG_WARNING("Target is lost and not recovered from search or wait");
+      recorded_wait_time_ = 0.0;
+      recorded_search_time_ = 0.0;
+      // Empty the search commands queue
+      while (!search_commands_queue_.empty()) {
+        search_commands_queue_.pop();
+      }
+      return TrajSearchResult();
+    }
+  };
 };
 
 } // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 6a5dcce8..4395a597 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -84,14 +84,11 @@ struct TrackedBbox3D {
   Eigen::Vector3f vel = {0.0, 0.0, 0.0};
   Eigen::Vector3f acc = {0.0, 0.0, 0.0};
   int unique_id = 0;
-  Eigen::Vector3f yaw_vec = {0.0, 0.0,
-                             0.0}; // To track yaw, omega and angular acc
 
   TrackedBbox3D(const Bbox3D &box) : box(box){};
 
   void setState(const Eigen::Matrix<float, 9, 1> &state_vector) {
     this->box.center = {state_vector(0), state_vector(1), 0.0f};
-    this->yaw_vec = {state_vector(2), state_vector(5), state_vector(8)};
     this->vel = {state_vector(3), state_vector(4), 0.0f};
     this->acc = {state_vector(6), state_vector(7), 0.0f};
   };
@@ -102,32 +99,24 @@ struct TrackedBbox3D {
 
   void updateFromNewDetection(const Bbox3D &new_box) {
     float time_step = new_box.timestamp - this->box.timestamp;
+    Eigen::Vector3f new_vel;
     if (time_step <= 0.0) {
-      LOG_ERROR("Cannot update from new detection, invalid time step = ",
-                "new_box.timestamp = ", new_box.timestamp,
-                ", old_box.timestamp = ", this->box.timestamp);
-      return; // Invalid time step
+      LOG_ERROR("Box updated with invalid time step, Velocity wil be reset to zero.");
+      this->vel = {0.0, 0.0, 0.0};
+      this->acc = {0.0, 0.0, 0.0};
+    }else{
+      // Compute velocity and acceleration based on location change
+      Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
+      this->acc = (new_vel - this->vel) / time_step;
+      this->vel = new_vel;
     }
-    // Compute velocity and acceleration based on location change
-    Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
-    this->acc = (new_vel - this->vel) / time_step;
-    this->vel = new_vel;
     // Update
     setfromBox(new_box);
-    // New orientation (yaw) based on new velocity
-    auto new_yaw = std::atan2(new_vel(1), new_vel(0));
-    // Orientation difference
-    auto new_omega = (new_yaw - this->yaw_vec(0)) / time_step;
-    this->yaw_vec(2) = (new_omega - this->yaw_vec(1)) / time_step;
-    this->yaw_vec(1) = new_omega;
-    this->yaw_vec(0) = new_yaw;
   }
 
   TrackedBbox3D predictConstantVel(const float &dt) {
     auto predicted_tracking = TrackedBbox3D(*this);
     predicted_tracking.box.center += predicted_tracking.vel * dt;
-    predicted_tracking.yaw_vec(0) += predicted_tracking.yaw_vec(1) * dt;
-    predicted_tracking.yaw_vec(2) = 0.0; // Set angular acceleration to zero
     // Set acceleration to zero for constant velocity prediction
     predicted_tracking.acc = {0.0, 0.0, 0.0};
     predicted_tracking.box.timestamp += dt;
@@ -138,8 +127,6 @@ struct TrackedBbox3D {
     auto predicted_tracking = TrackedBbox3D(*this);
     predicted_tracking.vel += this->acc * dt;
     predicted_tracking.box.center += predicted_tracking.vel * dt;
-    predicted_tracking.yaw_vec(1) += predicted_tracking.yaw_vec(2) * dt;
-    predicted_tracking.yaw_vec(0) += predicted_tracking.yaw_vec(1) * dt;
     predicted_tracking.box.timestamp += dt;
     return predicted_tracking;
   };
@@ -150,16 +137,17 @@ struct TrackedBbox3D {
 
   float y() const { return box.center.y(); };
 
-  float yaw() const { return this->yaw_vec(0); };
+  float yaw() const { return std::atan2(this->vel(1), this->vel(0)); };
 
-  float omega() const { return yaw_vec(1); };
+  float omega() const {return 0.0;}
+
+  float ang_acc() const { return 0.0; }
 
   float timestamp() const { return box.timestamp; };
 
   void update(const float timeStep) {
     box.center(0) += vel.x() * timeStep;
     box.center(1) += vel.y() * timeStep;
-    this->yaw_vec(0) += this->yaw_vec(1) * timeStep;
     box.timestamp += timeStep;
   }
 
@@ -170,7 +158,7 @@ struct TrackedBbox3D {
 
   Control::TrackedPose2D getTrackedPose() const {
     return Control::TrackedPose2D(box.center.x(), box.center.y(), yaw(),
-                                  vel.x(), vel.y(), omega());
+                                  vel.x(), vel.y(), 0.0);
   }
 };
 
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
index ac460c84..6b1bcc6e 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -25,10 +25,6 @@ class DepthDetector {
                 const Eigen::Vector2f &principal_point,
                 const float depth_conversion_factor = 1e-3);
 
-  void updateState(const Path::State& current_state);
-
-  void updateState(const Eigen::Isometry3f &robot_tf);
-
   void updateBoxes(const Eigen::MatrixX<unsigned short> aligned_depth_img,
                    const std::vector<Bbox2D> &detections);
 
@@ -38,7 +34,7 @@ class DepthDetector {
   float cx_, cy_, fx_, fy_;         // Depth Image camera intrinsics
   float minDepth_, maxDepth_, depthConversionFactor_;
   Eigen::MatrixX<unsigned short> alignedDepthImg_;
-  Eigen::Isometry3f camera_in_body_tf_, body_in_world_tf_;
+  Eigen::Isometry3f camera_in_body_tf_;
   std::unique_ptr<std::vector<Bbox3D>> boxes_;
 
   std::optional<Bbox3D> convert2Dboxto3Dbox(const Bbox2D &box2d);
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index e9568ced..82097347 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -58,31 +58,37 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance = tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
+  float distance =
+      tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
   distance = std::max(distance, 0.0f);
-  float psi = Angle::normalizeToMinusPiPlusPi(std::atan2(tracking_pose.y(), tracking_pose.x()));
+  float psi = Angle::normalizeToMinusPiPlusPi(
+      std::atan2(tracking_pose.y(), tracking_pose.x()));
 
   float distance_error = config_.target_distance() - distance;
   float angle_error =
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
-  // LOG_DEBUG("Distance to target", distance, ", ang_error=", angle_error);
-
   float distance_tolerance =
       std::max(0.01, config_.tolerance() * config_.target_distance());
   float angle_tolerance =
       std::max(0.01, config_.tolerance() * config_.target_orientation());
-  LOG_DEBUG("distance=", distance, "distance_error=", distance_error, "psi=", psi, ", angle_error=", angle_error);
+
+  LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error);
 
   Velocity2D followingVel;
   if (abs(distance_error) > distance_tolerance or
       abs(angle_error) > angle_tolerance) {
-    auto term1 = (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
-    auto term2 = - (config_.K_v() * tanh(distance_error)) / cos(angle_error);
+    auto term1 =
+        (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
+    auto term2 = -(config_.K_v() * ctrl_limits_.velXParams.maxVel *
+                   tanh(distance_error)) /
+                 cos(angle_error);
     double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
-                (config_.K_v() * tanh(distance_error))) /
+                (config_.K_v() * ctrl_limits_.velXParams.maxVel *
+                 tanh(distance_error))) /
                cos(angle_error);
-    LOG_INFO("v=", v, ", term1=", term1, ", term2=", term2);
+    LOG_DEBUG("v=", v, ", track_velocity_term=", term1,
+              ", dist_error_term=", term2);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     if (std::abs(v) < config_.min_vel()) {
@@ -91,18 +97,22 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
-      auto term1_o = track_velocity_ * (tracking_pose.omega() - 2.0 * sin(psi) / distance * tracking_pose.v());
+      auto term1_o =
+          track_velocity_ * (tracking_pose.omega() -
+                             2.0 * sin(psi) / distance * tracking_pose.v());
       auto term2_o = -2.0 * v * sin(angle_error) / distance;
-      auto term3_o = - (config_.K_omega() * tanh(angle_error));
+      auto term3_o = -(config_.K_omega() * tanh(angle_error));
       omega = -track_velocity_ * tracking_pose.omega() -
               2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() -
               2.0 * v * sin(angle_error) / distance -
-              2.0 * config_.K_omega() * tanh(angle_error);
-      LOG_INFO("omega=", omega, ", term_track_velocity=", term1_o, ", term_v=", term2_o,
-               ", term_angle_err=", term3_o);
+              2.0 * config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
+                  tanh(angle_error);
+      LOG_DEBUG("omega=", omega, ", term_track_velocity=", term1_o,
+                ", term_v=", term2_o, ", term_angle_err=", term3_o);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
-              2.0 * config_.K_omega() * tanh(angle_error);
+              2.0 * config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
+                  tanh(angle_error);
     }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
@@ -154,7 +164,6 @@ bool VisionDWA::setInitialTracking(
         "'VisionDWA::setCameraIntrinsics' first");
   }
   // Send current state to the detector
-  detector_->updateState(currentState);
   detector_->updateBoxes(aligned_depth_image, {target_box_2d});
   auto boxes_3d = detector_->get3dDetections();
   if (!boxes_3d) {
@@ -246,11 +255,10 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   return ref_traj;
 };
 
-
 void VisionDWA::generateSearchCommands(double total_rotation,
-                                            double search_radius,
-                                            int max_rotation_steps,
-                                            bool enable_pause) {
+                                       double search_radius,
+                                       int max_rotation_steps,
+                                       bool enable_pause) {
   // Calculate rotation direction and magnitude
   double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
   int rotation_steps = max_rotation_steps;
@@ -290,7 +298,7 @@ void VisionDWA::generateSearchCommands(double total_rotation,
   return;
 }
 
-void VisionDWA::getFindTargetCmds(const int  last_direction) {
+void VisionDWA::getFindTargetCmds(const int last_direction) {
   // Generate new search commands if starting a new search or no commands are
   // available
   LOG_DEBUG("Generating new search commands in direction: ", last_direction);
@@ -312,6 +320,5 @@ void VisionDWA::getFindTargetCmds(const int  last_direction) {
                          target_search_steps_max / 8);
 }
 
-
 } // namespace Control
 } // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index 272f885d..adefed40 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -26,7 +26,7 @@ DepthDetector::DepthDetector(
     const Eigen::Isometry3f &camera_in_body_tf,
     const Eigen::Vector2f &focal_length, const Eigen::Vector2f &principal_point,
     const float depth_conversion_factor) { // Range of interest for depth values
-                                           // in meters
+                                   // in meters
   minDepth_ = depth_range(0);
   maxDepth_ = depth_range(1);
   // Factor to convert depth image data to meters (in ROS2 its given in mm ->
@@ -34,7 +34,6 @@ DepthDetector::DepthDetector(
   depthConversionFactor_ = depth_conversion_factor;
   // Set camera tf
   camera_in_body_tf_ = camera_in_body_tf;
-  body_in_world_tf_ = Eigen::Isometry3f::Identity();
 
   // Set camera  intrinsic parameters
   fx_ = focal_length.x();
@@ -50,15 +49,6 @@ std::optional<std::vector<Bbox3D>> DepthDetector::get3dDetections() const {
   return std::nullopt;
 }
 
-void DepthDetector::updateState(const Path::State &current_state) {
-
-  body_in_world_tf_ = getTransformation(current_state);
-}
-
-void DepthDetector::updateState(const Eigen::Isometry3f &robot_tf) {
-  body_in_world_tf_ = robot_tf;
-}
-
 void DepthDetector::updateBoxes(
     const Eigen::MatrixX<unsigned short> aligned_depth_img,
     const std::vector<Bbox2D> &detections) {
@@ -126,12 +116,10 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
   size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_;
   size_camera_frame(2) = maximum_d - minimum_d;
 
-  // Eigen::Isometry3f camera_in_world = body_in_world_tf_ * camera_in_body_tf_;
-
   // Register center in the world frame
   box3d.center = camera_in_body_tf_ * center_in_camera_frame;
 
-  LOG_DEBUG("Got detected box in 3D world frame at :", box3d.center.x(), ", ",
+  LOG_DEBUG("Got detected box in 3D coordinates at :", box3d.center.x(), ", ",
             box3d.center.y(), ", ", box3d.center.z());
 
   // Transform size from camera frame to world frame
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index 30c073d7..e7f37629 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -46,13 +46,13 @@ bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   state_vec.resize(StateSize);
   state_vec(0) = bBox.box.center[0]; // x
   state_vec(1) = bBox.box.center[1]; // y
-  state_vec(2) = bBox.yaw_vec(0);    // yaw
+  state_vec(2) = bBox.yaw();    // yaw
   state_vec(3) = bBox.vel[0];        // vx
   state_vec(4) = bBox.vel[1];        // vy
-  state_vec(5) = bBox.yaw_vec(1);    // omega
+  state_vec(5) = bBox.omega();    // omega
   state_vec(6) = bBox.acc[0];        // ax
   state_vec(7) = bBox.acc[1];        // ay
-  state_vec(8) = bBox.yaw_vec(2);    // a_yaw
+  state_vec(8) = bBox.ang_acc();    // a_yaw
   stateKalmanFilter_->setInitialState(state_vec);
   return true;
 }
@@ -61,7 +61,6 @@ bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox,
                                                  const float yaw) {
   LOG_DEBUG("Setting initial tracked box");
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
-  trackedBox_->yaw_vec(0) = yaw;
   Eigen::Matrix<float, StateSize, 1> state_vec =
       Eigen::Matrix<float, StateSize, 1>::Zero();
   state_vec(0) = bBox.center.x();
@@ -105,13 +104,13 @@ void FeatureBasedBboxTracker::updateTrackedBoxState(const int numberSteps) {
   measurement.resize(StateSize, 1);
   measurement(0) = trackedBox_->box.center.x();
   measurement(1) = trackedBox_->box.center.y();
-  measurement(2) = trackedBox_->yaw_vec(0);
+  measurement(2) = trackedBox_->yaw();
   measurement(3) = trackedBox_->vel.x();
   measurement(4) = trackedBox_->vel.y();
-  measurement(5) = trackedBox_->yaw_vec(1);
+  measurement(5) = trackedBox_->omega();
   measurement(6) = trackedBox_->acc.x();
   measurement(7) = trackedBox_->acc.y();
-  measurement(8) = trackedBox_->yaw_vec(2);
+  measurement(8) = trackedBox_->ang_acc();
   stateKalmanFilter_->estimate(measurement, numberSteps);
 }
 
@@ -131,14 +130,13 @@ bool FeatureBasedBboxTracker::updateTracking(
   size_t similar_box_idx = 0, count = 0;
   for (auto box : detected_boxes) {
     detected_boxes_feature_vec = extractFeatures(box);
-    auto error_vec = detected_boxes_feature_vec - ref_box_features;
+    FeaturesVector error_vec = detected_boxes_feature_vec - ref_box_features;
     // Error vector normalization
-    // for(int i = 0; i < error_vec.size(); ++i) {
-    //   if (std::abs(ref_box_features(i)) > 0.0) {
-    //     float normalized_error = error_vec(i) / std::abs(ref_box_features(i));
-    //     error_vec(i) = normalized_error;
-    //   }
-    // }
+    for(int i = 0; i < error_vec.size(); ++i) {
+      if (std::abs(ref_box_features(i)) > 0.0) {
+        error_vec(i) = error_vec(i) / std::abs(ref_box_features(i));
+      }
+    }
     float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
 
     if (similarity_score > max_similarity_score) {
@@ -147,6 +145,7 @@ bool FeatureBasedBboxTracker::updateTracking(
     }
     count++;
   }
+  LOG_DEBUG("Max similarity score = ", max_similarity_score);
   if (max_similarity_score > minAcceptedSimilarityScore_) {
     // Update raw tracking
     int number_steps = std::max(static_cast<int>(dt / timeStep_), 1);
@@ -157,9 +156,7 @@ bool FeatureBasedBboxTracker::updateTracking(
     updateTrackedBoxState(number_steps);
     LOG_DEBUG("Update after ", number_steps,
               " timer steps, Box Now updated to: ", trackedBox_->box.center.x(),
-              ", ", trackedBox_->box.center.y(),
-              ", yaw=", trackedBox_->yaw_vec(0),
-              ", omega=", trackedBox_->yaw_vec(1), ", v=", trackedBox_->v());
+              ", ", trackedBox_->box.center.y(), ", v=", trackedBox_->v());
     return true;
   }
   LOG_DEBUG("Box not found in the detected boxes! Max similarity score = ",
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
index 42615c06..744929b8 100644
--- a/src/kompass_cpp/tests/vision_detector_test.cpp
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -4,7 +4,6 @@
 #include "utils/transformation.h"
 #include "vision/depth_detector.h"
 #include <Eigen/Dense>
-#include <Eigen/src/Geometry/Quaternion.h>
 #include <opencv2/opencv.hpp>
 #include <stdexcept>
 #define BOOST_TEST_MODULE KOMPASS TESTS
@@ -49,7 +48,6 @@ struct DepthDetectorTestConfig {
         std::make_unique<DepthDetector>(depth_range, cam_in_body, focal_length,
                                         principal_point, depth_conv_factor);
 
-    detector->updateState(current_state);
     cv_img = cv::imread(image_filename, cv::IMREAD_GRAYSCALE);
 
     if (cv_img.empty()) {
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 0e233f52..e7fc7522 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -6,10 +6,9 @@
 #include "test.h"
 #include "utils/cost_evaluator.h"
 #include "utils/logger.h"
-#include <Eigen/src/Geometry/Quaternion.h>
+#include "utils/transformation.h"
 #include <memory>
 #include <opencv2/opencv.hpp>
-#include <optional>
 #define BOOST_TEST_MODULE KOMPASS TESTS
 #include "json_export.h"
 #include <boost/dll/runtime_symbol_info.hpp> // for program_location
@@ -75,11 +74,11 @@ struct VisionDWATestConfig {
   // Constructor to initialize the struct
   VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
                       const double timeStep = 0.1,
-                      const int predictionHorizon = 10,
-                      const int controlHorizon = 1,
+                      const int predictionHorizon = 20,
+                      const int controlHorizon = 2,
                       const int maxLinearSamples = 20,
                       const int maxAngularSamples = 20,
-                      const float maxVel = 1.0, const float maxOmega = 4.0,
+                      const float maxVel = 2.0, const float maxOmega = 4.0,
                       const int maxNumThreads = 1,
                       const double reference_path_distance_weight = 1.0,
                       const double goal_distance_weight = 1.0,
@@ -108,11 +107,13 @@ struct VisionDWATestConfig {
     config.setParameter("control_time_step", timeStep);
     config.setParameter("control_horizon", controlHorizon);
     config.setParameter("prediction_horizon", predictionHorizon);
+    config.setParameter("track_velocity", false);
+    // config.setParameter("target_orientation", 0.2);
 
     // For depth config
     // Body to camera tf from robot of test pictures
     auto body_to_link_tf =
-        getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
+    getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
                           Eigen::Vector3f{0.32f, 0.0209f, 0.3f});
 
     auto link_to_cam_tf =
@@ -209,7 +210,7 @@ struct VisionDWATestConfig {
   }
 
   bool run_test(const int numPointsPerTrajectory, std::string pltFileName,
-                bool with_tracker, bool simulate_obstacle=false) {
+                 bool simulate_obstacle=false) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
     Control::TrajectoryVelocities2D simulated_velocities(
         numPointsPerTrajectory);
@@ -218,10 +219,10 @@ struct VisionDWATestConfig {
     Control::Velocity2D cmd;
     Control::TrajSearchResult result;
 
-    if (with_tracker) {
-      controller->setInitialTracking(ref_point_img(0),
+
+    controller->setInitialTracking(ref_point_img(0),
                                      ref_point_img(1), detected_boxes);
-    }
+
 
     int step = 0;
     while (step < numPointsPerTrajectory) {
@@ -232,20 +233,28 @@ struct VisionDWATestConfig {
       LOG_INFO("Target center at: ", tracked_pose.x(), ", ", tracked_pose.y(),
                ", ", tracked_pose.yaw());
       LOG_INFO("Robot at: ", point.x(), ", ", point.y());
-      auto seen_boxes = detected_boxes;
-      std::optional<Control::TrackedPose2D> seen_target = tracked_pose;
+
+      // Transform boxes to local coordinates
+      std::vector<Bbox3D> boxes_local_coordinates;
+      Eigen::Isometry3f world_in_robot_tf = getTransformation(robotState).inverse();
+      Eigen::Matrix3f abs_rotation = world_in_robot_tf.linear().cwiseAbs();
+      for (auto &box : detected_boxes) {
+        // Transform the box to the robot frame
+        Bbox3D box_local(box);
+        box_local.center = world_in_robot_tf * box.center;
+        box_local.size = abs_rotation * box.size;
+        boxes_local_coordinates.push_back(box_local);
+      }
+
+      auto seen_boxes = boxes_local_coordinates;
       // Simulate lost of target around obstacle
-      if (simulate_obstacle and robotState.y > 0.1 and robotState.y < 0.3) {
+      if (simulate_obstacle and robotState.y > 0.2 and robotState.y < 0.4) {
         LOG_INFO("Sending EMPTY BOXES NOW!!!!!");
         seen_boxes = {};
-        seen_target = std::nullopt;
-      }
-      if (with_tracker) {
-        result = controller->getTrackingCtrl(seen_boxes, cmd, cloud);
-      } else {
-        result = controller->getTrackingCtrl(seen_target, cmd, cloud);
       }
 
+      result = controller->getTrackingCtrl(seen_boxes, cmd, cloud);
+
       if (result.isTrajFound) {
         LOG_INFO(FMAG("STEP: "), step,
                  FMAG(", Found best trajectory with cost: "), result.trajCost);
@@ -257,21 +266,27 @@ struct VisionDWATestConfig {
                     RST);
           return false;
         }
-        cmd.setVx(controller->getLinearVelocityCmdX());
-        cmd.setVy(controller->getLinearVelocityCmdY());
-        cmd.setOmega(controller->getAngularVelocityCmd());
-
-        LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
-                  BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
-                  BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
-                  BOLD(FBLU("}")));
-
-        LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, cmd.vx(), RST,
-                  BOLD(FGRN(", Vy: ")), KGRN, cmd.vy(), RST,
-                  BOLD(FGRN(", Omega: ")), KGRN, cmd.omega(), RST,
-                  BOLD(FGRN("}")));
+        auto velocities = result.trajectory.velocities;
+        int numSteps = 0;
+        for (auto vel: velocities) {
+          if(numSteps >= controlHorizon){
+            break;
+          }
+          numSteps++;
+          cmd = vel;
+          LOG_DEBUG(BOLD(FBLU("Robot at: {x: ")), KBLU, robotState.x, RST,
+                    BOLD(FBLU(", y: ")), KBLU, robotState.y, RST,
+                    BOLD(FBLU(", yaw: ")), KBLU, robotState.yaw, RST,
+                    BOLD(FBLU("}")));
+
+          LOG_DEBUG(BOLD(FGRN("Found Control: {Vx: ")), KGRN, vel.vx(), RST,
+                    BOLD(FGRN(", Vy: ")), KGRN, vel.vy(), RST,
+                    BOLD(FGRN(", Omega: ")), KGRN, vel.omega(), RST,
+                    BOLD(FGRN("}")));
+
+          robotState.update(vel, timeStep);
+        }
 
-        robotState.update(cmd, timeStep);
       } else {
         LOG_ERROR(BOLD(FRED("DWA Planner failed with robot at: {x: ")), KRED,
                   robotState.x, RST, BOLD(FRED(", y: ")), KRED, robotState.y,
@@ -281,9 +296,9 @@ struct VisionDWATestConfig {
       }
 
       tracked_pose.update(timeStep);
-      if (with_tracker) {
-        moveDetectedBoxes();
-      }
+
+      moveDetectedBoxes();
+
       step++;
     }
     samples.push_back(simulated_velocities, robot_path);
@@ -311,12 +326,11 @@ struct VisionDWATestConfig {
   }
 };
 
-BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
+
+BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free) {
   // Create timer
   Timer time;
 
-  bool use_tracker = false;
-
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
 
@@ -325,88 +339,46 @@ BOOST_AUTO_TEST_CASE(test_VisionDWA_obstacle_free) {
   int numPointsPerTrajectory = 100;
 
   bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_obstacle_free"),
-      use_tracker);
+      numPointsPerTrajectory, std::string("vision_follower_with_tracker"));
   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 }
 
-BOOST_AUTO_TEST_CASE(test_VisionDWA_with_obstacle) {
-  // Create timer
-  Timer time;
-
-  bool use_tracker = false;
+// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
+//   // Create timer
+//   Timer time;
 
-  // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+//   // Robot pointcloud values (global frame)
+//   std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
 
-  VisionDWATestConfig testConfig(cloud);
+//   VisionDWATestConfig testConfig(cloud);
 
-  int numPointsPerTrajectory = 100;
+//   int numPointsPerTrajectory = 100;
 
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_obstacle"),
-      use_tracker, true);
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
-}
+//   bool test_passed = testConfig.run_test(
+//       numPointsPerTrajectory,
+//       std::string("vision_follower_with_tracker_and_obstacle"), true);
+//   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+// }
 
-BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_obs_free) {
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
   // Create timer
   Timer time;
 
-  // Sampling configuration
-  bool use_tracker = true;
-
-  // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
-
-  VisionDWATestConfig testConfig(cloud);
-
-  int numPointsPerTrajectory = 100;
-
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_tracker"),
-      use_tracker);
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
-}
-
-BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
-  // Create timer
-  Timer time;
+  std::string filename =
+      "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
+      "bag_image_depth.tif";
+  Bbox2D box({410, 0}, {410, 390});
+  std::vector<Bbox2D> detections_2d{box};
 
-  bool use_tracker = true;
+  auto initial_point = Eigen::Vector2i{610, 200};
 
   // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{0.3, 0.27, 0.1}};
+  std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
 
   VisionDWATestConfig testConfig(cloud);
 
-  int numPointsPerTrajectory = 100;
+  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
+                                                   initial_point, cloud);
 
-  bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory,
-      std::string("vision_follower_with_tracker_and_obstacle"), use_tracker, true);
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
 }
-
-// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
-//   // Create timer
-//   Timer time;
-
-//   std::string filename =
-//       "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
-//       "bag_image_depth.tif";
-//   Bbox2D box({410, 0}, {410, 390});
-//   std::vector<Bbox2D> detections_2d{box};
-
-//   auto initial_point = Eigen::Vector2i{610, 200};
-
-//   // Robot pointcloud values (global frame)
-//   std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
-
-//   VisionDWATestConfig testConfig(cloud);
-
-//   auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
-//                                                    initial_point, cloud);
-
-//   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
-// }
diff --git a/src/kompass_cpp/tests/vision_tracking_test.cpp b/src/kompass_cpp/tests/vision_tracking_test.cpp
index 64db5917..615ae0d5 100644
--- a/src/kompass_cpp/tests/vision_tracking_test.cpp
+++ b/src/kompass_cpp/tests/vision_tracking_test.cpp
@@ -23,7 +23,7 @@ struct VisionTrackingTestConfig {
   Control::TrajectoryPath reference_path, measured_path, tracked_path;
   Path::State reference_state;
   std::vector<Bbox3D> detected_boxes;
-  std::string pltFileName = "VisionTrackerTest";
+  std::string pltFileName = "vision_tracker_test";
   std::mt19937 gen;
   std::normal_distribution<float> noise;
 

From 926b5d02852ea71c282a0b2c68e37a36d8a3cd3d Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Tue, 27 May 2025 17:42:29 +0200
Subject: [PATCH 068/118] (refactor) Removes extra variable from command scope
 for lowest cost reduction kernel in cost evaluator

---
 src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
index 2cba6a7e..cd1a40e7 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/cost_evaluator_gpu.cpp
@@ -5,7 +5,6 @@
 #include "utils/logger.h"
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
-#include <limits>
 #include <sycl/sycl.hpp>
 #include <vector>
 
@@ -302,8 +301,7 @@ TrajSearchResult CostEvaluator::getMinTrajectoryCost(
     m_q.submit([&](sycl::handler &h) {
          h.depends_on(events);
          auto costs = m_devicePtrCosts;
-         auto minCost = m_minCost;
-         auto reduction = sycl::reduction(minCost, sycl::plus<LowestCost>());
+         auto reduction = sycl::reduction(m_minCost, sycl::plus<LowestCost>());
          // Kernel scope
          h.parallel_for<class minimumCostReduction>(
              sycl::range<1>(trajs_size), reduction,

From 454cdce563acf8bfaa56df1140134551b208f3f6 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Tue, 27 May 2025 17:55:36 +0200
Subject: [PATCH 069/118] (fix) Fixes first vision DWA test

---
 tests/test_controllers.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index bdc2daba..ee37a32e 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -13,7 +13,7 @@
 from kompass_cpp.types import PathInterpolationType, Path as PathCpp
 
 from kompass_core.datatypes.laserscan import LaserScanData
-from kompass_core.datatypes import TrackedPose2D, Bbox3D, Bbox2D
+from kompass_core.datatypes import Bbox3D, Bbox2D
 from kompass_core.control import (
     DVZ,
     DWAConfig,
@@ -499,7 +499,7 @@ def test_vision_dwa_with_depth_img():
     # Create a NumPy array from the OpenCV Mat
     depth_image = np.array(cv_img, dtype=np.ushort, order="F")
 
-    found_target = controller.set_initial_tracking_depth(
+    found_target = controller.set_initial_tracking_image(
         my_robot.state, clicked_point[0], clicked_point[1], detections, depth_image
     )
 

From 25935d8524a60e8f0072f2369334499d4c0cbf75 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Wed, 28 May 2025 09:22:46 +0200
Subject: [PATCH 070/118] (chore) Updates llvm version requirements for GPU
 installation

---
 build_dependencies/install_gpu.sh | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/build_dependencies/install_gpu.sh b/build_dependencies/install_gpu.sh
index 7fb12d73..76df7673 100644
--- a/build_dependencies/install_gpu.sh
+++ b/build_dependencies/install_gpu.sh
@@ -162,8 +162,8 @@ install_dependencies
 # Check for LLVM/Clang versions
 if [[ $LLVM_VERSION ]]; then
     # Check if given version is within range
-    if (( LLVM_VERSION < 14 || LLVM_VERSION > 17 )); then
-        log ERROR "LLVM Versions higher than 17 and lower than 14 are not compatible with kompass-core."
+    if (( LLVM_VERSION < 15 || LLVM_VERSION > 17 )); then
+        log ERROR "LLVM Versions higher than 20 and lower than 15 are not compatible with kompass-core."
         exit 1
     fi
     if check_llvm_clang_version $LLVM_VERSION; then

From c9b25b9cffe1806795a5accff5aa0b4928ff0cae Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 28 May 2025 19:24:05 +0800
Subject: [PATCH 071/118] (fix) Fixes DWA end of horizon computation

---
 src/kompass_core/control/dwa.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 48ea656f..cdcd9ffd 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -242,9 +242,7 @@ def __init__(
 
         # Init the following result
         self._result = kompass_cpp.control.SamplingControlResult()
-        self._end_of_ctrl_horizon: int = max(
-            int(self._config.control_horizon / self._config.control_time_step), 1
-        )
+        self._end_of_ctrl_horizon: int = max(self._config.control_horizon, 1)
         logging.info("DWA PATH CONTROLLER IS READY")
 
     @property

From 5b9044bf10418542ab3ddf00e2be7e03975fd46d Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 28 May 2025 13:48:03 +0200
Subject: [PATCH 072/118] (feature) Adds label to detections tracker

---
 src/kompass_cpp/bindings/bindings_types.cpp   | 246 +++++++++---------
 .../kompass_cpp/include/datatypes/tracking.h  |  22 +-
 .../kompass_cpp/include/vision/tracker.h      |   1 +
 .../kompass_cpp/src/vision/tracker.cpp        |  71 +++--
 4 files changed, 188 insertions(+), 152 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 02e5c040..f8629f90 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -2,15 +2,15 @@
 #include <nanobind/nanobind.h>
 #include <nanobind/operators.h>
 #include <nanobind/stl/array.h>
-#include <nanobind/stl/vector.h>
 #include <nanobind/stl/string.h>
+#include <nanobind/stl/vector.h>
 #include <vector>
 
 #include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
-#include "datatypes/trajectory.h"
 #include "datatypes/tracking.h"
+#include "datatypes/trajectory.h"
 #include "utils/collision_check.h"
 
 namespace py = nanobind;
@@ -40,123 +40,127 @@ void bindings_types(py::module_ &m) {
       .def_rw("yaw", &Path::State::yaw)
       .def_rw("speed", &Path::State::speed);
 
-py::class_<Path::PathPosition>(m_types, "PathPosition")
-    .def(py::init<>())
-    .def_rw("segment_index", &Path::PathPosition::segment_index)
-    .def_rw("segment_length", &Path::PathPosition::segment_length)
-    .def_rw("parallel_distance", &Path::PathPosition::parallel_distance)
-    .def_rw("normal_distance", &Path::PathPosition::normal_distance);
-
-py::class_<Path::Path>(m_types, "Path")
-    .def(py::init<const std::vector<Path::Point> &>(),
-         py::arg("points") = std::vector<Path::Point>())
-    .def("reached_end", &Path::Path::endReached)
-    .def("get_total_length", &Path::Path::totalPathLength)
-    .def("size", &Path::Path::getSize)
-    .def("getIndex", &Path::Path::getIndex, py::arg("index"))
-    .def("x", &Path::Path::getX)
-    .def("y", &Path::Path::getY);
-
-// Velocity control command
-py::class_<Control::Velocity2D>(m_types, "ControlCmd")
-    .def(py::init<float, float, float, float>(), py::arg("vx") = 0.0,
-         py::arg("vy") = 0.0, py::arg("omega") = 0.0,
-         py::arg("steer_ang") = 0.0)
-    .def_prop_rw("vx", &Control::Velocity2D::vx, &Control::Velocity2D::setVx)
-    .def_prop_rw("vy", &Control::Velocity2D::vy, &Control::Velocity2D::setVy)
-    .def_prop_rw("omega", &Control::Velocity2D::omega,
-                 &Control::Velocity2D::setOmega)
-    .def_prop_rw("steer_ang", &Control::Velocity2D::steer_ang,
-                 &Control::Velocity2D::setSteerAng)
-    .def("__str__", &printControlCmd);
-
-py::class_<Control::Velocities>(m_types, "ControlCmdList")
-    .def(py::init<>(), "Default constructor")
-    .def(py::init<int>(), "Constructor with length", py::arg("length"))
-    .def_rw("vx", &Control::Velocities::vx, "Speed on x-axis (m/s)")
-    .def_rw("vy", &Control::Velocities::vy, "Speed on y-axis (m/s)")
-    .def_rw("omega", &Control::Velocities::omega, "Angular velocity (rad/s)")
-    .def_rw("length", &Control::Velocities::_length, "Length of the vectors");
-
-py::class_<Control::TrajectoryPath>(m_types, "TrajectoryPath")
-    .def(py::init<>())
-    .def_ro("x", &Control::TrajectoryPath::x, py::rv_policy::reference_internal)
-    .def_ro("y", &Control::TrajectoryPath::y, py::rv_policy::reference_internal)
-    .def_ro("z", &Control::TrajectoryPath::z,
-            py::rv_policy::reference_internal);
-
-py::class_<Control::TrajectoryVelocities2D>(m_types, "TrajectoryVelocities2D")
-    .def(py::init<>())
-    .def_ro("vx", &Control::TrajectoryVelocities2D::vx,
-            py::rv_policy::reference_internal)
-    .def_ro("vy", &Control::TrajectoryVelocities2D::vy,
-            py::rv_policy::reference_internal)
-    .def_ro("omega", &Control::TrajectoryVelocities2D::omega,
-            py::rv_policy::reference_internal);
-
-py::class_<Control::Trajectory2D>(m_types, "Trajectory")
-    .def(py::init<>())
-    .def_ro("velocities", &Control::Trajectory2D::velocities)
-    .def_ro("path", &Control::Trajectory2D::path);
-
-py::class_<Control::LaserScan>(m_types, "LaserScan")
-    .def(py::init<std::vector<double>, std::vector<double>>(),
-         py::arg("ranges"), py::arg("angles"))
-    .def_ro("ranges", &Control::LaserScan::ranges)
-    .def_ro("angles", &Control::LaserScan::angles);
-
-// For collisions detection
-py::enum_<CollisionChecker::ShapeType>(m_types, "RobotGeometry")
-    .value("CYLINDER", CollisionChecker::ShapeType::CYLINDER)
-    .value("BOX", CollisionChecker::ShapeType::BOX)
-    .value("SPHERE", CollisionChecker::ShapeType::SPHERE)
-    .def("get", [](const std::string &key) {
-      if (key == "CYLINDER")
-        return CollisionChecker::ShapeType::CYLINDER;
-      if (key == "BOX")
-        return CollisionChecker::ShapeType::BOX;
-      if (key == "SPHERE")
-        return CollisionChecker::ShapeType::SPHERE;
-      throw std::runtime_error("Invalid key");
-    });
-
-// Vision types
-py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
-    .def(py::init<std::array<double, 2>, int, int, std::array<double, 2>,
-                  double>(),
-         py::arg("size_xy"), py::arg("img_width"), py::arg("img_height"),
-         py::arg("center_xy"), py::arg("depth") = -1.0)
-    .def_rw("size_xy", &Control::VisionFollower::TrackingData::size_xy)
-    .def_rw("img_width", &Control::VisionFollower::TrackingData::img_width)
-    .def_rw("img_height", &Control::VisionFollower::TrackingData::img_height)
-    .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy)
-    .def_rw("depth", &Control::VisionFollower::TrackingData::depth);
-
-py::class_<Bbox2D>(m_types, "Bbox2D")
-    .def(py::init<>())
-    .def(py::init<const Bbox2D &>())
-    .def(py::init<const Eigen::Vector2i &, const Eigen::Vector2i &,
-                  const float>(),
-         py::arg("top_left_corner"), py::arg("size"),
-         py::arg("timestamp") = 0.0)
-    .def_rw("top_left_corner", &Bbox2D::top_corner)
-    .def_rw("size", &Bbox2D::size)
-    .def_rw("timestamp", &Bbox2D::timestamp);
-
-py::class_<Bbox3D>(m_types, "Bbox3D")
-    .def(py::init<>())
-    .def(py::init<const Bbox3D &>())
-    .def(py::init<const Eigen::Vector3f &, const Eigen::Vector3f &,
-                  const Eigen::Vector2i &, const Eigen::Vector2i &, const float,
-                  const std::vector<Eigen::Vector3f> &>(),
-         py::arg("center"), py::arg("size"), py::arg("center_img_frame"),
-         py::arg("size_img_frame"), py::arg("timestamp") = 0.0,
-         py ::arg("pc_points") = py::list())
-    .def_rw("center", &Bbox3D::center)
-    .def_rw("size", &Bbox3D::size)
-    .def_rw("center_img_frame", &Bbox3D::center_img_frame)
-    .def_rw("size_img_frame", &Bbox3D::size_img_frame)
-    .def_rw("pc_points", &Bbox3D::pc_points)
-    .def_rw("timestamp", &Bbox3D::timestamp);
-
+  py::class_<Path::PathPosition>(m_types, "PathPosition")
+      .def(py::init<>())
+      .def_rw("segment_index", &Path::PathPosition::segment_index)
+      .def_rw("segment_length", &Path::PathPosition::segment_length)
+      .def_rw("parallel_distance", &Path::PathPosition::parallel_distance)
+      .def_rw("normal_distance", &Path::PathPosition::normal_distance);
+
+  py::class_<Path::Path>(m_types, "Path")
+      .def(py::init<const std::vector<Path::Point> &>(),
+           py::arg("points") = std::vector<Path::Point>())
+      .def("reached_end", &Path::Path::endReached)
+      .def("get_total_length", &Path::Path::totalPathLength)
+      .def("size", &Path::Path::getSize)
+      .def("getIndex", &Path::Path::getIndex, py::arg("index"))
+      .def("x", &Path::Path::getX)
+      .def("y", &Path::Path::getY);
+
+  // Velocity control command
+  py::class_<Control::Velocity2D>(m_types, "ControlCmd")
+      .def(py::init<float, float, float, float>(), py::arg("vx") = 0.0,
+           py::arg("vy") = 0.0, py::arg("omega") = 0.0,
+           py::arg("steer_ang") = 0.0)
+      .def_prop_rw("vx", &Control::Velocity2D::vx, &Control::Velocity2D::setVx)
+      .def_prop_rw("vy", &Control::Velocity2D::vy, &Control::Velocity2D::setVy)
+      .def_prop_rw("omega", &Control::Velocity2D::omega,
+                   &Control::Velocity2D::setOmega)
+      .def_prop_rw("steer_ang", &Control::Velocity2D::steer_ang,
+                   &Control::Velocity2D::setSteerAng)
+      .def("__str__", &printControlCmd);
+
+  py::class_<Control::Velocities>(m_types, "ControlCmdList")
+      .def(py::init<>(), "Default constructor")
+      .def(py::init<int>(), "Constructor with length", py::arg("length"))
+      .def_rw("vx", &Control::Velocities::vx, "Speed on x-axis (m/s)")
+      .def_rw("vy", &Control::Velocities::vy, "Speed on y-axis (m/s)")
+      .def_rw("omega", &Control::Velocities::omega, "Angular velocity (rad/s)")
+      .def_rw("length", &Control::Velocities::_length, "Length of the vectors");
+
+  py::class_<Control::TrajectoryPath>(m_types, "TrajectoryPath")
+      .def(py::init<>())
+      .def_ro("x", &Control::TrajectoryPath::x,
+              py::rv_policy::reference_internal)
+      .def_ro("y", &Control::TrajectoryPath::y,
+              py::rv_policy::reference_internal)
+      .def_ro("z", &Control::TrajectoryPath::z,
+              py::rv_policy::reference_internal);
+
+  py::class_<Control::TrajectoryVelocities2D>(m_types, "TrajectoryVelocities2D")
+      .def(py::init<>())
+      .def_ro("vx", &Control::TrajectoryVelocities2D::vx,
+              py::rv_policy::reference_internal)
+      .def_ro("vy", &Control::TrajectoryVelocities2D::vy,
+              py::rv_policy::reference_internal)
+      .def_ro("omega", &Control::TrajectoryVelocities2D::omega,
+              py::rv_policy::reference_internal);
+
+  py::class_<Control::Trajectory2D>(m_types, "Trajectory")
+      .def(py::init<>())
+      .def_ro("velocities", &Control::Trajectory2D::velocities)
+      .def_ro("path", &Control::Trajectory2D::path);
+
+  py::class_<Control::LaserScan>(m_types, "LaserScan")
+      .def(py::init<std::vector<double>, std::vector<double>>(),
+           py::arg("ranges"), py::arg("angles"))
+      .def_ro("ranges", &Control::LaserScan::ranges)
+      .def_ro("angles", &Control::LaserScan::angles);
+
+  // For collisions detection
+  py::enum_<CollisionChecker::ShapeType>(m_types, "RobotGeometry")
+      .value("CYLINDER", CollisionChecker::ShapeType::CYLINDER)
+      .value("BOX", CollisionChecker::ShapeType::BOX)
+      .value("SPHERE", CollisionChecker::ShapeType::SPHERE)
+      .def("get", [](const std::string &key) {
+        if (key == "CYLINDER")
+          return CollisionChecker::ShapeType::CYLINDER;
+        if (key == "BOX")
+          return CollisionChecker::ShapeType::BOX;
+        if (key == "SPHERE")
+          return CollisionChecker::ShapeType::SPHERE;
+        throw std::runtime_error("Invalid key");
+      });
+
+  // Vision types
+  py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
+      .def(py::init<std::array<double, 2>, int, int, std::array<double, 2>,
+                    double>(),
+           py::arg("size_xy"), py::arg("img_width"), py::arg("img_height"),
+           py::arg("center_xy"), py::arg("depth") = -1.0)
+      .def_rw("size_xy", &Control::VisionFollower::TrackingData::size_xy)
+      .def_rw("img_width", &Control::VisionFollower::TrackingData::img_width)
+      .def_rw("img_height", &Control::VisionFollower::TrackingData::img_height)
+      .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy)
+      .def_rw("depth", &Control::VisionFollower::TrackingData::depth);
+
+  py::class_<Bbox2D>(m_types, "Bbox2D")
+      .def(py::init<>())
+      .def(py::init<const Bbox2D &>())
+      .def(py::init<const Eigen::Vector2i &, const Eigen::Vector2i &,
+                    const float, const std::string>(),
+           py::arg("top_left_corner"), py::arg("size"),
+           py::arg("timestamp") = 0.0, py::arg("label") = "")
+      .def_rw("top_left_corner", &Bbox2D::top_corner)
+      .def_rw("size", &Bbox2D::size)
+      .def_rw("timestamp", &Bbox2D::timestamp)
+      .def_rw("label", &Bbox2D::label);
+
+  py::class_<Bbox3D>(m_types, "Bbox3D")
+      .def(py::init<>())
+      .def(py::init<const Bbox3D &>())
+      .def(py::init<const Eigen::Vector3f &, const Eigen::Vector3f &,
+                    const Eigen::Vector2i &, const Eigen::Vector2i &,
+                    const float, const std::string,
+                    const std::vector<Eigen::Vector3f> &>(),
+           py::arg("center"), py::arg("size"), py::arg("center_img_frame"),
+           py::arg("size_img_frame"), py::arg("timestamp") = 0.0,
+           py::arg("label") = "", py ::arg("pc_points") = py::list())
+      .def_rw("center", &Bbox3D::center)
+      .def_rw("size", &Bbox3D::size)
+      .def_rw("center_img_frame", &Bbox3D::center_img_frame)
+      .def_rw("size_img_frame", &Bbox3D::size_img_frame)
+      .def_rw("pc_points", &Bbox3D::pc_points)
+      .def_rw("timestamp", &Bbox3D::timestamp)
+      .def_rw("label", &Bbox3D::label);
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 4395a597..a467e9a3 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -12,13 +12,14 @@ struct Bbox2D {
   Eigen::Vector2i top_corner = {0, 0};
   Eigen::Vector2i size = {0, 0};
   float timestamp = 0.0; // Timestamp of the detection in seconds
+  std::string label = ""; // Label of the detection, e.g. "car", "pedestrian"
 
   Bbox2D(){};
 
-  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp){};
+  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp), label(box.label){};
 
-  Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size, const float timestamp = 0.0)
-      : top_corner(top_corner), size(size), timestamp(timestamp){};
+  Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size, const float timestamp = 0.0, const std::string &label = "")
+      : top_corner(top_corner), size(size), timestamp(timestamp), label(label){};
 
   Eigen::Vector2i getXLimits() const {
     return {top_corner.x(), top_corner.x() + size.x()};
@@ -36,6 +37,7 @@ struct Bbox3D {
   Eigen::Vector2i size_img_frame = {0, 0};
   std::vector<Eigen::Vector3f> pc_points = {};
   float timestamp = 0.0; // Timestamp of the detection in seconds
+  std::string label = "";
 
   Bbox3D(){};
 
@@ -43,15 +45,15 @@ struct Bbox3D {
       : center(box.center), size(box.size),
         center_img_frame(box.center_img_frame),
         size_img_frame(box.size_img_frame), pc_points(box.pc_points),
-        timestamp(box.timestamp){};
+        timestamp(box.timestamp), label(box.label){};
 
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Eigen::Vector2i center_img_frame,
-         const Eigen::Vector2i size_img_frame, const float timestamp = 0.0,
+         const Eigen::Vector2i size_img_frame, const float timestamp = 0.0, const std::string &label = "",
          const std::vector<Eigen::Vector3f> pc_points = {})
       : center(center), size(size), center_img_frame(center_img_frame),
         size_img_frame(size_img_frame), pc_points(pc_points),
-        timestamp(timestamp){};
+        timestamp(timestamp), label(label){};
 
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Bbox2D &box2d, std::vector<Eigen::Vector3f> pc_points = {})
@@ -60,13 +62,13 @@ struct Bbox3D {
             box2d.top_corner +
             Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
         size_img_frame(box2d.size), pc_points(pc_points),
-        timestamp(box2d.timestamp){};
+        timestamp(box2d.timestamp), label(box2d.label){};
 
   Bbox3D(const Bbox2D &box2d)
       : center_img_frame(
             box2d.top_corner +
             Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
-        size_img_frame(box2d.size), timestamp(box2d.timestamp){};
+        size_img_frame(box2d.size), timestamp(box2d.timestamp), label(box2d.label){};
 
   Eigen::Vector2f getXLimitsImg() const {
     return {center_img_frame.x() - (size_img_frame.x() / 2),
@@ -98,6 +100,10 @@ struct TrackedBbox3D {
   void setfromBox(const Bbox3D &box) { this->box = box; };
 
   void updateFromNewDetection(const Bbox3D &new_box) {
+    if(new_box.label != this->box.label){
+      LOG_ERROR("Box label mismatch, cannot update tracking.");
+      return;
+    }
     float time_step = new_box.timestamp - this->box.timestamp;
     Eigen::Vector3f new_vel;
     if (time_step <= 0.0) {
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index 12f6b2a6..b9f3f7be 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -38,6 +38,7 @@ class FeatureBasedBboxTracker{
 
     private:
       float timeStep_, minAcceptedSimilarityScore_ = 0.0;
+      std::string trackedLabel_;
       std::unique_ptr<TrackedBbox3D> trackedBox_;
       std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
 
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
index e7f37629..9e88dbd2 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/tracker.cpp
@@ -42,6 +42,7 @@ FeatureBasedBboxTracker::FeatureBasedBboxTracker(const float &time_step,
 
 bool FeatureBasedBboxTracker::setInitialTracking(const TrackedBbox3D &bBox) {
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  trackedLabel_ = bBox.box.label;
   Eigen::VectorXf state_vec;
   state_vec.resize(StateSize);
   state_vec(0) = bBox.box.center[0]; // x
@@ -61,6 +62,7 @@ bool FeatureBasedBboxTracker::setInitialTracking(const Bbox3D &bBox,
                                                  const float yaw) {
   LOG_DEBUG("Setting initial tracked box");
   trackedBox_ = std::make_unique<TrackedBbox3D>(bBox);
+  trackedLabel_ = bBox.label;
   Eigen::Matrix<float, StateSize, 1> state_vec =
       Eigen::Matrix<float, StateSize, 1>::Zero();
   state_vec(0) = bBox.center.x();
@@ -116,41 +118,64 @@ void FeatureBasedBboxTracker::updateTrackedBoxState(const int numberSteps) {
 
 bool FeatureBasedBboxTracker::updateTracking(
     const std::vector<Bbox3D> &detected_boxes) {
-  float new_timestamp = detected_boxes[0].timestamp;
-  float dt = new_timestamp - trackedBox_->box.timestamp;
+  std::vector<Bbox3D> label_boxes;
+  for(auto box : detected_boxes) {
+    if(box.label == trackedLabel_) {
+      label_boxes.push_back(box);
+    }
+  }
+  if (label_boxes.empty()) {
+    LOG_DEBUG("No boxes with label ", trackedLabel_, " found in the detected boxes!");
+    return false;
+  }
 
-  // Predicted the new location of the tracked box
-  auto predicted_tracked_box = trackedBox_->predictConstantAcc(dt);
+  float max_similarity_score = 0.0f; // Similarity score
+  Bbox3D * found_box;
+  float dt = label_boxes[0].timestamp - trackedBox_->box.timestamp;
 
-  auto ref_box_features = extractFeatures(predicted_tracked_box);
+  if(label_boxes.size() == 1) {
+    // Only one box detected, so it is the same
+    max_similarity_score = 1.0f;
+    found_box = &label_boxes[0];
+  }
+  else{
+    // Compute similarity score and find box
+    // Predicted the new location of the tracked box
+    auto predicted_tracked_box = trackedBox_->predictConstantAcc(dt);
 
-  // Get the features of all the new detections
-  FeaturesVector detected_boxes_feature_vec;
-  float max_similarity_score = 0.0; // Similarity score
-  size_t similar_box_idx = 0, count = 0;
-  for (auto box : detected_boxes) {
-    detected_boxes_feature_vec = extractFeatures(box);
-    FeaturesVector error_vec = detected_boxes_feature_vec - ref_box_features;
-    // Error vector normalization
-    for(int i = 0; i < error_vec.size(); ++i) {
-      if (std::abs(ref_box_features(i)) > 0.0) {
-        error_vec(i) = error_vec(i) / std::abs(ref_box_features(i));
+    auto ref_box_features = extractFeatures(predicted_tracked_box);
+
+    // Get the features of all the new detections
+    FeaturesVector detected_boxes_feature_vec;
+    size_t similar_box_idx = 0, count = 0;
+
+    for (auto box : label_boxes) {
+      detected_boxes_feature_vec = extractFeatures(box);
+      FeaturesVector error_vec = detected_boxes_feature_vec - ref_box_features;
+      // Error vector normalization
+      for (int i = 0; i < error_vec.size(); ++i) {
+        if (std::abs(ref_box_features(i)) > 0.0) {
+          error_vec(i) = error_vec(i) / std::abs(ref_box_features(i));
+        }
       }
-    }
-    float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
+      float similarity_score = std::exp(-std::pow(error_vec.norm(), 2));
 
-    if (similarity_score > max_similarity_score) {
-      max_similarity_score = similarity_score;
-      similar_box_idx = count;
+      if (similarity_score > max_similarity_score) {
+        max_similarity_score = similarity_score;
+        similar_box_idx = count;
+      }
+      count++;
     }
-    count++;
+    found_box = &label_boxes[similar_box_idx];
   }
+
   LOG_DEBUG("Max similarity score = ", max_similarity_score);
   if (max_similarity_score > minAcceptedSimilarityScore_) {
     // Update raw tracking
+    float dt = found_box->timestamp - trackedBox_->box.timestamp;
     int number_steps = std::max(static_cast<int>(dt / timeStep_), 1);
 
-    trackedBox_->updateFromNewDetection(detected_boxes[similar_box_idx]);
+    trackedBox_->updateFromNewDetection(*found_box);
 
     // Update state by applying kalman filter on the raw measurement
     updateTrackedBoxState(number_steps);

From 46bfad1c2bd25d1dbf7abb8ab92a6c1f876f260f Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 3 Jun 2025 19:31:23 +0800
Subject: [PATCH 073/118] (fix) Adds dummy run to DWA on init to execute JIT
 compile

---
 .../kompass_cpp/include/controllers/dwa.h       | 13 +++++++++++++
 .../kompass_cpp/src/controllers/dwa.cpp         | 17 +++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 5e4ad3df..f875c1c9 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -6,6 +6,7 @@
 #include "datatypes/trajectory.h"
 #include "follower.h"
 #include "utils/cost_evaluator.h"
+#include "utils/logger.h"
 #include "utils/trajectory_sampler.h"
 #include <Eigen/Dense>
 #include <memory>
@@ -184,6 +185,16 @@ class DWA : public Follower {
     // find closest segment to use in cost computation
     determineTarget();
 
+    if(rotate_in_place and currentTrackedTarget_->heading_error > goal_orientation_tolerance * 10.0){
+      // If the robot is rotating in place and the heading error is large, we
+      // do not need to sample trajectories
+      LOG_DEBUG("Rotating In Place ...");
+      TrajSearchResult result;
+      result.isTrajFound = true;
+      result.trajectory = trajSampler->generateSingleSampleFromVel(Velocity2D(0.0, 0.0, - currentTrackedTarget_->heading_error * ctrlimitsParams.omegaParams.maxOmega / M_PI));
+      return result;
+    }
+
     // Generate set of valid trajectories in the DW
     std::unique_ptr<TrajectorySamples2D> samples_ =
         trajSampler->generateTrajectories(global_vel, currentState,
@@ -216,6 +227,8 @@ class DWA : public Follower {
   size_t getMaxPathLength();
 
   Path::Path findTrackedPathSegment();
+
+  void initJitCompile();
 };
 
 }; // namespace Control
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 84c325e9..548f5829 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -34,6 +34,8 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
   } else {
     max_forward_distance_ = controlLimits.velXParams.maxVel * predictionHorizon;
   }
+
+  initJitCompile();
 }
 
 DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
@@ -59,6 +61,21 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
   } else {
     max_forward_distance_ = controlLimits.velXParams.maxVel * timeHorizon;
   }
+
+  initJitCompile();
+}
+
+void DWA::initJitCompile() {
+  const int dummyNumSamples = 1;
+  const int dummyNumPoints = 2;
+  TrajectoryVelocitySamples2D velocities(dummyNumSamples, dummyNumPoints);
+  TrajectoryPathSamples paths(dummyNumSamples, dummyNumPoints);
+  velocities.push_back({Velocity2D(1.0, 0.0, 0.0)});
+  auto dummyPath = Path::Path({Path::Point(0.0f, 0.0f, 0.0f), Path::Point(1.0f, 1.0f, 0.0f)});
+  paths.push_back(dummyPath);
+  std::unique_ptr<TrajectorySamples2D> dummySamples =
+      std::make_unique<TrajectorySamples2D>(velocities, paths);
+  trajCostEvaluator->getMinTrajectoryCost(dummySamples, &dummyPath, dummyPath);
 }
 
 void DWA::configure(ControlLimitsParams controlLimits, ControlType controlType,

From 5b068ec79f2c6319998482b92898bcd66535c35a Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 3 Jun 2025 19:32:42 +0800
Subject: [PATCH 074/118] (fix) Adds tolerance parameters for both distance and
 angle in VisionDWA

---
 src/kompass_core/control/vision_dwa.py        |  7 ++-
 .../include/controllers/vision_dwa.h          | 45 +++++++++++----
 .../src/controllers/vision_dwa.cpp            | 55 +++++++++----------
 3 files changed, 63 insertions(+), 44 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 27bc1d7d..dd78043a 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -24,8 +24,11 @@
 
 @define
 class VisionDWAConfig(DWAConfig):
-    tolerance: float = field(
-        default=0.01, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
+    distance_tolerance: float = field(
+        default=0.05, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
+    )
+    angle_tolerance: float = field(
+        default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
     )
     # Tolerance value for distance and angle following errors
 
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 7b1bafa3..37513c91 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -34,9 +34,13 @@ class VisionDWA : public DWA {
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
       addParameter(
-          "tolerance",
-          Parameter(0.01, 0.0, 1.0,
-                    "Tolerance value for distance and angle following errors"));
+          "distance_tolerance",
+          Parameter(0.05, 0.0, 100.0,
+                    "Tolerance value for distance (meters)"));
+      addParameter(
+        "angle_tolerance",
+        Parameter(0.1, 0.0, M_PI,
+                  "Tolerance value for angle (rad)"));
       addParameter(
           "target_distance",
           Parameter(
@@ -100,7 +104,8 @@ class VisionDWA : public DWA {
     int prediction_horizon() const {
       return getParameter<int>("prediction_horizon");
     }
-    double tolerance() const { return getParameter<double>("tolerance"); }
+    double dist_tolerance() const { return getParameter<double>("distance_tolerance"); }
+    double ang_tolerance() const { return getParameter<double>("angle_tolerance"); }
     double target_distance() const {
       double val = getParameter<double>("target_distance");
       return val < 0 ? -1.0 : val; // Return -1 for None
@@ -345,26 +350,39 @@ class VisionDWA : public DWA {
       if (config_.enable_search()) {
         if (recorded_search_time_ < config_.target_search_timeout()) {
           if (search_commands_queue_.empty()) {
+            LOG_DEBUG("Search commands queue is empty, generating new search "
+                      "commands");
             getFindTargetCmds();
           }
           LOG_DEBUG("Number of search commands remaining: ",
                     search_commands_queue_.size(),
                     "recorded search time: ", recorded_search_time_);
           // Create search command
-          Trajectory2D ref_traj(config_.control_horizon() + 1);
+          TrajectoryVelocities2D velocities(config_.control_horizon());
+          TrajectoryPath path(config_.control_horizon());
+          LOG_DEBUG("Initializing path and velocities with size: ",
+            config_.control_horizon());
+          path.add(0, 0.0, 0.0);
           std::array<double, 3> search_command;
-          for (int i = 0; i <= config_.control_horizon(); i++) {
+          for (int i = 0; i < config_.control_horizon() - 1; i++) {
+            if (search_commands_queue_.empty()) {
+              LOG_DEBUG("Search commands queue is empty. Ending Search ");
+              return TrajSearchResult();
+            }
             search_command = search_commands_queue_.front();
             search_commands_queue_.pop();
             recorded_search_time_ += config_.control_time_step();
-            ref_traj.velocities.add(i, Velocity2D(search_command[0],
+            path.add(i + 1, 0.0, 0.0);
+            velocities.add(i, Velocity2D(search_command[0],
                                                   search_command[1],
                                                   search_command[2]));
           }
+          LOG_DEBUG("Sending ", config_.control_horizon(), " search commands "
+                    "to the controller");
           auto result = TrajSearchResult();
           result.isTrajFound = true;
           result.trajCost = 0.0;
-          result.trajectory = ref_traj;
+          result.trajectory = Trajectory2D(velocities, path);
           return result;
         }
       } else {
@@ -374,15 +392,18 @@ class VisionDWA : public DWA {
                     "timeout in ",
                     timeout, " seconds");
           // Do nothing and wait
-          Trajectory2D ref_traj(config_.control_horizon() + 1);
-          for (int i = 0; i <= config_.control_horizon(); i++) {
+          TrajectoryVelocities2D velocities(config_.control_horizon());
+          TrajectoryPath path(config_.control_horizon());
+          path.add(0, 0.0, 0.0);
+          for (int i = 0; i < config_.control_horizon() - 1; i++) {
             recorded_wait_time_ += config_.control_time_step();
-            ref_traj.velocities.add(i, Velocity2D(0.0, 0.0, 0.0));
+            velocities.add(i, Velocity2D(0.0, 0.0, 0.0));
+            path.add(i + 1, 0.0, 0.0);
           }
           auto result = TrajSearchResult();
           result.isTrajFound = true;
           result.trajCost = 0.0;
-          result.trajectory = ref_traj;
+          result.trajectory = Trajectory2D(velocities, path);
           return result;
         }
       }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 82097347..3a82f621 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -68,27 +68,22 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   float angle_error =
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
-  float distance_tolerance =
-      std::max(0.01, config_.tolerance() * config_.target_distance());
-  float angle_tolerance =
-      std::max(0.01, config_.tolerance() * config_.target_orientation());
-
-  LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error);
+  // LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error);
 
   Velocity2D followingVel;
-  if (abs(distance_error) > distance_tolerance or
-      abs(angle_error) > angle_tolerance) {
-    auto term1 =
-        (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
-    auto term2 = -(config_.K_v() * ctrl_limits_.velXParams.maxVel *
-                   tanh(distance_error)) /
-                 cos(angle_error);
+  if (abs(distance_error) > config_.dist_tolerance() or
+      abs(angle_error) > config_.ang_tolerance()) {
+    // auto term1 =
+    //     (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
+    // auto term2 = -(config_.K_v() * ctrl_limits_.velXParams.maxVel *
+    //                tanh(distance_error)) /
+    //              cos(angle_error);
     double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
                 (config_.K_v() * ctrl_limits_.velXParams.maxVel *
-                 tanh(distance_error))) /
+                 tanh(distance_error / config_.target_distance()))) /
                cos(angle_error);
-    LOG_DEBUG("v=", v, ", track_velocity_term=", term1,
-              ", dist_error_term=", term2);
+    // LOG_DEBUG("v=", v, ", track_velocity_term=", term1,
+    //           ", dist_error_term=", term2);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     if (std::abs(v) < config_.min_vel()) {
@@ -97,21 +92,21 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
-      auto term1_o =
-          track_velocity_ * (tracking_pose.omega() -
-                             2.0 * sin(psi) / distance * tracking_pose.v());
-      auto term2_o = -2.0 * v * sin(angle_error) / distance;
-      auto term3_o = -(config_.K_omega() * tanh(angle_error));
+      // auto term1_o =
+      //     track_velocity_ * (tracking_pose.omega() -
+      //                        2.0 * sin(psi) / distance * tracking_pose.v());
+      // auto term2_o = -2.0 * v * sin(angle_error) / distance;
+      // auto term3_o = -(config_.K_omega() * tanh(angle_error));
       omega = -track_velocity_ * tracking_pose.omega() -
-              2.0 * track_velocity_ * sin(psi) / distance * tracking_pose.v() -
-              2.0 * v * sin(angle_error) / distance -
-              2.0 * config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
-                  tanh(angle_error);
-      LOG_DEBUG("omega=", omega, ", term_track_velocity=", term1_o,
-                ", term_v=", term2_o, ", term_angle_err=", term3_o);
+              track_velocity_ * sin(psi) / distance * tracking_pose.v() -
+              v * sin(angle_error) / distance -
+              config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
+                  tanh(angle_error / M_PI_2);
+      // LOG_DEBUG("omega=", omega, ", term_track_velocity=", term1_o,
+      //           ", term_v=", term2_o, ", term_angle_err=", term3_o);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
-              2.0 * config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
+              config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
                   tanh(angle_error);
     }
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
@@ -311,13 +306,13 @@ void VisionDWA::getFindTargetCmds(const int last_direction) {
                          target_search_steps_max / 4, true);
   // go back
   generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 8);
+                         target_search_steps_max / 8, true);
   // rotate -pi
   generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
                          target_search_steps_max / 4, true);
   // go back
   generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 8);
+                         target_search_steps_max / 8, true);
 }
 
 } // namespace Control

From 0cd25754f21f75149c25f0cd4b51bbc229da7301 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 3 Jun 2025 13:34:30 +0200
Subject: [PATCH 075/118] (fix) Remove repetitive code from follower

---
 .../kompass_cpp/src/controllers/follower.cpp     | 16 +---------------
 1 file changed, 1 insertion(+), 15 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index b9653a36..840c8848 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -34,21 +34,7 @@ Follower::Follower() : Controller(), config() {
 
 Follower::Follower(FollowerParameters config) : Controller() {
   this->config = config;
-  lookahead_distance = config.getParameter<double>("lookahead_distance");
-  enable_reverse_driving = config.getParameter<bool>("enable_reverse_driving");
-  goal_dist_tolerance = config.getParameter<double>("goal_dist_tolerance");
-  goal_orientation_tolerance =
-      config.getParameter<double>("goal_orientation_tolerance");
-  loosing_goal_distance = config.getParameter<double>("loosing_goal_distance");
-  path_segment_length = config.getParameter<double>("path_segment_length");
-  maxDist = config.getParameter<double>("max_point_interpolation_distance");
-  // Set rotate_in_place based on the robot type
-  if (ctrType == Control::ControlType::ACKERMANN) {
-    rotate_in_place = false;
-  } else {
-    rotate_in_place = true;
-  }
-  maxSegmentSize = getMaxSegmentSize();
+  Follower();
 }
 
 size_t Follower::getMaxSegmentSize() const {

From f10d8a4d22aa0c42f21efe5ebac8bb22fd855c74 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 3 Jun 2025 14:03:41 +0200
Subject: [PATCH 076/118] (fix) Minor fixes in VisionDWA

---
 src/kompass_cpp/kompass_cpp/include/controllers/dwa.h     | 8 +++-----
 .../kompass_cpp/include/controllers/vision_dwa.h          | 2 +-
 .../kompass_cpp/include/datatypes/trajectory.h            | 5 ++++-
 src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp  | 4 ++--
 4 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index f875c1c9..777938c8 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -189,10 +189,8 @@ class DWA : public Follower {
       // If the robot is rotating in place and the heading error is large, we
       // do not need to sample trajectories
       LOG_DEBUG("Rotating In Place ...");
-      TrajSearchResult result;
-      result.isTrajFound = true;
-      result.trajectory = trajSampler->generateSingleSampleFromVel(Velocity2D(0.0, 0.0, - currentTrackedTarget_->heading_error * ctrlimitsParams.omegaParams.maxOmega / M_PI));
-      return result;
+      auto trajectory = trajSampler->generateSingleSampleFromVel(Velocity2D(0.0, 0.0, - currentTrackedTarget_->heading_error * ctrlimitsParams.omegaParams.maxOmega / M_PI));
+      return TrajSearchResult{trajectory, true, 0.0};
     }
 
     // Generate set of valid trajectories in the DW
@@ -200,7 +198,7 @@ class DWA : public Follower {
         trajSampler->generateTrajectories(global_vel, currentState,
                                           scan_points);
     if (samples_->size() == 0) {
-      return TrajSearchResult();
+      return TrajSearchResult{Trajectory2D(), false, 0.0};
     }
 
     trajCostEvaluator->setPointScan(scan_points, currentState, maxLocalRange_);
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 37513c91..ff9fbc36 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -341,7 +341,7 @@ class VisionDWA : public DWA {
       // ---------------------------------------------------------------
       return result;
     }
-    if (!isGoalReached()) {
+    if (this->hasPath() and !isGoalReached()) {
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
     } else {
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
index 20f76db4..6928c7a6 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
@@ -577,7 +577,10 @@ struct TrajectorySamples2D {
 struct TrajSearchResult {
   Trajectory2D trajectory;
   bool isTrajFound = false;
-  float trajCost;
+  float trajCost = 0.0;
+
+  // default constructor
+  TrajSearchResult() = default;
 };
 
 // Lowest cost and its associated index for the trajectory sample
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index 840c8848..a3b09c5f 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -99,7 +99,7 @@ size_t Follower::getCurrentSegmentIndex() { return current_segment_index_; }
 
 bool Follower::isGoalReached() {
   if (!path_processing_) {
-    return reached_goal_;
+    return true;
   }
 
   const Path::Point goal_point = currentPath->getEnd();
@@ -319,7 +319,7 @@ const double Follower::getPathLength() const {
   return currentPath->totalPathLength();
 }
 const bool Follower::hasPath() const {
-  if (!path_processing_) {
+  if (!currentPath or !path_processing_) {
     return false;
   }
   return currentPath->totalPathLength() > 0.0;

From 1e96d836295379a4c21758cb73f0aec91af8f358 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 4 Jun 2025 21:42:00 +0800
Subject: [PATCH 077/118] (refactor) Remove debug logging

---
 src/kompass_core/control/vision_dwa.py        | 24 +++++---
 .../include/controllers/vision_dwa.h          | 27 +++++----
 .../kompass_cpp/src/controllers/dwa.cpp       |  3 +
 .../src/controllers/vision_dwa.cpp            | 57 +++++++------------
 4 files changed, 58 insertions(+), 53 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index dd78043a..27582502 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -276,17 +276,27 @@ def loop_step(
         :return: If planner found a valid solution
         :rtype: bool
         """
-        self._planner.set_current_state(
-            current_state.x, current_state.y, current_state.yaw, current_state.speed
-        )
+        if current_state is not None:
+            self._planner.set_current_state(
+                current_state.x, current_state.y, current_state.yaw, current_state.speed
+            )
+            current_velocity = ControlCmd(
+            vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
+            )
+        else:
+            if self.control_till_horizon is None:
+                current_velocity = ControlCmd(
+                    vx=0.0, vy=0.0, omega=0.0
+                )
+            else:
+                # Get latest cmd
+                current_velocity = ControlCmd(
+                vx=self.control_till_horizon.vx[-1], vy=self.control_till_horizon.vy[-1], omega=self.control_till_horizon.omega[-1]
+                )
 
         if local_map_resolution:
             self._planner.set_resolution(local_map_resolution)
 
-        current_velocity = ControlCmd(
-            vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
-        )
-
         if local_map is not None:
             sensor_data = PointCloudData.numpy_to_kompass_cpp(local_map)
         elif laser_scan:
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index ff9fbc36..5b731647 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -296,9 +296,9 @@ class VisionDWA : public DWA {
    */
   Velocity2D getPureTrackingCtrl(const float distance_erro, const float angle_error);
 
-  void generateSearchCommands(double total_rotation, double search_radius,
-                                int max_rotation_steps,
-                                bool enable_pause = false);
+  void generateSearchCommands(float total_rotation, float search_radius,
+                              float max_rotation_time,
+                              bool enable_pause = false);
   void getFindTargetCmds(const int last_direction = 1);
 
   /**
@@ -319,6 +319,9 @@ class VisionDWA : public DWA {
       // Reset recorded wait and search times
       recorded_wait_time_ = 0.0;
       recorded_search_time_ = 0.0;
+      LOG_INFO("Tracking target at position: ",
+                tracked_pose->x(), ", ",
+                tracked_pose->y());
       // Generate reference to target
       Trajectory2D ref_traj;
       if (is_diff_drive_) {
@@ -341,18 +344,23 @@ class VisionDWA : public DWA {
       // ---------------------------------------------------------------
       return result;
     }
-    if (this->hasPath() and !isGoalReached()) {
+    if (this->hasPath() and !isGoalReached() and
+        this->currentPath->getSize() > 1) {
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    } else {
-      LOG_WARNING("No tracking target is received and DWA goal is reached!");
+    }
+    else {
       // Start Search and/or Wait if enabled
       if (config_.enable_search()) {
         if (recorded_search_time_ < config_.target_search_timeout()) {
           if (search_commands_queue_.empty()) {
             LOG_DEBUG("Search commands queue is empty, generating new search "
                       "commands");
-            getFindTargetCmds();
+            int last_direction = 1;
+            if (latest_velocity_command_.omega() < 0){
+              last_direction = -1;
+            }
+            getFindTargetCmds(last_direction);
           }
           LOG_DEBUG("Number of search commands remaining: ",
                     search_commands_queue_.size(),
@@ -360,8 +368,6 @@ class VisionDWA : public DWA {
           // Create search command
           TrajectoryVelocities2D velocities(config_.control_horizon());
           TrajectoryPath path(config_.control_horizon());
-          LOG_DEBUG("Initializing path and velocities with size: ",
-            config_.control_horizon());
           path.add(0, 0.0, 0.0);
           std::array<double, 3> search_command;
           for (int i = 0; i < config_.control_horizon() - 1; i++) {
@@ -385,7 +391,8 @@ class VisionDWA : public DWA {
           result.trajectory = Trajectory2D(velocities, path);
           return result;
         }
-      } else {
+      }
+      else {
         if (recorded_wait_time_ < config_.target_wait_timeout()) {
           auto timeout = config_.target_wait_timeout() - recorded_wait_time_;
           LOG_DEBUG("Target lost, waiting to get tracked target again ... "
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 548f5829..62f47bec 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -152,6 +152,9 @@ Path::Path DWA::findTrackedPathSegment() {
       current_segment_index_ < max_segment_index_) {
     segment_index = current_segment_index_ + 1;
     return currentPath->segments[current_segment_index_ + 1];
+  } else if (closestPosition->index >= currentSegment.getSize() - 1){
+    // Return current segment directly (last segment)
+    return currentPath->segments[current_segment_index_];
   }
   // Else take the segment points from the current point onwards
   else {
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 3a82f621..525ac91a 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -73,17 +73,10 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   Velocity2D followingVel;
   if (abs(distance_error) > config_.dist_tolerance() or
       abs(angle_error) > config_.ang_tolerance()) {
-    // auto term1 =
-    //     (track_velocity_ * tracking_pose.v() * cos(psi)) / cos(angle_error);
-    // auto term2 = -(config_.K_v() * ctrl_limits_.velXParams.maxVel *
-    //                tanh(distance_error)) /
-    //              cos(angle_error);
     double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
                 (config_.K_v() * ctrl_limits_.velXParams.maxVel *
-                 tanh(distance_error / config_.target_distance()))) /
+                 tanh(distance_error))) /
                cos(angle_error);
-    // LOG_DEBUG("v=", v, ", track_velocity_term=", term1,
-    //           ", dist_error_term=", term2);
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     if (std::abs(v) < config_.min_vel()) {
@@ -92,18 +85,10 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     followingVel.setVx(v);
     double omega;
     if (distance > 0.0) {
-      // auto term1_o =
-      //     track_velocity_ * (tracking_pose.omega() -
-      //                        2.0 * sin(psi) / distance * tracking_pose.v());
-      // auto term2_o = -2.0 * v * sin(angle_error) / distance;
-      // auto term3_o = -(config_.K_omega() * tanh(angle_error));
       omega = -track_velocity_ * tracking_pose.omega() -
               track_velocity_ * sin(psi) / distance * tracking_pose.v() -
-              v * sin(angle_error) / distance -
               config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
-                  tanh(angle_error / M_PI_2);
-      // LOG_DEBUG("omega=", omega, ", term_track_velocity=", term1_o,
-      //           ", term_v=", term2_o, ", term_angle_err=", term3_o);
+                  tanh(angle_error);
     } else {
       omega = -track_velocity_ * tracking_pose.omega() -
               config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
@@ -250,31 +235,32 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   return ref_traj;
 };
 
-void VisionDWA::generateSearchCommands(double total_rotation,
-                                       double search_radius,
-                                       int max_rotation_steps,
+void VisionDWA::generateSearchCommands(float total_rotation,
+                                       float search_radius,
+                                       float max_rotation_time,
                                        bool enable_pause) {
   // Calculate rotation direction and magnitude
   double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
-  int rotation_steps = max_rotation_steps;
+  float rotation_time = max_rotation_time;
+  int num_pause_steps = static_cast<int>(
+    config_.search_pause() / config_.control_time_step());
   if (enable_pause) {
     // Modify the total number of active rotation to include the pause steps
-    rotation_steps = (max_rotation_steps + config_.search_pause()) /
-                     (config_.search_pause() + 1);
+    rotation_time = max_rotation_time * (1 - num_pause_steps / config_.control_time_step());
   }
   // Angular velocity to rotate 'total_rotation' in total time steps
   // 'rotation_steps' with dt = control_time_step
-  double rotation_value =
-      (total_rotation / rotation_steps) / config_.control_time_step();
-  rotation_value =
-      std::max(std::min(rotation_value, ctrl_limits_.omegaParams.maxOmega),
+  double omega_val = total_rotation / rotation_time;
+
+  omega_val =
+      std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega),
                config_.min_vel());
   // Generate velocity commands
-  for (int i = 0; i <= rotation_steps; i = i + 1) {
+  for (float t = 0.0f; t <= max_rotation_time; t = t + config_.control_time_step()) {
     if (is_diff_drive_) {
       // In-place rotation
       search_commands_queue_.emplace(
-          std::array<double, 3>{0.0, 0.0, rotation_sign * rotation_value});
+          std::array<double, 3>{0.0, 0.0, rotation_sign * omega_val});
     } else {
       // Angular velocity based on linear velocity and radius
       double omega_ackermann =
@@ -285,7 +271,7 @@ void VisionDWA::generateSearchCommands(double total_rotation,
     }
     if (enable_pause) {
       // Add zero commands for search pause
-      for (int j = 0; j <= config_.search_pause(); j++) {
+      for (int j = 0; j <= num_pause_steps; j++) {
         search_commands_queue_.emplace(std::array<double, 3>{0.0, 0.0, 0.0});
       }
     }
@@ -299,20 +285,19 @@ void VisionDWA::getFindTargetCmds(const int last_direction) {
   LOG_DEBUG("Generating new search commands in direction: ", last_direction);
 
   search_commands_queue_ = std::queue<std::array<double, 3>>();
-  int target_search_steps_max = static_cast<int>(
-      config_.target_search_timeout() / config_.control_time_step());
+  const float target_searchtimeout_part = config_.target_search_timeout() / 4;
   // rotate pi
   generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 4, true);
+                         target_searchtimeout_part);
   // go back
   generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 8, true);
+                         target_searchtimeout_part);
   // rotate -pi
   generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 4, true);
+                         target_searchtimeout_part);
   // go back
   generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
-                         target_search_steps_max / 8, true);
+                         target_searchtimeout_part);
 }
 
 } // namespace Control

From 8c563af44a92765598cc209c111e96d3a048f992 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 6 Jun 2025 10:07:59 +0200
Subject: [PATCH 078/118] (feature) Adds options to use local or global
 coordinates in VisionDWA

---
 src/kompass_core/control/vision_dwa.py        |  44 +++---
 .../include/controllers/vision_dwa.h          |  21 ++-
 .../include/vision/depth_detector.h           |   4 +-
 .../src/controllers/vision_dwa.cpp            |  79 ++++++-----
 .../kompass_cpp/src/datatypes/path.cpp        |   2 +-
 .../kompass_cpp/src/vision/depth_detector.cpp |  13 +-
 .../tests/vision_detector_test.cpp            |  43 +++++-
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 127 +++++++++++-------
 8 files changed, 218 insertions(+), 115 deletions(-)

diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 27582502..2738cd98 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -74,9 +74,9 @@ class VisionDWAConfig(DWAConfig):
 
     enable_search: bool = field(default=False)  # Enable or disable the search mechanism
 
-    track_velocity: bool = field(
+    use_local_coordinates: bool = field(
         default=True
-    )  # Enable or disable tracking the linear/angular velocity of the moving target
+    )  # Track the target using robot local coordinates (no need for robot location at lop step)
 
     error_pose: float = field(
         default=0.05, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
@@ -233,9 +233,10 @@ def set_initial_tracking_image(
         :type detected_boxes: List[Bbox3D]
         """
         try:
-            self._planner.set_current_state(
-                current_state.x, current_state.y, current_state.yaw, current_state.speed
-            )
+            if self._config.use_local_coordinates:
+                self._planner.set_current_state(
+                    current_state.x, current_state.y, current_state.yaw, current_state.speed
+                )
             if any(detected_boxes):
                 return self._planner.set_initial_tracking(
                     pose_x_img,
@@ -256,7 +257,7 @@ def set_initial_tracking_image(
     def loop_step(
         self,
         *,
-        current_state: RobotState,
+        current_state: Optional[RobotState] = None,
         detections_2d: Optional[List[Bbox2D]] = None,
         depth_image: Optional[np.ndarray] = None,
         laser_scan: Optional[LaserScanData] = None,
@@ -276,23 +277,14 @@ def loop_step(
         :return: If planner found a valid solution
         :rtype: bool
         """
-        if current_state is not None:
+        robot_cmd = None
+        if self._config.use_local_coordinates and current_state is not None:
             self._planner.set_current_state(
                 current_state.x, current_state.y, current_state.yaw, current_state.speed
             )
-            current_velocity = ControlCmd(
+            robot_cmd = ControlCmd(
             vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
             )
-        else:
-            if self.control_till_horizon is None:
-                current_velocity = ControlCmd(
-                    vx=0.0, vy=0.0, omega=0.0
-                )
-            else:
-                # Get latest cmd
-                current_velocity = ControlCmd(
-                vx=self.control_till_horizon.vx[-1], vy=self.control_till_horizon.vy[-1], omega=self.control_till_horizon.omega[-1]
-                )
 
         if local_map_resolution:
             self._planner.set_resolution(local_map_resolution)
@@ -311,7 +303,7 @@ def loop_step(
 
         try:
             self._result = self._planner.get_tracking_ctrl(
-                depth_image, detections_2d, current_velocity, sensor_data
+                depth_image, detections_2d, robot_cmd or self._last_cmd, sensor_data
             )
 
         except Exception as e:
@@ -405,3 +397,17 @@ def angular_control(self) -> np.ndarray:
         if self._result.is_found:
             return self.control_till_horizon.omega[: self._end_of_ctrl_horizon]
         return [0.0]
+
+    @property
+    def _last_cmd(self) -> ControlCmd:
+        """
+        Getter of the last command sent to the controller
+
+        :return: Last command sent to the controller
+        :rtype: ControlCmd
+        """
+        return ControlCmd(
+            vx=self.linear_x_control[-1],
+            vy=self.linear_y_control[-1],
+            omega=self.angular_control[-1],
+        )
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 5b731647..a83209cd 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -52,9 +52,9 @@ class VisionDWA : public DWA {
           Parameter(0.0, -M_PI, M_PI,
                     "Bearing angle to maintain with the target (rad)"));
       addParameter(
-          "track_velocity",
-          Parameter(false,
-                    "Track the linear and angular velocity of the target"));
+          "use_local_coordinates",
+          Parameter(true,
+                    "Track the item in the local frame of the robot. This mode cannot track the object velocity but can operate without knowing the robot's absolute position (world frame)"));
       // Search Parameters
       addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
       addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
@@ -83,7 +83,7 @@ class VisionDWA : public DWA {
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
     bool enable_vel_tracking() const {
-      return getParameter<bool>("track_velocity");
+      return not getParameter<bool>("use_local_coordinates");
     }
     double control_time_step() const {
       return getParameter<double>("control_time_step");
@@ -204,8 +204,13 @@ class VisionDWA : public DWA {
     }
     std::optional<TrackedPose2D> tracked_pose = std::nullopt;
     if (!detected_boxes_2d.empty()) {
-      // Send current state to the detector
-      detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
+      if (track_velocity_) {
+        // Send current state to the detector
+        detector_->updateBoxes(aligned_depth_img, detected_boxes_2d,
+                               currentState);
+      } else {
+        detector_->updateBoxes(aligned_depth_img, detected_boxes_2d);
+      }
       auto boxes_3d = detector_->get3dDetections();
       if (boxes_3d) {
         // Update the tracker with the detected boxes
@@ -321,7 +326,9 @@ class VisionDWA : public DWA {
       recorded_search_time_ = 0.0;
       LOG_INFO("Tracking target at position: ",
                 tracked_pose->x(), ", ",
-                tracked_pose->y());
+                tracked_pose->y(), " with velocity: ", tracked_pose->v(), ", ", tracked_pose->omega());
+      LOG_INFO("Robot current position: ", currentState.x, ", ",
+                currentState.y);
       // Generate reference to target
       Trajectory2D ref_traj;
       if (is_diff_drive_) {
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
index 6b1bcc6e..4f340fbc 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -26,7 +26,7 @@ class DepthDetector {
                 const float depth_conversion_factor = 1e-3);
 
   void updateBoxes(const Eigen::MatrixX<unsigned short> aligned_depth_img,
-                   const std::vector<Bbox2D> &detections);
+                   const std::vector<Bbox2D> &detections, const std::optional<Path::State> &robot_state = std::nullopt);
 
   std::optional<std::vector<Bbox3D>> get3dDetections() const;
 
@@ -34,7 +34,7 @@ class DepthDetector {
   float cx_, cy_, fx_, fy_;         // Depth Image camera intrinsics
   float minDepth_, maxDepth_, depthConversionFactor_;
   Eigen::MatrixX<unsigned short> alignedDepthImg_;
-  Eigen::Isometry3f camera_in_body_tf_;
+  Eigen::Isometry3f camera_in_body_tf_, body_in_world_tf_;
   std::unique_ptr<std::vector<Bbox3D>> boxes_;
 
   std::optional<Bbox3D> convert2Dboxto3Dbox(const Bbox2D &box2d);
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 525ac91a..54c9e401 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -58,25 +58,31 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance =
-      tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
+  float distance, psi;
+  if (track_velocity_) {
+    distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) -
+               trajSampler->getRobotRadius();
+    psi = Angle::normalizeToMinusPiPlusPi(
+        std::atan2(tracking_pose.y(), tracking_pose.x()) - currentState.yaw);
+  } else {
+    distance =
+        tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
+    psi = Angle::normalizeToMinusPiPlusPi(
+        std::atan2(tracking_pose.y(), tracking_pose.x()));
+  }
   distance = std::max(distance, 0.0f);
-  float psi = Angle::normalizeToMinusPiPlusPi(
-      std::atan2(tracking_pose.y(), tracking_pose.x()));
 
   float distance_error = config_.target_distance() - distance;
   float angle_error =
       Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
 
-  // LOG_DEBUG("distance_error=", distance_error, ", angle_error=", angle_error);
-
   Velocity2D followingVel;
   if (abs(distance_error) > config_.dist_tolerance() or
       abs(angle_error) > config_.ang_tolerance()) {
-    double v = ((track_velocity_ * tracking_pose.v() * cos(psi)) -
-                (config_.K_v() * ctrl_limits_.velXParams.maxVel *
-                 tanh(distance_error))) /
-               cos(angle_error);
+    double v =
+        track_velocity_ * tracking_pose.v() * cos(angle_error) -
+        config_.K_v() * ctrl_limits_.velXParams.maxVel * tanh(distance_error);
+
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
                    ctrl_limits_.velXParams.maxVel);
     if (std::abs(v) < config_.min_vel()) {
@@ -84,16 +90,12 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     }
     followingVel.setVx(v);
     double omega;
-    if (distance > 0.0) {
-      omega = -track_velocity_ * tracking_pose.omega() -
-              track_velocity_ * sin(psi) / distance * tracking_pose.v() -
-              config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
-                  tanh(angle_error);
-    } else {
-      omega = -track_velocity_ * tracking_pose.omega() -
-              config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
-                  tanh(angle_error);
-    }
+
+    omega = 2.0 *
+            (track_velocity_ * tracking_pose.v() * sin(angle_error) / distance -
+             config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
+                 tanh(angle_error));
+
     omega = std::clamp(omega, -ctrl_limits_.omegaParams.maxOmega,
                        ctrl_limits_.omegaParams.maxOmega);
     if (std::abs(omega) < config_.min_vel()) {
@@ -143,8 +145,12 @@ bool VisionDWA::setInitialTracking(
         "DepthDetector is not initialized with the camera intrinsics. Call "
         "'VisionDWA::setCameraIntrinsics' first");
   }
-  // Send current state to the detector
-  detector_->updateBoxes(aligned_depth_image, {target_box_2d});
+  if (track_velocity_) {
+    // Send current state to the detector
+    detector_->updateBoxes(aligned_depth_image, {target_box_2d}, currentState);
+  } else {
+    detector_->updateBoxes(aligned_depth_image, {target_box_2d});
+  }
   auto boxes_3d = detector_->get3dDetections();
   if (!boxes_3d) {
     LOG_DEBUG("Failed to get 3D box from 2D target box");
@@ -172,7 +178,9 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
     simulated_state.update(cmd, config_.control_time_step());
-    simulated_track.update(config_.control_time_step());
+    if (track_velocity_) {
+      simulated_track.update(config_.control_time_step());
+    }
     if (step < config_.prediction_horizon() - 1) {
       ref_traj.velocities.add(step, cmd);
     }
@@ -207,6 +215,9 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
                         Path::Point(simulated_state.x, simulated_state.y, 0.0));
       auto vel_rotate = Velocity2D(0.0, 0.0, cmd.omega());
       simulated_state.update(vel_rotate, config_.control_time_step());
+      if (track_velocity_) {
+        simulated_track.update(config_.control_time_step());
+      }
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, vel_rotate);
       }
@@ -216,6 +227,9 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
         ref_traj.path.add(
             step, Path::Point(simulated_state.x, simulated_state.y, 0.0));
         simulated_state.update(vel_move, config_.control_time_step());
+        if (track_velocity_) {
+          simulated_track.update(config_.control_time_step());
+        }
         ref_traj.velocities.add(step, vel_move);
         step++;
       }
@@ -223,7 +237,9 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
       ref_traj.path.add(step,
                         Path::Point(simulated_state.x, simulated_state.y, 0.0));
       simulated_state.update(cmd, config_.control_time_step());
-      simulated_track.update(config_.control_time_step());
+      if (track_velocity_) {
+        simulated_track.update(config_.control_time_step());
+      }
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, cmd);
       }
@@ -242,21 +258,22 @@ void VisionDWA::generateSearchCommands(float total_rotation,
   // Calculate rotation direction and magnitude
   double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
   float rotation_time = max_rotation_time;
-  int num_pause_steps = static_cast<int>(
-    config_.search_pause() / config_.control_time_step());
+  int num_pause_steps =
+      static_cast<int>(config_.search_pause() / config_.control_time_step());
   if (enable_pause) {
     // Modify the total number of active rotation to include the pause steps
-    rotation_time = max_rotation_time * (1 - num_pause_steps / config_.control_time_step());
+    rotation_time =
+        max_rotation_time * (1 - num_pause_steps / config_.control_time_step());
   }
   // Angular velocity to rotate 'total_rotation' in total time steps
   // 'rotation_steps' with dt = control_time_step
   double omega_val = total_rotation / rotation_time;
 
-  omega_val =
-      std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega),
-               config_.min_vel());
+  omega_val = std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega),
+                       config_.min_vel());
   // Generate velocity commands
-  for (float t = 0.0f; t <= max_rotation_time; t = t + config_.control_time_step()) {
+  for (float t = 0.0f; t <= max_rotation_time;
+       t = t + config_.control_time_step()) {
     if (is_diff_drive_) {
       // In-place rotation
       search_commands_queue_.emplace(
diff --git a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
index 0c8858ec..64a9d7bb 100644
--- a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
@@ -311,7 +311,7 @@ void Path::segment(double pathSegmentLength) {
     new_segment.resize(this->max_segment_size);
     segments.push_back(new_segment);
   } else {
-    int segmentsNumber = max(totalLength / pathSegmentLength, 1.0);
+    int segmentsNumber = max(static_cast<int>(totalLength / pathSegmentLength), 1);
     if (segmentsNumber == 1) {
       auto new_segment = *this;
       new_segment.resize(this->max_segment_size);
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index adefed40..5394c7fd 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -40,6 +40,8 @@ DepthDetector::DepthDetector(
   fy_ = focal_length.y();
   cx_ = principal_point.x();
   cy_ = principal_point.y();
+
+  body_in_world_tf_ = Eigen::Isometry3f::Identity();
 }
 
 std::optional<std::vector<Bbox3D>> DepthDetector::get3dDetections() const {
@@ -51,7 +53,11 @@ std::optional<std::vector<Bbox3D>> DepthDetector::get3dDetections() const {
 
 void DepthDetector::updateBoxes(
     const Eigen::MatrixX<unsigned short> aligned_depth_img,
-    const std::vector<Bbox2D> &detections) {
+    const std::vector<Bbox2D> &detections,
+    const std::optional<Path::State> &robot_state) {
+  if(robot_state.has_value()) {
+    body_in_world_tf_ = getTransformation(robot_state.value());
+  }
   alignedDepthImg_ = aligned_depth_img;
   boxes_ = std::make_unique<std::vector<Bbox3D>>();
   for (auto box2d : detections) {
@@ -116,14 +122,15 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
   size_camera_frame(1) = box2d.size.y() * medianDepth / this->fy_;
   size_camera_frame(2) = maximum_d - minimum_d;
 
+  Eigen::Isometry3f camera_in_world_tf = body_in_world_tf_ * camera_in_body_tf_;
   // Register center in the world frame
-  box3d.center = camera_in_body_tf_ * center_in_camera_frame;
+  box3d.center = camera_in_world_tf * center_in_camera_frame;
 
   LOG_DEBUG("Got detected box in 3D coordinates at :", box3d.center.x(), ", ",
             box3d.center.y(), ", ", box3d.center.z());
 
   // Transform size from camera frame to world frame
-  Eigen::Matrix3f abs_rotation = camera_in_body_tf_.linear().cwiseAbs();
+  Eigen::Matrix3f abs_rotation = camera_in_world_tf.linear().cwiseAbs();
   box3d.size = abs_rotation * size_camera_frame;
 
   return box3d;
diff --git a/src/kompass_cpp/tests/vision_detector_test.cpp b/src/kompass_cpp/tests/vision_detector_test.cpp
index 744929b8..fbc356ba 100644
--- a/src/kompass_cpp/tests/vision_detector_test.cpp
+++ b/src/kompass_cpp/tests/vision_detector_test.cpp
@@ -4,6 +4,7 @@
 #include "utils/transformation.h"
 #include "vision/depth_detector.h"
 #include <Eigen/Dense>
+#include <cmath>
 #include <opencv2/opencv.hpp>
 #include <stdexcept>
 #define BOOST_TEST_MODULE KOMPASS TESTS
@@ -16,7 +17,7 @@ using namespace Kompass;
 
 struct DepthDetectorTestConfig {
   std::unique_ptr<DepthDetector> detector;
-  Path::State current_state = {0.0, 0.0, 0.0};
+  Path::State current_state = {1.0, 1.0, 0.0};
   std::vector<Bbox2D> detected_boxes;
   std::string pltFileName = "DepthDetectorTest";
   Eigen::Vector2f focal_length = {911.71, 910.288};
@@ -64,8 +65,15 @@ struct DepthDetectorTestConfig {
     detections.push_back(box);
   };
 
-  std::vector<Bbox3D> run(const std::string outputFilename) {
-    detector->updateBoxes(depth_image, detections);
+  std::vector<Bbox3D> run(const std::string outputFilename,
+                          const bool local_frame = true) {
+    if(local_frame){
+      detector->updateBoxes(depth_image, detections);
+    }
+    else{
+      detector->updateBoxes(depth_image, detections, current_state);
+    }
+
     auto boxes3D = detector->get3dDetections();
     if (boxes3D) {
       auto res = boxes3D.value();
@@ -119,7 +127,9 @@ BOOST_AUTO_TEST_CASE(test_Depth_Detector_person_image) {
   auto boxes = config.run(outputFilename);
 }
 
-BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image) {
+
+BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image_local_frame) {
+  LOG_INFO("Testing and generating 3D boxes in local robot frame");
   // Create timer
   Timer time;
   std::string filename =
@@ -128,9 +138,30 @@ BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image) {
   auto config = DepthDetectorTestConfig(filename, box);
   std::string outputFilename =
       "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg";
-  auto boxes = config.run(outputFilename);
+  auto boxes = config.run(outputFilename, true);
+  float dist =
+      std::sqrt(std::pow((boxes[0].center.x()), 2) +
+                std::pow((boxes[0].center.y()), 2));
+  const float approx_actual_dist = 1.8;
+  BOOST_TEST(std::abs(dist - approx_actual_dist) <= 0.1,
+             "3D box distance is not equal to approximate measured distance");
+}
 
+BOOST_AUTO_TEST_CASE(test_Depth_Detector_bag_image_global_frame) {
+  LOG_INFO("Testing and generating 3D boxes in global world frame");
+  // Create timer
+  Timer time;
+  std::string filename =
+      "/home/ahr/kompass/uvmap_code/resources/bag_image_depth.tif";
+  Bbox2D box({410, 0}, {410, 390});
+  auto config = DepthDetectorTestConfig(filename, box);
+  std::string outputFilename =
+      "/home/ahr/kompass/uvmap_code/resources/bag_depth_output.jpg";
+  auto boxes = config.run(outputFilename, false);
+  float dist =
+      std::sqrt(std::pow((boxes[0].center.x() - config.current_state.x), 2) +
+                std::pow((boxes[0].center.y() - config.current_state.y), 2));
   const float approx_actual_dist = 1.8;
-  BOOST_TEST(std::abs(boxes[0].center.x() - approx_actual_dist) <= 0.1,
+  BOOST_TEST(std::abs(dist - approx_actual_dist) <= 0.1,
              "3D box distance is not equal to approximate measured distance");
 }
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index e7fc7522..f4bfee3f 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -1,4 +1,5 @@
 #include "controllers/vision_dwa.h"
+#include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/tracking.h"
@@ -19,8 +20,9 @@
 
 using namespace Kompass;
 
-
 struct VisionDWATestConfig {
+  bool use_local_frame;
+
   // Sampling configuration
   double timeStep;
   int predictionHorizon;
@@ -73,6 +75,7 @@ struct VisionDWATestConfig {
 
   // Constructor to initialize the struct
   VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
+                      const bool use_local_frame = true,
                       const double timeStep = 0.1,
                       const int predictionHorizon = 20,
                       const int controlHorizon = 2,
@@ -83,7 +86,7 @@ struct VisionDWATestConfig {
                       const double reference_path_distance_weight = 1.0,
                       const double goal_distance_weight = 1.0,
                       const double obstacles_distance_weight = 0.0)
-      : timeStep(timeStep), predictionHorizon(predictionHorizon),
+      : use_local_frame(use_local_frame), timeStep(timeStep), predictionHorizon(predictionHorizon),
         controlHorizon(controlHorizon), maxLinearSamples(maxLinearSamples),
         maxAngularSamples(maxAngularSamples), maxNumThreads(maxNumThreads),
         octreeRes(0.1), x_params(maxVel, 5.0, 10.0), y_params(1, 3, 5),
@@ -107,13 +110,15 @@ struct VisionDWATestConfig {
     config.setParameter("control_time_step", timeStep);
     config.setParameter("control_horizon", controlHorizon);
     config.setParameter("prediction_horizon", predictionHorizon);
-    config.setParameter("track_velocity", false);
-    // config.setParameter("target_orientation", 0.2);
+    config.setParameter("use_local_coordinates", use_local_frame);
+    if(!use_local_frame){
+      config.setParameter("speed_gain", 0.1);
+    }
 
     // For depth config
     // Body to camera tf from robot of test pictures
     auto body_to_link_tf =
-    getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
+        getTransformation(Eigen::Quaternionf{0.0f, 0.1987f, 0.0f, 0.98f},
                           Eigen::Vector3f{0.32f, 0.0209f, 0.3f});
 
     auto link_to_cam_tf =
@@ -128,8 +133,10 @@ struct VisionDWATestConfig {
         body_to_link_tf * link_to_cam_tf * cam_to_cam_opt_tf;
 
     Eigen::Vector3f translation = body_to_cam_tf.translation();
-    Eigen::Quaternionf rotation_quat = Eigen::Quaternionf(body_to_cam_tf.rotation());
-    Eigen::Vector4f rotation = {rotation_quat.w(), rotation_quat.x(), rotation_quat.y(), rotation_quat.z()};
+    Eigen::Quaternionf rotation_quat =
+        Eigen::Quaternionf(body_to_cam_tf.rotation());
+    Eigen::Vector4f rotation = {rotation_quat.w(), rotation_quat.x(),
+                                rotation_quat.y(), rotation_quat.z()};
 
     controller = std::make_unique<Control::VisionDWA>(
         controlType, controlLimits, maxLinearSamples, maxAngularSamples,
@@ -210,7 +217,7 @@ struct VisionDWATestConfig {
   }
 
   bool run_test(const int numPointsPerTrajectory, std::string pltFileName,
-                 bool simulate_obstacle=false) {
+                const bool simulate_obstacle = false) {
     Control::TrajectorySamples2D samples(2, numPointsPerTrajectory);
     Control::TrajectoryVelocities2D simulated_velocities(
         numPointsPerTrajectory);
@@ -219,10 +226,9 @@ struct VisionDWATestConfig {
     Control::Velocity2D cmd;
     Control::TrajSearchResult result;
 
-
-    controller->setInitialTracking(ref_point_img(0),
-                                     ref_point_img(1), detected_boxes);
-
+    controller->setCurrentState(robotState);
+    controller->setInitialTracking(ref_point_img(0), ref_point_img(1),
+                                   detected_boxes);
 
     int step = 0;
     while (step < numPointsPerTrajectory) {
@@ -234,25 +240,31 @@ struct VisionDWATestConfig {
                ", ", tracked_pose.yaw());
       LOG_INFO("Robot at: ", point.x(), ", ", point.y());
 
-      // Transform boxes to local coordinates
-      std::vector<Bbox3D> boxes_local_coordinates;
-      Eigen::Isometry3f world_in_robot_tf = getTransformation(robotState).inverse();
-      Eigen::Matrix3f abs_rotation = world_in_robot_tf.linear().cwiseAbs();
-      for (auto &box : detected_boxes) {
-        // Transform the box to the robot frame
-        Bbox3D box_local(box);
-        box_local.center = world_in_robot_tf * box.center;
-        box_local.size = abs_rotation * box.size;
-        boxes_local_coordinates.push_back(box_local);
+      std::vector<Bbox3D> seen_boxes;
+      if (use_local_frame) {
+        // Transform boxes to local coordinates
+        std::vector<Bbox3D> boxes_local_coordinates;
+        Eigen::Isometry3f world_in_robot_tf =
+            getTransformation(robotState).inverse();
+        Eigen::Matrix3f abs_rotation = world_in_robot_tf.linear().cwiseAbs();
+        for (auto &box : detected_boxes) {
+          // Transform the box to the robot frame
+          Bbox3D box_local(box);
+          box_local.center = world_in_robot_tf * box.center;
+          box_local.size = abs_rotation * box.size;
+          boxes_local_coordinates.push_back(box_local);
+        }
+        seen_boxes = boxes_local_coordinates;
+      } else {
+        seen_boxes = detected_boxes;
       }
 
-      auto seen_boxes = boxes_local_coordinates;
       // Simulate lost of target around obstacle
       if (simulate_obstacle and robotState.y > 0.2 and robotState.y < 0.4) {
         LOG_INFO("Sending EMPTY BOXES NOW!!!!!");
         seen_boxes = {};
       }
-
+      controller->setCurrentState(robotState);
       result = controller->getTrackingCtrl(seen_boxes, cmd, cloud);
 
       if (result.isTrajFound) {
@@ -268,8 +280,8 @@ struct VisionDWATestConfig {
         }
         auto velocities = result.trajectory.velocities;
         int numSteps = 0;
-        for (auto vel: velocities) {
-          if(numSteps >= controlHorizon){
+        for (auto vel : velocities) {
+          if (numSteps >= controlHorizon) {
             break;
           }
           numSteps++;
@@ -326,21 +338,43 @@ struct VisionDWATestConfig {
   }
 };
 
+BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free_local) {
+  // Create timer
+  Timer time;
+
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+  bool use_local_frame = true;
+
+  VisionDWATestConfig testConfig(cloud, use_local_frame);
+
+  int numPointsPerTrajectory = 100;
+
+  bool test_passed = testConfig.run_test(
+      numPointsPerTrajectory,
+      std::string("vision_follower_with_tracker_local_frame"));
+  BOOST_TEST(test_passed,
+             "VisionDWA Failed To Find Control in local frame mode");
+}
 
-BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free) {
+BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free_global_frame) {
   // Create timer
   Timer time;
 
   // Robot pointcloud values (global frame)
   std::vector<Path::Point> cloud = {{10.0, 10.0, 0.1}};
+  bool use_local_frame = false;
 
-  VisionDWATestConfig testConfig(cloud);
+  VisionDWATestConfig testConfig(cloud, use_local_frame);
 
   int numPointsPerTrajectory = 100;
 
   bool test_passed = testConfig.run_test(
-      numPointsPerTrajectory, std::string("vision_follower_with_tracker"));
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
+      numPointsPerTrajectory,
+      std::string("vision_follower_with_tracker_global_frame"),
+      use_local_frame);
+  BOOST_TEST(test_passed,
+             "VisionDWA Failed To Find Control in global frame mode");
 }
 
 // BOOST_AUTO_TEST_CASE(test_VisionDWA_with_tracker_and_obstacle) {
@@ -360,25 +394,26 @@ BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free) {
 //   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 // }
 
-BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
-  // Create timer
-  Timer time;
+// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
+//   // Create timer
+//   Timer time;
 
-  std::string filename =
-      "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
-      "bag_image_depth.tif";
-  Bbox2D box({410, 0}, {410, 390});
-  std::vector<Bbox2D> detections_2d{box};
+//   std::string filename =
+//       "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
+//       "bag_image_depth.tif";
+//   Bbox2D box({410, 0}, {410, 390});
+//   std::vector<Bbox2D> detections_2d{box};
 
-  auto initial_point = Eigen::Vector2i{610, 200};
+//   auto initial_point = Eigen::Vector2i{610, 200};
 
-  // Robot pointcloud values (global frame)
-  std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
+//   // Robot pointcloud values (global frame)
+//   std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
 
-  VisionDWATestConfig testConfig(cloud);
+//   VisionDWATestConfig testConfig(cloud);
 
-  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
-                                                   initial_point, cloud);
+//   auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
+//                                                    initial_point, cloud);
 
-  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
-}
+//   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth
+//   Image");
+// }

From 220f4891be06bcf6f0782ddf6de02431d1758c08 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 6 Jun 2025 12:31:41 +0200
Subject: [PATCH 079/118] (fix) Fixes error in reference following segment
 generation

Fixes tracked state update in local frame
---
 .../kompass_cpp/include/datatypes/control.h   | 10 +++++++
 .../src/controllers/vision_dwa.cpp            | 30 ++++++++++++++-----
 2 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index fc6037a8..e86e9805 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -130,6 +130,11 @@ class Velocity2D {
   void setOmega(double const value) { velocity_(2) = value; }
   void setSteerAng(double const value) { velocity_(3) = value; }
 
+  // Overload the unary minus operator
+  Velocity2D operator-() const {
+    return Velocity2D(-velocity_(0), -velocity_(1), -velocity_(2));
+  }
+
 private:
   Eigen::Vector4d velocity_{0.0, 0.0, 0.0, 0.0};
 };
@@ -166,6 +171,11 @@ class TrackedPose2D : public Pose3D {
     setRotation(0.0, 0.0, yaw);
   }
 
+  void update(const Velocity2D& vel, const float timeStep) {
+    vel_ = vel;
+    update(timeStep);
+  }
+
   float distance(const float x, const float y, const float z = 0.0) const{
     return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
                 pow(position_.z() - z, 2));
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index 54c9e401..c222a448 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -164,9 +164,8 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  Path::State simulated_state = currentState;
-  Path::State original_state = currentState;
-  TrackedPose2D simulated_track = tracking_pose;
+  auto simulated_state = Path::State(currentState);
+  auto simulated_track = TrackedPose2D(tracking_pose);
   Velocity2D cmd;
 
   // Simulate following the tracked target for the period til
@@ -180,6 +179,10 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
     simulated_state.update(cmd, config_.control_time_step());
     if (track_velocity_) {
       simulated_track.update(config_.control_time_step());
+    } else {
+      // Update the tracked state in the local frame after the robot have
+      // moved (apply inverse of robot velocity)
+      simulated_track.update(-cmd, config_.control_time_step());
     }
     if (step < config_.prediction_horizon() - 1) {
       ref_traj.velocities.add(step, cmd);
@@ -187,7 +190,7 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
 
     step++;
   }
-  this->setCurrentState(original_state);
+  this->setCurrentState(currentState);
 
   return ref_traj;
 };
@@ -197,9 +200,8 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  Path::State simulated_state = currentState;
-  Path::State original_state = currentState;
-  TrackedPose2D simulated_track = tracking_pose;
+  auto simulated_state = Path::State(currentState);
+  auto simulated_track = TrackedPose2D(tracking_pose);
   Velocity2D cmd;
 
   // Simulate following the tracked target for the period til
@@ -217,6 +219,9 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
       simulated_state.update(vel_rotate, config_.control_time_step());
       if (track_velocity_) {
         simulated_track.update(config_.control_time_step());
+      }else{
+        // Update the tracked state in the local frame after the robot have moved (apply inverse of robot velocity)
+        simulated_track.update(-vel_rotate, config_.control_time_step());
       }
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, vel_rotate);
@@ -229,6 +234,10 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
         simulated_state.update(vel_move, config_.control_time_step());
         if (track_velocity_) {
           simulated_track.update(config_.control_time_step());
+        } else {
+          // Update the tracked state in the local frame after the robot have
+          // moved (apply inverse of robot velocity)
+          simulated_track.update(-vel_move, config_.control_time_step());
         }
         ref_traj.velocities.add(step, vel_move);
         step++;
@@ -239,14 +248,19 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
       simulated_state.update(cmd, config_.control_time_step());
       if (track_velocity_) {
         simulated_track.update(config_.control_time_step());
+      } else {
+        // Update the tracked state in the local frame after the robot have
+        // moved (apply inverse of robot velocity)
+        simulated_track.update(-cmd, config_.control_time_step());
       }
+
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, cmd);
       }
       step++;
     }
   }
-  this->setCurrentState(original_state);
+  this->setCurrentState(currentState);
 
   return ref_traj;
 };

From d99e5f1dffd3e9b4695a5de664cef139ca3303c4 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 6 Jun 2025 16:32:40 +0200
Subject: [PATCH 080/118] (fix) Updates tests and adds debug logging

---
 .../src/controllers/vision_dwa.cpp            | 58 +++++++++++++++----
 src/kompass_cpp/tests/vision_dwa_test.cpp     | 38 ++++++------
 2 files changed, 65 insertions(+), 31 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index c222a448..ff07da28 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -58,12 +58,14 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
 }
 
 Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
-  float distance, psi;
+  float distance, psi, gamma = 0.0f;
   if (track_velocity_) {
     distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) -
                trajSampler->getRobotRadius();
     psi = Angle::normalizeToMinusPiPlusPi(
         std::atan2(tracking_pose.y(), tracking_pose.x()) - currentState.yaw);
+    gamma =
+        Angle::normalizeToMinusPiPlusPi(tracking_pose.yaw() - currentState.yaw);
   } else {
     distance =
         tracking_pose.distance(0.0, 0.0, 0.0) - trajSampler->getRobotRadius();
@@ -74,13 +76,13 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
 
   float distance_error = config_.target_distance() - distance;
   float angle_error =
-      Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - psi);
+      Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - gamma - psi);
 
   Velocity2D followingVel;
   if (abs(distance_error) > config_.dist_tolerance() or
       abs(angle_error) > config_.ang_tolerance()) {
     double v =
-        track_velocity_ * tracking_pose.v() * cos(angle_error) -
+        track_velocity_ * (tracking_pose.v() * cos(gamma - psi)) -
         config_.K_v() * ctrl_limits_.velXParams.maxVel * tanh(distance_error);
 
     v = std::clamp(v, -ctrl_limits_.velXParams.maxVel,
@@ -92,7 +94,8 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
     double omega;
 
     omega = 2.0 *
-            (track_velocity_ * tracking_pose.v() * sin(angle_error) / distance -
+            (track_velocity_ * tracking_pose.v() * sin(gamma - psi) / distance +
+             v * sin(psi) / distance -
              config_.K_omega() * ctrl_limits_.omegaParams.maxOmega *
                  tanh(angle_error));
 
@@ -164,8 +167,13 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  auto simulated_state = Path::State(currentState);
+  Path::State simulated_state(0.0, 0.0, 0.0);
+  if(track_velocity_){
+    // Global frame -> get actual state
+    simulated_state = Path::State(currentState);
+  }
   auto simulated_track = TrackedPose2D(tracking_pose);
+  Eigen::Isometry3f tracked_state_tf = Eigen::Isometry3f::Identity();
   Velocity2D cmd;
 
   // Simulate following the tracked target for the period til
@@ -182,7 +190,13 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
     } else {
       // Update the tracked state in the local frame after the robot have
       // moved (apply inverse of robot velocity)
-      simulated_track.update(-cmd, config_.control_time_step());
+      tracked_state_tf =
+          getTransformation(simulated_state);
+      auto new_state = transformPosition(
+          Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
+          tracked_state_tf.inverse());
+      simulated_track = TrackedPose2D(new_state.x(), new_state.y(),
+                      simulated_track.z(), 0.0, 0.0, 0.0);
     }
     if (step < config_.prediction_horizon() - 1) {
       ref_traj.velocities.add(step, cmd);
@@ -200,16 +214,25 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   int step = 0;
 
   Trajectory2D ref_traj(config_.prediction_horizon());
-  auto simulated_state = Path::State(currentState);
+  Path::State simulated_state(0.0, 0.0, 0.0);
+  if (track_velocity_) {
+    // Global frame -> get actual state
+    simulated_state = Path::State(currentState);
+  }
   auto simulated_track = TrackedPose2D(tracking_pose);
+  Eigen::Isometry3f tracked_state_tf = Eigen::Isometry3f::Identity();
   Velocity2D cmd;
 
   // Simulate following the tracked target for the period til
   // prediction_horizon assuming the target moves with its same current
   // velocity
   while (step < config_.prediction_horizon()) {
+    LOG_DEBUG("Step: ", step, "Simulated robot at", simulated_state.x, ",",
+             simulated_state.y, " simulated target at ",
+             simulated_track.x(), ",", simulated_track.y());
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
+    LOG_DEBUG("Got CMD: ", cmd.vx(), ",", cmd.omega());
     if (std::abs(cmd.vx()) >= config_.min_vel() &&
         std::abs(cmd.omega()) >= config_.min_vel()) {
       // Rotate then Move
@@ -221,7 +244,12 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
         simulated_track.update(config_.control_time_step());
       }else{
         // Update the tracked state in the local frame after the robot have moved (apply inverse of robot velocity)
-        simulated_track.update(-vel_rotate, config_.control_time_step());
+        tracked_state_tf = getTransformation(simulated_state);
+        auto new_state = transformPosition(
+            Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
+            tracked_state_tf.inverse());
+        simulated_track = TrackedPose2D(new_state.x(), new_state.y(),
+                                        simulated_track.z(), 0.0, 0.0, 0.0);
       }
       if (step < config_.prediction_horizon() - 1) {
         ref_traj.velocities.add(step, vel_rotate);
@@ -237,7 +265,12 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
         } else {
           // Update the tracked state in the local frame after the robot have
           // moved (apply inverse of robot velocity)
-          simulated_track.update(-vel_move, config_.control_time_step());
+          tracked_state_tf = getTransformation(simulated_state);
+          auto new_state = transformPosition(
+              Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
+              tracked_state_tf.inverse());
+          simulated_track = TrackedPose2D(new_state.x(), new_state.y(),
+                                          simulated_track.z(), 0.0, 0.0, 0.0);
         }
         ref_traj.velocities.add(step, vel_move);
         step++;
@@ -251,7 +284,12 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
       } else {
         // Update the tracked state in the local frame after the robot have
         // moved (apply inverse of robot velocity)
-        simulated_track.update(-cmd, config_.control_time_step());
+        tracked_state_tf = getTransformation(simulated_state);
+        auto new_state = transformPosition(
+            Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
+            tracked_state_tf.inverse());
+        simulated_track = TrackedPose2D(new_state.x(), new_state.y(),
+                                        simulated_track.z(), 0.0, 0.0, 0.0);
       }
 
       if (step < config_.prediction_horizon() - 1) {
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index f4bfee3f..296223fb 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -77,7 +77,7 @@ struct VisionDWATestConfig {
   VisionDWATestConfig(const std::vector<Path::Point> sensor_points,
                       const bool use_local_frame = true,
                       const double timeStep = 0.1,
-                      const int predictionHorizon = 20,
+                      const int predictionHorizon = 6,
                       const int controlHorizon = 2,
                       const int maxLinearSamples = 20,
                       const int maxAngularSamples = 20,
@@ -111,9 +111,6 @@ struct VisionDWATestConfig {
     config.setParameter("control_horizon", controlHorizon);
     config.setParameter("prediction_horizon", predictionHorizon);
     config.setParameter("use_local_coordinates", use_local_frame);
-    if(!use_local_frame){
-      config.setParameter("speed_gain", 0.1);
-    }
 
     // For depth config
     // Body to camera tf from robot of test pictures
@@ -394,26 +391,25 @@ BOOST_AUTO_TEST_CASE(Test_VisionDWA_Obstacle_Free_global_frame) {
 //   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control");
 // }
 
-// BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
-//   // Create timer
-//   Timer time;
+BOOST_AUTO_TEST_CASE(test_VisionDWA_with_depth_image) {
+  // Create timer
+  Timer time;
 
-//   std::string filename =
-//       "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
-//       "bag_image_depth.tif";
-//   Bbox2D box({410, 0}, {410, 390});
-//   std::vector<Bbox2D> detections_2d{box};
+  std::string filename =
+      "/home/ahr/kompass/kompass-navigation/tests/resources/control/"
+      "bag_image_depth.tif";
+  Bbox2D box({410, 0}, {410, 390});
+  std::vector<Bbox2D> detections_2d{box};
 
-//   auto initial_point = Eigen::Vector2i{610, 200};
+  auto initial_point = Eigen::Vector2i{610, 200};
 
-//   // Robot pointcloud values (global frame)
-//   std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
+  // Robot pointcloud values (global frame)
+  std::vector<Path::Point> cloud = {{10.3, 10.5, 0.2}};
 
-//   VisionDWATestConfig testConfig(cloud);
+  VisionDWATestConfig testConfig(cloud);
 
-//   auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
-//                                                    initial_point, cloud);
+  auto test_passed = testConfig.test_one_cmd_depth(filename, detections_2d,
+                                                   initial_point, cloud);
 
-//   BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth
-//   Image");
-// }
+  BOOST_TEST(test_passed, "VisionDWA Failed To Find Control Using Depth Image");
+}

From ad98443ba1312508805ef2d6c797c7be4ee18df8 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 9 Jun 2025 15:47:16 +0200
Subject: [PATCH 081/118] (refactor) Updates old VisionFollower to use Bbox2D
 datatype and renames to RGBFollower

---
 src/kompass_core/control/__init__.py          |   2 +-
 .../{vision_follower.py => rgb_follower.py}   |  31 +-
 src/kompass_core/control/vision_dwa.py        |   7 +-
 src/kompass_core/datatypes/__init__.py        |   3 -
 src/kompass_core/datatypes/vision.py          |  46 ---
 src/kompass_cpp/bindings/bindings_control.cpp |  20 +-
 src/kompass_cpp/bindings/bindings_types.cpp   |  12 -
 .../include/controllers/rgb_follower.h        | 109 +++++++
 .../include/controllers/vision_dwa.h          |  63 +---
 .../include/controllers/vision_follower.h     | 105 -------
 .../kompass_cpp/include/datatypes/tracking.h  |  50 ++-
 .../src/controllers/rgb_follower.cpp          | 261 ++++++++++++++++
 .../src/controllers/vision_follower.cpp       | 275 -----------------
 src/kompass_cpp/tests/vision_dwa_test.cpp     |   1 -
 tests/test_controllers.py                     | 288 +++++++++---------
 15 files changed, 587 insertions(+), 686 deletions(-)
 rename src/kompass_core/control/{vision_follower.py => rgb_follower.py} (82%)
 delete mode 100644 src/kompass_core/datatypes/vision.py
 create mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
 delete mode 100644 src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
 delete mode 100644 src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp

diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index 188f3cc1..48bbcaaa 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -10,7 +10,7 @@
 from .dvz import DVZ, DVZConfig
 from .dwa import DWA, DWAConfig
 from .stanley import StanleyConfig, Stanley
-from .vision_follower import VisionFollower, VisionFollowerConfig
+from .rgb_follower import VisionFollower, VisionFollowerConfig
 from .vision_dwa import VisionDWAConfig, VisionDWA
 
 ControllerType = FollowerTemplate
diff --git a/src/kompass_core/control/vision_follower.py b/src/kompass_core/control/rgb_follower.py
similarity index 82%
rename from src/kompass_core/control/vision_follower.py
rename to src/kompass_core/control/rgb_follower.py
index c32cd755..9c4b8fb6 100644
--- a/src/kompass_core/control/vision_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -4,8 +4,8 @@
 from attrs import define, field
 from ..utils.common import BaseAttrs, base_validators
 from ..models import Robot, RobotCtrlLimits, RobotType
-from ..datatypes import TrackingData
-import kompass_cpp
+from kompass_cpp.control import RGBFollower, RGBFollowerParameters
+from kompass_cpp.types import Bbox2D
 
 
 @define
@@ -20,7 +20,7 @@ class VisionFollowerConfig(BaseAttrs):
         default=3, validator=base_validators.in_range(min_value=1, max_value=10)
     )
     tolerance: float = field(
-        default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
+        default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1.0)
     )
     target_distance: Optional[float] = field(default=None)
     target_wait_timeout: float = field(
@@ -46,14 +46,14 @@ class VisionFollowerConfig(BaseAttrs):
     )
     enable_search: bool = field(default=True)
 
-    def to_kompass_cpp(self) -> kompass_cpp.control.VisionFollowerParameters:
+    def to_kompass_cpp(self) -> RGBFollowerParameters:
         """
         Convert to kompass_cpp lib config format
 
         :return: _description_
         :rtype: kompass_cpp.control.VisionFollowerParameters
         """
-        vision_config = kompass_cpp.control.VisionFollowerParameters()
+        vision_config = RGBFollowerParameters()
         vision_config.from_dict(self.asdict())
         return vision_config
 
@@ -63,16 +63,16 @@ def __init__(
         self,
         robot: Robot,
         ctrl_limits: RobotCtrlLimits,
-        config: Optional[VisionFollowerConfig] = None,
+        config: Optional[RGBFollowerParameters] = None,
         config_file: Optional[str] = None,
         config_yaml_root_name: Optional[str] = None,
         **_,
     ):
-        config = config or VisionFollowerConfig()
+        config = config or RGBFollowerParameters()
 
         if config_file:
             config.from_yaml(config_file, config_yaml_root_name, get_common=False)
-        self.__controller = kompass_cpp.control.VisionFollower(
+        self.__controller = RGBFollower(
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
             config=config.to_kompass_cpp(),
@@ -81,17 +81,16 @@ def __init__(
         self._found_ctrl = False
         logging.info("VISION TARGET FOLLOWING CONTROLLER IS READY")
 
-    def reset_target(self, tracking: TrackingData):
-        self.__controller.reset_target(tracking.to_kompass_cpp())
+    def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_):
+        self.__controller.reset_target(target_box)
 
     def loop_step(
         self,
         *,
-        tracking: TrackingData,
+        target_box: Bbox2D,
         **_,
     ) -> bool:
-        tracking_cpp = tracking.to_kompass_cpp() if tracking else None
-        self._found_ctrl = self.__controller.run(tracking_cpp)
+        self._found_ctrl = self.__controller.run(target_box)
         if self._found_ctrl:
             self._ctrl = self.__controller.get_ctrl()
         return self._found_ctrl
@@ -113,7 +112,7 @@ def linear_x_control(self):
         :return: Linear Velocity Control (m/s)
         :rtype: List[float]
         """
-        return self._ctrl.vx if self._found_ctrl else None
+        return [self._ctrl.vx] if self._found_ctrl else None
 
     @property
     def linear_y_control(self):
@@ -123,7 +122,7 @@ def linear_y_control(self):
         :return: Linear Velocity Control (m/s)
         :rtype: List[float]
         """
-        return self._ctrl.vy if self._found_ctrl else None
+        return [self._ctrl.vy] if self._found_ctrl else None
 
     @property
     def angular_control(self):
@@ -133,4 +132,4 @@ def angular_control(self):
         :return: Angular Velocity Control (rad/s)
         :rtype: List[float]
         """
-        return self._ctrl.omega if self._found_ctrl else None
+        return [self._ctrl.omega] if self._found_ctrl else None
diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/vision_dwa.py
index 2738cd98..799dc9ea 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/vision_dwa.py
@@ -235,7 +235,10 @@ def set_initial_tracking_image(
         try:
             if self._config.use_local_coordinates:
                 self._planner.set_current_state(
-                    current_state.x, current_state.y, current_state.yaw, current_state.speed
+                    current_state.x,
+                    current_state.y,
+                    current_state.yaw,
+                    current_state.speed,
                 )
             if any(detected_boxes):
                 return self._planner.set_initial_tracking(
@@ -283,7 +286,7 @@ def loop_step(
                 current_state.x, current_state.y, current_state.yaw, current_state.speed
             )
             robot_cmd = ControlCmd(
-            vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
+                vx=current_state.vx, vy=current_state.vy, omega=current_state.omega
             )
 
         if local_map_resolution:
diff --git a/src/kompass_core/datatypes/__init__.py b/src/kompass_core/datatypes/__init__.py
index e9d4722a..1ae6f16f 100644
--- a/src/kompass_core/datatypes/__init__.py
+++ b/src/kompass_core/datatypes/__init__.py
@@ -14,7 +14,6 @@
 )
 from .pointcloud import PointCloudData
 from .pose import PoseData
-from .vision import TrackingData, ImageMetaData
 from kompass_cpp.types import Bbox3D, Bbox2D
 
 __all__ = [
@@ -32,8 +31,6 @@
     "Odom2D",
     "PointCloudData",
     "PoseData",
-    "TrackingData",
-    "ImageMetaData",
     "Bbox3D",
     "Bbox2D",
 ]
diff --git a/src/kompass_core/datatypes/vision.py b/src/kompass_core/datatypes/vision.py
deleted file mode 100644
index ed97601d..00000000
--- a/src/kompass_core/datatypes/vision.py
+++ /dev/null
@@ -1,46 +0,0 @@
-from ..utils.common import BaseAttrs
-from attrs import define, field
-from typing import Optional, List, Union
-import kompass_cpp
-
-
-def _xy_validator(_, attribute, value):
-    if len(value) != 2:
-        raise ValueError(f"Attribute {attribute} should be a list of length 2: [x, y]")  # noqa: F821
-    return
-
-
-def _xy_optional_validator(_, attribute, value):
-    if value and len(value) != 2:
-        raise ValueError(
-            f"Attribute {attribute} should be None or a list of length 2: [x, y]"
-        )  # noqa: F821
-    return
-
-
-@define
-class ImageMetaData(BaseAttrs):
-    frame_id: str = field()
-    width: int = field()
-    height: int = field()
-    encoding: str = field(default="rgb8")
-
-
-@define
-class TrackingData(BaseAttrs):
-    label: str = field()
-    center_xy: List = field(validator=_xy_validator)
-    size_xy: List = field(validator=_xy_validator)
-    id: int = field()
-    img_meta: Union[ImageMetaData, None] = field()
-    velocity_xy: Optional[List] = field(default=None, validator=_xy_optional_validator)
-    depth: Optional[float] = field(default=None)
-
-    def to_kompass_cpp(self):
-        return kompass_cpp.types.TrackingData(
-            size_xy=self.size_xy,
-            center_xy=self.center_xy,
-            img_width=self.img_meta.width,
-            img_height=self.img_meta.height,
-            depth=self.depth or -1.0,
-        )
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 866572a6..93bea427 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -10,7 +10,7 @@
 #include "controllers/pid.h"
 #include "controllers/stanley.h"
 #include "controllers/vision_dwa.h"
-#include "controllers/vision_follower.h"
+#include "controllers/rgb_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/trajectory.h"
 
@@ -219,21 +219,21 @@ void bindings_control(py::module_ &m) {
       .def("set_resolution", &Control::DWA::resetOctreeResolution);
 
   // Vision Follower
-  py::class_<Control::VisionFollower::VisionFollowerConfig,
-             Control::Controller::ControllerParameters>(
-      m_control, "VisionFollowerParameters")
+  py::class_<Control::RGBFollower::RGBFollowerConfig,
+             Control::Controller::ControllerParameters>(m_control,
+                                                        "RGBFollowerParameters")
       .def(py::init<>());
 
-  py::class_<Control::VisionFollower, Control::Controller>(m_control,
-                                                           "VisionFollower")
+  py::class_<Control::RGBFollower, Control::Controller>(m_control,
+                                                        "RGBFollower")
       .def(py::init<const Control::ControlType,
                     const Control::ControlLimitsParams,
-                    const Control::VisionFollower::VisionFollowerConfig>(),
+                    const Control::RGBFollower::RGBFollowerConfig>(),
            py::arg("control_type"), py::arg("control_limits"),
            py::arg("config"))
-      .def("reset_target", &Control::VisionFollower::resetTarget)
-      .def("get_ctrl", &Control::VisionFollower::getCtrl)
-      .def("run", &Control::VisionFollower::run);
+      .def("reset_target", &Control::RGBFollower::resetTarget)
+      .def("get_ctrl", &Control::RGBFollower::getCtrl)
+      .def("run", &Control::RGBFollower::run);
 
   // Vision DWA
   py::class_<Control::VisionDWA::VisionDWAConfig,
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index f8629f90..05a0ef4a 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -6,7 +6,6 @@
 #include <nanobind/stl/vector.h>
 #include <vector>
 
-#include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/tracking.h"
@@ -123,17 +122,6 @@ void bindings_types(py::module_ &m) {
       });
 
   // Vision types
-  py::class_<Control::VisionFollower::TrackingData>(m_types, "TrackingData")
-      .def(py::init<std::array<double, 2>, int, int, std::array<double, 2>,
-                    double>(),
-           py::arg("size_xy"), py::arg("img_width"), py::arg("img_height"),
-           py::arg("center_xy"), py::arg("depth") = -1.0)
-      .def_rw("size_xy", &Control::VisionFollower::TrackingData::size_xy)
-      .def_rw("img_width", &Control::VisionFollower::TrackingData::img_width)
-      .def_rw("img_height", &Control::VisionFollower::TrackingData::img_height)
-      .def_rw("center_xy", &Control::VisionFollower::TrackingData::center_xy)
-      .def_rw("depth", &Control::VisionFollower::TrackingData::depth);
-
   py::class_<Bbox2D>(m_types, "Bbox2D")
       .def(py::init<>())
       .def(py::init<const Bbox2D &>())
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
new file mode 100644
index 00000000..605ffdee
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -0,0 +1,109 @@
+#pragma once
+
+#include "controller.h"
+#include "datatypes/control.h"
+#include "datatypes/parameter.h"
+#include "datatypes/tracking.h"
+#include <memory>
+#include <optional>
+#include <queue>
+
+namespace Kompass {
+namespace Control {
+
+class RGBFollower : public Controller {
+public:
+
+  class RGBFollowerConfig : public ControllerParameters {
+  public:
+    RGBFollowerConfig() : ControllerParameters() {
+      addParameter("control_time_step",
+                   Parameter(0.1, 1e-4, 1e6, "Control time step (s)"));
+      addParameter(
+          "control_horizon",
+          Parameter(2, 1, 1000, "Number of steps for applying the control"));
+      addParameter(
+          "tolerance",
+          Parameter(0.1, 0.0, 1.0, "Tolerance value"));
+      addParameter(
+          "target_distance",
+          Parameter(
+              0.1, -1.0, 1e9,
+              "Target distance to maintain with the target (m)")); // Use -1 for
+                                                                   // None
+      // Search Parameters
+      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
+      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
+      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
+      // Pure tracking control law parameters
+      addParameter("rotation_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
+      addParameter("min_vel", Parameter(0.01, 1e-9, 1e9));
+      addParameter("enable_search", Parameter(false));
+
+    }
+    bool enable_search() const { return getParameter<bool>("enable_search"); }
+    double control_time_step() const {
+      return getParameter<double>("control_time_step");
+    }
+    double target_search_timeout() const {
+      return getParameter<double>("target_search_timeout");
+    }
+    double target_wait_timeout() const {
+      return getParameter<double>("target_wait_timeout");
+    }
+    double target_search_radius() const {
+      return getParameter<double>("target_search_radius");
+    }
+    double search_pause() const {
+      return getParameter<double>("target_search_pause");
+    }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
+    double tolerance() const { return getParameter<double>("tolerance"); }
+    double target_distance() const {
+      double val = getParameter<double>("target_distance");
+      return val < 0 ? -1.0 : val; // Return -1 for None
+    }
+    void set_target_distance(double value) {
+      setParameter("target_distance", value);
+    }
+    double K_omega() const { return getParameter<double>("rotation_gain"); }
+    double K_v() const { return getParameter<double>("speed_gain"); }
+    double min_vel() const { return getParameter<double>("min_vel"); }
+  };
+
+  RGBFollower(const ControlType robotCtrlType,
+                 const ControlLimitsParams ctrl_limits,
+                 const RGBFollowerConfig config = RGBFollowerConfig());
+
+  // Default Destructor
+  ~RGBFollower() = default;
+
+  void resetTarget(const Bbox2D& tracking);
+
+  bool run(const std::optional<Bbox2D> tracking);
+
+  const Velocities getCtrl() const;
+
+private:
+  ControlType _ctrlType;
+  ControlLimitsParams ctrl_limits_;
+  RGBFollowerConfig config_;
+
+  bool rotate_in_place_;
+  Velocities out_vel_;
+  double recorded_search_time_ = 0.0, recorded_wait_time_ = 0.0;
+  std::queue<std::array<double, 3>> search_commands_queue_;
+  std::array<double, 3> search_command_;
+  std::unique_ptr<Bbox2D> last_tracking_ = nullptr;
+
+  void generateSearchCommands(float total_rotation, float search_radius,
+                              float max_rotation_time, bool enable_pause = false);
+  void getFindTargetCmds(const int last_direction = 1);
+  void trackTarget(const Bbox2D &tracking);
+  //
+};
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index a83209cd..17d899a2 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -9,8 +9,8 @@
 #include "utils/logger.h"
 #include "vision/depth_detector.h"
 #include "vision/tracker.h"
+#include "controllers/rgb_follower.h"
 #include <Eigen/Dense>
-#include <boost/filesystem/path.hpp>
 #include <cmath>
 #include <memory>
 #include <optional>
@@ -22,31 +22,12 @@ namespace Control {
 
 class VisionDWA : public DWA {
 public:
-  class VisionDWAConfig : public ControllerParameters {
+  class VisionDWAConfig : public RGBFollower::RGBFollowerConfig {
   public:
-    VisionDWAConfig() : ControllerParameters() {
-      addParameter("control_time_step",
-                   Parameter(0.1, 1e-4, 1e6, "Control time step (s)"));
-      addParameter(
-          "control_horizon",
-          Parameter(2, 1, 1000, "Number of steps for applying the control"));
+    VisionDWAConfig() : RGBFollower::RGBFollowerConfig() {
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
-      addParameter(
-          "distance_tolerance",
-          Parameter(0.05, 0.0, 100.0,
-                    "Tolerance value for distance (meters)"));
-      addParameter(
-        "angle_tolerance",
-        Parameter(0.1, 0.0, M_PI,
-                  "Tolerance value for angle (rad)"));
-      addParameter(
-          "target_distance",
-          Parameter(
-              0.1, -1.0, 1e9,
-              "Target distance to maintain with the target (m)")); // Use -1 for
-                                                                   // None
       addParameter(
           "target_orientation",
           Parameter(0.0, -M_PI, M_PI,
@@ -55,17 +36,6 @@ class VisionDWA : public DWA {
           "use_local_coordinates",
           Parameter(true,
                     "Track the item in the local frame of the robot. This mode cannot track the object velocity but can operate without knowing the robot's absolute position (world frame)"));
-      // Search Parameters
-      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
-      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
-      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
-      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
-      // Pure tracking control law parameters
-      addParameter("rotation_gain", Parameter(1.0, 1e-2, 10.0));
-      addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
-      addParameter("min_vel", Parameter(0.01, 1e-9, 1e9));
-      addParameter("enable_search", Parameter(false));
-      // Kalman Filter parameters
       addParameter("error_pose", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_vel", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_acc", Parameter(0.05, 1e-9, 1e9));
@@ -81,44 +51,17 @@ class VisionDWA : public DWA {
           "max_depth",
           Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value"));
     }
-    bool enable_search() const { return getParameter<bool>("enable_search"); }
     bool enable_vel_tracking() const {
       return not getParameter<bool>("use_local_coordinates");
     }
-    double control_time_step() const {
-      return getParameter<double>("control_time_step");
-    }
-    double target_search_timeout() const {
-      return getParameter<double>("target_search_timeout");
-    }
-    double target_wait_timeout() const {
-      return getParameter<double>("target_wait_timeout");
-    }
-    double target_search_radius() const {
-      return getParameter<double>("target_search_radius");
-    }
-    double search_pause() const {
-      return getParameter<double>("target_search_pause");
-    }
-    int control_horizon() const { return getParameter<int>("control_horizon"); }
     int prediction_horizon() const {
       return getParameter<int>("prediction_horizon");
     }
     double dist_tolerance() const { return getParameter<double>("distance_tolerance"); }
     double ang_tolerance() const { return getParameter<double>("angle_tolerance"); }
-    double target_distance() const {
-      double val = getParameter<double>("target_distance");
-      return val < 0 ? -1.0 : val; // Return -1 for None
-    }
     double target_orientation() const {
       return getParameter<double>("target_orientation");
     }
-    void set_target_distance(double value) {
-      setParameter("target_distance", value);
-    }
-    double K_omega() const { return getParameter<double>("rotation_gain"); }
-    double K_v() const { return getParameter<double>("speed_gain"); }
-    double min_vel() const { return getParameter<double>("min_vel"); }
     double e_pose() const { return getParameter<double>("error_pose"); }
     double e_vel() const { return getParameter<double>("error_vel"); }
     double e_acc() const { return getParameter<double>("error_acc"); }
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h
deleted file mode 100644
index db31c703..00000000
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_follower.h
+++ /dev/null
@@ -1,105 +0,0 @@
-#pragma once
-
-#include "controller.h"
-#include "datatypes/control.h"
-#include "datatypes/parameter.h"
-#include <memory>
-#include <optional>
-#include <queue>
-
-namespace Kompass {
-namespace Control {
-
-class VisionFollower : public Controller {
-public:
-  struct TrackingData {
-    std::array<double, 2> size_xy; // width and height of the bounding box
-    int img_width;
-    int img_height;
-    std::array<double, 2>
-        center_xy; // x, y coordinates of the object center in image frame
-    double depth;  // -1 is equivalent to none
-  };
-
-  class VisionFollowerConfig : public ControllerParameters {
-  public:
-    VisionFollowerConfig() : ControllerParameters() {
-      addParameter("control_time_step", Parameter(0.1, 1e-4, 1e6));
-      addParameter("control_horizon", Parameter(2, 1, 1000));
-      addParameter("tolerance", Parameter(0.1, 1e-6, 1e3));
-      addParameter("target_distance",
-                   Parameter(-1.0, -1.0, 1e9)); // Use -1 for None
-      addParameter("target_wait_timeout", Parameter(30.0, 0.0, 1e3));
-      addParameter("target_search_timeout", Parameter(30.0, 0.0, 1e3));
-      addParameter("target_search_radius", Parameter(0.5, 1e-4, 1e4));
-      addParameter("target_search_pause", Parameter(1.0, 0.0, 1e3));
-      addParameter("rotation_multiple", Parameter(1.0, 1e-2, 10.0));
-      addParameter("speed_depth_multiple", Parameter(1.0, 1e-2, 10.0));
-      addParameter("min_vel", Parameter(0.1, 1e-9, 1e9));
-      addParameter("enable_search", Parameter(false));
-    }
-    bool enable_search() const { return getParameter<bool>("enable_search"); }
-    double control_time_step() const {
-      return getParameter<double>("control_time_step");
-    }
-    double target_search_timeout() const {
-      return getParameter<double>("target_search_timeout");
-    }
-    double target_wait_timeout() const {
-      return getParameter<double>("target_wait_timeout");
-    }
-    double target_search_radius() const {
-      return getParameter<double>("target_search_radius");
-    }
-    double search_pause() const {
-      return getParameter<double>("target_search_pause");
-    }
-    int control_horizon() const { return getParameter<int>("control_horizon"); }
-    double tolerance() const { return getParameter<double>("tolerance"); }
-    double target_distance() const {
-      double val = getParameter<double>("target_distance");
-      return val < 0 ? -1.0 : val; // Return -1 for None
-    }
-    void set_target_distance(double value) {
-      setParameter("target_distance", value);
-    }
-    double alpha() const { return getParameter<double>("rotation_multiple"); }
-    double beta() const { return getParameter<double>("speed_depth_multiple"); }
-    double min_vel() const { return getParameter<double>("min_vel"); }
-  };
-
-  VisionFollower(const ControlType robotCtrlType,
-                 const ControlLimitsParams ctrl_limits,
-                 const VisionFollowerConfig config = VisionFollowerConfig());
-
-  // Default Destructor
-  ~VisionFollower() = default;
-
-  void resetTarget(const TrackingData tracking);
-
-  bool run(const std::optional<TrackingData> tracking);
-
-  const Velocities getCtrl() const;
-
-private:
-  ControlType _ctrlType;
-  ControlLimitsParams _ctrl_limits;
-  VisionFollowerConfig _config;
-
-  bool _rotate_in_place;
-  Velocities _out_vel;
-  double _recorded_search_time = 0.0, _recorded_wait_time = 0.0;
-  std::queue<std::array<double, 3>> _search_commands_queue;
-  std::array<double, 3> _search_command;
-  std::unique_ptr<TrackingData> _last_tracking = nullptr;
-
-  void generateSearchCommands(double total_rotation, double search_radius,
-                                int max_rotation_steps,
-                                bool enable_pause = false);
-  void getFindTargetCmds();
-  void trackTarget(const TrackingData &tracking);
-  //
-};
-
-} // namespace Control
-} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index a467e9a3..12c57117 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -4,6 +4,7 @@
 #include "utils/logger.h"
 #include <Eigen/Dense>
 #include <cmath>
+#include <stdexcept>
 #include <vector>
 
 namespace Kompass {
@@ -11,15 +12,28 @@ namespace Kompass {
 struct Bbox2D {
   Eigen::Vector2i top_corner = {0, 0};
   Eigen::Vector2i size = {0, 0};
-  float timestamp = 0.0; // Timestamp of the detection in seconds
+  float timestamp = 0.0;  // Timestamp of the detection in seconds
   std::string label = ""; // Label of the detection, e.g. "car", "pedestrian"
+  Eigen::Vector2i img_size = {640, 480}; // Size of the image frame
 
   Bbox2D(){};
 
-  Bbox2D(const Bbox2D &box) : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp), label(box.label){};
-
-  Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size, const float timestamp = 0.0, const std::string &label = "")
-      : top_corner(top_corner), size(size), timestamp(timestamp), label(label){};
+  Bbox2D(const Bbox2D &box)
+      : top_corner(box.top_corner), size(box.size), timestamp(box.timestamp),
+        label(box.label), img_size(box.img_size){};
+
+  Bbox2D(const Eigen::Vector2i top_corner, const Eigen::Vector2i size,
+         const float timestamp = 0.0, const std::string &label = "",
+         const Eigen::Vector2i img_size = {640, 480})
+      : top_corner(top_corner), size(size), timestamp(timestamp), label(label),
+        img_size(img_size) {
+    if (size.x() <= 0 || size.y() <= 0) {
+      throw std::invalid_argument("Invalid bounding box size");
+    }
+    if (img_size.x() <= 0 || img_size.y() <= 0) {
+      throw std::invalid_argument("Invalid image size");
+    }
+  };
 
   Eigen::Vector2i getXLimits() const {
     return {top_corner.x(), top_corner.x() + size.x()};
@@ -28,6 +42,17 @@ struct Bbox2D {
   Eigen::Vector2i getYLimits() const {
     return {top_corner.y(), top_corner.y() + size.y()};
   };
+
+  Eigen::Vector2i getCenter() const {
+    return {top_corner.x() + size.x() / 2, top_corner.y() + size.y() / 2};
+  };
+
+  void setImgSize(const Eigen::Vector2i &size) {
+    if (size.x() <= 0 || size.y() <= 0) {
+      throw std::invalid_argument("Invalid image size");
+    }
+    this->img_size = size;
+  };
 };
 
 struct Bbox3D {
@@ -49,7 +74,8 @@ struct Bbox3D {
 
   Bbox3D(const Eigen::Vector3f &center, const Eigen::Vector3f &size,
          const Eigen::Vector2i center_img_frame,
-         const Eigen::Vector2i size_img_frame, const float timestamp = 0.0, const std::string &label = "",
+         const Eigen::Vector2i size_img_frame, const float timestamp = 0.0,
+         const std::string &label = "",
          const std::vector<Eigen::Vector3f> pc_points = {})
       : center(center), size(size), center_img_frame(center_img_frame),
         size_img_frame(size_img_frame), pc_points(pc_points),
@@ -68,7 +94,8 @@ struct Bbox3D {
       : center_img_frame(
             box2d.top_corner +
             Eigen::Vector2i{box2d.size.x() / 2, box2d.size.y() / 2}),
-        size_img_frame(box2d.size), timestamp(box2d.timestamp), label(box2d.label){};
+        size_img_frame(box2d.size), timestamp(box2d.timestamp),
+        label(box2d.label){};
 
   Eigen::Vector2f getXLimitsImg() const {
     return {center_img_frame.x() - (size_img_frame.x() / 2),
@@ -100,17 +127,18 @@ struct TrackedBbox3D {
   void setfromBox(const Bbox3D &box) { this->box = box; };
 
   void updateFromNewDetection(const Bbox3D &new_box) {
-    if(new_box.label != this->box.label){
+    if (new_box.label != this->box.label) {
       LOG_ERROR("Box label mismatch, cannot update tracking.");
       return;
     }
     float time_step = new_box.timestamp - this->box.timestamp;
     Eigen::Vector3f new_vel;
     if (time_step <= 0.0) {
-      LOG_ERROR("Box updated with invalid time step, Velocity wil be reset to zero.");
+      LOG_ERROR(
+          "Box updated with invalid time step, Velocity wil be reset to zero.");
       this->vel = {0.0, 0.0, 0.0};
       this->acc = {0.0, 0.0, 0.0};
-    }else{
+    } else {
       // Compute velocity and acceleration based on location change
       Eigen::Vector3f new_vel = (new_box.center - this->box.center) / time_step;
       this->acc = (new_vel - this->vel) / time_step;
@@ -145,7 +173,7 @@ struct TrackedBbox3D {
 
   float yaw() const { return std::atan2(this->vel(1), this->vel(0)); };
 
-  float omega() const {return 0.0;}
+  float omega() const { return 0.0; }
 
   float ang_acc() const { return 0.0; }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
new file mode 100644
index 00000000..a6e93aa8
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
@@ -0,0 +1,261 @@
+#include "controllers/rgb_follower.h"
+#include "datatypes/control.h"
+#include "datatypes/tracking.h"
+#include "utils/logger.h"
+#include <cmath>
+#include <memory>
+
+namespace Kompass {
+namespace Control {
+
+RGBFollower::RGBFollower(const ControlType robotCtrlType,
+                         const ControlLimitsParams robotCtrlLimits,
+                         const RGBFollowerConfig config) {
+  _ctrlType = robotCtrlType;
+  ctrl_limits_ = robotCtrlLimits;
+  config_ = config;
+  // Initialize time steps
+  int num_steps = config_.control_horizon();
+  // Initialize control vectors
+  out_vel_ = Velocities(num_steps);
+  rotate_in_place_ = _ctrlType != ControlType::ACKERMANN;
+}
+
+void RGBFollower::resetTarget(const Bbox2D &target) {
+  // empty the search command queue
+  std::queue<std::array<double, 3>> empty;
+  std::swap(search_commands_queue_, empty);
+  // Set the target as the current tracking bounding
+  // box size
+  float size = (target.size.x() * target.size.y()) /
+               (target.img_size.x() * target.img_size.y());
+  LOG_DEBUG("Setting vision target reference distance to size: ", size);
+  config_.set_target_distance(size);
+}
+
+void RGBFollower::generateSearchCommands(float total_rotation,
+                                         float search_radius,
+                                         float max_rotation_time,
+                                         bool enable_pause) {
+  // Calculate rotation direction and magnitude
+  double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
+  float rotation_time = max_rotation_time;
+  int num_pause_steps =
+      static_cast<int>(config_.search_pause() / config_.control_time_step());
+  if (enable_pause) {
+    // Modify the total number of active rotation to include the pause steps
+    rotation_time =
+        max_rotation_time * (1 - num_pause_steps / config_.control_time_step());
+  }
+  // Angular velocity to rotate 'total_rotation' in total time steps
+  // 'rotation_steps' with dt = control_time_step
+  double omega_val = total_rotation / rotation_time;
+
+  omega_val = std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega),
+                       config_.min_vel());
+  // Generate velocity commands
+  for (float t = 0.0f; t <= max_rotation_time;
+       t = t + config_.control_time_step()) {
+    if (rotate_in_place_) {
+      // In-place rotation
+      search_commands_queue_.emplace(
+          std::array<double, 3>{0.0, 0.0, rotation_sign * omega_val});
+    } else {
+      // Angular velocity based on linear velocity and radius
+      double omega_ackermann =
+          rotation_sign * ctrl_limits_.velXParams.maxVel / search_radius;
+      // Non-holonomic circular motion
+      search_commands_queue_.emplace(std::array<double, 3>{
+          ctrl_limits_.velXParams.maxVel, 0.0, omega_ackermann});
+    }
+    if (enable_pause) {
+      // Add zero commands for search pause
+      for (int j = 0; j <= num_pause_steps; j++) {
+        search_commands_queue_.emplace(std::array<double, 3>{0.0, 0.0, 0.0});
+      }
+    }
+  }
+  return;
+}
+
+void RGBFollower::getFindTargetCmds(const int last_direction) {
+  // Generate new search commands if starting a new search or no commands are
+  // available
+  LOG_DEBUG("Generating new search commands in direction: ", last_direction);
+
+  search_commands_queue_ = std::queue<std::array<double, 3>>();
+  const float target_searchtimeout_part = config_.target_search_timeout() / 4;
+  // rotate pi
+  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
+                         target_searchtimeout_part);
+  // go back
+  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
+                         target_searchtimeout_part);
+  // rotate -pi
+  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
+                         target_searchtimeout_part);
+  // go back
+  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
+                         target_searchtimeout_part);
+}
+
+bool RGBFollower::run(const std::optional<Bbox2D> target) {
+  // Check if tracking has a value
+  if (target.has_value()) {
+    // clear all
+    recorded_wait_time_ = 0.0;
+    recorded_search_time_ = 0.0;
+    // Access the TrackingData object
+    const auto &data = target.value();
+    // ensure size_xy or depth have valid values
+    if (last_tracking_ == nullptr) {
+      resetTarget(data);
+    }
+    last_tracking_ = std::make_unique<Bbox2D>(data);
+    // Track the target
+    trackTarget(data);
+    return true;
+  }
+  // Tracking not available
+  if (config_.enable_search()) {
+    if (recorded_search_time_ < config_.target_search_timeout()) {
+      if (search_commands_queue_.empty()) {
+        int last_direction = 1;
+        if (last_tracking_ != nullptr) {
+          auto last_center = last_tracking_->getCenter();
+          last_direction =
+              ((last_center.x() - last_center.y() / 2.0) > 0.0) ? 1 : -1;
+          last_tracking_ = nullptr;
+        }
+        getFindTargetCmds(last_direction);
+      }
+      search_command_ = search_commands_queue_.front();
+      search_commands_queue_.pop();
+      recorded_search_time_ += config_.control_time_step();
+      return true;
+    } else {
+      recorded_search_time_ = 0.0;
+      // Failed to find target
+      return false;
+    }
+  } else {
+    if (recorded_wait_time_ < config_.target_wait_timeout()) {
+      LOG_DEBUG("Target lost, waiting to get tracked target again ...");
+      last_tracking_ = nullptr;
+      // Do nothing and wait
+      recorded_wait_time_ += config_.control_time_step();
+      return true;
+    } else {
+      recorded_wait_time_ = 0.0;
+      // Failed to get target after waiting
+      return false;
+    }
+  }
+}
+
+void RGBFollower::trackTarget(const Bbox2D &target) {
+  float current_dist = (target.size.x() * target.size.y()) /
+                       (target.img_size.x() * target.img_size.y());
+
+  float distance_error = config_.target_distance() - current_dist;
+
+  float distance_tolerance = config_.tolerance() * config_.target_distance();
+
+  double error_y = (2.0 * target.getCenter().y() / target.img_size.y() - 1.0);
+  double error_x = (2.0 * target.getCenter().x() / target.img_size.x() - 1.0);
+
+  LOG_DEBUG("Current distance: ", current_dist,
+            ", Reference: ", config_.target_distance(),
+            "Distance_error=", distance_error,
+            ", Distance_tolerance=", distance_tolerance);
+
+  // Initialize control vectors
+  std::fill(out_vel_.vx.begin(), out_vel_.vx.end(), 0.0);
+  std::fill(out_vel_.vy.begin(), out_vel_.vy.end(), 0.0);
+  std::fill(out_vel_.omega.begin(), out_vel_.omega.end(), 0.0);
+
+  double simulated_depth = current_dist;
+  double dist_speed, omega, v;
+  for (size_t i = 0; i < out_vel_._length; ++i) {
+    // If all errors are within limits -> break
+    if (std::abs(distance_error) < distance_tolerance &&
+        std::abs(error_y) < config_.tolerance() &&
+        std::abs(error_x) < config_.tolerance()) {
+      break;
+    }
+    if (rotate_in_place_ && i % 2 != 0)
+      continue;
+
+    dist_speed = std::abs(distance_error) > distance_tolerance
+                     ? distance_error * ctrl_limits_.velXParams.maxVel
+                     : 0.0;
+
+    // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0)
+    // in [-1, 1] Omega in [-alpha * omega_max, alpha * omega_max]
+    omega = -config_.K_omega() * error_x * ctrl_limits_.omegaParams.maxOmega;
+
+    // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height
+    // - 1.0) in [-1, 1] V in [-speed_max, speed_max]
+    v = config_.K_v() * dist_speed;
+
+    // Limit by the minimum allowed velocity to avoid sending meaningless low
+    // commands to the robot
+    omega = std::abs(omega) >= config_.min_vel() ? omega : 0.0;
+    v = std::abs(v) >= config_.min_vel() ? v : 0.0;
+
+    simulated_depth += -v * config_.control_time_step();
+    // Update distance error
+    distance_error = config_.target_distance() - simulated_depth;
+
+    if (rotate_in_place_) {
+      if (std::abs(v) < config_.min_vel()) {
+        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
+        out_vel_.set(i, 0.0, 0.0, omega);
+        if (i + 1 < out_vel_._length) {
+          out_vel_.set(i + 1, 0.0, 0.0, omega);
+        }
+      } else if (std::abs(omega) < config_.min_vel()) {
+        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
+        out_vel_.set(i, v, 0.0, 0.0);
+        if (i + 1 < out_vel_._length) {
+          out_vel_.set(i + 1, v, 0.0, 0.0);
+        }
+      } else {
+        // Set vx_ctrl[i] to 0.0
+        out_vel_.set(i, v, 0.0, 0.0);
+        // Set vx_ctrl[i+1] to max(v, 0.0)
+        if (i + 1 < out_vel_._length) {
+          out_vel_.set(i + 1, 0.0, 0.0, omega);
+        }
+      }
+    } else {
+      out_vel_.set(i, v, 0.0, omega);
+    }
+  }
+
+  return;
+}
+
+const Velocities RGBFollower::getCtrl() const {
+  if (recorded_search_time_ <= 0.0 && recorded_wait_time_ <= 0.0) {
+    return out_vel_;
+  }
+  // If search is on
+  else if (recorded_search_time_ > 0.0) {
+    Velocities _search(1);
+    LOG_DEBUG(
+        "Number of search commands remaining: ", search_commands_queue_.size(),
+        "recorded search time: ", recorded_search_time_);
+    _search.set(0, search_command_[0], search_command_[1], search_command_[2]);
+    return _search;
+  }
+  // If search not active -> wait till timeout
+  else {
+    // send 0.0 to wait
+    Velocities _wait(1);
+    return _wait;
+  }
+}
+
+} // namespace Control
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp
deleted file mode 100644
index cd4ec847..00000000
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_follower.cpp
+++ /dev/null
@@ -1,275 +0,0 @@
-#include "controllers/vision_follower.h"
-#include "datatypes/control.h"
-#include "utils/logger.h"
-#include <cmath>
-#include <memory>
-
-namespace Kompass {
-namespace Control {
-
-VisionFollower::VisionFollower(const ControlType robotCtrlType,
-                               const ControlLimitsParams robotCtrlLimits,
-                               const VisionFollowerConfig config) {
-  _ctrlType = robotCtrlType;
-  _ctrl_limits = robotCtrlLimits;
-  _config = config;
-  // Initialize time steps
-  int num_steps = _config.control_horizon();
-  // Initialize control vectors
-  _out_vel = Velocities(num_steps);
-  _rotate_in_place = _ctrlType != ControlType::ACKERMANN;
-}
-
-void VisionFollower::resetTarget(const TrackingData tracking) {
-  // empty the search command queue
-  std::queue<std::array<double, 3>> empty;
-  std::swap(_search_commands_queue, empty);
-  // Set the reference depth as the current depth if it is not provided in the
-  // config
-  if (tracking.depth > 0) {
-    if (_config.target_distance() < 0) {
-      _config.set_target_distance(tracking.depth);
-    }
-  }
-  // if depth is not provided set the target as the current tracking bounding
-  // box size
-  else {
-    double size = (tracking.size_xy[0] * tracking.size_xy[1]) /
-                  (tracking.img_width * tracking.img_height);
-    LOG_DEBUG("Setting vision target reference distance to size: ", size);
-    _config.set_target_distance(size);
-  }
-}
-
-void VisionFollower::generateSearchCommands(double total_rotation,
-                                            double search_radius,
-                                            int max_rotation_steps,
-                                            bool enable_pause) {
-  // Calculate rotation direction and magnitude
-  double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
-  int rotation_steps = max_rotation_steps;
-  if (enable_pause) {
-    // Modify the total number of active rotation to include the pause steps
-    rotation_steps = (max_rotation_steps + _config.search_pause()) /
-                     (_config.search_pause() + 1);
-  }
-  // Angular velocity to rotate 'total_rotation' in total time steps
-  // 'rotation_steps' with dt = control_time_step
-  double rotation_value =
-      (total_rotation / rotation_steps) / _config.control_time_step();
-  rotation_value =
-      std::max(std::min(rotation_value, _ctrl_limits.omegaParams.maxOmega),
-               _config.min_vel());
-  // Generate velocity commands
-  for (int i = 0; i <= rotation_steps; i = i + 1) {
-    if (_ctrlType != ControlType::ACKERMANN) {
-      // In-place rotation
-      _search_commands_queue.emplace(
-          std::array<double, 3>{0.0, 0.0, rotation_sign * rotation_value});
-    } else {
-      // Angular velocity based on linear velocity and radius
-      double omega_ackermann =
-          rotation_sign * _ctrl_limits.velXParams.maxVel / search_radius;
-      // Non-holonomic circular motion
-      _search_commands_queue.emplace(std::array<double, 3>{
-          _ctrl_limits.velXParams.maxVel, 0.0, omega_ackermann});
-    }
-    if (enable_pause) {
-      // Add zero commands for search pause
-      for (int j = 0; j <= _config.search_pause(); j++) {
-        _search_commands_queue.emplace(std::array<double, 3>{0.0, 0.0, 0.0});
-      }
-    }
-  }
-  return;
-}
-
-void VisionFollower::getFindTargetCmds() {
-  // Generate new search commands if starting a new search or no commands are
-  // available
-  double last_direction = 1.0;
-  if (_last_tracking != nullptr) {
-    last_direction =
-        ((_last_tracking->center_xy[0] - _last_tracking->img_width / 2.0) > 0.0)
-            ? 1.0
-            : -1.0;
-    _last_tracking = nullptr;
-  }
-  LOG_DEBUG("Generating new search commands in direction: ", last_direction);
-
-  _search_commands_queue = std::queue<std::array<double, 3>>();
-  int target_search_steps_max = static_cast<int>(
-      _config.target_search_timeout() / _config.control_time_step());
-  // rotate pi
-  generateSearchCommands(last_direction * M_PI, _config.target_search_radius(),
-                         target_search_steps_max / 4, true);
-  // go back
-  generateSearchCommands(-last_direction * M_PI, _config.target_search_radius(),
-                         target_search_steps_max / 8);
-  // rotate -pi
-  generateSearchCommands(-last_direction * M_PI, _config.target_search_radius(),
-                         target_search_steps_max / 4, true);
-  // go back
-  generateSearchCommands(last_direction * M_PI, _config.target_search_radius(),
-                         target_search_steps_max / 8);
-}
-
-bool VisionFollower::run(const std::optional<TrackingData> tracking) {
-  bool tracking_available = false;
-  // Check if tracking has a value
-  if (tracking.has_value()) {
-    // clear all
-    _recorded_wait_time = 0.0;
-    _recorded_search_time = 0.0;
-    // Access the TrackingData object
-    const auto &data = tracking.value();
-    // ensure size_xy or depth have valid values
-    tracking_available =
-        ((data.size_xy[0] > 0 && data.size_xy[1] > 0) || data.depth > 0);
-    if (tracking_available) {
-      if (_last_tracking == nullptr) {
-        resetTarget(data);
-      }
-      _last_tracking = std::make_unique<TrackingData>(data);
-      // Track the target
-      trackTarget(data);
-      return true;
-    }
-  }
-  // Tracking not available
-  if (_config.enable_search()) {
-    if (_recorded_search_time < _config.target_search_timeout()) {
-      if (_search_commands_queue.empty()) {
-        getFindTargetCmds();
-      }
-      _search_command = _search_commands_queue.front();
-      _search_commands_queue.pop();
-      _recorded_search_time += _config.control_time_step();
-      return true;
-    } else {
-      _recorded_search_time = 0.0;
-      // Failed to find target
-      return false;
-    }
-  } else {
-    if (_recorded_wait_time < _config.target_wait_timeout()) {
-      LOG_DEBUG("Target lost, waiting to get tracked target again ...");
-      _last_tracking = nullptr;
-      // Do nothing and wait
-      _recorded_wait_time += _config.control_time_step();
-      return true;
-    } else {
-      _recorded_wait_time = 0.0;
-      // Failed to get target after waiting
-      return false;
-    }
-  }
-}
-
-void VisionFollower::trackTarget(const TrackingData &tracking) {
-  double size = (tracking.size_xy[0] * tracking.size_xy[1]) /
-                (tracking.img_width * tracking.img_height);
-
-  double current_distance = tracking.depth > 0 ? tracking.depth : size;
-  double distance_error = _config.target_distance() - current_distance;
-
-  double distance_tolerance = _config.tolerance() * _config.target_distance();
-
-  double error_y = (2 * tracking.center_xy[1] / tracking.img_height - 1.0);
-  double error_x = (2 * tracking.center_xy[0] / tracking.img_width - 1.0);
-
-  LOG_DEBUG("Current distance: ", size,
-            ", Reference: ", _config.target_distance(),
-            "Distance_error=", distance_error,
-            ", Distance_tolerance=", distance_tolerance);
-
-  // Initialize control vectors
-  std::fill(_out_vel.vx.begin(), _out_vel.vx.end(), 0.0);
-  std::fill(_out_vel.vy.begin(), _out_vel.vy.end(), 0.0);
-  std::fill(_out_vel.omega.begin(), _out_vel.omega.end(), 0.0);
-
-  double simulated_depth = current_distance;
-  double dist_speed, omega, v;
-  for (size_t i = 0; i < _out_vel._length; ++i) {
-    // If all errors are within limits -> break
-    if (std::abs(distance_error) < distance_tolerance &&
-        std::abs(error_y) < _config.tolerance() &&
-        std::abs(error_x) < _config.tolerance()) {
-      break;
-    }
-    if (_rotate_in_place && i % 2 != 0)
-      continue;
-
-    dist_speed = std::abs(distance_error) > distance_tolerance
-                     ? distance_error * _ctrl_limits.velXParams.maxVel
-                     : 0.0;
-
-    // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0)
-    // in [-1, 1] Omega in [-alpha * omega_max, alpha * omega_max]
-    omega = -_config.alpha() * error_x * _ctrl_limits.omegaParams.maxOmega;
-
-    // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height
-    // - 1.0) in [-1, 1] V in [-speed_max, speed_max]
-    v = _config.beta() * dist_speed;
-
-    // Limit by the minimum allowed velocity to avoid sending meaningless low
-    // commands to the robot
-    omega = std::abs(omega) >= _config.min_vel() ? omega : 0.0;
-    v = std::abs(v) >= _config.min_vel() ? v : 0.0;
-
-    simulated_depth += -v * _config.control_time_step();
-    // Update distance error
-    distance_error = _config.target_distance() - simulated_depth;
-
-    if (_rotate_in_place) {
-      if (std::abs(v) < _config.min_vel()) {
-        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
-        _out_vel.set(i, 0.0, 0.0, omega);
-        if (i + 1 < _out_vel._length) {
-          _out_vel.set(i + 1, 0.0, 0.0, omega);
-        }
-      } else if (std::abs(omega) < _config.min_vel()) {
-        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
-        _out_vel.set(i, v, 0.0, 0.0);
-        if (i + 1 < _out_vel._length) {
-          _out_vel.set(i + 1, v, 0.0, 0.0);
-        }
-      } else {
-        // Set vx_ctrl[i] to 0.0
-        _out_vel.set(i, v, 0.0, 0.0);
-        // Set vx_ctrl[i+1] to max(v, 0.0)
-        if (i + 1 < _out_vel._length) {
-          _out_vel.set(i + 1, 0.0, 0.0, omega);
-        }
-      }
-    } else {
-      _out_vel.set(i, v, 0.0, omega);
-    }
-  }
-
-  return;
-}
-
-const Velocities VisionFollower::getCtrl() const {
-  if (_recorded_search_time <= 0.0 && _recorded_wait_time <= 0.0) {
-    return _out_vel;
-  }
-  // If search is on
-  else if (_recorded_search_time > 0.0) {
-    Velocities _search(1);
-    LOG_DEBUG(
-        "Number of search commands remaining: ", _search_commands_queue.size(),
-        "recorded search time: ", _recorded_search_time);
-    _search.set(0, _search_command[0], _search_command[1], _search_command[2]);
-    return _search;
-  }
-  // If search not active -> wait till timeout
-  else {
-    // send 0.0 to wait
-    Velocities _wait(1);
-    return _wait;
-  }
-}
-
-} // namespace Control
-} // namespace Kompass
diff --git a/src/kompass_cpp/tests/vision_dwa_test.cpp b/src/kompass_cpp/tests/vision_dwa_test.cpp
index 296223fb..30996b5e 100644
--- a/src/kompass_cpp/tests/vision_dwa_test.cpp
+++ b/src/kompass_cpp/tests/vision_dwa_test.cpp
@@ -1,5 +1,4 @@
 #include "controllers/vision_dwa.h"
-#include "controllers/vision_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/path.h"
 #include "datatypes/tracking.h"
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index ee37a32e..55210b4f 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -478,7 +478,7 @@ def test_vision_dwa_with_depth_img():
         max_depth=5.0,
         depth_conversion_factor=0.001,
         camera_position_to_robot=np.array([0.32, 0.02089, 0.2999]),
-        camera_rotation_to_robot=np.array([0.385 , -0.5846, 0.595, -0.395]),
+        camera_rotation_to_robot=np.array([0.385, -0.5846, 0.595, -0.395]),
     )
 
     controller = VisionDWA(
@@ -525,149 +525,149 @@ def test_vision_dwa_with_depth_img():
     print(f"Found Control {vx}, {vy}, {omega}")
 
 
-@pytest.mark.parametrize("use_tracker", [False, True])
-@pytest.mark.parametrize(
-    "point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]]
-)
-def test_vision_dwa(use_tracker: bool, point_cloud: List[np.ndarray]):
-    """Run DWA pytest and assert reaching end"""
-    global global_path, my_robot, robot_ctr_limits, control_time_step
-
-    control_horizon = 2
-    prediction_horizon = 10
-
-    # Simulate tracked pose and vel
-    tracked_vel_lin = 0.1
-    tracked_vel_ang = 0.3
-    tracked_pose = TrackedPose2D(0.0, 0.0, 0.0, tracked_vel_lin, 0.0, tracked_vel_ang)
-
-    # To plot
-    robot_x = []
-    robot_y = []
-    tracked_x = []
-    tracked_y = []
-    my_robot.state.x = -0.51731912
-    my_robot.state.y = 0.0
-    my_robot.state.yaw = 0.0
-
-    cost_weights = TrajectoryCostsWeights(
-        reference_path_distance_weight=1.0,
-        goal_distance_weight=0.0,
-        smoothness_weight=0.0,
-        jerk_weight=0.0,
-        obstacles_distance_weight=0.0,
-    )
-    config = VisionDWAConfig(
-        control_time_step=control_time_step,
-        control_horizon=control_horizon,
-        prediction_horizon=prediction_horizon,
-        speed_gain=1.0,
-        rotation_gain=1.0,
-        costs_weights=cost_weights,
-        max_linear_samples=20,
-        max_angular_samples=20,
-        octree_resolution=0.1,
-        max_num_threads=1,
-        min_vel=0.05,
-    )
-
-    controller = VisionDWA(robot=my_robot, ctrl_limits=robot_ctr_limits, config=config)
-
-    steps = 0
-
-    # Detected Boxes
-    ref_point_img = [150, 150]
-    detected_boxes = []
-    boxes_ori = 0.0
-    for i in range(3):
-        new_box = Bbox3D(
-            center=np.array([0.0, 0.0, 0.0], dtype=np.float32),
-            size=np.array([0.5, 0.5, 1.0], dtype=np.float32),
-            center_img_frame=np.array([150, 150], dtype=np.int32),
-            size_img_frame=np.array([25, 25], dtype=np.int32),
-        )
-        new_box_shift = np.array([0.7 * i, 0.7 * i, 0.0], dtype=np.float32)
-        img_frame_shift = np.array([50 * i, 50 * i], dtype=np.int32)
-        new_box.center = new_box_shift
-        new_box.center_img_frame = img_frame_shift + ref_point_img
-        detected_boxes.append(new_box)
-
-    def moveBoxes(boxes_orientation, boxes) -> float:
-        """Move boxes in the direction of the tracked pose"""
-        for box in boxes:
-            box.center[0] += (
-                tracked_vel_lin * np.cos(boxes_orientation) * control_time_step
-            )
-            box.center[1] += (
-                tracked_vel_lin * np.sin(boxes_orientation) * control_time_step
-            )
-        boxes_orientation += tracked_vel_ang * control_time_step
-        return boxes_orientation
-
-    if use_tracker:
-        controller.set_initial_tracking_3d(150, 150, detected_boxes)
-
-    while steps < 100:
-        robot_x.append(my_robot.state.x)
-        robot_y.append(my_robot.state.y)
-        tracked_x.append(tracked_pose.x())
-        tracked_y.append(tracked_pose.y())
-
-        if use_tracker:
-            res = controller.loop_step(
-                current_state=my_robot.state,
-                detections_3d=detected_boxes,
-                point_cloud=point_cloud,
-            )
-        else:
-            res = controller.loop_step(
-                current_state=my_robot.state,
-                tracked_pose=tracked_pose,
-                point_cloud=point_cloud,
-            )
-        if not res:
-            print("No control found")
-            break
-        # print(f"Found trajectory with cost {controller.result_cost}")
-        velocities = controller.control_till_horizon
-        (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
-        print(f"Found Control {vx}, {vy}, {omega}")
-        my_robot.set_control(
-            velocity_x=vx,
-            velocity_y=vy,
-            omega=omega,
-        )
-        my_robot.get_state(dt=control_time_step)
-        tracked_pose.update(control_time_step)
-        if use_tracker:
-            boxes_ori = moveBoxes(boxes_ori, detected_boxes)
-        steps += 1
-
-    figure_name = "vision_dwa"
-    if use_tracker:
-        figure_name += "_with_tracker"
-    else:
-        figure_name += "_no_tracker"
-
-    figure_name += f"{point_cloud[0]}"
-    plt.figure()
-    plt.plot(robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path")
-    plt.plot(
-        tracked_x,
-        tracked_y,
-        label="Tracked Object",
-        marker="*",
-        linestyle="-",
-        color="g",
-    )
-    plt.xlabel("X")
-    plt.ylabel("Y")
-    plt.title(figure_name)
-    plt.grid(True)
-    plt.legend()
-    plt.savefig(f"logs/{figure_name}.png")
-
-    assert steps == 100
+# @pytest.mark.parametrize("use_tracker", [False, True])
+# @pytest.mark.parametrize(
+#     "point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]]
+# )
+# def test_vision_dwa(use_tracker: bool, point_cloud: List[np.ndarray]):
+#     """Run DWA pytest and assert reaching end"""
+#     global global_path, my_robot, robot_ctr_limits, control_time_step
+
+#     control_horizon = 2
+#     prediction_horizon = 10
+
+#     # Simulate tracked pose and vel
+#     tracked_vel_lin = 0.1
+#     tracked_vel_ang = 0.3
+#     tracked_pose = TrackedPose2D(0.0, 0.0, 0.0, tracked_vel_lin, 0.0, tracked_vel_ang)
+
+#     # To plot
+#     robot_x = []
+#     robot_y = []
+#     tracked_x = []
+#     tracked_y = []
+#     my_robot.state.x = -0.51731912
+#     my_robot.state.y = 0.0
+#     my_robot.state.yaw = 0.0
+
+#     cost_weights = TrajectoryCostsWeights(
+#         reference_path_distance_weight=1.0,
+#         goal_distance_weight=0.0,
+#         smoothness_weight=0.0,
+#         jerk_weight=0.0,
+#         obstacles_distance_weight=0.0,
+#     )
+#     config = VisionDWAConfig(
+#         control_time_step=control_time_step,
+#         control_horizon=control_horizon,
+#         prediction_horizon=prediction_horizon,
+#         speed_gain=1.0,
+#         rotation_gain=1.0,
+#         costs_weights=cost_weights,
+#         max_linear_samples=20,
+#         max_angular_samples=20,
+#         octree_resolution=0.1,
+#         max_num_threads=1,
+#         min_vel=0.05,
+#     )
+
+#     controller = VisionDWA(robot=my_robot, ctrl_limits=robot_ctr_limits, config=config)
+
+#     steps = 0
+
+#     # Detected Boxes
+#     ref_point_img = [150, 150]
+#     detected_boxes = []
+#     boxes_ori = 0.0
+#     for i in range(3):
+#         new_box = Bbox3D(
+#             center=np.array([0.0, 0.0, 0.0], dtype=np.float32),
+#             size=np.array([0.5, 0.5, 1.0], dtype=np.float32),
+#             center_img_frame=np.array([150, 150], dtype=np.int32),
+#             size_img_frame=np.array([25, 25], dtype=np.int32),
+#         )
+#         new_box_shift = np.array([0.7 * i, 0.7 * i, 0.0], dtype=np.float32)
+#         img_frame_shift = np.array([50 * i, 50 * i], dtype=np.int32)
+#         new_box.center = new_box_shift
+#         new_box.center_img_frame = img_frame_shift + ref_point_img
+#         detected_boxes.append(new_box)
+
+#     def moveBoxes(boxes_orientation, boxes) -> float:
+#         """Move boxes in the direction of the tracked pose"""
+#         for box in boxes:
+#             box.center[0] += (
+#                 tracked_vel_lin * np.cos(boxes_orientation) * control_time_step
+#             )
+#             box.center[1] += (
+#                 tracked_vel_lin * np.sin(boxes_orientation) * control_time_step
+#             )
+#         boxes_orientation += tracked_vel_ang * control_time_step
+#         return boxes_orientation
+
+#     if use_tracker:
+#         controller.set_initial_tracking_3d(150, 150, detected_boxes)
+
+#     while steps < 100:
+#         robot_x.append(my_robot.state.x)
+#         robot_y.append(my_robot.state.y)
+#         tracked_x.append(tracked_pose.x())
+#         tracked_y.append(tracked_pose.y())
+
+#         if use_tracker:
+#             res = controller.loop_step(
+#                 current_state=my_robot.state,
+#                 detections_3d=detected_boxes,
+#                 point_cloud=point_cloud,
+#             )
+#         else:
+#             res = controller.loop_step(
+#                 current_state=my_robot.state,
+#                 tracked_pose=tracked_pose,
+#                 point_cloud=point_cloud,
+#             )
+#         if not res:
+#             print("No control found")
+#             break
+#         # print(f"Found trajectory with cost {controller.result_cost}")
+#         velocities = controller.control_till_horizon
+#         (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
+#         print(f"Found Control {vx}, {vy}, {omega}")
+#         my_robot.set_control(
+#             velocity_x=vx,
+#             velocity_y=vy,
+#             omega=omega,
+#         )
+#         my_robot.get_state(dt=control_time_step)
+#         tracked_pose.update(control_time_step)
+#         if use_tracker:
+#             boxes_ori = moveBoxes(boxes_ori, detected_boxes)
+#         steps += 1
+
+#     figure_name = "vision_dwa"
+#     if use_tracker:
+#         figure_name += "_with_tracker"
+#     else:
+#         figure_name += "_no_tracker"
+
+#     figure_name += f"{point_cloud[0]}"
+#     plt.figure()
+#     plt.plot(robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path")
+#     plt.plot(
+#         tracked_x,
+#         tracked_y,
+#         label="Tracked Object",
+#         marker="*",
+#         linestyle="-",
+#         color="g",
+#     )
+#     plt.xlabel("X")
+#     plt.ylabel("Y")
+#     plt.title(figure_name)
+#     plt.grid(True)
+#     plt.legend()
+#     plt.savefig(f"logs/{figure_name}.png")
+
+#     assert steps == 100
 
 
 def test_dwa_debug():

From 51c507d9239d9b5754814b289ca6a8e0a786fe99 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 9 Jun 2025 15:47:36 +0200
Subject: [PATCH 082/118] (fix) Removes unused import

---
 pyproject.toml                                      |  1 -
 src/kompass_core/mapping/local_mapper.py            |  8 +++++---
 src/kompass_core/utils/emergency_stop.py            | 13 ++++++++++---
 .../kompass_cpp/src/utils/critical_zone_check.cpp   |  1 -
 4 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 78ef0369..c0de873d 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -137,4 +137,3 @@ select = "*-manylinux_aarch64"
 inherit.environment = "append"
 environment.VCPKG_TARGET_TRIPLET = "arm64-linux-release"
 environment.LD_LIBRARY_PATH="$LD_LIBRARY_PATH:$VCPKG_ROOT/installed/$VCPKG_TARGET_TRIPLET/lib"
-
diff --git a/src/kompass_core/mapping/local_mapper.py b/src/kompass_core/mapping/local_mapper.py
index 938e04b8..55d346fc 100644
--- a/src/kompass_core/mapping/local_mapper.py
+++ b/src/kompass_core/mapping/local_mapper.py
@@ -271,9 +271,11 @@ def update_from_scan(
         if self.config.baysian_update:
             if self.processed:
                 self._calculate_grid_shift(robot_pose)
-            scan_occupancy, scan_occupancy_prob = self.local_mapper.scan_to_grid_baysian(
-                angles=laser_scan.angles,
-                ranges=filtered_ranges,
+            scan_occupancy, scan_occupancy_prob = (
+                self.local_mapper.scan_to_grid_baysian(
+                    angles=laser_scan.angles,
+                    ranges=filtered_ranges,
+                )
             )
 
             # Update grid
diff --git a/src/kompass_core/utils/emergency_stop.py b/src/kompass_core/utils/emergency_stop.py
index f18e8466..d3f99ed9 100644
--- a/src/kompass_core/utils/emergency_stop.py
+++ b/src/kompass_core/utils/emergency_stop.py
@@ -24,9 +24,15 @@ def __init__(
         self.__emergency_distance = emergency_distance
         self.__slowdown_distance = slowdown_distance
         self.__emergency_angle = emergency_angle
-        self.__sensor_position_robot = sensor_position_robot if sensor_position_robot is not None else np.array([0.0, 0.0, 0.0], dtype=np.float32)
-        self.__sensor_rotation_robot = sensor_rotation_robot if sensor_rotation_robot is not None else np.array(
-            [1.0, 0.0, 0.0, 0.0], dtype=np.float32
+        self.__sensor_position_robot = (
+            sensor_position_robot
+            if sensor_position_robot is not None
+            else np.array([0.0, 0.0, 0.0], dtype=np.float32)
+        )
+        self.__sensor_rotation_robot = (
+            sensor_rotation_robot
+            if sensor_rotation_robot is not None
+            else np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
         )
         self.__robot_shape = RobotGeometry.Type.to_kompass_cpp_lib(robot.geometry_type)
         self.__robot_dimensions = robot.geometry_params
@@ -37,6 +43,7 @@ def _init_checker(self, scan: LaserScanData) -> None:
         if self.__use_gpu:
             try:
                 from kompass_cpp.utils import CriticalZoneCheckerGPU
+
                 self._critical_zone_checker = CriticalZoneCheckerGPU(
                     robot_shape=self.__robot_shape,
                     robot_dimensions=self.__robot_dimensions,
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
index 095bc20f..817f0e66 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check.cpp
@@ -3,7 +3,6 @@
 #include "utils/logger.h"
 #include "utils/transformation.h"
 #include <Eigen/Core>
-#include <array>
 
 namespace Kompass {
 /**

From d23407629a88538e8de3ae8621c047877a538432 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 9 Jun 2025 19:59:58 +0200
Subject: [PATCH 083/118] (feature) Adds optional velocity to Bbox2D

---
 src/kompass_cpp/bindings/bindings_types.cpp              | 3 ++-
 src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h | 3 +++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 05a0ef4a..494db3af 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -132,7 +132,8 @@ void bindings_types(py::module_ &m) {
       .def_rw("top_left_corner", &Bbox2D::top_corner)
       .def_rw("size", &Bbox2D::size)
       .def_rw("timestamp", &Bbox2D::timestamp)
-      .def_rw("label", &Bbox2D::label);
+      .def_rw("label", &Bbox2D::label)
+      .def("set_vel", &Bbox2D::setVel);
 
   py::class_<Bbox3D>(m_types, "Bbox3D")
       .def(py::init<>())
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
index 12c57117..244c981d 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/tracking.h
@@ -15,6 +15,7 @@ struct Bbox2D {
   float timestamp = 0.0;  // Timestamp of the detection in seconds
   std::string label = ""; // Label of the detection, e.g. "car", "pedestrian"
   Eigen::Vector2i img_size = {640, 480}; // Size of the image frame
+  Eigen::Vector3f vel = {0.0, 0.0, 0.0};
 
   Bbox2D(){};
 
@@ -53,6 +54,8 @@ struct Bbox2D {
     }
     this->img_size = size;
   };
+
+  void setVel(const Eigen::Vector3f &vel) { this->vel = vel; };
 };
 
 struct Bbox3D {

From 1aa7c9729cfa6403c6309f78389d683246d01be0 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 10 Jun 2025 10:52:02 +0200
Subject: [PATCH 084/118] (fix) Exposes a unified interface for both RGB and
 RGBD vision followers

---
 src/kompass_core/control/__init__.py          | 19 ++++++----
 src/kompass_core/control/rgb_follower.py      | 14 +++----
 .../{vision_dwa.py => rgbd_follower.py}       | 38 +++----------------
 3 files changed, 24 insertions(+), 47 deletions(-)
 rename src/kompass_core/control/{vision_dwa.py => rgbd_follower.py} (90%)

diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index 48bbcaaa..96fea113 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -10,8 +10,8 @@
 from .dvz import DVZ, DVZConfig
 from .dwa import DWA, DWAConfig
 from .stanley import StanleyConfig, Stanley
-from .rgb_follower import VisionFollower, VisionFollowerConfig
-from .vision_dwa import VisionDWAConfig, VisionDWA
+from .rgb_follower import VisionRGBFollower, VisionRGBFollowerConfig
+from .rgbd_follower import VisionRGBDFollower, VisionRGBDFollowerConfig
 
 ControllerType = FollowerTemplate
 
@@ -71,19 +71,24 @@ class ControllersID(StrEnum):
     STANLEY = "Stanley"
     DWA = "DWA"
     DVZ = "DVZ"
-    VISION = "VisionDWA"
+    VISION_IMG = "VisionRGBFollower"
+    VISION_DEPTH = "VisionRGBDFollower"
 
 
 ControlClasses = {
     ControllersID.STANLEY: Stanley,
     ControllersID.DVZ: DVZ,
     ControllersID.DWA: DWA,
+    ControllersID.VISION_IMG : VisionRGBFollower,
+    ControllersID.VISION_DEPTH: VisionRGBDFollower
 }
 
 ControlConfigClasses = {
     ControllersID.STANLEY: StanleyConfig,
     ControllersID.DVZ: DVZConfig,
     ControllersID.DWA: DWAConfig,
+    ControllersID.VISION_IMG: VisionRGBFollowerConfig,
+    ControllersID.VISION_DEPTH: VisionRGBDFollowerConfig,
 }
 
 
@@ -101,8 +106,8 @@ class ControllersID(StrEnum):
     "DWA",
     "DWAConfig",
     "TrajectoryCostsWeights",
-    "VisionFollower",
-    "VisionFollowerConfig",
-    "VisionDWAConfig",
-    "VisionDWA",
+    "VisionRGBFollower",
+    "VisionRGBFollowerConfig",
+    "VisionRGBDFollower",
+    "VisionRGBDFollowerConfig",
 ]
diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index 9c4b8fb6..3bedc23e 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -1,6 +1,6 @@
 from ._base_ import ControllerTemplate
 import logging
-from typing import Optional
+from typing import Optional, List
 from attrs import define, field
 from ..utils.common import BaseAttrs, base_validators
 from ..models import Robot, RobotCtrlLimits, RobotType
@@ -9,7 +9,7 @@
 
 
 @define
-class VisionFollowerConfig(BaseAttrs):
+class VisionRGBFollowerConfig(BaseAttrs):
     control_time_step: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
     )
@@ -58,17 +58,17 @@ def to_kompass_cpp(self) -> RGBFollowerParameters:
         return vision_config
 
 
-class VisionFollower(ControllerTemplate):
+class VisionRGBFollower(ControllerTemplate):
     def __init__(
         self,
         robot: Robot,
         ctrl_limits: RobotCtrlLimits,
-        config: Optional[RGBFollowerParameters] = None,
+        config: Optional[VisionRGBFollowerConfig] = None,
         config_file: Optional[str] = None,
         config_yaml_root_name: Optional[str] = None,
         **_,
     ):
-        config = config or RGBFollowerParameters()
+        config = config or VisionRGBFollowerConfig()
 
         if config_file:
             config.from_yaml(config_file, config_yaml_root_name, get_common=False)
@@ -87,10 +87,10 @@ def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_):
     def loop_step(
         self,
         *,
-        target_box: Bbox2D,
+        detections_2d: Optional[List[Bbox2D]],
         **_,
     ) -> bool:
-        self._found_ctrl = self.__controller.run(target_box)
+        self._found_ctrl = self.__controller.run(detections_2d[0] if detections_2d else None)
         if self._found_ctrl:
             self._ctrl = self.__controller.get_ctrl()
         return self._found_ctrl
diff --git a/src/kompass_core/control/vision_dwa.py b/src/kompass_core/control/rgbd_follower.py
similarity index 90%
rename from src/kompass_core/control/vision_dwa.py
rename to src/kompass_core/control/rgbd_follower.py
index 799dc9ea..c59853b8 100644
--- a/src/kompass_core/control/vision_dwa.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -20,10 +20,11 @@
 from ..datatypes.laserscan import LaserScanData
 from ..datatypes.pointcloud import PointCloudData
 from .dwa import DWAConfig
+from .rgb_follower import VisionRGBFollowerConfig
 
 
 @define
-class VisionDWAConfig(DWAConfig):
+class VisionRGBDFollowerConfig(DWAConfig, VisionRGBFollowerConfig):
     distance_tolerance: float = field(
         default=0.05, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
     )
@@ -32,34 +33,11 @@ class VisionDWAConfig(DWAConfig):
     )
     # Tolerance value for distance and angle following errors
 
-    target_distance: Optional[float] = field(
-        default=0.1,
-        validator=base_validators.in_range(min_value=1e-9, max_value=1e9),
-    )  # Target distance to maintain with the target (m)
-
     target_orientation: float = field(
         default=0.0,
         validator=base_validators.in_range(min_value=-np.pi, max_value=np.pi),
     )  # Bearing angle to maintain with the target (rad)
 
-    target_wait_timeout: float = field(
-        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
-    )  # Wait for target to appear again timeout (seconds), used if search is disabled
-
-    enable_search: bool = field(default=False)  # Enable or disable the search mechanism
-
-    target_search_timeout: float = field(
-        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
-    )  # Search timeout in seconds
-
-    target_search_radius: float = field(
-        default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4)
-    )  # Search radius for finding the target (m)
-
-    target_search_pause: float = field(
-        default=1.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
-    )  # Pause between search actions to find target (seconds)
-
     rotation_gain: float = field(
         default=0.5, validator=base_validators.in_range(min_value=1e-2, max_value=10.0)
     )  # Gain for the rotation control law
@@ -68,12 +46,6 @@ class VisionDWAConfig(DWAConfig):
         default=1.0, validator=base_validators.in_range(min_value=1e-2, max_value=10.0)
     )  # Gain for the speed control law
 
-    min_vel: float = field(
-        default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
-    )  # Minimum velocity to apply (m/s)
-
-    enable_search: bool = field(default=False)  # Enable or disable the search mechanism
-
     use_local_coordinates: bool = field(
         default=True
     )  # Track the target using robot local coordinates (no need for robot location at lop step)
@@ -129,12 +101,12 @@ def to_kompass_cpp(self) -> VisionDWAParameters:
         return vision_dwa_params
 
 
-class VisionDWA(ControllerTemplate):
+class VisionRGBDFollower(ControllerTemplate):
     def __init__(
         self,
         robot: Robot,
         ctrl_limits: RobotCtrlLimits,
-        config: Optional[VisionDWAConfig] = None,
+        config: Optional[VisionRGBDFollowerConfig] = None,
         config_file: Optional[str] = None,
         config_yaml_root_name: Optional[str] = None,
         control_time_step: Optional[float] = None,
@@ -150,7 +122,7 @@ def __init__(
         :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
         :type params_file: str
         """
-        self._config = config or VisionDWAConfig()
+        self._config = config or VisionRGBDFollowerConfig()
 
         if config_file:
             self._config.from_yaml(

From 60054f103f09704795fd0cab8e33ad677bf21a9a Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 10 Jun 2025 12:54:12 +0200
Subject: [PATCH 085/118] (fix) Minor fix for config class

---
 src/kompass_core/control/rgbd_follower.py | 35 +++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index c59853b8..7eb7f53a 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -20,11 +20,42 @@
 from ..datatypes.laserscan import LaserScanData
 from ..datatypes.pointcloud import PointCloudData
 from .dwa import DWAConfig
-from .rgb_follower import VisionRGBFollowerConfig
 
 
 @define
-class VisionRGBDFollowerConfig(DWAConfig, VisionRGBFollowerConfig):
+class VisionRGBDFollowerConfig(DWAConfig):
+    control_time_step: float = field(
+        default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
+    )
+    control_horizon: int = field(
+        default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
+    )
+    prediction_horizon: int = field(
+        default=10, validator=base_validators.in_range(min_value=1, max_value=1000)
+    )
+    target_distance: Optional[float] = field(default=None)
+    target_wait_timeout: float = field(
+        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # wait for target to appear again timeout (seconds), used if search is disabled
+    target_search_timeout: float = field(
+        default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # search timeout in seconds
+    target_search_pause: float = field(
+        default=2.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
+    )  # pause between search actions to find target (seconds)
+    target_search_radius: float = field(
+        default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4)
+    )
+    rotation_multiple: float = field(
+        default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0)
+    )
+    speed_depth_multiple: float = field(
+        default=0.7, validator=base_validators.in_range(min_value=1e-9, max_value=10.0)
+    )
+    min_vel: float = field(
+        default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
+    )
+    enable_search: bool = field(default=True)
     distance_tolerance: float = field(
         default=0.05, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
     )

From 93f9b01b9cb61fa27c26c0d5c8ff1a005d19ac6a Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 12 Jun 2025 19:15:06 +0800
Subject: [PATCH 086/118] (fix) Updates RGBFollower

---
 src/kompass_core/control/rgb_follower.py      | 26 ++++---
 src/kompass_core/control/rgbd_follower.py     |  8 +--
 src/kompass_cpp/bindings/bindings_control.cpp | 12 ++--
 src/kompass_cpp/bindings/bindings_types.cpp   |  3 +-
 .../include/controllers/rgb_follower.h        | 21 +++---
 .../include/controllers/vision_dwa.h          | 17 ++---
 .../src/controllers/rgb_follower.cpp          | 65 ++++++++----------
 .../src/controllers/vision_dwa.cpp            | 68 +------------------
 8 files changed, 74 insertions(+), 146 deletions(-)

diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index 3bedc23e..d75b6fd0 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -6,6 +6,7 @@
 from ..models import Robot, RobotCtrlLimits, RobotType
 from kompass_cpp.control import RGBFollower, RGBFollowerParameters
 from kompass_cpp.types import Bbox2D
+import numpy as np
 
 
 @define
@@ -35,10 +36,10 @@ class VisionRGBFollowerConfig(BaseAttrs):
     target_search_radius: float = field(
         default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4)
     )
-    rotation_multiple: float = field(
+    rotation_gain: float = field(
         default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0)
     )
-    speed_depth_multiple: float = field(
+    speed_gain: float = field(
         default=0.7, validator=base_validators.in_range(min_value=1e-9, max_value=10.0)
     )
     min_vel: float = field(
@@ -46,6 +47,14 @@ class VisionRGBFollowerConfig(BaseAttrs):
     )
     enable_search: bool = field(default=True)
 
+    camera_position_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0], dtype=np.float32)
+    )
+
+    camera_rotation_to_robot: np.ndarray = field(
+        default=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32)
+    )
+
     def to_kompass_cpp(self) -> RGBFollowerParameters:
         """
         Convert to kompass_cpp lib config format
@@ -68,21 +77,22 @@ def __init__(
         config_yaml_root_name: Optional[str] = None,
         **_,
     ):
-        config = config or VisionRGBFollowerConfig()
+        self._config = config or VisionRGBFollowerConfig()
 
         if config_file:
             config.from_yaml(config_file, config_yaml_root_name, get_common=False)
         self.__controller = RGBFollower(
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
-            config=config.to_kompass_cpp(),
+            config=self._config.to_kompass_cpp(),
         )
 
         self._found_ctrl = False
         logging.info("VISION TARGET FOLLOWING CONTROLLER IS READY")
 
-    def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_):
+    def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_) -> bool:
         self.__controller.reset_target(target_box)
+        return True
 
     def loop_step(
         self,
@@ -112,7 +122,7 @@ def linear_x_control(self):
         :return: Linear Velocity Control (m/s)
         :rtype: List[float]
         """
-        return [self._ctrl.vx] if self._found_ctrl else None
+        return self._ctrl.vx if self._found_ctrl else None
 
     @property
     def linear_y_control(self):
@@ -122,7 +132,7 @@ def linear_y_control(self):
         :return: Linear Velocity Control (m/s)
         :rtype: List[float]
         """
-        return [self._ctrl.vy] if self._found_ctrl else None
+        return self._ctrl.vy if self._found_ctrl else None
 
     @property
     def angular_control(self):
@@ -132,4 +142,4 @@ def angular_control(self):
         :return: Angular Velocity Control (rad/s)
         :rtype: List[float]
         """
-        return [self._ctrl.omega] if self._found_ctrl else None
+        return self._ctrl.omega if self._found_ctrl else None
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index 7eb7f53a..e0c909b8 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -46,15 +46,9 @@ class VisionRGBDFollowerConfig(DWAConfig):
     target_search_radius: float = field(
         default=0.5, validator=base_validators.in_range(min_value=1e-4, max_value=1e4)
     )
-    rotation_multiple: float = field(
+    rotation_gain: float = field(
         default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1.0)
     )
-    speed_depth_multiple: float = field(
-        default=0.7, validator=base_validators.in_range(min_value=1e-9, max_value=10.0)
-    )
-    min_vel: float = field(
-        default=0.1, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
-    )
     enable_search: bool = field(default=True)
     distance_tolerance: float = field(
         default=0.05, validator=base_validators.in_range(min_value=1e-6, max_value=1e3)
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 93bea427..133740a9 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -219,12 +219,11 @@ void bindings_control(py::module_ &m) {
       .def("set_resolution", &Control::DWA::resetOctreeResolution);
 
   // Vision Follower
-  py::class_<Control::RGBFollower::RGBFollowerConfig,
-             Control::Controller::ControllerParameters>(m_control,
+  py::class_<Control::RGBFollower::RGBFollowerConfig, Parameters>(m_control,
                                                         "RGBFollowerParameters")
       .def(py::init<>());
 
-  py::class_<Control::RGBFollower, Control::Controller>(m_control,
+  py::class_<Control::RGBFollower>(m_control,
                                                         "RGBFollower")
       .def(py::init<const Control::ControlType,
                     const Control::ControlLimitsParams,
@@ -233,15 +232,14 @@ void bindings_control(py::module_ &m) {
            py::arg("config"))
       .def("reset_target", &Control::RGBFollower::resetTarget)
       .def("get_ctrl", &Control::RGBFollower::getCtrl)
-      .def("run", &Control::RGBFollower::run);
+      .def("run", &Control::RGBFollower::run, py::arg("detection") = py::none());
 
   // Vision DWA
-  py::class_<Control::VisionDWA::VisionDWAConfig,
-             Control::Controller::ControllerParameters>(m_control,
+  py::class_<Control::VisionDWA::VisionDWAConfig, Control::RGBFollower::RGBFollowerConfig>(m_control,
                                                         "VisionDWAParameters")
       .def(py::init<>());
 
-  py::class_<Control::VisionDWA, Control::DWA>(m_control, "VisionDWA")
+  py::class_<Control::VisionDWA, Control::DWA, Control::RGBFollower>(m_control, "VisionDWA")
       .def(py::init<const Control::ControlType &,
                     const Control::ControlLimitsParams &, const int, const int,
                     const CollisionChecker::ShapeType &,
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 494db3af..1c1c78ad 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -133,7 +133,8 @@ void bindings_types(py::module_ &m) {
       .def_rw("size", &Bbox2D::size)
       .def_rw("timestamp", &Bbox2D::timestamp)
       .def_rw("label", &Bbox2D::label)
-      .def("set_vel", &Bbox2D::setVel);
+      .def("set_vel", &Bbox2D::setVel)
+      .def("set_img_size", &Bbox2D::setImgSize);
 
   py::class_<Bbox3D>(m_types, "Bbox3D")
       .def(py::init<>())
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
index 605ffdee..2cf43276 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -11,12 +11,12 @@
 namespace Kompass {
 namespace Control {
 
-class RGBFollower : public Controller {
+class RGBFollower {
 public:
 
-  class RGBFollowerConfig : public ControllerParameters {
+  class RGBFollowerConfig : public Parameters {
   public:
-    RGBFollowerConfig() : ControllerParameters() {
+    RGBFollowerConfig() {
       addParameter("control_time_step",
                    Parameter(0.1, 1e-4, 1e6, "Control time step (s)"));
       addParameter(
@@ -86,21 +86,22 @@ class RGBFollower : public Controller {
 
   const Velocities getCtrl() const;
 
-private:
-  ControlType _ctrlType;
+protected:
+  bool is_diff_drive_;
   ControlLimitsParams ctrl_limits_;
-  RGBFollowerConfig config_;
-
-  bool rotate_in_place_;
-  Velocities out_vel_;
   double recorded_search_time_ = 0.0, recorded_wait_time_ = 0.0;
   std::queue<std::array<double, 3>> search_commands_queue_;
   std::array<double, 3> search_command_;
   std::unique_ptr<Bbox2D> last_tracking_ = nullptr;
 
   void generateSearchCommands(float total_rotation, float search_radius,
-                              float max_rotation_time, bool enable_pause = false);
+    float max_rotation_time, bool enable_pause = false);
   void getFindTargetCmds(const int last_direction = 1);
+
+private:
+  RGBFollowerConfig config_;
+  Velocities out_vel_;
+
   void trackTarget(const Bbox2D &tracking);
   //
 };
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 17d899a2..57d64e0c 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -20,7 +20,7 @@
 namespace Kompass {
 namespace Control {
 
-class VisionDWA : public DWA {
+class VisionDWA : public DWA, public RGBFollower {
 public:
   class VisionDWAConfig : public RGBFollower::RGBFollowerConfig {
   public:
@@ -28,6 +28,12 @@ class VisionDWA : public DWA {
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
+      addParameter(
+            "distance_tolerance",
+            Parameter(0.1, 1e-6, 1e3, "Distance tolerance value (m)"));
+      addParameter(
+              "angle_tolerance",
+              Parameter(0.1, 1e-6, M_PI, "Angle tolerance value (rad)"));
       addParameter(
           "target_orientation",
           Parameter(0.0, -M_PI, M_PI,
@@ -205,10 +211,6 @@ class VisionDWA : public DWA {
                      const float yaw = 0.0);
 
 private:
-  ControlLimitsParams ctrl_limits_;
-  bool is_diff_drive_;
-  float recorded_search_time_ = 0.0, recorded_wait_time_ = 0.0;
-  std::queue<std::array<double, 3>> search_commands_queue_;
   VisionDWAConfig config_;
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
   std::unique_ptr<DepthDetector> detector_;
@@ -244,11 +246,6 @@ class VisionDWA : public DWA {
    */
   Velocity2D getPureTrackingCtrl(const float distance_erro, const float angle_error);
 
-  void generateSearchCommands(float total_rotation, float search_radius,
-                              float max_rotation_time,
-                              bool enable_pause = false);
-  void getFindTargetCmds(const int last_direction = 1);
-
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
    * sampling by direct following of the tracked target in local frame
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
index a6e93aa8..b2806443 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
@@ -11,14 +11,13 @@ namespace Control {
 RGBFollower::RGBFollower(const ControlType robotCtrlType,
                          const ControlLimitsParams robotCtrlLimits,
                          const RGBFollowerConfig config) {
-  _ctrlType = robotCtrlType;
   ctrl_limits_ = robotCtrlLimits;
   config_ = config;
   // Initialize time steps
   int num_steps = config_.control_horizon();
   // Initialize control vectors
   out_vel_ = Velocities(num_steps);
-  rotate_in_place_ = _ctrlType != ControlType::ACKERMANN;
+  is_diff_drive_ = robotCtrlType == ControlType::DIFFERENTIAL_DRIVE;
 }
 
 void RGBFollower::resetTarget(const Bbox2D &target) {
@@ -27,16 +26,17 @@ void RGBFollower::resetTarget(const Bbox2D &target) {
   std::swap(search_commands_queue_, empty);
   // Set the target as the current tracking bounding
   // box size
-  float size = (target.size.x() * target.size.y()) /
-               (target.img_size.x() * target.img_size.y());
+  LOG_DEBUG("Got target size: ", target.size.x(), ", ", target.size.y(), ". Image size=", target.img_size.x(), ", ", target.img_size.y(), ". Center=", target.getCenter().x(), ", ", target.getCenter().y());
+  float size = static_cast<float>(target.size.x() * target.size.y()) /
+               static_cast<float>(target.img_size.x() * target.img_size.y());
   LOG_DEBUG("Setting vision target reference distance to size: ", size);
   config_.set_target_distance(size);
 }
 
 void RGBFollower::generateSearchCommands(float total_rotation,
-                                         float search_radius,
-                                         float max_rotation_time,
-                                         bool enable_pause) {
+                                       float search_radius,
+                                       float max_rotation_time,
+                                       bool enable_pause) {
   // Calculate rotation direction and magnitude
   double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
   float rotation_time = max_rotation_time;
@@ -56,7 +56,7 @@ void RGBFollower::generateSearchCommands(float total_rotation,
   // Generate velocity commands
   for (float t = 0.0f; t <= max_rotation_time;
        t = t + config_.control_time_step()) {
-    if (rotate_in_place_) {
+    if (is_diff_drive_) {
       // In-place rotation
       search_commands_queue_.emplace(
           std::array<double, 3>{0.0, 0.0, rotation_sign * omega_val});
@@ -154,20 +154,23 @@ bool RGBFollower::run(const std::optional<Bbox2D> target) {
 }
 
 void RGBFollower::trackTarget(const Bbox2D &target) {
-  float current_dist = (target.size.x() * target.size.y()) /
-                       (target.img_size.x() * target.img_size.y());
+  float current_dist = static_cast<float>(target.size.x() * target.size.y()) /
+                       static_cast<float>(target.img_size.x() * target.img_size.y());
 
   float distance_error = config_.target_distance() - current_dist;
 
   float distance_tolerance = config_.tolerance() * config_.target_distance();
 
+  // Error to have the target in the center of the image (imgz_size / 2)
   double error_y = (2.0 * target.getCenter().y() / target.img_size.y() - 1.0);
   double error_x = (2.0 * target.getCenter().x() / target.img_size.x() - 1.0);
 
-  LOG_DEBUG("Current distance: ", current_dist,
-            ", Reference: ", config_.target_distance(),
-            "Distance_error=", distance_error,
-            ", Distance_tolerance=", distance_tolerance);
+  LOG_DEBUG("Current size: ", current_dist,
+            ", Reference size: ", config_.target_distance(),
+            "size_error=", distance_error,
+            ", tolerance=", distance_tolerance);
+
+  LOG_DEBUG("Img error x,y: ", error_x, ", ", error_y, ", tolerance: ", config_.tolerance());
 
   // Initialize control vectors
   std::fill(out_vel_.vx.begin(), out_vel_.vx.end(), 0.0);
@@ -175,6 +178,7 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
   std::fill(out_vel_.omega.begin(), out_vel_.omega.end(), 0.0);
 
   double simulated_depth = current_dist;
+  double simulated_error_x = error_x;
   double dist_speed, omega, v;
   for (size_t i = 0; i < out_vel_._length; ++i) {
     // If all errors are within limits -> break
@@ -183,8 +187,6 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
         std::abs(error_x) < config_.tolerance()) {
       break;
     }
-    if (rotate_in_place_ && i % 2 != 0)
-      continue;
 
     dist_speed = std::abs(distance_error) > distance_tolerance
                      ? distance_error * ctrl_limits_.velXParams.maxVel
@@ -204,35 +206,26 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
     v = std::abs(v) >= config_.min_vel() ? v : 0.0;
 
     simulated_depth += -v * config_.control_time_step();
+    simulated_error_x += -omega * config_.control_time_step();
+
     // Update distance error
     distance_error = config_.target_distance() - simulated_depth;
+    error_x = simulated_error_x;
 
-    if (rotate_in_place_) {
-      if (std::abs(v) < config_.min_vel()) {
-        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
-        out_vel_.set(i, 0.0, 0.0, omega);
-        if (i + 1 < out_vel_._length) {
-          out_vel_.set(i + 1, 0.0, 0.0, omega);
-        }
-      } else if (std::abs(omega) < config_.min_vel()) {
-        // Set vx_ctrl[i] and vx_ctrl[i+1] to 0.0
-        out_vel_.set(i, v, 0.0, 0.0);
-        if (i + 1 < out_vel_._length) {
-          out_vel_.set(i + 1, v, 0.0, 0.0);
-        }
-      } else {
-        // Set vx_ctrl[i] to 0.0
-        out_vel_.set(i, v, 0.0, 0.0);
+    if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and std::abs(omega) >= config_.min_vel()) {
+        // Rotate then move
+        out_vel_.set(i, omega, 0.0, 0.0);
         // Set vx_ctrl[i+1] to max(v, 0.0)
         if (i + 1 < out_vel_._length) {
-          out_vel_.set(i + 1, 0.0, 0.0, omega);
+          i++;
+          out_vel_.set(i, v, 0.0, 0.0);
         }
-      }
-    } else {
+    }
+    else{
       out_vel_.set(i, v, 0.0, omega);
     }
-  }
 
+  }
   return;
 }
 
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index ff07da28..fc883602 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -30,9 +30,8 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
           config.prediction_horizon(), config.control_horizon(),
           maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
           proximity_sensor_position_body, proximity_sensor_rotation_body,
-          octreeRes, costWeights, maxNumThreads) {
+          octreeRes, costWeights, maxNumThreads), RGBFollower(robotCtrlType, ctrlLimits) {
   ctrl_limits_ = ctrlLimits;
-  is_diff_drive_ = robotCtrlType == ControlType::DIFFERENTIAL_DRIVE;
   config_ = config;
   // Set the reaching goal distance (used in the DWA mode when vision target is
   // lost)
@@ -303,71 +302,6 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   return ref_traj;
 };
 
-void VisionDWA::generateSearchCommands(float total_rotation,
-                                       float search_radius,
-                                       float max_rotation_time,
-                                       bool enable_pause) {
-  // Calculate rotation direction and magnitude
-  double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
-  float rotation_time = max_rotation_time;
-  int num_pause_steps =
-      static_cast<int>(config_.search_pause() / config_.control_time_step());
-  if (enable_pause) {
-    // Modify the total number of active rotation to include the pause steps
-    rotation_time =
-        max_rotation_time * (1 - num_pause_steps / config_.control_time_step());
-  }
-  // Angular velocity to rotate 'total_rotation' in total time steps
-  // 'rotation_steps' with dt = control_time_step
-  double omega_val = total_rotation / rotation_time;
-
-  omega_val = std::max(std::min(omega_val, ctrl_limits_.omegaParams.maxOmega),
-                       config_.min_vel());
-  // Generate velocity commands
-  for (float t = 0.0f; t <= max_rotation_time;
-       t = t + config_.control_time_step()) {
-    if (is_diff_drive_) {
-      // In-place rotation
-      search_commands_queue_.emplace(
-          std::array<double, 3>{0.0, 0.0, rotation_sign * omega_val});
-    } else {
-      // Angular velocity based on linear velocity and radius
-      double omega_ackermann =
-          rotation_sign * ctrl_limits_.velXParams.maxVel / search_radius;
-      // Non-holonomic circular motion
-      search_commands_queue_.emplace(std::array<double, 3>{
-          ctrl_limits_.velXParams.maxVel, 0.0, omega_ackermann});
-    }
-    if (enable_pause) {
-      // Add zero commands for search pause
-      for (int j = 0; j <= num_pause_steps; j++) {
-        search_commands_queue_.emplace(std::array<double, 3>{0.0, 0.0, 0.0});
-      }
-    }
-  }
-  return;
-}
-
-void VisionDWA::getFindTargetCmds(const int last_direction) {
-  // Generate new search commands if starting a new search or no commands are
-  // available
-  LOG_DEBUG("Generating new search commands in direction: ", last_direction);
-
-  search_commands_queue_ = std::queue<std::array<double, 3>>();
-  const float target_searchtimeout_part = config_.target_search_timeout() / 4;
-  // rotate pi
-  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
-                         target_searchtimeout_part);
-  // go back
-  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
-                         target_searchtimeout_part);
-  // rotate -pi
-  generateSearchCommands(-last_direction * M_PI, config_.target_search_radius(),
-                         target_searchtimeout_part);
-  // go back
-  generateSearchCommands(last_direction * M_PI, config_.target_search_radius(),
-                         target_searchtimeout_part);
-}
 
 } // namespace Control
 } // namespace Kompass

From 935d6c02d595466b9c175b09fce2568abbd29fa6 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 12 Jun 2025 13:51:01 +0200
Subject: [PATCH 087/118] (refactor) files reformatting

---
 src/kompass_cpp/bindings/bindings_control.cpp | 23 +++----
 .../include/controllers/rgb_follower.h        | 15 ++---
 .../include/controllers/vision_dwa.h          | 65 ++++++++++---------
 .../src/controllers/rgb_follower.cpp          | 42 ++++++------
 .../src/controllers/vision_dwa.cpp            | 24 +++----
 5 files changed, 86 insertions(+), 83 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 133740a9..43fdb213 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -8,9 +8,9 @@
 #include "controllers/dwa.h"
 #include "controllers/follower.h"
 #include "controllers/pid.h"
+#include "controllers/rgb_follower.h"
 #include "controllers/stanley.h"
 #include "controllers/vision_dwa.h"
-#include "controllers/rgb_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/trajectory.h"
 
@@ -168,7 +168,7 @@ void bindings_control(py::module_ &m) {
       .def(py::init<Control::ControlLimitsParams, Control::ControlType, double,
                     double, double, int, int, CollisionChecker::ShapeType,
                     std::vector<float>, const Eigen::Vector3f &,
-                    const  Eigen::Vector4f &, double,
+                    const Eigen::Vector4f &, double,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("control_limits"), py::arg("control_type"),
            py::arg("time_step"), py::arg("prediction_horizon"),
@@ -181,7 +181,7 @@ void bindings_control(py::module_ &m) {
       .def(py::init<Control::TrajectorySampler::TrajectorySamplerParameters,
                     Control::ControlLimitsParams, Control::ControlType,
                     CollisionChecker::ShapeType, std::vector<float>,
-                    const Eigen::Vector3f &, const  Eigen::Vector4f &,
+                    const Eigen::Vector3f &, const Eigen::Vector4f &,
                     Control::CostEvaluator::TrajectoryCostsWeights, int>(),
            py::arg("config"), py::arg("control_limits"),
            py::arg("control_type"), py::arg("robot_shape_type"),
@@ -219,12 +219,11 @@ void bindings_control(py::module_ &m) {
       .def("set_resolution", &Control::DWA::resetOctreeResolution);
 
   // Vision Follower
-  py::class_<Control::RGBFollower::RGBFollowerConfig, Parameters>(m_control,
-                                                        "RGBFollowerParameters")
+  py::class_<Control::RGBFollower::RGBFollowerConfig, Parameters>(
+      m_control, "RGBFollowerParameters")
       .def(py::init<>());
 
-  py::class_<Control::RGBFollower>(m_control,
-                                                        "RGBFollower")
+  py::class_<Control::RGBFollower>(m_control, "RGBFollower")
       .def(py::init<const Control::ControlType,
                     const Control::ControlLimitsParams,
                     const Control::RGBFollower::RGBFollowerConfig>(),
@@ -232,14 +231,16 @@ void bindings_control(py::module_ &m) {
            py::arg("config"))
       .def("reset_target", &Control::RGBFollower::resetTarget)
       .def("get_ctrl", &Control::RGBFollower::getCtrl)
-      .def("run", &Control::RGBFollower::run, py::arg("detection") = py::none());
+      .def("run", &Control::RGBFollower::run,
+           py::arg("detection") = py::none());
 
   // Vision DWA
-  py::class_<Control::VisionDWA::VisionDWAConfig, Control::RGBFollower::RGBFollowerConfig>(m_control,
-                                                        "VisionDWAParameters")
+  py::class_<Control::VisionDWA::VisionDWAConfig,
+             Control::RGBFollower::RGBFollowerConfig>(m_control,
+                                                      "VisionDWAParameters")
       .def(py::init<>());
 
-  py::class_<Control::VisionDWA, Control::DWA, Control::RGBFollower>(m_control, "VisionDWA")
+  py::class_<Control::VisionDWA, Control::DWA>(m_control, "VisionDWA")
       .def(py::init<const Control::ControlType &,
                     const Control::ControlLimitsParams &, const int, const int,
                     const CollisionChecker::ShapeType &,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
index 2cf43276..aeea030d 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -13,7 +13,6 @@ namespace Control {
 
 class RGBFollower {
 public:
-
   class RGBFollowerConfig : public Parameters {
   public:
     RGBFollowerConfig() {
@@ -22,9 +21,7 @@ class RGBFollower {
       addParameter(
           "control_horizon",
           Parameter(2, 1, 1000, "Number of steps for applying the control"));
-      addParameter(
-          "tolerance",
-          Parameter(0.1, 0.0, 1.0, "Tolerance value"));
+      addParameter("tolerance", Parameter(0.1, 0.0, 1.0, "Tolerance value"));
       addParameter(
           "target_distance",
           Parameter(
@@ -41,7 +38,6 @@ class RGBFollower {
       addParameter("speed_gain", Parameter(1.0, 1e-2, 10.0));
       addParameter("min_vel", Parameter(0.01, 1e-9, 1e9));
       addParameter("enable_search", Parameter(false));
-
     }
     bool enable_search() const { return getParameter<bool>("enable_search"); }
     double control_time_step() const {
@@ -74,13 +70,13 @@ class RGBFollower {
   };
 
   RGBFollower(const ControlType robotCtrlType,
-                 const ControlLimitsParams ctrl_limits,
-                 const RGBFollowerConfig config = RGBFollowerConfig());
+              const ControlLimitsParams ctrl_limits,
+              const RGBFollowerConfig config = RGBFollowerConfig());
 
   // Default Destructor
   ~RGBFollower() = default;
 
-  void resetTarget(const Bbox2D& tracking);
+  void resetTarget(const Bbox2D &tracking);
 
   bool run(const std::optional<Bbox2D> tracking);
 
@@ -95,7 +91,8 @@ class RGBFollower {
   std::unique_ptr<Bbox2D> last_tracking_ = nullptr;
 
   void generateSearchCommands(float total_rotation, float search_radius,
-    float max_rotation_time, bool enable_pause = false);
+                              float max_rotation_time,
+                              bool enable_pause = false);
   void getFindTargetCmds(const int last_direction = 1);
 
 private:
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 57d64e0c..eb93529a 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "controllers/rgb_follower.h"
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
 #include "datatypes/path.h"
@@ -9,7 +10,6 @@
 #include "utils/logger.h"
 #include "vision/depth_detector.h"
 #include "vision/tracker.h"
-#include "controllers/rgb_follower.h"
 #include <Eigen/Dense>
 #include <cmath>
 #include <memory>
@@ -28,12 +28,10 @@ class VisionDWA : public DWA, public RGBFollower {
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
-      addParameter(
-            "distance_tolerance",
-            Parameter(0.1, 1e-6, 1e3, "Distance tolerance value (m)"));
-      addParameter(
-              "angle_tolerance",
-              Parameter(0.1, 1e-6, M_PI, "Angle tolerance value (rad)"));
+      addParameter("distance_tolerance",
+                   Parameter(0.1, 1e-6, 1e3, "Distance tolerance value (m)"));
+      addParameter("angle_tolerance",
+                   Parameter(0.1, 1e-6, M_PI, "Angle tolerance value (rad)"));
       addParameter(
           "target_orientation",
           Parameter(0.0, -M_PI, M_PI,
@@ -41,7 +39,9 @@ class VisionDWA : public DWA, public RGBFollower {
       addParameter(
           "use_local_coordinates",
           Parameter(true,
-                    "Track the item in the local frame of the robot. This mode cannot track the object velocity but can operate without knowing the robot's absolute position (world frame)"));
+                    "Track the item in the local frame of the robot. This mode "
+                    "cannot track the object velocity but can operate without "
+                    "knowing the robot's absolute position (world frame)"));
       addParameter("error_pose", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_vel", Parameter(0.05, 1e-9, 1e9));
       addParameter("error_acc", Parameter(0.05, 1e-9, 1e9));
@@ -63,8 +63,12 @@ class VisionDWA : public DWA, public RGBFollower {
     int prediction_horizon() const {
       return getParameter<int>("prediction_horizon");
     }
-    double dist_tolerance() const { return getParameter<double>("distance_tolerance"); }
-    double ang_tolerance() const { return getParameter<double>("angle_tolerance"); }
+    double dist_tolerance() const {
+      return getParameter<double>("distance_tolerance");
+    }
+    double ang_tolerance() const {
+      return getParameter<double>("angle_tolerance");
+    }
     double target_orientation() const {
       return getParameter<double>("target_orientation");
     }
@@ -122,9 +126,9 @@ class VisionDWA : public DWA, public RGBFollower {
         // Update the tracker with the detected boxes
         bool tracking_updated = tracker_->updateTracking(detected_boxes);
         if (!tracking_updated) {
-          LOG_WARNING("Tracker failed to update target with the detected boxes");
-        }
-        else{
+          LOG_WARNING(
+              "Tracker failed to update target with the detected boxes");
+        } else {
           tracked_pose = tracker_->getFilteredTrackedPose2D();
         }
       } else {
@@ -165,8 +169,9 @@ class VisionDWA : public DWA, public RGBFollower {
         // Update the tracker with the detected boxes
         bool tracking_updated = tracker_->updateTracking(boxes_3d.value());
         if (!tracking_updated) {
-          LOG_WARNING("Tracker failed to update target with the detected boxes");
-        }else{
+          LOG_WARNING(
+              "Tracker failed to update target with the detected boxes");
+        } else {
           tracked_pose = tracker_->getFilteredTrackedPose2D();
         }
       } else {
@@ -207,8 +212,7 @@ class VisionDWA : public DWA, public RGBFollower {
 
   bool
   setInitialTracking(const Eigen::MatrixX<unsigned short> &aligned_depth_image,
-                     const Bbox2D &target_box_2d,
-                     const float yaw = 0.0);
+                     const Bbox2D &target_box_2d, const float yaw = 0.0);
 
 private:
   VisionDWAConfig config_;
@@ -244,7 +248,8 @@ class VisionDWA : public DWA, public RGBFollower {
    * @param angle_error
    * @return Velocity2D
    */
-  Velocity2D getPureTrackingCtrl(const float distance_erro, const float angle_error);
+  Velocity2D getPureTrackingCtrl(const float distance_erro,
+                                 const float angle_error);
 
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
@@ -264,11 +269,11 @@ class VisionDWA : public DWA, public RGBFollower {
       // Reset recorded wait and search times
       recorded_wait_time_ = 0.0;
       recorded_search_time_ = 0.0;
-      LOG_INFO("Tracking target at position: ",
-                tracked_pose->x(), ", ",
-                tracked_pose->y(), " with velocity: ", tracked_pose->v(), ", ", tracked_pose->omega());
+      LOG_INFO("Tracking target at position: ", tracked_pose->x(), ", ",
+               tracked_pose->y(), " with velocity: ", tracked_pose->v(), ", ",
+               tracked_pose->omega());
       LOG_INFO("Robot current position: ", currentState.x, ", ",
-                currentState.y);
+               currentState.y);
       // Generate reference to target
       Trajectory2D ref_traj;
       if (is_diff_drive_) {
@@ -295,8 +300,7 @@ class VisionDWA : public DWA, public RGBFollower {
         this->currentPath->getSize() > 1) {
       // The tracking sample has collisions -> use DWA-like sampling and control
       return this->computeVelocityCommandsSet(current_vel, sensor_points);
-    }
-    else {
+    } else {
       // Start Search and/or Wait if enabled
       if (config_.enable_search()) {
         if (recorded_search_time_ < config_.target_search_timeout()) {
@@ -304,7 +308,7 @@ class VisionDWA : public DWA, public RGBFollower {
             LOG_DEBUG("Search commands queue is empty, generating new search "
                       "commands");
             int last_direction = 1;
-            if (latest_velocity_command_.omega() < 0){
+            if (latest_velocity_command_.omega() < 0) {
               last_direction = -1;
             }
             getFindTargetCmds(last_direction);
@@ -326,11 +330,11 @@ class VisionDWA : public DWA, public RGBFollower {
             search_commands_queue_.pop();
             recorded_search_time_ += config_.control_time_step();
             path.add(i + 1, 0.0, 0.0);
-            velocities.add(i, Velocity2D(search_command[0],
-                                                  search_command[1],
-                                                  search_command[2]));
+            velocities.add(i, Velocity2D(search_command[0], search_command[1],
+                                         search_command[2]));
           }
-          LOG_DEBUG("Sending ", config_.control_horizon(), " search commands "
+          LOG_DEBUG("Sending ", config_.control_horizon(),
+                    " search commands "
                     "to the controller");
           auto result = TrajSearchResult();
           result.isTrajFound = true;
@@ -338,8 +342,7 @@ class VisionDWA : public DWA, public RGBFollower {
           result.trajectory = Trajectory2D(velocities, path);
           return result;
         }
-      }
-      else {
+      } else {
         if (recorded_wait_time_ < config_.target_wait_timeout()) {
           auto timeout = config_.target_wait_timeout() - recorded_wait_time_;
           LOG_DEBUG("Target lost, waiting to get tracked target again ... "
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
index b2806443..1474c17b 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
@@ -26,7 +26,9 @@ void RGBFollower::resetTarget(const Bbox2D &target) {
   std::swap(search_commands_queue_, empty);
   // Set the target as the current tracking bounding
   // box size
-  LOG_DEBUG("Got target size: ", target.size.x(), ", ", target.size.y(), ". Image size=", target.img_size.x(), ", ", target.img_size.y(), ". Center=", target.getCenter().x(), ", ", target.getCenter().y());
+  LOG_DEBUG("Got target size: ", target.size.x(), ", ", target.size.y(),
+            ". Image size=", target.img_size.x(), ", ", target.img_size.y(),
+            ". Center=", target.getCenter().x(), ", ", target.getCenter().y());
   float size = static_cast<float>(target.size.x() * target.size.y()) /
                static_cast<float>(target.img_size.x() * target.img_size.y());
   LOG_DEBUG("Setting vision target reference distance to size: ", size);
@@ -34,9 +36,9 @@ void RGBFollower::resetTarget(const Bbox2D &target) {
 }
 
 void RGBFollower::generateSearchCommands(float total_rotation,
-                                       float search_radius,
-                                       float max_rotation_time,
-                                       bool enable_pause) {
+                                         float search_radius,
+                                         float max_rotation_time,
+                                         bool enable_pause) {
   // Calculate rotation direction and magnitude
   double rotation_sign = (total_rotation < 0.0) ? -1.0 : 1.0;
   float rotation_time = max_rotation_time;
@@ -154,8 +156,9 @@ bool RGBFollower::run(const std::optional<Bbox2D> target) {
 }
 
 void RGBFollower::trackTarget(const Bbox2D &target) {
-  float current_dist = static_cast<float>(target.size.x() * target.size.y()) /
-                       static_cast<float>(target.img_size.x() * target.img_size.y());
+  float current_dist =
+      static_cast<float>(target.size.x() * target.size.y()) /
+      static_cast<float>(target.img_size.x() * target.img_size.y());
 
   float distance_error = config_.target_distance() - current_dist;
 
@@ -167,10 +170,10 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
 
   LOG_DEBUG("Current size: ", current_dist,
             ", Reference size: ", config_.target_distance(),
-            "size_error=", distance_error,
-            ", tolerance=", distance_tolerance);
+            "size_error=", distance_error, ", tolerance=", distance_tolerance);
 
-  LOG_DEBUG("Img error x,y: ", error_x, ", ", error_y, ", tolerance: ", config_.tolerance());
+  LOG_DEBUG("Img error x,y: ", error_x, ", ", error_y,
+            ", tolerance: ", config_.tolerance());
 
   // Initialize control vectors
   std::fill(out_vel_.vx.begin(), out_vel_.vx.end(), 0.0);
@@ -212,19 +215,18 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
     distance_error = config_.target_distance() - simulated_depth;
     error_x = simulated_error_x;
 
-    if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and std::abs(omega) >= config_.min_vel()) {
-        // Rotate then move
-        out_vel_.set(i, omega, 0.0, 0.0);
-        // Set vx_ctrl[i+1] to max(v, 0.0)
-        if (i + 1 < out_vel_._length) {
-          i++;
-          out_vel_.set(i, v, 0.0, 0.0);
-        }
-    }
-    else{
+    if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and
+        std::abs(omega) >= config_.min_vel()) {
+      // Rotate then move
+      out_vel_.set(i, omega, 0.0, 0.0);
+      // Set vx_ctrl[i+1] to max(v, 0.0)
+      if (i + 1 < out_vel_._length) {
+        i++;
+        out_vel_.set(i, v, 0.0, 0.0);
+      }
+    } else {
       out_vel_.set(i, v, 0.0, omega);
     }
-
   }
   return;
 }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index fc883602..e6e5b6a2 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -30,7 +30,8 @@ VisionDWA::VisionDWA(const ControlType &robotCtrlType,
           config.prediction_horizon(), config.control_horizon(),
           maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
           proximity_sensor_position_body, proximity_sensor_rotation_body,
-          octreeRes, costWeights, maxNumThreads), RGBFollower(robotCtrlType, ctrlLimits) {
+          octreeRes, costWeights, maxNumThreads),
+      RGBFollower(robotCtrlType, ctrlLimits) {
   ctrl_limits_ = ctrlLimits;
   config_ = config;
   // Set the reaching goal distance (used in the DWA mode when vision target is
@@ -74,8 +75,8 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   distance = std::max(distance, 0.0f);
 
   float distance_error = config_.target_distance() - distance;
-  float angle_error =
-      Angle::normalizeToMinusPiPlusPi(config_.target_orientation() - gamma - psi);
+  float angle_error = Angle::normalizeToMinusPiPlusPi(
+      config_.target_orientation() - gamma - psi);
 
   Velocity2D followingVel;
   if (abs(distance_error) > config_.dist_tolerance() or
@@ -167,7 +168,7 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
 
   Trajectory2D ref_traj(config_.prediction_horizon());
   Path::State simulated_state(0.0, 0.0, 0.0);
-  if(track_velocity_){
+  if (track_velocity_) {
     // Global frame -> get actual state
     simulated_state = Path::State(currentState);
   }
@@ -189,13 +190,12 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
     } else {
       // Update the tracked state in the local frame after the robot have
       // moved (apply inverse of robot velocity)
-      tracked_state_tf =
-          getTransformation(simulated_state);
+      tracked_state_tf = getTransformation(simulated_state);
       auto new_state = transformPosition(
           Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
           tracked_state_tf.inverse());
       simulated_track = TrackedPose2D(new_state.x(), new_state.y(),
-                      simulated_track.z(), 0.0, 0.0, 0.0);
+                                      simulated_track.z(), 0.0, 0.0, 0.0);
     }
     if (step < config_.prediction_horizon() - 1) {
       ref_traj.velocities.add(step, cmd);
@@ -227,8 +227,8 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   // velocity
   while (step < config_.prediction_horizon()) {
     LOG_DEBUG("Step: ", step, "Simulated robot at", simulated_state.x, ",",
-             simulated_state.y, " simulated target at ",
-             simulated_track.x(), ",", simulated_track.y());
+              simulated_state.y, " simulated target at ", simulated_track.x(),
+              ",", simulated_track.y());
     this->setCurrentState(simulated_state);
     cmd = this->getPureTrackingCtrl(simulated_track);
     LOG_DEBUG("Got CMD: ", cmd.vx(), ",", cmd.omega());
@@ -241,8 +241,9 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
       simulated_state.update(vel_rotate, config_.control_time_step());
       if (track_velocity_) {
         simulated_track.update(config_.control_time_step());
-      }else{
-        // Update the tracked state in the local frame after the robot have moved (apply inverse of robot velocity)
+      } else {
+        // Update the tracked state in the local frame after the robot have
+        // moved (apply inverse of robot velocity)
         tracked_state_tf = getTransformation(simulated_state);
         auto new_state = transformPosition(
             Eigen::Vector3f(simulated_track.x(), simulated_track.y(), 0.0),
@@ -302,6 +303,5 @@ Trajectory2D VisionDWA::getTrackingReferenceSegmentDiffDrive(
   return ref_traj;
 };
 
-
 } // namespace Control
 } // namespace Kompass

From d653c23b10fb9b7ee2bd38828350de2ed3853ccf Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Mon, 16 Jun 2025 17:38:45 +0200
Subject: [PATCH 088/118] (feature) Adds utility for checking available
 accelerators if GPU build is enabled

---
 src/kompass_cpp/bindings/bindings.cpp         |  4 ++++
 .../kompass_cpp/include/utils/gpu_check.h     | 18 +++++++++++++++
 .../kompass_cpp/src/utils/gpu_check.cpp       | 23 +++++++++++++++++++
 3 files changed, 45 insertions(+)
 create mode 100644 src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp

diff --git a/src/kompass_cpp/bindings/bindings.cpp b/src/kompass_cpp/bindings/bindings.cpp
index d1cc3b3d..7d73e43b 100644
--- a/src/kompass_cpp/bindings/bindings.cpp
+++ b/src/kompass_cpp/bindings/bindings.cpp
@@ -1,9 +1,11 @@
 #include <Eigen/Dense>
 #include <nanobind/nanobind.h>
 #include <nanobind/operators.h>
+#include <nanobind/stl/string.h>
 #include <variant>
 
 #include "utils/logger.h"
+#include "utils/gpu_check.h"
 
 namespace py = nanobind;
 
@@ -34,6 +36,8 @@ NB_MODULE(kompass_cpp, m) {
       .value("WARNING", LogLevel::WARNING)
       .value("ERROR", LogLevel::ERROR)
       .export_values();
+
   m.def("set_log_level", &setLogLevel, "Set the log level");
   m.def("set_log_file", &setLogFile, "Set the log file");
+  m.def("get_available_accelerators", &getAvailableAccelerators, "Get available accelerators");
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h b/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h
new file mode 100644
index 00000000..c9498226
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/utils/gpu_check.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <string>
+
+/**
+ * @brief Retrieves the names of SYCL-compatible accelerator devices available
+ * on the system.
+ *
+ * This function queries the system using SYCL's runtime API to list the names
+ * of all devices across all platforms (e.g., GPUs, CPUs, and accelerators). The
+ * function only executes if the GPU macro is defined at compile time (e.g., via
+ * -DGPU=1 or target_compile_definitions in CMake). If the GPU macro is not
+ * defined, or no devices are available, an empty string is returned.
+ *
+ * @return A newline-separated string of device names, or an empty string if GPU
+ * support is not enabled or no devices are found.
+ */
+std::string getAvailableAccelerators();
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp
new file mode 100644
index 00000000..199ab8ae
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp
@@ -0,0 +1,23 @@
+#include <string>
+
+#ifdef GPU
+  #include <sycl/sycl.hpp>
+#endif
+
+std::string getAvailableAccelerators() {
+#ifdef GPU
+    std::string result;
+    auto platforms = sycl::platform::get_platforms();
+
+    for (const auto& platform : platforms) {
+        for (const auto& device : platform.get_devices()) {
+            result += device.get_info<sycl::info::device::name>() + "\n";
+        }
+    }
+
+    return result;
+#else
+    return "";
+#endif
+}
+

From 3e01bd69c0ae6dbca34b536ff5d2f57a2153b452 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Mon, 16 Jun 2025 17:41:13 +0200
Subject: [PATCH 089/118] (chore) Formats files

---
 src/kompass_cpp/bindings/bindings.cpp         |   5 +-
 .../include/utils/critical_zone_check.h       |   8 +-
 .../include/utils/critical_zone_check_gpu.h   |  20 ++--
 .../kompass_cpp/include/utils/logger.h        |   4 +-
 .../src/utils/critical_zone_check_gpu.cpp     | 110 +++++++++---------
 .../kompass_cpp/src/utils/gpu_check.cpp       |  19 ++-
 6 files changed, 84 insertions(+), 82 deletions(-)

diff --git a/src/kompass_cpp/bindings/bindings.cpp b/src/kompass_cpp/bindings/bindings.cpp
index 7d73e43b..32aab78e 100644
--- a/src/kompass_cpp/bindings/bindings.cpp
+++ b/src/kompass_cpp/bindings/bindings.cpp
@@ -4,8 +4,8 @@
 #include <nanobind/stl/string.h>
 #include <variant>
 
-#include "utils/logger.h"
 #include "utils/gpu_check.h"
+#include "utils/logger.h"
 
 namespace py = nanobind;
 
@@ -39,5 +39,6 @@ NB_MODULE(kompass_cpp, m) {
 
   m.def("set_log_level", &setLogLevel, "Set the log level");
   m.def("set_log_file", &setLogFile, "Set the log file");
-  m.def("get_available_accelerators", &getAvailableAccelerators, "Get available accelerators");
+  m.def("get_available_accelerators", &getAvailableAccelerators,
+        "Get available accelerators");
 }
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
index 28560111..9fa4e96a 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check.h
@@ -23,8 +23,7 @@ class CriticalZoneChecker {
                       const std::vector<float> &robot_dimensions,
                       const Eigen::Vector3f &sensor_position_body,
                       const Eigen::Vector4f &sensor_rotation_body,
-                      const float critical_angle,
-                      const float critical_distance,
+                      const float critical_angle, const float critical_distance,
                       const float slowdown_distance);
 
   /**
@@ -41,10 +40,11 @@ class CriticalZoneChecker {
    * @param ranges    LaserScan ranges
    * @param angles    LaserScan angles
    * @param forward   True if the robot is moving forward, false otherwise
-   * @return    Slowdown factor (0.0 - 1.0) if in the slowdown zone, 0.0 if in the critical zone (stop), 1.0 otherwise
+   * @return    Slowdown factor (0.0 - 1.0) if in the slowdown zone, 0.0 if in
+   * the critical zone (stop), 1.0 otherwise
    */
   float check(const std::vector<double> &ranges,
-             const std::vector<double> &angles, const bool forward);
+              const std::vector<double> &angles, const bool forward);
 
 protected:
   double robotHeight_{1.0}, robotRadius_;
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
index 41c050d3..39e9695c 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/critical_zone_check_gpu.h
@@ -16,16 +16,18 @@ namespace Kompass {
 class CriticalZoneCheckerGPU : public CriticalZoneChecker {
 public:
   // Constructor
-  CriticalZoneCheckerGPU(
-      const CollisionChecker::ShapeType robot_shape_type,
-      const std::vector<float> &robot_dimensions,
-      const Eigen::Vector3f &sensor_position_body,
-      const Eigen::Vector4f &sensor_rotation_body, const float critical_angle,
-      const float critical_distance,
-      const float slowdown_distance, const std::vector<double> &angles)
+  CriticalZoneCheckerGPU(const CollisionChecker::ShapeType robot_shape_type,
+                         const std::vector<float> &robot_dimensions,
+                         const Eigen::Vector3f &sensor_position_body,
+                         const Eigen::Vector4f &sensor_rotation_body,
+                         const float critical_angle,
+                         const float critical_distance,
+                         const float slowdown_distance,
+                         const std::vector<double> &angles)
       : CriticalZoneChecker(robot_shape_type, robot_dimensions,
                             sensor_position_body, sensor_rotation_body,
-                            critical_angle, critical_distance, slowdown_distance),
+                            critical_angle, critical_distance,
+                            slowdown_distance),
         m_scanSize(angles.size()) {
     m_q = sycl::queue{sycl::default_selector_v,
                       sycl::property::queue::in_order{}};
@@ -86,7 +88,7 @@ class CriticalZoneCheckerGPU : public CriticalZoneChecker {
    * @param forward        If the robot is moving forward
    */
   float check(const std::vector<double> &ranges,
-             const std::vector<double> &angles, const bool forward);
+              const std::vector<double> &angles, const bool forward);
 
 private:
   const int m_scanSize;
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/logger.h b/src/kompass_cpp/kompass_cpp/include/utils/logger.h
index 1143d98e..14601346 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/logger.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/logger.h
@@ -8,7 +8,7 @@
 
 #ifdef GPU
 #include <sycl/sycl.hpp>
-#endif  //!GPU
+#endif //! GPU
 
 namespace Kompass {
 
@@ -136,6 +136,6 @@ inline void setLogFile(const std::string &filename) {
 #else
 #define KERNEL_DEBUG(...)
 #endif
-#endif  // !GPU
+#endif // !GPU
 
 } // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
index 5908d240..f5a57d4b 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/critical_zone_check_gpu.cpp
@@ -15,68 +15,68 @@ float CriticalZoneCheckerGPU::check(const std::vector<double> &ranges,
 
     // command scope
     m_q.submit([&](sycl::handler &h) {
-         // local copies of class members to be used inside the kernel
-         const double robot_radius = robotRadius_;
-         auto transformation_matrix = sensor_tf_body_.matrix();
-         const sycl::vec<float, 4> x_transform{
-             transformation_matrix(0, 0), transformation_matrix(0, 1),
-             transformation_matrix(0, 2), transformation_matrix(0, 3)};
+      // local copies of class members to be used inside the kernel
+      const double robot_radius = robotRadius_;
+      auto transformation_matrix = sensor_tf_body_.matrix();
+      const sycl::vec<float, 4> x_transform{
+          transformation_matrix(0, 0), transformation_matrix(0, 1),
+          transformation_matrix(0, 2), transformation_matrix(0, 3)};
 
-         sycl::vec<float, 4> y_transform{
-             transformation_matrix(1, 0), transformation_matrix(1, 1),
-             transformation_matrix(1, 2), transformation_matrix(1, 3)};
+      sycl::vec<float, 4> y_transform{
+          transformation_matrix(1, 0), transformation_matrix(1, 1),
+          transformation_matrix(1, 2), transformation_matrix(1, 3)};
 
-         const auto devicePtrRanges = m_devicePtrRanges;
-         const auto devicePtrAngles = m_devicePtrAngles;
-         const auto devicePtrOutput = m_devicePtrOutput;
-         const auto criticalDistance = critical_distance_;
-         const auto slowdownDistance = slowdown_distance_;
-         size_t *critical_indices;
-         float* result = m_result;
-         *result = 1.0f;
-         sycl::range<1> global_size;
-         if (forward) {
-           critical_indices = m_devicePtrForward;
-           global_size = sycl::range<1>(indicies_forward_.size());
-         } else {
-           critical_indices = m_devicePtrBackward;
-           global_size = sycl::range<1>(indicies_backward_.size());
-         }
+      const auto devicePtrRanges = m_devicePtrRanges;
+      const auto devicePtrAngles = m_devicePtrAngles;
+      const auto devicePtrOutput = m_devicePtrOutput;
+      const auto criticalDistance = critical_distance_;
+      const auto slowdownDistance = slowdown_distance_;
+      size_t *critical_indices;
+      float *result = m_result;
+      *result = 1.0f;
+      sycl::range<1> global_size;
+      if (forward) {
+        critical_indices = m_devicePtrForward;
+        global_size = sycl::range<1>(indicies_forward_.size());
+      } else {
+        critical_indices = m_devicePtrBackward;
+        global_size = sycl::range<1>(indicies_backward_.size());
+      }
 
-         // kernel scope
-         h.parallel_for<class checkCriticalZoneKernel>(
-             global_size, [=](sycl::id<1> idx) {
-               const size_t local_id = critical_indices[idx];
-               double range = devicePtrRanges[local_id];
-               double angle = devicePtrAngles[local_id];
+      // kernel scope
+      h.parallel_for<class checkCriticalZoneKernel>(
+          global_size, [=](sycl::id<1> idx) {
+            const size_t local_id = critical_indices[idx];
+            double range = devicePtrRanges[local_id];
+            double angle = devicePtrAngles[local_id];
 
-               sycl::vec<float, 4> point{range * sycl::cos(angle),
-                                         range * sycl::sin(angle), 0.0, 1.0};
-               sycl::vec<float, 2> transformed_point{0.0, 0.0};
+            sycl::vec<float, 4> point{range * sycl::cos(angle),
+                                      range * sycl::sin(angle), 0.0, 1.0};
+            sycl::vec<float, 2> transformed_point{0.0, 0.0};
 
-               for (size_t i = 0; i < 4; ++i) {
-                 transformed_point[0] += x_transform[i] * point[i];
-                 transformed_point[1] += y_transform[i] * point[i];
-               }
+            for (size_t i = 0; i < 4; ++i) {
+              transformed_point[0] += x_transform[i] * point[i];
+              transformed_point[1] += y_transform[i] * point[i];
+            }
 
-               float converted_range = sycl::length(transformed_point);
-               if (converted_range - robot_radius <= criticalDistance) {
-                 devicePtrOutput[idx] = 0.0f;
-               } else if (converted_range - robot_radius <= slowdownDistance) {
-                 devicePtrOutput[idx] =
-                     (converted_range - robot_radius - criticalDistance) /
-                     (slowdownDistance - criticalDistance);
-               } else {
-                 devicePtrOutput[idx] = 1.0f;
-               }
+            float converted_range = sycl::length(transformed_point);
+            if (converted_range - robot_radius <= criticalDistance) {
+              devicePtrOutput[idx] = 0.0f;
+            } else if (converted_range - robot_radius <= slowdownDistance) {
+              devicePtrOutput[idx] =
+                  (converted_range - robot_radius - criticalDistance) /
+                  (slowdownDistance - criticalDistance);
+            } else {
+              devicePtrOutput[idx] = 1.0f;
+            }
 
-               sycl::atomic_ref<float, sycl::memory_order::relaxed,
-                                sycl::memory_scope::device,
-                                sycl::access::address_space::global_space>
-                   atomic_cost(*result);
-               atomic_cost.fetch_min(devicePtrOutput[idx]);
-             });
-       });
+            sycl::atomic_ref<float, sycl::memory_order::relaxed,
+                             sycl::memory_scope::device,
+                             sycl::access::address_space::global_space>
+                atomic_cost(*result);
+            atomic_cost.fetch_min(devicePtrOutput[idx]);
+          });
+    });
 
     m_q.wait_and_throw();
 
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp
index 199ab8ae..85c742ed 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/gpu_check.cpp
@@ -1,23 +1,22 @@
 #include <string>
 
 #ifdef GPU
-  #include <sycl/sycl.hpp>
+#include <sycl/sycl.hpp>
 #endif
 
 std::string getAvailableAccelerators() {
 #ifdef GPU
-    std::string result;
-    auto platforms = sycl::platform::get_platforms();
+  std::string result;
+  auto platforms = sycl::platform::get_platforms();
 
-    for (const auto& platform : platforms) {
-        for (const auto& device : platform.get_devices()) {
-            result += device.get_info<sycl::info::device::name>() + "\n";
-        }
+  for (const auto &platform : platforms) {
+    for (const auto &device : platform.get_devices()) {
+      result += device.get_info<sycl::info::device::name>() + "\n";
     }
+  }
 
-    return result;
+  return result;
 #else
-    return "";
+  return "";
 #endif
 }
-

From 03efda15f8690a0ec39a57fb3b280550b68cd79b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 17 Jun 2025 16:31:08 +0800
Subject: [PATCH 090/118] (fix) Moves control horizon from RGBFollower to
 VisionDWA

---
 .../include/controllers/rgb_follower.h        |   4 -
 .../include/controllers/vision_dwa.h          |   4 +
 .../src/controllers/rgb_follower.cpp          | 117 ++++++++----------
 3 files changed, 57 insertions(+), 68 deletions(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
index aeea030d..64e50d90 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -18,9 +18,6 @@ class RGBFollower {
     RGBFollowerConfig() {
       addParameter("control_time_step",
                    Parameter(0.1, 1e-4, 1e6, "Control time step (s)"));
-      addParameter(
-          "control_horizon",
-          Parameter(2, 1, 1000, "Number of steps for applying the control"));
       addParameter("tolerance", Parameter(0.1, 0.0, 1.0, "Tolerance value"));
       addParameter(
           "target_distance",
@@ -55,7 +52,6 @@ class RGBFollower {
     double search_pause() const {
       return getParameter<double>("target_search_pause");
     }
-    int control_horizon() const { return getParameter<int>("control_horizon"); }
     double tolerance() const { return getParameter<double>("tolerance"); }
     double target_distance() const {
       double val = getParameter<double>("target_distance");
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index eb93529a..2d7d6252 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -25,6 +25,9 @@ class VisionDWA : public DWA, public RGBFollower {
   class VisionDWAConfig : public RGBFollower::RGBFollowerConfig {
   public:
     VisionDWAConfig() : RGBFollower::RGBFollowerConfig() {
+      addParameter(
+        "control_horizon",
+        Parameter(2, 1, 1000, "Number of steps for applying the control"));
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
@@ -57,6 +60,7 @@ class VisionDWA : public DWA, public RGBFollower {
           "max_depth",
           Parameter(1e3, 1e-3, 1e9, "Range of interest minimum depth value"));
     }
+    int control_horizon() const { return getParameter<int>("control_horizon"); }
     bool enable_vel_tracking() const {
       return not getParameter<bool>("use_local_coordinates");
     }
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
index 1474c17b..48ad64d0 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
@@ -4,6 +4,7 @@
 #include "utils/logger.h"
 #include <cmath>
 #include <memory>
+#include <algorithm>
 
 namespace Kompass {
 namespace Control {
@@ -13,10 +14,6 @@ RGBFollower::RGBFollower(const ControlType robotCtrlType,
                          const RGBFollowerConfig config) {
   ctrl_limits_ = robotCtrlLimits;
   config_ = config;
-  // Initialize time steps
-  int num_steps = config_.control_horizon();
-  // Initialize control vectors
-  out_vel_ = Velocities(num_steps);
   is_diff_drive_ = robotCtrlType == ControlType::DIFFERENTIAL_DRIVE;
 }
 
@@ -109,10 +106,6 @@ bool RGBFollower::run(const std::optional<Bbox2D> target) {
     recorded_search_time_ = 0.0;
     // Access the TrackingData object
     const auto &data = target.value();
-    // ensure size_xy or depth have valid values
-    if (last_tracking_ == nullptr) {
-      resetTarget(data);
-    }
     last_tracking_ = std::make_unique<Bbox2D>(data);
     // Track the target
     trackTarget(data);
@@ -165,69 +158,65 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
   float distance_tolerance = config_.tolerance() * config_.target_distance();
 
   // Error to have the target in the center of the image (imgz_size / 2)
-  double error_y = (2.0 * target.getCenter().y() / target.img_size.y() - 1.0);
-  double error_x = (2.0 * target.getCenter().x() / target.img_size.x() - 1.0);
+  float error_y = 2.0f * (static_cast<float>(target.getCenter().y()) / static_cast<float>(target.img_size.y()) -  0.5f);
+  float error_x = 2.0f * (static_cast<float>(target.getCenter().x()) / static_cast<float>(target.img_size.x()) -  0.5f);
 
   LOG_DEBUG("Current size: ", current_dist,
             ", Reference size: ", config_.target_distance(),
             "size_error=", distance_error, ", tolerance=", distance_tolerance);
 
-  LOG_DEBUG("Img error x,y: ", error_x, ", ", error_y,
-            ", tolerance: ", config_.tolerance());
-
-  // Initialize control vectors
-  std::fill(out_vel_.vx.begin(), out_vel_.vx.end(), 0.0);
-  std::fill(out_vel_.vy.begin(), out_vel_.vy.end(), 0.0);
-  std::fill(out_vel_.omega.begin(), out_vel_.omega.end(), 0.0);
-
-  double simulated_depth = current_dist;
-  double simulated_error_x = error_x;
-  double dist_speed, omega, v;
-  for (size_t i = 0; i < out_vel_._length; ++i) {
-    // If all errors are within limits -> break
-    if (std::abs(distance_error) < distance_tolerance &&
-        std::abs(error_y) < config_.tolerance() &&
-        std::abs(error_x) < config_.tolerance()) {
-      break;
-    }
-
-    dist_speed = std::abs(distance_error) > distance_tolerance
-                     ? distance_error * ctrl_limits_.velXParams.maxVel
-                     : 0.0;
-
-    // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0)
-    // in [-1, 1] Omega in [-alpha * omega_max, alpha * omega_max]
-    omega = -config_.K_omega() * error_x * ctrl_limits_.omegaParams.maxOmega;
-
-    // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height
-    // - 1.0) in [-1, 1] V in [-speed_max, speed_max]
-    v = config_.K_v() * dist_speed;
-
-    // Limit by the minimum allowed velocity to avoid sending meaningless low
-    // commands to the robot
-    omega = std::abs(omega) >= config_.min_vel() ? omega : 0.0;
-    v = std::abs(v) >= config_.min_vel() ? v : 0.0;
-
-    simulated_depth += -v * config_.control_time_step();
-    simulated_error_x += -omega * config_.control_time_step();
-
-    // Update distance error
-    distance_error = config_.target_distance() - simulated_depth;
-    error_x = simulated_error_x;
+  LOG_DEBUG("Img error x: ", error_x,
+            ", center_x: ", static_cast<float>(target.getCenter().x()), ", img_size_x: ", static_cast<float>(target.img_size.x()));
+  LOG_DEBUG("Img error y: ", error_y,
+  ", center_y: ", static_cast<float>(target.getCenter().y()), ", img_size_y: ", static_cast<float>(target.img_size.y()));
+
+  float dist_speed, omega, v;
+  // If all errors are within limits -> break
+  if (std::abs(distance_error) < distance_tolerance &&
+      std::abs(error_y) < config_.tolerance() &&
+      std::abs(error_x) < config_.tolerance()) {
+    // Set command to zero
+    out_vel_ = Velocities(1);
+    out_vel_.set(0, 0.0, 0.0, 0.0);
+    return;
+  }
 
-    if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and
-        std::abs(omega) >= config_.min_vel()) {
-      // Rotate then move
-      out_vel_.set(i, omega, 0.0, 0.0);
-      // Set vx_ctrl[i+1] to max(v, 0.0)
-      if (i + 1 < out_vel_._length) {
-        i++;
-        out_vel_.set(i, v, 0.0, 0.0);
-      }
-    } else {
-      out_vel_.set(i, v, 0.0, omega);
-    }
+  dist_speed = std::abs(distance_error) > distance_tolerance
+                    ? (distance_error / config_.target_distance()) * ctrl_limits_.velXParams.maxVel
+                    : 0.0;
+
+  // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0)
+  // in [-1, 1] Omega in [-K_omega * omega_max, K_omega * omega_max]
+  omega = -config_.K_omega() * error_x * ctrl_limits_.omegaParams.maxOmega;
+
+  // Y center error : (2.0 * tracking.center_xy[1] / tracking.img_height
+  // - 1.0) in [-1, 1] V in [-K_v * speed_max, K_v * speed_max]
+  v = config_.K_v() * dist_speed;
+
+  // Limit by the minimum allowed velocity to avoid sending meaningless low
+  // commands to the robot
+  omega = std::abs(omega) >= config_.min_vel() ? omega : 0.0;
+  float omega_limit = static_cast<float>(ctrl_limits_.omegaParams.maxOmega);
+  omega = std::clamp(omega, -omega_limit, omega_limit);
+
+  float v_limit = static_cast<float>(ctrl_limits_.velXParams.maxVel);
+  v = std::abs(v) >= config_.min_vel() ? v : 0.0;
+  v = std::clamp(v, -v_limit, v_limit);
+
+  LOG_DEBUG("dist_error ", distance_error, ", error_x: ", error_x);
+  LOG_DEBUG("v=", v, ", omega= ", omega);
+
+  if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and
+      std::abs(omega) >= config_.min_vel()) {
+    // Rotate then move
+    out_vel_ = Velocities(2);
+    out_vel_.set(0, 0.0, 0.0, omega);
+    out_vel_.set(1, v, 0.0, 0.0);
+  } else {
+    out_vel_ = Velocities(1);
+    out_vel_.set(0, v, 0.0, omega);
   }
+
   return;
 }
 

From a60c4f2e91a41fb02f5e3349f62f7f5ebf77df81 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 18 Jun 2025 16:05:34 +0800
Subject: [PATCH 091/118] (fix) Adds img_size to Bbox2D bindings

---
 src/kompass_core/control/rgb_follower.py    | 2 +-
 src/kompass_core/control/rgbd_follower.py   | 3 +++
 src/kompass_cpp/bindings/bindings_types.cpp | 1 +
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index d75b6fd0..f935462c 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -18,7 +18,7 @@ class VisionRGBFollowerConfig(BaseAttrs):
         default=2, validator=base_validators.in_range(min_value=1, max_value=1000)
     )
     buffer_size: int = field(
-        default=3, validator=base_validators.in_range(min_value=1, max_value=10)
+        default=1, validator=base_validators.in_range(min_value=1, max_value=10)
     )
     tolerance: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-6, max_value=1.0)
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index e0c909b8..d9dc49f7 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -33,6 +33,9 @@ class VisionRGBDFollowerConfig(DWAConfig):
     prediction_horizon: int = field(
         default=10, validator=base_validators.in_range(min_value=1, max_value=1000)
     )
+    buffer_size: int = field(
+        default=1, validator=base_validators.in_range(min_value=1, max_value=10)
+    )
     target_distance: Optional[float] = field(default=None)
     target_wait_timeout: float = field(
         default=30.0, validator=base_validators.in_range(min_value=0.0, max_value=1e3)
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 1c1c78ad..5d870d72 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -133,6 +133,7 @@ void bindings_types(py::module_ &m) {
       .def_rw("size", &Bbox2D::size)
       .def_rw("timestamp", &Bbox2D::timestamp)
       .def_rw("label", &Bbox2D::label)
+      .def_rw("img_size", &Bbox2D::img_size)
       .def("set_vel", &Bbox2D::setVel)
       .def("set_img_size", &Bbox2D::setImgSize);
 

From d0e4bee82a379b9fe6cfe7be67343fc95ba49f12 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 18 Jun 2025 10:47:52 +0200
Subject: [PATCH 092/118] (feature) Updates base attrs to get config from yaml,
 json or toml and remove Omegaconf dependency

---
 pyproject.toml                                |  2 +-
 src/kompass_core/algorithms/dvz.py            |  4 +-
 src/kompass_core/control/dvz.py               |  4 +-
 src/kompass_core/control/dwa.py               | 23 +++--
 src/kompass_core/control/rgb_follower.py      | 17 +++-
 src/kompass_core/control/rgbd_follower.py     | 27 ++++--
 src/kompass_core/control/stanley.py           | 23 +++--
 src/kompass_core/models.py                    |  8 +-
 src/kompass_core/py_path_tools/executor.py    |  6 +-
 .../third_party/fcl/collisions.py             |  8 +-
 src/kompass_core/third_party/ompl/planner.py  | 12 +--
 src/kompass_core/utils/base_attrs.py          | 94 +++++++++----------
 12 files changed, 131 insertions(+), 97 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index c0de873d..2db932e2 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -23,7 +23,7 @@ dependencies = [
     "numpy<=1.26.4",
     "numpy-quaternion>=2023.0.3",
     "python-fcl-fork",
-    "omegaconf",
+    "toml",
     "attrs>=23.2.0",
     "pyyaml",
     "scipy",
diff --git a/src/kompass_core/algorithms/dvz.py b/src/kompass_core/algorithms/dvz.py
index 94fd0413..a06cc37e 100644
--- a/src/kompass_core/algorithms/dvz.py
+++ b/src/kompass_core/algorithms/dvz.py
@@ -121,12 +121,12 @@ def _init_constant_zone_parameters(self) -> None:
         self.zone_shift_y_diff: float = 0.0
 
     def set_from_yaml(self, path_to_file: str) -> None:
-        """Setup the DVZ controller params from YAML.
+        """Setup the DVZ controller params from file.
 
         :param path_to_file:
         :type path_to_file: str
         """
-        self.config.from_yaml(path_to_file, nested_root_name="DVZ")
+        self.config.from_file(path_to_file, nested_root_name="DVZ")
         self._set_control_regularization()
 
     def _set_control_regularization(self) -> None:
diff --git a/src/kompass_core/control/dvz.py b/src/kompass_core/control/dvz.py
index 139179c2..6d03b6a0 100644
--- a/src/kompass_core/control/dvz.py
+++ b/src/kompass_core/control/dvz.py
@@ -71,11 +71,11 @@ def __init__(
         :type ctrl_limits: RobotCtrlLimits
         :param control_time_step: Time step (s)
         :type control_time_step: float
-        :param config_file: Path to YAML config file, defaults to None
+        :param config_file: Path to config file, defaults to None
         :type config_file: Optional[str], optional
         :param config: DVZ configuration, defaults to None
         :type config: Optional[DVZConfig], optional
-        :param config_yaml_root_name: Root name for the config in the YAML file, defaults to None
+        :param config_yaml_root_name: Root name for the config in the config file, defaults to None
         :type config_yaml_root_name: Optional[str], optional
         """
         # Init the controller
diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index cdcd9ffd..9283004e 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -194,17 +194,24 @@ def __init__(
         ctrl_limits: RobotCtrlLimits,
         config: Optional[DWAConfig] = None,
         config_file: Optional[str] = None,
-        config_yaml_root_name: Optional[str] = None,
+        config_root_name: Optional[str] = None,
         control_time_step: Optional[float] = None,
         **_,
     ):
-        """
-        Setup the controller
+        """Init DWA (Dynamic Window Approach) Controller
 
-        :param robot: Robot using the controller
+        :param robot: Robot object to be controlled
         :type robot: Robot
-        :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
-        :type params_file: str
+        :param ctrl_limits: Robot control limits
+        :type ctrl_limits: RobotCtrlLimits
+        :param config: Controller configuration, defaults to None
+        :type config: Optional[VisionRGBDFollowerConfig], optional
+        :param config_file: Path to config file (yaml, json, toml), defaults to None
+        :type config_file: Optional[str], optional
+        :param config_root_name: Root name for the controller config in the file, defaults to None
+        :type config_root_name: Optional[str], optional
+        :param control_time_step: Control time step (sec), defaults to None
+        :type control_time_step: Optional[float], optional
         """
         # Init and configure the planner
         if not config:
@@ -212,8 +219,8 @@ def __init__(
             config = DWAConfig()
 
         if config_file:
-            config.from_yaml(
-                file_path=config_file, nested_root_name=config_yaml_root_name
+            config.from_file(
+                file_path=config_file, nested_root_name=config_root_name
             )
 
         if control_time_step:
diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index f935462c..0cf4e586 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -74,13 +74,26 @@ def __init__(
         ctrl_limits: RobotCtrlLimits,
         config: Optional[VisionRGBFollowerConfig] = None,
         config_file: Optional[str] = None,
-        config_yaml_root_name: Optional[str] = None,
+        config_root_name: Optional[str] = None,
         **_,
     ):
+        """Init Vision RGB (Image) Follower Controller
+
+        :param robot: Robot object to be controlled
+        :type robot: Robot
+        :param ctrl_limits: Robot control limits
+        :type ctrl_limits: RobotCtrlLimits
+        :param config: Controller configuration, defaults to None
+        :type config: Optional[VisionRGBDFollowerConfig], optional
+        :param config_file: Path to config file (yaml, json, toml), defaults to None
+        :type config_file: Optional[str], optional
+        :param config_root_name: Root name for the controller config in the file, defaults to None
+        :type config_root_name: Optional[str], optional
+        """
         self._config = config or VisionRGBFollowerConfig()
 
         if config_file:
-            config.from_yaml(config_file, config_yaml_root_name, get_common=False)
+            config.from_file(config_file, config_root_name, get_common=False)
         self.__controller = RGBFollower(
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index d9dc49f7..131a6614 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -136,25 +136,36 @@ def __init__(
         ctrl_limits: RobotCtrlLimits,
         config: Optional[VisionRGBDFollowerConfig] = None,
         config_file: Optional[str] = None,
-        config_yaml_root_name: Optional[str] = None,
+        config_root_name: Optional[str] = None,
         control_time_step: Optional[float] = None,
         camera_focal_length: Optional[List[float]] = None,
         camera_principal_point: Optional[List[float]] = None,
         **_,
     ):
-        """
-        Setup the controller
+        """Init Vision RGBD (Depth) Follower Controller
 
-        :param robot: Robot using the controller
+        :param robot: Robot object to be controlled
         :type robot: Robot
-        :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
-        :type params_file: str
+        :param ctrl_limits: Robot control limits
+        :type ctrl_limits: RobotCtrlLimits
+        :param config: Controller configuration, defaults to None
+        :type config: Optional[VisionRGBDFollowerConfig], optional
+        :param config_file: Path to config file (yaml, json, toml), defaults to None
+        :type config_file: Optional[str], optional
+        :param config_root_name: Root name for the controller config in the file, defaults to None
+        :type config_root_name: Optional[str], optional
+        :param control_time_step: Control time step (sec), defaults to None
+        :type control_time_step: Optional[float], optional
+        :param camera_focal_length: Depth camera focal length, defaults to None
+        :type camera_focal_length: Optional[List[float]], optional
+        :param camera_principal_point: Depth camera principal point, defaults to None
+        :type camera_principal_point: Optional[List[float]], optional
         """
         self._config = config or VisionRGBDFollowerConfig()
 
         if config_file:
-            self._config.from_yaml(
-                file_path=config_file, nested_root_name=config_yaml_root_name
+            self._config.from_file(
+                file_path=config_file, nested_root_name=config_root_name
             )
 
         if control_time_step:
diff --git a/src/kompass_core/control/stanley.py b/src/kompass_core/control/stanley.py
index 451d375b..6d6bcbcc 100644
--- a/src/kompass_core/control/stanley.py
+++ b/src/kompass_core/control/stanley.py
@@ -110,17 +110,24 @@ def __init__(
         ctrl_limits: RobotCtrlLimits,
         config: Optional[StanleyConfig] = None,
         config_file: Optional[str] = None,
-        config_yaml_root_name: Optional[str] = None,
+        config_root_name: Optional[str] = None,
         generate_reference: bool = False,
         **_,
     ):
-        """
-        Setup the controller
+        """_summary_
 
-        :param robot: Robot using the controller
+        :param robot: Robot object to be controlled
         :type robot: Robot
-        :param params_file: Yaml file containing the parameters of the controller under 'dvz_controller'
-        :type params_file: str
+        :param ctrl_limits: Robot control limits
+        :type ctrl_limits: RobotCtrlLimits
+        :param config: Controller configuration, defaults to None
+        :type config: Optional[VisionRGBDFollowerConfig], optional
+        :param config_file: Path to config file (yaml, json, toml), defaults to None
+        :type config_file: Optional[str], optional
+        :param config_root_name: Root name for the controller config in the file, defaults to None
+        :type config_root_name: Optional[str], optional
+        :param generate_reference: Use to generate reference commands, defaults to False
+        :type generate_reference: bool, optional
         """
         self.__generate_reference = generate_reference
         self._robot = robot
@@ -130,8 +137,8 @@ def __init__(
             config = StanleyConfig(wheel_base=robot.wheelbase)
 
         if config_file:
-            config.from_yaml(
-                file_path=config_file, nested_root_name=config_yaml_root_name
+            config.from_file(
+                file_path=config_file, nested_root_name=config_root_name
             )
 
         self._config = config
diff --git a/src/kompass_core/models.py b/src/kompass_core/models.py
index 927368d9..d1c21d3b 100644
--- a/src/kompass_core/models.py
+++ b/src/kompass_core/models.py
@@ -175,14 +175,14 @@ def apply(
         )
         return output_state
 
-    def set_params_from_yaml(self, path_to_file: str) -> None:
+    def set_params_from_file(self, path_to_file: str) -> None:
         """
-        Sets the robot testing parameters values from a given yaml file under 'robot'
+        Sets the robot testing parameters values from a given config file under 'robot'
 
-        :param path_to_file: Path to YAML file
+        :param path_to_file: Path to file (yaml, json, toml)
         :type path_to_file: str
         """
-        self.params.from_yaml(path_to_file)
+        self.params.from_file(path_to_file)
 
     def set_linear_x_params(self, params: List[float]) -> None:
         """
diff --git a/src/kompass_core/py_path_tools/executor.py b/src/kompass_core/py_path_tools/executor.py
index 7321d5d2..9403002a 100644
--- a/src/kompass_core/py_path_tools/executor.py
+++ b/src/kompass_core/py_path_tools/executor.py
@@ -103,14 +103,14 @@ def __init__(self, params: Optional[PathExecutorParams] = None):
 
     def configure(self, config_file: str, nested_root_name: Optional[str] = None):
         """
-        Configure the executor using a yaml file
+        Configure the executor using a configuration file (yaml, json, toml)
 
-        :param config_file: _description_
+        :param config_file: Path to file with configuration parameters
         :type config_file: str
         :param nested_root_name: _description_, defaults to None
         :type nested_root_name: Optional[str], optional
         """
-        self.params.from_yaml(config_file, nested_root_name)
+        self.params.from_file(config_file, nested_root_name)
 
     def record_path_point(self, x: float, y: float, heading: float, vel: float) -> bool:
         """
diff --git a/src/kompass_core/third_party/fcl/collisions.py b/src/kompass_core/third_party/fcl/collisions.py
index b6ef0593..e3f9653f 100644
--- a/src/kompass_core/third_party/fcl/collisions.py
+++ b/src/kompass_core/third_party/fcl/collisions.py
@@ -49,11 +49,11 @@ def _setup_from_config(self):
 
         self.got_map = False
 
-    def configure(self, yaml_file: str, root_name: Optional[str] = None):
+    def configure(self, config_file: str, root_name: Optional[str] = None):
         """
-        Load configuration from yaml
+        Load configuration from file
 
-        :param yaml_file: Path to config file (.yaml)
+        :param yaml_file: Path to config file (yaml, json, toml)
         :type yaml_file: str
         :param root_name: Parent root name of the config in the file 'Parent.fcl' - config must be under 'fcl', defaults to None
         :type root_name: str | None, optional
@@ -62,7 +62,7 @@ def configure(self, yaml_file: str, root_name: Optional[str] = None):
             nested_root_name = root_name + ".fcl"
         else:
             nested_root_name = "fcl"
-        self._config.from_yaml(yaml_file, nested_root_name)
+        self._config.from_file(config_file, nested_root_name)
         self._setup_from_config()
 
     def update_state(self, robot_state: RobotState):
diff --git a/src/kompass_core/third_party/ompl/planner.py b/src/kompass_core/third_party/ompl/planner.py
index 6b6da3e8..a7f5a24d 100644
--- a/src/kompass_core/third_party/ompl/planner.py
+++ b/src/kompass_core/third_party/ompl/planner.py
@@ -144,15 +144,15 @@ def __init_fcl(self, map_3d: Optional[np.ndarray] = None):
 
     def configure(
         self,
-        yaml_file: str,
+        config_file: str,
         root_name: Optional[str] = None,
         planner_id: Optional[str] = None,
     ):
         """
-        Load config from a yaml file
+        Load config from a configuration file
 
-        :param yaml_file: Path to .yaml fila
-        :type yaml_file: str
+        :param config_file: Path to file (yaml, json, toml)
+        :type config_file: str
         :param root_name: Parent root name of the config in the file 'Parent.ompl' - config must be under 'ompl', defaults to None
         :type root_name: str | None, optional
         """
@@ -160,7 +160,7 @@ def configure(
             nested_root_name = root_name + ".ompl"
         else:
             nested_root_name = "ompl"
-        self._config.from_yaml(yaml_file, nested_root_name=nested_root_name)
+        self._config.from_file(config_file, nested_root_name=nested_root_name)
 
         # Set LOG level
         ompl.util.setLogLevel(
@@ -182,7 +182,7 @@ def configure(
         planner_config = self.available_planners[planner_id]
         planner_params = create_config_class(name=planner_name, conf=planner_config)()
 
-        planner_params.from_yaml(yaml_file, nested_root_name + "." + planner_name)
+        planner_params.from_file(config_file, nested_root_name + "." + planner_name)
 
         self.set_planner(planner_params, planner_id)
 
diff --git a/src/kompass_core/utils/base_attrs.py b/src/kompass_core/utils/base_attrs.py
index 086522bd..5bdcadf3 100644
--- a/src/kompass_core/utils/base_attrs.py
+++ b/src/kompass_core/utils/base_attrs.py
@@ -1,5 +1,7 @@
 import json
-# from types import NoneType, GenericAlias
+import os
+import yaml
+import toml
 from typing import (
     Any,
     Callable,
@@ -14,8 +16,7 @@
 from copy import deepcopy
 import numpy as np
 from attrs import asdict, define, fields_dict
-from attrs import Attribute, has as attrs_has
-from omegaconf import OmegaConf
+from attrs import Attribute
 
 
 def skip_no_init(a: Attribute, _) -> bool:
@@ -197,61 +198,56 @@ def from_dict(self, dict_obj: Dict) -> None:
                     )
                 setattr(self, key, value)
 
-    def from_yaml(
+    def _select_nested_config(
+        cls, config: Dict[str, Any], key_path: Optional[str]
+    ) -> Dict[str, Any]:
+        if not key_path:
+            return config
+        keys = key_path.split(".")
+        for key in keys:
+            config = config.get(key, {})
+        return config
+
+    def from_file(
         self,
         file_path: str,
         nested_root_name: Union[str, None] = None,
         get_common: bool = False,
-    ) -> None:
+    ) -> bool:
         """
-        Update class attributes from yaml
+        Update class attributes from yaml, json, or toml
 
-        :param file_path: Path to config file (.yaml)
-        :type file_path: str
+        :param file_path: Path to config file (.yaml, .json, .toml)
         :param nested_root_name: Nested root name for the config, defaults to None
-        :type nested_root_name: str | None, optional
+        :param get_common: Whether to get extra config root (for merging), defaults to False
         """
-        # Load the YAML file
-        raw_config = OmegaConf.load(file_path)
-
-        # check for root name if given
-        if nested_root_name:
-            config = OmegaConf.select(raw_config, nested_root_name)
-            if get_common:
-                extra_config = OmegaConf.select(raw_config, "/**")
+        ext: str = os.path.splitext(file_path)[1].lower()
+
+        with open(file_path, "r", encoding="utf-8") as f:
+            if ext in [".yaml", ".yml"]:
+                raw_config: Dict[str, Any] = yaml.safe_load(f)
+            elif ext == ".json":
+                raw_config = json.load(f)
+            elif ext == ".toml":
+                raw_config = toml.load(f)
             else:
-                extra_config = None
-        else:
-            config = raw_config
-            extra_config = None
-
-        for attr in self.__attrs_attrs__:
-            # Check in config
-            if hasattr(config, attr.name):
-                attr_value = getattr(self, attr.name)
-                # Check to handle nested config
-                if attrs_has(attr.type):
-                    root_name = f"{nested_root_name}.{attr.name}"
-
-                    attr_value.from_yaml(file_path, root_name)
-
-                    setattr(self, attr.name, attr_value)
-                else:
-                    setattr(self, attr.name, getattr(config, attr.name))
-
-            # Check in the common config if present
-            elif extra_config:
-                if hasattr(extra_config, attr.name):
-                    attr_value = getattr(self, attr.name)
-                    # Check to handle nested config
-                    if attrs_has(attr.type):
-                        root_name = f"/**.{attr.name}"
-
-                        attr_value.from_yaml(file_path, root_name)
-
-                        setattr(self, attr.name, attr_value)
-                    else:
-                        setattr(self, attr.name, getattr(extra_config, attr.name))
+                raise ValueError(f"Unsupported config format: {ext}")
+
+        # Extract specific and common config sections
+
+        config = self._select_nested_config(raw_config, nested_root_name)
+        extra_config = (
+            self._select_nested_config(raw_config, "/**") if get_common else {}
+        )
+
+        if not config and not extra_config:
+            return False
+
+        # Merge common config with selected config (selected config takes precedence)
+        merged_config = {**extra_config, **config}
+        # Set attributes from final merged config
+        self.from_dict(merged_config)
+        return True
 
     def to_json(self) -> Union[str, bytes, bytearray]:
         """

From f9b077d4e5a2741acbbb9c075347ac0ab909a64b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 19 Jun 2025 12:48:59 +0200
Subject: [PATCH 093/118] (docs) Updates README with package overview

---
 README.md | 126 ++++++++++++++++++++++++++++++++++--------------------
 1 file changed, 79 insertions(+), 47 deletions(-)

diff --git a/README.md b/README.md
index 4da4fe6d..fc3c5285 100644
--- a/README.md
+++ b/README.md
@@ -11,13 +11,14 @@
 [python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg
 [python-url]: https://www.python.org/downloads/
 
-Kompass Core is a fast, GPU powered motion planning and control package for robot navigation. The package contains C++ implementation for core algorithms along with Python wrappers. It also implements third party integrations with [OMPL](https://ompl.kavrakilab.org/) and [FCL](https://github.com/flexible-collision-library/fcl). The Kompass philosophy is to be blazzingly fast and highly reliable, by implementing GPGPU supported parallelized algorithms which are agnostic to underlying hardware. Thus Kompass Core can be run on CPUs, GPUs or FPGAs from a wide variety of vendors, making it easy for robot hardware manufacturers to switch underlying compute architecture.
+Kompass Core is a high-performance, GPU-accelerated library for motion planning, mapping, and control in robot navigation systems. The core algorithms are implemented in C++ with seamless Python bindings. It also implements third party integrations with [OMPL](https://ompl.kavrakilab.org/) and [FCL](https://github.com/flexible-collision-library/fcl). The Kompass philosophy is to be blazzingly fast and highly reliable, by implementing GPGPU supported parallelized algorithms which are agnostic to underlying hardware. Thus Kompass Core can be run on CPUs or GPUs from a wide variety of vendors, making it easy for robot hardware manufacturers to switch underlying compute architecture without overhauling their software stack.
 
 This package is developed to be used with [Kompass](https://github.com/automatika-robotics/kompass) for creating navigation stacks in [ROS2](https://docs.ros.org/en/rolling/index.html). For detailed usage documentation, check Kompass [docs](https://automatika-robotics.github.io/kompass/).
 
-## Installation
 
-### Install with GPU Support (Recommended)
+# Installation
+
+## Install with GPU Support (Recommended)
 
 To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack) based machine, you can simply run the following:
 
@@ -25,7 +26,7 @@ To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack)
 
 This script will install all relevant dependencies, including [AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp) and install the latest version of kompass-core from source. It is good practice to read the [script](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh) first.
 
-### Installing with pip (CPU only)
+## Installing with pip (CPU only)
 
 On Ubuntu versions >= 22.04, install dependencies by running the following:
 
@@ -37,7 +38,7 @@ Then install kompass-core as follows:
 
 Wheels are available on Pypi for linux x86_64 and aarch64 architectures. Please note that the version available on Pypi does not support GPU acceleration yet.
 
-### Installation Contents
+## Installation Contents
 
 The following three packages will become available once kompass-core is installed.
 
@@ -45,61 +46,92 @@ The following three packages will become available once kompass-core is installe
 - `kompass_cpp`: Python bindings for Kompass core C++ library containing the algorithms implementation for path tracking and motion control.
 - `omplpy`: Bespoke python bindings for the Open Motion Planning Library (OMPL).
 
-## Testing
 
-### Run Planning Test
+# 📦 Package Overview
+
+This repository consists the following modules:
+
+- `kompass_cpp/` — Core C++ implementation of planning, control, collision checking, and mapping algorithms.
+
+- `kompass_core/` — Python implementations and front-end classes for configuration and high-level logic.
+
+## `kompass_cpp` Overview
+
+`kompass_cpp/` contains the C++ package which includes mapping, control, trajectory planning, and vision-based tracking algorithms, with **GPU acceleration** support and Python bindings via `nanobind`.
+
+### 1. Mapping
+- Implements efficient local mapping algorithms.
+- Supports **GPU-accelerated** mapping for real-time performance.
+- Core classes: `LocalMapper`, `LocalMapperGPU`
+
+### 2. Control & Trajectory Planning
+- Includes multiple control strategies such as PID, Stanley, Dynamic Window Approach (DWA), and vision-guided controllers.
+- Supports **GPU-accelerated** trajectory sampling and cost evaluation with customizable weights.
+- Core classes: `Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator`
+
+### 3. Collision and Critical Zone Checking
+- Provides collision checking utilities and critical zone detection to ensure safe navigation.
+- Includes both CPU and GPU implementations.
+- Core classes: `CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU`
+
+### 4. Vision and Tracking
+- Feature-based bounding box tracking and depth detection for enhanced perception.
+- Supports robust vision-based navigation algorithms.
+- Core classes: `FeatureBasedBboxTracker`, `DepthDetector`
+
+### 5. Utilities
+- Thread pooling for efficient multi-threaded operations.
+- Logger utilities for runtime diagnostics.
+- Linear state-space Kalman filter implementation for state estimation.
+- Spline interpolation utilities in the `tk` namespace.
+
+### 6. Data Types and Parameters
+- Rich set of data types to represent paths, trajectories, controls, velocities, and bounding boxes.
+- Strongly-typed parameters and configuration classes to enable flexible tuning.
+
+### 7. Python Bindings
+- Comprehensive Python bindings built with `nanobind` to enable seamless integration with Python workflows.
+- Bindings cover core functionalities across mapping, control, vision, and utilities.
+
+
+
+## `kompass_core` Package Overview
+
+- `kompass_core.calibration` - Modules for robot motion model calibration, fitting and robot simulation.
+
+- `kompass_core.control` - A rich set of control strategies and configurations. Include the wrapper python classes for the C++ implementations:
+
+| Algorithm                                   | Description                                        |
+| ------------------------------------------- | -------------------------------------------------- |
+| **Stanley**                   | Path tracking with robust convergence              |
+| **DWA (Dynamic Window Approach)** | Velocity-space sampling and optimization           |
+| **DVZ**                           | Reactive obstacle avoidance using deformable zones |
+| **VisionRGBFollower**   | Follow visual targets using RGB images          |
+| **VisionRGBDFollower**   | Follow visual targets using RGBD (depth) images          |
+
+- `kompass_core.datatypes` - Standardized message/data formats for various robot and sensor data.
 
-- `cd tests`
-- `python3 test_ompl.py`
 
-To test path planning using OMPL bindings a reference planning problem is provided using Turtlebot3 Waffle map and fixed start and end position. The test will simulate the planning problem for all geometric planners for the number of desired repetitions to get average values.
+- `kompass_core.mapping` - Local mapping and occupancy grid generation, with configuration support for various laser models and grid resolution settings.
 
-### Run Controllers Test
+- `kompass_core.models` - Robot models and motion kinematics, supporting differential, omni-directional, and Ackermann robots. Along with geometry definitions, control limits and simulation-ready state representations.
 
-- `cd tests`
-- `python3 test_controllers.py`
+- `kompass_core.motion_cost` - Cost models for trajectory evaluation in Python with various costs including collision probabilities, reference tracking and dynamic/static obstacle handling.
 
-The test will simulate path tracking using a reference global path. The results plot for each available controller will be generated in tests/resources/control
+- `kompass_core.performance` - Modules for evaluating algorithms performance.
 
-## Usage Example
+- `kompass_core.py_path_tools` - Path interpolation and execution tools.
 
-```python
-from kompass_core.control import DVZ
+- `kompass_core.simulation` - Tools for simulating robot motion and evaluating path feasibility.
 
-from kompass_core.models import (
-    AngularCtrlLimits,
-    LinearCtrlLimits,
-    Robot,
-    RobotCtrlLimits,
-    RobotGeometry,
-    RobotType,
-)
+- `kompass_core.third_party` - Wrappers and integrations with external planning and collision libraries:
 
-# Setup the robot
-my_robot = Robot(
-        robot_type=RobotType.ACKERMANN,
-        geometry_type=RobotGeometry.Type.CYLINDER,
-        geometry_params=np.array([0.1, 0.4]),
-    )
+    - FCL (Flexible Collision Library)
 
-# Set the robot control limits
-robot_ctr_limits = RobotCtrlLimits(
-    vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=5.0, max_decel=10.0),
-    omega_limits=AngularCtrlLimits(
-        max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
-    ),
-)
+    - OMPL (Open Motion Planning Library)
 
-# Set the control time step (s)
-control_time_step = 0.1     # seconds
+- `kompass_core.utils` - General utilities.
 
-# Initialize the controller
-dvz = DVZ(
-        robot=my_robot,
-        ctrl_limits=robot_ctr_limits,
-        control_time_step=control_time_step,
-    )
-```
 
 ## Copyright
 

From f3c0ec32170468f9d8b0817aeaea1a9baf2d16ba Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 19 Jun 2025 12:53:59 +0200
Subject: [PATCH 094/118] (docs) Updates README

---
 README.md | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index fc3c5285..cf200802 100644
--- a/README.md
+++ b/README.md
@@ -16,6 +16,12 @@ Kompass Core is a high-performance, GPU-accelerated library for motion planning,
 This package is developed to be used with [Kompass](https://github.com/automatika-robotics/kompass) for creating navigation stacks in [ROS2](https://docs.ros.org/en/rolling/index.html). For detailed usage documentation, check Kompass [docs](https://automatika-robotics.github.io/kompass/).
 
 
+- [**Install**](#installation) Kompass Core 🛠️
+- Check the [**Package Overview**](#-package-overview)
+- [**Copyright**](#copyright) and [**Contributions**](#contributions)
+- To use Kompass Core on your robot with ROS2, check the [**Kompass**](https://automatika-robotics.github.io/kompass) framework 🚀
+
+
 # Installation
 
 ## Install with GPU Support (Recommended)
@@ -95,7 +101,7 @@ This repository consists the following modules:
 
 
 
-## `kompass_core` Package Overview
+## `kompass_core` Overview
 
 - `kompass_core.calibration` - Modules for robot motion model calibration, fitting and robot simulation.
 

From d93141b7a4ff6c6b10481367ee9131b6a895d926 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 23:35:01 +0800
Subject: [PATCH 095/118] (feature) Adds method to get current tracking errors
 in vision followers

---
 src/kompass_core/control/rgb_follower.py         |  8 ++++++++
 src/kompass_core/control/rgbd_follower.py        | 15 ++++++++++++---
 src/kompass_cpp/bindings/bindings_control.cpp    |  2 ++
 .../include/controllers/rgb_follower.h           |  5 +++++
 .../kompass_cpp/include/controllers/vision_dwa.h | 16 +++++-----------
 .../kompass_cpp/src/controllers/rgb_follower.cpp | 14 ++++++++------
 .../kompass_cpp/src/controllers/vision_dwa.cpp   | 12 ++++++++++--
 7 files changed, 50 insertions(+), 22 deletions(-)

diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index 0cf4e586..3d0cc556 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -107,6 +107,14 @@ def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_) -> bool:
         self.__controller.reset_target(target_box)
         return True
 
+    @property
+    def dist_error(self) -> float:
+        return self._planner.get_errors()[0]
+
+    @property
+    def orientation_error(self) -> float:
+        return self._planner.get_errors()[1]
+
     def loop_step(
         self,
         *,
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index 131a6614..7b4bca25 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -216,9 +216,10 @@ def set_initial_tracking_2d_target(
         :type detected_boxes: List[Bbox3D]
         """
         try:
-            self._planner.set_current_state(
-                current_state.x, current_state.y, current_state.yaw, current_state.speed
-            )
+            if current_state:
+                self._planner.set_current_state(
+                    current_state.x, current_state.y, current_state.yaw, current_state.speed
+                )
             return self._planner.set_initial_tracking(
                 aligned_depth_image,
                 target_box,
@@ -229,6 +230,14 @@ def set_initial_tracking_2d_target(
             logging.error(f"Could not set initial tracking state: {e}")
             return False
 
+    @property
+    def dist_error(self) -> float:
+        return self._planner.get_errors()[0]
+
+    @property
+    def orientation_error(self) -> float:
+        return self._planner.get_errors()[1]
+
     def set_initial_tracking_image(
         self,
         current_state: RobotState,
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 43fdb213..220be664 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -231,6 +231,7 @@ void bindings_control(py::module_ &m) {
            py::arg("config"))
       .def("reset_target", &Control::RGBFollower::resetTarget)
       .def("get_ctrl", &Control::RGBFollower::getCtrl)
+      .def("get_errors", &Control::RGBFollower::getErrors)
       .def("run", &Control::RGBFollower::run,
            py::arg("detection") = py::none());
 
@@ -281,6 +282,7 @@ void bindings_control(py::module_ &m) {
                &Control::VisionDWA::setInitialTracking),
            py::arg("aligned_depth_image"), py::arg("target_box_2d"),
            py::arg("robot_orientation") = 0.0)
+      .def("get_errors", &Control::VisionDWA::getErrors)
       .def("get_tracking_ctrl",
            py::overload_cast<const Eigen::MatrixX<unsigned short> &,
                              const std::vector<Bbox2D> &,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
index 64e50d90..87867fc4 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -78,6 +78,10 @@ class RGBFollower {
 
   const Velocities getCtrl() const;
 
+  Eigen::Vector2f getErrors() const {
+    return Eigen::Vector2f(dist_error_, orientation_error_);
+  }
+
 protected:
   bool is_diff_drive_;
   ControlLimitsParams ctrl_limits_;
@@ -85,6 +89,7 @@ class RGBFollower {
   std::queue<std::array<double, 3>> search_commands_queue_;
   std::array<double, 3> search_command_;
   std::unique_ptr<Bbox2D> last_tracking_ = nullptr;
+  float dist_error_ = 0.0f, orientation_error_ = 0.0f;
 
   void generateSearchCommands(float total_rotation, float search_radius,
                               float max_rotation_time,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 2d7d6252..8d63406d 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -218,6 +218,10 @@ class VisionDWA : public DWA, public RGBFollower {
   setInitialTracking(const Eigen::MatrixX<unsigned short> &aligned_depth_image,
                      const Bbox2D &target_box_2d, const float yaw = 0.0);
 
+  Eigen::Vector2f getErrors() const {
+      return Eigen::Vector2f(dist_error_, orientation_error_);
+    }
+
 private:
   VisionDWAConfig config_;
   std::unique_ptr<FeatureBasedBboxTracker> tracker_;
@@ -243,17 +247,7 @@ class VisionDWA : public DWA, public RGBFollower {
    * @param tracking_pose
    * @return Velocity2D
    */
-  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose);
-
-  /**
-   * @brief Get the Pure Tracking Ctrl object
-   *
-   * @param distance_erro
-   * @param angle_error
-   * @return Velocity2D
-   */
-  Velocity2D getPureTrackingCtrl(const float distance_erro,
-                                 const float angle_error);
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose, const bool update_global_error = false);
 
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
index 48ad64d0..5835649f 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/rgb_follower.cpp
@@ -153,7 +153,7 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
       static_cast<float>(target.size.x() * target.size.y()) /
       static_cast<float>(target.img_size.x() * target.img_size.y());
 
-  float distance_error = config_.target_distance() - current_dist;
+  dist_error_ = config_.target_distance() - current_dist;
 
   float distance_tolerance = config_.tolerance() * config_.target_distance();
 
@@ -161,9 +161,11 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
   float error_y = 2.0f * (static_cast<float>(target.getCenter().y()) / static_cast<float>(target.img_size.y()) -  0.5f);
   float error_x = 2.0f * (static_cast<float>(target.getCenter().x()) / static_cast<float>(target.img_size.x()) -  0.5f);
 
+  orientation_error_ = error_x;
+
   LOG_DEBUG("Current size: ", current_dist,
             ", Reference size: ", config_.target_distance(),
-            "size_error=", distance_error, ", tolerance=", distance_tolerance);
+            "size_error=", dist_error_, ", tolerance=", distance_tolerance);
 
   LOG_DEBUG("Img error x: ", error_x,
             ", center_x: ", static_cast<float>(target.getCenter().x()), ", img_size_x: ", static_cast<float>(target.img_size.x()));
@@ -172,7 +174,7 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
 
   float dist_speed, omega, v;
   // If all errors are within limits -> break
-  if (std::abs(distance_error) < distance_tolerance &&
+  if (std::abs(dist_error_) < distance_tolerance &&
       std::abs(error_y) < config_.tolerance() &&
       std::abs(error_x) < config_.tolerance()) {
     // Set command to zero
@@ -181,8 +183,8 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
     return;
   }
 
-  dist_speed = std::abs(distance_error) > distance_tolerance
-                    ? (distance_error / config_.target_distance()) * ctrl_limits_.velXParams.maxVel
+  dist_speed = std::abs(dist_error_) > distance_tolerance
+                    ? (dist_error_ / config_.target_distance()) * ctrl_limits_.velXParams.maxVel
                     : 0.0;
 
   // X center error : (2.0 * tracking.center_xy[0] / tracking.img_width - 1.0)
@@ -203,7 +205,7 @@ void RGBFollower::trackTarget(const Bbox2D &target) {
   v = std::abs(v) >= config_.min_vel() ? v : 0.0;
   v = std::clamp(v, -v_limit, v_limit);
 
-  LOG_DEBUG("dist_error ", distance_error, ", error_x: ", error_x);
+  LOG_DEBUG("dist_error ", dist_error_, ", error_x: ", error_x);
   LOG_DEBUG("v=", v, ", omega= ", omega);
 
   if (is_diff_drive_ and std::abs(v) >= config_.min_vel() and
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
index e6e5b6a2..e64b17e4 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/vision_dwa.cpp
@@ -57,7 +57,7 @@ void VisionDWA::setCameraIntrinsics(const float focal_length_x,
       config_.depth_conversion_factor());
 }
 
-Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
+Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose, const bool update_global_error) {
   float distance, psi, gamma = 0.0f;
   if (track_velocity_) {
     distance = tracking_pose.distance(currentState.x, currentState.y, 0.0) -
@@ -78,6 +78,12 @@ Velocity2D VisionDWA::getPureTrackingCtrl(const TrackedPose2D &tracking_pose) {
   float angle_error = Angle::normalizeToMinusPiPlusPi(
       config_.target_orientation() - gamma - psi);
 
+  // Update error is enabled (to avoid update for simulated states)
+  if (update_global_error) {
+    dist_error_ = distance_error;
+    orientation_error_ = angle_error;
+  }
+
   Velocity2D followingVel;
   if (abs(distance_error) > config_.dist_tolerance() or
       abs(angle_error) > config_.ang_tolerance()) {
@@ -183,7 +189,9 @@ VisionDWA::getTrackingReferenceSegment(const TrackedPose2D &tracking_pose) {
     ref_traj.path.add(step,
                       Path::Point(simulated_state.x, simulated_state.y, 0.0));
     this->setCurrentState(simulated_state);
-    cmd = this->getPureTrackingCtrl(simulated_track);
+    // Get the pure tracking control command
+    // If step == 0, update the global error to have the errors for the initial state
+    cmd = this->getPureTrackingCtrl(simulated_track, (step == 0));
     simulated_state.update(cmd, config_.control_time_step());
     if (track_velocity_) {
       simulated_track.update(config_.control_time_step());

From e53a9c57ff2d8b22eeee086f8334c44c9f0dddc4 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 17:59:15 +0200
Subject: [PATCH 096/118] (docs) Adds controllers docstrings and JA/ZH README

---
 README.md                                 |  24 +++-
 docs/README.ja.md                         | 158 ++++++++++++++++++++++
 docs/README.zh.md                         | 156 +++++++++++++++++++++
 src/kompass_core/control/__init__.py      |   4 +-
 src/kompass_core/control/dwa.py           |   6 +-
 src/kompass_core/control/rgb_follower.py  | 117 +++++++++++++++-
 src/kompass_core/control/rgbd_follower.py | 137 ++++++++++++++++++-
 src/kompass_core/control/stanley.py       |   4 +-
 8 files changed, 589 insertions(+), 17 deletions(-)
 create mode 100644 docs/README.ja.md
 create mode 100644 docs/README.zh.md

diff --git a/README.md b/README.md
index cf200802..f63ed3e8 100644
--- a/README.md
+++ b/README.md
@@ -1,9 +1,15 @@
 # Kompass Core
 
+[![中文版本][cn-badge]][cn-url]
+[![ドキュメント-日本語][jp-badge]][jp-url]
 [![PyPI][pypi-badge]][pypi-url]
 [![MIT licensed][mit-badge]][mit-url]
 [![Python Version][python-badge]][python-url]
 
+[cn-badge]: https://img.shields.io/badge/文档-中文-blue.svg
+[cn-url]: docs/README.zh.md
+[jp-badge]: https://img.shields.io/badge/ドキュメント-日本語-red.svg
+[jp-url]: docs/README.ja.md
 [pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg
 [pypi-url]: https://pypi.org/project/kompass-core/
 [mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg
@@ -26,21 +32,27 @@ This package is developed to be used with [Kompass](https://github.com/automatik
 
 ## Install with GPU Support (Recommended)
 
-To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack) based machine, you can simply run the following:
+- To install kompass-core with GPU support, on any Ubuntu 20+ (including Jetpack) based machine, you can simply run the following:
 
-- `curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash`
+```bash
+curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash
+```
 
 This script will install all relevant dependencies, including [AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp) and install the latest version of kompass-core from source. It is good practice to read the [script](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh) first.
 
 ## Installing with pip (CPU only)
 
-On Ubuntu versions >= 22.04, install dependencies by running the following:
+- On Ubuntu versions >= 22.04, install dependencies by running the following:
 
-- `sudo apt-get install libompl-dev libfcl-dev libpcl-dev`
+```bash
+sudo apt-get install libompl-dev libfcl-dev libpcl-dev
+```
 
-Then install kompass-core as follows:
+- Then install kompass-core as follows:
 
-- `pip install kompass-core`
+```bash
+pip install kompass-core
+```
 
 Wheels are available on Pypi for linux x86_64 and aarch64 architectures. Please note that the version available on Pypi does not support GPU acceleration yet.
 
diff --git a/docs/README.ja.md b/docs/README.ja.md
new file mode 100644
index 00000000..47b3c611
--- /dev/null
+++ b/docs/README.ja.md
@@ -0,0 +1,158 @@
+# Kompass Core
+
+[![English Version][en-badge]][en-url]
+[![中文版本][cn-badge]][cn-url]
+[![PyPI][pypi-badge]][pypi-url]
+[![MIT licensed][mit-badge]][mit-url]
+[![Python Version][python-badge]][python-url]
+
+[en-badge]: https://img.shields.io/badge/Documentation-English-green.svg
+[en-url]: ../README.md
+[cn-badge]: https://img.shields.io/badge/文档-中文-blue.svg
+[cn-url]: README.zh.md
+[pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg
+[pypi-url]: https://pypi.org/project/kompass-core/
+[mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg
+[mit-url]: https://github.com/automatika-robotics/kompass-core/LICENSE
+[python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg
+[python-url]: https://www.python.org/downloads/
+
+**Kompass Core** は、ロボットナビゲーションシステムにおける経路計画、マッピング、および制御のための高性能で GPU アクセラレーション対応のライブラリです。コアアルゴリズムは C++ で実装され、Python バインディングによってシームレスに利用できます。また、[OMPL](https://ompl.kavrakilab.org/) や [FCL](https://github.com/flexible-collision-library/fcl) との外部統合も備えています。
+
+Kompass の理念は、「驚異的な高速性」と「高信頼性」を追求することです。GPGPU 対応の並列アルゴリズムを基盤とし、ハードウェアに依存しない設計を実現しているため、Kompass Core はさまざまなベンダーの CPU または GPU 上で実行可能です。これにより、ロボットハードウェアメーカーは、ソフトウェアスタックを大きく変更することなく、計算アーキテクチャを柔軟に切り替えることが可能です。
+
+このパッケージは、[ROS2](https://docs.ros.org/en/rolling/index.html) 上でナビゲーションスタックを構築するための [Kompass](https://github.com/automatika-robotics/kompass) と共に使用するように設計されています。詳細な使用方法については、[Kompass ドキュメント](https://automatika-robotics.github.io/kompass/) をご覧ください。
+
+- [**インストール**](#installation) Kompass Core 🛠️
+- [**パッケージ概要**](#-package-overview) を確認
+- [**著作権**](#copyright) および [**コントリビューション**](#contributions)
+- ROS2 環境でロボットに Kompass Core を導入したい場合は、[**Kompass**](https://automatika-robotics.github.io/kompass) フレームワークをご参照ください 🚀
+
+# インストール方法
+
+## GPU サポート付きインストール（推奨）
+
+Ubuntu 20 以降（Jetpack を含む）の任意のマシンに GPU サポート付きで kompass-core をインストールするには、以下を実行します：
+
+```bash
+curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash
+```
+
+このスクリプトは、[AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp) を含むすべての関連依存関係をインストールし、`kompass-core` の最新版をソースから構築します。実行前に、[スクリプト](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh) の内容を確認することを推奨します。
+
+
+## pip によるインストール（CPU のみ）
+
+- Ubuntu 22.04 以降では、以下のコマンドで依存パッケージをインストールします：
+
+```bash
+  sudo apt-get install libompl-dev libfcl-dev libpcl-dev
+```
+- その後、以下のように kompass-core をインストールします：
+
+```bash
+pip install kompass-core
+```
+
+PyPI では、Linux x86_64 と aarch64 向けのホイールが提供されています。なお、現時点で PyPI にあるバージョンは GPU アクセラレーションには対応していません。
+
+
+## インストール内容
+
+kompass-core をインストールすると、以下の 3 つのパッケージが使用可能になります。
+
+- `kompass_core`：2D 空間でのナビゲーションのための運動計画と制御に関するラッパーやユーティリティを含む主要な Python API
+- `kompass_cpp`：経路追跡および運動制御アルゴリズムを実装した Kompass コア C++ ライブラリの Python バインディング
+- `omplpy`：Open Motion Planning Library（OMPL）向けに特化した Python バインディング
+
+# 📦 パッケージ概要
+
+本リポジトリには以下のモジュールが含まれます：
+
+- `kompass_cpp/` — 計画、制御、衝突判定、マッピングアルゴリズムを実装したコア C++ モジュール
+
+- `kompass_core/` — 設定や高レベルロジックのための Python 実装およびフロントエンドクラス
+
+## `kompass_cpp` モジュール概要
+
+`kompass_cpp/` はマッピング、制御、軌道計画、視覚ベースのトラッキングアルゴリズムを含む C++ パッケージであり、**GPU アクセラレーション** をサポートし、`nanobind` 経由で Python バインディングが提供されています。
+
+### 1. マッピング
+- 高速な局所マッピングアルゴリズムを実装
+- **GPU アクセラレーション** に対応しリアルタイム性能を実現
+- 主なクラス：`LocalMapper`, `LocalMapperGPU`
+
+### 2. 制御と軌道計画
+- PID、Stanley、動的ウィンドウ法（DWA）、ビジョンガイドコントローラなど複数の制御戦略を搭載
+- **GPU アクセラレーション** による軌道サンプリングとコスト評価、重みのカスタマイズが可能
+- 主なクラス：`Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator`
+
+### 3. 衝突判定とクリティカルゾーン検出
+- 安全なナビゲーションを実現する衝突判定とクリティカルゾーン検出機能を提供
+- CPU 実装と GPU 実装の両方に対応
+- 主なクラス：`CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU`
+
+### 4. ビジョンとトラッキング
+- 特徴点ベースのバウンディングボックス追跡と深度検出により認識性能を強化
+- 頑健な視覚ベースのナビゲーションアルゴリズムをサポート
+- 主なクラス：`FeatureBasedBboxTracker`, `DepthDetector`
+
+### 5. ユーティリティ
+- 高効率なマルチスレッド処理を実現するスレッドプール
+- 実行時診断用のロガー
+- 線形状態空間カルマンフィルタによる状態推定
+- `tk` 名前空間で提供されるスプライン補間ユーティリティ
+
+### 6. データ型とパラメータ
+- 経路、軌道、制御、速度、バウンディングボックスを表現する豊富なデータ型
+- 柔軟なパラメータ調整を可能にする強型の設定クラス
+
+### 7. Python バインディング
+- `nanobind` によって構築された包括的な Python バインディングにより、Python ワークフローとシームレスに統合可能
+- マッピング、制御、ビジョン、ユーティリティの主要機能を広くカバー
+
+## `kompass_core` モジュール概要
+
+- `kompass_core.calibration` - ロボット運動モデルのキャリブレーション、フィッティング、ロボットシミュレーション用モジュール
+
+- `kompass_core.control` - 多様な制御戦略と設定を含む。C++ 実装の Python ラッパークラスを提供：
+
+| アルゴリズム名                        | 説明                                       |
+|---------------------------------------|------------------------------------------|
+| **Stanley**                          | 高い収束性能を持つ経路追従                |
+| **DWA（動的ウィンドウ法）**         | 速度空間のサンプリングと最適化             |
+| **DVZ**                              | 可変ゾーンを用いたリアクティブな障害物回避 |
+| **VisionRGBFollower**               | RGB 画像を用いた視覚ターゲット追従         |
+| **VisionRGBDFollower**              | RGBD（深度付き）画像を用いた視覚ターゲット追従 |
+
+- `kompass_core.datatypes` - ロボットやセンサーデータ用の標準メッセージ・データ形式
+
+- `kompass_core.mapping` - 局所マッピングおよび占有グリッド生成。さまざまなレーザーモデルとグリッド解像度設定に対応
+
+- `kompass_core.models` - 差動型、全方向型、アッカーマン型ロボットの運動モデルおよび運動学をサポート。ジオメトリ定義、制御制限、シミュレーション用の状態表現も提供
+
+- `kompass_core.motion_cost` - Python 上で使用可能な軌道評価用コストモデル（衝突確率、リファレンストラッキング、動的・静的障害物対応を含む）
+
+- `kompass_core.performance` - アルゴリズム性能評価用モジュール
+
+- `kompass_core.py_path_tools` - 経路補間と実行ツール
+
+- `kompass_core.simulation` - ロボット運動のシミュレーションと経路の実行可能性評価ツール
+
+- `kompass_core.third_party` - 外部計画ライブラリおよび衝突判定ライブラリとのラッパーと統合：
+
+    - FCL（Flexible Collision Library）
+
+    - OMPL（Open Motion Planning Library）
+
+- `kompass_core.utils` - 汎用ユーティリティ群
+
+## 著作権
+
+本配布物に含まれるコードは、特記がない限り 2024 年 Automatika Robotics に著作権があります。
+Kompass Core は MIT ライセンスのもとで公開されています。詳細は [LICENSE](LICENSE) ファイルをご覧ください。
+
+## コントリビューション
+
+Kompass Core は [Automatika Robotics](https://automatikarobotics.com/) と [Inria](https://inria.fr/) の共同開発プロジェクトです。コミュニティからの貢献は大歓迎です。
+
diff --git a/docs/README.zh.md b/docs/README.zh.md
new file mode 100644
index 00000000..5f69b91d
--- /dev/null
+++ b/docs/README.zh.md
@@ -0,0 +1,156 @@
+# Kompass Core（核心库）
+
+[![English Version][en-badge]][en-url]
+[![ドキュメント-日本語][jp-badge]][jp-url]
+[![PyPI][pypi-badge]][pypi-url]
+[![MIT 许可][mit-badge]][mit-url]
+[![Python 版本][python-badge]][python-url]
+
+[en-badge]: https://img.shields.io/badge/Documentation-English-green.svg
+[en-url]: ../README.md
+[jp-badge]: https://img.shields.io/badge/ドキュメント-日本語-red.svg
+[jp-url]: README.ja.md
+[pypi-badge]: https://img.shields.io/pypi/v/kompass-core.svg
+[pypi-url]: https://pypi.org/project/kompass-core/
+[mit-badge]: https://img.shields.io/pypi/l/kompass-core.svg
+[mit-url]: https://github.com/automatika-robotics/kompass-core/LICENSE
+[python-badge]: https://img.shields.io/pypi/pyversions/kompass-core.svg
+[python-url]: https://www.python.org/downloads/
+
+Kompass Core 是一个高性能、支持 GPU 加速的库，用于机器人导航系统中的运动规划、建图与控制。核心算法采用 C++ 实现，并通过无缝 Python 绑定提供接口。它还集成了第三方库 [OMPL](https://ompl.kavrakilab.org/) 和 [FCL](https://github.com/flexible-collision-library/fcl)。Kompass 的设计哲学是极致高速与高度可靠，通过 GPGPU 支持的并行化算法实现与底层硬件无关。因此，Kompass Core 可运行于多种厂商的 CPU 或 GPU 上，便于机器人硬件厂商在不更换软件栈的情况下切换计算平台。
+
+本软件包可与 [Kompass](https://github.com/automatika-robotics/kompass) 一起使用，用于在 [ROS2](https://docs.ros.org/en/rolling/index.html) 中构建导航栈。详细使用文档请见 [Kompass 文档](https://automatika-robotics.github.io/kompass/)。
+
+- [**安装指南**](#安装指南) Kompass Core 🛠️
+- 查看 [**软件包概览**](#-软件包概览)
+- [**版权信息**](#版权信息) 与 [**贡献说明**](#贡献说明)
+- 若需在 ROS2 中使用 Kompass Core，请访问 [**Kompass 框架**](https://automatika-robotics.github.io/kompass) 🚀
+
+# 安装指南
+
+## 安装支持 GPU 的版本（推荐）
+
+- 要在任何基于 Ubuntu 20+（包括 Jetpack）系统的机器上安装支持 GPU 的 kompass-core，只需运行以下命令：
+
+```bash
+curl https://raw.githubusercontent.com/automatika-robotics/kompass-core/refs/heads/main/build_dependencies/install_gpu.sh | bash
+```
+
+该脚本将安装所有相关的依赖项，包括 [AdaptiveCPP](https://github.com/AdaptiveCpp/AdaptiveCpp)，并从源代码安装最新版的 kompass-core。建议您先阅读该[脚本](https://github.com/automatika-robotics/kompass-core/blob/main/build_dependencies/install_gpu.sh)。
+
+## 使用 pip 安装（仅支持 CPU）
+
+- 在 Ubuntu 22.04 或更高版本上，请先运行以下命令安装依赖项：
+
+```bash
+sudo apt-get install libompl-dev libfcl-dev libpcl-dev
+```
+
+- 然后按如下方式安装 kompass-core：
+```bash
+pip install kompass-core
+```
+
+适用于 linux x86_64 和 aarch64 架构的安装包已发布在 Pypi 上。请注意，Pypi 上提供的版本暂不支持 GPU 加速。
+
+## 安装内容
+
+安装 kompass-core 后，将提供以下三个软件包：
+
+- `kompass_core`：主 Python API，包含用于 2D 空间导航的运动规划与控制的所有封装器与工具。
+- `kompass_cpp`：Kompass 核心 C++ 库的 Python 绑定，包含路径跟踪与运动控制算法的实现。
+- `omplpy`：为 OMPL（开源运动规划库）量身定制的 Python 绑定。
+
+# 📦 软件包概览
+
+本仓库包含以下模块：
+
+- `kompass_cpp/` — 规划、控制、碰撞检测与建图算法的核心 C++ 实现。
+
+- `kompass_core/` — Python 实现与前端配置类，支持高级逻辑开发。
+
+## `kompass_cpp` 模块概览
+
+`kompass_cpp/` 是一个包含建图、控制、轨迹规划与基于视觉的跟踪算法的 C++ 软件包，支持 **GPU 加速**，并通过 `nanobind` 提供 Python 绑定。
+
+### 1. 建图
+- 实现高效的局部建图算法。
+- 支持 **GPU 加速**，实现实时性能。
+- 核心类：`LocalMapper`, `LocalMapperGPU`
+
+### 2. 控制与轨迹规划
+- 支持多种控制策略，包括 PID、Stanley、动态窗口法（DWA）与视觉引导控制器。
+- 支持 **GPU 加速** 的轨迹采样与代价评估，可自定义权重。
+- 核心类：`Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator`
+
+### 3. 碰撞检测与关键区域检测
+- 提供碰撞检测工具与关键区域识别功能，保障导航安全。
+- 提供 CPU 与 GPU 两种实现。
+- 核心类：`CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU`
+
+### 4. 视觉与跟踪
+- 基于特征的边框跟踪与深度检测，增强感知能力。
+- 支持鲁棒的视觉导航算法。
+- 核心类：`FeatureBasedBboxTracker`, `DepthDetector`
+
+### 5. 实用工具
+- 多线程池用于高效的并发处理。
+- 日志工具支持运行时诊断。
+- 基于线性状态空间的卡尔曼滤波器用于状态估计。
+- `tk` 命名空间下的样条插值工具。
+
+### 6. 数据类型与参数
+- 提供丰富的数据结构，用于表示路径、轨迹、控制、速度与边框等。
+- 强类型参数与配置类，支持灵活调参。
+
+### 7. Python 绑定
+- 使用 `nanobind` 构建的全面 Python 绑定，实现与 Python 工作流的无缝集成。
+- 绑定涵盖建图、控制、视觉与工具等核心功能。
+
+## `kompass_core` 模块概览
+
+- `kompass_core.calibration` - 机器人运动模型的校准、拟合与仿真模块。
+
+- `kompass_core.control` - 多种控制策略及配置，包含 C++ 控制器的 Python 包装类：
+
+| 算法名称                             | 描述                                              |
+| ------------------------------------ | ------------------------------------------------- |
+| **Stanley**                          | 具备鲁棒收敛性的路径跟踪                         |
+| **DWA（动态窗口法）**              | 基于速度空间的采样与优化                         |
+| **DVZ**                              | 使用可变形区域的反应式避障                       |
+| **VisionRGBFollower**               | 基于 RGB 图像跟踪视觉目标                        |
+| **VisionRGBDFollower**              | 基于 RGBD 图像（含深度）跟踪视觉目标             |
+
+- `kompass_core.datatypes` - 标准化的机器人与传感器数据格式。
+
+- `kompass_core.mapping` - 局部建图与占据栅格生成，支持多种激光模型与分辨率配置。
+
+- `kompass_core.models` - 机器人模型与运动学，支持差动式、全向轮式与 Ackermann 结构；包含几何定义、控制限制与仿真状态表示。
+
+- `kompass_core.motion_cost` - Python 中用于轨迹评估的代价模型，支持包括碰撞概率、参考轨迹跟踪、动态/静态障碍物等多种代价函数。
+
+- `kompass_core.performance` - 用于算法性能评估的模块。
+
+- `kompass_core.py_path_tools` - 路径插值与执行工具。
+
+- `kompass_core.simulation` - 用于机器人运动仿真与路径可行性评估的工具。
+
+- `kompass_core.third_party` - 与外部规划与碰撞库的封装与集成：
+
+    - FCL（灵活碰撞库）
+
+    - OMPL（开源运动规划库）
+
+- `kompass_core.utils` - 通用工具函数集。
+
+## 版权信息
+
+本发行版中的源代码（除非另有说明）版权归 Automatika Robotics 所有 © 2024。
+
+Kompass Core 遵循 MIT 开源许可证。详细信息请见 [LICENSE](LICENSE) 文件。
+
+## 贡献说明
+
+Kompass Core 由 [Automatika Robotics](https://automatikarobotics.com/) 与 [Inria](https://inria.fr/) 合作开发，欢迎社区开发者贡献代码与文档。
+
+
diff --git a/src/kompass_core/control/__init__.py b/src/kompass_core/control/__init__.py
index 96fea113..0b7f8c10 100644
--- a/src/kompass_core/control/__init__.py
+++ b/src/kompass_core/control/__init__.py
@@ -79,8 +79,8 @@ class ControllersID(StrEnum):
     ControllersID.STANLEY: Stanley,
     ControllersID.DVZ: DVZ,
     ControllersID.DWA: DWA,
-    ControllersID.VISION_IMG : VisionRGBFollower,
-    ControllersID.VISION_DEPTH: VisionRGBDFollower
+    ControllersID.VISION_IMG: VisionRGBFollower,
+    ControllersID.VISION_DEPTH: VisionRGBDFollower,
 }
 
 ControlConfigClasses = {
diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 9283004e..860f6655 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -1,5 +1,5 @@
 import logging
-from typing import List, Optional
+from typing import Optional
 import numpy as np
 from attrs import Factory, define, field
 from ..datatypes.laserscan import LaserScanData
@@ -219,9 +219,7 @@ def __init__(
             config = DWAConfig()
 
         if config_file:
-            config.from_file(
-                file_path=config_file, nested_root_name=config_root_name
-            )
+            config.from_file(file_path=config_file, nested_root_name=config_root_name)
 
         if control_time_step:
             config.control_time_step = control_time_step
diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index 3d0cc556..8fe16251 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -11,6 +11,28 @@
 
 @define
 class VisionRGBFollowerConfig(BaseAttrs):
+    """
+    Configuration class for an RGB-based vision target follower.
+
+    This class defines configuration parameters for controlling a robot that follows a target using RGB vision.
+    It provides settings for control behavior, target tracking, search strategies, velocity and tolerance tuning,
+    and camera-to-robot coordinate transformations.
+
+    Attributes:
+        control_time_step (float): Time interval between control updates (s).
+        control_horizon (int): Number of time steps in the control planning horizon.
+        buffer_size (int): Number of buffered detections to maintain.
+        tolerance (float): Acceptable error when tracking the target.
+        target_distance (Optional[float]): Desired distance to maintain from the target (m).
+        target_wait_timeout (float): Maximum time to wait for a target to reappear if lost (s).
+        target_search_timeout (float): Maximum duration to perform a search when target is lost (s).
+        target_search_pause (float): Delay between successive search attempts (s).
+        target_search_radius (float): Radius used for searching the target (m).
+        rotation_gain (float): Proportional gain for angular control.
+        speed_gain (float): Proportional gain for linear speed control.
+        min_vel (float): Minimum linear velocity allowed during target following (m/s).
+        enable_search (bool): Whether to activate search behavior when the target is lost.
+    """
     control_time_step: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
     )
@@ -68,6 +90,80 @@ def to_kompass_cpp(self) -> RGBFollowerParameters:
 
 
 class VisionRGBFollower(ControllerTemplate):
+    """
+    VisionRGBFollower is a controller for vision-based target following using RGB image data.
+
+    This controller processes 2D object detections (e.g., bounding boxes) and generates velocity commands
+    to follow a visual target using a proportional control law. It supports configuration via Python or external
+    configuration files and allows integration into Kompass-style robotic systems.
+
+    - Usage Example:
+
+    ```python
+    import numpy as np
+    from kompass_core.control import VisionRGBFollower, VisionRGBFollowerConfig
+    from kompass_core.models import (
+        Robot,
+        RobotType,
+        RobotCtrlLimits,
+        LinearCtrlLimits,
+        AngularCtrlLimits,
+        RobotGeometry,
+    )
+    from kompass_core.datatypes import Bbox2D
+
+    # Define robot
+    my_robot = Robot(
+        robot_type=RobotType.ACKERMANN,
+        geometry_type=RobotGeometry.Type.CYLINDER,
+        geometry_params=np.array([0.2, 0.5])
+    )
+
+    # Define control limits
+    ctrl_limits = RobotCtrlLimits(
+        vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=2.0, max_decel=4.0),
+        omega_limits=AngularCtrlLimits(
+            max_vel=2.0, max_acc=3.0, max_decel=3.0, max_steer=np.pi
+        )
+    )
+
+    # Create the controller
+    config = VisionRGBFollowerConfig(
+        target_search_timeout=20.0,
+        speed_gain=0.8,
+        rotation_gain=0.9,
+        enable_search=True,
+    )
+    controller = VisionRGBFollower(robot=my_robot, ctrl_limits=ctrl_limits, config=config)
+
+    # 2D detection
+    detection = Bbox2D(
+                top_left_corner=np.array(
+                    [30, 40], dtype=np.int32
+                ),
+                size=np.array(
+                    [
+                        100,
+                        200,
+                    ],
+                    dtype=np.int32,
+                ),
+                timestamp=0.0,
+                label="person",
+            )
+    detection.set_img_size(np.array([640, 480], dtype=np.int32))
+
+    # Set initial target
+    controller.set_initial_tracking_2d_target(detection)
+
+    # Perform a control loop step
+    success = controller.loop_step(detections_2d=[detection])
+
+    # Access control outputs
+    vx = controller.linear_x_control
+    omega = controller.angular_control
+    ```
+    """
     def __init__(
         self,
         robot: Robot,
@@ -104,15 +200,32 @@ def __init__(
         logging.info("VISION TARGET FOLLOWING CONTROLLER IS READY")
 
     def set_initial_tracking_2d_target(self, target_box: Bbox2D, **_) -> bool:
+        """Sets the initial target for the controller to track
+
+        :param target_box: 2D bounding box of the target
+        :type target_box: Bbox2D
+        :return: True if the target was set successfully, False otherwise
+        :rtype: bool
+        """
         self.__controller.reset_target(target_box)
         return True
 
     @property
     def dist_error(self) -> float:
+        """Getter of the last distance error computed by the controller
+
+        :return: Last distance error (m)
+        :rtype: float
+        """
         return self._planner.get_errors()[0]
 
     @property
     def orientation_error(self) -> float:
+        """Getter of the last orientation error computed by the controller (radians)
+
+        :return: Last orientation error (radians)
+        :rtype: float
+        """
         return self._planner.get_errors()[1]
 
     def loop_step(
@@ -121,7 +234,9 @@ def loop_step(
         detections_2d: Optional[List[Bbox2D]],
         **_,
     ) -> bool:
-        self._found_ctrl = self.__controller.run(detections_2d[0] if detections_2d else None)
+        self._found_ctrl = self.__controller.run(
+            detections_2d[0] if detections_2d else None
+        )
         if self._found_ctrl:
             self._ctrl = self.__controller.get_ctrl()
         return self._found_ctrl
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index 7b4bca25..4fd60a2b 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -24,6 +24,35 @@
 
 @define
 class VisionRGBDFollowerConfig(DWAConfig):
+    """
+    Configuration class for a vision-based RGB-D target follower using Dynamic Window Approach (DWA) planning.
+
+    Attributes:
+        control_time_step (float): Time interval between control updates (s).
+        control_horizon (int): Number of steps in the control horizon.
+        prediction_horizon (int): Number of steps in the prediction horizon.
+        buffer_size (int): Size of the trajectory buffer.
+        target_distance (Optional[float]): Desired distance to maintain from the target (m).
+        target_wait_timeout (float): Time to wait for the target to reappear before giving up (s).
+        target_search_timeout (float): Time limit for the search process if the target is lost (s).
+        target_search_pause (float): Pause between successive search attempts (s).
+        target_search_radius (float): Radius within which to search for the lost target (m).
+        rotation_gain (float): Gain applied to rotational control (unitless).
+        speed_gain (float): Gain applied to speed control (unitless).
+        enable_search (bool): Enables or disables the target search behavior.
+        distance_tolerance (float): Acceptable deviation in target distance (m).
+        angle_tolerance (float): Acceptable deviation in target bearing (rad).
+        target_orientation (float): Desired orientation relative to the target (rad).
+        use_local_coordinates (bool): Whether to use robot-local coordinates for tracking.
+        error_pose (float): Estimated error in pose measurements (m).
+        error_vel (float): Estimated error in velocity measurements (m/s).
+        error_acc (float): Estimated error in acceleration measurements (m/s²).
+        depth_conversion_factor (float): Factor to convert raw depth image values to meters.
+        min_depth (float): Minimum depth value considered valid (m).
+        max_depth (float): Maximum depth value considered valid (m).
+        camera_position_to_robot (np.ndarray): 3D translation vector from the camera frame to the robot base (m).
+        camera_rotation_to_robot (np.ndarray): Quaternion representing camera-to-robot rotation.
+    """
     control_time_step: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
     )
@@ -130,6 +159,88 @@ def to_kompass_cpp(self) -> VisionDWAParameters:
 
 
 class VisionRGBDFollower(ControllerTemplate):
+    """
+    VisionRGBDFollower is a controller for vision-based target tracking using RGB-D (color + depth) image data.
+
+    This controller combines image-based detections (2D bounding boxes) and depth data to estimate 3D positions
+    of visual targets and uses a sampling-based planner (similar to DWA) to compute optimal local motion commands.
+    It integrates camera intrinsics, robot geometry, and multiple sensor modalities (e.g., point clouds, laser scans,
+    local maps) to generate robust and feasible trajectories for following dynamic or static targets in the environment.
+
+    - Usage Example:
+
+    ```python
+    import numpy as np
+    from kompass_core.control import VisionRGBDFollower, VisionRGBDFollowerConfig
+    from kompass_core.models import (
+        Robot,
+        RobotType,
+        RobotCtrlLimits,
+        LinearCtrlLimits,
+        AngularCtrlLimits,
+        RobotGeometry,
+        RobotState,
+    )
+    from kompass_core.datatypes import Bbox2D, LaserScanData
+
+    # Define robot
+    my_robot = Robot(
+        robot_type=RobotType.ACKERMANN,
+        geometry_type=RobotGeometry.Type.CYLINDER,
+        geometry_params=np.array([0.3, 0.6])
+    )
+
+    # Define control limits
+    ctrl_limits = RobotCtrlLimits(
+        vx_limits=LinearCtrlLimits(max_vel=1.5, max_acc=3.0, max_decel=3.0),
+        omega_limits=AngularCtrlLimits(
+            max_vel=2.5, max_acc=2.5, max_decel=2.5, max_steer=np.pi / 2
+        )
+    )
+
+    # Configure controller
+    config = VisionRGBDFollowerConfig(
+        max_linear_samples=15,
+        max_angular_samples=15,
+        control_horizon=10,
+        enable_obstacle_avoidance=True,
+    )
+    controller = VisionRGBDFollower(
+        robot=my_robot,
+        ctrl_limits=ctrl_limits,
+        config=config,
+        camera_focal_length=[525.0, 525.0],
+        camera_principal_point=[319.5, 239.5],
+    )
+
+    # Prepare sensor inputs
+    bbox = Bbox2D(top_left_corner=np.array([200, 150]), size=np.array([50, 100]))
+    bbox.set_img_size(np.array([640, 480]))
+
+    aligned_depth_image = np.random.rand(480, 640).astype(np.int32)  # Fake depth
+    robot_state = RobotState(x=0.0, y=0.0, yaw=0.0, speed=0.0)
+
+    # Initialize target tracking
+    controller.set_initial_tracking_2d_target(
+        current_state=robot_state,
+        target_box=bbox,
+        aligned_depth_image=aligned_depth_image,
+    )
+
+    # Run control loop step
+    success = controller.loop_step(
+        current_state=robot_state,
+        detections_2d=[bbox],
+        depth_image=aligned_depth_image,
+        local_map=np.random.rand(100, 100),  # Fake local map
+        local_map_resolution=0.05
+    )
+
+    # Access control outputs
+    vx_cmd = controller.linear_x_control
+    omega_cmd = controller.angular_control
+    ```
+    """
     def __init__(
         self,
         robot: Robot,
@@ -201,6 +312,17 @@ def __init__(
         logging.info("VisionDWA CONTROLLER IS READY")
 
     def set_camera_intrinsics(self, fx: float, fy: float, cx: float, cy: float) -> None:
+        """Set depth camera intrinsics for the planner
+
+        :param fx: Focal length in x direction
+        :type fx: float
+        :param fy: Focal length in y direction
+        :type fy: float
+        :param cx: Principal point x coordinate
+        :type cx: float
+        :param cy: Principal point y coordinate
+        :type cy: float
+        """
         self._planner.set_camera_intrinsics(fx, fy, cx, cy)
 
     def set_initial_tracking_2d_target(
@@ -218,7 +340,10 @@ def set_initial_tracking_2d_target(
         try:
             if current_state:
                 self._planner.set_current_state(
-                    current_state.x, current_state.y, current_state.yaw, current_state.speed
+                    current_state.x,
+                    current_state.y,
+                    current_state.yaw,
+                    current_state.speed,
                 )
             return self._planner.set_initial_tracking(
                 aligned_depth_image,
@@ -232,10 +357,20 @@ def set_initial_tracking_2d_target(
 
     @property
     def dist_error(self) -> float:
+        """Getter of the last distance error computed by the controller
+
+        :return: Last distance error (m)
+        :rtype: float
+        """
         return self._planner.get_errors()[0]
 
     @property
     def orientation_error(self) -> float:
+        """Getter of the last orientation error computed by the controller (radians)
+
+        :return: Last orientation error (radians)
+        :rtype: float
+        """
         return self._planner.get_errors()[1]
 
     def set_initial_tracking_image(
diff --git a/src/kompass_core/control/stanley.py b/src/kompass_core/control/stanley.py
index 6d6bcbcc..3dd752ad 100644
--- a/src/kompass_core/control/stanley.py
+++ b/src/kompass_core/control/stanley.py
@@ -137,9 +137,7 @@ def __init__(
             config = StanleyConfig(wheel_base=robot.wheelbase)
 
         if config_file:
-            config.from_file(
-                file_path=config_file, nested_root_name=config_root_name
-            )
+            config.from_file(file_path=config_file, nested_root_name=config_root_name)
 
         self._config = config
         self._control_time_step = config.control_time_step

From 7171d68adb00f66f7b1f79d49f2a7c086abf7d40 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 18:09:15 +0200
Subject: [PATCH 097/118] (fix) Updates controllers test

---
 tests/test_controllers.py | 243 ++++++++++++--------------------------
 tests/test_fcl.py         |   2 +-
 tests/test_ompl.py        |   4 +-
 3 files changed, 76 insertions(+), 173 deletions(-)

diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index 55210b4f..a50f2f07 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -13,7 +13,7 @@
 from kompass_cpp.types import PathInterpolationType, Path as PathCpp
 
 from kompass_core.datatypes.laserscan import LaserScanData
-from kompass_core.datatypes import Bbox3D, Bbox2D
+from kompass_core.datatypes import Bbox2D
 from kompass_core.control import (
     DVZ,
     DWAConfig,
@@ -21,8 +21,10 @@
     DWA,
     StanleyConfig,
     Stanley,
-    VisionDWA,
-    VisionDWAConfig,
+    VisionRGBDFollower,
+    VisionRGBDFollowerConfig,
+    VisionRGBFollower,
+    VisionRGBFollowerConfig,
 )
 from kompass_core.models import (
     AngularCtrlLimits,
@@ -431,7 +433,7 @@ def test_dwa(plot: bool = False, figure_name: str = "dwa", figure_tag: str = "dw
 
 
 def test_vision_dwa_with_depth_img():
-    """Run DWA pytest and assert reaching end"""
+    """Run VisionRGBDFollower pytest and assert reaching end"""
     global global_path, my_robot, robot_ctr_limits, control_time_step
 
     from kompass_core import set_logging_level
@@ -462,7 +464,7 @@ def test_vision_dwa_with_depth_img():
         jerk_weight=0.0,
         obstacles_distance_weight=0.0,
     )
-    config = VisionDWAConfig(
+    config = VisionRGBDFollowerConfig(
         control_time_step=control_time_step,
         control_horizon=control_horizon,
         prediction_horizon=prediction_horizon,
@@ -473,7 +475,6 @@ def test_vision_dwa_with_depth_img():
         max_angular_samples=20,
         octree_resolution=0.1,
         max_num_threads=1,
-        min_vel=0.05,
         min_depth=0.001,
         max_depth=5.0,
         depth_conversion_factor=0.001,
@@ -481,7 +482,7 @@ def test_vision_dwa_with_depth_img():
         camera_rotation_to_robot=np.array([0.385, -0.5846, 0.595, -0.395]),
     )
 
-    controller = VisionDWA(
+    controller = VisionRGBDFollower(
         robot=my_robot,
         ctrl_limits=robot_ctr_limits,
         config=config,
@@ -525,149 +526,50 @@ def test_vision_dwa_with_depth_img():
     print(f"Found Control {vx}, {vy}, {omega}")
 
 
-# @pytest.mark.parametrize("use_tracker", [False, True])
-# @pytest.mark.parametrize(
-#     "point_cloud", [[np.array([10.3, 10.5, 0.2])], [np.array([0.3, 0.27, 0.1])]]
-# )
-# def test_vision_dwa(use_tracker: bool, point_cloud: List[np.ndarray]):
-#     """Run DWA pytest and assert reaching end"""
-#     global global_path, my_robot, robot_ctr_limits, control_time_step
-
-#     control_horizon = 2
-#     prediction_horizon = 10
-
-#     # Simulate tracked pose and vel
-#     tracked_vel_lin = 0.1
-#     tracked_vel_ang = 0.3
-#     tracked_pose = TrackedPose2D(0.0, 0.0, 0.0, tracked_vel_lin, 0.0, tracked_vel_ang)
-
-#     # To plot
-#     robot_x = []
-#     robot_y = []
-#     tracked_x = []
-#     tracked_y = []
-#     my_robot.state.x = -0.51731912
-#     my_robot.state.y = 0.0
-#     my_robot.state.yaw = 0.0
-
-#     cost_weights = TrajectoryCostsWeights(
-#         reference_path_distance_weight=1.0,
-#         goal_distance_weight=0.0,
-#         smoothness_weight=0.0,
-#         jerk_weight=0.0,
-#         obstacles_distance_weight=0.0,
-#     )
-#     config = VisionDWAConfig(
-#         control_time_step=control_time_step,
-#         control_horizon=control_horizon,
-#         prediction_horizon=prediction_horizon,
-#         speed_gain=1.0,
-#         rotation_gain=1.0,
-#         costs_weights=cost_weights,
-#         max_linear_samples=20,
-#         max_angular_samples=20,
-#         octree_resolution=0.1,
-#         max_num_threads=1,
-#         min_vel=0.05,
-#     )
-
-#     controller = VisionDWA(robot=my_robot, ctrl_limits=robot_ctr_limits, config=config)
-
-#     steps = 0
-
-#     # Detected Boxes
-#     ref_point_img = [150, 150]
-#     detected_boxes = []
-#     boxes_ori = 0.0
-#     for i in range(3):
-#         new_box = Bbox3D(
-#             center=np.array([0.0, 0.0, 0.0], dtype=np.float32),
-#             size=np.array([0.5, 0.5, 1.0], dtype=np.float32),
-#             center_img_frame=np.array([150, 150], dtype=np.int32),
-#             size_img_frame=np.array([25, 25], dtype=np.int32),
-#         )
-#         new_box_shift = np.array([0.7 * i, 0.7 * i, 0.0], dtype=np.float32)
-#         img_frame_shift = np.array([50 * i, 50 * i], dtype=np.int32)
-#         new_box.center = new_box_shift
-#         new_box.center_img_frame = img_frame_shift + ref_point_img
-#         detected_boxes.append(new_box)
-
-#     def moveBoxes(boxes_orientation, boxes) -> float:
-#         """Move boxes in the direction of the tracked pose"""
-#         for box in boxes:
-#             box.center[0] += (
-#                 tracked_vel_lin * np.cos(boxes_orientation) * control_time_step
-#             )
-#             box.center[1] += (
-#                 tracked_vel_lin * np.sin(boxes_orientation) * control_time_step
-#             )
-#         boxes_orientation += tracked_vel_ang * control_time_step
-#         return boxes_orientation
-
-#     if use_tracker:
-#         controller.set_initial_tracking_3d(150, 150, detected_boxes)
-
-#     while steps < 100:
-#         robot_x.append(my_robot.state.x)
-#         robot_y.append(my_robot.state.y)
-#         tracked_x.append(tracked_pose.x())
-#         tracked_y.append(tracked_pose.y())
-
-#         if use_tracker:
-#             res = controller.loop_step(
-#                 current_state=my_robot.state,
-#                 detections_3d=detected_boxes,
-#                 point_cloud=point_cloud,
-#             )
-#         else:
-#             res = controller.loop_step(
-#                 current_state=my_robot.state,
-#                 tracked_pose=tracked_pose,
-#                 point_cloud=point_cloud,
-#             )
-#         if not res:
-#             print("No control found")
-#             break
-#         # print(f"Found trajectory with cost {controller.result_cost}")
-#         velocities = controller.control_till_horizon
-#         (vx, vy, omega) = velocities.vx[0], velocities.vy[0], velocities.omega[0]
-#         print(f"Found Control {vx}, {vy}, {omega}")
-#         my_robot.set_control(
-#             velocity_x=vx,
-#             velocity_y=vy,
-#             omega=omega,
-#         )
-#         my_robot.get_state(dt=control_time_step)
-#         tracked_pose.update(control_time_step)
-#         if use_tracker:
-#             boxes_ori = moveBoxes(boxes_ori, detected_boxes)
-#         steps += 1
-
-#     figure_name = "vision_dwa"
-#     if use_tracker:
-#         figure_name += "_with_tracker"
-#     else:
-#         figure_name += "_no_tracker"
-
-#     figure_name += f"{point_cloud[0]}"
-#     plt.figure()
-#     plt.plot(robot_x, robot_y, marker="o", linestyle="-", color="b", label="Robot Path")
-#     plt.plot(
-#         tracked_x,
-#         tracked_y,
-#         label="Tracked Object",
-#         marker="*",
-#         linestyle="-",
-#         color="g",
-#     )
-#     plt.xlabel("X")
-#     plt.ylabel("Y")
-#     plt.title(figure_name)
-#     plt.grid(True)
-#     plt.legend()
-#     plt.savefig(f"logs/{figure_name}.png")
-
-#     assert steps == 100
+def test_vision_rgb_follower():
+    """Run VisionRGBFollower pytest and assert reaching end"""
+    global global_path, my_robot, robot_ctr_limits, control_time_step
+
+    from kompass_core import set_logging_level
+
+    set_logging_level("DEBUG")
+
+    my_robot.state.x = -0.5
+
+    box = Bbox2D(top_left_corner=np.array([410, 0]), size=np.array([410, 390]))
+    box.set_img_size(np.array([640, 480], dtype=np.int32))
+    detections = [box]
+
+    config = VisionRGBFollowerConfig(
+        control_time_step=control_time_step,
+        speed_gain=1.0,
+        rotation_gain=1.0
+    )
+
+    controller = VisionRGBFollower(
+        robot=my_robot,
+        ctrl_limits=robot_ctr_limits,
+        config=config,
+    )
+
+    found_target = controller.set_initial_tracking_2d_target(box)
+
+    if not found_target:
+        print("Point not found on image")
+        return
+    else:
+        print("Point found on image ...")
+
+    res = controller.loop_step(
+        detections_2d=detections,
+    )
+    if not res:
+        print("No control found")
+
+    assert res
+
+    (vx, vy, omega) = controller.linear_x_control, controller.linear_y_control, controller.angular_control
+    print(f"Found Control {vx}, {vy}, {omega}")
 
 
 def test_dwa_debug():
@@ -788,32 +690,35 @@ def main():
 
     control_time_step = 0.1
 
-    # print("RUNNING PATH INTERPOLATION TEST")
-    # test_path_interpolation(plot=True)
+    print("RUNNING PATH INTERPOLATION TEST")
+    test_path_interpolation(plot=True)
 
-    # ## TESTING STANLEY ##
-    # print("RUNNING STANLEY CONTROLLER TEST")
-    # test_stanley(
-    #     plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
-    # )
+    ## TESTING STANLEY ##
+    print("RUNNING STANLEY CONTROLLER TEST")
+    test_stanley(
+        plot=True, figure_name="stanley", figure_tag="Stanley Controller Test Results"
+    )
 
-    # ## TESTING DVZ ##
-    # print("RUNNING DVZ CONTROLLER TEST")
-    # test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
+    ## TESTING DVZ ##
+    print("RUNNING DVZ CONTROLLER TEST")
+    test_dvz(plot=True, figure_name="dvz", figure_tag="DVZ Controller Test Results")
 
-    # ## TESTING DWA DEBUG MODE ##
-    # print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
-    # test_dwa_debug()
+    ## TESTING DWA DEBUG MODE ##
+    print("RUNNING ONE DWA CONTROLLER DEBUG STEP TEST")
+    test_dwa_debug()
 
-    # ## TESTING DWA ##
-    # print("RUNNING DWA CONTROLLER TEST")
-    # test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
+    ## TESTING DWA ##
+    print("RUNNING DWA CONTROLLER TEST")
+    test_dwa(plot=True, figure_name="dwa", figure_tag="DWA Controller Test Results")
 
-    # ## TESTING VISION DWA ##
-    # print("RUNNING VISION DWA CONTROLLER TEST")
-    # test_vision_dwa(use_tracker=True, point_cloud=[np.array([0.3, 0.27, 0.1])])
+    ## TESTING VISION RGBD Follower (VISION DWA) ##
+    print("RUNNING VISION DWA CONTROLLER TEST")
     test_vision_dwa_with_depth_img()
 
+    ## TESTING VISION RGB Follower ##
+    print("RUNNING VISION RGB FOLLOWER TEST")
+    test_vision_rgb_follower()
+
 
 if __name__ == "__main__":
     main()
diff --git a/tests/test_fcl.py b/tests/test_fcl.py
index eeaeab95..d27885c4 100644
--- a/tests/test_fcl.py
+++ b/tests/test_fcl.py
@@ -344,7 +344,7 @@ def test_fcl(save_results: bool = False):
             "collision_manager_1_data": rdata.result.is_collision,
         }
         results["pointcloud_with_manager"] = res_dict
-        os.makedirs('logs', exist_ok=True)
+        os.makedirs("logs", exist_ok=True)
         with open("logs/fcl_result.json", "w") as f:
             json.dump(results, f)
 
diff --git a/tests/test_ompl.py b/tests/test_ompl.py
index a51a995f..76f657cf 100644
--- a/tests/test_ompl.py
+++ b/tests/test_ompl.py
@@ -41,9 +41,7 @@ def generate_all_geometric_planners_configs():
         print("-----------------------------\n")
 
 
-def ompl_solve_once(
-    ompl_planner: OMPLGeometric, map_data: Dict, map_numpy: np.ndarray
-):
+def ompl_solve_once(ompl_planner: OMPLGeometric, map_data: Dict, map_numpy: np.ndarray):
     """
     Setup and solve OMPL planning problem with given map and map metadata
 

From 94454d621e824cda78abb06e045cf74924b3af08 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 18:50:06 +0200
Subject: [PATCH 098/118] (fix) Removes Matplotlib and Pandas from package
 dependencies

---
 src/kompass_core/algorithms/dvz.py            |  9 ++-
 src/kompass_core/calibration.py               | 15 ++++-
 src/kompass_core/datatypes/path.py            | 14 ++++-
 src/kompass_core/performance.py               | 14 ++++-
 .../py_path_tools/interpolation.py            | 15 ++++-
 src/kompass_core/utils/visualization.py       | 46 +++++++++++---
 .../include/controllers/rgb_follower.h        |  1 -
 tests/test_controllers.py                     | 23 ++++++-
 tests/test_laserscan_emergency_stop.py        | 45 ++++++++------
 tests/test_ompl.py                            | 61 +++++++++++--------
 10 files changed, 182 insertions(+), 61 deletions(-)

diff --git a/src/kompass_core/algorithms/dvz.py b/src/kompass_core/algorithms/dvz.py
index a06cc37e..1d490142 100644
--- a/src/kompass_core/algorithms/dvz.py
+++ b/src/kompass_core/algorithms/dvz.py
@@ -3,10 +3,8 @@
 
 from ..utils.common import base_validators, BaseAttrs
 from ..utils.geometry import convert_to_0_2pi, convert_to_plus_minus_pi
-import matplotlib.pyplot as plt
 import numpy as np
 from attrs import define, field
-from matplotlib.patches import Ellipse, Polygon
 
 from ..models import Robot, RobotState, RobotCtrlLimits
 
@@ -512,6 +510,13 @@ def plt_robot_zone(
         :param display_now: Display the figure, defaults to False
         :type display_now: bool, optional
         """
+        try:
+            import matplotlib.pyplot as plt
+            from matplotlib.patches import Ellipse, Polygon
+        except ImportError as e:
+            raise ImportError(
+                "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'."
+            ) from e
         # Create new figure if none is given
         if not fig_ax:
             fig_ax = plt.gca()
diff --git a/src/kompass_core/calibration.py b/src/kompass_core/calibration.py
index 153d65fd..0f97020b 100644
--- a/src/kompass_core/calibration.py
+++ b/src/kompass_core/calibration.py
@@ -1,10 +1,8 @@
 import logging
 
 from .utils.geometry import convert_to_plus_minus_pi
-import matplotlib.pyplot as plt
 import numpy as np
 from .datatypes.path import MotionSample, PathSample
-from scipy import optimize
 
 from .models import MotionModel2D, Robot, RobotState
 from .simulation import RobotSim
@@ -63,6 +61,12 @@ def fit_data(self, log: bool = False) -> MotionModel2D:
         :return: Calibrated robot model
         :rtype: MotionModel2D
         """
+        try:
+            from scipy import optimize
+        except ImportError as e:
+            raise ImportError(
+                "SciPy is required for optimization. Please install it using 'pip install scipy'."
+            ) from e
         motion_model = self.robot.state.model
         vx_opt, vx_cov = optimize.curve_fit(
             MotionModel2D.x_model,
@@ -181,6 +185,13 @@ def vis_calibration(cls, robot_data: MotionSample, modeled_path: PathSample):
         :param modeled_path: Motion model output data
         :type modeled_path: PathSample
         """
+        try:
+            import matplotlib.pyplot as plt
+        except ImportError:
+            logging.error(
+                "Matplotlib is not installed. Please install it to visualize the calibration results."
+            )
+            return
         fig, [ax0, ax1, ax2, ax3] = plt.subplots(nrows=4, ncols=1, figsize=(8, 8))
         fig.suptitle("Calibration Results")
         fig.tight_layout(pad=3.0)
diff --git a/src/kompass_core/datatypes/path.py b/src/kompass_core/datatypes/path.py
index 93952098..56e1aa1a 100644
--- a/src/kompass_core/datatypes/path.py
+++ b/src/kompass_core/datatypes/path.py
@@ -3,7 +3,6 @@
 from typing import List, Union
 
 import numpy as np
-import pandas as pd
 from attrs import define, field
 
 
@@ -428,7 +427,12 @@ def save_to_csv(self, file_location: str, file_name: str) -> bool:
         """
         try:
             csv_mapping = self._csv_mapping()
-
+            try:
+                import pandas as pd
+            except ImportError as e:
+                raise ImportError(
+                    "Pandas is required for data handling. Please install it using 'pip install pandas'."
+                ) from e
             # Create a DataFrame using pandas
             motion_df = pd.DataFrame(csv_mapping)
             # Check if the directory exists, if not, create it
@@ -467,6 +471,12 @@ def get_from_csv(self, file_location: str, file_name: str) -> bool:
         :return: Motion sample loaded from file
         :rtype: bool
         """
+        try:
+            import pandas as pd
+        except ImportError as e:
+            raise ImportError(
+                "Pandas is required for data handling. Please install it using 'pip install pandas'."
+            ) from e
         if os.path.exists(file_location):
             _, extension = os.path.splitext(file_name)
 
diff --git a/src/kompass_core/performance.py b/src/kompass_core/performance.py
index 06b4b6f0..05f9c56b 100644
--- a/src/kompass_core/performance.py
+++ b/src/kompass_core/performance.py
@@ -1,8 +1,6 @@
 from typing import Any, List
 
-import matplotlib.pyplot as plt
 import numpy as np
-import pandas as pd
 from .datatypes.obstacles import ObstaclesData
 from .datatypes.path import (
     PathPoint,
@@ -44,6 +42,12 @@ def vis_result(
         :param figure_title: Title of the generated figure, defaults to 'Figure 0'
         :type figure_title: str, optional
         """
+        try:
+            import matplotlib.pyplot as plt
+        except ImportError as e:
+            raise ImportError(
+                "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'."
+            ) from e
         _fig_margin = 0.5
 
         fig, [ax0, ax1, ax2, ax3] = plt.subplots(nrows=4, ncols=1, figsize=(8, 8))
@@ -130,6 +134,12 @@ def reset(self):
 
 class TestAvgResults:
     def __init__(self) -> None:
+        try:
+            import pandas as pd
+        except ImportError as e:
+            raise ImportError(
+                "Pandas is required for data handling. Please install it using 'pip install pandas'."
+            ) from e
         columns = [
             "test_id",
             "test_type",
diff --git a/src/kompass_core/py_path_tools/interpolation.py b/src/kompass_core/py_path_tools/interpolation.py
index 50c585a4..c332a31e 100644
--- a/src/kompass_core/py_path_tools/interpolation.py
+++ b/src/kompass_core/py_path_tools/interpolation.py
@@ -5,11 +5,17 @@
 from ..utils import geometry as GeometryUtils
 import numpy as np
 from ..datatypes.path import InterpolationPoint, Point2D, Range2D, TrackedPoint
-from scipy.interpolate import CubicSpline
 
 
 class Spline:
     def __init__(self):
+        try:
+            from scipy.interpolate import CubicSpline
+        except ImportError as e:
+            raise ImportError(
+                "CubicSpline from scipy is required for spline interpolation. "
+                "Please install it using 'pip install scipy'."
+            ) from e
         self.x_points: np.ndarray = np.array([])
         self.y_points: np.ndarray = np.array([])
         self.func: Optional[CubicSpline] = None
@@ -23,6 +29,13 @@ def set_points(self, x_points: np.ndarray, y_points: np.ndarray) -> None:
         :param y_points: Y coordinates of the spline points
         :type y_points: np.ndarray
         """
+        try:
+            from scipy.interpolate import CubicSpline
+        except ImportError as e:
+            raise ImportError(
+                "CubicSpline from scipy is required for spline interpolation. "
+                "Please install it using 'pip install scipy'."
+            ) from e
         # arrange x points in increasing order
         sorted_indices = np.argsort(x_points)
         self.x_points = np.array(x_points[sorted_indices])
diff --git a/src/kompass_core/utils/visualization.py b/src/kompass_core/utils/visualization.py
index 11ee327c..ddaa9a2e 100644
--- a/src/kompass_core/utils/visualization.py
+++ b/src/kompass_core/utils/visualization.py
@@ -1,16 +1,11 @@
 from typing import List, Optional
 import numpy as np
-import cv2
-import matplotlib.colors as PltColors
-import matplotlib.markers as PltMarkers
-import matplotlib.pyplot as plt
-from matplotlib.axes import Axes
 
 from ..datatypes.obstacles import ObstaclesData, OCCUPANCY_TYPE
 from ..datatypes.path import PathPoint, PathSample
 
 
-def plt_map_obstacles(map: ObstaclesData, ax: Axes = None):
+def plt_map_obstacles(map: ObstaclesData, ax=None):
     """
     Plot a given map's obstacles as circles
 
@@ -19,6 +14,12 @@ def plt_map_obstacles(map: ObstaclesData, ax: Axes = None):
     :param ax: Plotting Axes, defaults to None
     :type ax: Axes, optional
     """
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError as e:
+        raise ImportError(
+            "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'."
+        ) from e
     if not ax:
         ax = plt.gca()
 
@@ -29,7 +30,7 @@ def plt_map_obstacles(map: ObstaclesData, ax: Axes = None):
 
 
 def plt_path_sample(
-    ref_path: PathSample, label="", color="blue", marker="", ax: Axes = None
+    ref_path: PathSample, label="", color="blue", marker="", ax=None
 ):
     """
     Plot a given path sample as a line
@@ -45,6 +46,14 @@ def plt_path_sample(
     :param ax: Plotting Axes, defaults to None
     :type ax: Axes, optional
     """
+    try:
+        import matplotlib.colors as PltColors
+        import matplotlib.markers as PltMarkers
+        import matplotlib.pyplot as plt
+    except ImportError as e:
+        raise ImportError(
+            "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'."
+        ) from e
     if color not in PltColors.cnames.keys():
         color = "blue"
 
@@ -64,7 +73,7 @@ def plt_path_sample(
 
 
 def plt_path_points_list(
-    ref_path: List[PathPoint], label="", color="blue", marker="", ax: Axes = None
+    ref_path: List[PathPoint], label="", color="blue", marker="", ax=None
 ):
     """
     Plot a given list of path points as a line
@@ -80,6 +89,14 @@ def plt_path_points_list(
     :param ax: Plotting Axes, defaults to None
     :type ax: Axes, optional
     """
+    try:
+        import matplotlib.colors as PltColors
+        import matplotlib.markers as PltMarkers
+        import matplotlib.pyplot as plt
+    except ImportError as e:
+        raise ImportError(
+            "Matplotlib is required for visualization. Please install it using 'pip install matplotlib'."
+        ) from e
     if color not in PltColors.cnames.keys():
         color = "blue"
 
@@ -110,7 +127,12 @@ def _resize_image(image: np.ndarray, scale: float) -> np.ndarray:
     :return:    _description_
     :rtype:     np.ndarray
     """
-
+    try:
+        import cv2
+    except ImportError as e:
+        raise ImportError(
+            "OpenCV is required for image processing. Please install it using 'pip install opencv-python'."
+        ) from e
     width = int(image.shape[1] * scale)
     height = int(image.shape[0] * scale)
     dim = (width, height)
@@ -197,6 +219,12 @@ def visualize_grid(
     :return:    grid image
     :rtype:     np.ndarray
     """
+    try:
+        import cv2
+    except ImportError as e:
+        raise ImportError(
+            "OpenCV is required for image processing. Please install it using 'pip install opencv-python'."
+        ) from e
     grid_image = MAPPING_GRID_TO_COLOR[
         grid_data
     ]  # map HxWx1 grid to HxWx3 RGB colored grid as image
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
index 87867fc4..6eb14930 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/rgb_follower.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include "controller.h"
 #include "datatypes/control.h"
 #include "datatypes/parameter.h"
 #include "datatypes/tracking.h"
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index a50f2f07..d84339a7 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -4,7 +4,6 @@
 import cv2
 from typing import Union, List
 
-import matplotlib.pyplot as plt
 import numpy as np
 import pytest
 from attrs import define, field, Factory
@@ -92,6 +91,13 @@ def plot_path(
     figure_tag: str,
 ):
     """Plot Test Results"""
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError:
+        logger.warning(
+            "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'."
+        )
+        return
     # Extract x and y coordinates from the Path message
     x_coords = [pose.pose.position.x for pose in path.poses]
     y_coords = [pose.pose.position.y for pose in path.poses]
@@ -287,6 +293,13 @@ def test_path_interpolation(plot: bool = False):
         x_inter_cub = cubic_spline_interpolation.x()
         y_inter_cub = cubic_spline_interpolation.y()
 
+        try:
+            import matplotlib.pyplot as plt
+        except ImportError:
+            logger.warning(
+                "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'."
+            )
+            return
         # Plot the path
         plt.figure()
         plt.plot(
@@ -620,6 +633,14 @@ def test_dwa_debug():
 
     dwa.planner.debug_velocity_search(current_velocity, sensor_data, False)
     (paths_x, paths_y) = dwa.planner.get_debugging_samples()
+
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError:
+        logger.warning(
+            "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'."
+        )
+        return
     _, ax = plt.subplots()
     # Add the two laserscan obstacles
     obstacle_1 = plt.Circle((-0.117319, 0), 0.1, color="black", label="obstacle")
diff --git a/tests/test_laserscan_emergency_stop.py b/tests/test_laserscan_emergency_stop.py
index 496ff871..f66ee913 100644
--- a/tests/test_laserscan_emergency_stop.py
+++ b/tests/test_laserscan_emergency_stop.py
@@ -3,7 +3,6 @@
 import numpy as np
 import logging
 import pytest
-import matplotlib.pyplot as plt
 from kompass_core.datatypes import LaserScanData
 from kompass_core.utils.geometry import get_laserscan_transformed_polar_coordinates
 from kompass_core.utils.emergency_stop import EmergencyChecker
@@ -59,20 +58,25 @@ def test_laserscan_polar_tf(laser_scan_data: LaserScanData, plot: bool = False):
         translation=translation,
         rotation=rotation,
     )
-
     if plot:
-        fig, ax = plt.subplots(subplot_kw={"projection": "polar"})
-        ax.plot(
-            laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan"
-        )
-        ax.plot(
-            transformed_scan.angles,
-            transformed_scan.ranges,
-            label="Transformed LaserScan",
-        )
-        fig.legend()
-        dir_name = os.path.dirname(os.path.abspath(__file__))
-        plt.savefig(os.path.join(dir_name, "laserscan_tf_test.png"))
+        try:
+            import matplotlib.pyplot as plt
+            fig, ax = plt.subplots(subplot_kw={"projection": "polar"})
+            ax.plot(
+                laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan"
+            )
+            ax.plot(
+                transformed_scan.angles,
+                transformed_scan.ranges,
+                label="Transformed LaserScan",
+            )
+            fig.legend()
+            dir_name = os.path.dirname(os.path.abspath(__file__))
+            plt.savefig(os.path.join(dir_name, "laserscan_tf_test.png"))
+        except ImportError:
+            logging.warning(
+                "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'."
+            )
 
     old_range = laser_scan_data.ranges[
         laser_scan_data.angles == laser_scan_data.angle_min
@@ -107,7 +111,17 @@ def test_laserscan_partial_data(laser_scan_data: LaserScanData, plot: bool = Fal
         right_angle=right_angle, left_angle=left_angle
     )
 
+    assert len(partial_angles) <= laser_scan_data.angles.size
+    assert len(partial_ranges) <= laser_scan_data.ranges.size
+
     if plot:
+        try:
+            import matplotlib.pyplot as plt
+        except ImportError:
+            logging.warning(
+                "Matplotlib is required for visualization. Figures will not be generated. To generate test figures, install it using 'pip install matplotlib'."
+            )
+            return
         fig, ax = plt.subplots(subplot_kw={"projection": "polar"})
         ax.plot(
             laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan"
@@ -117,9 +131,6 @@ def test_laserscan_partial_data(laser_scan_data: LaserScanData, plot: bool = Fal
         dir_name = os.path.dirname(os.path.abspath(__file__))
         plt.savefig(os.path.join(dir_name, "laserscan_partial_test.png"))
 
-    assert len(partial_angles) <= laser_scan_data.angles.size
-    assert len(partial_ranges) <= laser_scan_data.ranges.size
-
 
 @pytest.mark.parametrize("use_gpu", [False, True])
 def test_emergency_stop(laser_scan_data: LaserScanData, use_gpu):
diff --git a/tests/test_ompl.py b/tests/test_ompl.py
index 76f657cf..48d91395 100644
--- a/tests/test_ompl.py
+++ b/tests/test_ompl.py
@@ -4,7 +4,6 @@
 import timeit
 from typing import Dict
 import numpy as np
-import pandas as pd
 
 import omplpy as ompl
 from kompass_core.models import Robot, RobotGeometry, RobotType
@@ -19,10 +18,6 @@
 SOLUTION_TOLERANCE_TIME = 0.3
 SOLUTION_TOLERANCE_LENGTH = 0.1
 
-REF_RESULTS = pd.read_csv(
-    os.path.join(ompl_resources, "test_results_geometric_ref.csv")
-)
-
 
 def generate_all_geometric_planners_configs():
     """
@@ -120,16 +115,23 @@ def ompl_geometric_testing(test_repetitions: int = 1):
     :param test_repetitions: _description_, defaults to 7
     :type test_repetitions: int, optional
     """
-    ompl_df = pd.DataFrame(
-        columns=(
-            "method",
-            "solved",
-            "solution_time",
-            "solution_len",
-            "simplification_time",
-            "time_convert_2_ros",
+    try:
+        import pandas as pd
+        ompl_df = pd.DataFrame(
+            columns=(
+                "method",
+                "solved",
+                "solution_time",
+                "solution_len",
+                "simplification_time",
+                "time_convert_2_ros",
+            )
         )
-    )
+    except Exception:
+        logger.error(
+            "Pandas is not installed. Result CSV will not be generated. Please install pandas if you wish to generate a CSV of the results."
+        )
+        ompl_df = None
 
     robot = Robot(
         robot_type=RobotType.ACKERMANN,
@@ -189,20 +191,31 @@ def ompl_geometric_testing(test_repetitions: int = 1):
             except Exception as e:
                 logging.error(f"{e}")
 
-        ompl_df.loc[len(ompl_df)] = {
-            "method": planner_id,
-            "solved": solved,
-            "solution_time": solution_time / test_repetitions,
-            "solution_len": sol_len / test_repetitions,
-            "simplification_time": simplify_time / test_repetitions,
-        }
-
-    os.makedirs("logs", exist_ok=True)
-    ompl_df.to_csv("logs/ompl_test_results.csv", index=False)
+        if ompl_df is not None:
+            ompl_df.loc[len(ompl_df)] = {
+                "method": planner_id,
+                "solved": solved,
+                "solution_time": solution_time / test_repetitions,
+                "solution_len": sol_len / test_repetitions,
+                "simplification_time": simplify_time / test_repetitions,
+            }
+    if ompl_df is not None:
+        os.makedirs("logs", exist_ok=True)
+        ompl_df.to_csv("logs/ompl_test_results.csv", index=False)
     return ompl_df
 
 
 def ompl_test_all():
+    try:
+        import pandas as pd
+    except ImportError:
+        logger.error(
+            "Pandas is not installed. Result CSV will not be generated. lease install it to run OMPL test."
+        )
+
+    REF_RESULTS = pd.read_csv(
+        os.path.join(ompl_resources, "test_results_geometric_ref.csv")
+    )
     logging.info("Running all OMPL planners tests")
     results_df = ompl_geometric_testing(test_repetitions=20)
     logging.info("Done all OMPL planners tests")

From 42cc839ee1350770b16a1ebf11ba9fca942d7e13 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 18:50:31 +0200
Subject: [PATCH 099/118] (fix) removes scipy from package dependencies

---
 pyproject.toml | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 2db932e2..3ef82d07 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -26,9 +26,6 @@ dependencies = [
     "toml",
     "attrs>=23.2.0",
     "pyyaml",
-    "scipy",
-    "matplotlib",
-    "pandas",
 ]
 
 requires-python = ">=3.8.4,<3.13"

From b6dab8266933333f8852dd6b2f3f56185b689798 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Mon, 23 Jun 2025 18:50:59 +0200
Subject: [PATCH 100/118] (fix) Removes unused import

---
 src/kompass_core/models.py | 32 ++++++++++++++++++++++----------
 1 file changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/kompass_core/models.py b/src/kompass_core/models.py
index d1c21d3b..44c61465 100644
--- a/src/kompass_core/models.py
+++ b/src/kompass_core/models.py
@@ -1,7 +1,6 @@
 from enum import Enum
 from typing import List, Optional, Union
 
-from .utils import common as CommonUtils
 from .utils.common import BaseAttrs, base_validators, set_params_from_yaml
 from .utils import geometry as GeometryUtils
 
@@ -9,9 +8,6 @@
 from attrs import Factory, define, field, validators
 from .datatypes.path import Point2D
 
-import matplotlib.pyplot as plt
-from matplotlib.patches import Circle, Rectangle
-
 import kompass_cpp
 
 
@@ -490,6 +486,14 @@ def plt_robot(
         :param ax: Plot figure axis, defaults to None
         :type ax: _type_, optional
         """
+        try:
+            import matplotlib.pyplot as plt
+            from matplotlib.patches import Circle
+        except ImportError as e:
+            raise ImportError(
+                "Matplotlib is required for plotting robot footprints. "
+                "Please install it using 'pip install matplotlib'."
+            ) from e
         if not ax:
             ax = plt.gca()
 
@@ -538,7 +542,7 @@ class RectangleFootprint:
 
     def __init__(self, width=1.0, length=2.0):
         """
-        Inits a regtangular robot footprint
+        Inits a rectangular robot footprint
 
         :param width: Robot width (m), defaults to 2.0
         :type width: float, optional
@@ -582,6 +586,14 @@ def plt_robot(
         :param ax: Plot figure axis, defaults to None
         :type ax: _type_, optional
         """
+        try:
+            import matplotlib.pyplot as plt
+            from matplotlib.patches import Circle, Rectangle
+        except ImportError as e:
+            raise ImportError(
+                "Matplotlib is required for plotting robot footprints. "
+                "Please install it using 'pip install matplotlib'."
+            ) from e
         if not ax:
             ax = plt.gca()
 
@@ -788,7 +800,7 @@ def get_radius(cls, geometry_type: Type, parameters: np.ndarray) -> float:
             cls.Type.SPHERE,
             cls.Type.CAPSULE,
         ]:
-            # First parameter is the radius -> equivilant to wheelbase
+            # First parameter is the radius -> equivalent to wheelbase
             return parameters[0]
         else:
             return np.sqrt(parameters[1] + parameters[0]) / 2
@@ -831,7 +843,7 @@ def get_footprint(
             cls.Type.SPHERE,
             cls.Type.CAPSULE,
         ]:
-            # First parameter is the radius -> equivilant to wheelbase
+            # First parameter is the radius -> equivalent to wheelbase
             return CircularFootprint(rad=parameters[0])
         else:
             # Wheelbase is the distance on robot lateral axis (y-axis)
@@ -1191,7 +1203,7 @@ class RobotCtrlLimits(BaseAttrs):
 
     def to_kompass_cpp_lib(self) -> kompass_cpp.control.ControlLimitsParams:
         """
-        Get the control limits parameters transferred to Robotctrl library format
+        Get the control limits parameters transferred to Kompass_cpp library format
 
         :return: 2D control limits
         :rtype: kompass_cpp.control.ctr_limits_params
@@ -1206,7 +1218,7 @@ def linear_to_kompass_cpp_lib(
         self, linear_limits: LinearCtrlLimits
     ) -> kompass_cpp.control.LinearVelocityControlParams:
         """
-        Get linear velocity control limits parameters transfered to Robotctrl library format
+        Get linear velocity control limits parameters transferred to Kompass_cpp library format
 
         :return: Linear forward velocity Vx parameters
         :rtype: kompass_cpp.control.linear_vel_x_params
@@ -1221,7 +1233,7 @@ def angular_to_kompass_cpp_lib(
         self,
     ) -> kompass_cpp.control.AngularVelocityControlParams:
         """
-        Get Omega control limits parameters transfered to Robotctrl library format
+        Get Omega control limits parameters transferred to Kompass_cpp library format
 
         :return: Angular velocity Omega parameters
         :rtype: kompass_cpp.control.angular_vel_params

From 11b2370b07b71fac40200eccb58c13538c638039 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 24 Jun 2025 13:48:36 +0200
Subject: [PATCH 101/118] (docs) Updates README

---
 README.md | 90 ++++++++++++++-----------------------------------------
 1 file changed, 23 insertions(+), 67 deletions(-)

diff --git a/README.md b/README.md
index f63ed3e8..cc816345 100644
--- a/README.md
+++ b/README.md
@@ -60,64 +60,20 @@ Wheels are available on Pypi for linux x86_64 and aarch64 architectures. Please
 
 The following three packages will become available once kompass-core is installed.
 
-- `kompass_core`: The main Python API containing all the wrappers and utilities for motion planning and control for navigation in 2D spaces.
-- `kompass_cpp`: Python bindings for Kompass core C++ library containing the algorithms implementation for path tracking and motion control.
+- `kompass_cpp`: Core C++ library for control, collision checking, and mapping algorithms.
+- `kompass_core`: Python bindings for Kompass core C++ library with front-end classes for configuration and high-level logic.
 - `omplpy`: Bespoke python bindings for the Open Motion Planning Library (OMPL).
 
 
 # 📦 Package Overview
 
-This repository consists the following modules:
+The package includes modules for mapping, control, trajectory planning, and vision-based tracking algorithms, with **GPU acceleration** support and Python bindings via `nanobind`.
 
-- `kompass_cpp/` — Core C++ implementation of planning, control, collision checking, and mapping algorithms.
 
-- `kompass_core/` — Python implementations and front-end classes for configuration and high-level logic.
-
-## `kompass_cpp` Overview
-
-`kompass_cpp/` contains the C++ package which includes mapping, control, trajectory planning, and vision-based tracking algorithms, with **GPU acceleration** support and Python bindings via `nanobind`.
-
-### 1. Mapping
-- Implements efficient local mapping algorithms.
-- Supports **GPU-accelerated** mapping for real-time performance.
-- Core classes: `LocalMapper`, `LocalMapperGPU`
-
-### 2. Control & Trajectory Planning
-- Includes multiple control strategies such as PID, Stanley, Dynamic Window Approach (DWA), and vision-guided controllers.
-- Supports **GPU-accelerated** trajectory sampling and cost evaluation with customizable weights.
-- Core classes: `Controller`, `PID`, `Stanley`, `DWA`, `VisionDWA`, `TrajectorySampler`, `CostEvaluator`
-
-### 3. Collision and Critical Zone Checking
-- Provides collision checking utilities and critical zone detection to ensure safe navigation.
-- Includes both CPU and GPU implementations.
-- Core classes: `CollisionChecker`, `CriticalZoneChecker`, `CriticalZoneCheckerGPU`
-
-### 4. Vision and Tracking
-- Feature-based bounding box tracking and depth detection for enhanced perception.
-- Supports robust vision-based navigation algorithms.
-- Core classes: `FeatureBasedBboxTracker`, `DepthDetector`
-
-### 5. Utilities
-- Thread pooling for efficient multi-threaded operations.
-- Logger utilities for runtime diagnostics.
-- Linear state-space Kalman filter implementation for state estimation.
-- Spline interpolation utilities in the `tk` namespace.
-
-### 6. Data Types and Parameters
-- Rich set of data types to represent paths, trajectories, controls, velocities, and bounding boxes.
-- Strongly-typed parameters and configuration classes to enable flexible tuning.
-
-### 7. Python Bindings
-- Comprehensive Python bindings built with `nanobind` to enable seamless integration with Python workflows.
-- Bindings cover core functionalities across mapping, control, vision, and utilities.
-
-
-
-## `kompass_core` Overview
-
-- `kompass_core.calibration` - Modules for robot motion model calibration, fitting and robot simulation.
-
-- `kompass_core.control` - A rich set of control strategies and configurations. Include the wrapper python classes for the C++ implementations:
+### Control Module
+- Includes a rich set of optimized C++ control strategies implementations and their python wrappers.
+- Supports **GPU-accelerated** trajectory sampling and cost evaluation with customizable weights for sampling based controllers.
+- Internally implements feature-based bounding box tracking and depth detection for enhanced vision-based tracking control.
 
 | Algorithm                                   | Description                                        |
 | ------------------------------------------- | -------------------------------------------------- |
@@ -127,28 +83,28 @@ This repository consists the following modules:
 | **VisionRGBFollower**   | Follow visual targets using RGB images          |
 | **VisionRGBDFollower**   | Follow visual targets using RGBD (depth) images          |
 
-- `kompass_core.datatypes` - Standardized message/data formats for various robot and sensor data.
-
-
-- `kompass_core.mapping` - Local mapping and occupancy grid generation, with configuration support for various laser models and grid resolution settings.
-
-- `kompass_core.models` - Robot models and motion kinematics, supporting differential, omni-directional, and Ackermann robots. Along with geometry definitions, control limits and simulation-ready state representations.
-
-- `kompass_core.motion_cost` - Cost models for trajectory evaluation in Python with various costs including collision probabilities, reference tracking and dynamic/static obstacle handling.
-
-- `kompass_core.performance` - Modules for evaluating algorithms performance.
+### Mapping Module
+- Implements efficient local mapping and occupancy grid generation algorithms, with configuration support for various laser models and grid resolution settings.
+- Supports **GPU-accelerated** mapping for real-time performance.
 
-- `kompass_core.py_path_tools` - Path interpolation and execution tools.
 
-- `kompass_core.simulation` - Tools for simulating robot motion and evaluating path feasibility.
+### Utilities Module
+- Provides collision checking utilities and critical zone detection to ensure safe navigation, including both CPU and GPU implementations.
+- Logger utilities for runtime diagnostics.
+- Linear state-space Kalman filter implementation for state estimation (C++).
+- Spline interpolation utilities for path control.
 
-- `kompass_core.third_party` - Wrappers and integrations with external planning and collision libraries:
+### Data Types and Models Modules
+- Rich set of data types to represent paths, trajectories, controls, velocities, bounding boxes and various sensor data.
+- Strongly-typed parameters and configuration classes to enable flexible tuning.
+- Robot models and motion kinematics, supporting differential, omni-directional, and Ackermann robots. Along with geometry definitions, control limits and simulation-ready state representations.
 
-    - FCL (Flexible Collision Library)
+### Third Party Modules
+Includes wrappers and integrations with external planning and collision libraries:
 
-    - OMPL (Open Motion Planning Library)
+- FCL (Flexible Collision Library)
 
-- `kompass_core.utils` - General utilities.
+- OMPL (Open Motion Planning Library)
 
 
 ## Copyright

From 4299c4628030b1dbb3b8ed2e9e55f7e9e488c903 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 24 Jun 2025 14:08:06 +0200
Subject: [PATCH 102/118] (docs) Adds docstring

---
 .../include/vision/depth_detector.h           | 23 ++++++++++++++++++-
 1 file changed, 22 insertions(+), 1 deletion(-)

diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
index 4f340fbc..d2179c8b 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -1,3 +1,24 @@
+/**
+ * @brief Implementation based on the following work [Xu2024OnboardDO]
+ *
+ * "Onboard Dynamic-Object Detection and Tracking for Autonomous Robot
+ * Navigation With RGB-D Camera" Z. Xu, X. Zhan, Y. Xiu, C. Suzuki, and K.
+ * Shimada. IEEE Robotics and Automation Letters, vol. 9, no. 1, pp. 651–658,
+ * 2024. doi:10.1109/LRA.2023.3334683
+ *
+ * @article{Xu2024OnboardDO,
+ *   title   = {Onboard Dynamic-Object Detection and Tracking for Autonomous
+ * Robot Navigation With RGB-D Camera}, author  = {Z. Xu and X. Zhan and Y. Xiu
+ * and C. Suzuki and K. Shimada}, journal = {IEEE Robotics and Automation
+ * Letters}, volume  = {9}, number  = {1}, pages   = {651--658}, year    =
+ * {2024}, doi     = {10.1109/LRA.2023.3334683}, keywords = { Detectors,
+ * Cameras, Three-dimensional displays, Point cloud compression, Robot vision
+ * systems, Heuristic algorithms, Collision avoidance, RGB-D perception,
+ * Vision-based navigation, Visual tracking, 3D object detection
+ *   }
+ * }
+ */
+
 #pragma once
 
 #include "datatypes/path.h"
@@ -44,4 +65,4 @@ class DepthDetector {
   static float getMedian(const std::vector<float> &values);
 };
 
-} // namespace Kompass
+  } // namespace Kompass

From b6b0ea4bd4e558f8792882356af79d7937642b1b Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 24 Jun 2025 14:43:30 +0200
Subject: [PATCH 103/118] (feature) Adds 'WARN' to logging levels as an alias
 to 'WARNING'

---
 src/kompass_cpp/bindings/bindings.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/kompass_cpp/bindings/bindings.cpp b/src/kompass_cpp/bindings/bindings.cpp
index 32aab78e..9e4d976a 100644
--- a/src/kompass_cpp/bindings/bindings.cpp
+++ b/src/kompass_cpp/bindings/bindings.cpp
@@ -34,6 +34,7 @@ NB_MODULE(kompass_cpp, m) {
       .value("DEBUG", LogLevel::DEBUG)
       .value("INFO", LogLevel::INFO)
       .value("WARNING", LogLevel::WARNING)
+      .value("WARN", LogLevel::WARNING)
       .value("ERROR", LogLevel::ERROR)
       .export_values();
 

From ae1cccc6cbba7390eccb8e331bf9bc6e557cd0e3 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 24 Jun 2025 16:24:35 +0200
Subject: [PATCH 104/118] (feature) Adds logging level to ompl planner

---
 src/kompass_core/third_party/ompl/config.py   | 30 ++++++++++++----
 src/kompass_core/third_party/ompl/planner.py  | 18 ++--------
 src/kompass_cpp/bindings/bindings_control.cpp |  9 +++--
 src/kompass_cpp/bindings/bindings_types.cpp   |  2 +-
 .../kompass_cpp/include/datatypes/control.h   | 34 ++++++++++++-------
 5 files changed, 54 insertions(+), 39 deletions(-)

diff --git a/src/kompass_core/third_party/ompl/config.py b/src/kompass_core/third_party/ompl/config.py
index 5eea954f..a4b3a0c3 100644
--- a/src/kompass_core/third_party/ompl/config.py
+++ b/src/kompass_core/third_party/ompl/config.py
@@ -1,5 +1,5 @@
 import inspect
-from typing import Any
+from typing import Any, Union
 
 from attrs import field, make_class
 from ...utils.common import BaseAttrs, base_validators
@@ -87,14 +87,32 @@ def getPlanners(self):
         return self.available_planners
 
 
-def initializePlanners():
+def _convert_log_level(log_level: Union[ompl.util.LogLevel, str]) -> ompl.util.LogLevel:
+    """Convert log level to ompl.util.LogLevel."""
+    if isinstance(log_level, ompl.util.LogLevel):
+        return log_level
+    elif isinstance(log_level, str):
+        upper_log_level = log_level.upper()
+        if not upper_log_level.startswith("LOG_"):
+            upper_log_level = "LOG_" + upper_log_level
+        if upper_log_level == "LOG_WARNING":
+            # Handle the special case for LOG_WARNING
+            # as it is not a valid enum in ompl.util.LogLevel
+            upper_log_level = "LOG_WARN"
+        try:
+            return getattr(ompl.util.LogLevel, upper_log_level)
+        except AttributeError as e:
+            raise ValueError(f"Invalid OMPL log level string: {log_level}") from e
+    raise ValueError(f"Invalid OMPL log level: {log_level}")
+
+
+def initializePlanners(log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR):
     """Initialize planner map, similar to ompl python bindings."""
-    logLevel = ompl.util.getLogLevel()
-    # TODO: make log_level an input to initializePlanners to set it when using ompl
-    ompl.util.setLogLevel(ompl.util.LogLevel.LOG_ERROR)
+    log_level = _convert_log_level(log_level)
+    ompl.util.setLogLevel(log_level)
     if not hasattr(ompl.geometric, "planners"):
         ompl.geometric.planners = PlanningAlgorithms(ompl.geometric)
-    ompl.util.setLogLevel(logLevel)
+    ompl.util.setLogLevel(log_level)
 
 
 optimization_objectives = {
diff --git a/src/kompass_core/third_party/ompl/planner.py b/src/kompass_core/third_party/ompl/planner.py
index a7f5a24d..8aaf4270 100644
--- a/src/kompass_core/third_party/ompl/planner.py
+++ b/src/kompass_core/third_party/ompl/planner.py
@@ -39,11 +39,6 @@ class OMPLGeometricConfig(BaseAttrs):
         default=1.0, validator=base_validators.in_range(min_value=1e-9, max_value=1e3)
     )
 
-    log_level: str = field(
-        default="WARN",
-        validator=base_validators.in_(["DEBUG", "INFO", "WARN", "ERROR"]),
-    )
-
     map_resolution: float = field(
         default=0.01, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
     )
@@ -55,6 +50,7 @@ class OMPLGeometric:
     def __init__(
         self,
         robot: Robot,
+        log_level: str = "ERROR",
         use_fcl: bool = True,
         config: Optional[OMPLGeometricConfig] = None,
         config_file: Optional[str] = None,
@@ -73,7 +69,7 @@ def __init__(
         :type map_3d: np.ndarray | None, optional
         """
         # Initialize available planners lists in ompl.geometric and ompl.control
-        initializePlanners()
+        initializePlanners(log_level)
 
         self.solution = None
 
@@ -85,11 +81,6 @@ def __init__(
         else:
             self._config = OMPLGeometricConfig()
 
-        # Set log level
-        ompl.util.setLogLevel(
-            getattr(ompl.util.LogLevel, f"LOG_{self._config.log_level}")
-        )
-
         self._use_fcl = use_fcl
 
         # Create SE2 state space for 2D planning
@@ -162,11 +153,6 @@ def configure(
             nested_root_name = "ompl"
         self._config.from_file(config_file, nested_root_name=nested_root_name)
 
-        # Set LOG level
-        ompl.util.setLogLevel(
-            getattr(ompl.util.LogLevel, f"LOG_{self._config.log_level}")
-        )
-
         if not planner_id:
             planner_id = self._config.planner_id
 
diff --git a/src/kompass_cpp/bindings/bindings_control.cpp b/src/kompass_cpp/bindings/bindings_control.cpp
index 220be664..dbce2055 100644
--- a/src/kompass_cpp/bindings/bindings_control.cpp
+++ b/src/kompass_cpp/bindings/bindings_control.cpp
@@ -29,6 +29,7 @@ void bindings_control(py::module_ &m) {
   // Limits setup
   py::class_<Control::LinearVelocityControlParams>(
       m_control, "LinearVelocityControlParams")
+      .def(py::init<const Control::LinearVelocityControlParams &>())
       .def(py::init<double, double, double>(), py::arg("max_vel") = 0.0,
            py::arg("max_acc") = 0.0, py::arg("max_decel") = 0.0)
       .def_rw("max_vel", &Control::LinearVelocityControlParams::maxVel)
@@ -38,6 +39,7 @@ void bindings_control(py::module_ &m) {
 
   py::class_<Control::AngularVelocityControlParams>(
       m_control, "AngularVelocityControlParams")
+      .def(py::init<const Control::AngularVelocityControlParams &>())
       .def(py::init<double, double, double, double>(),
            py::arg("max_ang") = M_PI, py::arg("max_omega") = 0.0,
            py::arg("max_acc") = 0.0, py::arg("max_decel") = 0.0)
@@ -49,9 +51,10 @@ void bindings_control(py::module_ &m) {
               &Control::AngularVelocityControlParams::maxDeceleration);
 
   py::class_<Control::ControlLimitsParams>(m_control, "ControlLimitsParams")
-      .def(py::init<Control::LinearVelocityControlParams,
-                    Control::LinearVelocityControlParams,
-                    Control::AngularVelocityControlParams>(),
+      .def(py::init<>())
+      .def(py::init<Control::LinearVelocityControlParams &,
+                    Control::LinearVelocityControlParams &,
+                    Control::AngularVelocityControlParams &>(),
            py::arg("vel_x_ctr_params") = Control::LinearVelocityControlParams(),
            py::arg("vel_y_ctr_params") = Control::LinearVelocityControlParams(),
            py::arg("omega_ctr_params") =
diff --git a/src/kompass_cpp/bindings/bindings_types.cpp b/src/kompass_cpp/bindings/bindings_types.cpp
index 5d870d72..2c61dd2b 100644
--- a/src/kompass_cpp/bindings/bindings_types.cpp
+++ b/src/kompass_cpp/bindings/bindings_types.cpp
@@ -111,7 +111,7 @@ void bindings_types(py::module_ &m) {
       .value("CYLINDER", CollisionChecker::ShapeType::CYLINDER)
       .value("BOX", CollisionChecker::ShapeType::BOX)
       .value("SPHERE", CollisionChecker::ShapeType::SPHERE)
-      .def("get", [](const std::string &key) {
+      .def_static("get", [](const std::string &key) {
         if (key == "CYLINDER")
           return CollisionChecker::ShapeType::CYLINDER;
         if (key == "BOX")
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index e86e9805..6550bd75 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -233,22 +233,28 @@ struct Velocities {
 
 // Structure for Forward Linear Velocity Control parameters
 struct LinearVelocityControlParams {
-  double maxVel;          // Maximum allowed speed
-  double maxAcceleration; // Maximum acceleration in units per second squared
-  double maxDeceleration; // Maximum deceleration in units per second squared
+  double maxVel = 1.0;
+  double maxAcceleration = 10.0;
+  double maxDeceleration = 10.0;
 
+  LinearVelocityControlParams(const LinearVelocityControlParams &) = default;
+
+  // Parameterized constructor
   LinearVelocityControlParams(double maxVel = 1.0, double maxAcc = 10.0,
                               double maxDec = 10.0)
       : maxVel(maxVel), maxAcceleration(maxAcc), maxDeceleration(maxDec) {}
 };
 
-// Structure for Forward Linear Velocity Control parameters
+// Structure for Angular Velocity Control parameters
 struct AngularVelocityControlParams {
-  double maxAngle; // Maximum allowed steering angle
-  double maxOmega;
-  double maxAcceleration; // Maximum acceleration in units per second squared
-  double maxDeceleration; // Maximum deceleration in units per second squared
+  double maxAngle = M_PI;
+  double maxOmega = 1.0;
+  double maxAcceleration = 10.0;
+  double maxDeceleration = 10.0;
 
+  AngularVelocityControlParams(const AngularVelocityControlParams &) = default;
+
+  // Parameterized constructor
   AngularVelocityControlParams(double maxAng = M_PI, double maxOmg = 1.0,
                                double maxAcc = 10.0, double maxDec = 10.0)
       : maxAngle(maxAng), maxOmega(maxOmg), maxAcceleration(maxAcc),
@@ -261,11 +267,13 @@ struct ControlLimitsParams {
   LinearVelocityControlParams velYParams;
   AngularVelocityControlParams omegaParams;
 
-  ControlLimitsParams(
-      LinearVelocityControlParams velXCtrParams = LinearVelocityControlParams(),
-      LinearVelocityControlParams velYCtrParams = LinearVelocityControlParams(),
-      AngularVelocityControlParams omegaCtrParams =
-          AngularVelocityControlParams())
+  ControlLimitsParams() = default;
+  ControlLimitsParams(const ControlLimitsParams &) = default;
+
+  // Parameterized constructor
+  ControlLimitsParams(const LinearVelocityControlParams &velXCtrParams,
+                      const LinearVelocityControlParams &velYCtrParams,
+                      const AngularVelocityControlParams &omegaCtrParams)
       : velXParams(velXCtrParams), velYParams(velYCtrParams),
         omegaParams(omegaCtrParams) {}
 };

From b6f0b490b91dbda280dcb8bba8989742dcf90761 Mon Sep 17 00:00:00 2001
From: Maria Kabtoul <64647569+mkabtoul@users.noreply.github.com>
Date: Wed, 25 Jun 2025 13:07:09 +0200
Subject: [PATCH 105/118] Update build_and_deploy.yml

Adds installation for Python development headers
---
 .github/workflows/build_and_deploy.yml | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/.github/workflows/build_and_deploy.yml b/.github/workflows/build_and_deploy.yml
index ae6196b2..d3770d05 100644
--- a/.github/workflows/build_and_deploy.yml
+++ b/.github/workflows/build_and_deploy.yml
@@ -27,6 +27,12 @@ jobs:
       - name: Set environment variables
         run: echo "CIBW_ARCHS=${{ matrix.os-arch.arch }}" >> $GITHUB_ENV
 
+      - name: Install Python development headers
+      - run: |
+             PYTHON_VERSION=$(python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}')")
+             sudo apt-get update
+             sudo apt-get install -y python${PYTHON_VERSION}-dev
+
       - name: Build wheels
         uses: pypa/cibuildwheel@v2.21.3
 

From 7788120852845e90e7abfcc97c180371a4f5da87 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 26 Jun 2025 16:16:08 +0200
Subject: [PATCH 106/118] (fix) Fixes typo in DVZ controller

---
 src/kompass_core/control/dvz.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/kompass_core/control/dvz.py b/src/kompass_core/control/dvz.py
index 6d03b6a0..8063215d 100644
--- a/src/kompass_core/control/dvz.py
+++ b/src/kompass_core/control/dvz.py
@@ -228,7 +228,7 @@ def logging_info(self) -> str:
 
         :rtype: str
         """
-        return f"total deformation : {self._path_controller.dvz_controller.total_deformation}"
+        return f"Total DVZ deformation : {self._path_controller.total_deformation}"
 
     @property
     def linear_x_control(self) -> List[float]:

From 3ae16cd21f2cf35c0857aaacc30e0f2f46dd8933 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 26 Jun 2025 16:23:18 +0200
Subject: [PATCH 107/118] (fix) Fixes argument name typo

---
 src/kompass_core/calibration.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/kompass_core/calibration.py b/src/kompass_core/calibration.py
index 0f97020b..2bf50d3c 100644
--- a/src/kompass_core/calibration.py
+++ b/src/kompass_core/calibration.py
@@ -136,7 +136,7 @@ def simulate_calibrated_model_data(
             x=motion_sample.path_sample.x_points[0],
             y=motion_sample.path_sample.y_points[0],
             yaw=motion_sample.path_sample.heading_points[0],
-            motion_model=calibrated_model,
+            model=calibrated_model,
         )
 
         robot = Robot(

From 301d454f98a516eabfd7085b081b0bfc82716b7f Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Thu, 26 Jun 2025 19:20:27 +0200
Subject: [PATCH 108/118] (feature) Adds OMPL geometric planner and validity
 checking in C++

---
 src/kompass_core/third_party/ompl/config.py   |   4 +-
 src/kompass_core/third_party/ompl/planner.py  | 285 +++---------------
 src/kompass_cpp/CMakeLists.txt                |   1 +
 src/kompass_cpp/bindings/bindings.cpp         |   2 +
 .../bindings/bindings_planning.cpp            |  34 +++
 src/kompass_cpp/kompass_cpp/CMakeLists.txt    |   6 +-
 .../kompass_cpp/include/planning/ompl.h       |  93 ++++++
 .../include/utils/collision_check.h           |  10 +
 .../kompass_cpp/src/planning/ompl.cpp         | 103 +++++++
 .../kompass_cpp/src/utils/collision_check.cpp |  22 ++
 tests/test_ompl.py                            |  23 +-
 11 files changed, 319 insertions(+), 264 deletions(-)
 create mode 100644 src/kompass_cpp/bindings/bindings_planning.cpp
 create mode 100644 src/kompass_cpp/kompass_cpp/include/planning/ompl.h
 create mode 100644 src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp

diff --git a/src/kompass_core/third_party/ompl/config.py b/src/kompass_core/third_party/ompl/config.py
index a4b3a0c3..a6027c9b 100644
--- a/src/kompass_core/third_party/ompl/config.py
+++ b/src/kompass_core/third_party/ompl/config.py
@@ -106,7 +106,9 @@ def _convert_log_level(log_level: Union[ompl.util.LogLevel, str]) -> ompl.util.L
     raise ValueError(f"Invalid OMPL log level: {log_level}")
 
 
-def initializePlanners(log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR):
+def initializePlanners(
+    log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR,
+):
     """Initialize planner map, similar to ompl python bindings."""
     log_level = _convert_log_level(log_level)
     ompl.util.setLogLevel(log_level)
diff --git a/src/kompass_core/third_party/ompl/planner.py b/src/kompass_core/third_party/ompl/planner.py
index 8aaf4270..34b3d8a8 100644
--- a/src/kompass_core/third_party/ompl/planner.py
+++ b/src/kompass_core/third_party/ompl/planner.py
@@ -3,14 +3,12 @@
 import numpy as np
 from attrs import asdict, define, field
 from ...utils.common import BaseAttrs, base_validators
-from ...utils.geometry import convert_to_plus_minus_pi
 
 import omplpy as ompl
-from ...models import Robot, RobotState
+from ...models import Robot, RobotGeometry
 from omplpy import base, geometric
+from kompass_cpp.planning import OMPL2DGeometricPlanner
 
-from ..fcl.collisions import FCL
-from ..fcl.config import FCLConfig
 from .config import create_config_class, initializePlanners, optimization_objectives
 
 
@@ -54,7 +52,6 @@ def __init__(
         use_fcl: bool = True,
         config: Optional[OMPLGeometricConfig] = None,
         config_file: Optional[str] = None,
-        map_3d: Optional[np.ndarray] = None,
     ):
         """
         Init Open Motion Planning Lib geometric handler
@@ -93,45 +90,29 @@ def __init__(
         self.available_planners = ompl.geometric.planners.getPlanners()
 
         # create a simple setup object
-        self.setup = geometric.SimpleSetup(self.state_space)
-
-        # Configure FCL
-        if use_fcl:
-            self.__init_fcl(map_3d)
-            is_state_valid = self._validity_checker_with_fcl
+        self._ompl_setup = geometric.SimpleSetup(self.state_space)
 
-        else:
-            is_state_valid = self._validity_checker
+        # Set the selected planner
+        self._set_planner()
 
-        # Add validity method to ompl setup without custom validity checkers
-        self.setup.setStateValidityChecker(is_state_valid)
+        # Setup Path Optimization Objective
+        optimization_objective = getattr(base, self._config.optimization_objective)(
+            self._ompl_setup.getSpaceInformation()
+        )
 
-        # Set the selected planner
-        self.set_planner()
+        self._ompl_setup.setOptimizationObjective(optimization_objective)
 
         if config_file:
             self.configure(config_file)
 
-    def __init_fcl(self, map_3d: Optional[np.ndarray] = None):
-        """
-        Setup FCL
-
-        :param map_3d: Map data
-        :type map_3d: np.ndarray
-        """
-        fcl_config = FCLConfig(
+        self._cpp_planner: Optional[OMPL2DGeometricPlanner] = OMPL2DGeometricPlanner(
+            robot_shape=RobotGeometry.Type.to_kompass_cpp_lib(
+                self._robot.geometry_type
+            ),
+            robot_dimensions=self._robot.geometry_params,
+            ompl_setup=self._ompl_setup,
             map_resolution=self._config.map_resolution,
-            robot_geometry_type=self._robot.geometry_type,
-            robot_geometry_params=self._robot.geometry_params.tolist(),
         )
-        if not hasattr(self, "fcl"):
-            # setup collision check
-            if map_3d is not None:
-                self.fcl = FCL(fcl_config, map_3d)
-            else:
-                self.fcl = FCL(fcl_config)
-        else:
-            self.fcl._setup_from_config()
 
     def configure(
         self,
@@ -170,59 +151,19 @@ def configure(
 
         planner_params.from_file(config_file, nested_root_name + "." + planner_name)
 
-        self.set_planner(planner_params, planner_id)
+        self._set_planner(planner_params, planner_id)
 
         self.start = False
 
-        # configure FCL if it is used
-        if self._use_fcl:
-            self.__init_fcl()
-
-    def clear(self):
-        """
-        Clears planning setup
-        """
-        self.setup.clear()
-
     @property
-    def path_cost(self) -> Optional[float]:
+    def path_cost(self) -> float:
         """
         Getter of solution path cost using the configured optimization objective
 
         :return: Path cost
         :rtype: float | None
         """
-        if self.solution:
-            optimization_objective = getattr(base, self._config.optimization_objective)(
-                self.setup.getSpaceInformation()
-            )
-            cost = self.solution.cost(optimization_objective)
-            return cost.value()
-        return None
-
-    def get_cost_using_objective(self, objective_key: str) -> Optional[float]:
-        """
-        Get solution cost using a specific objective
-        This is used to get the cost using an objective other than the configured objective
-        To get the cost using the default objective use self.path_cost directly
-
-        :param objective_key: _description_
-        :type objective_key: str
-        :raises KeyError: Unknown objective name
-        :return: _description_
-        :rtype: _type_
-        """
-        if not self.solution:
-            return None
-        if objective_key in optimization_objectives:
-            optimization_objective = getattr(base, objective_key)(
-                self.setup.getSpaceInformation()
-            )
-            cost = self.solution.cost(optimization_objective)
-            return cost.value()
-        raise KeyError(
-            f"Unknown optimization objective. Available optimization objectives are: {optimization_objectives.keys()}"
-        )
+        return self._cpp_planner.get_cost()
 
     def setup_problem(
         self,
@@ -236,7 +177,7 @@ def setup_problem(
         map_3d: Optional[np.ndarray] = None,
     ):
         """
-        Setup a new planning problem with a map, start and goal infromation
+        Setup a new planning problem with a map, start and goal information
 
         :param map_meta_data: Global map meta data as a dictionary
         :type map_meta_data: Dict
@@ -255,132 +196,16 @@ def setup_problem(
         :param map_3d: 3D array for map PointCloud data, defaults to None
         :type map_3d: np.ndarray | None, optional
         """
-        # Clear previous setup
-        self.setup.clear()
-
-        self.set_space_bounds(map_meta_data)
-
-        scoped_start_state = base.ScopedState(self.state_space)
-        start_state = scoped_start_state.get()
-        start_state.setX(start_x)
-        start_state.setY(start_y)
-
-        # SE2 takes angle in [-pi,+pi]
-        yaw = convert_to_plus_minus_pi(start_yaw)
-        start_state.setYaw(yaw)
-
-        scoped_goal_state = base.ScopedState(self.state_space)
-        goal_state = scoped_goal_state.get()
-        goal_state.setX(goal_x)
-        goal_state.setY(goal_y)
-
-        # SE2 takes angle in [-pi,+pi]
-        yaw = convert_to_plus_minus_pi(goal_yaw)
-        goal_state.setYaw(yaw)
-
-        self.setup.setStartAndGoalStates(
-            start=scoped_start_state, goal=scoped_goal_state
-        )
-
-        # Setup Path Optimization Objective
-        optimization_objective = getattr(base, self._config.optimization_objective)(
-            self.setup.getSpaceInformation()
-        )
-
-        self.setup.setOptimizationObjective(optimization_objective)
-
-        if self._use_fcl and map_3d is None:
-            raise ValueError(
-                "OMPL is started with collision check -> Map should be provided"
-            )
-        elif self._use_fcl:
-            # TODO: Add an option to update the map periodically or just at the start, or after a time interval
-            if not self.start:
-                self.fcl.set_map(map_3d)
-                self.start = True
-            self.fcl.update_state(
-                robot_state=RobotState(x=start_x, y=start_y, yaw=start_yaw)
-            )
-
-    def add_validity_check(self, name: str, validity_function: Callable) -> None:
-        """
-        Add method for state validity check during planning
-
-        :param name: Method key name
-        :type name: str
-        :param validity_function: Validity check method
-        :type validity_function: Callable
-
-        :raises TypeError: If validity check is not callable
-        :raises TypeError: If validity check method does not return a boolean
-        """
-        # Check that validity function is callable
-        if callable(validity_function) and callable(validity_function):
-            # Check if the function returns a boolean value
-            args = (None,) * validity_function.__code__.co_argcount
-            if isinstance(validity_function(*args), bool):
-                self._custom_validity_check[name] = validity_function
-            else:
-                raise TypeError("Validity check function needs to return a boolean")
-        else:
-            raise TypeError("Validity check function must be callable")
-
-    def remove_validity_check(self, name: str) -> bool:
-        """
-        Removes an added validity chec!= Nonek
-
-        :param name: Validity check name key
-        :type name: str
-
-        :raises ValueError: If given key does not correspond to an added validity check
-
-        :return: If validity check is removed
-        :rtype: bool
-        """
-        deleted_method = self._custom_validity_check.pop(name, None)
-        if deleted_method is not None:
-            return True
-        else:
-            raise ValueError(
-                f"Cannot remove validity check titled {name} as it does not exist"
-            )
-
-    def _validity_checker(self, state, **_) -> bool:
-        """
-        State validity checker method
-
-        :param state: Robot state
-        :type state: SE2State
-
-        :return: If state is valid
-        :rtype: bool
-        """
-        # Run bounds and state check
-        return self.setup.getSpaceInformation().satisfiesBounds(state)
-
-    def _validity_checker_with_fcl(self, state, **_) -> bool:
-        """
-        State validity checker method
-
-        :param state: Robot state
-        :type state: SE2State
-
-        :return: If state is valid
-        :rtype: bool
-        """
-        # Run bounds and state check
-        state_space_valid: bool = self.setup.getSpaceInformation().satisfiesBounds(
-            state
+        self._set_space_bounds(map_meta_data)
+        self._cpp_planner.setup_problem(
+            start_x=start_x,
+            start_y=start_y,
+            start_yaw=start_yaw,
+            goal_x=goal_x,
+            goal_y=goal_y,
+            goal_yaw=goal_yaw,
+            map_3d=map_3d,
         )
-        if self.fcl.got_map:
-            self.fcl.update_state(
-                RobotState(x=state.getX(), y=state.getY(), yaw=state.getYaw())
-            )
-            is_collision = self.fcl.check_collision()
-        else:
-            is_collision = False
-
-        return state_space_valid and not is_collision
 
     @property
     def planner_params(self) -> Optional[ompl.base.ParamSet]:
@@ -420,7 +245,7 @@ def planner_id(self) -> str:
         """
         return self._config.planner_id
 
-    def set_planner(
+    def _set_planner(
         self,
         planner_config: Optional[BaseAttrs] = None,
         planner_id: Optional[str] = None,
@@ -441,40 +266,32 @@ def set_planner(
             else:
                 self._config.planner_id = planner_id
 
-            self.planner = eval("%s(self.setup.getSpaceInformation())" % planner_id)
+            self.planner = eval(
+                "%s(self._ompl_setup.getSpaceInformation())" % planner_id
+            )
 
             if planner_config:
                 self.planner_params = planner_config
 
-            self.setup.setPlanner(self.planner)
+            self._ompl_setup.setPlanner(self.planner)
 
         except Exception:
             raise
 
-    def set_space_bounds(self, map_meta_data: Dict):
+    def _set_space_bounds(self, map_meta_data: Dict):
         """
         Set planning space bounds from map data
 
         :param map_meta_data: Map meta data
         :type map_meta_data: Dict
         """
-        # X-axis bounds
-        x_lower = map_meta_data["origin_x"]
-        x_upper = x_lower + map_meta_data["resolution"] * map_meta_data["width"]
-
-        # Y-axis bounds
-        y_lower = map_meta_data["origin_y"]
-        y_upper = y_lower + map_meta_data["resolution"] * map_meta_data["height"]
-
-        # set lower and upper bounds
-        bounds = base.RealVectorBounds(2)
-
-        bounds.setLow(index=0, value=x_lower)
-        bounds.setLow(index=1, value=y_lower)
-        bounds.setHigh(index=0, value=x_upper)
-        bounds.setHigh(index=1, value=y_upper)
-
-        self.state_space.setBounds(bounds)
+        self._cpp_planner.set_space_bounds_from_map(
+            origin_x=map_meta_data["origin_x"],
+            origin_y=map_meta_data["origin_y"],
+            width=map_meta_data["width"],
+            height=map_meta_data["height"],
+            resolution=map_meta_data["resolution"],
+        )
 
     def solve(self) -> ompl.geometric.PathGeometric:
         """
@@ -483,23 +300,9 @@ def solve(self) -> ompl.geometric.PathGeometric:
         :return: Solution (Path points) or None if no solution is found
         :rtype: ompl.geometric.PathGeometric
         """
-        solved = self.setup.solve(self._config.planning_timeout)
+        solved = self._cpp_planner.solve(self._config.planning_timeout)
 
         if solved:
-            self.solution = self.setup.getSolutionPath()
-
-            return self.solution
-        return None
-
-    def simplify_solution(self) -> ompl.geometric.PathGeometric:
-        """
-        Simplify the path
-
-        :return: Simplified path
-        :rtype: ompl.geometric.PathGeometric
-        """
-        if self.solution:
-            self.setup.simplifySolution(self._config.simplification_timeout)
-            self.solution = self.setup.getSolutionPath()
+            self.solution = self._cpp_planner.get_solution()
             return self.solution
         return None
diff --git a/src/kompass_cpp/CMakeLists.txt b/src/kompass_cpp/CMakeLists.txt
index 23e73f2d..d6075a06 100644
--- a/src/kompass_cpp/CMakeLists.txt
+++ b/src/kompass_cpp/CMakeLists.txt
@@ -2,6 +2,7 @@
 find_package(Eigen3 3.4 REQUIRED NO_MODULE)
 find_package(PCL COMPONENTS common io REQUIRED)
 find_package(fcl REQUIRED)
+find_package(ompl REQUIRED)
 
 # find sycl
 find_package(AdaptiveCpp)
diff --git a/src/kompass_cpp/bindings/bindings.cpp b/src/kompass_cpp/bindings/bindings.cpp
index 9e4d976a..08f3f99f 100644
--- a/src/kompass_cpp/bindings/bindings.cpp
+++ b/src/kompass_cpp/bindings/bindings.cpp
@@ -14,6 +14,7 @@ void bindings_config(py::module_ &);
 void bindings_control(py::module_ &);
 void bindings_mapping(py::module_ &);
 void bindings_utils(py::module_ &);
+void bindings_planning(py::module_ &);
 
 using namespace Kompass;
 
@@ -28,6 +29,7 @@ NB_MODULE(kompass_cpp, m) {
   bindings_config(m);
   bindings_control(m);
   bindings_mapping(m);
+  bindings_planning(m);
 
   // Utils bindings submodule
   py::enum_<LogLevel>(m, "LogLevel")
diff --git a/src/kompass_cpp/bindings/bindings_planning.cpp b/src/kompass_cpp/bindings/bindings_planning.cpp
new file mode 100644
index 00000000..15850a98
--- /dev/null
+++ b/src/kompass_cpp/bindings/bindings_planning.cpp
@@ -0,0 +1,34 @@
+#include "planning/ompl.h"
+#include <nanobind/eigen/dense.h>
+#include <nanobind/nanobind.h>
+#include <nanobind/stl/optional.h>
+#include <nanobind/stl/vector.h>
+
+namespace py = nanobind;
+using namespace Kompass;
+
+
+// Utils submodule
+void bindings_planning(py::module_ &m) {
+  auto m_planning = m.def_submodule("planning", "KOMPASS CPP Planning Module");
+
+  py::class_<Planning::OMPL2DGeometricPlanner>(m_planning,
+                                               "OMPL2DGeometricPlanner")
+      .def(py::init<const CollisionChecker::ShapeType &,
+                    const std::vector<float> &,
+                    const ompl::geometric::SimpleSetup &, const float>(),
+           py::arg("robot_shape"), py::arg("robot_dimensions"),
+           py::arg("ompl_setup"), py::arg("map_resolution") = 0.01)
+      .def("setup_problem", &Planning::OMPL2DGeometricPlanner::setupProblem,
+           py::arg("start_x"), py::arg("start_y"), py::arg("start_yaw"),
+           py::arg("goal_x"), py::arg("goal_y"), py::arg("goal_yaw"),
+           py::arg("map_3d"))
+      .def("solve", &Planning::OMPL2DGeometricPlanner::solve,
+               py::arg("planning_timeout") = 1.0)
+      .def("get_solution", &Planning::OMPL2DGeometricPlanner::getPath)
+      .def("set_space_bounds_from_map",
+           &Planning::OMPL2DGeometricPlanner::setSpaceBoundsFromMap,
+           py::arg("origin_x"), py::arg("origin_y"), py::arg("width"),
+           py::arg("height"), py::arg("resolution"))
+      .def("get_cost", &Planning::OMPL2DGeometricPlanner::getCost);
+}
diff --git a/src/kompass_cpp/kompass_cpp/CMakeLists.txt b/src/kompass_cpp/kompass_cpp/CMakeLists.txt
index 82d8b43c..c8f69cd4 100644
--- a/src/kompass_cpp/kompass_cpp/CMakeLists.txt
+++ b/src/kompass_cpp/kompass_cpp/CMakeLists.txt
@@ -32,10 +32,10 @@ endif()
 # add project as library
 add_library(${MODULE_NAME} STATIC ${SOURCES})
 target_include_directories(${MODULE_NAME} PUBLIC include)
-target_include_directories(${MODULE_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS})
-
+target_include_directories(${MODULE_NAME} PUBLIC ${OMPL_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS})
+message(STATUS "OMPL_INCLUDE_DIR ${OMPL_INCLUDE_DIR}")
 # Link fcl and PCL
-target_link_libraries(${MODULE_NAME} PRIVATE fcl ${PCL_LIBRARIES})
+target_link_libraries(${MODULE_NAME} PRIVATE fcl ${OMPL_LIBRARIES} ${PCL_LIBRARIES})
 
 # add sycl
 if(AdaptiveCpp_FOUND)
diff --git a/src/kompass_cpp/kompass_cpp/include/planning/ompl.h b/src/kompass_cpp/kompass_cpp/include/planning/ompl.h
new file mode 100644
index 00000000..0e481674
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/include/planning/ompl.h
@@ -0,0 +1,93 @@
+#pragma once
+
+#include "utils/collision_check.h"
+#include <fcl/fcl.h>
+#include <ompl/base/Cost.h>
+#include <ompl/base/Planner.h>
+#include <ompl/base/PlannerStatus.h>
+#include <ompl/base/SpaceInformation.h>
+#include <ompl/base/spaces/SE2StateSpace.h>
+#include <ompl/geometric/PathGeometric.h>
+#include <ompl/geometric/SimpleSetup.h>
+#include <optional>
+
+namespace Kompass {
+
+namespace Planning {
+
+class OMPL2DGeometricPlanner {
+public:
+  OMPL2DGeometricPlanner(const CollisionChecker::ShapeType &robot_shape_type,
+                         const std::vector<float> &robot_dimensions,
+                         const ompl::geometric::SimpleSetup &setup,
+                         const float map_resolution = 0.01);
+  ~OMPL2DGeometricPlanner();
+
+  /**
+   * @brief Setup OMPL planning problem
+   *
+   * @param start_x
+   * @param start_y
+   * @param start_yaw
+   * @param goal_x
+   * @param goal_y
+   * @param goal_yaw
+   * @param map_3d
+   */
+  void setupProblem(double start_x, double start_y, double start_yaw,
+                    double goal_x, double goal_y, double goal_yaw,
+                    const std::vector<Eigen::Vector3f> &map_3d);
+  /**
+   * @brief Solve the planning problem
+   *
+   * @param planning_timeout
+   * @return true
+   * @return false
+   */
+  bool solve(double planning_timeout);
+
+  /**
+   * @brief Get the Path object
+   *
+   * @return std::optional<ompl::geometric::PathGeometric>
+   */
+  std::optional<ompl::geometric::PathGeometric> getPath() const;
+
+  /**
+   * @brief Get the Cost object
+   *
+   * @return float
+   */
+  float getCost() const;
+
+  /**
+   * @brief Set the Space Bounds From Map object
+   *
+   * @param origin_x
+   * @param origin_y
+   * @param width
+   * @param height
+   * @param resolution
+   */
+  void setSpaceBoundsFromMap(const float origin_x, const float origin_y,
+                             const int width, const int height,
+                             const float resolution);
+
+private:
+  bool gotSolution_ = false;
+  ompl::geometric::SimpleSetupPtr setup_;
+  std::shared_ptr<CollisionChecker> collision_checker_;
+
+  /**
+   * @brief State validity checking function (uses collision checker)
+   *
+   * @param state
+   * @return true
+   * @return false
+   */
+  bool state_validity_checker(const ompl::base::State *state);
+};
+
+} // namespace Planning
+
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
index bc52d8db..0d504be5 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
@@ -2,6 +2,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <Eigen/src/Core/Matrix.h>
 #include <memory>
 #include <vector>
 
@@ -98,6 +99,15 @@ class CollisionChecker {
   void updatePointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                         const bool global_frame = true);
 
+  /**
+   * @brief Update the sensor input from Map points (Eigen::Matrix3Xf) data
+   *
+   * @param points
+   * @param global_frame
+   */
+  void update3DMap(const std::vector<Eigen::Vector3f> &points,
+                        const bool global_frame = true);
+
   /**
    * @brief Update the sensor input from PointCloud like struct
    * std::vector<Control::Point3D>
diff --git a/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp b/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp
new file mode 100644
index 00000000..4d3a66e5
--- /dev/null
+++ b/src/kompass_cpp/kompass_cpp/src/planning/ompl.cpp
@@ -0,0 +1,103 @@
+#include "planning/ompl.h"
+#include "utils/logger.h"
+
+namespace Kompass {
+namespace Planning {
+OMPL2DGeometricPlanner::OMPL2DGeometricPlanner(
+    const CollisionChecker::ShapeType &robot_shape_type,
+    const std::vector<float> &robot_dimensions,
+    const ompl::geometric::SimpleSetup &setup, const float map_resolution) {
+  setup_ = ompl::geometric::SimpleSetupPtr(
+      std::make_shared<ompl::geometric::SimpleSetup>(setup));
+  setup_->setStateValidityChecker([this](const ompl::base::State *state) {
+    return this->state_validity_checker(state);
+  });
+  collision_checker_ = std::make_shared<CollisionChecker>(
+      robot_shape_type, robot_dimensions, Eigen::Vector3f(0.0, 0.0, 0.0),
+      Eigen::Quaternionf(1.0, 0.0, 0.0, 0.0), map_resolution);
+}
+
+OMPL2DGeometricPlanner::~OMPL2DGeometricPlanner() {}
+
+void OMPL2DGeometricPlanner::setupProblem(
+    double start_x, double start_y, double start_yaw, double goal_x,
+    double goal_y, double goal_yaw,
+    const std::vector<Eigen::Vector3f> &map_3d) {
+  setup_->clear();
+  collision_checker_->update3DMap(map_3d);
+  ompl::base::ScopedState<ompl::base::SE2StateSpace> start(
+      setup_->getStateSpace());
+  ompl::base::ScopedState<ompl::base::SE2StateSpace> goal(
+      setup_->getStateSpace());
+  start->setX(start_x);
+  start->setY(start_y);
+  start->setYaw(start_yaw);
+
+  goal->setX(goal_x);
+  goal->setY(goal_y);
+  goal->setYaw(goal_yaw);
+
+  setup_->setStartAndGoalStates(start, goal);
+}
+
+void OMPL2DGeometricPlanner::setSpaceBoundsFromMap(const float origin_x,
+                                                   const float origin_y,
+                                                   const int width,
+                                                   const int height,
+                                                   const float resolution) {
+  auto bounds = ompl::base::RealVectorBounds(2);
+  bounds.setLow(0, origin_x);
+  bounds.setLow(1, origin_y);
+  bounds.setHigh(0, origin_x + float(resolution * width));
+  bounds.setHigh(1, origin_y + float(resolution * height));
+  setup_->getStateSpace()->as<ompl::base::SE2StateSpace>()->setBounds(bounds);
+}
+
+bool OMPL2DGeometricPlanner::solve(double planning_timeout) {
+  auto state = setup_->solve(planning_timeout);
+  if (state == ompl::base::PlannerStatus::APPROXIMATE_SOLUTION or
+      state == ompl::base::PlannerStatus::EXACT_SOLUTION) {
+    gotSolution_ = true;
+    setup_->simplifySolution();
+  } else {
+    LOG_ERROR("OMPL planner failed to find path, status = ", state.asString());
+    gotSolution_ = false;
+  }
+  return gotSolution_;
+}
+
+std::optional<ompl::geometric::PathGeometric>
+OMPL2DGeometricPlanner::getPath() const {
+  if (gotSolution_) {
+    return setup_->getSolutionPath();
+  }
+  return std::nullopt;
+}
+
+float OMPL2DGeometricPlanner::getCost() const {
+  auto solution = this->getPath();
+  if (solution) {
+    auto optimization_objective = setup_->getOptimizationObjective();
+    ompl::base::Cost cost = solution->cost(optimization_objective);
+    return float(cost.value());
+  }
+  return 0.0f;
+}
+
+bool OMPL2DGeometricPlanner::state_validity_checker(
+    const ompl::base::State *state) {
+  bool is_state_valid = setup_->getSpaceInformation()->satisfiesBounds(state);
+  auto se2_state = state->as<ompl::base::SE2StateSpace::StateType>();
+  double x = se2_state->getX();
+  double y = se2_state->getY();
+  double yaw = se2_state->getYaw();
+
+  collision_checker_->updateState(x, y, yaw);
+
+  bool has_collisions = collision_checker_->checkCollisions();
+
+  return is_state_valid and not has_collisions;
+}
+} // namespace Planning
+
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
index 8ccccfc2..de35901f 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
@@ -162,6 +162,28 @@ void CollisionChecker::updatePointCloud(const std::vector<Path::Point> &cloud,
   convertPointCloudToOctomap(cloud);
 }
 
+void CollisionChecker::update3DMap(const std::vector<Eigen::Vector3f> &points,
+                                   const bool global_frame) {
+  if (global_frame) {
+    sensor_tf_world_ = Eigen::Isometry3f::Identity();
+  } else {
+    // Transform the sensor position to the world frame
+    sensor_tf_world_ = body->tf * sensor_tf_body_;
+  }
+
+  // Clear old data
+  octTree_->clear();
+
+  octomapCloud_.clear();
+  for (auto &point : points) {
+    octomapCloud_.push_back(point.x(), point.y(), point.z());
+  }
+
+  octTree_->insertPointCloud(octomapCloud_, octomap::point3d(0, 0, 0));
+
+  updateOctreePtr();
+}
+
 void CollisionChecker::convertPointCloudToOctomap(
     const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, const bool global_frame) {
 
diff --git a/tests/test_ompl.py b/tests/test_ompl.py
index 48d91395..caf8e6c4 100644
--- a/tests/test_ompl.py
+++ b/tests/test_ompl.py
@@ -108,7 +108,7 @@ def load_map_meta(map_file: str) -> Dict:
         raise Exception(f"Failed to load map metadata: {str(e)}") from e
 
 
-def ompl_geometric_testing(test_repetitions: int = 1):
+def ompl_geometric_testing(test_repetitions: int = 10):
     """
     Test all OMPL geometric planners
 
@@ -117,14 +117,13 @@ def ompl_geometric_testing(test_repetitions: int = 1):
     """
     try:
         import pandas as pd
+
         ompl_df = pd.DataFrame(
             columns=(
                 "method",
                 "solved",
                 "solution_time",
                 "solution_len",
-                "simplification_time",
-                "time_convert_2_ros",
             )
         )
     except Exception:
@@ -150,7 +149,6 @@ def ompl_geometric_testing(test_repetitions: int = 1):
 
     for planner_id in ompl_planner.available_planners.keys():
         solution_time: float = 0.0
-        simplify_time: float = 0.0
         sol_len: float = 0.0
         if planner_id in ["ompl.geometric.AITstar", "ompl.geometric.LazyLBTRRT"]:
             continue
@@ -172,21 +170,9 @@ def ompl_geometric_testing(test_repetitions: int = 1):
 
                 solution_time += end_time - start_time
 
-                solved = True
+                solved = path is not None
                 if path:
-                    start_time = timeit.default_timer()
-
-                    path = ompl_planner.simplify_solution()
-
-                    end_time = timeit.default_timer()
-
-                    simplify_time += end_time - start_time
-
-                    solved = True
-
-                    sol_len += ompl_planner.solution.length()
-                else:
-                    solved = False
+                    sol_len += path.length()
 
             except Exception as e:
                 logging.error(f"{e}")
@@ -197,7 +183,6 @@ def ompl_geometric_testing(test_repetitions: int = 1):
                 "solved": solved,
                 "solution_time": solution_time / test_repetitions,
                 "solution_len": sol_len / test_repetitions,
-                "simplification_time": simplify_time / test_repetitions,
             }
     if ompl_df is not None:
         os.makedirs("logs", exist_ok=True)

From 163767ade45972c0d6382e7d733e93bc1d757cf2 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 27 Jun 2025 13:52:21 +0200
Subject: [PATCH 109/118] (feature) Implements FCL collision checker directly
 with OcTree data

---
 src/kompass_core/utils/visualization.py       |  4 +-
 .../include/utils/collision_check.h           | 19 ++++----
 .../kompass_cpp/src/utils/collision_check.cpp | 43 +++++++++----------
 .../tests/trajectory_sampler_plt.py           |  7 ++-
 tests/test_controllers.py                     | 10 +++--
 tests/test_laserscan_emergency_stop.py        |  5 ++-
 6 files changed, 46 insertions(+), 42 deletions(-)

diff --git a/src/kompass_core/utils/visualization.py b/src/kompass_core/utils/visualization.py
index ddaa9a2e..46a8fba9 100644
--- a/src/kompass_core/utils/visualization.py
+++ b/src/kompass_core/utils/visualization.py
@@ -29,9 +29,7 @@ def plt_map_obstacles(map: ObstaclesData, ax=None):
         ax.add_patch(circle)
 
 
-def plt_path_sample(
-    ref_path: PathSample, label="", color="blue", marker="", ax=None
-):
+def plt_path_sample(ref_path: PathSample, label="", color="blue", marker="", ax=None):
     """
     Plot a given path sample as a line
 
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
index 0d504be5..b49e0132 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
@@ -1,8 +1,6 @@
 #pragma once
 
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/Dense>
 #include <memory>
 #include <vector>
 
@@ -106,7 +104,7 @@ class CollisionChecker {
    * @param global_frame
    */
   void update3DMap(const std::vector<Eigen::Vector3f> &points,
-                        const bool global_frame = true);
+                   const bool global_frame = true);
 
   /**
    * @brief Update the sensor input from PointCloud like struct
@@ -255,13 +253,12 @@ class CollisionChecker {
   std::shared_ptr<octomap::OcTree>
       octTree_; // Octomap octree used to get data from laserscan
                 // or pointcloud and convert it to an Octree
-  std::unique_ptr<fcl::OcTreef> fclTree_ =
+  std::shared_ptr<fcl::OcTreef> fclTree_ =
       nullptr; // FCL Octree updated after converting the Octomap octree
   // (required for creating the collision object)
   octomap::Pointcloud octomapCloud_;
-  std::vector<fcl::CollisionObjectf *>
-      OctreeBoxes; // Vector of Boxes collision objects used to check
-                   // collisions with an octTree
+  std::unique_ptr<fcl::CollisionObjectf>
+      OctreeCollObj_; // Octree collision object
 
   double octree_resolution_{0.01};
 
@@ -272,14 +269,14 @@ class CollisionChecker {
   void updateOctreePtr();
 
   /**
-   * @brief Generates a vector of fcl::Box collision objects from an
+   * @brief Helper method to generate a vector of fcl::Box collision objects from an
    * fcl::Octree
    *
    * @param boxes
    * @param tree
    */
-  void generateBoxesFromOctomap(std::vector<fcl::CollisionObjectf *> &boxes,
-                                fcl::OcTreef &tree);
+  std::vector<fcl::CollisionObjectf *>
+  generateBoxesFromOctomap(fcl::OcTreef &tree);
 
   /**
    * @brief Helper method to convert PointCloud data to an Octomap
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
index de35901f..eae1d781 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/collision_check.cpp
@@ -1,7 +1,6 @@
 #include "utils/collision_check.h"
 #include "utils/logger.h"
 #include "utils/transformation.h"
-#include <Eigen/Dense>
 #include <algorithm>
 #include <cmath>
 #include <fcl/broadphase/default_broadphase_callbacks.h>
@@ -28,7 +27,7 @@ CollisionChecker::CollisionChecker(
 
   octree_resolution_ = octree_resolution;
   octTree_ = std::make_shared<octomap::OcTree>(octree_resolution_);
-  fclTree_ = std::make_unique<fcl::OcTreef>(octTree_);
+  fclTree_ = std::make_shared<fcl::OcTreef>(octTree_);
 
   body->shapeType = robot_shape_type;
 
@@ -74,13 +73,11 @@ void CollisionChecker::resetOctreeResolution(const double resolution) {
   }
 }
 
-float CollisionChecker::getRadius() const{
-  return robotRadius_;
-}
-
-void CollisionChecker::generateBoxesFromOctomap(
-    std::vector<fcl::CollisionObjectf *> &boxes, fcl::OcTreef &tree) {
+float CollisionChecker::getRadius() const { return robotRadius_; }
 
+std::vector<fcl::CollisionObjectf *>
+CollisionChecker::generateBoxesFromOctomap(fcl::OcTreef &tree) {
+  std::vector<fcl::CollisionObjectf *> boxes;
   // Turn OctTree nodes into boxes
   std::vector<std::array<float, 6>> boxes_ = tree.toBoxes();
 
@@ -108,19 +105,20 @@ void CollisionChecker::generateBoxesFromOctomap(
     obj->computeAABB();
     // Uncomment for debug
     // auto trans = obj->getTransform();
-    // std::cout << "Adding box with translation " << trans.translation() << std::endl;
+    // std::cout << "Adding box with translation " << trans.translation() <<
+    // std::endl;
     boxes.push_back(obj);
   }
 
   LOG_DEBUG("Total Generated boxes above: ", boxes.size());
+  return boxes;
 }
 
 void CollisionChecker::updateOctreePtr() {
   fclTree_.reset(new fcl::OcTreef(octTree_));
-
-  // Transform the tree into a set of boxes and generate collision objects in
-  // OctreeBoxes
-  generateBoxesFromOctomap(OctreeBoxes, *fclTree_);
+  OctreeCollObj_ =
+      std::make_unique<fcl::CollisionObjectf>(fclTree_, sensor_tf_world_);
+  OctreeCollObj_->computeAABB();
 }
 
 void CollisionChecker::updateState(const Path::State current_state) {
@@ -211,8 +209,8 @@ void CollisionChecker::convertPointCloudToOctomap(
     const std::vector<Path::Point> &cloud, const bool global_frame) {
 
   // Transform the sensor position to the world frame
-  // NOTE: Transformation will be applied to the points when generating the
-  // collision boxes
+  // NOTE: Transformation will be applied to the points when creating the
+  // collision object
   if (global_frame) {
     sensor_tf_world_ = Eigen::Isometry3f::Identity();
   } else {
@@ -238,8 +236,8 @@ void CollisionChecker::convertLaserScanToOctomap(
     double height) {
 
   // Transform the sensor position to the world frame
-  // NOTE: Transformation will be applied to the points when generating the
-  // collision boxes
+  // NOTE: Transformation will be applied to the points when creating the
+  // collision object
   sensor_tf_world_ = body->tf * sensor_tf_body_;
 
   // Clear old data
@@ -268,8 +266,8 @@ void CollisionChecker::convertLaserScanToOctomap(
     double height) {
 
   // Transform the sensor position to the world frame
-  // NOTE: Transformation will be applied to the points when generating the
-  // collision boxes
+  // NOTE: Transformation will be applied to the points when creating the
+  // collision object
   sensor_tf_world_ = body->tf * sensor_tf_body_;
 
   // Clear old data
@@ -298,7 +296,7 @@ bool CollisionChecker::checkCollisionsOctree() {
 
   collManager_->clear();
 
-  collManager_->registerObjects(OctreeBoxes);
+  collManager_->registerObject(OctreeCollObj_.get());
 
   collManager_->setup();
 
@@ -308,6 +306,7 @@ bool CollisionChecker::checkCollisionsOctree() {
   return collisionData.result.isCollision();
 
   // NOTE: Code below for testing box by box
+  // auto OctreeBoxes = generateBoxesFromOctomap(*fclTree_);
   // for (auto boxObj : OctreeBoxes) {
   //   collManager->clear();
 
@@ -348,7 +347,7 @@ float CollisionChecker::getMinDistance() {
 
   collManager_->clear();
 
-  collManager_->registerObjects(OctreeBoxes);
+  collManager_->registerObject(OctreeCollObj_.get());
 
   collManager_->setup();
 
@@ -416,7 +415,7 @@ bool CollisionChecker::checkCollisions(const Path::State current_state) {
       std::make_unique<fcl::DynamicAABBTreeCollisionManagerf>();
 
   m_collManager->clear();
-  m_collManager->registerObjects(OctreeBoxes);
+  m_collManager->registerObject(OctreeCollObj_.get());
   m_collManager->setup();
   m_collManager->collide(m_stateObjPtr.get(), &collisionData,
                          fcl::DefaultCollisionFunction);
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index 3a73acb0..cda272cb 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -1,5 +1,4 @@
 import json
-import matplotlib.pyplot as plt
 import argparse
 
 
@@ -30,6 +29,12 @@ def read_path_from_json(filename: str):
 
 
 def plot_samples(figure_name, trajectories, reference=None):
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError:
+        print("Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'")
+        return
+
     if reference:
         # Plot reference
         ref_path = reference["points"]
diff --git a/tests/test_controllers.py b/tests/test_controllers.py
index d84339a7..5b4e3262 100644
--- a/tests/test_controllers.py
+++ b/tests/test_controllers.py
@@ -554,9 +554,7 @@ def test_vision_rgb_follower():
     detections = [box]
 
     config = VisionRGBFollowerConfig(
-        control_time_step=control_time_step,
-        speed_gain=1.0,
-        rotation_gain=1.0
+        control_time_step=control_time_step, speed_gain=1.0, rotation_gain=1.0
     )
 
     controller = VisionRGBFollower(
@@ -581,7 +579,11 @@ def test_vision_rgb_follower():
 
     assert res
 
-    (vx, vy, omega) = controller.linear_x_control, controller.linear_y_control, controller.angular_control
+    (vx, vy, omega) = (
+        controller.linear_x_control,
+        controller.linear_y_control,
+        controller.angular_control,
+    )
     print(f"Found Control {vx}, {vy}, {omega}")
 
 
diff --git a/tests/test_laserscan_emergency_stop.py b/tests/test_laserscan_emergency_stop.py
index f66ee913..c0047e35 100644
--- a/tests/test_laserscan_emergency_stop.py
+++ b/tests/test_laserscan_emergency_stop.py
@@ -61,9 +61,12 @@ def test_laserscan_polar_tf(laser_scan_data: LaserScanData, plot: bool = False):
     if plot:
         try:
             import matplotlib.pyplot as plt
+
             fig, ax = plt.subplots(subplot_kw={"projection": "polar"})
             ax.plot(
-                laser_scan_data.angles, laser_scan_data.ranges, label="Original LaserScan"
+                laser_scan_data.angles,
+                laser_scan_data.ranges,
+                label="Original LaserScan",
             )
             ax.plot(
                 transformed_scan.angles,

From bf716dc53c74cf07e535c06567437a239465351f Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 27 Jun 2025 13:57:50 +0200
Subject: [PATCH 110/118] (feature) Removes python-fcl dependency and maintains
 all FCL collision checking in kompass_cpp

---
 pyproject.toml                                |   1 -
 src/kompass_core/third_party/fcl/__init__.py  |   0
 .../third_party/fcl/collisions.py             | 115 ------------------
 src/kompass_core/third_party/fcl/config.py    |  34 ------
 4 files changed, 150 deletions(-)
 delete mode 100644 src/kompass_core/third_party/fcl/__init__.py
 delete mode 100644 src/kompass_core/third_party/fcl/collisions.py
 delete mode 100644 src/kompass_core/third_party/fcl/config.py

diff --git a/pyproject.toml b/pyproject.toml
index 3ef82d07..dbf9ccbf 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -22,7 +22,6 @@ keywords = ["robotics", "gpgpu", "navigation", "control", "planning", "mapping"]
 dependencies = [
     "numpy<=1.26.4",
     "numpy-quaternion>=2023.0.3",
-    "python-fcl-fork",
     "toml",
     "attrs>=23.2.0",
     "pyyaml",
diff --git a/src/kompass_core/third_party/fcl/__init__.py b/src/kompass_core/third_party/fcl/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/src/kompass_core/third_party/fcl/collisions.py b/src/kompass_core/third_party/fcl/collisions.py
deleted file mode 100644
index e3f9653f..00000000
--- a/src/kompass_core/third_party/fcl/collisions.py
+++ /dev/null
@@ -1,115 +0,0 @@
-from typing import Optional
-
-import fcl
-import numpy as np
-
-from ...models import RobotState
-
-from .config import FCLConfig, fcl_object_geometry
-
-
-class FCL:
-    def __init__(
-        self, config: Optional[FCLConfig] = None, map_3d: Optional[np.ndarray] = None
-    ) -> None:
-        """
-        Setup a handler using the Flexible Collision Library (FCL)
-
-        :param config: FCL configuration parameters
-        :type config: FCLConfig
-
-        :param map_3d: 3D array for map PointCloud data, defaults to None
-        :type map_3d: np.ndarray | None, optional
-
-        :raises Exception: FCL exceptions
-        """
-        try:
-            if config:
-                self._config = config
-            else:
-                self._config = FCLConfig()
-
-            self._setup_from_config()
-
-            if map_3d is not None:
-                self.set_map(map_3d)
-        except Exception as e:
-            raise Exception(f"FCL geometry setup error: {e}") from e
-
-    def _setup_from_config(self):
-        """
-        Setup fcl from provided config
-        """
-        self._robot_geometry = fcl_object_geometry[self._config.robot_geometry_type](
-            *tuple(self._config.robot_geometry_params)
-        )
-        self._map_resolution = self._config.map_resolution
-
-        self._collision_manager = fcl.DynamicAABBTreeCollisionManager()
-
-        self.got_map = False
-
-    def configure(self, config_file: str, root_name: Optional[str] = None):
-        """
-        Load configuration from file
-
-        :param yaml_file: Path to config file (yaml, json, toml)
-        :type yaml_file: str
-        :param root_name: Parent root name of the config in the file 'Parent.fcl' - config must be under 'fcl', defaults to None
-        :type root_name: str | None, optional
-        """
-        if root_name:
-            nested_root_name = root_name + ".fcl"
-        else:
-            nested_root_name = "fcl"
-        self._config.from_file(config_file, nested_root_name)
-        self._setup_from_config()
-
-    def update_state(self, robot_state: RobotState):
-        """
-        Update robot collision object from new robot state
-
-        :param robot_state: Robot pose (x, y, yaw)
-        :type robot_state: RobotState
-        """
-        self._robot_object = fcl.CollisionObject(self._robot_geometry)
-
-        # Get translation and rotation from robot state
-        translation = np.array([robot_state.x, robot_state.y, 0.0])
-        c_t = np.cos(robot_state.yaw)
-        s_t = np.sin(robot_state.yaw)
-        R = np.array([[c_t, -s_t, 0.0], [s_t, c_t, 0.0], [0.0, 0.0, 1.0]])
-        transform = fcl.Transform(R, translation)
-
-        # Set transform to robot object
-        self._robot_object.setTransform(transform)
-
-    def set_map(self, map_3d: np.ndarray):
-        """
-        Set new map to the collision manager
-
-        :param map_3d: 3D array (PointCloud data)
-        :type map_3d: np.ndarray
-        """
-        map_octree = fcl.OcTree(self._map_resolution, points=map_3d)
-        self._map_object = fcl.CollisionObject(map_octree)
-        self.got_map = True
-
-    def check_collision(self) -> bool:
-        """
-        Check collision between the robot and the map obstacles
-
-        :return: If any collisions are found
-        :rtype: bool
-        """
-        # Clear olf robot pose and old map data from the collision manager
-        self._collision_manager.clear()
-
-        # Register up to dat data to manager
-        self._collision_manager.registerObjects([self._map_object, self._robot_object])
-        self._collision_manager.setup()
-
-        # Get collision between objects registered in the manager
-        collision_data = fcl.CollisionData()
-        self._collision_manager.collide(collision_data, fcl.defaultCollisionCallback)
-        return collision_data.result.is_collision
diff --git a/src/kompass_core/third_party/fcl/config.py b/src/kompass_core/third_party/fcl/config.py
deleted file mode 100644
index cbab71b0..00000000
--- a/src/kompass_core/third_party/fcl/config.py
+++ /dev/null
@@ -1,34 +0,0 @@
-from typing import Union, Tuple
-
-import fcl
-from attrs import define, field
-from ...utils.common import BaseAttrs, base_validators
-
-from ...models import RobotGeometry
-
-# Mapping from RobotGeometry to FCL Geometry
-fcl_object_geometry = {
-    RobotGeometry.Type.BOX: fcl.Box,  # (x, y, z) Axis-aligned box with given side lengths
-    RobotGeometry.Type.SPHERE: fcl.Sphere,  # (rad) Sphere with given radius
-    RobotGeometry.Type.ELLIPSOID: fcl.Ellipsoid,  # (x, y, z) Axis-aligned ellipsoid with given radis
-    RobotGeometry.Type.CAPSULE: fcl.Capsule,  # (rad, lz) Capsule with given radius and height along z-axis
-    RobotGeometry.Type.CONE: fcl.Cone,  # (rad, lz) Cone with given radius and cylinder height along z-axis
-    RobotGeometry.Type.CYLINDER: fcl.Cylinder,  # (rad, lz) Cylinder with given radius and height along z-axis
-}
-
-
-@define
-class FCLConfig(BaseAttrs):
-    """
-    FCL parameters
-    """
-
-    map_resolution: float = field(
-        default=0.01, validator=base_validators.in_range(min_value=1e-9, max_value=1e9)
-    )
-
-    robot_geometry_type: Union[str, RobotGeometry.Type] = field(
-        default=RobotGeometry.Type.BOX,
-        converter=lambda value: RobotGeometry.Type.from_str(value),
-    )
-    robot_geometry_params: Tuple[float] = field(default=(1.0, 1.0, 1.0))

From 35a0ae4fb5bd1ee758a3e8fc0bb8470593b069c9 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Fri, 27 Jun 2025 15:48:35 +0200
Subject: [PATCH 111/118] (refactor) Removes numpy-quaternion dependency and
 implements transformations using numpy

---
 pyproject.toml                                |  1 -
 src/kompass_core/datatypes/pose.py            | 43 ++++++-------
 src/kompass_core/utils/geometry.py            | 61 +++++++++++++------
 .../tests/trajectory_sampler_plt.py           |  4 +-
 4 files changed, 63 insertions(+), 46 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index dbf9ccbf..548e072c 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -21,7 +21,6 @@ keywords = ["robotics", "gpgpu", "navigation", "control", "planning", "mapping"]
 
 dependencies = [
     "numpy<=1.26.4",
-    "numpy-quaternion>=2023.0.3",
     "toml",
     "attrs>=23.2.0",
     "pyyaml",
diff --git a/src/kompass_core/datatypes/pose.py b/src/kompass_core/datatypes/pose.py
index 7f44fe5c..c4e3a03b 100644
--- a/src/kompass_core/datatypes/pose.py
+++ b/src/kompass_core/datatypes/pose.py
@@ -1,35 +1,30 @@
 from typing import Union
 
 import numpy as np
-import quaternion
-from quaternion import quaternion as quat
 
 
 def _equal_approx(
-    v1, v2, is_quaternion=False, absolute_tolerance=0.01
+    v1: np.ndarray,
+    v2: np.ndarray,
+    is_quaternion: bool = False,
+    absolute_tolerance: float = 0.01,
 ) -> Union[bool, np.bool_]:
     """
-        check if two vector/quaternion arrays are approximately equals
-        to within absolute_tolerance
-
-    :param      v1:                 vector 1
-    :type       v1:                 numpy/ numpy-quaternion
-    :param      v2:                 vector 2
-    :type       v2:                 vector numpy / numpy-quaternion
-    :param      is_quaternion:      is it quaternion check?, defaults to False
-    :type       is_quaternion:      bool, optional
-    :param      absolute_tolerance: tolerate some difference if exist, defaults to 0.01
-    :type       absolute_tolerance: float, optional
-
-    :return:    is both vectors are equal?
-    :rtype:     bool
+    Check if two vectors or quaternions are approximately equal within a tolerance.
+
+    :param v1: First vector or quaternion [w, x, y, z]
+    :param v2: Second vector or quaternion
+    :param is_quaternion: Set to True if comparing quaternions
+    :param absolute_tolerance: Absolute tolerance for comparison
+    :return: True if approximately equal
     """
     if is_quaternion:
-        equal = quaternion.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance)
+        # q and -q represent the same rotation
+        return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance) or np.allclose(
+            v1, -v2, rtol=0.0, atol=absolute_tolerance
+        )
     else:
-        equal = np.allclose(a=v1, b=v2, rtol=0.0, atol=absolute_tolerance)
-
-    return equal
+        return np.allclose(v1, v2, rtol=0.0, atol=absolute_tolerance)
 
 
 class PoseData:
@@ -134,14 +129,14 @@ def get_position(self) -> np.ndarray:
         """
         return np.array([self.x, self.y, self.z], dtype=np.float32)
 
-    def get_orientation(self) -> quaternion.quaternion:
+    def get_orientation(self) -> np.ndarray:
         """
         Get the orientation represented as quaternion
 
         :return: orientation represented as quaternion
-        :rtype: quaternion.quaternion
+        :rtype: np.ndarray
         """
-        return quat(self.qw, self.qx, self.qy, self.qz)
+        return np.array([self.qw, self.qx, self.qy, self.qz])
 
     def get_yaw(self) -> np.float64:
         """
diff --git a/src/kompass_core/utils/geometry.py b/src/kompass_core/utils/geometry.py
index 40b40349..c15dc681 100644
--- a/src/kompass_core/utils/geometry.py
+++ b/src/kompass_core/utils/geometry.py
@@ -2,8 +2,6 @@
 from typing import List, Union, Tuple
 
 import numpy as np
-import quaternion
-from quaternion import quaternion as quat
 
 from ..datatypes.pose import PoseData
 from ..datatypes.laserscan import LaserScanData
@@ -81,21 +79,41 @@ def probability_of_collision(
         return prop_col
 
 
-def _rotate_vector_by_quaternion(q: quaternion.quaternion, v: List) -> List:
+def _np_quaternion_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
     """
-    rotate a vector v by a rotation quaternion q
+    Multiplies two quaternions q1 * q2
+    Each quaternion is an array [w, x, y, z]
+    """
+    w0, x0, y0, z0 = q1
+    w1, x1, y1, z1 = q2
+    return np.array([
+        w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1,
+        w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1,
+        w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1,
+        w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1,
+    ])
+
+
+def _np_quaternion_conjugate(q: np.ndarray) -> np.ndarray:
+    """
+    Returns the conjugate of a quaternion
+    """
+    w, x, y, z = q
+    return np.array([w, -x, -y, -z])
+
 
-    :param      q: the rotation to perform
-    :type       q: quaternion.quaternion
-    :param      v: the vector to be rotated
-    :type       v: List
+def _rotate_vector_by_quaternion(q: np.ndarray, v: List[float]) -> List[float]:
+    """
+    Rotate a 3D vector v by a quaternion q.
 
-    :return:    the rotated position of the vector
-    :rtype:     List
+    :param      q: quaternion [w, x, y, z]
+    :param      v: vector [x, y, z]
+    :return:    rotated vector
     """
-    vq = quat(0, 0, 0, 0)
-    vq.imag = v
-    return (q * vq * q.inverse()).imag
+    vq = np.array([0.0, *v])
+    q_conj = _np_quaternion_conjugate(q)
+    rotated_vq = _np_quaternion_multiply(_np_quaternion_multiply(q, vq), q_conj)
+    return rotated_vq[1:].tolist()
 
 
 def get_pose_target_in_reference_frame(
@@ -119,10 +137,13 @@ def get_pose_target_in_reference_frame(
     reference_position = reference_pose.get_position()
     reference_rotation = reference_pose.get_orientation()
 
-    orientation_target_in_ref = reference_rotation.inverse() * orientation_target
+    orientation_target_in_ref = (
+        _np_quaternion_conjugate(reference_rotation) * orientation_target
+    )
 
     position_target_in_ref = _rotate_vector_by_quaternion(
-        reference_rotation.inverse(), (position_target - reference_position).tolist()
+        _np_quaternion_conjugate(reference_rotation),
+        (position_target - reference_position).tolist(),
     )
 
     target_pose_in_ref = PoseData()
@@ -130,10 +151,10 @@ def get_pose_target_in_reference_frame(
         x=position_target_in_ref[0],
         y=position_target_in_ref[1],
         z=position_target_in_ref[2],
-        qw=orientation_target_in_ref.w,
-        qx=orientation_target_in_ref.x,
-        qy=orientation_target_in_ref.y,
-        qz=orientation_target_in_ref.z,
+        qw=orientation_target_in_ref[0],
+        qx=orientation_target_in_ref[1],
+        qy=orientation_target_in_ref[2],
+        qz=orientation_target_in_ref[3],
     )
 
     return target_pose_in_ref
@@ -209,7 +230,7 @@ def from_2d_to_PoseData(x: float, y: float, heading: float) -> PoseData:
     :rtype: PoseData
     """
     pose_data = PoseData()
-    quat_data: quat = from_euler_to_quaternion(heading, 0.0, 0.0)
+    quat_data: np.ndarray = from_euler_to_quaternion(heading, 0.0, 0.0)
     pose_data.set_position(x, y, 0.0)
     pose_data.set_orientation(
         qw=quat_data[0], qx=quat_data[1], qy=quat_data[2], qz=quat_data[3]
diff --git a/src/kompass_cpp/tests/trajectory_sampler_plt.py b/src/kompass_cpp/tests/trajectory_sampler_plt.py
index cda272cb..06815fc2 100755
--- a/src/kompass_cpp/tests/trajectory_sampler_plt.py
+++ b/src/kompass_cpp/tests/trajectory_sampler_plt.py
@@ -32,7 +32,9 @@ def plot_samples(figure_name, trajectories, reference=None):
     try:
         import matplotlib.pyplot as plt
     except ImportError:
-        print("Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'")
+        print(
+            "Matplotlib is not installed. Test figures will not be generated. To generate figures run 'pip install matplotlib'"
+        )
         return
 
     if reference:

From 3901d24986a483d7a799ab1570684cef1bd0391f Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Sat, 28 Jun 2025 16:43:24 +0200
Subject: [PATCH 112/118] (refactor) Updates motion models names

---
 pyproject.toml             |  2 +-
 src/kompass_core/models.py | 16 ++++++++--------
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/pyproject.toml b/pyproject.toml
index 548e072c..b76f10f5 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -20,7 +20,7 @@ license = { text = "MIT" }
 keywords = ["robotics", "gpgpu", "navigation", "control", "planning", "mapping"]
 
 dependencies = [
-    "numpy<=1.26.4",
+    "numpy",
     "toml",
     "attrs>=23.2.0",
     "pyyaml",
diff --git a/src/kompass_core/models.py b/src/kompass_core/models.py
index 44c61465..86205baf 100644
--- a/src/kompass_core/models.py
+++ b/src/kompass_core/models.py
@@ -1108,9 +1108,9 @@ def init_zero(cls, **kwargs):
 class RobotType(Enum):
     """RobotType."""
 
-    ACKERMANN = "ACKERMANN_ROBOT"
-    DIFFERENTIAL_DRIVE = "DIFFERENTIAL_DRIVE_ROBOT"
-    OMNI = "OMNI_ROBOT"
+    ACKERMANN = "ACKERMANN"
+    DIFFERENTIAL_DRIVE = "DIFFERENTIAL_DRIVE"
+    OMNI = "OMNI"
 
     @classmethod
     def values(cls) -> List[str]:
@@ -1148,17 +1148,17 @@ def to_kompass_cpp_lib(cls, value: str) -> kompass_cpp.control.ControlType:
         :return: Robot Type
         :rtype: kompass_cpp.control_types
         """
-        if value == "ACKERMANN_ROBOT":
+        if value == "ACKERMANN":
             return kompass_cpp.control.ControlType.ACKERMANN
-        if value == "DIFFERENTIAL_DRIVE_ROBOT":
+        if value == "DIFFERENTIAL_DRIVE":
             return kompass_cpp.control.ControlType.DIFFERENTIAL_DRIVE
         return kompass_cpp.control.ControlType.OMNI
 
 
 control_types = {
-    "ACKERMANN_ROBOT": AckermannControl,
-    "DIFFERENTIAL_DRIVE_ROBOT": DifferentialDriveControl,
-    "OMNI_ROBOT": OmniDirectionalControl,
+    "ACKERMANN": AckermannControl,
+    "DIFFERENTIAL_DRIVE": DifferentialDriveControl,
+    "OMNI": OmniDirectionalControl,
 }
 
 

From a147d0f0c3c3cbc7b3316be54aaa9db69cb4b1f3 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Sun, 29 Jun 2025 08:59:47 +0200
Subject: [PATCH 113/118] (fix) Fixes typos

---
 src/kompass_core/control/dwa.py          | 10 +++-------
 src/kompass_core/control/rgb_follower.py |  8 +++++---
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 860f6655..1101b7d3 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -214,20 +214,16 @@ def __init__(
         :type control_time_step: Optional[float], optional
         """
         # Init and configure the planner
-        if not config:
-            # Default config
-            config = DWAConfig()
+        self._config = config or DWAConfig()
 
         if config_file:
-            config.from_file(file_path=config_file, nested_root_name=config_root_name)
+            self._config.from_file(file_path=config_file, nested_root_name=config_root_name)
 
         if control_time_step:
-            config.control_time_step = control_time_step
+            self._config.control_time_step = control_time_step
 
         self._got_path = False
 
-        self._config = config
-
         self._planner = kompass_cpp.control.DWA(
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
diff --git a/src/kompass_core/control/rgb_follower.py b/src/kompass_core/control/rgb_follower.py
index 8fe16251..b2b13795 100644
--- a/src/kompass_core/control/rgb_follower.py
+++ b/src/kompass_core/control/rgb_follower.py
@@ -33,6 +33,7 @@ class VisionRGBFollowerConfig(BaseAttrs):
         min_vel (float): Minimum linear velocity allowed during target following (m/s).
         enable_search (bool): Whether to activate search behavior when the target is lost.
     """
+
     control_time_step: float = field(
         default=0.1, validator=base_validators.in_range(min_value=1e-4, max_value=1e6)
     )
@@ -164,6 +165,7 @@ class VisionRGBFollower(ControllerTemplate):
     omega = controller.angular_control
     ```
     """
+
     def __init__(
         self,
         robot: Robot,
@@ -189,7 +191,7 @@ def __init__(
         self._config = config or VisionRGBFollowerConfig()
 
         if config_file:
-            config.from_file(config_file, config_root_name, get_common=False)
+            self._config.from_file(config_file, config_root_name, get_common=False)
         self.__controller = RGBFollower(
             control_type=RobotType.to_kompass_cpp_lib(robot.robot_type),
             control_limits=ctrl_limits.to_kompass_cpp_lib(),
@@ -217,7 +219,7 @@ def dist_error(self) -> float:
         :return: Last distance error (m)
         :rtype: float
         """
-        return self._planner.get_errors()[0]
+        return self.__controller.get_errors()[0]
 
     @property
     def orientation_error(self) -> float:
@@ -226,7 +228,7 @@ def orientation_error(self) -> float:
         :return: Last orientation error (radians)
         :rtype: float
         """
-        return self._planner.get_errors()[1]
+        return self.__controller.get_errors()[1]
 
     def loop_step(
         self,

From 67691d1bb400942ab97aacb772636a3ac38d2efb Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Sun, 29 Jun 2025 09:31:41 +0200
Subject: [PATCH 114/118] (refactor) Adds type hints, typo fix and formatting

---
 src/kompass_core/control/_base_.py            | 16 ++---
 src/kompass_core/control/dvz.py               |  2 +-
 src/kompass_core/control/dwa.py               |  8 +--
 src/kompass_core/control/rgbd_follower.py     |  8 +--
 src/kompass_core/performance.py               |  6 +-
 src/kompass_core/utils/emergency_stop.py      |  2 +-
 .../bindings/bindings_planning.cpp            |  3 +-
 .../kompass_cpp/include/controllers/dwa.h     | 23 ++++---
 .../include/controllers/vision_dwa.h          | 11 ++--
 .../kompass_cpp/include/datatypes/control.h   | 33 +++++-----
 .../kompass_cpp/include/datatypes/path.h      | 18 +++---
 .../include/datatypes/trajectory.h            | 10 +++-
 .../include/utils/collision_check.h           |  4 +-
 .../include/utils/cost_evaluator.h            |  9 +--
 .../include/utils/trajectory_sampler.h        |  2 +-
 .../include/vision/depth_detector.h           | 13 ++--
 .../kompass_cpp/include/vision/tracker.h      | 60 ++++++++++---------
 .../kompass_cpp/src/controllers/dwa.cpp       | 32 +++++-----
 .../kompass_cpp/src/controllers/follower.cpp  | 15 ++---
 .../kompass_cpp/src/datatypes/path.cpp        | 40 +++++++------
 .../src/utils/trajectory_sampler.cpp          |  4 +-
 .../kompass_cpp/src/vision/depth_detector.cpp |  7 ++-
 src/kompass_cpp/tests/collisions_test.cpp     |  7 ++-
 23 files changed, 179 insertions(+), 154 deletions(-)

diff --git a/src/kompass_core/control/_base_.py b/src/kompass_core/control/_base_.py
index 1f2ca6cf..429e9516 100644
--- a/src/kompass_core/control/_base_.py
+++ b/src/kompass_core/control/_base_.py
@@ -1,5 +1,5 @@
 from abc import abstractmethod
-from typing import Optional, List
+from typing import Optional, List, Union
 import numpy as np
 import kompass_cpp
 from ..models import RobotState
@@ -95,7 +95,7 @@ def logging_info(self) -> str:
 
     @property
     @abstractmethod
-    def linear_x_control(self):
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -106,7 +106,7 @@ def linear_x_control(self):
 
     @property
     @abstractmethod
-    def linear_y_control(self):
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -117,7 +117,7 @@ def linear_y_control(self):
 
     @property
     @abstractmethod
-    def angular_control(self):
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
@@ -255,7 +255,7 @@ def tracked_state(self) -> Optional[RobotState]:
 
     @property
     @abstractmethod
-    def linear_x_control(self) -> List[float]:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -266,7 +266,7 @@ def linear_x_control(self) -> List[float]:
 
     @property
     @abstractmethod
-    def linear_y_control(self) -> List[float]:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -277,7 +277,7 @@ def linear_y_control(self) -> List[float]:
 
     @property
     @abstractmethod
-    def angular_control(self) -> List[float]:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
@@ -298,7 +298,7 @@ def distance_error(self) -> float:
         return target.crosstrack_error
 
     @property
-    def orientation_error(self) -> float:
+    def orientation_error(self) -> Union[float, np.ndarray]:
         """
         Getter of the path tracking orientation error (rad)
 
diff --git a/src/kompass_core/control/dvz.py b/src/kompass_core/control/dvz.py
index 8063215d..6e647f2a 100644
--- a/src/kompass_core/control/dvz.py
+++ b/src/kompass_core/control/dvz.py
@@ -130,7 +130,7 @@ def interpolated_path(self) -> kompass_cpp.types.Path:
         return self.__reference_cmd_generator.interpolated_path()
 
     @property
-    def tracked_state(self) -> RobotState:
+    def tracked_state(self) -> Optional[RobotState]:
         """
         Tracked state on the path
 
diff --git a/src/kompass_core/control/dwa.py b/src/kompass_core/control/dwa.py
index 1101b7d3..e125df76 100644
--- a/src/kompass_core/control/dwa.py
+++ b/src/kompass_core/control/dwa.py
@@ -1,5 +1,5 @@
 import logging
-from typing import Optional
+from typing import Optional, Union, List
 import numpy as np
 from attrs import Factory, define, field
 from ..datatypes.laserscan import LaserScanData
@@ -386,7 +386,7 @@ def result_cost(self) -> Optional[float]:
         return None
 
     @property
-    def linear_x_control(self) -> np.ndarray:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -398,7 +398,7 @@ def linear_x_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def linear_y_control(self) -> np.ndarray:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -410,7 +410,7 @@ def linear_y_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def angular_control(self) -> np.ndarray:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
diff --git a/src/kompass_core/control/rgbd_follower.py b/src/kompass_core/control/rgbd_follower.py
index 4fd60a2b..aac15d34 100644
--- a/src/kompass_core/control/rgbd_follower.py
+++ b/src/kompass_core/control/rgbd_follower.py
@@ -12,7 +12,7 @@
     TrajectoryVelocities2D,
     TrajectoryPath,
 )
-from typing import Optional, List
+from typing import Optional, List, Union
 import numpy as np
 import logging
 from ._base_ import ControllerTemplate
@@ -521,7 +521,7 @@ def result_cost(self) -> Optional[float]:
         return None
 
     @property
-    def linear_x_control(self) -> np.ndarray:
+    def linear_x_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last linear forward velocity control computed by the controller
 
@@ -533,7 +533,7 @@ def linear_x_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def linear_y_control(self) -> np.ndarray:
+    def linear_y_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter the last linear velocity lateral control computed by the controller
 
@@ -545,7 +545,7 @@ def linear_y_control(self) -> np.ndarray:
         return [0.0]
 
     @property
-    def angular_control(self) -> np.ndarray:
+    def angular_control(self) -> Union[List[float], np.ndarray]:
         """
         Getter of the last angular velocity control computed by the controller
 
diff --git a/src/kompass_core/performance.py b/src/kompass_core/performance.py
index 05f9c56b..d30b9df1 100644
--- a/src/kompass_core/performance.py
+++ b/src/kompass_core/performance.py
@@ -1,4 +1,4 @@
-from typing import Any, List
+from typing import Any, List, Optional
 
 import numpy as np
 from .datatypes.obstacles import ObstaclesData
@@ -18,7 +18,7 @@ class MotionResult:
     def __init__(self) -> None:
         self.time = []
         self.steer_cmds: List[float] = []
-        self.robot_path: PathSample = None
+        self.robot_path: Optional[PathSample] = None
         self.speed_cmd: List[float] = []
         self.ori_error: List[float] = []
         self.lat_error: List[float] = []
@@ -93,7 +93,7 @@ def vis_result(
         )
 
         # Plot test path
-        visualization.plt_path_points_List(test, color="red", ax=ax3)
+        visualization.plt_path_points_list(test, color="red", ax=ax3)
 
         # Plot robot initial state
         robot_footprint.plt_robot(
diff --git a/src/kompass_core/utils/emergency_stop.py b/src/kompass_core/utils/emergency_stop.py
index d3f99ed9..6b873b5c 100644
--- a/src/kompass_core/utils/emergency_stop.py
+++ b/src/kompass_core/utils/emergency_stop.py
@@ -1,4 +1,4 @@
-from typing import Optional, List
+from typing import Optional
 from logging import Logger
 from ..models import (
     Robot,
diff --git a/src/kompass_cpp/bindings/bindings_planning.cpp b/src/kompass_cpp/bindings/bindings_planning.cpp
index 15850a98..4d033249 100644
--- a/src/kompass_cpp/bindings/bindings_planning.cpp
+++ b/src/kompass_cpp/bindings/bindings_planning.cpp
@@ -7,7 +7,6 @@
 namespace py = nanobind;
 using namespace Kompass;
 
-
 // Utils submodule
 void bindings_planning(py::module_ &m) {
   auto m_planning = m.def_submodule("planning", "KOMPASS CPP Planning Module");
@@ -24,7 +23,7 @@ void bindings_planning(py::module_ &m) {
            py::arg("goal_x"), py::arg("goal_y"), py::arg("goal_yaw"),
            py::arg("map_3d"))
       .def("solve", &Planning::OMPL2DGeometricPlanner::solve,
-               py::arg("planning_timeout") = 1.0)
+           py::arg("planning_timeout") = 1.0)
       .def("get_solution", &Planning::OMPL2DGeometricPlanner::getPath)
       .def("set_space_bounds_from_map",
            &Planning::OMPL2DGeometricPlanner::setSpaceBoundsFromMap,
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
index 777938c8..852596ad 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/dwa.h
@@ -72,7 +72,8 @@ class DWA : public Follower {
                  const int maxNumThreads = 1);
 
   /**
-   * @brief Reset the resolution of the robot's local map, this is equivalent to the box size representing each obstacle after conversion
+   * @brief Reset the resolution of the robot's local map, this is equivalent to
+   * the box size representing each obstacle after conversion
    *
    * @param octreeRes
    */
@@ -111,7 +112,7 @@ class DWA : public Follower {
 
   template <typename T>
   Controller::Result computeVelocityCommand(const Velocity2D &global_vel,
-                                            const T &scan_points){
+                                            const T &scan_points) {
     TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
     Controller::Result finalResult;
     if (searchRes.isTrajFound) {
@@ -123,18 +124,18 @@ class DWA : public Follower {
       finalResult.status = Controller::Result::Status::NO_COMMAND_POSSIBLE;
     }
     return finalResult;
-                                            };
+  };
 
   template <typename T>
   TrajSearchResult computeVelocityCommandsSet(const Velocity2D &global_vel,
-                                              const T &scan_points){
+                                              const T &scan_points) {
     TrajSearchResult searchRes = findBestPath(global_vel, scan_points);
     // Update latest velocity command
     if (searchRes.isTrajFound) {
       latest_velocity_command_ = searchRes.trajectory.velocities.getFront();
     }
     return searchRes;
-                                              };
+  };
 
   std::tuple<MatrixXfR, MatrixXfR> getDebuggingSamples() const;
 
@@ -175,7 +176,7 @@ class DWA : public Follower {
    */
   template <typename T>
   TrajSearchResult findBestPath(const Velocity2D &global_vel,
-                                const T &scan_points){
+                                const T &scan_points) {
     // Throw an error if the global path is not set
     if (!currentPath) {
       throw std::invalid_argument("Pointer to global path is NULL. Cannot use "
@@ -185,11 +186,15 @@ class DWA : public Follower {
     // find closest segment to use in cost computation
     determineTarget();
 
-    if(rotate_in_place and currentTrackedTarget_->heading_error > goal_orientation_tolerance * 10.0){
+    if (rotate_in_place and currentTrackedTarget_->heading_error >
+                                goal_orientation_tolerance * 10.0) {
       // If the robot is rotating in place and the heading error is large, we
       // do not need to sample trajectories
       LOG_DEBUG("Rotating In Place ...");
-      auto trajectory = trajSampler->generateSingleSampleFromVel(Velocity2D(0.0, 0.0, - currentTrackedTarget_->heading_error * ctrlimitsParams.omegaParams.maxOmega / M_PI));
+      auto trajectory = trajSampler->generateSingleSampleFromVel(
+          Velocity2D(0.0, 0.0,
+                     -currentTrackedTarget_->heading_error *
+                         ctrlimitsParams.omegaParams.maxOmega / M_PI));
       return TrajSearchResult{trajectory, true, 0.0};
     }
 
@@ -208,7 +213,7 @@ class DWA : public Follower {
     // Evaluate the samples and get the sample with the minimum cost
     return trajCostEvaluator->getMinTrajectoryCost(samples_, currentPath.get(),
                                                    trackedRefPathSegment);
-                                };
+  };
 
 private:
   double max_forward_distance_ = 0.0;
diff --git a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
index 8d63406d..27b67a27 100644
--- a/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
+++ b/src/kompass_cpp/kompass_cpp/include/controllers/vision_dwa.h
@@ -26,8 +26,8 @@ class VisionDWA : public DWA, public RGBFollower {
   public:
     VisionDWAConfig() : RGBFollower::RGBFollowerConfig() {
       addParameter(
-        "control_horizon",
-        Parameter(2, 1, 1000, "Number of steps for applying the control"));
+          "control_horizon",
+          Parameter(2, 1, 1000, "Number of steps for applying the control"));
       addParameter(
           "prediction_horizon",
           Parameter(10, 1, 1000, "Number of steps for future prediction"));
@@ -219,8 +219,8 @@ class VisionDWA : public DWA, public RGBFollower {
                      const Bbox2D &target_box_2d, const float yaw = 0.0);
 
   Eigen::Vector2f getErrors() const {
-      return Eigen::Vector2f(dist_error_, orientation_error_);
-    }
+    return Eigen::Vector2f(dist_error_, orientation_error_);
+  }
 
 private:
   VisionDWAConfig config_;
@@ -247,7 +247,8 @@ class VisionDWA : public DWA, public RGBFollower {
    * @param tracking_pose
    * @return Velocity2D
    */
-  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose, const bool update_global_error = false);
+  Velocity2D getPureTrackingCtrl(const TrackedPose2D &tracking_pose,
+                                 const bool update_global_error = false);
 
   /**
    * @brief Get the Tracking Control Result based on object tracking and DWA
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
index 6550bd75..93003c51 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/control.h
@@ -17,11 +17,11 @@ class Pose3D {
 public:
   Pose3D(const Eigen::Vector3f &position, const Eigen::Vector4f &orientation)
       : position_(position), orientation_(orientation),
-        rotation_matrix_(orientation_.toRotationMatrix()){};
+        rotation_matrix_(orientation_.toRotationMatrix()) {};
 
   Pose3D(const Eigen::Vector3f &position, const Eigen::Quaternionf &orientation)
       : position_(position), orientation_(orientation),
-        rotation_matrix_(orientation.toRotationMatrix()){};
+        rotation_matrix_(orientation.toRotationMatrix()) {};
 
   /**
    * @brief Construct a new Pose3D object using 2D pose information
@@ -30,8 +30,9 @@ class Pose3D {
    * @param pose_y
    * @param pose_yaw
    */
-  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw)
-      {update(pose_x, pose_y, pose_yaw);};
+  Pose3D(const float &pose_x, const float &pose_y, const float &pose_yaw) {
+    update(pose_x, pose_y, pose_yaw);
+  };
 
   void setFrame(const std::string &frame_id) { frame_id_ = frame_id; }
 
@@ -89,12 +90,12 @@ class Pose3D {
     return std::atan2(rotation_matrix_(1, 0), rotation_matrix_(0, 0));
   };
 
-  void update(const float &pose_x, const float &pose_y, const float &pose_yaw){
+  void update(const float &pose_x, const float &pose_y, const float &pose_yaw) {
     position_ = {pose_x, pose_y, 0.0};
     setRotation(0.0, 0.0, pose_yaw);
   }
 
-  void setRotation(const float pitch, const float roll, const float yaw){
+  void setRotation(const float pitch, const float roll, const float yaw) {
     Eigen::AngleAxisf rotZ(yaw, Eigen::Vector3f::UnitZ());
     Eigen::AngleAxisf rotY(pitch, Eigen::Vector3f::UnitY());
     Eigen::AngleAxisf rotX(roll, Eigen::Vector3f::UnitX());
@@ -102,7 +103,7 @@ class Pose3D {
     rotation_matrix_ = orientation_.toRotationMatrix();
   }
 
-  protected :
+protected:
   Eigen::Vector3f position_;
   Eigen::Quaternionf orientation_;
   Eigen::Matrix3f rotation_matrix_;
@@ -144,19 +145,19 @@ class TrackedPose2D : public Pose3D {
 public:
   TrackedPose2D(const Eigen::Vector3f &position,
                 const Eigen::Vector4f &orientation, const Velocity2D &vel)
-      : Pose3D(position, orientation), vel_(vel){};
+      : Pose3D(position, orientation), vel_(vel) {};
 
   TrackedPose2D(const Eigen::Vector3f &position,
                 const Eigen::Quaternionf &orientation, const Velocity2D &vel)
-      : Pose3D(position, orientation), vel_(vel){};
+      : Pose3D(position, orientation), vel_(vel) {};
 
   TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
                 const Velocity2D &vel)
-      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel){};
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vel) {};
 
   TrackedPose2D(const float &pose_x, const float &pose_y, const float &pose_yaw,
                 const float &vx, const float &vy, const float &omega)
-      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega){};
+      : Pose3D(pose_x, pose_y, pose_yaw), vel_(vx, vy, omega) {};
 
   float v() const { return Eigen::Vector2f{vel_.vx(), vel_.vy()}.norm(); };
 
@@ -164,19 +165,21 @@ class TrackedPose2D : public Pose3D {
 
   void update(const float timeStep) {
     position_(0) +=
-        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) * timeStep;
+        (vel_.vx() * cos(this->yaw()) - vel_.vy() * sin(this->yaw())) *
+        timeStep;
     position_(1) +=
-        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) * timeStep;
+        (vel_.vx() * sin(this->yaw()) + vel_.vy() * cos(this->yaw())) *
+        timeStep;
     float yaw = this->yaw() + vel_.omega() * timeStep;
     setRotation(0.0, 0.0, yaw);
   }
 
-  void update(const Velocity2D& vel, const float timeStep) {
+  void update(const Velocity2D &vel, const float timeStep) {
     vel_ = vel;
     update(timeStep);
   }
 
-  float distance(const float x, const float y, const float z = 0.0) const{
+  float distance(const float x, const float y, const float z = 0.0) const {
     return sqrt(pow(position_.x() - x, 2) + pow(position_.y() - y, 2) +
                 pow(position_.z() - z, 2));
   }
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
index ee3fe241..b7702505 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/path.h
@@ -19,7 +19,7 @@ struct State {
 
   State(double poseX = 0.0, double poseY = 0.0, double PoseYaw = 0.0,
         double speedValue = 0.0)
-      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue){};
+      : x(poseX), y(poseY), yaw(PoseYaw), speed(speedValue) {};
 
   void update(const Kompass::Control::Velocity2D &vel, const float timeStep) {
     this->x +=
@@ -46,7 +46,7 @@ struct Path {
   // interpolation
   int max_segment_size{10};
 
-  Path(const Path& other) = default;
+  Path(const Path &other) = default;
 
   Path(const std::vector<Point> &points = {}, const size_t new_max_size = 10);
 
@@ -70,7 +70,7 @@ struct Path {
   Path getPart(const size_t start, const size_t end,
                const size_t max_part_size = 0) const;
 
-  void pushPoint(const Point& point);
+  void pushPoint(const Point &point);
 
   size_t getSize() const;
 
@@ -109,9 +109,7 @@ struct Path {
 
     Iterator(const Path &p, size_t idx) : path(p), index(idx) {}
 
-    Point operator*() const {
-      return path.getIndex(index);
-    }
+    Point operator*() const { return path.getIndex(index); }
 
     Iterator &operator++() {
       ++index;
@@ -131,10 +129,10 @@ struct Path {
   Iterator end() const { return Iterator(*this, current_size_); }
 
 private:
-  Eigen::VectorXf X_; // Vector of X coordinates
-  Eigen::VectorXf Y_; // Vector of Y coordinates
-  Eigen::VectorXf Z_; // Vector of Z coordinates
-  size_t current_size_{0}; // Current size of the path
+  Eigen::VectorXf X_;                   // Vector of X coordinates
+  Eigen::VectorXf Y_;                   // Vector of Y coordinates
+  Eigen::VectorXf Z_;                   // Vector of Z coordinates
+  size_t current_size_{0};              // Current size of the path
   size_t max_interpolation_iterations_; // Max number of iterations for
                                         // interpolation between two path points
   // Max interpolation distance and total path distance are updated from user
diff --git a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
index 6928c7a6..ae79fc2e 100644
--- a/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
+++ b/src/kompass_cpp/kompass_cpp/include/datatypes/trajectory.h
@@ -90,7 +90,8 @@ struct TrajectoryVelocities2D {
   // get the first element
   Velocity2D getFront() const {
     assert(numPointsPerTrajectory_ > 1 && "Velocities are empty");
-    return Velocity2D(vx(0), vy(0), omega(0)); };
+    return Velocity2D(vx(0), vy(0), omega(0));
+  };
   // get the last element
   Velocity2D getEnd() const {
     assert(numPointsPerTrajectory_ > 1 && "Velocities are empty");
@@ -211,7 +212,8 @@ struct TrajectoryPath {
   // get the first element
   Path::Point getFront() const {
     assert(numPointsPerTrajectory_ > 0 && "Path is empty");
-    return Path::Point(x(0), y(0), z(0)); };
+    return Path::Point(x(0), y(0), z(0));
+  };
   // get the last element
   Path::Point getEnd() const {
     assert(numPointsPerTrajectory_ > 0 && "Path is empty");
@@ -401,7 +403,9 @@ struct TrajectoryPathSamples {
 
   // Add a new path from a Path struct.
   void push_back(const Path::Path &path) {
-    assert(path.getSize() == numPointsPerTrajectory_ && "Path points vector must have size equivalent to numPointsPerTrajectory");
+    assert(path.getSize() == numPointsPerTrajectory_ &&
+           "Path points vector must have size equivalent to "
+           "numPointsPerTrajectory");
 
     pathIndex_++;
     for (size_t i = 0; i < numPointsPerTrajectory_; ++i) {
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
index b49e0132..79f9c7b6 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/collision_check.h
@@ -269,8 +269,8 @@ class CollisionChecker {
   void updateOctreePtr();
 
   /**
-   * @brief Helper method to generate a vector of fcl::Box collision objects from an
-   * fcl::Octree
+   * @brief Helper method to generate a vector of fcl::Box collision objects
+   * from an fcl::Octree
    *
    * @param boxes
    * @param tree
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
index c35ad07a..de4a7030 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/cost_evaluator.h
@@ -105,7 +105,7 @@ class CostEvaluator {
    */
   TrajSearchResult
   getMinTrajectoryCost(const std::unique_ptr<TrajectorySamples2D> &trajs,
-                       const Path::Path* reference_path,
+                       const Path::Path *reference_path,
                        const Path::Path &tracked_segment);
 
   /**
@@ -148,7 +148,9 @@ class CostEvaluator {
   };
 
   void setPointScan(const std::vector<Path::Point> &cloud,
-                    const Path::State &current_state, const float max_sensor_range, const float max_obstacle_cost_range_multiple = 3.0) {
+                    const Path::State &current_state,
+                    const float max_sensor_range,
+                    const float max_obstacle_cost_range_multiple = 3.0) {
     obstaclePointsX.clear();
     obstaclePointsY.clear();
     maxObstaclesDist = max_sensor_range / max_obstacle_cost_range_multiple;
@@ -229,8 +231,7 @@ class CostEvaluator {
    * @param trajectories
    * @param reference_path
    */
-  sycl::event goalCostFunc(const size_t trajs_size,
-                           const float ref_path_length,
+  sycl::event goalCostFunc(const size_t trajs_size, const float ref_path_length,
                            const double cost_weight);
 
   /**
diff --git a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
index 1fcb39bc..dc58d5d1 100644
--- a/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
+++ b/src/kompass_cpp/kompass_cpp/include/utils/trajectory_sampler.h
@@ -135,7 +135,7 @@ class TrajectorySampler {
 
   template <typename T>
   bool checkStatesFeasibility(const std::vector<Path::State> &states,
-                                const T &sensor_points);
+                              const T &sensor_points);
 
   size_t numTrajectories;
   size_t numPointsPerTrajectory;
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
index d2179c8b..3470788e 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/depth_detector.h
@@ -46,13 +46,15 @@ class DepthDetector {
                 const Eigen::Vector2f &principal_point,
                 const float depth_conversion_factor = 1e-3);
 
-  void updateBoxes(const Eigen::MatrixX<unsigned short> aligned_depth_img,
-                   const std::vector<Bbox2D> &detections, const std::optional<Path::State> &robot_state = std::nullopt);
+  void
+  updateBoxes(const Eigen::MatrixX<unsigned short> aligned_depth_img,
+              const std::vector<Bbox2D> &detections,
+              const std::optional<Path::State> &robot_state = std::nullopt);
 
   std::optional<std::vector<Bbox3D>> get3dDetections() const;
 
 private:
-  float cx_, cy_, fx_, fy_;         // Depth Image camera intrinsics
+  float cx_, cy_, fx_, fy_; // Depth Image camera intrinsics
   float minDepth_, maxDepth_, depthConversionFactor_;
   Eigen::MatrixX<unsigned short> alignedDepthImg_;
   Eigen::Isometry3f camera_in_body_tf_, body_in_world_tf_;
@@ -60,9 +62,10 @@ class DepthDetector {
 
   std::optional<Bbox3D> convert2Dboxto3Dbox(const Bbox2D &box2d);
 
-  static void calculateMAD(const std::vector<float>& depthValues, float& median, float& mad);
+  static void calculateMAD(const std::vector<float> &depthValues, float &median,
+                           float &mad);
 
   static float getMedian(const std::vector<float> &values);
 };
 
-  } // namespace Kompass
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
index b9f3f7be..0e3b5e23 100644
--- a/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
+++ b/src/kompass_cpp/kompass_cpp/include/vision/tracker.h
@@ -1,54 +1,56 @@
 #pragma once
 
+#include "datatypes/control.h"
+#include "datatypes/tracking.h"
+#include "utils/kalman_filter.h"
 #include <Eigen/Dense>
 #include <memory>
 #include <optional>
 #include <vector>
-#include "datatypes/tracking.h"
-#include "datatypes/control.h"
-#include "utils/kalman_filter.h"
-
-namespace  Kompass{
 
+namespace Kompass {
 
-class FeatureBasedBboxTracker{
-    public:
-      FeatureBasedBboxTracker(const float& time_step, const float& e_pos, const float& e_vel, const float& e_acc);
+class FeatureBasedBboxTracker {
+public:
+  FeatureBasedBboxTracker(const float &time_step, const float &e_pos,
+                          const float &e_vel, const float &e_acc);
 
-      static constexpr int StateSize = 9;
+  static constexpr int StateSize = 9;
 
-      using FeaturesVector = Eigen::Vector<float, 9>;
+  using FeaturesVector = Eigen::Vector<float, 9>;
 
-      bool setInitialTracking(const TrackedBbox3D& bBox);
+  bool setInitialTracking(const TrackedBbox3D &bBox);
 
-      bool setInitialTracking(const Bbox3D &bBox, const float yaw = 0.0);
+  bool setInitialTracking(const Bbox3D &bBox, const float yaw = 0.0);
 
-      bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
-                              const std::vector<Bbox3D> &detected_boxes, const float yaw = 0.0);
+  bool setInitialTracking(const int &pose_x_img, const int &pose_y_img,
+                          const std::vector<Bbox3D> &detected_boxes,
+                          const float yaw = 0.0);
 
-      bool trackerInitialized() const;
+  bool trackerInitialized() const;
 
-      bool updateTracking(const std::vector<Bbox3D> &detected_boxes);
+  bool updateTracking(const std::vector<Bbox3D> &detected_boxes);
 
-      std::optional<TrackedBbox3D> getRawTracking() const;
+  std::optional<TrackedBbox3D> getRawTracking() const;
 
-      std::optional<Eigen::MatrixXf> getTrackedState() const;
+  std::optional<Eigen::MatrixXf> getTrackedState() const;
 
-      std::optional<Control::TrackedPose2D> getFilteredTrackedPose2D() const;
+  std::optional<Control::TrackedPose2D> getFilteredTrackedPose2D() const;
 
-    private:
-      float timeStep_, minAcceptedSimilarityScore_ = 0.0;
-      std::string trackedLabel_;
-      std::unique_ptr<TrackedBbox3D> trackedBox_;
-      std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
+private:
+  float timeStep_, minAcceptedSimilarityScore_ = 0.0;
+  std::string trackedLabel_;
+  std::unique_ptr<TrackedBbox3D> trackedBox_;
+  std::unique_ptr<LinearSSKalmanFilter> stateKalmanFilter_;
 
-      FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const;
+  FeaturesVector extractFeatures(const TrackedBbox3D &bBox) const;
 
-      FeaturesVector extractFeatures(const Bbox3D &bBox) const;
+  FeaturesVector extractFeatures(const Bbox3D &bBox) const;
 
-      Eigen::Vector3f computePointsStdDev(const std::vector<Eigen::Vector3f> &pc_points) const;
+  Eigen::Vector3f
+  computePointsStdDev(const std::vector<Eigen::Vector3f> &pc_points) const;
 
-      void updateTrackedBoxState(const int numberSteps = 1);
+  void updateTrackedBoxState(const int numberSteps = 1);
 };
 
-}
+} // namespace Kompass
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
index 62f47bec..02ccd52b 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/dwa.cpp
@@ -16,15 +16,15 @@ DWA::DWA(ControlLimitsParams controlLimits, ControlType controlType,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
          const Eigen::Vector3f &sensor_position_body,
-         const  Eigen::Vector4f &sensor_rotation_body, const double octreeRes,
+         const Eigen::Vector4f &sensor_rotation_body, const double octreeRes,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler and cost evaluator
   configure(controlLimits, controlType, timeStep, predictionHorizon,
-            controlHorizon, maxLinearSamples, maxAngularSamples,
-            robotShapeType, robotDimensions, sensor_position_body,
-            sensor_rotation_body, octreeRes, costWeights, maxNumThreads);
+            controlHorizon, maxLinearSamples, maxAngularSamples, robotShapeType,
+            robotDimensions, sensor_position_body, sensor_rotation_body,
+            octreeRes, costWeights, maxNumThreads);
 
   // Update the max forward distance the robot can make
   if (controlType == ControlType::OMNI) {
@@ -43,14 +43,14 @@ DWA::DWA(TrajectorySampler::TrajectorySamplerParameters config,
          const CollisionChecker::ShapeType robotShapeType,
          const std::vector<float> robotDimensions,
          const Eigen::Vector3f &sensor_position_body,
-         const  Eigen::Vector4f &sensor_rotation_body,
+         const Eigen::Vector4f &sensor_rotation_body,
          CostEvaluator::TrajectoryCostsWeights costWeights,
          const int maxNumThreads)
     : Follower() {
   // Setup the trajectory sampler and cost evaluator
-  configure(config, controlLimits, controlType, robotShapeType,
-            robotDimensions, sensor_position_body, sensor_rotation_body,
-            costWeights, maxNumThreads);
+  configure(config, controlLimits, controlType, robotShapeType, robotDimensions,
+            sensor_position_body, sensor_rotation_body, costWeights,
+            maxNumThreads);
 
   // Update the max forward distance the robot can make
   double timeHorizon = config.getParameter<double>("control_horizon");
@@ -71,7 +71,8 @@ void DWA::initJitCompile() {
   TrajectoryVelocitySamples2D velocities(dummyNumSamples, dummyNumPoints);
   TrajectoryPathSamples paths(dummyNumSamples, dummyNumPoints);
   velocities.push_back({Velocity2D(1.0, 0.0, 0.0)});
-  auto dummyPath = Path::Path({Path::Point(0.0f, 0.0f, 0.0f), Path::Point(1.0f, 1.0f, 0.0f)});
+  auto dummyPath = Path::Path(
+      {Path::Point(0.0f, 0.0f, 0.0f), Path::Point(1.0f, 1.0f, 0.0f)});
   paths.push_back(dummyPath);
   std::unique_ptr<TrajectorySamples2D> dummySamples =
       std::make_unique<TrajectorySamples2D>(velocities, paths);
@@ -85,14 +86,15 @@ void DWA::configure(ControlLimitsParams controlLimits, ControlType controlType,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
                     const Eigen::Vector3f &sensor_position_body,
-                    const  Eigen::Vector4f &sensor_rotation_body,
+                    const Eigen::Vector4f &sensor_rotation_body,
                     const double octreeRes,
                     CostEvaluator::TrajectoryCostsWeights costWeights,
                     const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
       controlLimits, controlType, timeStep, predictionHorizon, controlHorizon,
       maxLinearSamples, maxAngularSamples, robotShapeType, robotDimensions,
-      sensor_position_body, Eigen::Quaternionf(sensor_rotation_body), octreeRes, maxNumThreads);
+      sensor_position_body, Eigen::Quaternionf(sensor_rotation_body), octreeRes,
+      maxNumThreads);
 
   trajCostEvaluator = std::make_unique<CostEvaluator>(
       costWeights, sensor_position_body,
@@ -107,7 +109,7 @@ void DWA::configure(TrajectorySampler::TrajectorySamplerParameters config,
                     const CollisionChecker::ShapeType robotShapeType,
                     const std::vector<float> robotDimensions,
                     const Eigen::Vector3f &sensor_position_body,
-                    const  Eigen::Vector4f &sensor_rotation_body,
+                    const Eigen::Vector4f &sensor_rotation_body,
                     CostEvaluator::TrajectoryCostsWeights costWeights,
                     const int maxNumThreads) {
   trajSampler = std::make_unique<TrajectorySampler>(
@@ -152,13 +154,15 @@ Path::Path DWA::findTrackedPathSegment() {
       current_segment_index_ < max_segment_index_) {
     segment_index = current_segment_index_ + 1;
     return currentPath->segments[current_segment_index_ + 1];
-  } else if (closestPosition->index >= currentSegment.getSize() - 1){
+  } else if (closestPosition->index >= currentSegment.getSize() - 1) {
     // Return current segment directly (last segment)
     return currentPath->segments[current_segment_index_];
   }
   // Else take the segment points from the current point onwards
   else {
-    auto trackedPath = currentSegment.getPart(closestPosition->index, currentSegment.getSize() - 1, this->maxSegmentSize);
+    auto trackedPath = currentSegment.getPart(closestPosition->index,
+                                              currentSegment.getSize() - 1,
+                                              this->maxSegmentSize);
     size_t point_index{0};
 
     float segment_length = trackedPath.totalPathLength();
diff --git a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
index a3b09c5f..568a4513 100644
--- a/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/controllers/follower.cpp
@@ -206,8 +206,7 @@ Path::State Follower::projectPointOnSegment(const Path::Point &a,
 
 Path::PathPosition Follower::findClosestPointOnSegment(size_t segment_index) {
 
-  const Path::Path &segment_path =
-      currentPath->segments[segment_index];
+  const Path::Path &segment_path = currentPath->segments[segment_index];
   double min_distance = std::numeric_limits<double>::max();
   Path::State closest_point;
   double segment_position = 0.0; // in [0, 1]
@@ -260,13 +259,15 @@ void Follower::determineTarget() {
 
   currentTrackedTarget_ = std::make_unique<Target>();
   LOG_DEBUG("Closest point index on segment ", closestPosition->index,
-           " max index on segment is",
-           currentPath->segments[current_segment_index_].getSize() - 1,
-           " its segment length = ", closestPosition->segment_length);
+            " max index on segment is",
+            currentPath->segments[current_segment_index_].getSize() - 1,
+            " its segment length = ", closestPosition->segment_length);
   // If closest position is never updated
-  // OR If we reached end of a segment or end of the path -> Find new segment then new point on segment
+  // OR If we reached end of a segment or end of the path -> Find new segment
+  // then new point on segment
   if ((closestPosition->segment_length <= 0.0) ||
-      (closestPosition->index >= currentPath->segments[current_segment_index_].getSize() - 1) ||
+      (closestPosition->index >=
+       currentPath->segments[current_segment_index_].getSize() - 1) ||
       (closestPosition->segment_length >= 1.0)) {
     *closestPosition = findClosestPathPoint();
   }
diff --git a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
index 64a9d7bb..000257bd 100644
--- a/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/datatypes/path.cpp
@@ -47,20 +47,22 @@ Path::Path(const Eigen::VectorXf &x_points, const Eigen::VectorXf &y_points,
   Z_.head(current_size_) = z_points;
 }
 
-const Eigen::VectorXf Path::getX() const { return X_.segment(0, current_size_); }
-
-const Eigen::VectorXf Path::getY() const {return Y_.segment(0, current_size_);}
-
-const Eigen::VectorXf Path::getZ() const { return Z_.segment(0, current_size_); }
+const Eigen::VectorXf Path::getX() const {
+  return X_.segment(0, current_size_);
+}
 
-size_t Path::getSize() const {
-  return current_size_;
+const Eigen::VectorXf Path::getY() const {
+  return Y_.segment(0, current_size_);
 }
 
-void Path::setMaxLength(double max_length) {
-  max_path_length_ = max_length;
+const Eigen::VectorXf Path::getZ() const {
+  return Z_.segment(0, current_size_);
 }
 
+size_t Path::getSize() const { return current_size_; }
+
+void Path::setMaxLength(double max_length) { max_path_length_ = max_length; }
+
 void Path::resize(const size_t new_max_size) {
   max_size_ = new_max_size;
   X_.resize(max_size_);
@@ -78,9 +80,7 @@ bool Path::endReached(State currentState, double minDist) {
   return dist <= minDist;
 }
 
-size_t Path::getMaxSize() const{
-  return max_size_;
-}
+size_t Path::getMaxSize() const { return max_size_; }
 
 size_t Path::getMaxNumSegments() { return segments.size() - 1; }
 
@@ -88,13 +88,14 @@ Point Path::getEnd() const { return getIndex(current_size_ - 1); }
 
 Point Path::getStart() const { return getIndex(0); }
 
-Point Path::getIndex(const size_t index) const{
+Point Path::getIndex(const size_t index) const {
   assert(index < current_size_ && "Index out of range");
   return Point(X_(index), Y_(index), Z_(index));
 }
 
-Path Path::getPart(const size_t start, const size_t end, const size_t max_part_size) const{
-  if (start >= current_size_ || end >= current_size_ || start >= end ) {
+Path Path::getPart(const size_t start, const size_t end,
+                   const size_t max_part_size) const {
+  if (start >= current_size_ || end >= current_size_ || start >= end) {
     throw std::out_of_range("Invalid range for path part.");
   }
   auto part_size = std::max(max_part_size, end - start + 1);
@@ -105,7 +106,7 @@ Path Path::getPart(const size_t start, const size_t end, const size_t max_part_s
 }
 
 void Path::pushPoint(const Point &point) {
-  if(current_size_ >= max_size_) {
+  if (current_size_ >= max_size_) {
     throw std::out_of_range("Path is full. Cannot add more points.");
   }
   X_(current_size_) = point.x();
@@ -219,7 +220,7 @@ void Path::interpolate(double max_interpolation_point_dist,
   this->max_interpolation_dist_ = max_interpolation_point_dist;
   // Set the maximum size for the points
   auto maxSize = static_cast<size_t>(this->max_path_length_ /
-                                       this->max_interpolation_dist_);
+                                     this->max_interpolation_dist_);
   resize(maxSize);
   // Remaining iteration when interpolating the path (interpolation points
   // between each two path points)
@@ -311,7 +312,8 @@ void Path::segment(double pathSegmentLength) {
     new_segment.resize(this->max_segment_size);
     segments.push_back(new_segment);
   } else {
-    int segmentsNumber = max(static_cast<int>(totalLength / pathSegmentLength), 1);
+    int segmentsNumber =
+        max(static_cast<int>(totalLength / pathSegmentLength), 1);
     if (segmentsNumber == 1) {
       auto new_segment = *this;
       new_segment.resize(this->max_segment_size);
@@ -325,7 +327,7 @@ void Path::segment(double pathSegmentLength) {
 // Segment using a number of segments
 void Path::segmentBySegmentNumber(int numSegments) {
   segments.clear();
-  if (numSegments <= 0 || current_size_ <= 0 ) {
+  if (numSegments <= 0 || current_size_ <= 0) {
     throw std::invalid_argument(
         "Invalid number of segments or empty points vector.");
   }
diff --git a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
index b25bb830..19712a9f 100644
--- a/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/utils/trajectory_sampler.cpp
@@ -98,7 +98,7 @@ void TrajectorySampler::setSampleDroppingMode(const bool drop_samples) {
   this->drop_samples_ = drop_samples;
 }
 
-float TrajectorySampler::getRobotRadius() const{
+float TrajectorySampler::getRobotRadius() const {
   return collChecker->getRadius();
 }
 
@@ -516,7 +516,7 @@ bool TrajectorySampler::checkStatesFeasibility<LaserScan>(
   for (auto state : states) {
     collChecker->updateState(state);
     // Update the PointCloud values
-    if(collChecker->checkCollisions()){
+    if (collChecker->checkCollisions()) {
       return true;
     }
   }
diff --git a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
index 5394c7fd..0a16d1e7 100644
--- a/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
+++ b/src/kompass_cpp/kompass_cpp/src/vision/depth_detector.cpp
@@ -26,7 +26,7 @@ DepthDetector::DepthDetector(
     const Eigen::Isometry3f &camera_in_body_tf,
     const Eigen::Vector2f &focal_length, const Eigen::Vector2f &principal_point,
     const float depth_conversion_factor) { // Range of interest for depth values
-                                   // in meters
+                                           // in meters
   minDepth_ = depth_range(0);
   maxDepth_ = depth_range(1);
   // Factor to convert depth image data to meters (in ROS2 its given in mm ->
@@ -55,7 +55,7 @@ void DepthDetector::updateBoxes(
     const Eigen::MatrixX<unsigned short> aligned_depth_img,
     const std::vector<Bbox2D> &detections,
     const std::optional<Path::State> &robot_state) {
-  if(robot_state.has_value()) {
+  if (robot_state.has_value()) {
     body_in_world_tf_ = getTransformation(robot_state.value());
   }
   alignedDepthImg_ = aligned_depth_img;
@@ -115,7 +115,8 @@ std::optional<Bbox3D> DepthDetector::convert2Dboxto3Dbox(const Bbox2D &box2d) {
       this->fy_;
   center_in_camera_frame(2) = medianDepth;
 
-  LOG_DEBUG("Median depth = ", medianDepth, ", min=", minimum_d, ", max=", maximum_d);
+  LOG_DEBUG("Median depth = ", medianDepth, ", min=", minimum_d,
+            ", max=", maximum_d);
 
   // Size in meters
   size_camera_frame(0) = box2d.size.x() * medianDepth / this->fx_;
diff --git a/src/kompass_cpp/tests/collisions_test.cpp b/src/kompass_cpp/tests/collisions_test.cpp
index b4526505..25ac6e13 100644
--- a/src/kompass_cpp/tests/collisions_test.cpp
+++ b/src/kompass_cpp/tests/collisions_test.cpp
@@ -129,7 +129,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   initLaserscan(360, 10.0, scan_ranges, scan_angles);
 
   bool forward_motion = true;
-  float critical_angle = 160.0, critical_distance = 0.3, slowdown_distance = 0.6;
+  float critical_angle = 160.0, critical_distance = 0.3,
+        slowdown_distance = 0.6;
 
   CriticalZoneChecker zoneChecker(robotShapeType, robotDimensions,
                                   sensor_position_body, sensor_rotation_body,
@@ -144,8 +145,8 @@ BOOST_AUTO_TEST_CASE(test_critical_zone_check) {
   setLaserscanAtAngle(-0.1, 0.2, scan_ranges, scan_angles);
   float result = zoneChecker.check(scan_ranges, scan_angles, forward_motion);
   BOOST_TEST(result == 1.0, "Angles are behind and robot is moving forward -> "
-                      "Critical zone result should be 1.0, returned "
-                          << result);
+                            "Critical zone result should be 1.0, returned "
+                                << result);
   if (result == 1.0) {
     LOG_INFO("Test1 PASSED: Angles are behind and robot is moving forward");
   }

From ffa79844b57ad300e0499627c49894b34b08d4d3 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Sun, 29 Jun 2025 09:42:49 +0200
Subject: [PATCH 115/118] (fix) Fixes build wheel CI

---
 .github/workflows/build_and_deploy.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/build_and_deploy.yml b/.github/workflows/build_and_deploy.yml
index d3770d05..013dac59 100644
--- a/.github/workflows/build_and_deploy.yml
+++ b/.github/workflows/build_and_deploy.yml
@@ -28,7 +28,7 @@ jobs:
         run: echo "CIBW_ARCHS=${{ matrix.os-arch.arch }}" >> $GITHUB_ENV
 
       - name: Install Python development headers
-      - run: |
+        run: |
              PYTHON_VERSION=$(python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}')")
              sudo apt-get update
              sudo apt-get install -y python${PYTHON_VERSION}-dev

From 315f0dbf45b51f86a4eaaeb101f32d8456a53985 Mon Sep 17 00:00:00 2001
From: ahrasheed <ahrasheed@gmail.com>
Date: Mon, 30 Jun 2025 05:38:48 +0200
Subject: [PATCH 116/118] (chore) Removes arm builds from CI for testing

---
 .github/workflows/build_and_deploy.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/build_and_deploy.yml b/.github/workflows/build_and_deploy.yml
index 013dac59..eb69c98a 100644
--- a/.github/workflows/build_and_deploy.yml
+++ b/.github/workflows/build_and_deploy.yml
@@ -16,7 +16,7 @@ jobs:
       matrix:
         os-arch:
           - { os: ubuntu-latest, arch: x86_64 }
-          - { os: ubuntu-24.04-arm, arch: aarch64 }
+          # - { os: ubuntu-24.04-arm, arch: aarch64 }
 
     name: Build wheels on ${{ matrix.os-arch.os }} ${{ matrix.os-arch.arch }}
     runs-on: ${{ matrix.os-arch.os }}

From c60690600016cbe64ecbf366fdb09e0cb7d4e3bf Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Wed, 2 Jul 2025 00:41:30 +0800
Subject: [PATCH 117/118] (fix) Fixes geometry transformation utils

---
 src/kompass_core/mapping/local_mapper.py |  10 +--
 src/kompass_core/utils/geometry.py       | 106 +++++++++++------------
 2 files changed, 57 insertions(+), 59 deletions(-)

diff --git a/src/kompass_core/mapping/local_mapper.py b/src/kompass_core/mapping/local_mapper.py
index 55d346fc..adc83eee 100644
--- a/src/kompass_core/mapping/local_mapper.py
+++ b/src/kompass_core/mapping/local_mapper.py
@@ -9,7 +9,7 @@
     OCCUPANCY_TYPE,
 )
 
-from ..utils.geometry import from_frame1_to_frame2, get_pose_target_in_reference_frame
+from ..utils.geometry import transform_point_from_local_to_global, get_relative_pose
 
 from .laserscan_model import LaserScanModelConfig
 from ..utils.common import BaseAttrs, base_validators
@@ -221,8 +221,8 @@ def _calculate_grid_shift(self, current_robot_pose: PoseData):
         # i.e. we have a t+1 state
         # get current shift in translation and orientation of the new center
         # with respect to the previous old center
-        pose_current_robot_in_previous_robot = get_pose_target_in_reference_frame(
-            reference_pose=self._pose_robot_in_world, target_pose=current_robot_pose
+        pose_current_robot_in_previous_robot = get_relative_pose(
+            pose_1_in_ref=self._pose_robot_in_world, pose_2_in_ref=current_robot_pose
         )
         # new position and orientation with respect to the previous pose
         _position_in_previous_pose = pose_current_robot_in_previous_robot.get_position()
@@ -264,8 +264,8 @@ def update_from_scan(
 
         # Calculate new grid pose
         self._pose_robot_in_world = robot_pose
-        self.lower_right_corner_pose = from_frame1_to_frame2(
-            robot_pose, self._local_lower_right_corner_point
+        self.lower_right_corner_pose = transform_point_from_local_to_global(
+            self._local_lower_right_corner_point, robot_pose
         )
 
         if self.config.baysian_update:
diff --git a/src/kompass_core/utils/geometry.py b/src/kompass_core/utils/geometry.py
index c15dc681..5de47bbe 100644
--- a/src/kompass_core/utils/geometry.py
+++ b/src/kompass_core/utils/geometry.py
@@ -116,74 +116,72 @@ def _rotate_vector_by_quaternion(q: np.ndarray, v: List[float]) -> List[float]:
     return rotated_vq[1:].tolist()
 
 
-def get_pose_target_in_reference_frame(
-    reference_pose: PoseData, target_pose: PoseData
-) -> PoseData:
+def _transform_pose(pose: PoseData, reference_pose: PoseData) -> PoseData:
     """
-    Computes a target pose with respect to the frame (coordinate system) defined by a reference.
-    target and reference should be given first in a common frame
-
-    :param      reference_pose:     Pose of reference frame in a common frame
-    :type       reference_pose:     PoseData
-    :param      target_pose:        Pose of target frame in a common frame
-    :type       target_pose:        PoseData
+    Transforms a pose from a local frame to a global frame using a reference pose.
+    Equivalent to: global_pose = reference_pose * local_pose
 
-    :return:    The pose of target in the reference frame
-    :rtype:     PoseData
+    :param pose: PoseData in local frame
+    :type pose: PoseData
+    :param reference_pose: PoseData of local frame in global frame
+    :type reference_pose: PoseData
+    :return: PoseData in global frame
+    :rtype: PoseData
     """
-    position_target = target_pose.get_position()
-    orientation_target = target_pose.get_orientation()
-
-    reference_position = reference_pose.get_position()
-    reference_rotation = reference_pose.get_orientation()
-
-    orientation_target_in_ref = (
-        _np_quaternion_conjugate(reference_rotation) * orientation_target
-    )
+    rotated_position = _rotate_vector_by_quaternion(reference_pose.get_orientation(), pose.get_position().tolist())
+    translated_position = reference_pose.get_position() + np.array(rotated_position)
 
-    position_target_in_ref = _rotate_vector_by_quaternion(
-        _np_quaternion_conjugate(reference_rotation),
-        (position_target - reference_position).tolist(),
-    )
+    combined_orientation = _np_quaternion_multiply(reference_pose.get_orientation(), pose.get_orientation())
 
-    target_pose_in_ref = PoseData()
-    target_pose_in_ref.set_pose(
-        x=position_target_in_ref[0],
-        y=position_target_in_ref[1],
-        z=position_target_in_ref[2],
-        qw=orientation_target_in_ref[0],
-        qx=orientation_target_in_ref[1],
-        qy=orientation_target_in_ref[2],
-        qz=orientation_target_in_ref[3],
+    result = PoseData()
+    result.set_pose(
+        *translated_position,
+        *combined_orientation
     )
+    return result
 
-    return target_pose_in_ref
 
+def _inverse_pose(pose: PoseData) -> PoseData:
+    """
+    Computes the inverse of a pose (converts pose_in_frame to frame_in_pose)
 
-def from_frame1_to_frame2(
-    pose_1_in_2: PoseData, pose_target_in_1: PoseData
-) -> PoseData:
+    :param pose: PoseData to invert
+    :type pose: PoseData
+    :return: Inverse pose
+    :rtype: PoseData
     """
-    get the pose of a target in frame 2 instead of frame 1
+    inv_orientation = _np_quaternion_conjugate(pose.get_orientation())
+    inv_position = _rotate_vector_by_quaternion(inv_orientation, (-pose.get_position()).tolist())
+
+    result = PoseData()
+    result.set_pose(
+        *inv_position,
+        *inv_orientation
+    )
+    return result
 
-    :param      pose_1_in_2:        pose of frame 1 in frame 2
-    :type       pose_1_in_2:        PoseData
-    :param      pose_target_in_1:   pose of target in frame 1
-    :type       pose_target_in_1:   PoseData
+def transform_point_from_local_to_global(point_pose_in_local: PoseData, robot_pose_in_global: PoseData) -> PoseData:
+    """Transforms a point from the robot frame to the global frame.
 
-    :return:    pose of target in frame 2
-    :rtype:     PoseData
+    :param point_pose_in_local: Target point in robot frame
+    :type point_pose_in_local: PoseData
+    :param robot_pose_in_global: Robot pose in global frame
+    :type robot_pose_in_global: PoseData
+    :return: Target point in global frame
+    :rtype: PoseData
     """
-    pose_2_origin = PoseData()
-    pose_2_in_1 = get_pose_target_in_reference_frame(
-        reference_pose=pose_1_in_2, target_pose=pose_2_origin
-    )
+    return _transform_pose(point_pose_in_local, robot_pose_in_global)
 
-    pose_target_in_2 = get_pose_target_in_reference_frame(
-        reference_pose=pose_2_in_1, target_pose=pose_target_in_1
-    )
+def get_relative_pose(pose_1_in_ref: PoseData, pose_2_in_ref: PoseData) -> PoseData:
+    """
+    Computes the pose of pose_2 in the coordinate frame of pose_1.
 
-    return pose_target_in_2
+    :param pose_1_in_ref: PoseData of frame 1 in reference frame
+    :param pose_2_in_ref: PoseData of frame 2 in reference frame
+    :return: PoseData of pose_2 in frame 1
+    """
+    pose_ref_in_1 = _inverse_pose(pose_1_in_ref)
+    return _transform_pose(pose_2_in_ref, pose_ref_in_1)
 
 
 def from_euler_to_quaternion(yaw: float, pitch: float, roll: float) -> np.ndarray:
@@ -275,7 +273,7 @@ def from_frame1_to_frame2_2d(
     )
 
     # Get transformd pose
-    pose_target_in_2: PoseData = from_frame1_to_frame2(pose_1_in_2, pose_target_in_1)
+    pose_target_in_2: PoseData = get_relative_pose(pose_1_in_2, pose_target_in_1)
 
     return pose_target_in_2
 

From 78c273c114af4478ac59a60730ae77be3aa4d5b4 Mon Sep 17 00:00:00 2001
From: mkabtoul <kabtoul.maria@gmail.com>
Date: Tue, 1 Jul 2025 18:48:13 +0200
Subject: [PATCH 118/118] (docs) Minor readme update

---
 README.md         | 1 -
 docs/README.ja.md | 2 --
 docs/README.zh.md | 3 ---
 3 files changed, 6 deletions(-)

diff --git a/README.md b/README.md
index cc816345..229c71f2 100644
--- a/README.md
+++ b/README.md
@@ -24,7 +24,6 @@ This package is developed to be used with [Kompass](https://github.com/automatik
 
 - [**Install**](#installation) Kompass Core 🛠️
 - Check the [**Package Overview**](#-package-overview)
-- [**Copyright**](#copyright) and [**Contributions**](#contributions)
 - To use Kompass Core on your robot with ROS2, check the [**Kompass**](https://automatika-robotics.github.io/kompass) framework 🚀
 
 
diff --git a/docs/README.ja.md b/docs/README.ja.md
index 47b3c611..65330da3 100644
--- a/docs/README.ja.md
+++ b/docs/README.ja.md
@@ -25,7 +25,6 @@ Kompass の理念は、「驚異的な高速性」と「高信頼性」を追求
 
 - [**インストール**](#installation) Kompass Core 🛠️
 - [**パッケージ概要**](#-package-overview) を確認
-- [**著作権**](#copyright) および [**コントリビューション**](#contributions)
 - ROS2 環境でロボットに Kompass Core を導入したい場合は、[**Kompass**](https://automatika-robotics.github.io/kompass) フレームワークをご参照ください 🚀
 
 # インストール方法
@@ -155,4 +154,3 @@ Kompass Core は MIT ライセンスのもとで公開されています。詳
 ## コントリビューション
 
 Kompass Core は [Automatika Robotics](https://automatikarobotics.com/) と [Inria](https://inria.fr/) の共同開発プロジェクトです。コミュニティからの貢献は大歓迎です。
-
diff --git a/docs/README.zh.md b/docs/README.zh.md
index 5f69b91d..d0d6f217 100644
--- a/docs/README.zh.md
+++ b/docs/README.zh.md
@@ -23,7 +23,6 @@ Kompass Core 是一个高性能、支持 GPU 加速的库，用于机器人导
 
 - [**安装指南**](#安装指南) Kompass Core 🛠️
 - 查看 [**软件包概览**](#-软件包概览)
-- [**版权信息**](#版权信息) 与 [**贡献说明**](#贡献说明)
 - 若需在 ROS2 中使用 Kompass Core，请访问 [**Kompass 框架**](https://automatika-robotics.github.io/kompass) 🚀
 
 # 安装指南
@@ -152,5 +151,3 @@ Kompass Core 遵循 MIT 开源许可证。详细信息请见 [LICENSE](LICENSE)
 ## 贡献说明
 
 Kompass Core 由 [Automatika Robotics](https://automatikarobotics.com/) 与 [Inria](https://inria.fr/) 合作开发，欢迎社区开发者贡献代码与文档。
-
-