feat(multi_object_tracker): object position update by size change (#7170)

* fix: update object position offset caused by size change

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: update object size only by updating object size, do not memorize previous object size

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: rename message package

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

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

@@ -49,9 +49,7 @@ class BigVehicleTracker : public Tracker
     double height;
   };
   BoundingBox bounding_box_;
-  BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
-  int last_nearest_corner_index_;
 
 private:
   BicycleMotionModel motion_model_;
@@ -77,16 +75,6 @@ class BigVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_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

@@ -49,9 +49,7 @@ class NormalVehicleTracker : public Tracker
     double height;
   };
   BoundingBox bounding_box_;
-  BoundingBox last_input_bounding_box_;
   Eigen::Vector2d tracking_offset_;
-  int last_nearest_corner_index_;
 
 private:
   BicycleMotionModel motion_model_;
@@ -77,16 +75,6 @@ class NormalVehicleTracker : public Tracker
     const rclcpp::Time & time,
     autoware_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/include/multi_object_tracker/utils/utils.hpp

@@ -280,7 +280,7 @@ inline void calcAnchorPointOffset(
 
   // update offset
   const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx);
-  tracking_offset += offset;
+  tracking_offset = offset;
 
   // offset input object
   const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix();

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -44,7 +44,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification;
 
 BigVehicleTracker::BigVehicleTracker(
   const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
   const uint & channel_index)
 : Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("BigVehicleTracker")),
@@ -76,7 +76,6 @@ BigVehicleTracker::BigVehicleTracker(
   if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
     bounding_box_ = {
       object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
-    last_input_bounding_box_ = bounding_box_;
   } else {
     autoware_perception_msgs::msg::DetectedObject bbox_object;
     if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
@@ -89,7 +88,6 @@ BigVehicleTracker::BigVehicleTracker(
         bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
         bbox_object.shape.dimensions.z};
     }
-    last_input_bounding_box_ = bounding_box_;
   }
   // set maximum and minimum size
   constexpr double max_size = 30.0;
@@ -178,9 +176,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)
@@ -214,11 +209,11 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje
   }
 
   // get offset measurement
-  int nearest_corner_index = utils::getNearestCornerOrSurface(
+  const int nearest_corner_index = utils::getNearestCornerOrSurface(
     tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
   utils::calcAnchorPointOffset(
-    last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index,
-    bbox_object, tracked_yaw, updating_object, tracking_offset_);
+    bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw,
+    updating_object, tracking_offset_);
 
   // UNCERTAINTY MODEL
   if (!object.kinematics.has_position_covariance) {
@@ -332,15 +327,13 @@ bool BigVehicleTracker::measureWithShape(
     return false;
   }
 
-  constexpr double gain = 0.1;
+  constexpr double gain = 0.5;
   constexpr double gain_inv = 1.0 - gain;
 
   // update object size
   bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
   bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
   bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
-  last_input_bounding_box_ = {
-    object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
 
   // set maximum and minimum size
   constexpr double max_size = 30.0;
@@ -352,11 +345,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;
 }
@@ -390,9 +391,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;
 }
 
@@ -414,15 +412,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_perception_msgs::msg::ObjectClassification;
 
 NormalVehicleTracker::NormalVehicleTracker(
   const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
   const uint & channel_index)
 : Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("NormalVehicleTracker")),
@@ -76,7 +76,6 @@ NormalVehicleTracker::NormalVehicleTracker(
   if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {
     bounding_box_ = {
       object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
-    last_input_bounding_box_ = bounding_box_;
   } else {
     autoware_perception_msgs::msg::DetectedObject bbox_object;
     if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) {
@@ -90,7 +89,6 @@ NormalVehicleTracker::NormalVehicleTracker(
         bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y,
         bbox_object.shape.dimensions.z};
     }
-    last_input_bounding_box_ = bounding_box_;
   }
   // set maximum and minimum size
   constexpr double max_size = 20.0;
@@ -179,9 +177,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)
@@ -216,11 +211,11 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO
   }
 
   // get offset measurement
-  int nearest_corner_index = utils::getNearestCornerOrSurface(
+  const int nearest_corner_index = utils::getNearestCornerOrSurface(
     tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform);
   utils::calcAnchorPointOffset(
-    last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index,
-    bbox_object, tracked_yaw, updating_object, tracking_offset_);
+    bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw,
+    updating_object, tracking_offset_);
 
   // UNCERTAINTY MODEL
   if (!object.kinematics.has_position_covariance) {
@@ -334,15 +329,13 @@ bool NormalVehicleTracker::measureWithShape(
     return false;
   }
 
-  constexpr double gain = 0.1;
+  constexpr double gain = 0.5;
   constexpr double gain_inv = 1.0 - gain;
 
   // update object size
   bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x;
   bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y;
   bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z;
-  last_input_bounding_box_ = {
-    object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
 
   // set maximum and minimum size
   constexpr double max_size = 20.0;
@@ -354,11 +347,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;
 }
@@ -392,9 +393,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;
 }
 
@@ -416,15 +414,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_;