From 153952ddf00fb23e3cfb4e6427e02a18f46a708c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Tue, 4 Sep 2018 10:12:19 +0200 Subject: [PATCH] Move OverlappingSubmapTrimmerOptions to PoseGraphOptions (#1408) --- .../2d/overlapping_submaps_trimmer_2d.cc | 7 ++++--- .../2d/overlapping_submaps_trimmer_2d.h | 8 ++++---- .../2d/overlapping_submaps_trimmer_2d_test.cc | 12 ++++++------ .../mapping/internal/2d/pose_graph_2d.cc | 11 ++++++++++- cartographer/mapping/map_builder.cc | 14 -------------- cartographer/mapping/pose_graph.cc | 17 +++++++++++++++++ .../mapping/proto/pose_graph_options.proto | 10 ++++++++++ .../proto/trajectory_builder_options.proto | 7 +------ .../mapping/trajectory_builder_interface.cc | 18 ------------------ configuration_files/pose_graph.lua | 5 +++++ configuration_files/trajectory_builder.lua | 5 ----- 11 files changed, 57 insertions(+), 57 deletions(-) diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 3c5640e..a62b0cd 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -41,6 +41,7 @@ class SubmapCoverageGrid2D { } const std::map& 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 all_submap_ids = AddSubmapsToSubmapCoverageGrid2D( submap_freshness, submap_data, &coverage_grid); - const std::vector submap_ids_to_remove = - FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_, - min_covered_cells_count_); + const std::vector submap_ids_to_remove = FindSubmapIdsToTrim( + coverage_grid, all_submap_ids, fresh_submaps_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); diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h index 641e3e2..bd74691 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h +++ b/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. - const uint16 min_covered_cells_count_; + // Minimum area of covered space to keep submap from trimming measured in m^2. + const double min_covered_area_; // Number of added submaps before the trimmer is invoked. const uint16 min_added_submaps_count_; // Current finished submap count. diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index db63c60..1f2b985 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/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(), diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 814103a..88addd7 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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( + trimmer_options.fresh_submaps_count(), + trimmer_options.min_covered_area(), + trimmer_options.min_added_submaps_count())); + } +} PoseGraph2D::~PoseGraph2D() { WaitForAllComputations(); diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 0eeaeae..80eb7fe 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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(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( - 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()); diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index 05ecd00..ab7371a 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -69,6 +69,22 @@ std::vector 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; } diff --git a/cartographer/mapping/proto/pose_graph_options.proto b/cartographer/mapping/proto/pose_graph_options.proto index 14d08c5..de2c8a3 100644 --- a/cartographer/mapping/proto/pose_graph_options.proto +++ b/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; } diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 52747bb..d6d9c2f 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/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 { - int32 fresh_submaps_count = 1; - double min_covered_area = 2; - int32 min_added_submaps_count = 3; - } - OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5; + reserved 5; bool pure_localization = 3 [deprecated = true]; message PureLocalizationTrimmerOptions { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index c8fa31e..8ed8377 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/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( diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index eead726..e2ae45d 100644 --- a/configuration_files/pose_graph.lua +++ b/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, + -- }, } diff --git a/configuration_files/trajectory_builder.lua b/configuration_files/trajectory_builder.lua index ecd51d8..11c2e3e 100644 --- a/configuration_files/trajectory_builder.lua +++ b/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,