feat(lidar_centerpoint): yaw norm filter (#1728)

* Added a filter by the norm of the yaw

* Fixed typos and tested with rosbags

* Added the yaw norm parameter in the launch file

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -33,6 +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));
   // densification param
   const std::string densification_world_frame_id =
     this->declare_parameter("densification_world_frame_id", "map");
@@ -64,7 +66,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_threshold);
 
   // create detector
   detector_ptr_ = std::make_unique<image_projection_based_fusion::PointPaintingTRT>(

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<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 score_threshold, const float circle_nms_dist_threshold,
+    const float yaw_norm_threshold)
   {
     class_size_ = class_size;
     point_feature_size_ = point_feature_size;
@@ -56,6 +57,10 @@ 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;
+    }
+
     grid_size_x_ = static_cast<std::size_t>((range_max_x_ - range_min_x_) / voxel_size_x_);
     grid_size_y_ = static_cast<std::size_t>((range_max_y_ - range_min_y_) / voxel_size_y_);
     grid_size_z_ = static_cast<std::size_t>((range_max_z_ - range_min_z_) / voxel_size_z_);
@@ -97,6 +102,7 @@ class CenterPointConfig
   // post-process params
   float score_threshold_{0.4f};
   float circle_nms_dist_threshold_{1.5f};
+  float yaw_norm_threshold_{0.5f};
 
   // 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

@@ -6,12 +6,14 @@
   <arg name="model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
   <arg name="model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
   <arg name="score_threshold" default="0.45"/>
+  <arg name="yaw_norm_threshold" default="0.5"/>
   <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/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,
-  Box3D * det_boxes3d)
+  const float yaw_norm_threshold, 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 +88,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_threshold ? max_score : 0.f;
   det_boxes3d[idx].x = x;
   det_boxes3d[idx].y = y;
   det_boxes3d[idx].z = z;
@@ -123,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_,
-    thrust::raw_pointer_cast(boxes3d_d_.data()));
+    config_.yaw_norm_threshold_, thrust::raw_pointer_cast(boxes3d_d_.data()));
 
   // suppress by socre
   const auto num_det_boxes3d = thrust::count_if(

perception/lidar_centerpoint/src/node.cpp

@@ -39,6 +39,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     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 std::string densification_world_frame_id =
     this->declare_parameter("densification_world_frame_id", "map");
   const int densification_num_past_frames =
@@ -80,7 +82,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_threshold);
   detector_ptr_ =
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);