12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Move OverlappingSubmapTrimmerOptions to PoseGraphOptions (#1408)

master
Christoph Schütte 2018-09-04 10:12:19 +02:00 committed by Wally B. Feed
parent a4ff055d8f
commit 153952ddf0
11 changed files with 57 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc
View File

@ -41,6 +41,7 @@ class SubmapCoverageGrid2D {
}
const std::map<CellId, StoredType>& 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<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
submap_freshness, submap_data, &coverage_grid);
const std::vector<SubmapId> submap_ids_to_remove =
FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
min_covered_cells_count_);
const std::vector<SubmapId> 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);

8
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h
View File

@ -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.

12
cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc
View File

@ -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(),

11
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -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<OverlappingSubmapsTrimmer2D>(
trimmer_options.fresh_submaps_count(),
trimmer_options.min_covered_area(),
trimmer_options.min_added_submaps_count()));
}
}
PoseGraph2D::~PoseGraph2D() {
WaitForAllComputations();

14
cartographer/mapping/map_builder.cc
View File

@ -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<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(absl::make_unique<OverlappingSubmapsTrimmer2D>(
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());

17
cartographer/mapping/pose_graph.cc
View File

@ -69,6 +69,22 @@ std::vector<PoseGraph::Constraint> 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;
}

10
cartographer/mapping/proto/pose_graph_options.proto
View File

@ -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;
}

7
cartographer/mapping/proto/trajectory_builder_options.proto
View File

@ -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 {

18
cartographer/mapping/trajectory_builder_interface.cc
View File

@ -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(

5
configuration_files/pose_graph.lua
View File

@ -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,
-- },
}

5
configuration_files/trajectory_builder.lua
View File

@ -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,