Cartographer源码阅读2D&3D----前端自适应滤波----AdaptiveVoxelFilter()

Cartographer源码阅读2D&3D----前端自适应滤波----AdaptiveVoxelFilter()

基本原理：
调用体素滤波，如果体素滤波后点数大于阈值，则返回，如果小于阈值，则接着使用二分法进行体素滤波。

前端使用接口Local_Trajectory_builder.cc

  // 对重力对齐的点云数据进行自适应滤波
  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;
  }

对重力对齐的点云数据进行自适应滤波,点击AdaptiveVoxelFilter(),

进入voxel_filter.h

PointCloud AdaptiveVoxelFilter(
    const PointCloud& point_cloud,
    const proto::AdaptiveVoxelFilterOptions& options) {
  return AdaptivelyVoxelFiltered(
      options, FilterByMaxRange(point_cloud, options.max_range()));
}

点击AdaptivelyVoxelFiltered()函数

进入voxel_filter.cc

PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud) {
  if (point_cloud.size() <= options.min_num_points()) {
    // 'point_cloud' is already sparse enough.
    return point_cloud;
  }
  // 又调用VoxelFilter来滤波，其中cell的size=max_length
  PointCloud result = VoxelFilter(point_cloud, options.max_length());
  // 如果剩下的点大于min_num_points，则返回
  if (result.size() >= options.min_num_points()) {
    // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
    return result;
  }
  // 如果小于min_num_points，二分法滤波
  // 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(point_cloud, low_length);
    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(point_cloud, mid_length);
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        } else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;
}

参考链接: https://blog.csdn.net/yeluohanchan/article/details/108674896.