Invoke trimmer only when there are enough added submaps. (#1095)

master
Alexander Belyaev 2018-04-20 09:05:05 +02:00 committed by GitHub
parent 29c3e056ed
commit 8816d5710c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 52 additions and 10 deletions

View File

@ -179,7 +179,9 @@ std::vector<SubmapId> FindSubmapIdsToTrim(
void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
const auto submap_data = pose_graph->GetAllSubmapData();
if (submap_data.size() == 0) return;
if (submap_data.size() - current_submap_count_ <= min_added_submaps_count_) {
return;
}
SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
const std::map<SubmapId, common::Time> submap_freshness =
@ -190,6 +192,7 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
const std::vector<SubmapId> submap_ids_to_remove =
FindSubmapIdsToTrim(coverage_grid, all_submap_ids, fresh_submaps_count_,
min_covered_cells_count_);
current_submap_count_ = submap_data.size() - submap_ids_to_remove.size();
for (const SubmapId& id : submap_ids_to_remove) {
pose_graph->MarkSubmapAsTrimmed(id);
}

View File

@ -28,9 +28,11 @@ namespace mapping {
class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
public:
OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
uint16 min_covered_cells_count)
uint16 min_covered_cells_count,
uint16 min_added_submaps_count)
: fresh_submaps_count_(fresh_submaps_count),
min_covered_cells_count_(min_covered_cells_count) {}
min_covered_cells_count_(min_covered_cells_count),
min_added_submaps_count_(min_added_submaps_count) {}
~OverlappingSubmapsTrimmer2D() override = default;
void Trim(Trimmable* pose_graph) override;
@ -41,6 +43,10 @@ class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
const uint16 fresh_submaps_count_;
// Minimal number of covered cells to keep submap from trimming.
const uint16 min_covered_cells_count_;
// Number of added submaps before the trimmer is invoked.
const uint16 min_added_submaps_count_;
// Current finished submap count.
uint16 current_submap_count_ = 0;
bool finished_ = false;
};

View File

@ -66,7 +66,6 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
know_cells_box->set_max_y(num_cells - 1);
grid->mutable_probability_grid_2d();
LOG(INFO) << submap_2d.DebugString();
fake_pose_graph_.mutable_submap_data()->Insert(
{0 /* trajectory_id */, submap_index},
{std::make_shared<const Submap2D>(submap_2d), pose_3d});
@ -101,7 +100,8 @@ class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
0 /* min_covered_cells_count */);
0 /* min_covered_cells_count */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
}
@ -117,12 +117,38 @@ 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_cells_count */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0})));
}
TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
true /* is_finished */);
AddSquareSubmap(Rigid2d::Identity(), 1 /* submap_index */, 1 /* num_cells */,
true /* is_finished */);
AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
0 /* min_covered_cells_count */,
2 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
AddSquareSubmap(Rigid2d::Identity(), 2 /* submap_index */, 1 /* num_cells */,
true /* is_finished */);
AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0}), EqualsSubmapId({0, 1})));
}
TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
false /* is_finished */);
@ -134,7 +160,8 @@ 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_cells_count */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
}
@ -152,7 +179,8 @@ 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_cells_count */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 1})));
@ -178,7 +206,8 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
4 /* min_covered_cells_count */);
4 /* min_covered_cells_count */,
0 /* min_added_submaps_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0})));

View File

@ -130,7 +130,8 @@ int MapBuilder::AddTrajectoryBuilder(
trimmer_options.min_covered_area() /
common::Pow2(trajectory_options.trajectory_builder_2d_options()
.submaps_options()
.resolution())));
.resolution()),
trimmer_options.min_added_submaps_count()));
}
}
if (trajectory_options.pure_localization()) {

View File

@ -35,6 +35,7 @@ message TrajectoryBuilderOptions {
message OverlappingSubmapsTrimmerOptions2D {
int32 fresh_submaps_count = 1;
double min_covered_area = 2;
int32 min_added_submaps_count = 3;
}
OverlappingSubmapsTrimmerOptions2D overlapping_submaps_trimmer_2d = 5;
}

View File

@ -37,6 +37,8 @@ void PopulateOverlappingSubmapsTrimmerOptions2D(
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"));
}
} // namespace