Merge pull request autowarefoundation#719 from tier4/hotfix/perception

fix(image_projection_based_fusion, tier4_perception_launch): update camera-lidar-fusion based detection

launch/tier4_perception_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>
     <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>

perception/image_projection_based_fusion/CMakeLists.txt

@@ -124,15 +124,15 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
       else()
         message(STATUS "diff ${FILE_NAME}")
         message(STATUS "File hash changes. Downloading now ...")
-        file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME}
+        file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME}
                       ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
         list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
         list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
       endif()
     else()
       message(STATUS "not found ${FILE_NAME}")
       message(STATUS "File doesn't exists. Downloading now ...")
-      file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME}
+      file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME}
                     ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300)
       list(GET DOWNLOAD_STATUS 0 STATUS_CODE)
       list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE)
@@ -145,8 +145,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   endfunction()
 
   # default model
-  download(pts_voxel_encoder_pointpainting.onnx e674271096f6fbbe0f808eef87f5a5ab)
-  download(pts_backbone_neck_head_pointpainting.onnx d4527a4da08959c2bf9c997112a1de35)
+  download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410)
+  download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868)
   find_package(OpenCV REQUIRED)
   find_package(Eigen3 REQUIRED)
 

perception/image_projection_based_fusion/config/pointpainting.param.yaml

@@ -1,15 +1,16 @@
 /**:
   ros__parameters:
     class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
-    point_feature_size: 9  # x, y, z, time-lag and car, pedestrian, bicycle
+    paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]
+    point_feature_size: 7  # x, y, z, time-lag and car, pedestrian, bicycle
     max_voxel_size: 40000
-    point_cloud_range: [-102.4, -102.4, -4.0, 102.4, 102.4, 6.0]
-    voxel_size: [0.32, 0.32, 10.0]
+    point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
+    voxel_size: [0.32, 0.32, 8.0]
     downsample_factor: 1
-    encoder_in_feature_size: 14
+    encoder_in_feature_size: 12
     yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
     # post-process params
-    circle_nms_dist_threshold: 0.5
+    circle_nms_dist_threshold: 0.3
     iou_nms_target_class_names: ["CAR"]
     iou_nms_search_distance_2d: 10.0
     iou_nms_threshold: 0.1

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

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
 

perception/image_projection_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,15 +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(
-    const std::vector<DetectedObject> & input_objects, const double image_width,
-    const double image_height, const Eigen::Affine3d & object2camera_affine,
-    const Eigen::Matrix4d & camera_projection);
+  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 std::vector<DetectedObject> & objects,
+    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;
 
@@ -57,13 +58,16 @@ 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::vector<bool> passthrough_object_flags_, fused_object_flags_, ignored_object_flags_;
+  std::map<int64_t, std::vector<bool>> passthrough_object_flags_map_, fused_object_flags_map_,
+    ignored_object_flags_map_;
 };
 
 }  // namespace image_projection_based_fusion

perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml

@@ -41,7 +41,7 @@
     <node pkg="image_projection_based_fusion" exec="pointpainting_fusion_node" name="pointpainting" output="screen">
       <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
       <remap from="~/output/objects" to="$(var output/objects)"/>
-      <param name="score_threshold" value="0.45"/>
+      <param name="score_threshold" value="0.35"/>
       <param name="densification_world_frame_id" value="map"/>
       <param name="densification_num_past_frames" value="1"/>
       <param name="trt_precision" value="fp16"/>

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)"/>

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>

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -22,13 +22,26 @@
 #include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>
 #include <lidar_centerpoint/ros_utils.hpp>
 #include <lidar_centerpoint/utils.hpp>
+#include <pcl_ros/transforms.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/math/constants.hpp>
 
 #include <omp.h>
 
 #include <chrono>
 
+namespace
+{
+
+Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t)
+{
+  Eigen::Affine3f a;
+  a.matrix() = tf2::transformToEigen(t).matrix().cast<float>();
+  return a;
+}
+
+}  // namespace
+
 namespace image_projection_based_fusion
 {
 
@@ -102,8 +115,12 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   const std::string head_engine_path = this->declare_parameter("head_engine_path", "");
 
   class_names_ = this->declare_parameter<std::vector<std::string>>("class_names");
+  const auto paint_class_names =
+    this->declare_parameter<std::vector<std::string>>("paint_class_names");
   std::vector<std::string> classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"};
-  if (std::find(class_names_.begin(), class_names_.end(), "TRUCK") != class_names_.end()) {
+  if (
+    std::find(paint_class_names.begin(), paint_class_names.end(), "TRUCK") !=
+    paint_class_names.end()) {
     isClassTable_["CAR"] = std::bind(&isCar, std::placeholders::_1);
   } else {
     isClassTable_["CAR"] = std::bind(&isVehicle, std::placeholders::_1);
@@ -113,9 +130,9 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   isClassTable_["BICYCLE"] = std::bind(&isBicycle, std::placeholders::_1);
   isClassTable_["PEDESTRIAN"] = std::bind(&isPedestrian, std::placeholders::_1);
   for (const auto & cls : classes_) {
-    auto it = find(class_names_.begin(), class_names_.end(), cls);
-    if (it != class_names_.end()) {
-      int index = it - class_names_.begin();
+    auto it = find(paint_class_names.begin(), paint_class_names.end(), cls);
+    if (it != paint_class_names.end()) {
+      int index = it - paint_class_names.begin();
       class_index_[cls] = index + 1;
     } else {
       isClassTable_.erase(cls);
@@ -253,39 +270,38 @@ void PointPaintingFusionNode::fuseOnSingleImage(
   std::vector<Eigen::Vector2d> debug_image_points;
 
   // get transform from cluster frame id to camera optical frame id
-  geometry_msgs::msg::TransformStamped transform_stamped;
+  // geometry_msgs::msg::TransformStamped transform_stamped;
+  Eigen::Affine3f lidar2cam_affine;
   {
     const auto transform_stamped_optional = getTransformStamped(
-      tf_buffer_, /*target*/ camera_info.header.frame_id,
-      /*source*/ painted_pointcloud_msg.header.frame_id, camera_info.header.stamp);
+      tf_buffer_, /*target*/ input_roi_msg.header.frame_id,
+      /*source*/ painted_pointcloud_msg.header.frame_id, input_roi_msg.header.stamp);
     if (!transform_stamped_optional) {
       return;
     }
