From 0da3fad9b0ee1411c322a127eed1f6a643e65b2a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 10 Jul 2017 16:14:50 +0200 Subject: [PATCH] Improve handling of grid updates. (#400) Changes the grids to only contain update markers during updates. Serialized grids will never contain them. --- cartographer/mapping_2d/probability_grid.h | 20 +++++++--------- .../mapping_2d/probability_grid_test.cc | 23 +++++++------------ .../mapping_2d/proto/probability_grid.proto | 1 - .../mapping_2d/range_data_inserter.cc | 6 ++--- .../mapping_2d/range_data_inserter_test.cc | 2 +- .../fast_correlative_scan_matcher_test.cc | 6 ++--- ...real_time_correlative_scan_matcher_test.cc | 2 +- cartographer/mapping_2d/submaps.cc | 1 - cartographer/mapping_3d/hybrid_grid.h | 6 ++--- cartographer/mapping_3d/hybrid_grid_test.cc | 9 ++++---- .../mapping_3d/range_data_inserter.cc | 3 ++- .../fast_correlative_scan_matcher_test.cc | 2 +- 12 files changed, 33 insertions(+), 48 deletions(-) diff --git a/cartographer/mapping_2d/probability_grid.h b/cartographer/mapping_2d/probability_grid.h index f5f0b77..f850b02 100644 --- a/cartographer/mapping_2d/probability_grid.h +++ b/cartographer/mapping_2d/probability_grid.h @@ -46,10 +46,7 @@ class ProbabilityGrid { mapping::kUnknownProbabilityValue) {} explicit ProbabilityGrid(const proto::ProbabilityGrid& proto) - : limits_(proto.limits()), - cells_(), - update_indices_(proto.update_indices().begin(), - proto.update_indices().end()) { + : limits_(proto.limits()), cells_() { if (proto.has_min_x()) { known_cells_box_ = Eigen::AlignedBox2i(Eigen::Vector2i(proto.min_x(), proto.min_y()), @@ -65,8 +62,8 @@ class ProbabilityGrid { // Returns the limits of this ProbabilityGrid. const MapLimits& limits() const { return limits_; } - // Starts the next update sequence. - void StartUpdate() { + // Finishes the update sequence. + void FinishUpdate() { while (!update_indices_.empty()) { DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker); cells_[update_indices_.back()] -= mapping::kUpdateMarker; @@ -87,7 +84,7 @@ class ProbabilityGrid { // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'cell_index' if the cell has not already // been updated. Multiple updates of the same cell will be ignored until - // StartUpdate() is called. Returns true if the cell was updated. + // FinishUpdate() is called. Returns true if the cell was updated. // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. @@ -136,7 +133,7 @@ class ProbabilityGrid { // Grows the map as necessary to include 'point'. This changes the meaning of // these coordinates going forward. This method must be called immediately - // after 'StartUpdate', before any calls to 'ApplyLookupTable'. + // after 'FinishUpdate', before any calls to 'ApplyLookupTable'. void GrowLimits(const Eigen::Vector2f& point) { CHECK(update_indices_.empty()); while (!limits_.Contains(limits_.GetCellIndex(point))) { @@ -175,10 +172,9 @@ class ProbabilityGrid { for (const auto cell : cells_) { result.mutable_cells()->Add(cell); } - result.mutable_update_indices()->Reserve(update_indices_.size()); - for (const auto update : update_indices_) { - result.mutable_update_indices()->Add(update); - } + CHECK(update_indices_.empty()) << "Serialiazing a probability grid during " + "an update is not supported. Finish the " + "update first."; if (!known_cells_box_.isEmpty()) { result.set_max_x(known_cells_box_.max().x()); result.set_max_y(known_cells_box_.max().y()); diff --git a/cartographer/mapping_2d/probability_grid_test.cc b/cartographer/mapping_2d/probability_grid_test.cc index 74a0da5..a476f1b 100644 --- a/cartographer/mapping_2d/probability_grid_test.cc +++ b/cartographer/mapping_2d/probability_grid_test.cc @@ -32,9 +32,6 @@ TEST(ProbabilityGridTest, ProtoConstructor) { for (int i = 6; i < 12; ++i) { proto.mutable_cells()->Add(static_cast(i)); } - for (int i = 13; i < 18; ++i) { - proto.mutable_update_indices()->Add(i); - } proto.set_max_x(19); proto.set_max_y(20); proto.set_min_x(21); @@ -43,8 +40,8 @@ TEST(ProbabilityGridTest, ProtoConstructor) { ProbabilityGrid grid(proto); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); - // TODO(macmason): Figure out how to test the contents of cells_, - // update_indices_, and {min, max}_{x, y}_ gracefully. + // TODO(macmason): Figure out how to test the contents of cells_ and + // {min, max}_{x, y}_ gracefully. } TEST(ProbabilityGridTest, ToProto) { @@ -55,8 +52,8 @@ TEST(ProbabilityGridTest, ToProto) { EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(), proto.limits().DebugString()); - // TODO(macmason): Figure out how to test the contents of cells_, - // update_indices_, and {min, max}_{x, y}_ gracefully. + // TODO(macmason): Figure out how to test the contents of cells_ and + // {min, max}_{x, y}_ gracefully. } TEST(ProbabilityGridTest, ApplyOdds) { @@ -75,36 +72,34 @@ TEST(ProbabilityGridTest, ApplyOdds) { probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5); - probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 0), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); + probability_grid.FinishUpdate(); EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5); - probability_grid.StartUpdate(); probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5); - probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(0, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1))); + probability_grid.FinishUpdate(); EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5); // Tests adding odds to an unknown cell. - probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42))); EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, 1e-4); - // Tests that further updates are ignored if StartUpdate() isn't called. + // Tests that further updates are ignored if FinishUpdate() isn't called. probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, 1e-4); - probability_grid.StartUpdate(); + probability_grid.FinishUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); @@ -123,7 +118,6 @@ TEST(ProbabilityGridTest, GetProbability) { ASSERT_EQ(2, cell_limits.num_x_cells); ASSERT_EQ(2, cell_limits.num_y_cells); - probability_grid.StartUpdate(); probability_grid.SetProbability( limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)), mapping::kMaxProbability); @@ -185,7 +179,6 @@ TEST(ProbabilityGridTest, CorrectCropping) { mapping::kMinProbability, mapping::kMaxProbability); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400))); - probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator( Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) { probability_grid.SetProbability(xy_index, value_distribution(rng)); diff --git a/cartographer/mapping_2d/proto/probability_grid.proto b/cartographer/mapping_2d/proto/probability_grid.proto index 0883818..6fc5b4b 100644 --- a/cartographer/mapping_2d/proto/probability_grid.proto +++ b/cartographer/mapping_2d/proto/probability_grid.proto @@ -23,7 +23,6 @@ message ProbabilityGrid { // These values are actually int16s, but protos don't have a native int16 // type. repeated int32 cells = 2; - repeated int32 update_indices = 3; optional int32 max_x = 4; optional int32 max_y = 5; optional int32 min_x = 6; diff --git a/cartographer/mapping_2d/range_data_inserter.cc b/cartographer/mapping_2d/range_data_inserter.cc index 77233e7..ba13ed4 100644 --- a/cartographer/mapping_2d/range_data_inserter.cc +++ b/cartographer/mapping_2d/range_data_inserter.cc @@ -53,11 +53,11 @@ RangeDataInserter::RangeDataInserter( void RangeDataInserter::Insert(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) const { - // By not starting a new update after hits are inserted, we give hits priority + // By not finishing the update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). - CHECK_NOTNULL(probability_grid)->StartUpdate(); CastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(), - probability_grid); + CHECK_NOTNULL(probability_grid)); + probability_grid->FinishUpdate(); } } // namespace mapping_2d diff --git a/cartographer/mapping_2d/range_data_inserter_test.cc b/cartographer/mapping_2d/range_data_inserter_test.cc index baf5702..9c76a7a 100644 --- a/cartographer/mapping_2d/range_data_inserter_test.cc +++ b/cartographer/mapping_2d/range_data_inserter_test.cc @@ -51,8 +51,8 @@ class RangeDataInserterTest : public ::testing::Test { range_data.returns.emplace_back(-0.5f, 3.5f, 0.f); range_data.origin.x() = -0.5f; range_data.origin.y() = 0.5f; - probability_grid_.StartUpdate(); range_data_inserter_->Insert(range_data, &probability_grid_); + probability_grid_.FinishUpdate(); } ProbabilityGrid probability_grid_; diff --git a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc index 6a45faa..f774a29 100644 --- a/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -41,7 +41,6 @@ TEST(PrecomputationGridTest, CorrectValues) { std::uniform_int_distribution distribution(0, 255); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250))); - probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { probability_grid.SetProbability( @@ -76,7 +75,6 @@ TEST(PrecomputationGridTest, TinyProbabilityGrid) { std::uniform_int_distribution distribution(0, 255); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4))); - probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { probability_grid.SetProbability( @@ -151,7 +149,6 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); - probability_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ Eigen::Vector3f(expected_pose.translation().x(), @@ -160,6 +157,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { point_cloud, transform::Embed3D(expected_pose.cast())), {}}, &probability_grid); + probability_grid.FinishUpdate(); FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); @@ -204,7 +202,6 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); - probability_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ transform::Embed3D(expected_pose * perturbation).translation(), @@ -212,6 +209,7 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { transform::Embed3D(expected_pose)), {}}, &probability_grid); + probability_grid.FinishUpdate(); FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); diff --git a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc index 004a7f5..a099f49 100644 --- a/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc @@ -55,10 +55,10 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { point_cloud_.emplace_back(-0.125f, 0.125f, 0.f); point_cloud_.emplace_back(-0.125f, 0.075f, 0.f); point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); - probability_grid_.StartUpdate(); range_data_inserter_->Insert( sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, &probability_grid_); + probability_grid_.FinishUpdate(); { auto parameter_dictionary = common::MakeDictionary( "return {" diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index f45b77a..1f78fb9 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -40,7 +40,6 @@ ProbabilityGrid ComputeCroppedProbabilityGrid( probability_grid.limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits)); - cropped_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { if (probability_grid.IsKnown(xy_index + offset)) { cropped_grid.SetProbability( diff --git a/cartographer/mapping_3d/hybrid_grid.h b/cartographer/mapping_3d/hybrid_grid.h index 0472546..6a3103e 100644 --- a/cartographer/mapping_3d/hybrid_grid.h +++ b/cartographer/mapping_3d/hybrid_grid.h @@ -485,8 +485,8 @@ class HybridGrid : public HybridGridBase { *mutable_value(index) = mapping::ProbabilityToValue(probability); } - // Starts the next update sequence. - void StartUpdate() { + // Finishes the update sequence. + void FinishUpdate() { while (!update_indices_.empty()) { DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker); *update_indices_.back() -= mapping::kUpdateMarker; @@ -497,7 +497,7 @@ class HybridGrid : public HybridGridBase { // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'index' if the cell has not already been // updated. Multiple updates of the same cell will be ignored until - // StartUpdate() is called. Returns true if the cell was updated. + // FinishUpdate() is called. Returns true if the cell was updated. // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. diff --git a/cartographer/mapping_3d/hybrid_grid_test.cc b/cartographer/mapping_3d/hybrid_grid_test.cc index ba78622..32e6354 100644 --- a/cartographer/mapping_3d/hybrid_grid_test.cc +++ b/cartographer/mapping_3d/hybrid_grid_test.cc @@ -40,33 +40,32 @@ TEST(HybridGridTest, ApplyOdds) { hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); - hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 0, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); + hybrid_grid.FinishUpdate(); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); - hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(0, 1, 0), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f))); + hybrid_grid.FinishUpdate(); EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); // Tests adding odds to an unknown cell. - hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); - // Tests that further updates are ignored if StartUpdate() isn't called. + // Tests that further updates are ignored if FinishUpdate() isn't called. hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); - hybrid_grid.StartUpdate(); + hybrid_grid.FinishUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); diff --git a/cartographer/mapping_3d/range_data_inserter.cc b/cartographer/mapping_3d/range_data_inserter.cc index 0516dc4..dc29901 100644 --- a/cartographer/mapping_3d/range_data_inserter.cc +++ b/cartographer/mapping_3d/range_data_inserter.cc @@ -78,7 +78,7 @@ RangeDataInserter::RangeDataInserter( void RangeDataInserter::Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid) const { - CHECK_NOTNULL(hybrid_grid)->StartUpdate(); + CHECK_NOTNULL(hybrid_grid); for (const Eigen::Vector3f& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); @@ -89,6 +89,7 @@ void RangeDataInserter::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()); + hybrid_grid->FinishUpdate(); } } // namespace mapping_3d diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc index 505ee3d..074cf6e 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc @@ -88,13 +88,13 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) { Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); HybridGrid hybrid_grid(0.05f); - hybrid_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ expected_pose.translation(), sensor::TransformPointCloud(point_cloud, expected_pose), {}}, &hybrid_grid); + hybrid_grid.FinishUpdate(); FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, options);