近处的物体经常扫描得到更多的点，而远处的物体的点经常比较稀少。为了降低计算量，需要对点云数据进行降采样，但简单的随机采样仍然是这个效果。

cartographer中的VoxelFilter是把空间分为很多个立方体的栅格，一个栅格内可能有很多点， 但是只取落入栅格内的第一个点，而PCL库实现的体素滤波是用所有点的重心来代表整体

VoxelFilter

看LocalTrajectoryBuilder2D::AddRangeData函数的最后：

return AddAccumulatedRangeData(
        time,
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, 
        sensor_duration);

也就是第二个参数，这个很长名字的函数如下：

sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const
{
  //裁剪数据，指定一定z轴范围内的数据
  const sensor::RangeData cropped =
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z());
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns),
      sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)  };
}

简单说，这些滤波就是对点云数据的预处理：去除不合理的范围的点、合并相同位置的点等，从而减少点云数量，提高点云质量。

滤波的基本原理：将xyz分别除以分辨率，并取整，得到xyz所在cell的id(x,y,z)，再将该三个整数分别放入3*32个字节的数中（很好的技巧，避免了大量的遍历和比较），插入voxelFileter::voxel_set_中，后面如果是第一次插入该数，则保留该点，如果之前存在该数，即意味着有其他点落在该网格内，不再保留该点。

// using PointCloud = std::vector<RangefinderPoint>;
PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) 
{
  PointCloud results;
  for (const RangefinderPoint& point : point_cloud)
  {
    auto it_inserted =
        voxel_set_.insert(IndexToKey(GetCellIndex(point.position)));
        // 该体素之前没有没占据过，则插入，否则不插入
    if (it_inserted.second)
        results.push_back(point);
  }
  return results;
}

GetCellIndex: 3D点除以体素大小，得到所在的cell_index

IndexToKey: 将cell_index转换为一个32字节的数

AdaptiveVoxelFilter

adaptive_voxel_filter可以在最大边长 adaptive_voxel_filter.max_length 的限制下优化确定 voxel_filter_size 来实现目标的 .adaptive_voxel_filter.min_num_points

还是LocalTrajectoryBuilder2D::AddAccumulatedRangeData，这次看中间部分代码：

const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
  sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
      .Filter(gravity_aligned_range_data.returns);

也就是

PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const 
{
  return AdaptivelyVoxelFiltered(
      options_, FilterByMaxRange(point_cloud, options_.max_range())  );
}

AdaptiveVoxelFilter的构造函数按老规矩还是全赋值option，然后调用Filter，接着就是AdaptivelyVoxelFiltered函数: 再调用一次体素滤波，如果体素滤波后点数大于阈值，则返回; 如果小于阈值，则接着使用二分法进行体素滤波，使最终的点云数接近 min_num_points。 min_num_points <= 最终点云数 <= min_num_points x 1.1 网上说这里的二分查找计算量比较大，如果参数min_num_points 和 max_length 设置不好，会增加CPU，但我试了试发现影响很小。

PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud)
{
  // 已经足够稀疏，默认200
  if (point_cloud.size() <= options.min_num_points()) {
    return point_cloud;
  }
  // 还是先调用VoxelFilter来滤波，参数 max_length
  PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);
  // 按max_length进行滤波，还超过这个数，说明是这个参数能实现的最稀疏
  if (result.size() >= options.min_num_points()) {
    return result;
  }

  // 如果小于min_num_points，点云太稀疏，用于扫描匹配就不准了
  // 说明上面的 max_length太大了，现在要使用二分法找一个合适的参数
  // 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.
  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(low_length).Filter(point_cloud);
    if (result.size() >= options.min_num_points())
    {
      // 二分查找法找到合适的过滤器
      // low_length给出足够稠密的结果
      // 当 edge length is at most 10% off 时停止
      while ((high_length - low_length) / low_length > 1e-1f)
      {
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate =
            VoxelFilter(mid_length).Filter(point_cloud);
        // 点数又多了，增加边长，让 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;
}

举例：刚开始，点云数量390，Voxel Filter之后只有58个，而参数min_num_points默认50，此时就返回了。

参数min_num_points改为100，点云数量390，Voxel Filter之后只有58个，进入二分法过滤，最后的点云数量为110.