cartographer 体素滤波器 voxel_filter.cc

1、简介：

在cartographer/sensor/internal/voxel_filter.cc

在localTrajectoryBuild_2d.cc 中被调用

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

参数：重力坐标系下，激光数据的returns 点，的点云

2、实现

1.AdaptiveVoxelFilter 构造：

AdaptiveVoxelFilter::AdaptiveVoxelFilter(
    const proto::AdaptiveVoxelFilterOptions& options)
    : options_(options) {}

cartographer代码中配置参数：

adaptive_voxel_filter = {
max_length = 0.5,
min_num_points = 200,
max_range = 50.,
},

闭环自适应体素滤波器

loop_closure_adaptive_voxel_filter = {
max_length = 0.9,
min_num_points = 100,
max_range = 50.,
},

2.Filter函数

调用：AdaptiveVoxelFilter函数，体素滤波返回所需点云

FilterByMaxRange(point_cloud, options_.max_range()) 筛选点云

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

3.AdaptiveVoxelFilter函数

对点云数据，根据指定范围max_range进行l2距离过滤，筛选点云数据

// l2范数,向量各个元素平方和的1/2次方

扫描二维码关注公众号，回复： 4582454 查看本文章

  PointCloud result;
  for (const RangefinderPoint& point : point_cloud) {
    if (point.position.norm() <= max_range) {
      result.push_back(point);
    }
  }
  return result;

4.AdaptiveVoxelFilter函数：

PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud)

voxels_：表征一系列的Cell，每个Cell有一个size，piont以size为间距分布，实现稀疏表示。 Cell的数量和即为稀疏表达的point的数量和。

参数说明：

形参1：滤波器option，形参2：待滤波的points

代码注解：

1. 'point_cloud'已经足够稀疏了直接返回

  if (point_cloud.size() <= options.min_num_points()) {
    // 'point_cloud' is already sparse enough.
    return point_cloud;
  }

2. 以max_size为间距，稀疏提取point。调用Filter函数

  PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud);

**.Filter函数：

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(point.position) 将三维点除以分辨率 options.max_length() 四舍五入取整

IndexToKey

  KeyType k_0(static_cast<uint32>(index[0]));
  KeyType k_1(static_cast<uint32>(index[1]));
  KeyType k_2(static_cast<uint32>(index[2]));
  return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2;

absl::flat_hash_set<KeyType> voxel_set_;

3.使用'max_length'过滤会产生足够密集的点云。

  if (result.size() >= options.min_num_points()) {
    // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
    return result;
  }

4.以max_size为间距没有超过min_num.则继续减小edge大小，使得以low_length为间距。

low_length是每个Cell的长度。每个Cell“占据”的points不能超过min_num. 二分查找法： for()循环每次将edge减半对每一个low_length: while()循环控制edge不能小于10% 满足要求则记录，否则将high折半。

  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()) {
      // 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.
      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);
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        } else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;