(02)Cartographer源码无死角解析-(31) LocalTrajectoryBuilder2D::AddRangeData()→点云数据重力对齐,Z轴过滤

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

在上一篇博客，介绍了激光雷达运动畸变较正，其原理是利用每个点云数据生成时机器人的位姿 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}$(相对于local坐标系)对该点云进行校正，校正之后的点云记为 $Poin{t}^{local}$，其表示为在 local 坐标系下的位置。总的来说，此时的点云数据，已经完成了时间同步，以及运动畸变校正。

回到 LocalTrajectoryBuilder2D::AddRangeData() 函数，还有一部分代码没有进行讲解，先先粘贴一下代码如下所示：

  // 有一帧有效的数据了
  ++num_accumulated_;

  // param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配
  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    // 计算2次有效点云数据的的时间差
    const common::Time current_sensor_time = synchronized_data.time;
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value()) {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;

    // 重置变量
    num_accumulated_ = 0;

    // 获取重力对齐变换矩阵，该矩阵只包含旋转，平移为0，
    //可理解机器人坐标系的Z轴需要与重力矢量平行
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));

    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    // 以最后一个点的时间戳估计出的坐标为这帧数据的原点
    accumulated_range_data_.origin = range_data_poses.back().translation();
    
    return AddAccumulatedRangeData(
        time,
        // 对点云进行重力对齐，也就是让点云的Z轴与重力方向平行
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }

其主要目的就是对点云进行重力对立，这里假设一个坐标系的 Z轴与重力方向平行，则认为该坐标系为重力坐标系。那么这里所谓的重力对齐，就是把 local 坐标系下的点云变换到重力坐标系,注意这里的 local 坐标系 与 重力坐标系原点重合，也就是说他们的相对平移为0.那么下面，就来对这些代码逐句分析吧。
 

二、简易代码

根据上面的代码，可以看到首先使用 if 语句进行判断，需要满足条件：

num_accumulated_ >= options_.num_accumulated_range_data()

实际就是对畸变校正过的点云帧进行累计，当数目达到 num_accumulated_range_data 帧之后统一进行处理，该参数在如下文件中都有设置：

src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_3d.lua

默认为1，也就是说，每帧点云经过校正之后都会进行接下来的处理。首先计算当前时间与上一帧点云数据的时间差，然后把重置变量num_accumulated_。
 

三、重力对齐变换矩阵

接下来，可以看到如下代码：

    // 获取重力对齐变换矩阵，该矩阵只包含旋转，平移为0，
    //可理解机器人坐标系的Z轴需要与重力矢量平行
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));

    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    // 以最后一个点的时间戳估计出的坐标为这帧数据的原点
    accumulated_range_data_.origin = range_data_poses.back().translation();

