refactor(multi_object_tracker): update calcAnchorPointOffset function…

… signature and streamline object handling

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp

@@ -41,8 +41,8 @@ void getNearestCornerOrSurface(
   const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object);
 
 void calcAnchorPointOffset(
-  const types::DynamicObject & this_object, const types::DynamicObject & input_object,
-  Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object);
+  const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset,
+  types::DynamicObject & offset_object);
 }  // namespace shapes
 }  // namespace autoware::multi_object_tracker
 

perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

@@ -218,17 +218,16 @@ void getNearestCornerOrSurface(
 }
 
 void calcAnchorPointOffset(
-  const types::DynamicObject & this_object, const types::DynamicObject & input_object,
-  Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object)
+  const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset,
+  types::DynamicObject & updating_object)
 {
   // copy value
-  offset_object = input_object;
-  const geometry_msgs::msg::Point anchor_vector = input_object.anchor_point;
+  const geometry_msgs::msg::Point anchor_vector = updating_object.anchor_point;
   // invalid anchor
   if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) {
     return;
   }
-  double input_yaw = tf2::getYaw(input_object.pose.orientation);
+  double input_yaw = tf2::getYaw(updating_object.pose.orientation);
 
   // current object width and height
   const double length = this_object.shape.dimensions.x;
@@ -250,8 +249,8 @@ void calcAnchorPointOffset(
   // offset input object
   const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix();
   const Eigen::Vector2d rotated_offset = R * tracking_offset;
-  offset_object.pose.position.x += rotated_offset.x();
-  offset_object.pose.position.y += rotated_offset.y();
+  updating_object.pose.position.x += rotated_offset.x();
+  updating_object.pose.position.y += rotated_offset.y();
 }
 
 }  // namespace shapes

perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp

@@ -215,12 +215,12 @@ bool BicycleTracker::getTrackedObject(
   const rclcpp::Time & time, types::DynamicObject & object) const
 {
   object = object_;
+
+  // predict from motion model
   auto & pose = object.pose;
   auto & pose_cov = object.pose_covariance;
   auto & twist = object.twist;
   auto & twist_cov = object.twist_covariance;
-
-  // predict from motion model
   if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) {
     RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state.");
     return false;

perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp

@@ -168,9 +168,6 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object)
 
 bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
 {
-  // keep the latest input object
-  object_ = object;
-
   // check time gap
   const double dt = motion_model_.getDeltaTime(time);
   if (0.01 /*10msec*/ < dt) {
@@ -192,12 +189,12 @@ bool PedestrianTracker::getTrackedObject(
   const rclcpp::Time & time, types::DynamicObject & object) const
 {
   object = object_;
+
+  // predict from motion model
   auto & pose = object.pose;
   auto & pose_cov = object.pose_covariance;
   auto & twist = object.twist;
   auto & twist_cov = object.twist_covariance;
-
-  // predict from motion model
   if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) {
     RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state.");
     return false;

perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp

@@ -142,8 +142,8 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object)
 
 bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
 {
-  // keep the latest input object
-  object_ = object;
+  // update object shape
+  object_.shape = object.shape;
 
   // check time gap
   const double dt = motion_model_.getDeltaTime(time);

perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp

@@ -227,11 +227,8 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object)
   return true;
 }
 
-bool VehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
+bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcpp::Time & time)
 {
-  // keep the latest input object
-  object_ = object;
-
   // check time gap
   const double dt = motion_model_.getDeltaTime(time);
   if (0.01 /*10msec*/ < dt) {
@@ -243,8 +240,8 @@ bool VehicleTracker::measure(const types::DynamicObject & object, const rclcpp::
   }
 
   // update object
-  types::DynamicObject updating_object = object;
-  shapes::calcAnchorPointOffset(object_, object, tracking_offset_, updating_object);
+  types::DynamicObject updating_object = in_object;
+  shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object);
   measureWithPose(updating_object);
   measureWithShape(updating_object);
 
@@ -255,12 +252,12 @@ bool VehicleTracker::getTrackedObject(
   const rclcpp::Time & time, types::DynamicObject & object) const
 {
   object = object_;
+
+  // predict from motion model
   auto & pose = object.pose;
   auto & pose_cov = object.pose_covariance;
   auto & twist = object.twist;
   auto & twist_cov = object.twist_covariance;
-
-  // predict from motion model
   if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) {
     RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state.");
     return false;