pointcloud_preprocessor

目录

pointcloud_preprocessor.cc

配置文件

Preprocess

TransformCloud

Register 

---

pointcloud_preprocessor.cc

代码路径 /apollo/modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor.h

配置文件 Init

接下来解析一下config manager

PointCloudPreprocessorInitOptions

 64线激光雷达 preprocessor conf

通过proto文件获取配置文件内容到 变量 PointCloudPreprocessorConfig config

Preprocess

bool PointCloudPreprocessor::Preprocess(
    const PointCloudPreprocessorOptions& options,
    const std::shared_ptr<apollo::drivers::PointCloud const>& message,
    LidarFrame* frame) const 

 frame->cloud 和 frame->world_cloud区别

首先遍历message中的点云，去除掉 std::isnan(pt.x()) 和 fabs(pt.x()) > kPointInfThreshold 的点云

文件开头定义了 const float PointCloudPreprocessor::kPointInfThreshold = 1e3;

for (int i = 0; i < message->point_size(); ++i) {
      const apollo::drivers::PointXYZIT& pt = message->point(i);
      if (filter_naninf_points_) {
        if (std::isnan(pt.x()) || std::isnan(pt.y()) || std::isnan(pt.z())) {
          continue;
        }
        if (fabs(pt.x()) > kPointInfThreshold ||
            fabs(pt.y()) > kPointInfThreshold ||
            fabs(pt.z()) > kPointInfThreshold) {
          continue;
        }
      }

增加 图片来解释 

然后过滤车身附近的点，包含x, y, z 3个方向，但是配置文件里面关闭了。

filter_naninf_points: false

filter_nearby_box_points: false

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_) {
        continue;
      }

给张图片解释

然后将点云存储push_back到frame中。

point.x = pt.x();
point.y = pt.y();
point.z = pt.z();
point.intensity = static_cast<float>(pt.intensity());
frame->cloud->push_back(point, static_cast<double>(pt.timestamp()) * 1e-9,
                              std::numeric_limits<float>::max(), i, 0);

最后，当然会有坐标转换。自动驾驶里面，坐标很重要。

TransformCloud(frame->cloud, frame->lidar2world_pose, frame->world_cloud);

然后来看一下重构的Preprocess有什么区别。

bool PointCloudPreprocessor::Preprocess(
    const PointCloudPreprocessorOptions& options, LidarFrame* frame) const

重构的Preprocess缺少message信息。

进行的流程跟上面一样，只是缺少message，所以swap了一下frame中的点云。目的目前还不清楚。看代码是倒序了一下。

TransformCloud

bool PointCloudPreprocessor::TransformCloud(
    const base::PointFCloudPtr& local_cloud, const Eigen::Affine3d& pose,
    base::PointDCloudPtr world_cloud) const 

核心代码，完成坐标转换

trans_point = pose * trans_point;

Register 

代码最后，完成preprocessor注册。

PERCEPTION_REGISTER_POINTCLOUDPREPROCESSOR(PointCloudPreprocessor);