refactor(multi_object_tracker): modify getNearestCornerOrSurface func…

…tion signature and update related logic

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

refactor(multi_object_tracker): remove self_transform parameter from measure and update methods

refactor(multi_object_tracker): update calcAnchorPointOffset function signature and streamline object handling

refactor(multi_object_tracker): set shape type to BOUNDING_BOX for object trackers

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

@@ -37,12 +37,11 @@ bool convertConvexHullToBoundingBox(
 bool getMeasurementYaw(
   const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw);
 
-geometry_msgs::msg::Point getNearestCornerOrSurface(
-  const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
+void getNearestCornerOrSurface(
+  const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object);
 
 void calcAnchorPointOffset(
-  const types::DynamicObject & this_object, const types::DynamicObject & input_object,
-  const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset,
+  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/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -43,17 +43,10 @@ class BicycleTracker : public Tracker
   BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
-
-private:
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object,
-    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

@@ -39,9 +39,7 @@ class MultipleVehicleTracker : public Tracker
   MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~MultipleVehicleTracker() {}
 };

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp

@@ -37,9 +37,7 @@ class PassThroughTracker : public Tracker
 public:
   PassThroughTracker(const rclcpp::Time & time, const types::DynamicObject & object);
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 };
 

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp

@@ -39,9 +39,7 @@ class PedestrianAndBicycleTracker : public Tracker
   PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~PedestrianAndBicycleTracker() {}
 };

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

@@ -44,17 +44,10 @@ class PedestrianTracker : public Tracker
   PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
-
-private:
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object,
-    const geometry_msgs::msg::Transform & self_transform) const;
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -61,7 +61,7 @@ class Tracker
   }
   bool updateWithMeasurement(
     const types::DynamicObject & object, const rclcpp::Time & measurement_time,
-    const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info);
+    const types::InputChannel & channel_info);
   bool updateWithoutMeasurement(const rclcpp::Time & now);
 
   std::uint8_t getHighestProbLabel() const
@@ -87,9 +87,7 @@ class Tracker
   void limitObjectExtension(const object_model::ObjectModel object_model);
 
   // virtual functions
-  virtual bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) = 0;
+  virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0;
 
 public:
   virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp

@@ -43,11 +43,7 @@ class UnknownTracker : public Tracker
   UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp

@@ -48,16 +48,10 @@ class VehicleTracker : public Tracker
     const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(
-    const types::DynamicObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) override;
+  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
-
-private:
-  types::DynamicObject getUpdatingObject(
-    const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform);
 };
 
 }  // namespace autoware::multi_object_tracker

perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp

@@ -171,8 +171,8 @@ enum BBOX_IDX {
  * @param self_transform: Ego vehicle position in map frame
  * @return int index
  */
-geometry_msgs::msg::Point getNearestCornerOrSurface(
-  const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform)
+void getNearestCornerOrSurface(
+  const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object)
 {
   const double x = object.pose.position.x;
   const double y = object.pose.position.y;
@@ -212,24 +212,22 @@ geometry_msgs::msg::Point getNearestCornerOrSurface(
   } else {
     anchor_y = -width / 2.0;
   }
-  geometry_msgs::msg::Point anchor_point;
-  anchor_point.x = anchor_x;
-  anchor_point.y = anchor_y;
-  return anchor_point;
+
+  object.anchor_point.x = anchor_x;
+  object.anchor_point.y = anchor_y;
 }
 
 void calcAnchorPointOffset(
-  const types::DynamicObject & this_object, const types::DynamicObject & input_object,
-  const geometry_msgs::msg::Point anchor_vector, 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 = 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;
@@ -251,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

@@ -50,6 +50,8 @@ BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicOb
     object_extension.y = object_model_.init_size.width;
     object_extension.z = object_model_.init_size.height;
   }
+  object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+
   // set maximum and minimum size
   limitObjectExtension(object_model_);
 
@@ -126,13 +128,6 @@ bool BicycleTracker::predict(const rclcpp::Time & time)
   return motion_model_.predictState(time);
 }
 
