From 8816d5710c8529d6e0463b72ff36e194a9227918 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 20 Apr 2018 09:05:05 +0200 Subject: [PATCH] Invoke trimmer only when there are enough added submaps. (#1095) --- .../2d/overlapping_submaps_trimmer_2d.cc | 5 ++- .../2d/overlapping_submaps_trimmer_2d.h | 10 ++++- .../2d/overlapping_submaps_trimmer_2d_test.cc | 41 ++++++++++++++++--- cartographer/mapping/map_builder.cc | 3 +- .../proto/trajectory_builder_options.proto | 1 + .../mapping/trajectory_builder_interface.cc | 2 + 6 files changed, 52 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index bf9515f..3869617 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -179,7 +179,9 @@ std::vector FindSubmapIdsToTrim( void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { const auto submap_data = pose_graph->GetAllSubmapData(); - if (submap_data.size() == 0) return; + if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) { + return; + } SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data)); const std::map submap_freshness = @@ -190,6 +192,7 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { const std::vector submap_ids_to_remove = FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_, min_covered_cells_count_); + current_submap_count_ = submap_data.size() - submap_ids_to_remove.size(); for (const SubmapId& id : submap_ids_to_remove) { pose_graph->MarkSubmapAsTrimmed(id); } diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h index 53af80c..641e3e2 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h @@ -28,9 +28,11 @@ namespace mapping { class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer { public: OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count, - uint16 min_covered_cells_count) + uint16 min_covered_cells_count, + uint16 min_added_submaps_count) : fresh_submaps_count_(fresh_submaps_count), - min_covered_cells_count_(min_covered_cells_count) {} + min_covered_cells_count_(min_covered_cells_count), + min_added_submaps_count_(min_added_submaps_count) {} ~OverlappingSubmapsTrimmer2D() override = default; void Trim(Trimmable* pose_graph) override; @@ -41,6 +43,10 @@ class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer { const uint16 fresh_submaps_count_; // Minimal number of covered cells to keep submap from trimming. const uint16 min_covered_cells_count_; + // Number of added submaps before the trimmer is invoked. + const uint16 min_added_submaps_count_; + // Current finished submap count. + uint16 current_submap_count_ = 0; bool finished_ = false; }; diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index b7c7f41..ef2689c 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -66,7 +66,6 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { know_cells_box->set_max_y(num_cells - 1); grid->mutable_probability_grid_2d(); - LOG(INFO) << submap_2d.DebugString(); fake_pose_graph_.mutable_submap_data()->Insert( {0 /* trajectory_id */, submap_index}, {std::make_shared(submap_2d), pose_3d}); @@ -101,7 +100,8 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test { TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) { OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, - 0 /* min_covered_cells_count */); + 0 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); trimmer.Trim(&fake_pose_graph_); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); } @@ -117,12 +117,38 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) { AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, - 0 /* min_covered_cells_count */); + 0 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); trimmer.Trim(&fake_pose_graph_); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), ElementsAre(EqualsSubmapId({0, 0}))); } +TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) { + AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */); + AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */); + AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true); + AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); + + OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, + 0 /* min_covered_cells_count */, + 2 /* min_added_submaps_count */); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); + + AddSquareSubmap(Rigid2d::Identity(), 2 /* submap_index */, 1 /* num_cells */, + true /* is_finished */); + AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */); + AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true); + trimmer.Trim(&fake_pose_graph_); + EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), + ElementsAre(EqualsSubmapId({0, 0}), EqualsSubmapId({0, 1}))); +} + TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) { AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */, false /* is_finished */); @@ -134,7 +160,8 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) { AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, - 0 /* min_covered_cells_count */); + 0 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); trimmer.Trim(&fake_pose_graph_); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty()); } @@ -152,7 +179,8 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) { AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, - 0 /* min_covered_cells_count */); + 0 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); trimmer.Trim(&fake_pose_graph_); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), ElementsAre(EqualsSubmapId({0, 1}))); @@ -178,7 +206,8 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) { AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true); OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */, - 4 /* min_covered_cells_count */); + 4 /* min_covered_cells_count */, + 0 /* min_added_submaps_count */); trimmer.Trim(&fake_pose_graph_); EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), ElementsAre(EqualsSubmapId({0, 0}))); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 2648f43..d601a9f 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -130,7 +130,8 @@ int MapBuilder::AddTrajectoryBuilder( trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options() .submaps_options() - .resolution()))); + .resolution()), + trimmer_options.min_added_submaps_count())); } } if (trajectory_options.pure_localization()) { diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index ab42a5d..489a194 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -35,6 +35,7 @@ message TrajectoryBuilderOptions { message OverlappingSubmapsTrimmerOptions2D { int32 fresh_submaps_count = 1; double min_covered_area = 2; + int32 min_added_submaps_count = 3; } OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5; } diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index cd689e0..3b6706c 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -37,6 +37,8 @@ void PopulateOverlappingSubmapsTrimmerOptions2D( options_dictionary->GetInt("fresh_submaps_count")); options->set_min_covered_area( options_dictionary->GetDouble("min_covered_area")); + options->set_min_added_submaps_count( + options_dictionary->GetInt("min_added_submaps_count")); } } // namespace