diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index bb1bcf0..858ff5b 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -36,15 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto( LOG(FATAL) << "Unsupported tag."; } -std::unordered_map ComputeTrajectoryIds( - const std::vector& trajectories) { - std::unordered_map result; - for (const auto& trajectory : trajectories) { - result.emplace(trajectory, result.size()); - } - return result; -} - void GroupTrajectoryNodes( const std::vector& trajectory_nodes, const std::unordered_map& trajectory_ids, @@ -100,21 +91,14 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; - const auto trajectory_nodes = GetTrajectoryNodes(); - std::vector submap_pointers; - for (const auto& trajectory_node : trajectory_nodes) { - submap_pointers.push_back(trajectory_node.constant_data->trajectory); - } - - const auto trajectory_ids = ComputeTrajectoryIds(submap_pointers); - std::vector> grouped_nodes; std::vector> grouped_node_indices; - GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes, + GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes, &grouped_node_indices); std::vector> grouped_submap_indices; - GroupSubmapStates(GetSubmapStates(), trajectory_ids, &grouped_submap_indices); + GroupSubmapStates(GetSubmapStates(), trajectory_ids(), + &grouped_submap_indices); for (const auto& constraint : constraints()) { auto* const constraint_proto = proto.add_constraint(); diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index e390eda..a2cef06 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -35,11 +35,6 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); -// Construct a mapping from trajectory (ie Submaps*) to an integer. These -// values are used to track trajectory identity between scans and submaps. -std::unordered_map ComputeTrajectoryIds( - const std::vector& trajectories); - // TrajectoryNodes are provided in a flat vector, but serialization requires // that we group them by trajectory. This groups the elements of // 'trajectory_nodes' into 'grouped_nodes' (so that (*grouped_nodes)[i] @@ -132,6 +127,9 @@ class SparsePoseGraph { // Returns the collection of constraints. virtual std::vector constraints() = 0; + + // Returns the mapping from Submaps* to trajectory IDs. + virtual const std::unordered_map& trajectory_ids() = 0; }; // Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be diff --git a/cartographer/mapping/sparse_pose_graph_test.cc b/cartographer/mapping/sparse_pose_graph_test.cc index 168616a..a2224c5 100644 --- a/cartographer/mapping/sparse_pose_graph_test.cc +++ b/cartographer/mapping/sparse_pose_graph_test.cc @@ -56,21 +56,11 @@ TEST(SparsePoseGraphTest, TrajectoryFunctions) { } } - std::vector submap_pointers; - for (const auto& submap : submaps) { - submap_pointers.push_back(&submap); - } - - const auto index_map = ComputeTrajectoryIds(submap_pointers); - ASSERT_EQ(submaps.size(), index_map.size()); - for (const auto& kv : index_map) { - const auto pointer_iterator = - std::find(submap_pointers.begin(), submap_pointers.end(), kv.first); - ASSERT_TRUE(pointer_iterator != submap_pointers.end()); - const auto pointer_index = pointer_iterator - submap_pointers.begin(); - EXPECT_EQ(kv.second, pointer_index); - } - + const std::unordered_map index_map = {{&submaps[0], 0}, + {&submaps[1], 1}, + {&submaps[2], 2}, + {&submaps[3], 3}, + {&submaps[4], 4}}; std::vector> grouped_nodes; std::vector> new_indices; GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes, diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index e1abf70..d92d040 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -97,6 +97,7 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); + trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); const int j = trajectory_nodes_.size(); CHECK_LT(j, std::numeric_limits::max()); @@ -392,9 +393,16 @@ std::vector SparsePoseGraph::GetSubmapStates() { } std::vector SparsePoseGraph::constraints() { + common::MutexLocker locker(&mutex_); return constraints_; } +const std::unordered_map& +SparsePoseGraph::trajectory_ids() { + common::MutexLocker locker(&mutex_); + return trajectory_ids_; +} + transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps* const submaps) { const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index b852db7..84519d3 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -91,9 +91,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); - std::vector GetSubmapStates() override EXCLUDES(mutex_); - std::vector constraints() override; + protected: + std::vector GetSubmapStates() override EXCLUDES(mutex_); + std::vector constraints() override EXCLUDES(mutex_); + const std::unordered_map& trajectory_ids() + override EXCLUDES(mutex_); private: // Handles a new work item. @@ -168,7 +171,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); - std::vector constraints_; + std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an index and state as soon as they are seen, even // before they take part in the background computations. @@ -190,6 +193,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current submap transforms used for displaying data. std::vector optimized_submap_transforms_ GUARDED_BY(mutex_); + + // Map from submap pointers to trajectory IDs. + std::unordered_map trajectory_ids_ + GUARDED_BY(mutex_); }; } // namespace mapping_2d diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 2e0917c..074e7ae 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -395,9 +395,16 @@ SparsePoseGraph::GetSubmapStates() { } std::vector SparsePoseGraph::constraints() { + common::MutexLocker locker(&mutex_); return constraints_; } +const std::unordered_map& +SparsePoseGraph::trajectory_ids() { + common::MutexLocker locker(&mutex_); + return trajectory_ids_; +} + transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const mapping::Submaps* const submaps) { const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index ea61f79..86be198 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -93,8 +93,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { EXCLUDES(mutex_) override; std::vector GetTrajectoryNodes() override EXCLUDES(mutex_); - std::vector GetSubmapStates() override; - std::vector constraints() override; + + protected: + std::vector GetSubmapStates() EXCLUDES(mutex_) override; + std::vector constraints() EXCLUDES(mutex_) override; + const std::unordered_map& trajectory_ids() + EXCLUDES(mutex_) override; private: // This is 'mapping::SubmapState', but with the 3D versions of 'submap' and @@ -187,7 +191,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); - std::vector constraints_; + std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an index and state as soon as they are seen, even // before they take part in the background computations. @@ -209,6 +213,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Current submap transforms used for displaying data. std::vector optimized_submap_transforms_ GUARDED_BY(mutex_); + + // Map from submap pointers to trajectory IDs. + std::unordered_map trajectory_ids_ + GUARDED_BY(mutex_); }; } // namespace mapping_3d