cartographer源码解读----前端2D体素滤波(VoxelFilter)

cartographer源码解读----前端2D体素滤波(VoxelFilter)

VoxelFilter类

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

最新一版的代码,并没有VoxelFilter类,但依然在voxel_filter.h的头文件中声明这些函数

namespace cartographer {
namespace sensor {

std::vector<RangefinderPoint> VoxelFilter(
    const std::vector<RangefinderPoint>& points, const float resolution);
    
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution);

TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
                            const float resolution);

std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
    const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
        range_measurements,
    const float resolution);

proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions(
    common::LuaParameterDictionary* const parameter_dictionary);

PointCloud AdaptiveVoxelFilter(
    const PointCloud& point_cloud,
    const proto::AdaptiveVoxelFilterOptions& options);

}  // namespace sensor
}  // namespace cartographer

相关接口函数

std::vector<RangefinderPoint> VoxelFilter(
    const std::vector<RangefinderPoint>& points, const float resolution) {
  return RandomizedVoxelFilter(
      points, resolution,
      [](const RangefinderPoint& point) { return point.position; });
}

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

  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]);
    }
  }
  return PointCloud(std::move(filtered_points),
                    std::move(filtered_intensities));
}

TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,
                            const float resolution) {
  return RandomizedVoxelFilter(
      timed_point_cloud, resolution,
      [](const TimedRangefinderPoint& point) { return point.position; });
}

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