-types::DynamicObject BicycleTracker::getUpdatingObject(
-  const types::DynamicObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/) const
-{
-  return object;
-}
-
 bool BicycleTracker::measureWithPose(const types::DynamicObject & object)
 {
   // get measurement yaw angle to update
@@ -199,9 +194,7 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object)
   return true;
 }
 
-bool BicycleTracker::measure(
-  const types::DynamicObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform)
+bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
 {
   // check time gap
   const double dt = motion_model_.getDeltaTime(time);
@@ -214,9 +207,8 @@ bool BicycleTracker::measure(
   }
 
   // update object
-  const types::DynamicObject updating_object = getUpdatingObject(object, self_transform);
-  measureWithPose(updating_object);
-  measureWithShape(updating_object);
+  measureWithPose(object);
+  measureWithShape(object);
 
   return true;
 }
@@ -225,12 +217,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/multiple_vehicle_tracker.cpp

@@ -37,12 +37,10 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time)
   return true;
 }
 
-bool MultipleVehicleTracker::measure(
-  const types::DynamicObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform)
+bool MultipleVehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
 {
-  big_vehicle_tracker_.measure(object, time, self_transform);
-  normal_vehicle_tracker_.measure(object, time, self_transform);
+  big_vehicle_tracker_.measure(object, time);
+  normal_vehicle_tracker_.measure(object, time);
 
   return true;
 }

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

@@ -52,9 +52,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time)
   return true;
 }
 
-bool PassThroughTracker::measure(
-  const types::DynamicObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform)
+bool PassThroughTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time)
 {
   prev_observed_object_ = object_;
   object_ = object;
@@ -68,7 +66,6 @@ bool PassThroughTracker::measure(
   }
   last_update_time_ = time;
 
-  (void)self_transform;  // currently do not use self vehicle position
   return true;
 }
 

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

@@ -34,11 +34,10 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time)
 }
 
 bool PedestrianAndBicycleTracker::measure(
-  const types::DynamicObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform)
+  const types::DynamicObject & object, const rclcpp::Time & time)
 {
-  pedestrian_tracker_.measure(object, time, self_transform);
-  bicycle_tracker_.measure(object, time, self_transform);
+  pedestrian_tracker_.measure(object, time);
+  bicycle_tracker_.measure(object, time);
 
   return true;
 }

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

@@ -116,13 +116,6 @@ bool PedestrianTracker::predict(const rclcpp::Time & time)
   return motion_model_.predictState(time);
 }
 
-types::DynamicObject PedestrianTracker::getUpdatingObject(
-  const types::DynamicObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/) const
-{
-  return object;
-}
-
 bool PedestrianTracker::measureWithPose(const types::DynamicObject & object)
 {
   // update motion model
@@ -170,16 +163,14 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object)
     return false;
   }
 
+  // update shape type
+  object_.shape.type = object.shape.type;
+
   return true;
 }
 
-bool PedestrianTracker::measure(
-  const types::DynamicObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform)
+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) {
@@ -191,24 +182,22 @@ bool PedestrianTracker::measure(
   }
 
   // update object
-  const types::DynamicObject updating_object = getUpdatingObject(object, self_transform);
-  measureWithPose(updating_object);
-  measureWithShape(updating_object);
+  measureWithPose(object);
+  measureWithShape(object);
 
-  (void)self_transform;  // currently do not use self vehicle position
   return true;
 }
 
 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/tracker_base.cpp

@@ -79,7 +79,7 @@ void Tracker::initializeExistenceProbabilities(
 
 bool Tracker::updateWithMeasurement(
   const types::DynamicObject & object, const rclcpp::Time & measurement_time,
-  const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info)
+  const types::InputChannel & channel_info)
 {
   // Update existence probability
   {
@@ -120,7 +120,7 @@ bool Tracker::updateWithMeasurement(
   }
 
   // Update object
-  measure(object, measurement_time, self_transform);
+  measure(object, measurement_time);
 
   return true;
 }