cartographer代码学习笔记-自适应体素滤波AdaptiveVoxelFilter

Running小学生

于 2023-12-08 16:24:31 发布

阅读量263

点赞数

分类专栏： cartographer 文章标签：学习笔记

本文链接：https://blog.csdn.net/weixin_28683491/article/details/134878676

版权

cartographer 专栏收录该内容

19 篇文章 1 订阅

订阅专栏

自适应体素滤波

进入接口
gravity_aligned_range_data是已经转换到gravity下的点云数据

  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;
  }

进入函数内部，通过AdaptivelyVoxelFiltered进行处理

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

自适应体素滤波相关参数

  adaptive_voxel_filter = {         --自适应滤波 
    max_length = 0.5,               --网格滤波的大小
    min_num_points = 200,           --最小点云数据
    max_range = 50.,                --最远点云距离
  },

1、通过设置最大范围，筛选满足条件的点云

PointCloud FilterByMaxRange(const PointCloud& point_cloud,
                            const float max_range) {
  return point_cloud.copy_if([max_range](const RangefinderPoint& point) {
    return point.position.norm() <= max_range;
  });
}

2、对点云进行滤波

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;
  }
  //通过体素滤波对点云进行过滤
  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.

//小于最低标准数量 按照0.25～ 0.5 * 当前分辨率进行降低分辨率滤波
  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.
      //利用二分查找法保证有足够多的数量
      //通过二分查找逐步扩大voxel的size, 当high_length与low_length的长度足够小时, 停止查找!!
      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;
}

体素滤波器

PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      //lamba表达式，返回该点的位置Vector3f
      [](const RangefinderPoint& point) { return point.position; });

  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]);
    }
  }
  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

// 进行体素滤波, 标记体素滤波后的点
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;
}

GetVoxelCellIndex(point_function(point_cloud[i]), resolution) 根据point的position以及voxel的size, 得到该点处于的voxel的序号, 然后通过hash_map 找到对应的voxel, 然后将voxel内部的点的数量++,
当点的数量为1时, 那么保留的就自然是该点,因此 voxel.second = i, 当点的数量逐渐增多时,该保留哪个点, 就取决与当前生成的随机数是否等于 voxel.first, 如果相等, 那么就保留当前点.

VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point,
                               const float resolution) {
  const Eigen::Array3f index = point.array() / resolution;
  const uint64_t x = common::RoundToInt(index.x());
  const uint64_t y = common::RoundToInt(index.y());
  const uint64_t z = common::RoundToInt(index.z());
  return (x << 42) + (y << 21) + z;
}