Add overlapping submaps trimmer. (#1027)

Trims submaps that have less than 'min_covered_cells_count' cells not overlapped by at least 'fresh_submaps_count` submaps.
master
Alexander Belyaev 2018-04-12 11:53:56 +02:00 committed by GitHub
parent e2966ca156
commit 771336b3c9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 496 additions and 7 deletions

View File

@ -331,6 +331,15 @@ class MapById {
: 0; : 0;
} }
// Returns count of all elements.
size_t size() const {
size_t size = 0;
for (const auto& item : trajectories_) {
size += item.second.data_.size();
}
return size;
}
// Returns Range object for range-based loops over the nodes of a trajectory. // Returns Range object for range-based loops over the nodes of a trajectory.
Range<ConstIterator> trajectory(const int trajectory_id) const { Range<ConstIterator> trajectory(const int trajectory_id) const {
return Range<ConstIterator>(BeginOfTrajectory(trajectory_id), return Range<ConstIterator>(BeginOfTrajectory(trajectory_id),

View File

@ -60,10 +60,12 @@ TEST(IdTest, EmptyMapById) {
EXPECT_FALSE(map_by_id.empty()); EXPECT_FALSE(map_by_id.empty());
map_by_id.Trim(id); map_by_id.Trim(id);
EXPECT_TRUE(map_by_id.empty()); EXPECT_TRUE(map_by_id.empty());
EXPECT_EQ(0, map_by_id.size());
} }
TEST(IdTest, MapByIdIterator) { TEST(IdTest, MapByIdIterator) {
MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>(); MapById<NodeId, int> map_by_id = CreateTestMapById<NodeId>();
EXPECT_EQ(4, map_by_id.size());
EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data); EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data);
EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) == EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) ==
map_by_id.EndOfTrajectory(7)); map_by_id.EndOfTrajectory(7));

View File

