feat(lidar_centerpoint): enabled per class yaw norm threshold

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.5, 0.0, 0.5]

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

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.5, 0.0, 0.5]

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.5, 0.0, 0.5]

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

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

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()));
 

perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu

@@ -54,7 +54,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 +93,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;
@@ -124,7 +124,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_,
-    config_.yaw_norm_threshold_, thrust::raw_pointer_cast(boxes3d_d_.data()));
+    config_.yaw_norm_thresholds_.data(), thrust::raw_pointer_cast(boxes3d_d_.data()));
 
   // suppress by socre
   const auto num_det_boxes3d = thrust::count_if(

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