fix: update object position offset caused by size change

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp

@@ -51,7 +51,6 @@ 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,16 +75,6 @@ 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_

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp

@@ -51,7 +51,6 @@ 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_;
@@ -76,16 +75,6 @@ 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_

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")),
   z_(object.kinematics.pose_with_covariance.pose.position.z),
@@ -166,9 +166,6 @@ 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)
@@ -319,11 +316,19 @@ 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
+  {
+    // rotate back the offset vector from object coordinate to global coordinate
+    const double yaw = motion_model_.getStateElement(IDX::YAW);
+    const double offset_x_global =
+      tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw);
+    const double offset_y_global =
+      tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw);
+    motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global);
+    // update offset (object coordinate)
+    tracking_offset_.x() = gain_inv * tracking_offset_.x();
+    tracking_offset_.y() = gain_inv * tracking_offset_.y();
+  }
 
   return true;
 }
@@ -357,9 +362,6 @@ 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;
 }
 
@@ -381,15 +383,6 @@ 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_;
 

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")),
   z_(object.kinematics.pose_with_covariance.pose.position.z),
@@ -166,9 +166,6 @@ 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)
@@ -319,11 +316,19 @@ 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
+  {
+    // rotate back the offset vector from object coordinate to global coordinate
+    const double yaw = motion_model_.getStateElement(IDX::YAW);
+    const double offset_x_global =
+      tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw);
+    const double offset_y_global =
+      tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw);
+    motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global);
+    // update offset (object coordinate)
+    tracking_offset_.x() = gain_inv * tracking_offset_.x();
+    tracking_offset_.y() = gain_inv * tracking_offset_.y();
+  }
 
   return true;
 }
@@ -357,9 +362,6 @@ 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;
 }
 
@@ -381,15 +383,6 @@ 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_;