feat(image_projection_based_fusion): add objects filter by rois (#4546)

* tmp Signed-off-by: yukke42 <yukke42@users.noreply.github.com> style(pre-commit): autofix update Signed-off-by: yukke42 <yukke42@users.noreply.github.com> style(pre-commit): autofix * fix: fix association bug Signed-off-by: yukke42 <yukke42@users.noreply.github.com> * feat: add prob_threshold for each class Signed-off-by: yukke42 <yukke42@users.noreply.github.com> * feat: use class label association between roi and object Signed-off-by: yukke42 <yukke42@users.noreply.github.com> * feat: add to tier4_perception_launch Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: disable debug_mode Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * docs: update params Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * fix: apply suggestion Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: update prob_thresholds of bicycle Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * feat: add thresut_distance for each class Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * docs: add thrust_distances Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * style(pre-commit): autofix * chore: remove unnecessary variable Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * chore: rename to trust Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * style(pre-commit): autofix * chore: add param Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * Update perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --------- Signed-off-by: yukke42 <yukke42@users.noreply.github.com> Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
autowarefoundation · Aug 9, 2023 · 9a2e27e · 9a2e27e
1 parent e14d71e
commit 9a2e27e
Show file tree

Hide file tree

Showing 7 changed files with 129 additions and 46 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -224,12 +224,44 @@
     </include>
   </group>
 
+  <group>
+    <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_detected_object_fusion.launch.xml">
+      <arg name="input/camera_info0" value="$(var camera_info0)"/>
+      <arg name="input/rois0" value="$(var detection_rois0)"/>
+      <arg name="input/camera_info1" value="$(var camera_info1)"/>
+      <arg name="input/rois1" value="$(var detection_rois1)"/>
+      <arg name="input/camera_info2" value="$(var camera_info2)"/>
+      <arg name="input/rois2" value="$(var detection_rois2)"/>
+      <arg name="input/camera_info3" value="$(var camera_info3)"/>
+      <arg name="input/rois3" value="$(var detection_rois3)"/>
+      <arg name="input/camera_info4" value="$(var camera_info4)"/>
+      <arg name="input/rois4" value="$(var detection_rois4)"/>
+      <arg name="input/camera_info5" value="$(var camera_info5)"/>
+      <arg name="input/rois5" value="$(var detection_rois5)"/>
+      <arg name="input/camera_info6" value="$(var camera_info6)"/>
+      <arg name="input/rois6" value="$(var detection_rois6)"/>
+      <arg name="input/camera_info7" value="$(var camera_info7)"/>
+      <arg name="input/rois7" value="$(var detection_rois7)"/>
+      <arg name="input/rois_number" value="$(var image_number)"/>
+      <arg name="input/image0" value="$(var image_raw0)"/>
+      <arg name="input/image1" value="$(var image_raw1)"/>
+      <arg name="input/image2" value="$(var image_raw2)"/>
+      <arg name="input/image3" value="$(var image_raw3)"/>
+      <arg name="input/image4" value="$(var image_raw4)"/>
+      <arg name="input/image5" value="$(var image_raw5)"/>
+      <arg name="input/image6" value="$(var image_raw6)"/>
+      <arg name="input/image7" value="$(var image_raw7)"/>
+      <arg name="input/objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="output/objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
+    </include>
+  </group>
+
   <!-- Validator -->
   <group if="$(eval &quot;'$(var objects_validation_method)'=='obstacle_pointcloud'&quot;)">
     <let name="validator/input/obstacle_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
     <let name="validator/input/obstacle_pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
     <include file="$(find-pkg-share detected_object_validation)/launch/obstacle_pointcloud_based_validator.launch.xml" if="$(var use_validator)">
-      <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
+      <arg name="input/detected_objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
     </include>

diff --git a/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml b/perception/image_projection_based_fusion/config/roi_detected_object_fusion.param.yaml
@@ -0,0 +1,19 @@
+/**:
+  ros__parameters:
+    # UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+    passthrough_lower_bound_probability_thresholds: [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.50]
+    trust_distances: [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0]
+    min_iou_threshold: 0.5
+    use_roi_probability: false
+    roi_probability_threshold: 0.5
+
+    can_assign_matrix:
+      #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg
+      [1,       0,   0,     0,   0,       0,         0,       0,         # UNKNOWN <-detected_objects
+       0,       1,   1,     1,   1,       0,         0,       0,         # CAR
+       0,       1,   1,     1,   1,       0,         0,       0,         # TRUCK
+       0,       1,   1,     1,   1,       0,         0,       0,         # BUS
+       0,       1,   1,     1,   1,       0,         0,       0,         # TRAILER
+       0,       0,   0,     0,   0,       1,         1,       1,         # MOTORBIKE
+       0,       0,   0,     0,   0,       1,         1,       1,         # BICYCLE
+       0,       0,   0,     0,   0,       1,         1,       1]         # PEDESTRIAN
diff --git a/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/image_projection_based_fusion/docs/roi-detected-object-fusion.md
@@ -41,14 +41,16 @@ The DetectedObject has three possible shape choices/implementations, where the p
 
 ### Core Parameters
 
-| Name                                            | Type   | Description                                                                                                               |
-| ----------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- |
-| `rois_number`                                   | int    | the number of input rois                                                                                                  |
-| `debug_mode`                                    | bool   | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings.                      |
-| `min_iou_threshold`                             | double | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused.    |
-| `passthrough_lower_bound_probability_threshold` | double | If the `existence_probability` of a detected object is greater than the threshold, it is published in output.             |
-| `use_roi_probability`                           | float  | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects.          |
-| `roi_probability_threshold`                     | double | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`. |
+| Name                                             | Type           | Description                                                                                                                |
+| ------------------------------------------------ | -------------- | -------------------------------------------------------------------------------------------------------------------------- |
+| `rois_number`                                    | int            | the number of input rois                                                                                                   |
+| `debug_mode`                                     | bool           | If set to `true`, the node subscribes to the image topic and publishes an image with debug drawings.                       |
+| `passthrough_lower_bound_probability_thresholds` | vector[double] | If the `existence_probability` of a detected object is greater than the threshold, it is published in output.              |
+| `trust_distances`                                | vector[double] | If the distance of a detected object from the origin of frame_id is greater than the threshold, it is published in output. |
+| `min_iou_threshold`                              | double         | If the iou between detected objects and rois is greater than `min_iou_threshold`, the objects are classified as fused.     |
+| `use_roi_probability`                            | float          | If set to `true`, the algorithm uses `existence_probability` of ROIs to match with the that of detected objects.           |
+| `roi_probability_threshold`                      | double         | If the `existence_probability` of ROIs is greater than the threshold, matched detected objects are published in `output`.  |
+| `can_assign_matrix`                              | vector[int]    | association matrix between rois and detected_objects to check that two rois on images can be match                         |
 
 ## Assumptions / Known limits
 

diff --git a/...on_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/...on_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
@@ -18,6 +18,8 @@
 #include "image_projection_based_fusion/fusion_node.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+
 #include <map>
 #include <memory>
 #include <vector>
@@ -40,14 +42,14 @@ class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedO
     const DetectedObjectsWithFeature & input_roi_msg,
     const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override;
 
-  std::map<std::size_t, RegionOfInterest> generateDetectedObjectRoIs(
+  std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
     const DetectedObjects & input_object_msg, const double image_width, const double image_height,
     const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection);
 
   void fuseObjectsOnImage(
     const DetectedObjects & input_object_msg,
     const std::vector<DetectedObjectWithFeature> & image_rois,
-    const std::map<std::size_t, sensor_msgs::msg::RegionOfInterest> & object_roi_map);
+    const std::map<std::size_t, DetectedObjectWithFeature> & object_roi_map);
 
   void publish(const DetectedObjects & output_msg) override;
 
@@ -56,10 +58,12 @@ class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, DetectedO
 private:
   struct
   {
-    double passthrough_lower_bound_probability_threshold{};
+    std::vector<double> passthrough_lower_bound_probability_thresholds{};
+    std::vector<double> trust_distances{};
     double min_iou_threshold{};
     bool use_roi_probability{};
     double roi_probability_threshold{};
+    Eigen::MatrixXi can_assign_matrix;
   } fusion_params_;
 
   std::map<int64_t, std::vector<bool>> passthrough_object_flags_map_, fused_object_flags_map_,

diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
@@ -18,6 +18,7 @@
   <arg name="input/camera_info7" default="/camera_info7"/>
   <arg name="input/objects" default="objects"/>
   <arg name="output/objects" default="fused_objects"/>
+  <arg name="param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_detected_object_fusion.param.yaml"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
 
   <!-- for eval variable-->
@@ -39,13 +40,9 @@
     <node pkg="image_projection_based_fusion" exec="roi_detected_object_fusion_node" name="roi_detected_object_fusion" output="screen">
       <param name="rois_number" value="$(var input/rois_number)"/>
       <param from="$(var sync_param_path)"/>
+      <param from="$(var param_path)"/>
       <remap from="input" to="$(var input/objects)"/>
       <remap from="output" to="$(var output/objects)"/>
-      <param name="passthrough_lower_bound_probability_threshold" value="0.35"/>
-      <param name="iou_threshold" value="0.3"/>
-      <param name="use_roi_probability" value="false"/>
-      <param name="roi_probability_threshold" value="0.5"/>
-      <param name="min_iou_threshold" value="0.5"/>
 
       <!-- rois, camera and info -->
       <param name="input/rois0" value="$(var input/rois0)"/>

diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml
@@ -17,6 +17,7 @@
   <depend>image_transport</depend>
   <depend>lidar_centerpoint</depend>
   <depend>message_filters</depend>
+  <depend>object_recognition_utils</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>

diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
@@ -14,6 +14,8 @@
 
 #include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp"
 
+#include "object_recognition_utils/object_recognition_utils.hpp"
+
 #include <image_projection_based_fusion/utils/geometry.hpp>
 #include <image_projection_based_fusion/utils/utils.hpp>
 
@@ -25,11 +27,20 @@ namespace image_projection_based_fusion
 RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options)
 : FusionNode<DetectedObjects, DetectedObject>("roi_detected_object_fusion", options)
 {
-  fusion_params_.passthrough_lower_bound_probability_threshold =
-    declare_parameter<double>("passthrough_lower_bound_probability_threshold");
+  fusion_params_.passthrough_lower_bound_probability_thresholds =
+    declare_parameter<std::vector<double>>("passthrough_lower_bound_probability_thresholds");
+  fusion_params_.min_iou_threshold = declare_parameter<double>("min_iou_threshold");
+  fusion_params_.trust_distances = declare_parameter<std::vector<double>>("trust_distances");
   fusion_params_.use_roi_probability = declare_parameter<bool>("use_roi_probability");
   fusion_params_.roi_probability_threshold = declare_parameter<double>("roi_probability_threshold");
-  fusion_params_.min_iou_threshold = declare_parameter<double>("min_iou_threshold");
+  {
+    const auto can_assign_vector_tmp = declare_parameter<std::vector<int64_t>>("can_assign_matrix");
+    std::vector<int> can_assign_vector(can_assign_vector_tmp.begin(), can_assign_vector_tmp.end());
+    const int label_num = static_cast<int>(std::sqrt(can_assign_vector.size()));
+    Eigen::Map<Eigen::MatrixXi> can_assign_matrix_tmp(
+      can_assign_vector.data(), label_num, label_num);
+    fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose();
+  }
 }
 
 void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg)
@@ -39,9 +50,15 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg)
   fused_object_flags.resize(output_msg.objects.size());
   ignored_object_flags.resize(output_msg.objects.size());
   for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) {
-    if (
-      output_msg.objects.at(obj_i).existence_probability >
-      fusion_params_.passthrough_lower_bound_probability_threshold) {
+    const auto & object = output_msg.objects.at(obj_i);
+    const auto label = object_recognition_utils::getHighestProbLabel(object.classification);
+    const auto pos = object_recognition_utils::getPose(object).position;
+    const auto object_sqr_dist = pos.x * pos.x + pos.y * pos.y;
+    const auto prob_threshold =
+      fusion_params_.passthrough_lower_bound_probability_thresholds.at(label);
+    const auto trust_sqr_dist =
+      fusion_params_.trust_distances.at(label) * fusion_params_.trust_distances.at(label);
+    if (object.existence_probability > prob_threshold || object_sqr_dist > trust_sqr_dist) {
       passthrough_object_flags.at(obj_i) = true;
     }
   }
@@ -90,11 +107,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
   }
 }
 
-std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
+std::map<std::size_t, DetectedObjectWithFeature>
+RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
   const DetectedObjects & input_object_msg, const double image_width, const double image_height,
   const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection)
 {
-  std::map<std::size_t, RegionOfInterest> object_roi_map;
+  std::map<std::size_t, DetectedObjectWithFeature> object_roi_map;
   int64_t timestamp_nsec =
     input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec;
   if (passthrough_object_flags_map_.size() == 0) {
@@ -106,18 +124,18 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
   const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec);
   for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) {
     std::vector<Eigen::Vector3d> vertices_camera_coord;
-    {
-      const auto & object = input_object_msg.objects.at(obj_i);
+    const auto & object = input_object_msg.objects.at(obj_i);
 
-      if (passthrough_object_flags.at(obj_i)) {
-        continue;
-      }
+    if (passthrough_object_flags.at(obj_i)) {
+      continue;
+    }
 
-      // filter point out of scope
-      if (debugger_ && out_of_scope(object)) {
-        continue;
-      }
+    // filter point out of scope
+    if (debugger_ && out_of_scope(object)) {
+      continue;
+    }
 
+    {
       std::vector<Eigen::Vector3d> vertices;
       objectToVertices(object.kinematics.pose_with_covariance.pose, object.shape, vertices);
       transformPoints(vertices, object2camera_affine, vertices_camera_coord);
@@ -154,7 +172,7 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
         }
       }
     }
-    if (point_on_image_cnt == 3) {
+    if (point_on_image_cnt < 3) {
       continue;
     }
 
@@ -163,16 +181,18 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
     max_x = std::min(max_x, image_width - 1);
     max_y = std::min(max_y, image_height - 1);
 
-    // build roi
-    RegionOfInterest roi;
-    roi.x_offset = static_cast<std::uint32_t>(min_x);
-    roi.y_offset = static_cast<std::uint32_t>(min_y);
-    roi.width = static_cast<std::uint32_t>(max_x) - static_cast<std::uint32_t>(min_x);
-    roi.height = static_cast<std::uint32_t>(max_y) - static_cast<std::uint32_t>(min_y);
-    object_roi_map.insert(std::make_pair(obj_i, roi));
+    DetectedObjectWithFeature object_roi;
+    object_roi.feature.roi.x_offset = static_cast<std::uint32_t>(min_x);
+    object_roi.feature.roi.y_offset = static_cast<std::uint32_t>(min_y);
+    object_roi.feature.roi.width =
+      static_cast<std::uint32_t>(max_x) - static_cast<std::uint32_t>(min_x);
+    object_roi.feature.roi.height =
+      static_cast<std::uint32_t>(max_y) - static_cast<std::uint32_t>(min_y);
+    object_roi.object = object;
+    object_roi_map.insert(std::make_pair(obj_i, object_roi));
 
     if (debugger_) {
-      debugger_->obstacle_rois_.push_back(roi);
+      debugger_->obstacle_rois_.push_back(object_roi.feature.roi);
     }
   }
 
@@ -182,7 +202,7 @@ std::map<std::size_t, RegionOfInterest> RoiDetectedObjectFusionNode::generateDet
 void RoiDetectedObjectFusionNode::fuseObjectsOnImage(
   const DetectedObjects & input_object_msg,
   const std::vector<DetectedObjectWithFeature> & image_rois,
-  const std::map<std::size_t, sensor_msgs::msg::RegionOfInterest> & object_roi_map)
+  const std::map<std::size_t, DetectedObjectWithFeature> & object_roi_map)
 {
   int64_t timestamp_nsec =
     input_object_msg.header.stamp.sec * (int64_t)1e9 + input_object_msg.header.stamp.nanosec;
@@ -207,7 +227,15 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage(
     float max_iou = 0.0f;
     for (const auto & image_roi : image_rois) {
       const auto & object_roi = object_pair.second;
-      const double iou = calcIoU(object_roi, image_roi.feature.roi);
+      const auto object_roi_label =
+        object_recognition_utils::getHighestProbLabel(object_roi.object.classification);
+      const auto image_roi_label =
+        object_recognition_utils::getHighestProbLabel(image_roi.object.classification);
+      if (!fusion_params_.can_assign_matrix(object_roi_label, image_roi_label)) {
+        continue;
+      }
+
+      const double iou = calcIoU(object_roi.feature.roi, image_roi.feature.roi);
       if (iou > max_iou) {
         max_iou = iou;
         roi_prob = image_roi.object.existence_probability;
@@ -301,7 +329,7 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg)
 
   passthrough_object_flags_map_.erase(timestamp_nsec);
   fused_object_flags_map_.erase(timestamp_nsec);
-  fused_object_flags_map_.erase(timestamp_nsec);
+  ignored_object_flags_map_.erase(timestamp_nsec);
 }
 
 }  // namespace image_projection_based_fusion