refactor(multi_object_tracker): use common utils (tier4#1408)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

perception/multi_object_tracker/CMakeLists.txt

@@ -21,7 +21,6 @@ include_directories(
 # Generate exe file
 set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
-  src/utils/utils.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/big_vehicle_tracker.cpp
   src/tracker/model/normal_vehicle_tracker.cpp

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

@@ -21,6 +21,7 @@
 
 #define EIGEN_MPL2_ONLY
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
@@ -63,7 +64,10 @@ class Tracker
   {
     return classification_;
   }
-  std::uint8_t getHighestProbLabel() const { return utils::getHighestProbLabel(classification_); }
+  std::uint8_t getHighestProbLabel() const
+  {
+    return perception_utils::getHighestProbLabel(classification_);
+  }
   int getNoMeasurementCount() const { return no_measurement_count_; }
   int getTotalNoMeasurementCount() const { return total_no_measurement_count_; }
   int getTotalMeasurementCount() const { return total_measurement_count_; }

perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp

@@ -33,25 +33,6 @@
 
 namespace utils
 {
-double getPolygonArea(const geometry_msgs::msg::Polygon & footprint);
-double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions);
-double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions);
-double getArea(const autoware_auto_perception_msgs::msg::Shape & shape);
-double get2dIoU(
-  const std::tuple<
-    const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object1,
-  const std::tuple<
-    const geometry_msgs::msg::Pose &, const autoware_auto_perception_msgs::msg::Shape &> & object2);
-double get2dIoU(
-  const autoware_auto_perception_msgs::msg::TrackedObject & object1,
-  const autoware_auto_perception_msgs::msg::TrackedObject & object2);
-std::uint8_t getHighestProbLabel(
-  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
-autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(
-  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object);
-geometry_msgs::msg::Polygon rotatePolygon(
-  const geometry_msgs::msg::Polygon & polygon, const double angle);
-
 enum MSG_COV_IDX {
   X_X = 0,
   X_Y = 1,

perception/multi_object_tracker/package.xml

@@ -17,6 +17,7 @@
   <depend>eigen</depend>
   <depend>kalman_filter</depend>
   <depend>mussp</depend>
+  <depend>perception_utils</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2</depend>

perception/multi_object_tracker/src/data_association/data_association.cpp

@@ -16,6 +16,7 @@
 
 #include "multi_object_tracker/data_association/solver/gnn_solver.hpp"
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <algorithm>
 #include <list>
@@ -25,15 +26,6 @@
 
 namespace
 {
-double getDistance(
-  const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker)
-{
-  const double diff_x = tracker.x - measurement.x;
-  const double diff_y = tracker.y - measurement.y;
-  // const double diff_z = tracker.z - measurement.z;
-  return std::sqrt(diff_x * diff_x + diff_y * diff_y);
-}
-
 double getMahalanobisDistance(
   const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker,
   const Eigen::Matrix2d & covariance)
@@ -170,15 +162,15 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
       const autoware_auto_perception_msgs::msg::DetectedObject & measurement_object =
         measurements.objects.at(measurement_idx);
       const std::uint8_t measurement_label =
-        utils::getHighestProbLabel(measurement_object.classification);
+        perception_utils::getHighestProbLabel(measurement_object.classification);
 
       double score = 0.0;
       if (can_assign_matrix_(tracker_label, measurement_label)) {
         autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
         (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object);
 
         const double max_dist = max_dist_matrix_(tracker_label, measurement_label);
-        const double dist = getDistance(
+        const double dist = tier4_autoware_utils::calcDistance2d(
           measurement_object.kinematics.pose_with_covariance.pose.position,
           tracked_object.kinematics.pose_with_covariance.pose.position);
 
@@ -191,7 +183,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
         if (passed_gate) {
           const double max_area = max_area_matrix_(tracker_label, measurement_label);
           const double min_area = min_area_matrix_(tracker_label, measurement_label);
-          const double area = utils::getArea(measurement_object.shape);
+          const double area = tier4_autoware_utils::getArea(measurement_object.shape);
           if (area < min_area || max_area < area) passed_gate = false;
         }
         // angle gate
@@ -214,9 +206,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
         // 2d iou gate
         if (passed_gate) {
           const double min_iou = min_iou_matrix_(tracker_label, measurement_label);
-          const double iou = utils::get2dIoU(
-            {measurement_object.kinematics.pose_with_covariance.pose, measurement_object.shape},
-            {tracked_object.kinematics.pose_with_covariance.pose, tracked_object.shape});
+          const double iou = perception_utils::get2dIoU(measurement_object, tracked_object);
           if (iou < min_iou) passed_gate = false;
         }
 

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -30,6 +30,7 @@
 #define EIGEN_MPL2_ONLY
 #include "multi_object_tracker/multi_object_tracker_core.hpp"
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -39,7 +40,7 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 namespace
 {
-boost::optional<geometry_msgs::msg::Transform> getTransform(
+boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
   const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
   const std::string & target_frame_id, const rclcpp::Time & time)
 {
@@ -55,37 +56,6 @@ boost::optional<geometry_msgs::msg::Transform> getTransform(
   }
 }
 
-bool transformDetectedObjects(
-  const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
-  const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
-  autoware_auto_perception_msgs::msg::DetectedObjects & output_msg)
-{
-  output_msg = input_msg;
-
-  /* transform to world coordinate */
-  if (input_msg.header.frame_id != target_frame_id) {
-    output_msg.header.frame_id = target_frame_id;
-    tf2::Transform tf_target2objects_world;
-    tf2::Transform tf_target2objects;
-    tf2::Transform tf_objects_world2objects;
-    {
-      const auto ros_target2objects_world =
-        getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
-      if (!ros_target2objects_world) {
-        return false;
-      }
-      tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
-    }
-    for (auto & object : output_msg.objects) {
-      tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
-      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
-      tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
-      // TODO(yukkysaito) transform covariance
-    }
-  }
-  return true;
-}
-
 inline float getVelocity(const autoware_auto_perception_msgs::msg::TrackedObject & object)
 {
   return std::hypot(
@@ -217,15 +187,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 void MultiObjectTracker::onMeasurement(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
 {
-  const auto self_transform =
-    getTransform(tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
+  const auto self_transform = getTransformAnonymous(
+    tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
   if (!self_transform) {
     return;
   }
 
   /* transform to world coordinate */
   autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
-  if (!transformDetectedObjects(
+  if (!perception_utils::transformObjects(
         *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
     return;
   }
@@ -279,7 +249,7 @@ std::shared_ptr<Tracker> MultiObjectTracker::createNewTracker(
   const autoware_auto_perception_msgs::msg::DetectedObject & object,
   const rclcpp::Time & time) const
 {
-  const std::uint8_t label = utils::getHighestProbLabel(object.classification);
+  const std::uint8_t label = perception_utils::getHighestProbLabel(object.classification);
   if (tracker_map_.count(label) != 0) {
     const auto tracker = tracker_map_.at(label);
 
@@ -305,7 +275,8 @@ std::shared_ptr<Tracker> MultiObjectTracker::createNewTracker(
 void MultiObjectTracker::onTimer()
 {
   rclcpp::Time current_time = this->now();
-  const auto self_transform = getTransform(tf_buffer_, world_frame_id_, "base_link", current_time);
+  const auto self_transform =
+    getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time);
   if (!self_transform) {
     return;
   }
@@ -360,7 +331,7 @@ void MultiObjectTracker::sanitizeTracker(
         continue;
       }
 
-      const auto iou = utils::get2dIoU(object1, object2);
+      const auto iou = perception_utils::get2dIoU(object1, object2);
       const auto & label1 = (*itr1)->getHighestProbLabel();
       const auto & label2 = (*itr2)->getHighestProbLabel();
       bool should_delete_tracker1 = false;

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

@@ -32,6 +32,7 @@
 #else
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
+#include "perception_utils/perception_utils.hpp"
 
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
@@ -318,7 +319,7 @@ bool BicycleTracker::measure(
 {
   const auto & current_classification = getClassification();
   object_ = object;
-  if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
+  if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
     setClassification(current_classification);
   }
 
@@ -337,7 +338,7 @@ bool BicycleTracker::measure(
 bool BicycleTracker::getTrackedObject(
   const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
 {
-  object = utils::toTrackedObject(object_);
+  object = perception_utils::toTrackedObject(object_);
   object.object_id = getUUID();
   object.classification = getClassification();
 
@@ -410,7 +411,8 @@ bool BicycleTracker::getTrackedObject(
   object.shape.dimensions.z = bounding_box_.height;
   const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation);
   const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
-  object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
+  object.shape.footprint =
+    tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
 
   return true;
 }

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

@@ -30,6 +30,7 @@
 #define EIGEN_MPL2_ONLY
 #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp"
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -225,7 +226,7 @@ bool BigVehicleTracker::measureWithPose(
 
   float r_cov_x;
   float r_cov_y;
-  const uint8_t label = utils::getHighestProbLabel(object.classification);
+  const uint8_t label = perception_utils::getHighestProbLabel(object.classification);
 
   if (label == Label::CAR) {
     constexpr float r_stddev_x = 8.0;  // [m]
@@ -367,7 +368,7 @@ bool BigVehicleTracker::measure(
 {
   const auto & current_classification = getClassification();
   object_ = object;
-  if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
+  if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
     setClassification(current_classification);
   }
 
@@ -386,7 +387,7 @@ bool BigVehicleTracker::measure(
 bool BigVehicleTracker::getTrackedObject(
   const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
 {
-  object = utils::toTrackedObject(object_);
+  object = perception_utils::toTrackedObject(object_);
   object.object_id = getUUID();
   object.classification = getClassification();
 
@@ -459,7 +460,8 @@ bool BigVehicleTracker::getTrackedObject(
   object.shape.dimensions.z = bounding_box_.height;
   const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation);
   const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
-  object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
+  object.shape.footprint =
+    tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
 
   return true;
 }

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

@@ -42,7 +42,7 @@ bool MultipleVehicleTracker::measure(
 {
   big_vehicle_tracker_.measure(object, time);
   normal_vehicle_tracker_.measure(object, time);
-  if (utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
+  if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
     setClassification(object.classification);
   return true;
 }

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

@@ -30,6 +30,7 @@
 #define EIGEN_MPL2_ONLY
 #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp"
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -225,7 +226,7 @@ bool NormalVehicleTracker::measureWithPose(
 
   float r_cov_x;
   float r_cov_y;
-  const uint8_t label = utils::getHighestProbLabel(object.classification);
+  const uint8_t label = perception_utils::getHighestProbLabel(object.classification);
 
   if (label == Label::CAR) {
     r_cov_x = ekf_params_.r_cov_x;
@@ -369,7 +370,7 @@ bool NormalVehicleTracker::measure(
 {
   const auto & current_classification = getClassification();
   object_ = object;
-  if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
+  if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
     setClassification(current_classification);
   }
 
@@ -388,7 +389,7 @@ bool NormalVehicleTracker::measure(
 bool NormalVehicleTracker::getTrackedObject(
   const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
 {
-  object = utils::toTrackedObject(object_);
+  object = perception_utils::toTrackedObject(object_);
   object.object_id = getUUID();
   object.classification = getClassification();
 
@@ -461,6 +462,7 @@ bool NormalVehicleTracker::getTrackedObject(
   object.shape.dimensions.z = bounding_box_.height;
   const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation);
   const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation);
-  object.shape.footprint = utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
+  object.shape.footprint =
+    tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw);
   return true;
 }

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

@@ -30,6 +30,7 @@
 #define EIGEN_MPL2_ONLY
 #include "multi_object_tracker/tracker/model/pass_through_tracker.hpp"
 #include "multi_object_tracker/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -79,7 +80,7 @@ bool PassThroughTracker::measure(
 bool PassThroughTracker::getTrackedObject(
   const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
 {
-  object = utils::toTrackedObject(object_);
+  object = perception_utils::toTrackedObject(object_);
   object.object_id = getUUID();
   object.classification = getClassification();
   object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0;

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

@@ -42,7 +42,7 @@ bool PedestrianAndBicycleTracker::measure(
 {
   pedestrian_tracker_.measure(object, time);
   bicycle_tracker_.measure(object, time);
-  if (utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
+  if (perception_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN)
     setClassification(object.classification);
   return true;
 }