这里的 extrapolator_->EstimateGravityOrientation(time) 函数后续在进行详细讲解，但是注意，该函数返回的结果变量中的$平移为0$，重力对齐只需要坐标系Z轴与与重力方向平行即可。这里的 gravity_alignment 记为 ${\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{tracking}^{Gravity}$，表示机器人相对对于 Gravity 坐标系的旋转，这里我们把Z轴与重力方向平行的坐标系称为重力坐标系。

另外，这里的 accumulated_range_data_ 存储的校正过后的多帧点云数据，这里把其看作是一帧数据，然后把该数据最后一个点云数据的原点赋值给 accumulated_range_data_.origin。也就是说，默认情况下，认为点云数据最后一个点的原点就是该帧数据的原点。这里的原点是相对于local坐标系的，记为 $Origi{n}^{local}$。

 

四、重力对齐欧式变换矩阵

下面就是要重点分析与讲解的类容了。首先来看调用代码：

    return AddAccumulatedRangeData(
        time,
        // 对点云进行重力对齐，也就是让点云的Z轴与重力方向平行
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);

可见该部分代码的核核心就是 TransformToGravityAlignedFrameAndFilter() 函数，总命名上可以看出该函数主要有两个功能，一个是进行重力对齐，另外就是根据 Z 轴进行过滤的，过滤的内容后面也会进行详细讲解。首先着重需要关注的代码为：

gravity_alignment.cast<float>() * range_data_poses.back().inverse(),

这里是两个欧式变换矩阵相乘，其中 range_data_poses 在上一篇博客中已经进行过了介绍，其表示机器人在 local 坐标系下的位姿，记为 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{loacl}$，那么其上代码计算公式为：
$${\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}={\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{tracking}^{gravity}\ast [{\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{loacl}{]}^{-1}\text{\hspace{1em}(01)}$$其获得的 ${\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}$ 变换矩可以把 local 坐标系的坐标变换到 gravity 坐标系下(注意，此处的 gravity 坐标系原地与 local 坐标系原点重合)，从而达到重力对齐的效果。
 

五、重力对齐与Z轴过滤

TransformToGravityAlignedFrameAndFilter() 函数在 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 中实现，代码如下：

/**
 * @brief 先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量
 * 
 * @param[in] transform_to_gravity_aligned_frame 将点云变换到重力坐标系(Z轴与重力方向平行)
 * @param[in] range_data 传入的点云
 * @return sensor::RangeData 处理后的点云 拷贝
 */
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame, //重力对齐矩阵
    const sensor::RangeData& range_data) const { //基于local坐标系下的位姿
  // 通过重力对齐矩阵对local坐标系下点云数据进行重力对齐，然后进行Z轴上的过滤
  const sensor::RangeData cropped =
      //先通过sensor::TransformRangeData对点云进行
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z()); // param: min_z max_z
  // Step: 6 对点云进行体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}

该函数主要对点云数据做了三个处理：①利用重力矩阵与点云数据相乘，把local坐标系的点云变换到 gravity 重力坐标系下。②根据配置参数 options_.min_z()与options_.max_z() 对点云进行过滤。③对点云进行体素滤波。

1、重力对齐

重力对齐的核心代码就是sensor::TransformRangeData函数，其会对传入的点云数据 range_data.returns 与 range_data.misses 都进行处理。该函数的计算公式表达如下：
$$Poin{t}_{}^{gravity}={\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}\ast Poin{t}_{}^{local}\text{\hspace{1em}(02)}$$简单的说，就是把原本基于local坐标系的点云，变换到gravity坐标系。其中 gravity 坐标系相对于 local 坐标系只有旋转变换，无位移变换，也就是说两坐标系原点相同。

2、Z轴过滤

Z轴过滤是针对于经过重力对齐之后的点云数据，其主要核心上就上面的 sensor::CropRangeData() 函数，该函数实现于 src/cartographer/cartographer/sensor/range_data.cc 文件中，该函数需要的两个参数在 options_.min_z()与options_.max_z() 在如下文件中配置：

src/cartographer/configuration_files/trajectory_builder_2d.lua
src/cartographer/configuration_files/trajectory_builder_23.lua

通过调整 min_z 参数，$过滤掉世界坐标系下比较低的点云数据，可以有效的放置点云打在地上$。但是需要注意的是，一般只有多线雷达才会把点云打在地上，所以通常单线雷达是不需要对该参数进行设置的。

 

六、细节追击点云过滤与拷贝

这里主要是对 Z轴过滤 的函数 RangeData CropRangeData() 再进行细节上的分析。首先来看代码如下：

/**
 * @brief 对输入的点云进行z轴上的过滤
 * 
 * @param[in] range_data 原始点云数据
 * @param[in] min_z 最小的z坐标
 * @param[in] max_z 最大的z坐标
 * @return RangeData 裁剪之后的点云 拷贝
 */
RangeData CropRangeData(const RangeData& range_data, const float min_z,
                        const float max_z) {
  return RangeData{range_data.origin,
                   CropPointCloud(range_data.returns, min_z, max_z),  // 拷贝
                   CropPointCloud(range_data.misses, min_z, max_z)};  // 拷贝
}

很明显，其主要调用了 CropPointCloud() 函数对点云数据进行过滤。该函数返回值并非应用，所以这里进行了一次点云的拷贝，也就是处理 CropPointCloud() 处理完点云之后。再来看该函数细节：

/**
 * @brief 对输入的点云进行滤波, 保留数据点的z坐标处于min_z与max_z之间的点
 * 
 * @param[in] point_cloud 输入的点云
 * @param[in] min_z 最小的z
 * @param[in] max_z 最大的z
 * @return PointCloud 裁剪之后的点云 拷贝
 */
PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z,
                          const float max_z) {
  // 将lamda表达式传入copy_if, 当lamda表达式返回true时才进行复制, 
  return point_cloud.copy_if([min_z, max_z](const RangefinderPoint& point) {
    return min_z <= point.position.z() && point.position.z() <= max_z;
  });
}

该部分的代码比较巧妙，不是很直观，其首先构建了一个 lambda 表达式函数，该函数的的功能可以根据捕获的参数 min_z, max_z 判断点云是否合格，然后返回 ture 或者 false。构建出来的函数指针作为实参传递给了 point_cloud.copy_if() 函数，该函数实际为类成员模板函数，但是这里没有传入模板实参，其利用了编译器自动推断类型的能力。

  // Creates a PointCloud consisting of all the points for which `predicate`
  // returns true, together with the corresponding intensities.
  // 根据条件进行赋值
  template <class UnaryPredicate>
  PointCloud copy_if(UnaryPredicate predicate) const {
    std::vector<PointType> points;//创建一个容器用来存储过滤之后的点云
    std::vector<float> intensities;//记录点云点云强度

    // Note: benchmarks show that it is better to have this conditional outside
    // the loop.
    if (intensities_.empty()) { //如果点云数据存在强度信息
      for (size_t index = 0; index < size(); ++index) {//循环对所有点云进行处理
        const PointType& point = points_[index];//通过索引获得点云数据的引用
        // 表达式为true时才使用这个点
        if (predicate(point)) {
          points.push_back(point);//把引用添加到 points 容器中
          std::cout<<123456<<std::endl;
        }
      }
    } else {//如果点云数据不存在强度信息
      for (size_t index = 0; index < size(); ++index) {
        const PointType& point = points_[index];//
        if (predicate(point)) {
          points.push_back(point); //通过索引获得点云数据的引用
          intensities.push_back(intensities_[index]); //把强度添加到intensities变量中
        }
      }
    }
    //利用过滤之后的点云引用以及强度构建PointCloud对象。
    return PointCloud(points, intensities);
  }

从上面的代码中可以看到 PointCloud(points, intensities)，其创建对象时，调用构造函数：

// 构造时先拷贝, 再进行移动
PointCloud::PointCloud(std::vector<PointType> points,
                       std::vector<float> intensities)
    : points_(std::move(points)), intensities_(std::move(intensities)) {
  if (!intensities_.empty()) {
    CHECK_EQ(points_.size(), intensities_.size());
  }
}

这里也进行了一次拷贝，在从实参变成形参的时候进行了数据拷贝，初始化列表中，并没有进行点云的拷贝，直接使用 std::move(points) 移交了形参的所有权。
 

七、结语

到目前位置，对于 LocalTrajectoryBuilder2D::AddRangeData() 函数可以说时讲解得差不多了，除了 LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter() 函数中得如下代码：

  // Step: 6 对点云进行体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};

进行体素滤波的点云数据帧已经完成了时间同步、运动畸变校正，以及重力校正与Z轴过滤。下一篇博客，就会 对点云的体素滤波进行详细的讲解。