(02)Cartographer源码无死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→点云的体素滤波

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

首先这里对点云的体素滤波进行一个简单的介绍：体素滤波器是一种下采样的滤波器，它的作用是使用体素化方法减少点云数量，采用体素格中接近中心点的点替代体素内的所有点云，这种方法比直接使用中心点要慢，但是更加精确。这种方式即减少点云数据，并同时保存点云的形状特征，在提高配准，曲面重建，形状识别等算法速度中非常实用，来看下面两张效果图：
在这里插入图片描述

可以看到，经过体素滤波的点云更加稀疏，其原理也比较简单：把三维空间按照一定分辨率分割成一个个的小正方体(或者说长方体)，每个小正方体中使用一个点云来表示，这样就起到下采样的效果。至于如何处理获得一个小正方体最具有代表性的点云，不同算法，实现的方式也是不一样的。另外，这里的小正方体，也称为体素，所以叫体素滤波。

二、滤波函数总览

首先说明一下，该篇博客主要接着上一篇博客进行讲解，也就是对src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 文件中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轴过滤。不过需要注意的是，src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件中定义了很多滤波函数，其中有些为模板函数，如下所示：

1.// 进行体素滤波
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution){
    
    ......}

2.
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,const float resolution) {
    
    ......}

3.
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
	const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&range_measurements,
	const float resolution) {
    
    ....}

为什么定义这么的滤波函数暂时不是很了解，可能是为了处理各种类型的点云(如带时间与不带时间)，且需要返回不同的数据格式。下面要讲解的是其上的 1，如下：

PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {

该滤波输入与输出的数据格式都是都是 PointCloud 类型的实例，该类在上一篇博客中提及过，且对其成员函数：

  template <class UnaryPredicate>
  PointCloud copy_if(UnaryPredicate predicate) const {
    
    ....}

进行了细节的分析。简单的可以把该类理解为，存储一帧点云数据的类，且该类还实现了点云的过滤成员函数→copy_if()。

另外还有一个参数 options_.voxel_filter_size() 表示滤波的体素大小，该变量在 src/cartographer/configuration_files/trajectory_builder_2d.lua 文件中设置，默认为 0.025，也就是每2.5cm为一个体素。

三、sensor::VoxelFilter() 总体介绍

/**
 * @brief 对点云数据进行体素滤波
 * 
 * @param[in] point_cloud 该类包含需要处理的所有点云数据
 * @param[in] resolution 滤波分辨率，即每个体素的体积大小
 * @return PointCloud 过滤之后的点云数据
 */
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
    
    
  
  //得到标记后的点，记录一个体素中，应该取那个点。(每个体素只取一个点进行代替)
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) {
    
     return point.position; });

  // 根据标记，把对应的点云存储在 filtered_points 之中
  std::vector<RangefinderPoint> filtered_points;
  for (size_t i = 0; i < point_cloud.size(); i++) {
    
    
    if (points_used[i]) {
    
    
      filtered_points.push_back(point_cloud[i]);
    }
  }
  //根据滤波之后的点云标记，把点云对应的强度添加到 filtered_intensities 之中
  std::vector<float> filtered_intensities;
  CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size());
  for (size_t i = 0; i < point_cloud.intensities().size(); i++) {
    
    
    if (points_used[i]) {
    
    
      filtered_intensities.push_back(point_cloud.intensities()[i]);
    }
  }

  //通过std::move移交所有权，减少数据的拷贝
  return PointCloud(std::move(filtered_points),
                    std::move(filtered_intensities));
}

该函数主要分三个步骤对点云进行处理，这里主要分析第一个部分，即 RandomizedVoxelFilterIndices() 函数。如下所示：

  //得到标记后的点
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) {
    
     return point.position; });

可见其传入了三个参数，①需要处理的点云帧数据；②体素滤波分辨率；③lambda表达式，该表示的功能为传入一个 RangefinderPoint 实例对象，其会返回 Eigen::Vector3f 类型的点云坐标。该函数具体实现于
src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的，代码如下：

