From 74970be757b8080ca5fe2e9e0af088e0c01c80ea Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Tue, 17 Apr 2018 13:00:04 +0200
Subject: [PATCH] Add overlapping trimmer params to TrajectoryBuilderOptions.
 (#1076)

---
 cartographer/mapping/map_builder.cc           |  9 +++++++++
 .../proto/trajectory_builder_options.proto    |  6 ++++++
 .../mapping/trajectory_builder_interface.cc   | 19 +++++++++++++++++++
 3 files changed, 34 insertions(+)

diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc
index ead1931..b3303dc 100644
--- a/cartographer/mapping/map_builder.cc
+++ b/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;
diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto
index 4967e8c..13873d1 100644
--- a/cartographer/mapping/proto/trajectory_builder_options.proto
+++ b/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 {
diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc
index 36057bb..a64dc10 100644
--- a/cartographer/mapping/trajectory_builder_interface.cc
+++ b/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;
 }