Move OverlappingSubmapTrimmerOptions to PoseGraphOptions (#1408)

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

@@ -41,6 +41,7 @@ class SubmapCoverageGrid2D {
   }
 
   const std::map<CellId, StoredType>& cells() const { return cells_; }
+  double resolution() const { return resolution_; }
 
  private:
   Eigen::Vector2d offset_;
@@ -198,9 +199,9 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
                              pose_graph->GetConstraints());
   const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
       submap_freshness, submap_data, &coverage_grid);
-  const std::vector<SubmapId> submap_ids_to_remove =
+  const std::vector<SubmapId> submap_ids_to_remove = FindSubmapIdsToTrim(
-      FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
+      coverage_grid, all_submap_ids, fresh_submaps_count_,
-                          min_covered_cells_count_);
+      min_covered_area_ / common::Pow2(coverage_grid.resolution()));
   current_submap_count_ = submap_data.size() - submap_ids_to_remove.size();
   for (const SubmapId& id : submap_ids_to_remove) {
     pose_graph->TrimSubmap(id);

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

@@ -28,10 +28,10 @@ namespace mapping {
 class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
  public:
   OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
-                              uint16 min_covered_cells_count,
+                              double min_covered_area,
                               uint16 min_added_submaps_count)
       : fresh_submaps_count_(fresh_submaps_count),
-        min_covered_cells_count_(min_covered_cells_count),
+        min_covered_area_(min_covered_area),
         min_added_submaps_count_(min_added_submaps_count) {}
   ~OverlappingSubmapsTrimmer2D() override = default;
 
@@ -41,8 +41,8 @@ class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
  private:
   // Number of the most recent submaps to keep.
   const uint16 fresh_submaps_count_;
-  // Minimal number of covered cells to keep submap from trimming.
+  // Minimum area of covered space to keep submap from trimming measured in m^2.
-  const uint16 min_covered_cells_count_;
+  const double min_covered_area_;
   // Number of added submaps before the trimmer is invoked.
   const uint16 min_added_submaps_count_;
   // Current finished submap count.

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

@@ -103,7 +103,7 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
 
 TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
-                                      0 /* min_covered_cells_count */,
+                                      0 /* min_covered_area */,
                                       0 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
@@ -126,7 +126,7 @@ 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_area */,
                                       0 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
@@ -150,7 +150,7 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
 
   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
-                                      0 /* min_covered_cells_count */,
+                                      0 /* min_covered_area */,
                                       2 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
@@ -184,7 +184,7 @@ 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_area */,
                                       0 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
@@ -209,7 +209,7 @@ 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_area */,
                                       0 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
@@ -267,7 +267,7 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) {
   AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
 
   OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
-                                      0 /* min_covered_cells_count */,
+                                      0 /* min_covered_area */,
                                       0 /* min_added_submaps_count */);
   trimmer.Trim(&fake_pose_graph_);
   EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),

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

@@ -31,6 +31,7 @@
 #include "Eigen/Eigenvalues"
 #include "absl/memory/memory.h"
 #include "cartographer/common/math.h"
+#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
 #include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
 #include "cartographer/sensor/internal/voxel_filter.h"
@@ -49,7 +50,15 @@ PoseGraph2D::PoseGraph2D(
     : options_(options),
       optimization_problem_(std::move(optimization_problem)),
       constraint_builder_(options_.constraint_builder_options(), thread_pool),
-      thread_pool_(thread_pool) {}
+      thread_pool_(thread_pool) {
+  if (options.has_overlapping_submaps_trimmer_2d()) {
+    const auto& trimmer_options = options.overlapping_submaps_trimmer_2d();
+    AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
+        trimmer_options.fresh_submaps_count(),
+        trimmer_options.min_covered_area(),
+        trimmer_options.min_added_submaps_count()));
+  }
+}
 
 PoseGraph2D::~PoseGraph2D() {
   WaitForAllComputations();

cartographer/mapping/map_builder.cc

@@ -23,7 +23,6 @@
 #include "cartographer/io/proto_stream_deserializer.h"
 #include "cartographer/io/serialization_format_migration.h"
 #include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
-#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
 #include "cartographer/mapping/internal/2d/pose_graph_2d.h"
 #include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
 #include "cartographer/mapping/internal/3d/pose_graph_3d.h"
@@ -150,19 +149,6 @@ int MapBuilder::AddTrajectoryBuilder(
             std::move(local_trajectory_builder), trajectory_id,
             static_cast<PoseGraph2D*>(pose_graph_.get()),
             local_slam_result_callback)));