// 进行体素滤波, 标记体素滤波后的点
template <class T, class PointFunction>
std::vector<bool> RandomizedVoxelFilterIndices(       
    const std::vector<T>& point_cloud, //需要进行处理的点云帧数据，  
     const float resolution, //体素滤波分辨率，即体素大小
    PointFunction&& point_function) {
    
     //获取点云坐标的函数指针
  // According to https://en.wikipedia.org/wiki/Reservoir_sampling
  std::minstd_rand0 generator; //定义一个随机数生成器

  //std::pair<int, int>> 第一个元素保存该voxel内部的点的个数, 
  //std::pair<int, int>> 第二个元素保存该voxel中选择的那一个点的序号
  //VoxelKeyType 就是 uint64_t 类型，可以理解为体素的索引
  absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
      voxel_count_and_point_index;

  // 遍历所有的点, 计算
  for (size_t i = 0; i < point_cloud.size(); i++) {
    
    
    //通过GetVoxelCellIndex()计算该点处于的voxel的序号
    //最终获取VoxelKeyType对应的value的引用
    auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(
        point_function(point_cloud[i]), resolution)];

    voxel.first++;//该体素记录的点云数量+1

    // 如果这个体素格子只有1个点, 那这个体素格子里的点的索引就是i
    if (voxel.first == 1) {
    
    
      voxel.second = i; //这里的i为目前遍历点云数据的索引
    } 
    else {
    
    
      // 生成随机数的范围是 [1, voxel.first]
      std::uniform_int_distribution<> distribution(1, voxel.first);
      // 生成的随机数与个数相等, 就让这个点代表这个体素格子
      if (distribution(generator) == voxel.first) {
    
    
        voxel.second = i;
      }
    }
  }

  // 为体素滤波之后的点做标记
  std::vector<bool> points_used(point_cloud.size(), false);
  for (const auto& voxel_and_index : voxel_count_and_point_index) {
    
    
    points_used[voxel_and_index.second.second] = true;
  }
  return points_used;
}

注释比较详细，这里就不在重复了，其主要是返回变量：

std::vector<bool> points_used(point_cloud.size(), false);

可见其大小与输入的点云数目一致，默认设置为都为 false，其对应的点云如果被选中会标志为 true。该函数执行完成之后则会调用

四、sensor::VoxelFilter() 函数返回

其上完成体素滤波之后，这里回到之前的 LocalTrajectoryBuilder2D::AddRangeData() 函数，可见代码如下：

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

可知其又调用了 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数，该函数后面再进行详细的讲解，因为涉及到了点云的扫描匹配，但是其中还有调用滤波相关的代码，所以先对该部分内容进行讲解，如下所示：


  // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    
    
    return nullptr;
  }

首先其只对 gravity_aligned_range_data.returns 点云进行了自适应体素滤波，而 gravity_aligned_range_data.returns.misses 点云没有进行滤波，该函数定义如下所示：

// 进行自适应体素滤波
PointCloud AdaptiveVoxelFilter(
    const PointCloud& point_cloud,
    const proto::AdaptiveVoxelFilterOptions& options) {
    
    
  return AdaptivelyVoxelFiltered(
      // param: adaptive_voxel_filter.max_range 距远离原点超过max_range的点被移除
      // 这里的最大距离是相对于local坐标系原点的
      options, FilterByMaxRange(point_cloud, options.max_range()));
}

也就是重点还是 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的 AdaptivelyVoxelFiltered() 函数。注意其上的 options 包含如下参数：

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    
    
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离local坐标系原点超过 max_range 的点被移除
  },

