From c84da8ec0f69a4dae2c9dddb2f01db0041431855 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 30 Sep 2020 18:32:39 +0200 Subject: [PATCH] Random voxel filtering. (#1753) This leads to slightly slower performance but better quality. Also replaces the VoxelFilter class by free standing functions. Signed-off-by: Wolfgang Hess --- .../2d/local_trajectory_builder_2d.cc | 8 +- .../3d/local_trajectory_builder_3d.cc | 26 ++-- cartographer/sensor/internal/voxel_filter.cc | 131 ++++++++++-------- cartographer/sensor/internal/voxel_filter.h | 56 ++------ .../sensor/internal/voxel_filter_test.cc | 39 +++--- 5 files changed, 123 insertions(+), 137 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 55a1e4f..5bda793 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -58,8 +58,8 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns), - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)}; + sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), + sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())}; } std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( @@ -226,8 +226,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); const sensor::PointCloud& filtered_gravity_aligned_point_cloud = - sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options()) - .Filter(gravity_aligned_range_data.returns); + sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns, + options_.adaptive_voxel_filter_options()); if (filtered_gravity_aligned_point_cloud.empty()) { return nullptr; } diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 773dbce..361f724 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -154,9 +154,8 @@ LocalTrajectoryBuilder3D::AddRangeData( accumulated_point_cloud_origin_data_.clear(); } - synchronized_data.ranges = - sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) - .Filter(synchronized_data.ranges); + synchronized_data.ranges = sensor::VoxelFilter( + synchronized_data.ranges, 0.5f * options_.voxel_filter_size()); accumulated_point_cloud_origin_data_.emplace_back( std::move(synchronized_data)); ++num_accumulated_; @@ -237,10 +236,10 @@ LocalTrajectoryBuilder3D::AddRangeData( const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { extrapolation_result.current_pose.translation().cast(), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data.returns), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data.misses)}; + sensor::VoxelFilter(accumulated_range_data.returns, + options_.voxel_filter_size()), + sensor::VoxelFilter(accumulated_range_data.misses, + options_.voxel_filter_size())}; const auto voxel_filter_stop = std::chrono::steady_clock::now(); const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; @@ -274,19 +273,18 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const auto scan_matcher_start = std::chrono::steady_clock::now(); - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud high_resolution_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.high_resolution_adaptive_voxel_filter_options()); if (high_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty high resolution point cloud data."; return nullptr; } - sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( - options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = - low_resolution_adaptive_voxel_filter.Filter( - filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.low_resolution_adaptive_voxel_filter_options()); if (low_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty low resolution point cloud data."; return nullptr; diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index 4185b33..d7c9028 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -17,7 +17,10 @@ #include "cartographer/sensor/internal/voxel_filter.h" #include +#include +#include +#include "absl/container/flat_hash_map.h" #include "cartographer/common/math.h" namespace cartographer { @@ -43,7 +46,7 @@ PointCloud AdaptivelyVoxelFiltered( // 'point_cloud' is already sparse enough. return point_cloud; } - PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud); + PointCloud result = VoxelFilter(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; @@ -54,15 +57,14 @@ PointCloud AdaptivelyVoxelFiltered( for (float high_length = options.max_length(); high_length > 1e-2f * options.max_length(); high_length /= 2.f) { float low_length = high_length / 2.f; - result = VoxelFilter(low_length).Filter(point_cloud); + result = VoxelFilter(point_cloud, low_length); if (result.size() >= options.min_num_points()) { // Binary search to find the right amount of filtering. 'low_length' gave // a sufficiently dense 'result', 'high_length' did not. We stop when the // 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 PointCloud candidate = - VoxelFilter(mid_length).Filter(point_cloud); + const PointCloud candidate = VoxelFilter(point_cloud, mid_length); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; @@ -76,58 +78,77 @@ PointCloud AdaptivelyVoxelFiltered( return result; } +using VoxelKeyType = uint64_t; + +VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point, + const float resolution) { + const Eigen::Array3f index = point.array() / resolution; + const uint64_t x = common::RoundToInt(index.x()); + const uint64_t y = common::RoundToInt(index.y()); + const uint64_t z = common::RoundToInt(index.z()); + return (x << 42) + (y << 21) + z; +} + +template +std::vector RandomizedVoxelFilter(const std::vector& point_cloud, + const float resolution, + PointFunction&& point_function) { + // According to https://en.wikipedia.org/wiki/Reservoir_sampling + std::minstd_rand0 generator; + absl::flat_hash_map> + voxel_count_and_point_index; + for (size_t i = 0; i < point_cloud.size(); i++) { + auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex( + point_function(point_cloud[i]), resolution)]; + voxel.first++; + if (voxel.first == 1) { + voxel.second = i; + } else { + std::uniform_int_distribution<> distribution(1, voxel.first); + if (distribution(generator) == voxel.first) { + voxel.second = i; + } + } + } + std::vector points_used(point_cloud.size(), false); + for (const auto& voxel_and_index : voxel_count_and_point_index) { + points_used[voxel_and_index.second.second] = true; + } + + std::vector results; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + results.push_back(point_cloud[i]); + } + } + return results; +} + } // namespace -PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { - PointCloud results; - for (const RangefinderPoint& point : point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); - } - } - return results; +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { + return RandomizedVoxelFilter( + point_cloud, resolution, + [](const RangefinderPoint& point) { return point.position; }); } -TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { - TimedPointCloud results; - for (const TimedRangefinderPoint& point : timed_point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); - } - } - return results; +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution) { + return RandomizedVoxelFilter( + timed_point_cloud, resolution, + [](const TimedRangefinderPoint& point) { return point.position; }); } -std::vector VoxelFilter::Filter( - const std::vector& - range_measurements) { - std::vector results; - for (const auto& range_measurement : range_measurements) { - auto it_inserted = voxel_set_.insert( - IndexToKey(GetCellIndex(range_measurement.point_time.position))); - if (it_inserted.second) { - results.push_back(range_measurement); - } - } - return results; -} - -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())); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution) { + return RandomizedVoxelFilter( + range_measurements, resolution, + [](const sensor::TimedPointCloudOriginData::RangeMeasurement& + range_measurement) { + return range_measurement.point_time.position; + }); } proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( @@ -140,13 +161,11 @@ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( return options; } -AdaptiveVoxelFilter::AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options) - : options_(options) {} - -PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options) { return AdaptivelyVoxelFiltered( - options_, FilterByMaxRange(point_cloud, options_.max_range())); + options, FilterByMaxRange(point_cloud, options.max_range())); } } // namespace sensor diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h index f14575b..fd0c647 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -19,7 +19,6 @@ #include -#include "absl/container/flat_hash_set.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" @@ -28,55 +27,20 @@ namespace cartographer { namespace sensor { -// Voxel filter for point clouds. For each voxel, the assembled point cloud -// contains the first point that fell into it from any of the inserted point -// clouds. -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 Filter( - const std::vector& - 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_; - absl::flat_hash_set voxel_set_; -}; +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution); proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary); -class AdaptiveVoxelFilter { - public: - explicit AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options); - - AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; - AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; - - PointCloud Filter(const PointCloud& point_cloud) const; - - private: - const proto::AdaptiveVoxelFilterOptions options_; -}; +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options); } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index 320a99e..4c18f1e 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -24,33 +24,38 @@ namespace cartographer { namespace sensor { namespace { -using ::testing::ContainerEq; +using ::testing::Contains; -TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { - PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}, - {Eigen::Vector3f{0.1f, -0.1f, 0.1f}}, - {Eigen::Vector3f{0.3f, -0.1f, 0.f}}, - {Eigen::Vector3f{0.f, 0.f, 0.1f}}}; - EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); +TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { + const PointCloud point_cloud({{{0.f, 0.f, 0.f}}, + {{0.1f, -0.1f, 0.1f}}, + {{0.3f, -0.1f, 0.f}}, + {{0.f, 0.f, 0.1f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + ASSERT_EQ(result.size(), 2); + EXPECT_THAT(point_cloud, Contains(result[0])); + EXPECT_THAT(point_cloud, Contains(result[1])); + EXPECT_THAT(result, Contains(point_cloud[2])); } TEST(VoxelFilterTest, HandlesLargeCoordinates) { - PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}}, - {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}}, - {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}}, - {Eigen::Vector3f{-200000.f, 0.f, 0.f}}}; - EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[3]})); + const 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}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.01f); + EXPECT_EQ(result.size(), 2); + EXPECT_THAT(result, Contains(point_cloud[3])); } TEST(VoxelFilterTest, IgnoresTime) { TimedPointCloud timed_point_cloud; for (int i = 0; i < 100; ++i) { - timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i}); + timed_point_cloud.push_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]})); + const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f); + ASSERT_EQ(result.size(), 1); + EXPECT_THAT(timed_point_cloud, Contains(result[0])); } } // namespace