-    transform_stamped = transform_stamped_optional.value();
+    lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform);
   }
 
   // transform
-  sensor_msgs::msg::PointCloud2 transformed_pointcloud;
-  tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped);
-
-  std::chrono::system_clock::time_point start, end;
-  start = std::chrono::system_clock::now();
+  // sensor_msgs::msg::PointCloud2 transformed_pointcloud;
+  // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped);
 
   const auto x_offset =
-    transformed_pointcloud.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::X))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::X))
       .offset;
   const auto y_offset =
-    transformed_pointcloud.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Y))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Y))
       .offset;
   const auto z_offset =
-    transformed_pointcloud.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Z))
+    painted_pointcloud_msg.fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Z))
       .offset;
   const auto class_offset = painted_pointcloud_msg.fields.at(4).offset;
-  const auto p_step = transformed_pointcloud.point_step;
+  const auto p_step = painted_pointcloud_msg.point_step;
   // projection matrix
   Eigen::Matrix3f camera_projection;  // use only x,y,z
   camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
     camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6);
+  Eigen::Vector3f point_lidar, point_camera;
   /** dc : don't care
 
 x    | f  x1 x2  dc ||xc|
@@ -295,18 +311,24 @@ dc   | dc dc dc  dc ||zc|
    **/
 
   auto objects = input_roi_msg.feature_objects;
-  int iterations = transformed_pointcloud.data.size() / transformed_pointcloud.point_step;
+  int iterations = painted_pointcloud_msg.data.size() / painted_pointcloud_msg.point_step;
   // iterate points
   // Requires 'OMP_NUM_THREADS=N'
   omp_set_num_threads(omp_num_threads_);
 #pragma omp parallel for
   for (int i = 0; i < iterations; i++) {
     int stride = p_step * i;
-    unsigned char * data = &transformed_pointcloud.data[0];
+    unsigned char * data = &painted_pointcloud_msg.data[0];
     unsigned char * output = &painted_pointcloud_msg.data[0];
     float p_x = *reinterpret_cast<const float *>(&data[stride + x_offset]);
     float p_y = *reinterpret_cast<const float *>(&data[stride + y_offset]);
     float p_z = *reinterpret_cast<const float *>(&data[stride + z_offset]);
+    point_lidar << p_x, p_y, p_z;
+    point_camera = lidar2cam_affine * point_lidar;
+    p_x = point_camera.x();
+    p_y = point_camera.y();
+    p_z = point_camera.z();
+
     if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) {
       continue;
     }