feat: radar object fusion

perception/radar_fusion_to_detected_object/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.5)
+project(radar_fusion_to_detected_object)
+
+## Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+## Dependencies
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+## Targets
+ament_auto_add_library(radar_object_fusion_to_detected_object_node_component SHARED
+  src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp
+  src/radar_fusion_to_detected_object.cpp
+)
+
+rclcpp_components_register_node(radar_object_fusion_to_detected_object_node_component
+  PLUGIN "radar_fusion_to_detected_object::RadarObjectFusionToDetectedObjectNode"
+  EXECUTABLE radar_object_fusion_to_detected_object_node
+)
+
+## Tests
+if(BUILD_TESTING)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
+
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+## Package
+ament_auto_package(
+  INSTALL_TO_SHARE
+    config
+    launch
+)

perception/radar_fusion_to_detected_object/README.md

@@ -0,0 +1,75 @@
+# radar_fusion_to_detected_object
+
+This package is radar-based sensor fusion module to 3d detected object.
+Main feature of this package are as following.
+
+- Attach velocity to lidar detection result from radar data to improve for tracking result and planning like adaptive cruise.
+- Improve detection result with radar sensor information. If both lidar 3d detected objects with low score and high confidence of radar pointcloud / objects, then improve score of objects.
+
+![process_low_confidence](docs/radar_fusion_to_detected_object_6.drawio.svg)
+
+## Core algorithm
+
+The document of core algorithm is [here](docs/algorithm.md)
+
+### Parameters for sensor fusion
+
+| Name                     | Type   | Description                                                                                            | Default value |
+| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ |
+| bounding_box_margin      | double | The margin distance from bird's-eye view to choose radar pointcloud/objects within 3D bounding box [m] | 2.0           |
+| split_threshold_velocity | double | The threshold velocity to judge splitting two objects from radar information (now unused) [m/s]        | 5.0           |
+
+### Weight parameters for velocity estimation
+
+| Name                                 | Type   | Description                                                                                                                                                                             | Default value |
+| :----------------------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| velocity_weight_average              | double | The twist coefficient of average twist of radar data in velocity estimation.                                                                                                            | 0.0           |
+| velocity_weight_median               | double | The twist coefficient of median twist of radar data in velocity estimation.                                                                                                             | 0.0           |
+| velocity_weight_min_distance         | double | The twist coefficient of nearest radar data to the center of bounding box in velocity estimation.                                                                                       | 1.0           |
+| velocity_weight_target_value_average | double | The twist coefficient of target value weighted average in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects. |
+| 0.0                                  |
+| velocity_weight_target_value_top     | double | The twist coefficient of top target value radar data in velocity estimation. Target value is amplitude if using radar pointcloud. Target value is probability if using radar objects.   | 0.0           |
+
+### Parameters for fixed object information
+
+| Name                     | Type  | Description                                                                                                                                             | Default value |
+| :----------------------- | :---- | :------------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ |
+| convert_doppler_to_twist | bool  | Convert doppler velocity to twist using the yaw information of a detected object.                                                                       | false         |
+| threshold_probability    | float | If the probability of an output object is lower than this parameter, and the output object doesn not have radar points/objects, then delete the object. | 0.4           |
+
+## radar_object_fusion_to_detected_object
+
+Sensor fusion with radar objects and a detected object.
+
+- Calculation cost is O(nm).
+  - n: the number of radar objects.
+  - m: the number of objects from 3d detection.
+
+### How to launch
+
+```sh
+roslaunch radar_fusion_to_detected_object radar_object_to_detected_object.launch
+```
+
+### Input
+
+| Name                    | Type                                                 | Description          |
+| ----------------------- | ---------------------------------------------------- | -------------------- |
+| `~/input/objects`       | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects. |
+| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/TrackedObjects.msg | Radar objects.       |
+
+### Output
+
+| Name               | Type                                                  | Description                    |
+| ------------------ | ----------------------------------------------------- | ------------------------------ |
+| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | 3D detected object with twist. |
+
+### Parameters
+
+| Name           | Type   | Description           | Default value |
+| :------------- | :----- | :-------------------- | :------------ |
+| update_rate_hz | double | The update rate [hz]. | 20.0          |
+
+## radar_scan_fusion_to_detected_object (TBD)
+
+TBD

perception/radar_fusion_to_detected_object/config/radar_object_fusion_to_detected_object.param.yaml

@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    node_params:
+      update_rate_hz: 10.0
+
+    core_params:
+      bounding_box_margin: 2.0
+      split_threshold_velocity: 5.0
+      velocity_weight_average: 0.0
+      velocity_weight_median: 0.0
+      velocity_weight_min_distance: 1.0
+      velocity_weight_target_value_average: 0.0
+      velocity_weight_target_value_top: 0.0
+      convert_doppler_to_twist: false
+      threshold_probability: 0.4

perception/radar_fusion_to_detected_object/docs/algorithm.md

@@ -0,0 +1,34 @@
+## Common Algorithm
+
+### 1. Link between 3d bounding box and radar data
+
+Choose radar pointcloud/objects within 3D bounding box from lidar-base detection with margin space from bird's-eye view.
+
+![choose_radar](radar_fusion_to_detected_object_1.drawio.svg)
+
+### 2. [Feature support] Split the object going in a different direction
+
+- Split two object for the low confidence object that can be estimated to derive two object.
+
+![process_low_confidence](radar_fusion_to_detected_object_4.drawio.svg)
+
+### 3. Estimate twist of object
+
+Estimate twist from chosen radar pointcloud/objects.
+Attach object to twist information of estimated twist.
+
+![estimate_doppler_velocity](radar_fusion_to_detected_object_2.drawio.svg)
+
+### 4. [Feature support] [Option] Convert doppler velocity to twist
+
+If the twist information of radars is doppler velocity, convert from doppler velocity to twist using yaw angle of DetectedObject.
+Because radar pointcloud has only doppler velocity information, radar pointcloud fusion should use this feature.
+On the other hand, because radar objects have twist information, radar object fusion should not use this feature.
+
+![process_high_confidence](radar_fusion_to_detected_object_3.drawio.svg)
+
+### 5. Delete objects with low probability
+
+- Delete low confidence objects that do not have some radar points/objects.
+
+![process_low_confidence](radar_fusion_to_detected_object_5.drawio.svg)