chore: sync upstream

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 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>

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,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_,

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/fusion_node.cpp

@@ -205,7 +205,8 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
   // if matching rois exist, fuseOnSingle
   for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
     if (camera_info_map_.find(roi_i) == camera_info_map_.end()) {
-      RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i);
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i);
       continue;
     }
 
@@ -298,7 +299,8 @@ void FusionNode<Msg, Obj>::roiCallback(
 
     if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) {
       if (camera_info_map_.find(roi_i) == camera_info_map_.end()) {
-        RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i);
+        RCLCPP_WARN_THROTTLE(
+          this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i);
         (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg;
         return;
       }