refactor(object_merger): use common utils (autowarefoundation#1409)

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

Author: takayuki5168

perception/object_merger/CMakeLists.txt

@@ -23,7 +23,6 @@ ament_auto_add_library(mu_successive_shortest_path SHARED
 )
 
 ament_auto_add_library(object_association_merger SHARED
-  src/object_association_merger/utils/utils.cpp
   src/object_association_merger/data_association/data_association.cpp
   src/object_association_merger/node.cpp
 )

perception/object_merger/include/object_association_merger/utils/utils.hpp

@@ -32,29 +32,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::DetectedObject & object1,
-  const autoware_auto_perception_msgs::msg::DetectedObject & object2);
-double get2dPrecision(
-  const autoware_auto_perception_msgs::msg::DetectedObject & source_object,
-  const autoware_auto_perception_msgs::msg::DetectedObject & target_object);
-double get2dRecall(
-  const autoware_auto_perception_msgs::msg::DetectedObject & source_object,
-  const autoware_auto_perception_msgs::msg::DetectedObject & target_object);
-std::uint8_t getHighestProbLabel(
-  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
-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/object_merger/package.xml

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

perception/object_merger/src/object_association_merger/data_association/data_association.cpp

@@ -16,6 +16,8 @@
 
 #include "object_association_merger/data_association/solver/gnn_solver.hpp"
 #include "object_association_merger/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <algorithm>
 #include <list>
@@ -25,15 +27,6 @@
 
 namespace
 {
-double getDistance(
-  const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1)
-{
-  const double diff_x = point1.x - point0.x;
-  const double diff_y = point1.y - point0.y;
-  // const double diff_z = point1.z - point0.z;
-  return std::sqrt(diff_x * diff_x + diff_y * diff_y);
-}
-
 double getFormedYawAngle(
   const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1,
   const bool distinguish_front_or_back = true)
@@ -128,17 +121,19 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
   for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) {
     const autoware_auto_perception_msgs::msg::DetectedObject & object1 =
       objects1.objects.at(objects1_idx);
-    const std::uint8_t object1_label = utils::getHighestProbLabel(object1.classification);
+    const std::uint8_t object1_label =
+      perception_utils::getHighestProbLabel(object1.classification);
 
     for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) {
       const autoware_auto_perception_msgs::msg::DetectedObject & object0 =
         objects0.objects.at(objects0_idx);
-      const std::uint8_t object0_label = utils::getHighestProbLabel(object0.classification);
+      const std::uint8_t object0_label =
+        perception_utils::getHighestProbLabel(object0.classification);
 
       double score = 0.0;
       if (can_assign_matrix_(object1_label, object0_label)) {
         const double max_dist = max_dist_matrix_(object1_label, object0_label);
-        const double dist = getDistance(
+        const double dist = tier4_autoware_utils::calcDistance2d(
           object0.kinematics.pose_with_covariance.pose.position,
           object1.kinematics.pose_with_covariance.pose.position);
 
@@ -159,9 +154,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
         // 2d iou gate
         if (passed_gate) {
           const double min_iou = min_iou_matrix_(object1_label, object0_label);
-          const double iou = utils::get2dIoU(
-            {object0.kinematics.pose_with_covariance.pose, object0.shape},
-            {object1.kinematics.pose_with_covariance.pose, object1.shape});
+          const double iou = perception_utils::get2dIoU(object0, object1);
           if (iou < min_iou) passed_gate = false;
         }
 

perception/object_merger/src/object_association_merger/node.cpp

@@ -15,6 +15,7 @@
 #include "object_association_merger/node.hpp"
 
 #include "object_association_merger/utils/utils.hpp"
+#include "perception_utils/perception_utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 #include <boost/optional.hpp>
@@ -30,53 +31,6 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 namespace
 {
-boost::optional<geometry_msgs::msg::Transform> getTransform(
-  const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
-  const std::string & target_frame_id, const rclcpp::Time & time)
-{
-  try {
-    geometry_msgs::msg::TransformStamped self_transform_stamped;
-    self_transform_stamped = tf_buffer.lookupTransform(
-      /*target*/ target_frame_id, /*src*/ source_frame_id, time,
-      rclcpp::Duration::from_seconds(0.5));
-    return self_transform_stamped.transform;
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_WARN_STREAM(rclcpp::get_logger("object_association_merger"), ex.what());
-    return boost::none;
-  }
-}
-
-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;
-}
-
 bool isUnknownObjectOverlapped(
   const autoware_auto_perception_msgs::msg::DetectedObject & unknown_object,
   const autoware_auto_perception_msgs::msg::DetectedObject & known_object,
@@ -87,8 +41,8 @@ bool isUnknownObjectOverlapped(
     unknown_object.kinematics.pose_with_covariance.pose,
     known_object.kinematics.pose_with_covariance.pose);
   if (sq_distance_threshold < sq_distance) return false;
-  const auto precision = utils::get2dPrecision(unknown_object, known_object);
-  const auto recall = utils::get2dRecall(unknown_object, known_object);
+  const auto precision = perception_utils::get2dPrecision(unknown_object, known_object);
+  const auto recall = perception_utils::get2dRecall(unknown_object, known_object);
   return precision > precision_threshold || recall > recall_threshold;
 }
 }  // namespace
@@ -140,9 +94,9 @@ void ObjectAssociationMergerNode::objectsCallback(
   /* transform to base_link coordinate */
   autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1;
   if (
-    !transformDetectedObjects(
+    !perception_utils::transformObjects(
       *input_objects0_msg, base_link_frame_id_, tf_buffer_, transformed_objects0) ||
-    !transformDetectedObjects(
+    !perception_utils::transformObjects(
       *input_objects1_msg, base_link_frame_id_, tf_buffer_, transformed_objects1)) {
     return;
   }
@@ -185,7 +139,7 @@ void ObjectAssociationMergerNode::objectsCallback(
     unknown_objects.reserve(output_msg.objects.size());
     known_objects.reserve(output_msg.objects.size());
     for (const auto & object : output_msg.objects) {
-      if (utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
+      if (perception_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) {
         unknown_objects.push_back(object);
       } else {
         known_objects.push_back(object);