From cb41777b9ead9006705ffe917a2e63ece20f0c9d Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 12 Oct 2017 11:58:59 +0200 Subject: [PATCH] Introduces mapping::MapById in the 2D pose graph for submaps. (#578) PAIR=cschuet --- cartographer/mapping/id.h | 66 +++++++++++++----- cartographer/mapping/id_test.cc | 21 ++++++ cartographer/mapping_2d/sparse_pose_graph.cc | 67 +++++++------------ cartographer/mapping_2d/sparse_pose_graph.h | 4 +- .../sparse_pose_graph/optimization_problem.cc | 12 +--- .../sparse_pose_graph/optimization_problem.h | 2 +- cartographer/sensor/compressed_point_cloud.h | 9 ++- 7 files changed, 107 insertions(+), 74 deletions(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 0e85125..aa73590 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -19,11 +19,13 @@ #include #include +#include #include #include #include #include +#include "cartographer/common/port.h" #include "glog/logging.h" namespace cartographer { @@ -109,7 +111,8 @@ class NestedVectorsById { std::vector> data_; }; -// Like std::map, but indexed by 'IdType' which can be 'NodeId' or 'SubmapId'. +// Reminiscent of std::map, but indexed by 'IdType' which can be 'NodeId' or +// 'SubmapId'. template class MapById { private: @@ -121,25 +124,24 @@ class MapById { const DataType& data; }; - class ConstIterator - : public std::iterator { + class ConstIterator { public: - explicit ConstIterator(const MapById& map_by_id) - : current_trajectory_(map_by_id.trajectories_.begin()), + using iterator_category = std::bidirectional_iterator_tag; + using value_type = IdDataReference; + using difference_type = int64; + using pointer = const IdDataReference*; + using reference = const IdDataReference&; + + explicit ConstIterator(const MapById& map_by_id, const int trajectory_id) + : current_trajectory_( + map_by_id.trajectories_.lower_bound(trajectory_id)), end_trajectory_(map_by_id.trajectories_.end()) { if (current_trajectory_ != end_trajectory_) { current_data_ = current_trajectory_->second.data_.begin(); - end_data_ = current_trajectory_->second.data_.end(); AdvanceToValidDataIterator(); } } - static ConstIterator EndIterator(const MapById& map_by_id) { - auto it = ConstIterator(map_by_id); - it.current_trajectory_ = it.end_trajectory_; - return it; - } - IdDataReference operator*() const { CHECK(current_trajectory_ != end_trajectory_); return IdDataReference{ @@ -154,6 +156,16 @@ class MapById { return *this; } + ConstIterator& operator--() { + while (current_trajectory_ == end_trajectory_ || + current_data_ == current_trajectory_->second.data_.begin()) { + --current_trajectory_; + current_data_ = current_trajectory_->second.data_.end(); + } + --current_data_; + return *this; + } + bool operator==(const ConstIterator& it) const { if (current_trajectory_ == end_trajectory_ || it.current_trajectory_ == it.end_trajectory_) { @@ -168,20 +180,18 @@ class MapById { private: void AdvanceToValidDataIterator() { CHECK(current_trajectory_ != end_trajectory_); - while (current_data_ == end_data_) { + while (current_data_ == current_trajectory_->second.data_.end()) { ++current_trajectory_; if (current_trajectory_ == end_trajectory_) { return; } current_data_ = current_trajectory_->second.data_.begin(); - end_data_ = current_trajectory_->second.data_.end(); } } typename std::map::const_iterator current_trajectory_; typename std::map::const_iterator end_trajectory_; typename std::map::const_iterator current_data_; - typename std::map::const_iterator end_data_; }; // Appends data to a 'trajectory_id', creating trajectories as needed. @@ -217,6 +227,11 @@ class MapById { trajectory.data_.erase(it); } + bool Contains(const IdType& id) const { + return trajectories_.count(id.trajectory_id) != 0 && + trajectories_.at(id.trajectory_id).data_.count(GetIndex(id)) != 0; + } + const DataType& at(const IdType& id) const { return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id)); } @@ -225,8 +240,25 @@ class MapById { return trajectories_.at(id.trajectory_id).data_.at(GetIndex(id)); } - ConstIterator begin() const { return ConstIterator(*this); } - ConstIterator end() const { return ConstIterator::EndIterator(*this); } + // Support querying by trajectory. + ConstIterator BeginOfTrajectory(const int trajectory_id) const { + return ConstIterator(*this, trajectory_id); + } + ConstIterator EndOfTrajectory(const int trajectory_id) const { + return BeginOfTrajectory(trajectory_id + 1); + } + + // Returns 0 if 'trajectory_id' does not exist. + size_t SizeOfTrajectoryOrZero(const int trajectory_id) const { + return trajectories_.count(trajectory_id) + ? trajectories_.at(trajectory_id).data_.size() + : 0; + } + + ConstIterator begin() const { return BeginOfTrajectory(0); } + ConstIterator end() const { + return BeginOfTrajectory(std::numeric_limits::max()); + } bool empty() const { return begin() == end(); } diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index f7d33a2..46d448e 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/id.h" #include +#include #include #include "gtest/gtest.h" @@ -40,6 +41,9 @@ TEST(IdTest, MapByIdIterator) { map_by_id.Append(42, 3); map_by_id.Append(0, 0); map_by_id.Append(0, 1); + EXPECT_EQ(2, (*map_by_id.BeginOfTrajectory(7)).data); + EXPECT_TRUE(std::next(map_by_id.BeginOfTrajectory(7)) == + map_by_id.EndOfTrajectory(7)); std::deque> expected_id_data = { {NodeId{0, 0}, 0}, {NodeId{0, 1}, 1}, @@ -55,6 +59,23 @@ TEST(IdTest, MapByIdIterator) { EXPECT_TRUE(expected_id_data.empty()); } +TEST(IdTest, MapByIdPrevIterator) { + MapById map_by_id; + map_by_id.Append(42, 42); + auto it = map_by_id.end(); + ASSERT_TRUE(it != map_by_id.begin()); + std::advance(it, -1); + EXPECT_TRUE(it == map_by_id.begin()); +} + +TEST(IdTest, InsertIntoMapById) { + MapById map_by_id; + EXPECT_EQ(0, map_by_id.SizeOfTrajectoryOrZero(42)); + map_by_id.Append(42, 42); + map_by_id.Insert(NodeId{42, 5}, 42); + EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42)); +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 0b0cfe8..64a5286 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -55,30 +56,27 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( const int trajectory_id, const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); - auto submap_data = optimization_problem_.submap_data(); + const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. - if (static_cast(trajectory_id) >= submap_data.size() || - submap_data[trajectory_id].empty()) { + if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { optimization_problem_.AddSubmap( trajectory_id, sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])); - submap_data = optimization_problem_.submap_data(); } - CHECK_EQ(submap_data[trajectory_id].size(), 1); + CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); const mapping::SubmapId submap_id{trajectory_id, 0}; CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); return {submap_id}; } CHECK_EQ(2, insertion_submaps.size()); - CHECK(!submap_data.at(trajectory_id).empty()); - const mapping::SubmapId last_submap_id{ - trajectory_id, submap_data.at(trajectory_id).rbegin()->first}; + const auto end_it = submap_data.EndOfTrajectory(trajectory_id); + CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); + const mapping::SubmapId last_submap_id = (*std::prev(end_it)).id; if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // and 'insertions_submaps.back()' is new. - const auto& first_submap_pose = - submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose; + const auto& first_submap_pose = submap_data.at(last_submap_id).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * @@ -189,10 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, // submap's trajectory, it suffices to do a match constrained to a local // search window. const transform::Rigid2d initial_relative_pose = - optimization_problem_.submap_data() - .at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose.inverse() * + optimization_problem_.submap_data().at(submap_id).pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) .at(node_id.node_index) @@ -250,10 +245,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( constant_data->initial_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); const transform::Rigid2d optimized_pose = - optimization_problem_.submap_data() - .at(matching_id.trajectory_id) - .at(matching_id.submap_index) - .pose * + optimization_problem_.submap_data().at(matching_id).pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) .inverse() * pose; @@ -428,14 +420,9 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the optimized pose. - CHECK_GE(static_cast(submap_data_.num_trajectories()), - optimized_submap_transforms_.size()); - optimized_submap_transforms_.resize(submap_data_.num_trajectories()); - CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(), - submap_id.submap_index); - optimized_submap_transforms_.at(trajectory_id) - .emplace(submap_id.submap_index, - sparse_pose_graph::SubmapData{initial_pose_2d}); + CHECK_EQ(submap_id, + optimized_submap_transforms_.Append( + trajectory_id, sparse_pose_graph::SubmapData{initial_pose_2d})); AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -500,7 +487,7 @@ void SparsePoseGraph::RunOptimization() { optimization_problem_.Solve(constraints_, frozen_trajectories_); common::MutexLocker locker(&mutex_); - const auto submap_data = optimization_problem_.submap_data(); + const auto& submap_data = optimization_problem_.submap_data(); const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { @@ -601,19 +588,18 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const std::vector>& + const mapping::MapById& submap_transforms, const int trajectory_id) const { - if (trajectory_id >= static_cast(submap_transforms.size()) || - submap_transforms.at(trajectory_id).empty()) { + auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); + auto end_it = submap_transforms.EndOfTrajectory(trajectory_id); + if (begin_it == end_it) { return transform::Rigid3d::Identity(); } - - const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first; - const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index}; + const mapping::SubmapId last_optimized_submap_id = (*std::prev(end_it)).id; // Accessing 'local_pose' in Submap is okay, since the member is const. return transform::Embed3D( - submap_transforms.at(trajectory_id).at(submap_index).pose) * + submap_transforms.at(last_optimized_submap_id).pose) * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -625,16 +611,10 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( return {}; } auto submap = submap_data_.at(submap_id).submap; - if (submap_id.trajectory_id < - static_cast(optimized_submap_transforms_.size()) && - submap_id.submap_index < static_cast(optimized_submap_transforms_ - .at(submap_id.trajectory_id) - .size())) { + if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. return {submap, transform::Embed3D( - optimized_submap_transforms_.at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose)}; + optimized_submap_transforms_.at(submap_id).pose)}; } // We have to extrapolate. return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, @@ -647,7 +627,8 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) int SparsePoseGraph::TrimmingHandle::num_submaps( const int trajectory_id) const { - return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); + const auto& submap_data = parent_->optimization_problem_.submap_data(); + return submap_data.SizeOfTrajectoryOrZero(trajectory_id); } void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index a0bd973..590b05b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -161,7 +161,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Computes the local to global frame transform based on the given optimized // 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( - const std::vector>& + const mapping::MapById& submap_transforms, int trajectory_id) const REQUIRES(mutex_); @@ -214,7 +214,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - std::vector> + mapping::MapById optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 5dd902f..515466b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -255,15 +255,9 @@ const std::vector>& OptimizationProblem::node_data() return node_data_; } -std::vector> OptimizationProblem::submap_data() - const { - std::vector> result; - for (const auto& submap_id_data : submap_data_) { - result.resize(submap_id_data.id.trajectory_id + 1); - result[submap_id_data.id.trajectory_id].emplace( - submap_id_data.id.submap_index, submap_id_data.data); - } - return result; +const mapping::MapById& +OptimizationProblem::submap_data() const { + return submap_data_; } } // namespace sparse_pose_graph diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 9873b64..708f3bd 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -80,7 +80,7 @@ class OptimizationProblem { const std::set& frozen_trajectories); const std::vector>& node_data() const; - std::vector> submap_data() const; + const mapping::MapById& submap_data() const; private: struct TrajectoryData { diff --git a/cartographer/sensor/compressed_point_cloud.h b/cartographer/sensor/compressed_point_cloud.h index 03b5c00..2fbc798 100644 --- a/cartographer/sensor/compressed_point_cloud.h +++ b/cartographer/sensor/compressed_point_cloud.h @@ -58,9 +58,14 @@ class CompressedPointCloud { }; // Forward iterator for compressed point clouds. -class CompressedPointCloud::ConstIterator - : public std::iterator { +class CompressedPointCloud::ConstIterator { public: + using iterator_category = std::forward_iterator_tag; + using value_type = Eigen::Vector3f; + using difference_type = int64; + using pointer = const Eigen::Vector3f*; + using reference = const Eigen::Vector3f&; + // Creates begin iterator. explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud);