perception: delete some PointsCloudProcess comments

modules/perception/lidar/app/lidar_obstacle_segmentation.cc

@@ -91,6 +91,8 @@ bool LidarObstacleSegmentation::Init(
 LidarProcessResult LidarObstacleSegmentation::Process(
     const LidarObstacleSegmentationOptions& options, LidarFrame* frame) {
   PointCloudPreprocessorOptions preprocessor_options;
+  preprocessor_options.sensor2novatel_extrinsics =
+    options.sensor2novatel_extrinsics;
   if (cloud_preprocessor_.Preprocess(preprocessor_options, frame)) {
     return ProcessCommon(options, frame);
   } else {
@@ -109,6 +111,8 @@ LidarProcessResult LidarObstacleSegmentation::Process(
 
   PERCEPTION_PERF_BLOCK_START();
   PointCloudPreprocessorOptions preprocessor_options;
+  preprocessor_options.sensor2novatel_extrinsics =
+    options.sensor2novatel_extrinsics;
   if (cloud_preprocessor_.Preprocess(preprocessor_options, message, frame)) {
     PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(sensor_name, "preprocess");
     return ProcessCommon(options, frame);

modules/perception/lidar/app/lidar_obstacle_segmentation.h

@@ -39,6 +39,7 @@ struct LidarObstacleSegmentationInitOptions {
 
 struct LidarObstacleSegmentationOptions {
   std::string sensor_name;
+  Eigen::Affine3d sensor2novatel_extrinsics;
 };
 
 class LidarObstacleSegmentation {

modules/perception/lidar/lib/pointcloud_preprocessor/BUILD

@@ -21,6 +21,8 @@ cc_library(
         "//modules/perception/lidar/common",
         "//modules/perception/lidar/lib/pointcloud_preprocessor/proto:pointcloud_preprocessor_config_proto",
         "//modules/perception/proto:perception_config_schema_proto",
+        "//modules/common/configs:vehicle_config_helper",
+        "//modules/common/configs/proto:vehicle_config_proto",
         "@eigen",
     ],
 )

modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor.cc

@@ -17,6 +17,7 @@
 
 #include <memory>
 
+#include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/util/file.h"
 #include "modules/perception/base/object_pool_types.h"
 #include "modules/perception/lidar/lib/pointcloud_preprocessor/proto/pointcloud_preprocessor_config.pb.h"
@@ -51,10 +52,16 @@ bool PointCloudPreprocessor::Init(
   CHECK(apollo::common::util::GetProtoFromFile(config_file, &config));
   filter_naninf_points_ = config.filter_naninf_points();
   filter_nearby_box_points_ = config.filter_nearby_box_points();
-  box_forward_x_ = config.box_forward_x();
-  box_backward_x_ = config.box_backward_x();
-  box_forward_y_ = config.box_forward_y();
-  box_backward_y_ = config.box_backward_y();
+  // box_forward_x_ = config.box_forward_x();
+  // box_backward_x_ = config.box_backward_x();
+  // box_forward_y_ = config.box_forward_y();
+  // box_backward_y_ = config.box_backward_y();
+  const auto &vehicle_param =
+    common::VehicleConfigHelper::GetConfig().vehicle_param();
+  box_forward_x_ = vehicle_param.right_edge_to_center();
+  box_backward_x_ = -vehicle_param.left_edge_to_center();
+  box_forward_y_ = vehicle_param.front_edge_to_center();
+  box_backward_y_ = -vehicle_param.back_edge_to_center();
   filter_high_z_points_ = config.filter_high_z_points();
   z_threshold_ = config.z_threshold();
   return true;
@@ -89,9 +96,13 @@ bool PointCloudPreprocessor::Preprocess(
           continue;
         }
       }
-      if (filter_nearby_box_points_ && pt.x() < box_forward_x_ &&
-          pt.x() > box_backward_x_ && pt.y() < box_forward_y_ &&
-          pt.y() > box_backward_y_) {
+      Eigen::Vector3d vec3d_lidar(pt.x(), pt.y(), pt.z());
+      Eigen::Vector3d vec3d_novatel =
+        options.sensor2novatel_extrinsics * vec3d_lidar;
+      if (filter_nearby_box_points_ && vec3d_novatel[0] < box_forward_x_ &&
+          vec3d_novatel[0] > box_backward_x_ &&
+          vec3d_novatel[1] < box_forward_y_ &&
+          vec3d_novatel[1] > box_backward_y_) {
         continue;
       }
       if (filter_high_z_points_ && pt.z() > z_threshold_) {
@@ -134,9 +145,13 @@ bool PointCloudPreprocessor::Preprocess(
           continue;
         }
       }
-      if (filter_nearby_box_points_ && pt.x < box_forward_x_ &&
-          pt.x > box_backward_x_ && pt.y < box_forward_y_ &&
-          pt.y > box_backward_y_) {
+      Eigen::Vector3d vec3d_lidar(pt.x, pt.y, pt.z);
+      Eigen::Vector3d vec3d_novatel =
+        options.sensor2novatel_extrinsics * vec3d_lidar;
+      if (filter_nearby_box_points_ && vec3d_novatel[0] < box_forward_x_ &&
+          vec3d_novatel[0] > box_backward_x_ &&
+          vec3d_novatel[1] < box_forward_y_ &&
+          vec3d_novatel[1] > box_backward_y_) {
         frame->cloud->SwapPoint(i, size--);
         continue;
       }

modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor.h

@@ -29,7 +29,9 @@ struct PointCloudPreprocessorInitOptions {
   std::string sensor_name = "velodyne64";
 };
 
-struct PointCloudPreprocessorOptions {};
+struct PointCloudPreprocessorOptions {
+  Eigen::Affine3d sensor2novatel_extrinsics;
+};
 
 class PointCloudPreprocessor {
  public:

modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor_test.cc

@@ -95,6 +95,7 @@ TEST_F(PointCloudPreprocessorTest, basic_test) {
     EXPECT_FALSE(preprocessor.Preprocess(option, &frame));
     frame.cloud = base::PointFCloudPool::Instance().Get();
     frame.lidar2world_pose = Eigen::Affine3d::Identity();
+    option.sensor2novatel_extrinsics = Eigen::Affine3d::Identity();
     MockPointcloud(frame.cloud.get());
     EXPECT_EQ(frame.cloud->size(), 10);
     EXPECT_TRUE(preprocessor.Preprocess(option, &frame));

modules/perception/onboard/component/segmentation_component.cc

@@ -137,6 +137,7 @@ bool SegmentationComponent::InternalProc(
 
   lidar::LidarObstacleSegmentationOptions segment_opts;
   segment_opts.sensor_name = sensor_name_;
+  lidar2world_trans_.GetExtrinsics(&segment_opts.sensor2novatel_extrinsics);
   lidar::LidarProcessResult ret =
       segmentor_->Process(segment_opts, in_message, frame.get());
   if (ret.error_code != lidar::LidarErrorCode::Succeed) {

modules/perception/production/data/perception/lidar/models/pointcloud_preprocessor/velodyne16/pointcloud_preprocessor.conf

 filter_naninf_points: false
-filter_nearby_box_points: false
+filter_nearby_box_points: true
 box_forward_x: 2.0
 box_backward_x: -2.0
 box_forward_y: 2.0