diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc index fe496a0..ad28646 100644 --- a/cartographer/sensor/voxel_filter.cc +++ b/cartographer/sensor/voxel_filter.cc @@ -76,31 +76,43 @@ PointCloud AdaptivelyVoxelFiltered( return result; } -template -PointCloudType FilterPointCloudUsingVoxels( - const PointCloudType& point_cloud, mapping::HybridGridBase* voxels) { - PointCloudType results; - for (const typename PointCloudType::value_type& point : point_cloud) { - auto* const value = - voxels->mutable_value(voxels->GetCellIndex(point.template head<3>())); - if (*value == 0) { +} // namespace + +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); - *value = 1; } } return results; } -} // namespace - -VoxelFilter::VoxelFilter(const float size) : voxels_(size) {} - -PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { - return FilterPointCloudUsingVoxels(point_cloud, &voxels_); +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; } -TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { - return FilterPointCloudUsingVoxels(timed_point_cloud, &voxels_); +VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) { + KeyType k_0(static_cast(index[0])); + KeyType k_1(static_cast(index[1])); + KeyType k_2(static_cast(index[2])); + return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2; +} + +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())); } proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( diff --git a/cartographer/sensor/voxel_filter.h b/cartographer/sensor/voxel_filter.h index 0470b0b..ed08f5b 100644 --- a/cartographer/sensor/voxel_filter.h +++ b/cartographer/sensor/voxel_filter.h @@ -17,8 +17,10 @@ #ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ #define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ +#include +#include + #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" @@ -31,7 +33,7 @@ namespace sensor { class VoxelFilter { public: // 'size' is the length of a voxel edge. - explicit VoxelFilter(float size); + explicit VoxelFilter(float size) : resolution_(size){}; VoxelFilter(const VoxelFilter&) = delete; VoxelFilter& operator=(const VoxelFilter&) = delete; @@ -43,7 +45,14 @@ class VoxelFilter { TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); private: - mapping::HybridGridBase voxels_; + 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 voxel_set_; }; proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( diff --git a/cartographer/sensor/voxel_filter_test.cc b/cartographer/sensor/voxel_filter_test.cc index 8187eea..b0f39c7 100644 --- a/cartographer/sensor/voxel_filter_test.cc +++ b/cartographer/sensor/voxel_filter_test.cc @@ -35,6 +35,24 @@ TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); } +TEST(VoxelFilterTest, HandlesLargeCoordinates) { + PointCloud point_cloud = {{100000.f, 0.f, 0.f}, + {100000.001f, -0.0001f, 0.0001f}, + {100000.003f, -0.0001f, 0.f}, + {-200000.f, 0.f, 0.f}}; + EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud), + ContainerEq(PointCloud{point_cloud[0], point_cloud[3]})); +} + +TEST(VoxelFilterTest, IgnoresTime) { + TimedPointCloud timed_point_cloud; + for (int i = 0; i < 100; ++i) { + timed_point_cloud.emplace_back(-100.f, 0.3f, 0.4f, 1.f * i); + } + EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud), + ContainerEq(TimedPointCloud{timed_point_cloud[0]})); +} + } // namespace } // namespace sensor } // namespace cartographer