Add max_submaps_with_pure_localization parameter (#1141)

Adds a parameter for the currently hard coded `kSubmapsToKeep` in `pure_localization` mode.

The new parameter `max_submaps_with_pure_localization` allows to specify the history size of submaps in `pure_localization` mode.

Closes #1116

cartographer/cloud/internal/handlers/add_trajectory_handler.cc

@@ -53,7 +53,7 @@ void AddTrajectoryHandler::OnRequest(
 
     // Don't instantiate the 'PureLocalizationTrimmer' on the server and don't
     // freeze the trajectory on the server.
-    trajectory_builder_options.set_pure_localization(false);
+    trajectory_builder_options.clear_pure_localization_trimmer();
 
     GetContext<MapBuilderContextInterface>()
         ->local_trajectory_uploader()

cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc

@@ -49,7 +49,9 @@ const std::string kMessage = R"(
         min_range: 20
         max_range: 30
       }
-      pure_localization: true
+      pure_localization_trimmer {
+        max_submaps_to_keep: 3
+      }
       initial_trajectory_pose {
         relative_pose {
           translation {
@@ -122,7 +124,7 @@ TEST_F(AddTrajectoryHandlerTest, WithLocalSlamUploader) {
       request.trajectory_builder_options();
   upstream_trajectory_builder_options.clear_trajectory_builder_2d_options();
   upstream_trajectory_builder_options.clear_trajectory_builder_3d_options();
-  upstream_trajectory_builder_options.set_pure_localization(false);
+  upstream_trajectory_builder_options.clear_pure_localization_trimmer();
   EXPECT_CALL(*mock_local_trajectory_uploader_,
               AddTrajectory(Eq(13), ParseSensorIds(request),
                             Truly(testing::BuildProtoPredicateEquals(

cartographer/mapping/map_builder.cc

@@ -145,10 +145,10 @@ int MapBuilder::AddTrajectoryBuilder(
           trimmer_options.min_added_submaps_count()));
     }
   }
-  if (trajectory_options.pure_localization()) {
-    constexpr int kSubmapsToKeep = 3;
+  if (trajectory_options.has_pure_localization_trimmer()) {
     pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
-        trajectory_id, kSubmapsToKeep));
+        trajectory_id,
+        trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
   }
   if (trajectory_options.has_initial_trajectory_pose()) {
     const auto& initial_trajectory_pose =

cartographer/mapping/pose_graph_trimmer.cc

@@ -24,7 +24,7 @@ namespace mapping {
 PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
                                                  const int num_submaps_to_keep)
     : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) {
-  CHECK_GE(num_submaps_to_keep, 3);
+  CHECK_GE(num_submaps_to_keep, 2) << "Cannot trim with less than 2 submaps";
 }
 
 void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -29,7 +29,6 @@ message InitialTrajectoryPose {
 message TrajectoryBuilderOptions {
   LocalTrajectoryBuilderOptions2D trajectory_builder_2d_options = 1;
   LocalTrajectoryBuilderOptions3D trajectory_builder_3d_options = 2;
-  bool pure_localization = 3;
   InitialTrajectoryPose initial_trajectory_pose = 4;
 
   message OverlappingSubmapsTrimmerOptions2D {
@@ -38,6 +37,11 @@ message TrajectoryBuilderOptions {
     int32 min_added_submaps_count = 3;
   }
   OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5;
+
+  message PureLocalizationTrimmerOptions {
+    int32 max_submaps_to_keep = 1;
+  }
+  PureLocalizationTrimmerOptions pure_localization_trimmer = 6;
 }
 
 message SensorId {

cartographer/mapping/trajectory_builder_interface.cc

@@ -41,6 +41,19 @@ void PopulateOverlappingSubmapsTrimmerOptions2D(
       options_dictionary->GetInt("min_added_submaps_count"));
 }
 
+void PopulatePureLocalizationTrimmerOptions(
+    proto::TrajectoryBuilderOptions* const trajectory_builder_options,
+    common::LuaParameterDictionary* const parameter_dictionary) {
+  constexpr char kDictionaryKey[] = "pure_localization_trimmer";
+  if (!parameter_dictionary->HasKey(kDictionaryKey)) return;
+
+  auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey);
+  auto* options =
+      trajectory_builder_options->mutable_pure_localization_trimmer();
+  options->set_max_submaps_to_keep(
+      options_dictionary->GetInt("max_submaps_to_keep"));
+}
+
 }  // namespace
 
 proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
@@ -52,9 +65,8 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
   *options.mutable_trajectory_builder_3d_options() =
       CreateLocalTrajectoryBuilderOptions3D(
           parameter_dictionary->GetDictionary("trajectory_builder_3d").get());
-  options.set_pure_localization(
-      parameter_dictionary->GetBool("pure_localization"));
   PopulateOverlappingSubmapsTrimmerOptions2D(&options, parameter_dictionary);
+  PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary);
   return options;
 }
 

configuration_files/trajectory_builder.lua

@@ -18,5 +18,7 @@ include "trajectory_builder_3d.lua"
 TRAJECTORY_BUILDER = {
   trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
   trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
-  pure_localization = false,
+--   pure_localization_trimmer = {
+--     max_submaps_to_keep = 3,
+--   },
 }