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.