@ -0,0 +1,203 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
#include <algorithm>
#include "cartographer/mapping/2d/submap_2d.h"
namespace cartographer {
namespace mapping {
namespace {
class SubmapCoverageGrid2D {
public:
// Aliases for documentation only (no type-safety).
using CellId = std::pair<int64 /* x cells */, int64 /* y cells */>;
using StoredType = std::vector<std::pair<SubmapId, common::Time>>;
explicit SubmapCoverageGrid2D(const Eigen::Vector2d& offset)
: offset_(offset) {}
void AddPoint(const Eigen::Vector2d& point, const SubmapId& submap_id,
const common::Time& time) {
CellId cell_id{common::RoundToInt64(offset_(0) - point(0)),
common::RoundToInt64(offset_(1) - point(1))};
cells_[cell_id].emplace_back(submap_id, time);
}
const std::map<CellId, StoredType>& cells() const { return cells_; }
private:
const Eigen::Vector2d offset_;
std::map<CellId, StoredType> cells_;
};
Eigen::Vector2d GetCornerOfFirstSubmap(
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
auto submap_2d = std::static_pointer_cast<const Submap2D>(
submap_data.begin()->data.submap);
return submap_2d->probability_grid().limits().max();
}
// Iterates over every cell in a submap, transforms the center of the cell to
// the global frame and then adds the submap id and the timestamp of the most
// recent range data insertion into the global grid.
std::set<SubmapId> AddSubmapsToSubmapCoverageGrid2D(
const std::map<SubmapId, common::Time>& submap_freshness,
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
SubmapCoverageGrid2D* coverage_grid) {
std::set<SubmapId> all_submap_ids;
for (const auto& submap : submap_data) {
auto freshness = submap_freshness.find(submap.id);
if (freshness == submap_freshness.end()) continue;
if (!submap.data.submap->finished()) continue;
all_submap_ids.insert(submap.id);
const ProbabilityGrid& probability_grid =
std::static_pointer_cast<const Submap2D>(submap.data.submap)
->probability_grid();
// Iterate over every cell in a submap.
Eigen::Array2i offset;
CellLimits cell_limits;
probability_grid.ComputeCroppedLimits(&offset, &cell_limits);
if (cell_limits.num_x_cells == 0 || cell_limits.num_y_cells == 0) {
LOG(WARNING) << "Empty grid found in submap ID = " << submap.id;
continue;
}
const transform::Rigid2d projected_submap_pose =
transform::Project2D(submap.data.pose);
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
const Eigen::Array2i index = xy_index + offset;
if (!probability_grid.IsKnown(index)) continue;
const transform::Rigid2d center_of_cell_in_global_frame =
projected_submap_pose *
transform::Rigid2d::Translation({index.x() + 0.5, index.y() + 0.5});
coverage_grid->AddPoint(center_of_cell_in_global_frame.translation(),
submap.id, freshness->second);
}
}
return all_submap_ids;
}
// Uses intra-submap constraints and trajectory node timestamps to identify time
// of the last range data insertion to the submap.
std::map<SubmapId, common::Time> ComputeSubmapFreshness(
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
const MapById<NodeId, TrajectoryNode>& trajectory_nodes,
const std::vector<PoseGraphInterface::Constraint>& constraints) {
std::map<SubmapId, common::Time> submap_freshness;
// Find the node with the largest NodeId per SubmapId.
std::map<SubmapId, NodeId> submap_to_latest_node;
for (const PoseGraphInterface::Constraint& constraint : constraints) {
if (constraint.tag != PoseGraphInterface::Constraint::INTRA_SUBMAP) {
continue;
}
auto submap_to_node = submap_to_latest_node.find(constraint.submap_id);
if (submap_to_node == submap_to_latest_node.end()) {
submap_to_latest_node.insert(
std::make_pair(constraint.submap_id, constraint.node_id));
continue;
}
submap_to_node->second =
std::max(submap_to_node->second, constraint.node_id);
}
// Find timestamp of every latest node.
for (const auto& submap_id_to_node_id : submap_to_latest_node) {
auto submap_data_item = submap_data.find(submap_id_to_node_id.first);
if (submap_data_item == submap_data.end()) {
LOG(WARNING) << "Intra-submap constraint between SubmapID = "
<< submap_id_to_node_id.first << " and NodeID "
<< submap_id_to_node_id.second << " is missing submap data";
continue;
}
auto latest_node_id = trajectory_nodes.find(submap_id_to_node_id.second);
if (latest_node_id == trajectory_nodes.end()) continue;
submap_freshness[submap_data_item->id] = latest_node_id->data.time();
}
return submap_freshness;
}
// Returns IDs of submaps that have less than 'min_covered_cells_count' cells
// not overlapped by at least 'fresh_submaps_count' submaps.
std::vector<SubmapId> FindSubmapIdsToTrim(
const SubmapCoverageGrid2D& coverage_grid,
const std::set<SubmapId>& all_submap_ids, uint16 fresh_submaps_count,
uint16 min_covered_cells_count) {
std::map<SubmapId, uint16> cells_covered_by_submap;
for (const auto& cell : coverage_grid.cells()) {
std::vector<std::pair<SubmapId, common::Time>> submaps_per_cell(
cell.second);
// In case there are several submaps covering the cell, only the freshest
// submaps are kept.
if (submaps_per_cell.size() > fresh_submaps_count) {
// Sort by time in descending order.
std::sort(submaps_per_cell.begin(), submaps_per_cell.end(),
[](const std::pair<SubmapId, common::Time>& left,
const std::pair<SubmapId, common::Time>& right) {
return left.second > right.second;
});
submaps_per_cell.erase(submaps_per_cell.begin() + fresh_submaps_count,
submaps_per_cell.end());
}
for (const std::pair<SubmapId, common::Time>& submap : submaps_per_cell) {
++cells_covered_by_submap[submap.first];
}
}
std::vector<SubmapId> submap_ids_to_keep;
for (const auto& id_to_cells_count : cells_covered_by_submap) {
if (id_to_cells_count.second < min_covered_cells_count) continue;
submap_ids_to_keep.push_back(id_to_cells_count.first);
}
DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end()));
std::vector<SubmapId> result;
std::set_difference(all_submap_ids.begin(), all_submap_ids.end(),
submap_ids_to_keep.begin(), submap_ids_to_keep.end(),
std::inserter(result, result.begin()));
return result;
}
} // namespace
void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) {
const auto submap_data = pose_graph->GetAllSubmapData();
if (submap_data.size() == 0) return;
const auto constraints = pose_graph->GetConstraints();
const auto trajectory_nodes = pose_graph->GetTrajectoryNodes();
SubmapCoverageGrid2D coverage_grid(GetCornerOfFirstSubmap(submap_data));
const std::set<SubmapId> all_submap_ids = AddSubmapsToSubmapCoverageGrid2D(
ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints),
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_);
for (const SubmapId& id : submap_ids_to_remove) {
pose_graph->MarkSubmapAsTrimmed(id);
}
finished_ = true;
}
} // namespace mapping
} // namespace cartographer

