Perception: down sample point cloud through filtering beams

modules/perception/base/point_cloud.h

@@ -451,6 +451,9 @@ class AttributePointCloud : public PointCloud<PointT> {
 
   const std::vector<int32_t>& points_beam_id() const { return points_beam_id_; }
   std::vector<int32_t>* mutable_points_beam_id() { return &points_beam_id_; }
+  int32_t& points_beam_id(size_t i) { return points_beam_id_[i]; }
+  const int32_t& points_beam_id(size_t i) const { return points_beam_id_[i]; }
+
   const std::vector<uint8_t>& points_label() const { return points_label_; }
   std::vector<uint8_t>* mutable_points_label() { return &points_label_; }
 

modules/perception/base/point_cloud_util.cc

@@ -24,6 +24,25 @@ namespace apollo {
 namespace perception {
 namespace base {
 
+bool DownSamplePointCloudBeams(base::PointFCloudPtr cloud_ptr,
+                               base::PointFCloudPtr out_cloud_ptr,
+                               int downsample_factor) {
+  if (downsample_factor <= 0) {
+    return false;
+  }
+  for (size_t i = 0; i < cloud_ptr->size(); ++i) {
+    int32_t beam_id = cloud_ptr->points_beam_id(i);
+    if (beam_id % downsample_factor == 0) {
+      base::PointF point = cloud_ptr->at(i);
+      double timestamp = cloud_ptr->points_timestamp(i);
+      float height = cloud_ptr->points_height(i);
+      uint8_t label = cloud_ptr->points_label(i);
+      out_cloud_ptr->push_back(point, timestamp, height, beam_id, label);
+    }
+  }
+  return true;
+}
+
 void GetPointCloudCentroid(const PointFCloud& cloud, PointF* centroid) {
   for (size_t i = 0; i < cloud.size(); ++i) {
     centroid->x += cloud[i].x;

modules/perception/base/point_cloud_util.h

@@ -43,6 +43,10 @@ struct BoundingCube {
   float trans_z;  // center of points
 };
 
+bool DownSamplePointCloudBeams(base::PointFCloudPtr cloud_ptr,
+                               base::PointFCloudPtr out_cloud_ptr,
+                               int downsample_factor);
+
 bool GetPointCloudMinareaBbox(const base::PointFCloud& pc, BoundingCube* box,
                               const int& min_num_points = 5,
                               const bool& verbose = false);

modules/perception/common/perception_gflags.cc

@@ -52,7 +52,11 @@ DEFINE_double(normalizing_factor, 255,
 DEFINE_int32(num_point_feature, 5,
              "Length of raw point feature. Features include x, y, z,"
              "intensity and delta of time.");
-DEFINE_bool(enable_downsample_pointcloud, true,
+DEFINE_bool(enable_downsample_beams, false,
+            "Enable down sampling point cloud beams with a factor.");
+DEFINE_int32(downsample_beams_factor, 4,
+             "Down sample point cloud beams with this factor.");
+DEFINE_bool(enable_downsample_pointcloud, false,
             "Enable down sampling point cloud into centroids of voxel grid.");
 DEFINE_double(downsample_voxel_size_x, 0.01,
               "X-axis size of voxels used for down sampling point cloud.");

modules/perception/common/perception_gflags.h

@@ -37,6 +37,8 @@ DECLARE_string(pfe_onnx_file);
 DECLARE_string(rpn_onnx_file);
 DECLARE_double(normalizing_factor);
 DECLARE_int32(num_point_feature);
+DECLARE_bool(enable_downsample_beams);
+DECLARE_int32(downsample_beams_factor);
 DECLARE_bool(enable_downsample_pointcloud);
 DECLARE_double(downsample_voxel_size_x);
 DECLARE_double(downsample_voxel_size_y);

modules/perception/lidar/lib/detection/lidar_point_pillars/point_pillars_detection.cc

@@ -24,6 +24,7 @@
 #include "cyber/common/log.h"
 
 #include "modules/perception/base/object_pool_types.h"
+#include "modules/perception/base/point_cloud_util.h"
 #include "modules/perception/common/perception_gflags.h"
 #include "modules/perception/lidar/common/lidar_timer.h"
 #include "modules/perception/lidar/common/pcl_util.h"
@@ -77,7 +78,19 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
   Timer timer;
 
   int num_points;
-  cur_cloud_ptr_ = std::make_shared<base::AttributePointCloud<base::PointF>>();
+  cur_cloud_ptr_ = std::make_shared<base::PointFCloud>(*original_cloud_);
+
+  // down sample the point cloud through filtering beams
+  if (FLAGS_enable_downsample_beams) {
+    base::PointFCloudPtr downsample_beams_cloud_ptr(new base::PointFCloud());
+    if (DownSamplePointCloudBeams(original_cloud_, downsample_beams_cloud_ptr,
+        FLAGS_downsample_beams_factor)) {
+      cur_cloud_ptr_ = downsample_beams_cloud_ptr;
+    } else {
+      AWARN << "Down sample beams factor must be >= 1. Cancel down sampling."
+               " Current factor: " << FLAGS_downsample_beams_factor;
+    }
+  }
 
   // down sample the point cloud through filtering voxel grid
   if (FLAGS_enable_downsample_pointcloud) {
@@ -85,21 +98,25 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
         new pcl::PointCloud<pcl::PointXYZI>());
     pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr(
         new pcl::PointCloud<pcl::PointXYZI>());
-    TransformToPCLXYZI(*original_cloud_, pcl_cloud_ptr);
+    TransformToPCLXYZI(*cur_cloud_ptr_, pcl_cloud_ptr);
     DownSampleCloudByVoxelGrid(pcl_cloud_ptr, filtered_cloud_ptr,
                                FLAGS_downsample_voxel_size_x,
                                FLAGS_downsample_voxel_size_y,
                                FLAGS_downsample_voxel_size_z);
 
     // transform pcl point cloud to apollo point cloud
-    TransformFromPCLXYZI(filtered_cloud_ptr, cur_cloud_ptr_);
+    base::PointFCloudPtr downsample_voxel_cloud_ptr(new base::PointFCloud());
+    TransformFromPCLXYZI(filtered_cloud_ptr, downsample_voxel_cloud_ptr);
+    cur_cloud_ptr_ = downsample_voxel_cloud_ptr;
   }
   downsample_time_ = timer.toc(true);
 
   num_points = cur_cloud_ptr_->size();
-  ADEBUG << "num points before fusing: " << num_points;
+  AINFO << "num points before fusing: " << num_points;
 
   // fuse clouds of preceding frames with current cloud
+  cur_cloud_ptr_->mutable_points_timestamp()->assign(
+      cur_cloud_ptr_->size(), 0.0);
   if (FLAGS_enable_fuse_frames && FLAGS_num_fuse_frames > 1) {
     // before fusing
     while (!prev_world_clouds_.empty() &&
@@ -136,7 +153,7 @@ bool PointPillarsDetection::Detect(const DetectionOptions& options,
     }
     prev_world_clouds_.emplace_back(cur_world_cloud_ptr);
   }
-  ADEBUG << "num points after fusing: " << num_points;
+  AINFO << "num points after fusing: " << num_points;
   fuse_time_ = timer.toc(true);
 
   // shuffle points and cut off

modules/perception/production/conf/perception/perception_common.flag

@@ -86,8 +86,12 @@
 --config_manager_path=./
 --start_visualizer=false
 
-# configs for lidar point pillars
---score_threshold=0.7
+###########################################################################
+# Flags from lidar_point_pillars
+--score_threshold=0.5
+--enable_downsample_beams=false
+--downsample_beams_factor=4
+--enable_downsample_pointcloud=false
 --downsample_voxel_size_x=0.1
 --downsample_voxel_size_y=0.1
 --downsample_voxel_size_z=0.1