Add overlapping trimmer params to TrajectoryBuilderOptions. (#1076)

cartographer/mapping/map_builder.cc

@@ -19,6 +19,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.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"
@@ -120,6 +121,14 @@ 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(common::make_unique<OverlappingSubmapsTrimmer2D>(
+          trimmer_options.fresh_submaps_count(),
+          trimmer_options.min_covered_cells_count()));
+    }
   }
   if (trajectory_options.pure_localization()) {
     constexpr int kSubmapsToKeep = 3;

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -31,6 +31,12 @@ message TrajectoryBuilderOptions {
   LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2;
   bool pure_localization = 3;
   InitialTrajectoryPose initial_trajectory_pose = 4;
+
+  message OverlappingSubmapsTrimmerOptions2D {
+    int32 fresh_submaps_count = 1;
+    int32 min_covered_cells_count = 2;
+  }
+  OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5;
 }
 
 message SensorId {

cartographer/mapping/trajectory_builder_interface.cc

@@ -22,6 +22,24 @@
 
 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_cells_count(
+      options_dictionary->GetInt("min_covered_cells_count"));
+}
+
+}  // namespace
 
 proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
     common::LuaParameterDictionary* const parameter_dictionary) {
@@ -34,6 +52,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
           parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
   options.set_pure_localization(
       parameter_dictionary->GetBool("pure_localization"));
+  PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary);
   return options;
 }