diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 63f8bcf..0bef976 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -182,7 +182,7 @@ void ConstraintBuilder::ComputeConstraint( const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_index); const sensor::PointCloud filtered_point_cloud = - sensor::ToPointCloud(adaptive_voxel_filter_.Filter(*point_cloud)); + adaptive_voxel_filter_.Filter(sensor::ToPointCloud(*point_cloud)); // The 'constraint_transform' (i <- j) is computed from: // - a 'filtered_point_cloud' in j, diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index fd6be7a..3fd818c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -72,8 +72,8 @@ class SparsePoseGraphTest : public ::testing::Test { sampling_ratio = 1., max_constraint_distance = 6., adaptive_voxel_filter = { - max_length = 1e-5, - min_num_points = 1, + max_length = 1e-2, + min_num_points = 1000, max_range = 50., }, min_score = 0.5, diff --git a/cartographer/sensor/voxel_filter.cc b/cartographer/sensor/voxel_filter.cc index 239010b..bdbd3f7 100644 --- a/cartographer/sensor/voxel_filter.cc +++ b/cartographer/sensor/voxel_filter.cc @@ -25,11 +25,10 @@ namespace sensor { namespace { -template -PointCloudType FilterByMaxRange(const PointCloudType& point_cloud, - const float max_range) { - PointCloudType result; - for (const auto& point : point_cloud) { +PointCloud FilterByMaxRange(const PointCloud& point_cloud, + const float max_range) { + PointCloud result; + for (const Eigen::Vector3f& point : point_cloud) { if (point.norm() <= max_range) { result.push_back(point); } @@ -37,15 +36,14 @@ PointCloudType FilterByMaxRange(const PointCloudType& point_cloud, return result; } -template -PointCloudType AdaptivelyVoxelFiltered( +PointCloud AdaptivelyVoxelFiltered( const proto::AdaptiveVoxelFilterOptions& options, - const PointCloudType& point_cloud) { + const PointCloud& point_cloud) { if (point_cloud.size() <= options.min_num_points()) { // 'point_cloud' is already sparse enough. return point_cloud; } - PointCloudType result = VoxelFiltered(point_cloud, options.max_length()); + PointCloud result = VoxelFiltered(point_cloud, options.max_length()); if (result.size() >= options.min_num_points()) { // Filtering with 'max_length' resulted in a sufficiently dense point cloud. return result; @@ -63,7 +61,7 @@ PointCloudType AdaptivelyVoxelFiltered( // edge length is at most 10% off. while ((high_length - low_length) / low_length > 1e-1f) { const float mid_length = (low_length + high_length) / 2.f; - const PointCloudType candidate = VoxelFiltered(point_cloud, mid_length); + const PointCloud candidate = VoxelFiltered(point_cloud, mid_length); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; @@ -79,37 +77,16 @@ PointCloudType AdaptivelyVoxelFiltered( } // namespace -PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, const float size) { +PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) { VoxelFilter voxel_filter(size); voxel_filter.InsertPointCloud(point_cloud); return voxel_filter.point_cloud(); } -PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) { - VoxelFilter3D voxel_filter(size); - voxel_filter.InsertPointCloud(point_cloud); - return voxel_filter.point_cloud(); -} - -VoxelFilter::VoxelFilter(const float size) : size_(size) {} - -void VoxelFilter::InsertPointCloud(const PointCloud2D& point_cloud) { - for (const Eigen::Vector2f& point : point_cloud) { - if (voxels_ - .emplace(common::RoundToInt64(point.x() / size_), - common::RoundToInt64(point.y() / size_)) - .second) { - point_cloud_.push_back(point); - } - } -} - -const PointCloud2D& VoxelFilter::point_cloud() const { return point_cloud_; } - -VoxelFilter3D::VoxelFilter3D(const float size) +VoxelFilter::VoxelFilter(const float size) : voxels_(size, Eigen::Vector3f::Zero()) {} -void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) { +void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) { for (const Eigen::Vector3f& point : point_cloud) { auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point)); if (*value == 0) { @@ -119,7 +96,7 @@ void VoxelFilter3D::InsertPointCloud(const PointCloud& point_cloud) { } } -const PointCloud& VoxelFilter3D::point_cloud() const { return point_cloud_; } +const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; } proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary) { @@ -135,12 +112,6 @@ AdaptiveVoxelFilter::AdaptiveVoxelFilter( const proto::AdaptiveVoxelFilterOptions& options) : options_(options) {} -PointCloud2D AdaptiveVoxelFilter::Filter( - const PointCloud2D& point_cloud) const { - return AdaptivelyVoxelFiltered( - options_, FilterByMaxRange(point_cloud, options_.max_range())); -} - PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { return AdaptivelyVoxelFiltered( options_, FilterByMaxRange(point_cloud, options_.max_range())); diff --git a/cartographer/sensor/voxel_filter.h b/cartographer/sensor/voxel_filter.h index 4f4da37..5b6da59 100644 --- a/cartographer/sensor/voxel_filter.h +++ b/cartographer/sensor/voxel_filter.h @@ -17,10 +17,6 @@ #ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ #define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ -#include -#include -#include - #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" @@ -29,10 +25,6 @@ namespace cartographer { namespace sensor { -// Returns a voxel filtered copy of 'point_cloud' where 'size' is the length -// a voxel edge. -PointCloud2D VoxelFiltered(const PointCloud2D& point_cloud, float size); - // Returns a voxel filtered copy of 'point_cloud' where 'size' is the length // a voxel edge. PointCloud VoxelFiltered(const PointCloud& point_cloud, float size); @@ -48,35 +40,6 @@ class VoxelFilter { VoxelFilter(const VoxelFilter&) = delete; VoxelFilter& operator=(const VoxelFilter&) = delete; - // Inserts a point cloud into the voxel filter. - void InsertPointCloud(const PointCloud2D& point_cloud); - - // Returns the filtered point cloud representing the occupied voxels. - const PointCloud2D& point_cloud() const; - - private: - struct IntegerPairHash { - size_t operator()(const std::pair& x) const { - const uint64 first = x.first; - const uint64 second = x.second; - return first ^ (first + 0x9e3779b9u + (second << 6) + (second >> 2)); - } - }; - - const float size_; - std::unordered_set, IntegerPairHash> voxels_; - PointCloud2D point_cloud_; -}; - -// The same as VoxelFilter but for 3D PointClouds. -class VoxelFilter3D { - public: - // 'size' is the length of a voxel edge. - explicit VoxelFilter3D(float size); - - VoxelFilter3D(const VoxelFilter3D&) = delete; - VoxelFilter3D& operator=(const VoxelFilter3D&) = delete; - // Inserts a point cloud into the voxel filter. void InsertPointCloud(const PointCloud& point_cloud); @@ -99,7 +62,6 @@ class AdaptiveVoxelFilter { AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; - PointCloud2D Filter(const PointCloud2D& point_cloud) const; PointCloud Filter(const PointCloud& point_cloud) const; private: diff --git a/cartographer/sensor/voxel_filter_test.cc b/cartographer/sensor/voxel_filter_test.cc index e3e0964..ca95291 100644 --- a/cartographer/sensor/voxel_filter_test.cc +++ b/cartographer/sensor/voxel_filter_test.cc @@ -27,13 +27,6 @@ namespace { using ::testing::ContainerEq; TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { - PointCloud2D point_cloud = { - {0.f, 0.f}, {0.1f, -0.1f}, {0.3f, -0.1f}, {0.f, 0.f}}; - EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f), - ContainerEq(PointCloud2D{point_cloud[0], point_cloud[2]})); -} - -TEST(VoxelFilter3DTest, ReturnsTheFirstPointInEachVoxel) { PointCloud point_cloud = {{0.f, 0.f, 0.f}, {0.1f, -0.1f, 0.1f}, {0.3f, -0.1f, 0.f},