Invoke trimmer only when there are enough added submaps. (#1095)

cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc

@@ -179,7 +179,9 @@ std::vector<SubmapId> 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<SubmapId, common::Time> submap_freshness =
@@ -190,6 +192,7 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
   const std::vector<SubmapId> 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);
   }

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;
 };

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<const Submap2D>(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})));

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()) {

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;
 }

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