diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 2d983dc47545c..6e3883346b4a4 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -224,12 +224,44 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt
index 8c0ba744491e7..906ad71d21732 100644
--- a/perception/image_projection_based_fusion/CMakeLists.txt
+++ b/perception/image_projection_based_fusion/CMakeLists.txt
@@ -124,7 +124,7 @@ 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)
@@ -132,7 +132,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
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)
diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml
index 56346b5a3eb41..e1be5426cba4b 100755
--- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml
+++ b/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
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
new file mode 100755
index 0000000000000..bd49dc65749a9
--- /dev/null
+++ 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
index accdc350bf256..4e054fc911dc6 100644
--- 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/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
index 8168ad2c9f265..e11a62c060480 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
+++ b/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