另外还有一个比较简单的函数 FilterByMaxRange() , 距远离 local坐标系原点超过max_range 的点云被移除。这里需要注意的，其是相对云 local坐标系原点，与前面分析的代码 LocalTrajectoryBuilder2D::AddRangeData() 函数中的 options_.min_range() 与 options_.mix_range() 是不一样的，如下：

    // param: min_range max_range
    if (range >= options_.min_range()) {
    
    //如果该点云大于等于最近测量距离
      if (range <= options_.max_range()) {
    
    //如果该点小于等于最远测量距离

  //默认配置
  min_range = 0.,                 -- 雷达数据的最近滤波, 保存中间值
  max_range = 30.,                -- 雷达数据的最远滤波, 保存中间值

在 LocalTrajectoryBuilder2D::AddRangeData() 函数中过滤的是相对于雷达原点的距离，默认 max_range = 30。AdaptivelyVoxelFiltered() 是相对于 local坐标系原点的距离 max_range = 30，通常情况下后者的设置都比前者大。max_range 过滤之后，随后就是正式进入到 AdaptivelyVoxelFiltered() 函数了。

五、AdaptivelyVoxelFiltered()

该函数还是一样，位于 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件中，再复制一下 adaptive_voxel_filter 配置信息：

  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {
    
    
    max_length = 0.5,             -- 尝试确定最佳的立方体边长, 边长最大为0.5
    min_num_points = 200,         -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
    max_range = 50.,              -- 距远离local坐标系原点超过 max_range 的点被移除
  },

$\color{blue}(01)$ 判断需要进行自适应体素滤波的帧点云数据，是否已经比较稀疏。如果 point_cloud.size() <= options.min_num_points() 表示目前点云足够稀疏不需要再进行滤波了。直接返回 point_cloud 即可。

$\color{blue}(03)$ 如果目前点云的非常稠密，尝试使用把体素的边长设置为 max_length = 0.5 进行滤波。滤波之后依旧不够稀疏，那么没有办法，说明已经是 max_length = 0.5 参数下最稀疏的状态了, 直接返回。

$\color{blue}(04)$ 如果目前的点云不是非常稠密(如果非常稠密第二步就返回了)，那么就使用二分法，找到最合适的体素边长进行滤波。简单的说，就是每次体素边长缩小到原来的1/2 ，进行滤波，如果滤波之后的点云多了，则表示需要增加体素边长，如果少了，则减少体素的边长。

$\color{blue}(05)$ 源码中首先使用一个 for 循环，对体素边长进行控制，每次循环都会把体素边长缩小为上一次的1/2，如果边长低于 1e-2f * options.max_length() 则跳出 for 循环。但是注意，如果 for 循环执行完了，说明其一直不满足条件 result.size() >= options.min_num_points()，也就时点云一直比较稀疏，最终使用体素边长为 1e-2f * options.max_length() 的滤波。

$\color{blue}(06)$ 如果for循环的过程中，发现点云比较稠密了，即满足 options.min_num_points() 条件。则进入到 while 循环进行精细求解，该求解建立在 for循环的基础上，即在来自于 for 循环获得的 low_length 与 high_length 之间找到最合适的体素边长。

$\color{blue}结果$ 经过 for 循环与 while 的最终结果，是希望点云数目保持在 options.min_num_points()=200 最优。代码注释如下：

// 自适应体素滤波
PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud) {
    
    
  // param: adaptive_voxel_filter.min_num_points 满足小于等于这个值的点云满足要求, 足够稀疏
  //也就是说，该帧数据的点云已经比较稀疏了，没有必要再进行滤波了
  if (point_cloud.size() <= options.min_num_points()) {
    
    
    // 'point_cloud' is already sparse enough.
    return point_cloud;
  }

  // param: adaptive_voxel_filter.max_length 进行一次体素滤波
  PointCloud result = VoxelFilter(point_cloud, options.max_length());
  // 如果按照最大边长进行体素滤波之后还超过这个数了, 就说明已经是这个参数下最稀疏的状态了, 直接返回
  if (result.size() >= options.min_num_points()) {
    
    
    // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
    return result;
  }

  // Search for a 'low_length' that is known to result in a sufficiently
  // dense point cloud. We give up and use the full 'point_cloud' if reducing
  // the edge length by a factor of 1e-2 is not enough.
  // 将体素滤波的边长从max_length逐渐减小, 每次除以2
  for (float high_length = options.max_length();
       high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
    
    
    // 减小边长再次进行体素滤波
    float low_length = high_length / 2.f;
    result = VoxelFilter(point_cloud, low_length);
    
    // 重复for循环直到 滤波后的点数多于min_num_points
    if (result.size() >= options.min_num_points()) {
    
    
      // Binary search to find the right amount of filtering. 'low_length' gave
      // a sufficiently dense 'result', 'high_length' did not. We stop when the
      // edge length is at most 10% off.
      // 以二分查找的方式找到合适的过滤边长, 当边缘长度最多减少 10% 时停止
      while ((high_length - low_length) / low_length > 1e-1f) {
    
    
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
        // 如果点数多, 就将边长变大, 让low_length变大
        if (candidate.size() >= options.min_num_points()) {
    
    
          low_length = mid_length;
          result = candidate;
        } 
        // 如果点数少, 就将边长变小, 让high_length变小
        else {
    
    
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;
}

六、结语

该篇博客主要讲解了点云体素滤波的知识，但是讲解代码过程中跳跃得比较多，可能导致不太好理解。不过没有关系，下一篇博客我们来做一个复盘。这样就不会觉得比较杂乱了。