Move trajectory id mapping into the SparsePoseGraph. (#270)

cartographer/mapping/sparse_pose_graph.cc

@@ -36,15 +36,6 @@ proto::SparsePoseGraph::Constraint::Tag ToProto(
   LOG(FATAL) << "Unsupported tag.";
 }
 
-std::unordered_map<const Submaps*, int> ComputeTrajectoryIds(
-    const std::vector<const Submaps*>& trajectories) {
-  std::unordered_map<const Submaps*, int> result;
-  for (const auto& trajectory : trajectories) {
-    result.emplace(trajectory, result.size());
-  }
-  return result;
-}
-
 void GroupTrajectoryNodes(
     const std::vector<TrajectoryNode>& trajectory_nodes,
     const std::unordered_map<const Submaps*, int>& trajectory_ids,
@@ -100,21 +91,14 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
 
-  const auto trajectory_nodes = GetTrajectoryNodes();
-  std::vector<const Submaps*> 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<std::vector<TrajectoryNode>> grouped_nodes;
   std::vector<std::pair<int, int>> grouped_node_indices;
-  GroupTrajectoryNodes(trajectory_nodes, trajectory_ids, &grouped_nodes,
+  GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
                        &grouped_node_indices);
 
   std::vector<std::pair<int, int>> 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();

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<const Submaps*, int> ComputeTrajectoryIds(
-    const std::vector<const Submaps*>& 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<Constraint> constraints() = 0;
+
+  // Returns the mapping from Submaps* to trajectory IDs.
+  virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
 };
 
 // Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be

cartographer/mapping/sparse_pose_graph_test.cc

@@ -56,21 +56,11 @@ TEST(SparsePoseGraphTest, TrajectoryFunctions) {
     }
   }
 
-  std::vector<const Submaps*> submap_pointers;
+  const std::unordered_map<const Submaps*, int> index_map = {{&submaps[0], 0},
-  for (const auto& submap : submaps) {
+                                                             {&submaps[1], 1},
-    submap_pointers.push_back(&submap);
+                                                             {&submaps[2], 2},
-  }
+                                                             {&submaps[3], 3},
-
+                                                             {&submaps[4], 4}};
-  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);
-  }
-
   std::vector<std::vector<TrajectoryNode>> grouped_nodes;
   std::vector<std::pair<int, int>> new_indices;
   GroupTrajectoryNodes(trajectory_nodes, index_map, &grouped_nodes,

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<int>::max());
 
@@ -392,9 +393,16 @@ std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
+  common::MutexLocker locker(&mutex_);
   return constraints_;
 }
 
+const std::unordered_map<const mapping::Submaps*, int>&
+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);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -91,9 +91,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
-  std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
 
-  std::vector<Constraint> constraints() override;
+ protected:
+  std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
+  std::vector<Constraint> constraints() override EXCLUDES(mutex_);
+  const std::unordered_map<const mapping::Submaps*, int>& 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<Constraint> constraints_;
+  std::vector<Constraint> 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<transform::Rigid2d> optimized_submap_transforms_
       GUARDED_BY(mutex_);
+
+  // Map from submap pointers to trajectory IDs.
+  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
+      GUARDED_BY(mutex_);
 };
 
 }  // namespace mapping_2d

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -395,9 +395,16 @@ SparsePoseGraph::GetSubmapStates() {
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
+  common::MutexLocker locker(&mutex_);
   return constraints_;
 }
 
+const std::unordered_map<const mapping::Submaps*, int>&
+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);

cartographer/mapping_3d/sparse_pose_graph.h

@@ -93,8 +93,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<mapping::TrajectoryNode> GetTrajectoryNodes() override
       EXCLUDES(mutex_);
-  std::vector<SubmapState> GetSubmapStates() override;
+
-  std::vector<Constraint> constraints() override;
+ protected:
+  std::vector<SubmapState> GetSubmapStates() EXCLUDES(mutex_) override;
+  std::vector<Constraint> constraints() EXCLUDES(mutex_) override;
+  const std::unordered_map<const mapping::Submaps*, int>& 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<Constraint> constraints_;
+  std::vector<Constraint> 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<transform::Rigid3d> optimized_submap_transforms_
       GUARDED_BY(mutex_);
+
+  // Map from submap pointers to trajectory IDs.
+  std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
+      GUARDED_BY(mutex_);
 };
 
 }  // namespace mapping_3d