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