-
-    if (trajectory_options.has_overlapping_submaps_trimmer_2d()) {
-      const auto& trimmer_options =
-          trajectory_options.overlapping_submaps_trimmer_2d();
-      pose_graph_->AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
-          trimmer_options.fresh_submaps_count(),
-          trimmer_options.min_covered_area() /
-              common::Pow2(trajectory_options.trajectory_builder_2d_options()
-                               .submaps_options()
-                               .grid_options_2d()
-                               .resolution()),
-          trimmer_options.min_added_submaps_count()));
-    }
   }
   MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                   pose_graph_.get());

cartographer/mapping/pose_graph.cc

@@ -69,6 +69,22 @@ std::vector<PoseGraph::Constraint> FromProto(
   return constraints;
 }
 
+void PopulateOverlappingSubmapsTrimmerOptions2D(
+    proto::PoseGraphOptions* const pose_graph_options,
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  constexpr char kDictionaryKey[] = "overlapping_submaps_trimmer_2d";
+  if (!parameter_dictionary->HasKey(kDictionaryKey)) return;
+
+  auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey);
+  auto* options = pose_graph_options->mutable_overlapping_submaps_trimmer_2d();
+  options->set_fresh_submaps_count(
+      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"));
+}
+
 proto::PoseGraphOptions CreatePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
   proto::PoseGraphOptions options;
@@ -94,6 +110,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
   options.set_global_constraint_search_after_n_seconds(
       parameter_dictionary->GetDouble(
           "global_constraint_search_after_n_seconds"));
+  PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary);
   return options;
 }
 

cartographer/mapping/proto/pose_graph_options.proto

@@ -55,4 +55,14 @@ message PoseGraphOptions {
   // added between two trajectories, loop closure searches will be performed
   // globally rather than in a smaller search window.
   double global_constraint_search_after_n_seconds = 10;
+
+  message OverlappingSubmapsTrimmerOptions2D {
+    int32 fresh_submaps_count = 1;
+    double min_covered_area = 2;
+    int32 min_added_submaps_count = 3;
+  }
+
+  // Instantiates the 'OverlappingSubmapsTrimmer2d' which trims submaps from the
+  // pose graph based on the area of overlap.
+  OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 11;
 }

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -31,12 +31,7 @@ message TrajectoryBuilderOptions {
   LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2;
   InitialTrajectoryPose initial_trajectory_pose = 4;
 
-  message OverlappingSubmapsTrimmerOptions2D {
+  reserved 5;
-    int32 fresh_submaps_count = 1;
-    double min_covered_area = 2;
-    int32 min_added_submaps_count = 3;
-  }
-  OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5;
 
   bool pure_localization = 3 [deprecated = true];
   message PureLocalizationTrimmerOptions {

cartographer/mapping/trajectory_builder_interface.cc

@@ -24,23 +24,6 @@ namespace cartographer {
 namespace mapping {
 namespace {
 
-void PopulateOverlappingSubmapsTrimmerOptions2D(
-    proto::TrajectoryBuilderOptions* const trajectory_builder_options,
-    common::LuaParameterDictionary* const parameter_dictionary) {
-  constexpr char kDictionaryKey[] = "overlapping_submaps_trimmer_2d";
-  if (!parameter_dictionary->HasKey(kDictionaryKey)) return;
-
-  auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey);
-  auto* options =
-      trajectory_builder_options->mutable_overlapping_submaps_trimmer_2d();
-  options->set_fresh_submaps_count(
-      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"));
-}
-
 void PopulatePureLocalizationTrimmerOptions(
     proto::TrajectoryBuilderOptions* const trajectory_builder_options,
     common::LuaParameterDictionary* const parameter_dictionary) {
@@ -65,7 +48,6 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
   *options.mutable_trajectory_builder_3d_options() =
       CreateLocalTrajectoryBuilderOptions3D(
           parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
-  PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary);
   options.set_collate_fixed_frame(
       parameter_dictionary->GetBool("collate_fixed_frame"));
   options.set_collate_landmarks(

configuration_files/pose_graph.lua

@@ -83,4 +83,9 @@ POSE_GRAPH = {
   global_sampling_ratio = 0.003,
   log_residual_histograms = true,
   global_constraint_search_after_n_seconds = 10.,
+  --  overlapping_submaps_trimmer_2d = {
+  --    fresh_submaps_count = 1,
+  --    min_covered_area = 2,
+  --    min_added_submaps_count = 5,
+  --  },
 }

configuration_files/trajectory_builder.lua

@@ -20,11 +20,6 @@ TRAJECTORY_BUILDER = {
   trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
 --  pure_localization_trimmer = {
 --    max_submaps_to_keep = 3,
---  },
---  overlapping_submaps_trimmer_2d = {
---    fresh_submaps_count = 1,
---    min_covered_area = 2,
---    min_added_submaps_count = 5,
 --  },
   collate_fixed_frame = true,
   collate_landmarks = false,