feat(lidar_centerpoint): enabled per class yaw norm threshold (autowa…

…refoundation#1962) * feat(lidar_centerpoint): enabled per class yaw norm threshold Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Updated thresholds Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Moved the yaw threshold parameters to gpu memory Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Moved the thresholds as a member variable Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
boyali · Oct 19, 2022 · 74d2caa · 74d2caa
1 parent d034cb4
commit 74d2caa
Show file tree

Hide file tree

Showing 11 changed files with 27 additions and 17 deletions.
diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml
@@ -7,3 +7,4 @@
     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/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -33,8 +33,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt
     static_cast<float>(this->declare_parameter<double>("score_threshold", 0.4));
   const float circle_nms_dist_threshold =
     static_cast<float>(this->declare_parameter<double>("circle_nms_dist_threshold", 1.5));
-  const float yaw_norm_threshold =
-    static_cast<float>(this->declare_parameter<double>("yaw_norm_threshold", 0.5));
+  const auto yaw_norm_thresholds =
+    this->declare_parameter<std::vector<double>>("yaw_norm_thresholds");
   // densification param
   const std::string densification_world_frame_id =
     this->declare_parameter("densification_world_frame_id", "map");
@@ -73,7 +73,7 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt
   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,
-    yaw_norm_threshold);
+    yaw_norm_thresholds);
 
   // create detector
   detector_ptr_ = std::make_unique<image_projection_based_fusion::PointPaintingTRT>(

diff --git a/perception/lidar_centerpoint/config/centerpoint.param.yaml b/perception/lidar_centerpoint/config/centerpoint.param.yaml
@@ -12,3 +12,4 @@
     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/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml
@@ -12,3 +12,4 @@
     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/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp
@@ -28,7 +28,7 @@ class CenterPointConfig
     const std::vector<double> & point_cloud_range, const std::vector<double> & 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 yaw_norm_threshold)
+    const std::vector<double> yaw_norm_thresholds)
   {
     class_size_ = class_size;
     point_feature_size_ = point_feature_size;
@@ -57,8 +57,12 @@ class CenterPointConfig
       circle_nms_dist_threshold_ = circle_nms_dist_threshold;
     }
 
-    if (yaw_norm_threshold >= 0 && yaw_norm_threshold < 1) {
-      yaw_norm_threshold_ = yaw_norm_threshold;
+    yaw_norm_thresholds_ =
+      std::vector<float>(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<std::size_t>((range_max_x_ - range_min_x_) / voxel_size_x_);
@@ -102,7 +106,7 @@ class CenterPointConfig
   // post-process params
   float score_threshold_{0.35f};
   float circle_nms_dist_threshold_{1.5f};
-  float yaw_norm_threshold_{0.0f};
+  std::vector<float> 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/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<Box3D> boxes3d_d_;
+  thrust::device_vector<float> yaw_norm_thresholds_d_;
 };
 
 }  // namespace centerpoint

diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml
@@ -7,14 +7,12 @@
   <arg name="model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
   <arg name="class_remapper_param_path" default="$(find-pkg-share lidar_centerpoint)/config/detection_class_remapper.param.yaml"/>
   <arg name="score_threshold" default="0.35"/>
-  <arg name="yaw_norm_threshold" default="0.0"/>
   <arg name="has_twist" default="false"/>
 
   <node pkg="lidar_centerpoint" exec="lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
     <remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
     <param name="score_threshold" value="$(var score_threshold)"/>
-    <param name="yaw_norm_threshold" value="$(var yaw_norm_threshold)"/>
     <param name="densification_world_frame_id" value="map"/>
     <param name="densification_num_past_frames" value="1"/>
     <param name="trt_precision" value="fp16"/>

diff --git a/perception/lidar_centerpoint/lib/detection_class_remapper.cpp b/perception/lidar_centerpoint/lib/detection_class_remapper.cpp
@@ -23,7 +23,8 @@ void DetectionClassRemapper::setParameters(
 {
   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(std::sqrt(min_area_matrix.size()), 2) == std::sqrt(min_area_matrix.size()));
+  assert(
+    std::pow(static_cast<int>(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size());
 
   num_labels_ = static_cast<int>(std::sqrt(min_area_matrix.size()));
 

diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp
@@ -23,7 +23,6 @@ namespace centerpoint
 
 void NonMaximumSuppression::setParameters(const NMSParams & params)
 {
-  assert(params.target_class_names_.size() == 8);
   assert(params.search_distance_2d_ >= 0.0);
   assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0);
 

diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu
@@ -17,6 +17,7 @@
 #include <lidar_centerpoint/postprocess/postprocess_kernel.hpp>
 
 #include <thrust/count.h>
+#include <thrust/device_vector.h>
 #include <thrust/sort.h>
 
 namespace
@@ -54,7 +55,7 @@ __global__ void generateBoxes3D_kernel(
   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,
-  const float yaw_norm_threshold, 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)
@@ -93,7 +94,7 @@ __global__ void generateBoxes3D_kernel(
   const float vel_y = out_vel[down_grid_size * 1 + idx];
 
   det_boxes3d[idx].label = label;
-  det_boxes3d[idx].score = yaw_norm >= yaw_norm_threshold ? max_score : 0.f;
+  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;
@@ -109,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<Box3D>(num_raw_boxes3d);
+  yaw_norm_thresholds_d_ = thrust::device_vector<float>(
+    config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end());
 }
 
 cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch(
@@ -124,7 +127,8 @@ 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_,
-    config_.yaw_norm_threshold_, thrust::raw_pointer_cast(boxes3d_d_.data()));
+    thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()),
+    thrust::raw_pointer_cast(boxes3d_d_.data()));
 
   // suppress by socre
   const auto num_det_boxes3d = thrust::count_if(

diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp
@@ -42,8 +42,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     static_cast<float>(this->declare_parameter<double>("score_threshold", 0.35));
   const float circle_nms_dist_threshold =
     static_cast<float>(this->declare_parameter<double>("circle_nms_dist_threshold"));
-  const float yaw_norm_threshold =
-    static_cast<float>(this->declare_parameter<double>("yaw_norm_threshold", 0.0));
+  const auto yaw_norm_thresholds =
+    this->declare_parameter<std::vector<double>>("yaw_norm_thresholds");
   const std::string densification_world_frame_id =
     this->declare_parameter("densification_world_frame_id", "map");
   const int densification_num_past_frames =
@@ -102,7 +102,7 @@ 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,
-    yaw_norm_threshold);
+    yaw_norm_thresholds);
   detector_ptr_ =
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);