fix: rollback tracking size and offset algorithm

Signed-off-by: Taekjin LEE <[email protected]>
technolojin · Mar 12, 2024 · c73878a · c73878a
1 parent a40814d
commit c73878a
Show file tree

Hide file tree

Showing 4 changed files with 66 additions and 12 deletions.
diff --git a/...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
@@ -52,6 +52,7 @@ class BigVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
+  int last_nearest_corner_index_;
 
 private:
   BicycleMotionModel motion_model_;
@@ -76,6 +77,16 @@ class BigVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
   virtual ~BigVehicleTracker() {}
+
+private:
+  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
+  {
+    Eigen::MatrixXd X_t(DIM, 1);
+    motion_model_.getStateVector(X_t);
+    last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
+      X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
+      self_transform);
+  }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
diff --git a/...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
@@ -53,6 +53,7 @@ class NormalVehicleTracker : public Tracker
   BoundingBox bounding_box_;
   BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
+  int last_nearest_corner_index_;
 
 private:
   BicycleMotionModel motion_model_;
@@ -77,6 +78,16 @@ class NormalVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObject & object) const override;
   virtual ~NormalVehicleTracker() {}
+
+private:
+  void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform)
+  {
+    Eigen::MatrixXd X_t(DIM, 1);
+    motion_model_.getStateVector(X_t);
+    last_nearest_corner_index_ = utils::getNearestCornerOrSurface(
+      X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length,
+      self_transform);
+  }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
@@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 BigVehicleTracker::BigVehicleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
+  const geometry_msgs::msg::Transform & self_transform)
 : Tracker(time, object.classification),
   logger_(rclcpp::get_logger("BigVehicleTracker")),
   // last_update_time_(time),
@@ -167,6 +167,9 @@ BigVehicleTracker::BigVehicleTracker(
     // initialize motion model
     motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
   }
+
+  /* calc nearest corner index*/
+  setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 }
 
 bool BigVehicleTracker::predict(const rclcpp::Time & time)
@@ -316,11 +319,12 @@ bool BigVehicleTracker::measureWithShape(
 
   // update motion model
   motion_model_.updateExtendedState(bounding_box_.length);
-  // update offset into object position
-  motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
-  // update offset
-  tracking_offset_.x() = gain_inv * tracking_offset_.x();
-  tracking_offset_.y() = gain_inv * tracking_offset_.y();
+
+  // // update offset into object position
+  // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
+  // // update offset
+  // tracking_offset_.x() = gain_inv * tracking_offset_.x();
+  // tracking_offset_.y() = gain_inv * tracking_offset_.y();
 
   return true;
 }
@@ -354,6 +358,9 @@ bool BigVehicleTracker::measure(
   measureWithPose(updating_object);
   measureWithShape(updating_object);
 
+  /* calc nearest corner index*/
+  setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
+
   return true;
 }
 
@@ -375,6 +382,15 @@ bool BigVehicleTracker::getTrackedObject(
     return false;
   }
 
+  // recover bounding box from tracking point
+  const double dl = bounding_box_.length - last_input_bounding_box_.length;
+  const double dw = bounding_box_.width - last_input_bounding_box_.width;
+  const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
+    pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
+    motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
+  pose_with_cov.pose.position.x = recovered_pose.x();
+  pose_with_cov.pose.position.y = recovered_pose.y();
+
   // position
   pose_with_cov.pose.position.z = z_;
 

diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
@@ -44,7 +44,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 NormalVehicleTracker::NormalVehicleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
+  const geometry_msgs::msg::Transform & self_transform)
 : Tracker(time, object.classification),
   logger_(rclcpp::get_logger("NormalVehicleTracker")),
   // last_update_time_(time),
@@ -167,6 +167,9 @@ NormalVehicleTracker::NormalVehicleTracker(
     // initialize motion model
     motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length);
   }
+
+  /* calc nearest corner index*/
+  setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
 }
 
 bool NormalVehicleTracker::predict(const rclcpp::Time & time)
@@ -316,11 +319,12 @@ bool NormalVehicleTracker::measureWithShape(
 
   // update motion model
   motion_model_.updateExtendedState(bounding_box_.length);
-  // update offset into object position
-  motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
-  // update offset
-  tracking_offset_.x() = gain_inv * tracking_offset_.x();
-  tracking_offset_.y() = gain_inv * tracking_offset_.y();
+
+  // // update offset into object position
+  // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y());
+  // // update offset
+  // tracking_offset_.x() = gain_inv * tracking_offset_.x();
+  // tracking_offset_.y() = gain_inv * tracking_offset_.y();
 
   return true;
 }
@@ -354,6 +358,9 @@ bool NormalVehicleTracker::measure(
   measureWithPose(updating_object);
   measureWithShape(updating_object);
 
+  /* calc nearest corner index*/
+  setNearestCornerOrSurfaceIndex(self_transform);  // this index is used in next measure step
+
   return true;
 }
 
@@ -375,6 +382,15 @@ bool NormalVehicleTracker::getTrackedObject(
     return false;
   }
 
+  // recover bounding box from tracking point
+  const double dl = bounding_box_.length - last_input_bounding_box_.length;
+  const double dw = bounding_box_.width - last_input_bounding_box_.width;
+  const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint(
+    pose_with_cov.pose.position.x, pose_with_cov.pose.position.y,
+    motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_);
+  pose_with_cov.pose.position.x = recovered_pose.x();
+  pose_with_cov.pose.position.y = recovered_pose.y();
+
   // position
   pose_with_cov.pose.position.z = z_;