From a9ad813c26053403e40ae7c8dc46590ac06365e2 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 22 Oct 2020 09:24:47 +0200 Subject: [PATCH] Use intensities in `Submap3D` and `RangeDataInserter3D`. (#1765) `Submap3D` now also stores a pointer to `IntensityHybridGrid`. Adapted `RangeDataInserter3D` to also insert intensities. Signed-off-by: Wolfgang Hess --- .../io/hybrid_grid_points_processor.cc | 3 +- .../mapping/3d/range_data_inserter_3d.cc | 28 +++++++++-- .../mapping/3d/range_data_inserter_3d.h | 7 +-- .../mapping/3d/range_data_inserter_3d_test.cc | 49 +++++++++++++++++-- cartographer/mapping/3d/submap_3d.cc | 13 ++++- cartographer/mapping/3d/submap_3d.h | 9 ++++ .../3d/local_trajectory_builder_3d_test.cc | 1 + .../fast_correlative_scan_matcher_3d_test.cc | 4 +- .../range_data_inserter_options_3d.proto | 3 ++ configuration_files/trajectory_builder_3d.lua | 2 + 10 files changed, 106 insertions(+), 13 deletions(-) diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index a573498..c81c1d5 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -40,7 +40,8 @@ HybridGridPointsProcessor::FromDictionary( void HybridGridPointsProcessor::Process(std::unique_ptr batch) { range_data_inserter_.Insert( - {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_); + {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); next_->Process(std::move(batch)); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.cc b/cartographer/mapping/3d/range_data_inserter_3d.cc index 104f805..5553237 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -51,6 +51,21 @@ void InsertMissesIntoGrid(const std::vector& miss_table, } } +void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns, + IntensityHybridGrid* intensity_hybrid_grid, + const float intensity_threshold) { + if (returns.intensities().size() > 0) { + for (size_t i = 0; i < returns.size(); ++i) { + if (returns.intensities()[i] > intensity_threshold) { + continue; + } + const Eigen::Array3i hit_cell = + intensity_hybrid_grid->GetCellIndex(returns[i].position); + intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]); + } + } +} + } // namespace proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( @@ -62,6 +77,8 @@ proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( parameter_dictionary->GetDouble("miss_probability")); options.set_num_free_space_voxels( parameter_dictionary->GetInt("num_free_space_voxels")); + options.set_intensity_threshold( + parameter_dictionary->GetDouble("intensity_threshold")); CHECK_GT(options.hit_probability(), 0.5); CHECK_LT(options.miss_probability(), 0.5); return options; @@ -75,9 +92,10 @@ RangeDataInserter3D::RangeDataInserter3D( miss_table_( ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {} -void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const { - CHECK(hybrid_grid != nullptr); +void RangeDataInserter3D::Insert( + const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const { + CHECK_NOTNULL(hybrid_grid); for (const sensor::RangefinderPoint& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); @@ -88,6 +106,10 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, // (i.e. no hits will be ignored because of a miss in the same cell). InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, hybrid_grid, options_.num_free_space_voxels()); + if (intensity_hybrid_grid != nullptr) { + InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid, + options_.intensity_threshold()); + } hybrid_grid->FinishUpdate(); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.h b/cartographer/mapping/3d/range_data_inserter_3d.h index d21d2fa..0dd5a8e 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.h +++ b/cartographer/mapping/3d/range_data_inserter_3d.h @@ -36,9 +36,10 @@ class RangeDataInserter3D { RangeDataInserter3D(const RangeDataInserter3D&) = delete; RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete; - // Inserts 'range_data' into 'hybrid_grid'. - void Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const; + // Inserts 'range_data' into 'hybrid_grid' and optionally into + // 'intensity_hybrid_grid'. + void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const; private: const proto::RangeDataInserterOptions3D options_; diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index 0abc75a..83dcd4e 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -28,12 +28,13 @@ namespace { class RangeDataInserter3DTest : public ::testing::Test { protected: - RangeDataInserter3DTest() : hybrid_grid_(1.f) { + RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 1000, " + "intensity_threshold = 100, " "}"); options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get()); range_data_inserter_.reset(new RangeDataInserter3D(options_)); @@ -41,14 +42,28 @@ class RangeDataInserter3DTest : public ::testing::Test { void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); - std::vector returns = { + const std::vector returns = { {Eigen::Vector3f{-3.f, -1.f, 4.f}}, {Eigen::Vector3f{-2.f, 0.f, 4.f}}, {Eigen::Vector3f{-1.f, 1.f, 4.f}}, {Eigen::Vector3f{0.f, 2.f, 4.f}}}; range_data_inserter_->Insert( sensor::RangeData{origin, sensor::PointCloud(returns), {}}, - &hybrid_grid_); + &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); + } + + void InsertPointCloudWithIntensities() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + const std::vector intensities{7.f, 8.f, 9.f, 10.f}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}}, + &hybrid_grid_, &intensity_hybrid_grid_); } float GetProbability(float x, float y, float z) const { @@ -56,6 +71,11 @@ class RangeDataInserter3DTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } + float GetIntensity(float x, float y, float z) const { + return intensity_hybrid_grid_.GetIntensity( + intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + float IsKnown(float x, float y, float z) const { return hybrid_grid_.IsKnown( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); @@ -65,6 +85,7 @@ class RangeDataInserter3DTest : public ::testing::Test { private: HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; std::unique_ptr range_data_inserter_; proto::RangeDataInserterOptions3D options_; }; @@ -89,6 +110,28 @@ TEST_F(RangeDataInserter3DTest, InsertPointCloud) { } } +TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) { + InsertPointCloudWithIntensities(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x, y, 4.f)); + EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6); + } else { + EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), + 1e-4); + EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6); + } + } + } +} + TEST_F(RangeDataInserter3DTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 7d75bd9..9facbe4 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -202,6 +202,8 @@ Submap3D::Submap3D(const float high_resolution, const float low_resolution, absl::make_unique(high_resolution)), low_resolution_hybrid_grid_( absl::make_unique(low_resolution)), + high_resolution_intensity_hybrid_grid_( + absl::make_unique(high_resolution)), rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {} Submap3D::Submap3D(const proto::Submap3D& proto) @@ -278,9 +280,11 @@ void Submap3D::InsertData(const sensor::RangeData& range_data_in_local, range_data_inserter.Insert( FilterRangeDataByMaxRange(transformed_range_data, high_resolution_max_range), - high_resolution_hybrid_grid_.get()); + high_resolution_hybrid_grid_.get(), + high_resolution_intensity_hybrid_grid_.get()); range_data_inserter.Insert(transformed_range_data, - low_resolution_hybrid_grid_.get()); + low_resolution_hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); set_num_range_data(num_range_data() + 1); const float yaw_in_submap_from_gravity = transform::GetYaw( local_pose().inverse().rotation() * local_from_gravity_aligned); @@ -332,6 +336,11 @@ void ActiveSubmaps3D::AddSubmap( // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. CHECK(submaps_.front()->insertion_finished()); + // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use + // active submaps and their associated intensity hybrid grids for scan + // matching, we call `ForgetIntensityHybridGrid` once we remove the submap + // from active submaps and no longer need the intensity hybrid grid. + submaps_.front()->ForgetIntensityHybridGrid(); submaps_.erase(submaps_.begin()); } const Eigen::VectorXf initial_rotational_scan_matcher_histogram = diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index 5be68cb..e5d7e8b 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -60,6 +60,14 @@ class Submap3D : public Submap { const HybridGrid& low_resolution_hybrid_grid() const { return *low_resolution_hybrid_grid_; } + const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const { + CHECK(high_resolution_intensity_hybrid_grid_ != nullptr); + return *high_resolution_intensity_hybrid_grid_; + } + void ForgetIntensityHybridGrid() { + high_resolution_intensity_hybrid_grid_.reset(); + } + const Eigen::VectorXf& rotational_scan_matcher_histogram() const { return rotational_scan_matcher_histogram_; } @@ -79,6 +87,7 @@ class Submap3D : public Submap { std::unique_ptr high_resolution_hybrid_grid_; std::unique_ptr low_resolution_hybrid_grid_; + std::unique_ptr high_resolution_intensity_hybrid_grid_; Eigen::VectorXf rotational_scan_matcher_histogram_; }; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 381fde2..e59a1b9 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -129,6 +129,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { hit_probability = 0.7, miss_probability = 0.4, num_free_space_voxels = 0, + intensity_threshold = 100.0, }, }, } diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 85ea48a..0bcfece 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -94,6 +94,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " + "intensity_threshold = 100.0, " "}"); return CreateRangeDataInserterOptions3D(parameter_dictionary.get()); } @@ -106,7 +107,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), {}}, - hybrid_grid_.get()); + hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); hybrid_grid_->FinishUpdate(); return absl::make_unique( diff --git a/cartographer/mapping/proto/range_data_inserter_options_3d.proto b/cartographer/mapping/proto/range_data_inserter_options_3d.proto index 5fcf198..fd5a44a 100644 --- a/cartographer/mapping/proto/range_data_inserter_options_3d.proto +++ b/cartographer/mapping/proto/range_data_inserter_options_3d.proto @@ -28,4 +28,7 @@ message RangeDataInserterOptions3D { // Up to how many free space voxels are updated for scan matching. // 0 disables free space. int32 num_free_space_voxels = 3; + + // Do not insert intensities above this threshold into IntensityHybridGrid. + float intensity_threshold = 4; } diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 3bd3dd8..6e3d3b6 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -13,6 +13,7 @@ -- limitations under the License. MAX_3D_RANGE = 60. +INTENSITY_THRESHOLD = 40 TRAJECTORY_BUILDER_3D = { min_range = 1., @@ -96,6 +97,7 @@ TRAJECTORY_BUILDER_3D = { hit_probability = 0.55, miss_probability = 0.49, num_free_space_voxels = 2, + intensity_threshold = INTENSITY_THRESHOLD, }, }, }