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

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

一、前言

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


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

二、滤波函数总览

首先说明一下，该篇博客主要接着上一篇博客进行讲解，也就是对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> 第一个元素保存该voxel内部的点的个数, 
  //std::pair> 第二个元素保存该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。该函数执行完成之后则会调用
 

四、

其上完成体素滤波之后，这里回到之前的 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.,              -- 距远离雷达原点超过max_range 的点被移除
  },

另外还有一个比较简单的函数 FilterByMaxRange()