diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 96d8742..7b729c5 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -79,40 +79,6 @@ inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; } -template -class NestedVectorsById { - public: - // Appends data to a trajectory, creating trajectories as needed. - IdType Append(int trajectory_id, const ValueType& value) { - data_.resize(std::max(data_.size(), trajectory_id + 1)); - const IdType id{trajectory_id, - static_cast(data_[trajectory_id].size())}; - data_[trajectory_id].push_back(value); - return id; - } - - const ValueType& at(const IdType& id) const { - return data_.at(id.trajectory_id).at(GetIndex(id)); - } - ValueType& at(const IdType& id) { - return data_.at(id.trajectory_id).at(GetIndex(id)); - } - - int num_trajectories() const { return static_cast(data_.size()); } - int num_indices(int trajectory_id) const { - return static_cast(data_.at(trajectory_id).size()); - } - - // TODO(whess): Remove once no longer needed. - const std::vector> data() const { return data_; } - - private: - static int GetIndex(const NodeId& id) { return id.node_index; } - static int GetIndex(const SubmapId& id) { return id.submap_index; } - - std::vector> data_; -}; - template class Range { public: @@ -161,13 +127,15 @@ class MapById { } explicit ConstIterator(const MapById& map_by_id, const IdType& id) - : current_trajectory_( - map_by_id.trajectories_.find(id.trajectory_id)), + : current_trajectory_(map_by_id.trajectories_.find(id.trajectory_id)), end_trajectory_(map_by_id.trajectories_.end()) { - CHECK(current_trajectory_ != end_trajectory_); - current_data_ = - current_trajectory_->second.data_.find(MapById::GetIndex(id)); - CHECK(current_data_ != current_trajectory_->second.data_.end()); + if (current_trajectory_ != end_trajectory_) { + current_data_ = + current_trajectory_->second.data_.find(MapById::GetIndex(id)); + if (current_data_ == current_trajectory_->second.data_.end()) { + current_trajectory_ = end_trajectory_; + } + } } IdDataReference operator*() const { @@ -273,10 +241,9 @@ class MapById { return IdType{trajectory_id, index}; } - // Returns an iterator to the element at 'id' which must exist. - ConstIterator FindChecked(const IdType& id) { - return ConstIterator(*this, id); - } + // Returns an iterator to the element at 'id' or the end iterator if it does + // not exist. + ConstIterator find(const IdType& id) { return ConstIterator(*this, id); } // Inserts data (which must not exist already) into a trajectory. void Insert(const IdType& id, const DataType& data) { diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index c4f4ac4..8a6b23a 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -127,20 +127,22 @@ TEST(IdTest, InsertIntoMapById) { EXPECT_EQ(2, map_by_id.SizeOfTrajectoryOrZero(42)); } -TEST(IdTest, FindCheckedNodeId) { +TEST(IdTest, FindNodeId) { MapById map_by_id; map_by_id.Append(42, 42); map_by_id.Append(42, 43); map_by_id.Append(42, 44); - CHECK_EQ(map_by_id.FindChecked(NodeId{42, 1})->data, 43); + CHECK_EQ(map_by_id.find(NodeId{42, 1})->data, 43); + EXPECT_TRUE(map_by_id.find(NodeId{42, 3}) == map_by_id.end()); } -TEST(IdTest, FindCheckedSubmapId) { +TEST(IdTest, FindSubmapId) { MapById map_by_id; map_by_id.Append(42, 42); map_by_id.Append(42, 43); map_by_id.Append(42, 44); - CHECK_EQ(map_by_id.FindChecked(SubmapId{42, 1})->data, 43); + CHECK_EQ(map_by_id.find(SubmapId{42, 1})->data, 43); + EXPECT_TRUE(map_by_id.find(SubmapId{42, 3}) == map_by_id.end()); } } // namespace diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 06dd264..daf69d4 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -125,20 +125,11 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id, std::to_string(num_trajectory_builders()) + " trajectories."; } - const int num_submaps = - sparse_pose_graph_->num_submaps(submap_id.trajectory_id); - if (submap_id.submap_index < 0 || submap_id.submap_index >= num_submaps) { - return "Requested submap " + std::to_string(submap_id.submap_index) + - " from trajectory " + std::to_string(submap_id.trajectory_id) + - " but there are only " + std::to_string(num_submaps) + - " submaps in this trajectory."; - } - const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id); if (submap_data.submap == nullptr) { return "Requested submap " + std::to_string(submap_id.submap_index) + " from trajectory " + std::to_string(submap_id.trajectory_id) + - " but it has been trimmed."; + " but it does not exist: maybe it has been trimmed."; } submap_data.submap->ToResponseProto(submap_data.pose, response); return ""; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 146c752..73938c8 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -99,11 +99,9 @@ class SparsePoseGraph { // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() = 0; - // Return the number of submaps for the given 'trajectory_id'. - virtual int num_submaps(int trajectory_id) = 0; - // Returns the current optimized transform and submap itself for the given - // 'submap_id'. + // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does + // not exist (anymore). virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0; // Returns data for all Submaps by trajectory. diff --git a/cartographer/mapping/trajectory_node.cc b/cartographer/mapping/trajectory_node.cc index 2b4553b..368de8a 100644 --- a/cartographer/mapping/trajectory_node.cc +++ b/cartographer/mapping/trajectory_node.cc @@ -44,8 +44,7 @@ proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) { proto.add_rotational_scan_matcher_histogram( constant_data.rotational_scan_matcher_histogram(i)); } - *proto.mutable_local_pose() = - transform::ToProto(constant_data.local_pose); + *proto.mutable_local_pose() = transform::ToProto(constant_data.local_pose); return proto; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 0cad06e..ace52f9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -107,12 +107,9 @@ void SparsePoseGraph::AddScan( ++num_trajectory_nodes_; // Test if the 'insertion_submap.back()' is one we never saw before. - if (trajectory_id >= submap_data_.num_trajectories() || - submap_data_.num_indices(trajectory_id) == 0 || - submap_data_ - .at(mapping::SubmapId{ - trajectory_id, submap_data_.num_indices(trajectory_id) - 1}) - .submap != insertion_submaps.back()) { + if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != + insertion_submaps.back()) { // We grow 'submap_data_' as needed. This code assumes that the first // time we see a new submap is as 'insertion_submaps.back()'. const mapping::SubmapId submap_id = @@ -249,16 +246,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories(); - ++trajectory_id) { - for (int submap_index = 0; - submap_index < submap_data_.num_indices(trajectory_id); - ++submap_index) { - const mapping::SubmapId submap_id{trajectory_id, submap_index}; - if (submap_data_.at(submap_id).state == SubmapState::kFinished) { - CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0); - ComputeConstraint(node_id, submap_id); - } + for (const auto& submap_id_data : submap_data_) { + if (submap_id_data.data.state == SubmapState::kFinished) { + CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); + ComputeConstraint(node_id, submap_id_data.id); } } @@ -489,8 +480,7 @@ void SparsePoseGraph::RunOptimization() { const mapping::NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id; - auto node_it = - std::next(trajectory_nodes_.FindChecked(last_optimized_node_id)); + auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); ++node_it) { auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id); @@ -544,14 +534,6 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -int SparsePoseGraph::num_submaps(const int trajectory_id) { - common::MutexLocker locker(&mutex_); - if (trajectory_id >= submap_data_.num_trajectories()) { - return 0; - } - return submap_data_.num_indices(trajectory_id); -} - mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); @@ -561,20 +543,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( std::vector> SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - std::vector> - all_submap_data(submap_data_.num_trajectories()); - for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories(); - ++trajectory_id) { - all_submap_data[trajectory_id].reserve( - submap_data_.num_indices(trajectory_id)); - for (int submap_index = 0; - submap_index < submap_data_.num_indices(trajectory_id); - ++submap_index) { - all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock( - mapping::SubmapId{trajectory_id, submap_index})); + std::vector> result; + for (const int trajectory_id : submap_data_.trajectory_ids()) { + result.resize(trajectory_id + 1); + for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) { + result[trajectory_id].resize(submap_id_data.id.submap_index + 1); + result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id); } } - return all_submap_data; + return result; } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( @@ -597,10 +574,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { - if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) { + const auto it = submap_data_.find(submap_id); + if (it == submap_data_.end()) { return {}; } - auto submap = submap_data_.at(submap_id).submap; + auto submap = it->data.submap; if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. return {submap, transform::Embed3D( @@ -666,11 +644,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( } // Mark the submap with 'submap_id' as trimmed and remove its data. - auto& submap_data = parent_->submap_data_.at(submap_id); - CHECK(submap_data.state == SubmapState::kFinished); - submap_data.state = SubmapState::kTrimmed; - CHECK(submap_data.submap != nullptr); - submap_data.submap.reset(); + CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); + parent_->submap_data_.Trim(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 1382c14..b322f8c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - int num_submaps(int trajectory_id) EXCLUDES(mutex_) override; mapping::SparsePoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; std::vector> @@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // The current state of the submap in the background threads. When this // transitions to kFinished, all scans are tried to match against this submap. // Likewise, all new scans are matched against submaps which are finished. - enum class SubmapState { kActive, kFinished, kTrimmed }; + enum class SubmapState { kActive, kFinished }; struct SubmapData { std::shared_ptr submap; @@ -205,7 +204,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. - mapping::NestedVectorsById submap_data_ + mapping::MapById submap_data_ GUARDED_BY(mutex_); // Data that are currently being shown. diff --git a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc index fef87b6..2e6326a 100644 --- a/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc @@ -132,8 +132,9 @@ std::vector> HistogramsAtAnglesFromNodes( histograms_at_angles.emplace_back( node.constant_data->rotational_scan_matcher_histogram, transform::GetYaw( - node.global_pose * transform::Rigid3d::Rotation( - node.constant_data->gravity_alignment.inverse()))); + node.global_pose * + transform::Rigid3d::Rotation( + node.constant_data->gravity_alignment.inverse()))); } return histograms_at_angles; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 6f4e6ab..946ed96 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -104,12 +104,9 @@ void SparsePoseGraph::AddScan( ++num_trajectory_nodes_; // Test if the 'insertion_submap.back()' is one we never saw before. - if (trajectory_id >= submap_data_.num_trajectories() || - submap_data_.num_indices(trajectory_id) == 0 || - submap_data_ - .at(mapping::SubmapId{ - trajectory_id, submap_data_.num_indices(trajectory_id) - 1}) - .submap != insertion_submaps.back()) { + if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != + insertion_submaps.back()) { // We grow 'submap_data_' as needed. This code assumes that the first // time we see a new submap is as 'insertion_submaps.back()'. const mapping::SubmapId submap_id = @@ -185,7 +182,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, submap_data_.at(submap_id).node_ids) { submap_nodes.push_back(mapping::TrajectoryNode{ trajectory_nodes_.at(submap_node_id).constant_data, - inverse_submap_pose * trajectory_nodes_.at(submap_node_id).global_pose}); + inverse_submap_pose * + trajectory_nodes_.at(submap_node_id).global_pose}); } const common::Time scan_time = GetLatestScanTime(node_id, submap_id); @@ -268,16 +266,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( Constraint::INTRA_SUBMAP}); } - for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories(); - ++trajectory_id) { - for (int submap_index = 0; - submap_index < submap_data_.num_indices(trajectory_id); - ++submap_index) { - const mapping::SubmapId submap_id{trajectory_id, submap_index}; - if (submap_data_.at(submap_id).state == SubmapState::kFinished) { - CHECK_EQ(submap_data_.at(submap_id).node_ids.count(node_id), 0); - ComputeConstraint(node_id, submap_id); - } + for (const auto& submap_id_data : submap_data_) { + if (submap_id_data.data.state == SubmapState::kFinished) { + CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); + ComputeConstraint(node_id, submap_id_data.id); } } @@ -524,9 +516,8 @@ void SparsePoseGraph::RunOptimization() { const mapping::NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id; - auto node_it = - std::next(trajectory_nodes_.FindChecked(last_optimized_node_id)); - for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); + auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); + for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); ++node_it) { auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id); mutable_trajectory_node.global_pose = @@ -572,14 +563,6 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -int SparsePoseGraph::num_submaps(const int trajectory_id) { - common::MutexLocker locker(&mutex_); - if (trajectory_id >= submap_data_.num_trajectories()) { - return 0; - } - return submap_data_.num_indices(trajectory_id); -} - mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); @@ -589,20 +572,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( std::vector> SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - std::vector> - all_submap_data(submap_data_.num_trajectories()); - for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories(); - ++trajectory_id) { - all_submap_data[trajectory_id].reserve( - submap_data_.num_indices(trajectory_id)); - for (int submap_index = 0; - submap_index < submap_data_.num_indices(trajectory_id); - ++submap_index) { - all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock( - mapping::SubmapId{trajectory_id, submap_index})); + std::vector> result; + for (const int trajectory_id : submap_data_.trajectory_ids()) { + result.resize(trajectory_id + 1); + for (const auto& submap_id_data : submap_data_.trajectory(trajectory_id)) { + result[trajectory_id].resize(submap_id_data.id.submap_index + 1); + result[trajectory_id].back() = GetSubmapDataUnderLock(submap_id_data.id); } } - return all_submap_data; + return result; } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( @@ -624,10 +602,11 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { - if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) { + const auto it = submap_data_.find(submap_id); + if (it == submap_data_.end()) { return {}; } - auto submap = submap_data_.at(submap_id).submap; + auto submap = it->data.submap; if (optimized_submap_transforms_.Contains(submap_id)) { // We already have an optimized pose. return {submap, optimized_submap_transforms_.at(submap_id).pose}; @@ -692,11 +671,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( } // Mark the submap with 'submap_id' as trimmed and remove its data. - auto& submap_data = parent_->submap_data_.at(submap_id); - CHECK(submap_data.state == SubmapState::kFinished); - submap_data.state = SubmapState::kTrimmed; - CHECK(submap_data.submap != nullptr); - submap_data.submap.reset(); + CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); + parent_->submap_data_.Trim(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 9ceae10..5218e54 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -92,7 +92,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - int num_submaps(int trajectory_id) EXCLUDES(mutex_) override; mapping::SparsePoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; std::vector> @@ -107,7 +106,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // The current state of the submap in the background threads. When this // transitions to kFinished, all scans are tried to match against this submap. // Likewise, all new scans are matched against submaps which are finished. - enum class SubmapState { kActive, kFinished, kTrimmed }; + enum class SubmapState { kActive, kFinished }; struct SubmapData { std::shared_ptr submap; @@ -209,7 +208,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. - mapping::NestedVectorsById submap_data_ + mapping::MapById submap_data_ GUARDED_BY(mutex_); // Data that are currently being shown.