View File

@ -0,0 +1,51 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_
#include "cartographer/common/port.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
namespace cartographer {
namespace mapping {
// Trims submaps that have less than 'min_covered_cells_count' cells not
// overlapped by at least 'fresh_submaps_count` submaps.
class OverlappingSubmapsTrimmer2D : public PoseGraphTrimmer {
public:
OverlappingSubmapsTrimmer2D(uint16 fresh_submaps_count,
uint16 min_covered_cells_count)
: fresh_submaps_count_(fresh_submaps_count),
min_covered_cells_count_(min_covered_cells_count) {}
~OverlappingSubmapsTrimmer2D() override = default;
void Trim(Trimmable* pose_graph) override;
bool IsFinished() override { return finished_; }
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_;
bool finished_ = false;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_OVERLAPPING_SUBMAPS_TRIMMER_H_

View File

@ -0,0 +1,187 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
#include <vector>
#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/internal/testing/fake_trimmable.h"
#include "cartographer/transform/transform.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace {
using ::cartographer::transform::Rigid2d;
using ::cartographer::transform::Rigid3d;
using ::testing::ElementsAre;
using ::testing::Field;
using ::testing::IsEmpty;
class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
protected:
// Creates a submap with num_cells x num_cells grid.
void AddSquareSubmap(const Rigid2d& pose_2d, int submap_index, int num_cells,
bool is_finished) {
const Rigid3d pose_3d = transform::Embed3D(pose_2d);
proto::Submap2D submap_2d;
submap_2d.set_num_range_data(1);
submap_2d.set_finished(is_finished);
*submap_2d.mutable_local_pose() = transform::ToProto(pose_3d);
auto* probability_grid = submap_2d.mutable_probability_grid();
for (int i = 0; i < num_cells * num_cells; ++i) {
probability_grid->add_cells(1);
}
auto* map_limits = probability_grid->mutable_limits();
map_limits->set_resolution(1.0);
*map_limits->mutable_max() =
transform::ToProto(Eigen::Vector2d(num_cells, num_cells));
map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
auto* know_cells_box = probability_grid->mutable_known_cells_box();
know_cells_box->set_min_x(0);
know_cells_box->set_min_y(0);
know_cells_box->set_max_x(num_cells - 1);
know_cells_box->set_max_y(num_cells - 1);
fake_pose_graph_.mutable_submap_data()->Insert(
{0 /* trajectory_id */, submap_index},
{std::make_shared<const Submap2D>(submap_2d), pose_3d});
}
void AddTrajectoryNode(int node_index, int64 timestamp) {
TrajectoryNode::Data data;
data.time = common::FromUniversal(timestamp);
fake_pose_graph_.mutable_trajectory_nodes()->Insert(
NodeId{0 /* trajectory_id */, node_index},
{std::make_shared<const TrajectoryNode::Data>(data), {} /* pose */});
}
void AddConstraint(int submap_index, int node_index, bool is_intra_submap) {
fake_pose_graph_.mutable_constraints()->push_back(
{SubmapId{0 /* trajectory_id */, submap_index},
NodeId{0 /* trajectory_id */, node_index},
{} /* pose */,
is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
: PoseGraphInterface::Constraint::INTER_SUBMAP});
}
testing::FakeTrimmable fake_pose_graph_;
};
::testing::Matcher<const SubmapId&> EqualsSubmapId(const SubmapId& expected) {
return ::testing::AllOf(
Field(&SubmapId::trajectory_id, expected.trajectory_id),
Field(&SubmapId::submap_index, expected.submap_index));
}
TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
0 /* min_covered_cells_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
}
TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
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 */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0})));
}
TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 1 /* num_cells */,
false /* 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 */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
}
TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
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 */);
AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false);
AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true);
AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
0 /* min_covered_cells_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 1})));
}
// This test covers two 4-cells submaps that overlap each other displaced like:
// ___
// _| |
// | |_ _|
// |___|
//
// The background submap should be trimmed, since it has only 3 cells
// not-covered by another submap.
TEST_F(OverlappingSubmapsTrimmer2DTest, ) {
AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */,
true /* is_finished */);
AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)),
1 /* submap_index */, 2 /* 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 */,
4 /* min_covered_cells_count */);
trimmer.Trim(&fake_pose_graph_);
EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
ElementsAre(EqualsSubmapId({0, 0})));
}
} // namespace
} // namespace mapping
} // namespace cartographer

