From 771336b3c9372ccf1fb539323f54910acfd15f4f Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 12 Apr 2018 11:53:56 +0200 Subject: [PATCH] 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. --- cartographer/mapping/id.h | 9 + cartographer/mapping/id_test.cc | 2 + .../2d/overlapping_submaps_trimmer_2d.cc | 203 ++++++++++++++++++ .../2d/overlapping_submaps_trimmer_2d.h | 51 +++++ .../2d/overlapping_submaps_trimmer_2d_test.cc | 187 ++++++++++++++++ .../mapping/internal/testing/fake_trimmable.h | 51 ++++- 6 files changed, 496 insertions(+), 7 deletions(-) create mode 100644 cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc create mode 100644 cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h create mode 100644 cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 3fc49cb..49662c2 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -331,6 +331,15 @@ class MapById { : 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. Range trajectory(const int trajectory_id) const { return Range(BeginOfTrajectory(trajectory_id), diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index 1a7b8b7..bd6337c 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -60,10 +60,12 @@ TEST(IdTest, EmptyMapById) { EXPECT_FALSE(map_by_id.empty()); map_by_id.Trim(id); EXPECT_TRUE(map_by_id.empty()); + EXPECT_EQ(0, map_by_id.size()); } TEST(IdTest, MapByIdIterator) { MapById map_by_id = CreateTestMapById(); + EXPECT_EQ(4, map_by_id.size()); EXPECT_EQ(2, map_by_id.BeginOfTrajectory(7)->data); EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) == map_by_id.EndOfTrajectory(7)); diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc new file mode 100644 index 0000000..6c1cdd2 --- /dev/null +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -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 + +#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; + using StoredType = std::vector>; + + 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& cells() const { return cells_; } + + private: + const Eigen::Vector2d offset_; + std::map cells_; +}; + +Eigen::Vector2d GetCornerOfFirstSubmap( + const MapById& submap_data) { + auto submap_2d = std::static_pointer_cast( + 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 AddSubmapsToSubmapCoverageGrid2D( + const std::map& submap_freshness, + const MapById& submap_data, + SubmapCoverageGrid2D* coverage_grid) { + std::set 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(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 ComputeSubmapFreshness( + const MapById& submap_data, + const MapById& trajectory_nodes, + const std::vector& constraints) { + std::map submap_freshness; + + // Find the node with the largest NodeId per SubmapId. + std::map 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 FindSubmapIdsToTrim( + const SubmapCoverageGrid2D& coverage_grid, + const std::set& all_submap_ids, uint16 fresh_submaps_count, + uint16 min_covered_cells_count) { + std::map cells_covered_by_submap; + for (const auto& cell : coverage_grid.cells()) { + std::vector> 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& left, + const std::pair& 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& submap : submaps_per_cell) { + ++cells_covered_by_submap[submap.first]; + } + } + std::vector 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 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 all_submap_ids = AddSubmapsToSubmapCoverageGrid2D( + ComputeSubmapFreshness(submap_data, trajectory_nodes, constraints), + submap_data, &coverage_grid); + const std::vector 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 diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h new file mode 100644 index 0000000..53af80c --- /dev/null +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h @@ -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_ diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc new file mode 100644 index 0000000..f60f161 --- /dev/null +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -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 + +#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(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(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 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 diff --git a/cartographer/mapping/internal/testing/fake_trimmable.h b/cartographer/mapping/internal/testing/fake_trimmable.h index f6cb885..707e1b2 100644 --- a/cartographer/mapping/internal/testing/fake_trimmable.h +++ b/cartographer/mapping/internal/testing/fake_trimmable.h @@ -27,32 +27,66 @@ namespace testing { class FakeTrimmable : public Trimmable { public: + FakeTrimmable() = default; + + // Populates dummy SubmapIDs. FakeTrimmable(int trajectory_id, int num_submaps) { for (int index = 0; index < num_submaps; ++index) { - submaps_.push_back(SubmapId{trajectory_id, index}); + submap_data_.Insert(SubmapId{trajectory_id, index}, {}); } } ~FakeTrimmable() 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 GetSubmapIds(int trajectory_id) const override { - return submaps_; + std::vector submap_ids; + for (const auto& submap : submap_data_) { + submap_ids.push_back(submap.id); + } + return submap_ids; + } + + void set_submap_data( + const MapById& submap_data) { + submap_data_ = submap_data; + } + + MapById* mutable_submap_data() { + return &submap_data_; } MapById GetAllSubmapData() const override { - return {}; + return submap_data_; + } + + void set_trajectory_nodes( + const MapById& trajectory_nodes) { + trajectory_nodes_ = trajectory_nodes; + } + + MapById* mutable_trajectory_nodes() { + return &trajectory_nodes_; } MapById GetTrajectoryNodes() const override { - return {}; + return trajectory_nodes_; + } + + void set_constraints( + const std::vector& constraints) { + constraints_ = constraints; + } + + std::vector* mutable_constraints() { + return &constraints_; } std::vector GetConstraints() const override { - return {}; + return constraints_; } void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { @@ -64,8 +98,11 @@ class FakeTrimmable : public Trimmable { std::vector trimmed_submaps() { return trimmed_submaps_; } private: - std::vector submaps_; std::vector trimmed_submaps_; + + std::vector constraints_; + MapById trajectory_nodes_; + MapById submap_data_; }; } // namespace testing