apollo自动驾驶-感知-点云预处理：pointcloud_preprocess

功能介绍

该组件是对接收的激光雷达点云数据做预处理，主要实现点云过滤和坐标系转换两个功能。

注意，输入的激光雷达点云数据是由激光雷达驱动解析后发出的，激光雷达驱动的发送周期为100ms，为激光雷达旋转一圈的数据量。

点云过滤功能

1. 滤除点云坐标中出现 nan 值的点（一般不会出现）；
2. 滤除点的距离值超过1km的点云（一般不会出现）；
3. 滤除车身旁边的点云，范围可在配置文件中设置；
4. 滤除高于一定范围的点云，范围可在配置文件中设置；；

坐标系转换

这里涉及3个坐标系，分别是激光雷达自身坐标系、卫星导航系统组件坐标系、世界坐标系。

实现方法是：

• 先查询激光雷达坐标系到卫星导航系统组件坐标系的静态变换，即外参；
• 再查询卫星导航系统组件坐标系到世界坐标系动态变换；
• 最后把上述两个变换整合起来，就得到雷达自身坐标系到世界坐标系的动态变矩阵，从而实现将本地激光点云数据转换为世界坐标系下点云的数据。

核心代码解读

若您想了解更多关于此模块的内容，请参考上方与该文章配套的资源，里面含有源码详细的注释和打印，以方便大家理解 。

如何查询坐标变换

查询坐标变换的入口函数。

  if (!lidar2world_trans_.GetSensor2worldTrans(lidar_query_tf_timestamp, &pose,
                                               &pose_novatel)) {
    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;
    AERROR << "Failed to get pose at time: " << lidar_query_tf_timestamp;
    return false;
  }

查询激光雷达坐标系到卫星导航系统组件坐标系的静态变换，即外参。函数第一次运行会走进该分支，后续会跳过，也就是整个上电周期只允许一次。正常执行的是 QueryTrans 函数，并且会返回true。

    if (sensor2novatel_extrinsics_ == nullptr) {
        // 传感器到卫星定位系统组件的坐标变换的外参是静态变换，函数第一次运行会走进该分支，后续会跳过
        StampedTransform trans_sensor2novatel;
        // 查询激光雷达（velodyne64）到卫星定位系统组件（novatel）的静态变换
        if (!QueryTrans(
                    timestamp,
                    &trans_sensor2novatel,
                    sensor2novatel_tf2_frame_id_,
                    sensor2novatel_tf2_child_frame_id_)) {
            return false;
        }
        sensor2novatel_extrinsics_.reset(new Eigen::Affine3d);
        // 计算激光雷达到卫星导航系统的外参
        *sensor2novatel_extrinsics_ = trans_sensor2novatel.translation * trans_sensor2novatel.rotation;
    }

查询卫星导航系统组件坐标系到世界坐标系动态变换，正常执行的是 QueryTrans 函数，并且会返回true。

   // 查询卫星定位系统组件（novatel）到世界坐标系（word）的坐标变换，这个坐标变换会随着定位的不同而不同
    if (!QueryTrans(timestamp, &trans_novatel2world, novatel2world_tf2_frame_id_, novatel2world_tf2_child_frame_id_)) {
        if (FLAGS_obs_enable_local_pose_extrapolation) {
            if (!transform_cache_.QueryTransform(
                        timestamp, &trans_novatel2world, FLAGS_obs_max_local_pose_extrapolation_latency)) {
                return false;
            }
        } else {
            return false;
        }
    } else if (FLAGS_obs_enable_local_pose_extrapolation) {
        transform_cache_.AddTransform(trans_novatel2world);
    }

上述的两个查询函数，最终会落实到transform模块中的buffer.cc中的Buffer::canTransform成员函数。该成员函数中会执行下面语句：

    bool retval = tf2::BufferCore::canTransform(target_frame, source_frame,
                                                time.ToNanosecond(), errstr);

学过ROS的同学，就很容易明白最终通过tf2开源库做的查询。

请注意，若要最终查询到上述的两个变换，需要通过cyber_recorder命令回放sensor_rgb.record数据集，并且保证程序运行和回放数据集同时运行，以免查询不到。

对于3个坐标系之间的变换，用的是仿射变换，包括平移和旋转。这属于机器人学中的基础知识，感兴趣的同学可以自己动手查询一下。

效果展示

点云过滤效果

蓝色点为未滤除的点云，红色点为被滤除的点云。其中，左图为三维视图，右图为XY俯视图。

点云坐标系变换后效果

左图为雷达自身坐标系中一帧点云数据，右图为转换到世界坐标系后的点云数据。通过对比可见，实际上将点云进行了平移和旋转。另外用不同的颜色代表不同的反射强度，1为最大反射率。