From be021a9a96a078ed9cff543dc2088a3eb6e3829c Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 14 Oct 2022 13:12:17 +0900 Subject: [PATCH] feat(centerpoint): update ceterpoint (#145) * fix(lidar_centerpoint): fix function sigmoid() (#1555) fix(lidar_centerpoint): fix function sigmoid() (#1545) Signed-off-by: WfHit <33973397+WfHit@users.noreply.github.com> * feat(lidar_centerpoint): add score threshold parameter to center point (#1699) Signed-off-by: scepter914 Signed-off-by: scepter914 * feat(behavior_path_planner): add new turn signal algorithm (#1964) * clean code Signed-off-by: yutaka * clean format Signed-off-by: yutaka * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add test Signed-off-by: yutaka * add test * add test Signed-off-by: yutaka * fix(avoidance): use new turn signal algorithm Signed-off-by: satoshi-ota * fix(avoidance): fix desired_start_point position Signed-off-by: satoshi-ota * update Signed-off-by: yutaka * change policy Signed-off-by: yutaka * feat(behavior_path_planner): update pull_over for new blinker logic Signed-off-by: kosuke55 * feat(behavior_path_planner): update pull_out for new blinker logic * tmp: install flask via pip Signed-off-by: Takayuki Murooka * feat(lane_change): added lane change point * fix start_point and non backward driving turn signal Signed-off-by: kosuke55 * get 3 second during constructing lane change path Signed-off-by: Muhammad Zulfaqar Azmi * fix pull over desired start point Signed-off-by: kosuke55 * update Signed-off-by: yutaka * delete Signed-off-by: yutaka * Update Readme * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * fix format Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: kosuke55 Co-authored-by: Takayuki Murooka Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Fumiya Watanabe Signed-off-by: tomoya.kimura * fix(lidar_centerpoint): include zero in range of yaw_norm_threshold (#1830) Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(multi_object_tracker): increase max-area for truck and trailer (#1710) * feat(multi_object_tracker): increase max-area for truck Signed-off-by: yukke42 * feat: change truck and trailer max-area gate params Signed-off-by: yukke42 * feat: change trailer params Signed-off-by: yukke42 Signed-off-by: yukke42 Signed-off-by: tomoya.kimura * feat(multi_object_tracker): update bus size (#1887) Signed-off-by: Yukihiro Saito Signed-off-by: Yukihiro Saito * fix(multi_object_tracker): add missing trailer tracker (#1885) Signed-off-by: yukke42 Signed-off-by: yukke42 * feat(lidar_centerpoint): implemented a class remapping according to the detections shape (#1876) * Implemented a small package to remap detection classes depending on their shape Signed-off-by: Kenzo Lobos-Tsunekawa * Update perception/detection_class_adapter/include/detection_class_adapter/detection_class_adapter.hpp Co-authored-by: Yukihiro Saito * Removed the hardcoded mapping from centerpoint and pointfusion. Fixed the description of the config file Signed-off-by: Kenzo Lobos-Tsunekawa * Deleted the new package and moved the logic to centerpoint Signed-off-by: Kenzo Lobos-Tsunekawa * Update perception/lidar_centerpoint/config/detection_class_adapter.param.yaml Co-authored-by: Yusuke Muramatsu * Changed: adapter->remapper * Update perception/lidar_centerpoint/lib/detection_class_remapper.cpp Co-authored-by: Yukihiro Saito * Delted duplicated file Signed-off-by: Kenzo Lobos-Tsunekawa * Modified the parameters to match #1710 Now we do not map cars to buses no mather the size Signed-off-by: Kenzo Lobos-Tsunekawa * Update perception/lidar_centerpoint/config/detection_class_remapper.param.yaml Co-authored-by: Yusuke Muramatsu Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Yukihiro Saito Co-authored-by: Yusuke Muramatsu Signed-off-by: tomoya.kimura * feat(lidar_centerpoint): add IoU-based NMS (#1935) * feat(lidar_centerpoint): add IoU-based NMS Signed-off-by: yukke42 * feat: add magic number name Signed-off-by: yukke42 * feat: remove unnecessary headers Signed-off-by: yukke42 * fix: typo Signed-off-by: yukke42 * fix: typo Signed-off-by: yukke42 Signed-off-by: yukke42 Signed-off-by: tomoya.kimura * feat(lidar_centerpoint): enabled per class yaw norm threshold (#1962) * feat(lidar_centerpoint): enabled per class yaw norm threshold Signed-off-by: Kenzo Lobos-Tsunekawa * Updated thresholds Signed-off-by: Kenzo Lobos-Tsunekawa * Moved the yaw threshold parameters to gpu memory Signed-off-by: Kenzo Lobos-Tsunekawa * Moved the thresholds as a member variable Signed-off-by: Kenzo Lobos-Tsunekawa Signed-off-by: Kenzo Lobos-Tsunekawa Signed-off-by: tomoya.kimura * fix(lidar_centeproint): fix cmake Signed-off-by: tomoya.kimura * revert(lidar_centerpoint): "fix(lidar_centeproint): fix cmake" This reverts commit 2efc31db9bfe37cda8ffa92c313a2488c41094d2. Signed-off-by: yukke42 * chore(lidar_centerpoint): add trained model of centerpoint (#1202) * chore(lidar_centerpoint): add trained model of centerpoint Signed-off-by: yukke42 * docs: add description of models Signed-off-by: yukke42 Signed-off-by: WfHit <33973397+WfHit@users.noreply.github.com> Signed-off-by: scepter914 Signed-off-by: yutaka Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: tomoya.kimura Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: yukke42 Signed-off-by: Yukihiro Saito Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: WfHit <33973397+WfHit@users.noreply.github.com> Co-authored-by: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: kosuke55 Co-authored-by: Takayuki Murooka Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Fumiya Watanabe Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Yukihiro Saito Co-authored-by: yukke42 --- .../data_association_matrix.param.yaml | 12 +- .../config/pointpainting.param.yaml | 2 +- .../pointpainting_fusion/node.hpp | 4 +- .../launch/pointpainting_fusion.launch.xml | 2 + .../src/pointpainting_fusion/node.cpp | 18 ++- perception/lidar_centerpoint/CMakeLists.txt | 26 +++-- perception/lidar_centerpoint/README.md | 51 +++++++-- .../config/aip_x2.param.yaml | 10 -- .../config/centerpoint.param.yaml | 15 +++ ...param.yaml => centerpoint_tiny.param.yaml} | 7 +- .../detection_class_remapper.param.yaml | 38 +++++++ .../lidar_centerpoint/centerpoint_config.hpp | 12 +- .../detection_class_remapper.hpp | 47 ++++++++ .../include/lidar_centerpoint/node.hpp | 7 +- .../postprocess/non_maximum_suppression.hpp | 81 ++++++++++++++ .../postprocess/postprocess_kernel.hpp | 1 + .../include/lidar_centerpoint/ros_utils.hpp | 3 +- .../launch/lidar_centerpoint.launch.xml | 8 +- .../lib/detection_class_remapper.cpp | 71 ++++++++++++ .../postprocess/non_maximum_suppression.cpp | 104 ++++++++++++++++++ .../lib/postprocess/postprocess_kernel.cu | 11 +- .../lidar_centerpoint/lib/ros_utils.cpp | 14 +-- perception/lidar_centerpoint/package.xml | 1 + perception/lidar_centerpoint/src/node.cpp | 42 +++++-- .../config/data_association_matrix.param.yaml | 12 +- .../config/default_tracker.param.yaml | 1 + .../multi_object_tracker/utils/utils.hpp | 7 ++ .../src/multi_object_tracker_core.cpp | 4 +- .../src/tracker/model/big_vehicle_tracker.cpp | 2 +- .../model/multiple_vehicle_tracker.cpp | 2 +- .../tracker/model/normal_vehicle_tracker.cpp | 2 +- perception/tensorrt_yolox | 1 + 32 files changed, 537 insertions(+), 81 deletions(-) mode change 100755 => 100644 perception/image_projection_based_fusion/config/pointpainting.param.yaml delete mode 100644 perception/lidar_centerpoint/config/aip_x2.param.yaml create mode 100644 perception/lidar_centerpoint/config/centerpoint.param.yaml rename perception/lidar_centerpoint/config/{default.param.yaml => centerpoint_tiny.param.yaml} (57%) create mode 100644 perception/lidar_centerpoint/config/detection_class_remapper.param.yaml create mode 100644 perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp create mode 100644 perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp create mode 100644 perception/lidar_centerpoint/lib/detection_class_remapper.cpp create mode 100644 perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp create mode 120000 perception/tensorrt_yolox diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 6fb495c2588f1..1790315ae557d 100644 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -22,12 +22,14 @@ 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN max_area_matrix: + # NOTE(yukke42): The size of truck is 12 m length x 3 m width. + # NOTE(yukke42): The size of trailer is 20 m length x 3 m width. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #CAR - 19.75, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRUCK - 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #BUS - 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRAILER + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml old mode 100755 new mode 100644 index 3aeebe2db4148..0340add0b2b55 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 6 # x, y, z and car, pedestrian, bicycle max_voxel_size: 40000 point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 encoder_in_feature_size: 11 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index 117722b72320c..53f21089b768d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -52,9 +53,10 @@ class PointpaintingFusionNode : public FusionNode class_names_; std::vector pointcloud_range; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + centerpoint::DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; bool out_of_scope(const DetectedObjects & obj); diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 3e44fddb7c5fd..d02a6484207b9 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -21,6 +21,7 @@ + @@ -74,6 +75,7 @@ + diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 870028bce8161..d5c860a9a5fc3 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -33,6 +33,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); // densification param const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); @@ -45,7 +47,6 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -57,6 +58,13 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -64,7 +72,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt densification_world_frame_id, densification_num_past_frames); centerpoint::CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold); + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds); // create detector detector_ptr_ = std::make_unique( @@ -237,11 +246,12 @@ void PointpaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte continue; } autoware_auto_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject( - box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); + centerpoint::box3DToDetectedObject(box3d, class_names_, has_twist_, obj); output_obj_msg.objects.emplace_back(obj); } + detection_class_remapper_.mapClasses(output_obj_msg); + obj_pub_ptr_->publish(output_obj_msg); } diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index a67484b7353ea..7a1a9d7bfcda7 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -70,10 +70,12 @@ message(STATUS "start to download") if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) # Download trained models set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) + set(DOWNLOAD_BASE_URL https://awf.ml.dev.web.auto/perception/models/centerpoint) execute_process(COMMAND mkdir -p ${DATA_PATH}) - function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") + function(download VERSION FILE_NAME FILE_HASH) + message(STATUS "Checking and downloading ${FILE_NAME} ") + set(DOWNLOAD_URL ${DOWNLOAD_BASE_URL}/${VERSION}/${FILE_NAME}) set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) message(STATUS "start ${FILE_NAME}") @@ -86,31 +88,31 @@ 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/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) + file(DOWNLOAD ${DOWNLOAD_URL} ${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/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) + file(DOWNLOAD ${DOWNLOAD_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() if(${STATUS_CODE} EQUAL 0) message(STATUS "Download completed successfully!") else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") + message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE} ${DOWNLOAD_URL}") endif() endfunction() - # default model - download(pts_voxel_encoder_default.onnx 410f730c537968cb27fbd70c941849a8) - download(pts_backbone_neck_head_default.onnx e97c165c7877222c0e27e44409a07517) + # centerpoint + download(v1 pts_voxel_encoder_centerpoint.onnx 94dcf01ccd53ef14a79787afaada63b4) + download(v1 pts_backbone_neck_head_centerpoint.onnx 7046995e502e11f35c9459668d8afde3) - # aip_x2 model - download(pts_voxel_encoder_aip_x2.onnx 3ae5e9efd7b2ed12115e6f0b28cac58d) - download(pts_backbone_neck_head_aip_x2.onnx 6a406a19e05660677c162486ab332de8) + # centerpoint_tiny + download(v1 pts_voxel_encoder_centerpoint_tiny.onnx 410f730c537968cb27fbd70c941849a8) + download(v1 pts_backbone_neck_head_centerpoint_tiny.onnx e97c165c7877222c0e27e44409a07517) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -123,10 +125,12 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ### centerpoint ### ament_auto_add_library(centerpoint_lib SHARED lib/centerpoint_trt.cpp + lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp lib/network/network_trt.cpp lib/network/tensorrt_wrapper.cpp + lib/postprocess/non_maximum_suppression.cpp lib/preprocess/pointcloud_densification.cpp lib/preprocess/voxel_generator.cpp ) diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 5955666cda17a..72eb5498b71d4 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -30,21 +30,45 @@ We trained the models using . ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------- | ------ | ------------- | ----------------------------------------------------------- | -| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | -| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | -| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | -| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | -| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | -| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | -| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | -| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| Name | Type | Default Value | Description | +| ------------------------------- | ------------ | ------------- | ------------------------------------------------------------- | +| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored | +| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud | +| `densification_num_past_frames` | int | `1` | the number of past frames to fuse with the current frame | +| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` | +| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file | +| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | +| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | +| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| `nms_iou_target_class_names` | list[string] | - | target classes for IoU-based Non Maximum Suppression | +| `nms_iou_search_distance_2d` | double | - | If two objects are farther than the value, NMS isn't applied. | +| `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | ## Assumptions / Known limits - The `object.existence_probability` is stored the value of classification confidence of a DNN, not probability. +## Trained Models + +You can download the onnx format of trained models by clicking on the links below. + +### Changelog + +#### v1 (2022/07/06) + +| Name | URLs | Description | +| ------------------ | -------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------- | +| `centerpoint` | [pts_voxel_encoder][v1-encoder-centerpoint]
[pts_backbone_neck_head][v1-head-centerpoint] | There is a single change due to the limitation in the implementation of this package. `num_filters=[32, 32]` of `PillarFeatureNet` | +| `centerpoint_tiny` | [pts_voxel_encoder][v1-encoder-centerpoint-tiny]
[pts_backbone_neck_head][v1-head-centerpoint-tiny] | The same model as `default` of `v0`. | + +These changes are compared with [this configuration](https://github.com/tianweiy/CenterPoint/blob/v0.2/configs/waymo/pp/waymo_centerpoint_pp_two_pfn_stride1_3x.py). + +#### v0 (2021/12/03) + +| Name | URLs | Description | +| --------- | -------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------- | +| `default` | [pts_voxel_encoder][v0-encoder-default]
[pts_backbone_neck_head][v0-head-default] | There are two changes from the original CenterPoint architecture. `num_filters=[32]` of `PillarFeatureNet` and `ds_layer_strides=[2, 2, 2]` of `RPN` | + ## (Optional) Error detection and handling + +[v0-encoder-default]: https://awf.ml.dev.web.auto/perception/models/pts_voxel_encoder_default.onnx +[v0-head-default]: https://awf.ml.dev.web.auto/perception/models/pts_backbone_neck_head_default.onnx +[v1-encoder-centerpoint]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_voxel_encoder_centerpoint.onnx +[v1-head-centerpoint]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_backbone_neck_head_centerpoint.onnx +[v1-encoder-centerpoint-tiny]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_voxel_encoder_centerpoint_tiny.onnx +[v1-head-centerpoint-tiny]: https://awf.ml.dev.web.auto/perception/models/centerpoint/v1/pts_backbone_neck_head_centerpoint_tiny.onnx diff --git a/perception/lidar_centerpoint/config/aip_x2.param.yaml b/perception/lidar_centerpoint/config/aip_x2.param.yaml deleted file mode 100644 index 02979979954dc..0000000000000 --- a/perception/lidar_centerpoint/config/aip_x2.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] - rename_car_to_truck_and_bus: false - point_feature_size: 4 - max_voxel_size: 40000 - point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] - voxel_size: [0.32, 0.32, 8.0] - downsample_factor: 2 - encoder_in_feature_size: 9 diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml new file mode 100644 index 0000000000000..42a15067d55eb --- /dev/null +++ b/perception/lidar_centerpoint/config/centerpoint.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] + point_feature_size: 4 + max_voxel_size: 40000 + point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] + voxel_size: [0.32, 0.32, 8.0] + downsample_factor: 1 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/lidar_centerpoint/config/default.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml similarity index 57% rename from perception/lidar_centerpoint/config/default.param.yaml rename to perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 0b23c48887b6f..b7c2faf89c0b6 100644 --- a/perception/lidar_centerpoint/config/default.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -1,10 +1,15 @@ /**: ros__parameters: class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - rename_car_to_truck_and_bus: true point_feature_size: 4 max_voxel_size: 40000 point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] voxel_size: [0.32, 0.32, 8.0] downsample_factor: 2 encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.0, 0.3] diff --git a/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..ed378ffa44a70 --- /dev/null +++ b/perception/lidar_centerpoint/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, inf, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, inf, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 373e661650e16..dfbcf1e2eda1a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -27,7 +27,8 @@ class CenterPointConfig const std::size_t class_size, const float point_feature_size, const std::size_t max_voxel_size, const std::vector & point_cloud_range, const std::vector & voxel_size, const std::size_t downsample_factor, const std::size_t encoder_in_feature_size, - const float score_threshold, const float circle_nms_dist_threshold) + const float score_threshold, const float circle_nms_dist_threshold, + const std::vector yaw_norm_thresholds) { class_size_ = class_size; point_feature_size_ = point_feature_size; @@ -56,6 +57,14 @@ class CenterPointConfig circle_nms_dist_threshold_ = circle_nms_dist_threshold; } + yaw_norm_thresholds_ = + std::vector(yaw_norm_thresholds.begin(), yaw_norm_thresholds.end()); + + for (auto & yaw_norm_threshold : yaw_norm_thresholds_) { + yaw_norm_threshold = + (yaw_norm_threshold >= 0.f && yaw_norm_threshold < 1.f) ? yaw_norm_threshold : 0.f; + } + grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); grid_size_y_ = static_cast((range_max_y_ - range_min_y_) / voxel_size_y_); grid_size_z_ = static_cast((range_max_z_ - range_min_z_) / voxel_size_z_); @@ -97,6 +106,7 @@ class CenterPointConfig // post-process params float score_threshold_{0.4f}; float circle_nms_dist_threshold_{1.5f}; + std::vector yaw_norm_thresholds_{}; // calculated params std::size_t grid_size_x_ = (range_max_x_ - range_min_x_) / voxel_size_x_; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp new file mode 100644 index 0000000000000..18a78f122e170 --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace centerpoint +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 66ffdfd400cfd..474c9884eb36f 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -15,7 +15,10 @@ #ifndef LIDAR_CENTERPOINT__NODE_HPP_ #define LIDAR_CENTERPOINT__NODE_HPP_ +#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + #include +#include #include #include #include @@ -49,9 +52,11 @@ class LidarCenterPointNode : public rclcpp::Node float score_threshold_{0.0}; std::vector class_names_; - bool rename_car_to_truck_and_bus_{false}; bool has_twist_{false}; + NonMaximumSuppression iou_bev_nms_; + DetectionClassRemapper detection_class_remapper_; + std::unique_ptr detector_ptr_{nullptr}; // debugger diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp new file mode 100644 index 0000000000000..78e362f77e7f1 --- /dev/null +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp @@ -0,0 +1,81 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ + +#include "lidar_centerpoint/ros_utils.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/detected_object.hpp" + +#include +#include + +namespace centerpoint +{ +using autoware_auto_perception_msgs::msg::DetectedObject; + +// TODO(yukke42): now only support IoU_BEV +enum class NMS_TYPE { + IoU_BEV + // IoU_3D + // Distance_2D + // Distance_3D +}; + +struct NMSParams +{ + NMS_TYPE nms_type_{}; + std::vector target_class_names_{}; + double search_distance_2d_{}; + double iou_threshold_{}; + // double distance_threshold_{}; +}; + +std::vector classNamesToBooleanMask(const std::vector & class_names) +{ + std::vector mask; + constexpr std::size_t num_object_classification = 8; + mask.resize(num_object_classification); + for (const auto & class_name : class_names) { + const auto semantic_type = getSemanticType(class_name); + mask.at(semantic_type) = true; + } + + return mask; +} + +class NonMaximumSuppression +{ +public: + void setParameters(const NMSParams &); + + std::vector apply(const std::vector &); + +private: + bool isTargetLabel(const std::uint8_t); + + bool isTargetPairObject(const DetectedObject &, const DetectedObject &); + + Eigen::MatrixXd generateIoUMatrix(const std::vector &); + + NMSParams params_{}; + std::vector target_class_mask_{}; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp index e005be30a4c1a..d790e896dea9a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/postprocess_kernel.hpp @@ -39,6 +39,7 @@ class PostProcessCUDA private: CenterPointConfig config_; thrust::device_vector boxes3d_d_; + thrust::device_vector yaw_norm_thresholds_d_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp index 8204cac708bad..69b75b614498e 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/ros_utils.hpp @@ -31,8 +31,7 @@ namespace centerpoint { void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj); uint8_t getSemanticType(const std::string & class_name); diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 1dbc247377c8c..55690b145c5a0 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -2,14 +2,17 @@ - + + + + - + @@ -18,5 +21,6 @@ + diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..4637dc5537f37 --- /dev/null +++ b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp @@ -0,0 +1,71 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace centerpoint +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert( + std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_auto_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp new file mode 100644 index 0000000000000..5adcd11078a03 --- /dev/null +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -0,0 +1,104 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + +#include "perception_utils/geometry.hpp" +#include "perception_utils/perception_utils.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +namespace centerpoint +{ + +void NonMaximumSuppression::setParameters(const NMSParams & params) +{ + assert(params.search_distance_2d_ >= 0.0); + assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); + + params_ = params; + target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); +} + +bool NonMaximumSuppression::isTargetLabel(const uint8_t label) +{ + if (label >= target_class_mask_.size()) { + return false; + } + return target_class_mask_.at(label); +} + +bool NonMaximumSuppression::isTargetPairObject( + const DetectedObject & object1, const DetectedObject & object2) +{ + const auto label1 = perception_utils::getHighestProbLabel(object1.classification); + const auto label2 = perception_utils::getHighestProbLabel(object2.classification); + + if (isTargetLabel(label1) && isTargetLabel(label2)) { + return true; + } + + const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; + const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + perception_utils::getPose(object1), perception_utils::getPose(object2)); + return sqr_dist_2d <= search_sqr_dist_2d; +} + +Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( + const std::vector & input_objects) +{ + // NOTE(yukke42): row = target objects to be suppressed, col = source objects to be compared + Eigen::MatrixXd triangular_matrix = + Eigen::MatrixXd::Zero(input_objects.size(), input_objects.size()); + for (std::size_t target_i = 0; target_i < input_objects.size(); ++target_i) { + for (std::size_t source_i = 0; source_i < target_i; ++source_i) { + const auto & target_obj = input_objects.at(target_i); + const auto & source_obj = input_objects.at(source_i); + if (!isTargetPairObject(target_obj, source_obj)) { + continue; + } + + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + const double iou = perception_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE(yukke42): If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; + } + } + } + } + + return triangular_matrix; +} + +std::vector NonMaximumSuppression::apply( + const std::vector & input_objects) +{ + Eigen::MatrixXd iou_matrix = generateIoUMatrix(input_objects); + + std::vector output_objects; + output_objects.reserve(input_objects.size()); + for (std::size_t i = 0; i < input_objects.size(); ++i) { + const auto value = iou_matrix.row(i).maxCoeff(); + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); + } + } + } + + return output_objects; +} +} // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 0f7623ded603a..8942bdd33ceb5 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -17,6 +17,7 @@ #include #include +#include #include namespace @@ -47,14 +48,14 @@ struct score_greater __device__ bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } }; -__device__ inline float sigmoid(float x) { return 1.0f / expf(-x); } +__device__ inline float sigmoid(float x) { return 1.0f / (1.0f + expf(-x)); } __global__ void generateBoxes3D_kernel( const float * out_heatmap, const float * out_offset, const float * out_z, const float * out_dim, const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - Box3D * det_boxes3d) + const float * yaw_norm_thresholds, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -88,11 +89,12 @@ __global__ void generateBoxes3D_kernel( const float h = out_dim[down_grid_size * 2 + idx]; const float yaw_sin = out_rot[down_grid_size * 0 + idx]; const float yaw_cos = out_rot[down_grid_size * 1 + idx]; + const float yaw_norm = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); const float vel_x = out_vel[down_grid_size * 0 + idx]; const float vel_y = out_vel[down_grid_size * 1 + idx]; det_boxes3d[idx].label = label; - det_boxes3d[idx].score = max_score; + det_boxes3d[idx].score = yaw_norm >= yaw_norm_thresholds[label] ? max_score : 0.f; det_boxes3d[idx].x = x; det_boxes3d[idx].y = y; det_boxes3d[idx].z = z; @@ -108,6 +110,8 @@ PostProcessCUDA::PostProcessCUDA(const CenterPointConfig & config) : config_(con { const auto num_raw_boxes3d = config.down_grid_size_y_ * config.down_grid_size_x_; boxes3d_d_ = thrust::device_vector(num_raw_boxes3d); + yaw_norm_thresholds_d_ = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); } cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( @@ -123,6 +127,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, + thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by socre diff --git a/perception/lidar_centerpoint/lib/ros_utils.cpp b/perception/lidar_centerpoint/lib/ros_utils.cpp index 57237609139e5..9f18dfc80974d 100644 --- a/perception/lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/lidar_centerpoint/lib/ros_utils.cpp @@ -23,8 +23,7 @@ namespace centerpoint using Label = autoware_auto_perception_msgs::msg::ObjectClassification; void box3DToDetectedObject( - const Box3D & box3d, const std::vector & class_names, - const bool rename_car_to_truck_and_bus, const bool has_twist, + const Box3D & box3d, const std::vector & class_names, const bool has_twist, autoware_auto_perception_msgs::msg::DetectedObject & obj) { // TODO(yukke42): the value of classification confidence of DNN, not probability. @@ -41,17 +40,6 @@ void box3DToDetectedObject( rclcpp::get_logger("lidar_centerpoint"), "Unexpected label: UNKNOWN is set."); } - float l = box3d.length; - float w = box3d.width; - if (classification.label == Label::CAR && rename_car_to_truck_and_bus) { - // Note: object size is referred from multi_object_tracker - if ((w * l > 2.2 * 5.5) && (w * l <= 2.5 * 7.9)) { - classification.label = Label::TRUCK; - } else if (w * l > 2.5 * 7.9) { - classification.label = Label::BUS; - } - } - if (isCarLikeVehicleLabel(classification.label)) { obj.kinematics.orientation_availability = autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 71e8f71f6212f..4372b1848d08b 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -13,6 +13,7 @@ autoware_auto_perception_msgs pcl_ros + perception_utils rclcpp rclcpp_components tf2_eigen diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index c733171d78c99..93fdbc22644c8 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -26,6 +26,9 @@ #include #endif +#include +#include + #include #include #include @@ -38,7 +41,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const float score_threshold = static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + static_cast(this->declare_parameter("circle_nms_dist_threshold")); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); const int densification_num_past_frames = @@ -50,7 +55,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); const std::string head_engine_path = this->declare_parameter("head_engine_path"); class_names_ = this->declare_parameter>("class_names"); - rename_car_to_truck_and_bus_ = this->declare_parameter("rename_car_to_truck_and_bus", false); has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -62,6 +66,23 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti static_cast(this->declare_parameter("downsample_factor")); const std::size_t encoder_in_feature_size = static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + { + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names"); + p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + iou_bev_nms_.setParameters(p); + } NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); @@ -80,7 +101,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti } CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold); + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); @@ -120,14 +142,20 @@ void LidarCenterPointNode::pointCloudCallback( return; } - autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = input_pointcloud_msg->header; + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, rename_car_to_truck_and_bus_, has_twist_, obj); - output_msg.objects.emplace_back(obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + raw_objects.emplace_back(obj); } + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = input_pointcloud_msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + if (objects_sub_count > 0) { objects_pub_->publish(output_msg); } diff --git a/perception/multi_object_tracker/config/data_association_matrix.param.yaml b/perception/multi_object_tracker/config/data_association_matrix.param.yaml index 6fb495c2588f1..1790315ae557d 100644 --- a/perception/multi_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/multi_object_tracker/config/data_association_matrix.param.yaml @@ -22,12 +22,14 @@ 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN max_area_matrix: + # NOTE(yukke42): The size of truck is 12 m length x 3 m width. + # NOTE(yukke42): The size of trailer is 20 m length x 3 m width. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #CAR - 19.75, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRUCK - 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #BUS - 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRAILER + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/default_tracker.param.yaml index 495df3a30d96e..23e88d8274172 100644 --- a/perception/multi_object_tracker/config/default_tracker.param.yaml +++ b/perception/multi_object_tracker/config/default_tracker.param.yaml @@ -3,6 +3,7 @@ car_tracker: "multi_vehicle_tracker" truck_tracker: "multi_vehicle_tracker" bus_tracker: "multi_vehicle_tracker" + trailer_tracker: "multi_vehicle_tracker" pedestrian_tracker: "pedestrian_and_bicycle_tracker" bicycle_tracker: "pedestrian_and_bicycle_tracker" motorcycle_tracker: "pedestrian_and_bicycle_tracker" diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index f754b7e57d708..ad3eefb02a999 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -90,6 +90,13 @@ enum MSG_COV_IDX { YAW_PITCH = 34, YAW_YAW = 35 }; + +inline bool isLargeVehicleLabel(const uint8_t label) +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d8a90e14a5e77..acf7fc553c516 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -136,7 +136,7 @@ bool isSpecificAlivePattern( tracker->getTotalMeasurementCount() / (tracker->getTotalNoMeasurementCount() + tracker->getTotalMeasurementCount()); - const bool big_vehicle = (label == Label::TRUCK || label == Label::BUS); + const bool big_vehicle = utils::isLargeVehicleLabel(label); const bool slow_velocity = getVelocity(object) < max_velocity; @@ -202,6 +202,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); tracker_map_.insert( std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); tracker_map_.insert( std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); tracker_map_.insert( diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 8c15ec8d94adf..4b394ca518040 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -232,7 +232,7 @@ bool BigVehicleTracker::measureWithPose( constexpr float r_stddev_y = 0.8; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (label == Label::TRUCK || label == Label::BUS) { + } else if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else { diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index c80b7aa0e165d..4763cd987a6c9 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -55,7 +55,7 @@ bool MultipleVehicleTracker::getTrackedObject( if (label == Label::CAR) { normal_vehicle_tracker_.getTrackedObject(time, object); - } else if (label == Label::BUS || label == Label::TRUCK) { + } else if (utils::isLargeVehicleLabel(label)) { big_vehicle_tracker_.getTrackedObject(time, object); } object.object_id = getUUID(); diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 474bb6f232cbf..ea1bb19244e94 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -230,7 +230,7 @@ bool NormalVehicleTracker::measureWithPose( if (label == Label::CAR) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::TRUCK || label == Label::BUS) { + } else if (utils::isLargeVehicleLabel(label)) { constexpr float r_stddev_x = 8.0; // [m] constexpr float r_stddev_y = 0.8; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); diff --git a/perception/tensorrt_yolox b/perception/tensorrt_yolox new file mode 120000 index 0000000000000..2911ceb4060aa --- /dev/null +++ b/perception/tensorrt_yolox @@ -0,0 +1 @@ +/home/yusuke/ghq/github.com/tier4/tensorrt_yolox \ No newline at end of file