View File

@ -27,32 +27,66 @@ namespace testing {
class FakeTrimmable : public Trimmable { class FakeTrimmable : public Trimmable {
public: public:
FakeTrimmable() = default;
// Populates dummy SubmapIDs.
FakeTrimmable(int trajectory_id, int num_submaps) { FakeTrimmable(int trajectory_id, int num_submaps) {
for (int index = 0; index < num_submaps; ++index) { for (int index = 0; index < num_submaps; ++index) {
submaps_.push_back(SubmapId{trajectory_id, index}); submap_data_.Insert(SubmapId{trajectory_id, index}, {});
} }
} }
~FakeTrimmable() override {} ~FakeTrimmable() override {}
int num_submaps(const int trajectory_id) const override { int num_submaps(const int trajectory_id) const override {
return submaps_.size() - trimmed_submaps_.size(); return submap_data_.size() - trimmed_submaps_.size();
} }
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override { std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override {
return submaps_; std::vector<SubmapId> submap_ids;
for (const auto& submap : submap_data_) {
submap_ids.push_back(submap.id);
}
return submap_ids;
}
void set_submap_data(
const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data) {
submap_data_ = submap_data;
}
MapById<SubmapId, PoseGraphInterface::SubmapData>* mutable_submap_data() {
return &submap_data_;
} }
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override { const override {
return {}; return submap_data_;
}
void set_trajectory_nodes(
const MapById<NodeId, TrajectoryNode>& trajectory_nodes) {
trajectory_nodes_ = trajectory_nodes;
}
MapById<NodeId, TrajectoryNode>* mutable_trajectory_nodes() {
return &trajectory_nodes_;
} }
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override { MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override {
return {}; return trajectory_nodes_;
}
void set_constraints(
const std::vector<PoseGraphInterface::Constraint>& constraints) {
constraints_ = constraints;
}
std::vector<PoseGraphInterface::Constraint>* mutable_constraints() {
return &constraints_;
} }
std::vector<PoseGraphInterface::Constraint> GetConstraints() const override { std::vector<PoseGraphInterface::Constraint> GetConstraints() const override {
return {}; return constraints_;
} }
void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { void MarkSubmapAsTrimmed(const SubmapId& submap_id) override {
@ -64,8 +98,11 @@ class FakeTrimmable : public Trimmable {
std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; } std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
private: private:
std::vector<SubmapId> submaps_;
std::vector<SubmapId> trimmed_submaps_; std::vector<SubmapId> trimmed_submaps_;
std::vector<PoseGraphInterface::Constraint> constraints_;
MapById<NodeId, TrajectoryNode> trajectory_nodes_;
MapById<SubmapId, PoseGraphInterface::SubmapData> submap_data_;
}; };
} // namespace testing } // namespace testing