diff --git a/cartographer/cloud/internal/handlers/add_trajectory_handler.cc b/cartographer/cloud/internal/handlers/add_trajectory_handler.cc index 569b193..237bcf6 100644 --- a/cartographer/cloud/internal/handlers/add_trajectory_handler.cc +++ b/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() ->local_trajectory_uploader() diff --git a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc b/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc index 76c757b..b392b26 100644 --- a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc +++ b/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( diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index d0ba2df..9639281 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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( - 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 = diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index 27af3f9..8604444 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/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) { diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 6c07eb9..227d905 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/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 { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 3b6706c..2461347 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/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; } diff --git a/configuration_files/trajectory_builder.lua b/configuration_files/trajectory_builder.lua index 52338a0..1fdc63c 100644 --- a/configuration_files/trajectory_builder.lua +++ b/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, +-- }, }