Cartographer源码阅读2D&3D-前端体素滤波-VoxelFilter

Cartographer源码阅读-2D前端体素滤波-VoxelFilter

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

class VoxelFilter {
 public:
  // 'size' is the length of a voxel edge.
  // 构造函数传入体素大小
  explicit VoxelFilter(float size) : resolution_(size) {}

  VoxelFilter(const VoxelFilter&) = delete;
  VoxelFilter& operator=(const VoxelFilter&) = delete;

  // Returns a voxel filtered copy of 'point_cloud'.
  PointCloud Filter(const PointCloud& point_cloud);

  // Same for TimedPointCloud.
  TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud);

  // Same for RangeMeasurement.
  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> Filter(
      const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&
          range_measurements);

 private:
  using KeyType = std::bitset<3 * 32>;

  static KeyType IndexToKey(const Eigen::Array3i& index);

  Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const;

  float resolution_;
  std::unordered_set<KeyType> voxel_set_;
};

接口：

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

TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) {
  TimedPointCloud results;
  for (const Eigen::Vector4f& point : timed_point_cloud) {
   // 该体素之前没有没占据过，则插入，否则，不插入
    auto it_inserted =
        voxel_set_.insert(IndexToKey(GetCellIndex(point.head<3>())));
    if (it_inserted.second) {
      results.push_back(point);
    }
  }
  return results;
}
// 将cell_index转换为一个32字节的数
VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) {
  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;
}
// 3D点除以体素大小，得到所在的cell_index
Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const {
  Eigen::Array3f index = point.array() / resolution_;
  return Eigen::Array3i(common::RoundToInt(index.x()),
                        common::RoundToInt(index.y()),
                        common::RoundToInt(index.z()));
}
~~