Invoke trimmer only when there are enough added submaps. (#1095)
parent
29c3e056ed
commit
8816d5710c
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
|
|
@ -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})));
|
||||
|
|
|
@ -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()) {
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue