Make TrajectoryConnectivity use trajectory IDs. (#275)

Related to #256.

cartographer/mapping/map_builder.cc

@@ -113,8 +113,7 @@ int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
 }
 
 proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
-  return ToProto(sparse_pose_graph_->GetConnectedTrajectories(),
+  return ToProto(sparse_pose_graph_->GetConnectedTrajectories());
-                 trajectory_ids_);
 }
 
 string MapBuilder::SubmapToProto(const int trajectory_id,

cartographer/mapping/sparse_pose_graph.h

@@ -58,7 +58,7 @@ class SparsePoseGraph {
       Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
     };
 
-    mapping::SubmapId submap_id; // 'i' in the paper.
+    mapping::SubmapId submap_id;  // 'i' in the paper.
 
     // Scan index.
     int j;
@@ -82,8 +82,7 @@ class SparsePoseGraph {
   virtual void RunFinalOptimization() = 0;
 
   // Get the current trajectory clusters.
-  virtual std::vector<std::vector<const Submaps*>>
+  virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
-  GetConnectedTrajectories() = 0;
 
   // Returns the current optimized transforms for the given 'trajectory'.
   virtual std::vector<transform::Rigid3d> GetSubmapTransforms(

cartographer/mapping/trajectory_connectivity.cc

@@ -28,35 +28,30 @@ namespace mapping {
 TrajectoryConnectivity::TrajectoryConnectivity()
     : lock_(), forest_(), connection_map_() {}
 
-void TrajectoryConnectivity::Add(const Submaps* trajectory) {
+void TrajectoryConnectivity::Add(const int trajectory_id) {
-  CHECK(trajectory != nullptr);
   common::MutexLocker locker(&lock_);
-  forest_.emplace(trajectory, trajectory);
+  forest_.emplace(trajectory_id, trajectory_id);
 }
 
-void TrajectoryConnectivity::Connect(const Submaps* trajectory_a,
+void TrajectoryConnectivity::Connect(const int trajectory_id_a,
-                                     const Submaps* trajectory_b) {
+                                     const int trajectory_id_b) {
-  CHECK(trajectory_a != nullptr);
-  CHECK(trajectory_b != nullptr);
   common::MutexLocker locker(&lock_);
-  Union(trajectory_a, trajectory_b);
+  Union(trajectory_id_a, trajectory_id_b);
-  auto sorted_pair =
+  auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
-      std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>());
   ++connection_map_[sorted_pair];
 }
 
-void TrajectoryConnectivity::Union(const Submaps* const trajectory_a,
+void TrajectoryConnectivity::Union(const int trajectory_id_a,
-                                   const Submaps* const trajectory_b) {
+                                   const int trajectory_id_b) {
-  forest_.emplace(trajectory_a, trajectory_a);
+  forest_.emplace(trajectory_id_a, trajectory_id_a);
-  forest_.emplace(trajectory_b, trajectory_b);
+  forest_.emplace(trajectory_id_b, trajectory_id_b);
-  const Submaps* const representative_a = FindSet(trajectory_a);
+  const int representative_a = FindSet(trajectory_id_a);
-  const Submaps* const representative_b = FindSet(trajectory_b);
+  const int representative_b = FindSet(trajectory_id_b);
   forest_[representative_a] = representative_b;
 }
 
-const Submaps* TrajectoryConnectivity::FindSet(
+int TrajectoryConnectivity::FindSet(const int trajectory_id) {
-    const Submaps* const trajectory) {
+  auto it = forest_.find(trajectory_id);
-  auto it = forest_.find(trajectory);
   CHECK(it != forest_.end());
   if (it->first != it->second) {
     it->second = FindSet(it->second);
@@ -64,28 +59,27 @@ const Submaps* TrajectoryConnectivity::FindSet(
   return it->second;
 }
 
-bool TrajectoryConnectivity::TransitivelyConnected(
+bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
-    const Submaps* trajectory_a, const Submaps* trajectory_b) {
+                                                   const int trajectory_id_b) {
-  CHECK(trajectory_a != nullptr);
-  CHECK(trajectory_b != nullptr);
   common::MutexLocker locker(&lock_);
 
-  if (forest_.count(trajectory_a) == 0 || forest_.count(trajectory_b) == 0) {
+  if (forest_.count(trajectory_id_a) == 0 ||
+      forest_.count(trajectory_id_b) == 0) {
     return false;
   }
-  return FindSet(trajectory_a) == FindSet(trajectory_b);
+  return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
 }
 
-std::vector<std::vector<const Submaps*>>
+std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
-TrajectoryConnectivity::ConnectedComponents() {
   // Map from cluster exemplar -> growing cluster.
-  std::unordered_map<const Submaps*, std::vector<const Submaps*>> map;
+  std::unordered_map<int, std::vector<int>> map;
   common::MutexLocker locker(&lock_);
-  for (const auto& trajectory_entry : forest_) {
+  for (const auto& trajectory_id_entry : forest_) {
-    map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first);
+    map[FindSet(trajectory_id_entry.first)].push_back(
+        trajectory_id_entry.first);
   }
 
-  std::vector<std::vector<const Submaps*>> result;
+  std::vector<std::vector<int>> result;
   result.reserve(map.size());
   for (auto& pair : map) {
     result.emplace_back(std::move(pair.second));
@@ -93,33 +87,23 @@ TrajectoryConnectivity::ConnectedComponents() {
   return result;
 }
 
-int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a,
+int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
-                                            const Submaps* trajectory_b) {
+                                            const int trajectory_id_b) {
-  CHECK(trajectory_a != nullptr);
-  CHECK(trajectory_b != nullptr);
   common::MutexLocker locker(&lock_);
-  const auto it = connection_map_.find(
+  const auto it =
-      std::minmax(trajectory_a, trajectory_b, std::less<const Submaps*>()));
+      connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
   return it != connection_map_.end() ? it->second : 0;
 }
 
 proto::TrajectoryConnectivity ToProto(
-    std::vector<std::vector<const Submaps*>> connected_components,
+    std::vector<std::vector<int>> connected_components) {
-    std::unordered_map<const mapping::Submaps*, int> trajectory_indices) {
   proto::TrajectoryConnectivity proto;
-  std::vector<std::vector<int>> connected_components_by_indices;
   for (const auto& connected_component : connected_components) {
-    connected_components_by_indices.emplace_back();
+    std::sort(connected_components.back().begin(),
-    for (const mapping::Submaps* trajectory : connected_component) {
+              connected_components.back().end());
-      connected_components_by_indices.back().push_back(
-          trajectory_indices.at(trajectory));
-    }
-    std::sort(connected_components_by_indices.back().begin(),
-              connected_components_by_indices.back().end());
   }
-  std::sort(connected_components_by_indices.begin(),
+  std::sort(connected_components.begin(), connected_components.end());
-            connected_components_by_indices.end());
+  for (const auto& connected_component : connected_components) {
-  for (const auto& connected_component : connected_components_by_indices) {
     auto* proto_connected_component = proto.add_connected_component();
     for (const int trajectory_id : connected_component) {
       proto_connected_component->add_trajectory_id(trajectory_id);

cartographer/mapping/trajectory_connectivity.h

@@ -41,50 +41,43 @@ class TrajectoryConnectivity {
   TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
 
   // Add a trajectory which is initially connected to nothing.
-  void Add(const Submaps* trajectory) EXCLUDES(lock_);
+  void Add(int trajectory_id) EXCLUDES(lock_);
 
   // Connect two trajectories. If either trajectory is untracked, it will be
   // tracked. This function is invariant to the order of its arguments. Repeated
   // calls to Connect increment the connectivity count.
-  void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b)
+  void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
-      EXCLUDES(lock_);
 
   // Determines if two trajectories have been (transitively) connected. If
   // either trajectory is not being tracked, returns false. This function is
   // invariant to the order of its arguments.
-  bool TransitivelyConnected(const Submaps* trajectory_a,
+  bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
-                             const Submaps* trajectory_b) EXCLUDES(lock_);
-
-  // Return the number of _direct_ connections between trajectory_a and
-  // trajectory_b. If either trajectory is not being tracked, returns 0. This
-  // function is invariant to the order of its arguments.
-  int ConnectionCount(const Submaps* trajectory_a, const Submaps* trajectory_b)
       EXCLUDES(lock_);
 
-  // The trajectories, grouped by connectivity.
+  // Return the number of _direct_ connections between 'trajectory_id_a' and
-  std::vector<std::vector<const Submaps*>> ConnectedComponents()
+  // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0.
-      EXCLUDES(lock_);
+  // This function is invariant to the order of its arguments.
+  int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
+
+  // The trajectory IDs, grouped by connectivity.
+  std::vector<std::vector<int>> ConnectedComponents() EXCLUDES(lock_);
 
  private:
   // Find the representative and compresses the path to it.
-  const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_);
+  int FindSet(int trajectory_id) REQUIRES(lock_);
-  void Union(const Submaps* trajectory_a, const Submaps* trajectory_b)
+  void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_);
-      REQUIRES(lock_);
 
   common::Mutex lock_;
   // Tracks transitive connectivity using a disjoint set forest, i.e. each
   // entry points towards the representative for the given trajectory.
-  std::map<const Submaps*, const Submaps*> forest_ GUARDED_BY(lock_);
+  std::map<int, int> forest_ GUARDED_BY(lock_);
   // Tracks the number of direct connections between a pair of trajectories.
-  std::map<std::pair<const Submaps*, const Submaps*>, int> connection_map_
+  std::map<std::pair<int, int>, int> connection_map_ GUARDED_BY(lock_);
-      GUARDED_BY(lock_);
 };
 
-// Returns a proto encoding connected components according to
+// Returns a proto encoding connected components.
-// 'trajectory_indices'.
 proto::TrajectoryConnectivity ToProto(
-    std::vector<std::vector<const Submaps*>> connected_components,
+    std::vector<std::vector<int>> connected_components);
-    std::unordered_map<const mapping::Submaps*, int> trajectory_indices);
 
 // Returns the connected component containing 'trajectory_index'.
 proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(

cartographer/mapping/trajectory_connectivity_test.cc

@@ -20,88 +20,60 @@
 #include <memory>
 #include <vector>
 
-#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
-#include "cartographer/mapping_2d/submaps.h"
 #include "gtest/gtest.h"
 
 namespace cartographer {
 namespace mapping {
 namespace {
 
-class TrajectoryConnectivityTest : public ::testing::Test {
+constexpr int kNumTrajectories = 10;
- protected:
-  TrajectoryConnectivityTest() {
-    for (int i = 0; i < 10; ++i) {
-      auto parameter_dictionary = common::MakeDictionary(R"text(
-          return {
-            resolution = 0.05,
-            half_length = 10.,
-            num_range_data = 10,
-            output_debug_images = false,
-            range_data_inserter = {
-              insert_free_space = true,
-              hit_probability = 0.53,
-              miss_probability = 0.495,
-            },
-          })text");
-      trajectories_.emplace_back(new mapping_2d::Submaps(
-          mapping_2d::CreateSubmapsOptions(parameter_dictionary.get())));
-    }
-  }
 
-  // Helper function to avoid .get() noise.
+TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
-  const Submaps* trajectory(int index) { return trajectories_.at(index).get(); }
+  TrajectoryConnectivity trajectory_connectivity;
 
-  TrajectoryConnectivity trajectory_connectivity_;
-  std::vector<std::unique_ptr<const Submaps>> trajectories_;
-};
-
-TEST_F(TrajectoryConnectivityTest, TransitivelyConnected) {
   // Make sure nothing's connected until we connect some things.
-  for (auto& trajectory_a : trajectories_) {
+  for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) {
-    for (auto& trajectory_b : trajectories_) {
+    for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) {
-      EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(
+      EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
-          trajectory_a.get(), trajectory_b.get()));
+                                                                 trajectory_b));
     }
   }
 
   // Connect some stuff up.
-  trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
+  trajectory_connectivity.Connect(0, 1);
-  EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
+  EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1));
-                                                             trajectory(1)));
+  trajectory_connectivity.Connect(8, 9);
-  trajectory_connectivity_.Connect(trajectory(8), trajectory(9));
+  EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9));
-  EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8),
+  EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9));
-                                                             trajectory(9)));
-  EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0),
-                                                              trajectory(9)));
 
-  trajectory_connectivity_.Connect(trajectory(1), trajectory(8));
+  trajectory_connectivity.Connect(1, 8);
   for (int i : {0, 1}) {
     for (int j : {8, 9}) {
-      EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(
+      EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j));
-          trajectory(i), trajectory(j)));
     }
   }
 }
 
-TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) {
+TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) {
-  auto connections = trajectory_connectivity_.ConnectedComponents();
+  TrajectoryConnectivity trajectory_connectivity;
+  auto connections = trajectory_connectivity.ConnectedComponents();
   EXPECT_EQ(0, connections.size());
 }
 
-TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
+TEST(TrajectoryConnectivityTest, ConnectedComponents) {
+  TrajectoryConnectivity trajectory_connectivity;
   for (int i = 0; i <= 4; ++i) {
-    trajectory_connectivity_.Connect(trajectory(0), trajectory(i));
+    trajectory_connectivity.Connect(0, i);
   }
   for (int i = 5; i <= 9; ++i) {
-    trajectory_connectivity_.Connect(trajectory(5), trajectory(i));
+    trajectory_connectivity.Connect(5, i);
   }
-  auto connections = trajectory_connectivity_.ConnectedComponents();
+  auto connections = trajectory_connectivity.ConnectedComponents();
   ASSERT_EQ(2, connections.size());
   // The clustering is arbitrary; we need to figure out which one is which.
-  const std::vector<const Submaps*>* zero_cluster = nullptr;
+  const std::vector<int>* zero_cluster = nullptr;
-  const std::vector<const Submaps*>* five_cluster = nullptr;
+  const std::vector<int>* five_cluster = nullptr;
-  if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) !=
+  if (std::find(connections[0].begin(), connections[0].end(), 0) !=
       connections[0].end()) {
     zero_cluster = &connections[0];
     five_cluster = &connections[1];
@@ -111,22 +83,21 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) {
   }
   for (int i = 0; i <= 9; ++i) {
     EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(),
-                                trajectory(i)) != zero_cluster->end());
+                                i) != zero_cluster->end());
-    EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(),
+    EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) !=
-                               trajectory(i)) != five_cluster->end());
+                         five_cluster->end());
   }
 }
 
-TEST_F(TrajectoryConnectivityTest, ConnectionCount) {
+TEST(TrajectoryConnectivityTest, ConnectionCount) {
-  for (int i = 0; i < 10; ++i) {
+  TrajectoryConnectivity trajectory_connectivity;
-    trajectory_connectivity_.Connect(trajectory(0), trajectory(1));
+  for (int i = 0; i < kNumTrajectories; ++i) {
+    trajectory_connectivity.Connect(0, 1);
     // Permute the arguments to check invariance.
-    EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1),
+    EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0));
-                                                              trajectory(0)));
   }
   for (int i = 1; i < 9; ++i) {
-    EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i),
+    EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1));
-                                                          trajectory(i + 1)));
   }
 }
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -99,6 +99,7 @@ void SparsePoseGraph::AddScan(
 
   common::MutexLocker locker(&mutex_);
   trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
+  const int trajectory_id = trajectory_ids_.at(trajectory);
   const int j = trajectory_nodes_.size();
   CHECK_LT(j, std::numeric_limits<int>::max());
 
@@ -109,16 +110,15 @@ void SparsePoseGraph::AddScan(
   trajectory_nodes_.push_back(mapping::TrajectoryNode{
       &constant_node_data_.back(), optimized_pose,
   });
-  trajectory_connectivity_.Add(trajectory);
+  trajectory_connectivity_.Add(trajectory_id);
 
   if (submap_indices_.count(insertion_submaps.back()) == 0) {
     submap_indices_.emplace(insertion_submaps.back(),
                             static_cast<int>(submap_indices_.size()));
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
-    submap_states_.back().trajectory = trajectory;
     submap_states_.back().id = mapping::SubmapId{
-        trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
+        trajectory_id, num_submaps_in_trajectory_[trajectory]};
     ++num_submaps_in_trajectory_[trajectory];
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
@@ -172,23 +172,24 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
 
   const mapping::Submaps* const scan_trajectory =
       trajectory_nodes_[scan_index].constant_data->trajectory;
-  const mapping::Submaps* const submap_trajectory =
+  const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
-      submap_states_[submap_index].trajectory;
+  const int submap_trajectory_id =
+      submap_states_[submap_index].id.trajectory_id;
 
   // Only globally match against submaps not in this trajectory.
-  if (scan_trajectory != submap_trajectory &&
+  if (scan_trajectory_id != submap_trajectory_id &&
       global_localization_samplers_[scan_trajectory]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id, submap_states_[submap_index].submap, scan_index,
-        scan_trajectory, submap_trajectory, &trajectory_connectivity_,
+        scan_trajectory_id, &trajectory_connectivity_,
         &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
   } else {
     const bool scan_and_submap_trajectories_connected =
-        reverse_connected_components_.count(scan_trajectory) > 0 &&
+        reverse_connected_components_.count(scan_trajectory_id) > 0 &&
-        reverse_connected_components_.count(submap_trajectory) > 0 &&
+        reverse_connected_components_.count(submap_trajectory_id) > 0 &&
-        reverse_connected_components_.at(scan_trajectory) ==
+        reverse_connected_components_.at(scan_trajectory_id) ==
-            reverse_connected_components_.at(submap_trajectory);
+            reverse_connected_components_.at(submap_trajectory_id);
-    if (scan_trajectory == submap_trajectory ||
+    if (scan_trajectory_id == submap_trajectory_id ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_states_[submap_index].submap, scan_index,
@@ -386,8 +387,8 @@ void SparsePoseGraph::RunOptimization() {
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
   reverse_connected_components_.clear();
   for (size_t i = 0; i != connected_components_.size(); ++i) {
-    for (const auto* trajectory : connected_components_[i]) {
+    for (const int trajectory_id : connected_components_[i]) {
-      reverse_connected_components_.emplace(trajectory, i);
+      reverse_connected_components_.emplace(trajectory_id, i);
     }
   }
 }
@@ -417,8 +418,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
              .inverse();
 }
 
-std::vector<std::vector<const mapping::Submaps*>>
+std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-SparsePoseGraph::GetConnectedTrajectories() {
   common::MutexLocker locker(&mutex_);
   return connected_components_;
 }
@@ -433,17 +433,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
         submap_transforms,
     const mapping::Submaps* const trajectory) const {
-  std::vector<transform::Rigid3d> result;
+  if (trajectory_ids_.count(trajectory) == 0) {
+    return {transform::Rigid3d::Identity()};
+  }
+  const int trajectory_id = trajectory_ids_.at(trajectory);
   size_t flat_index = 0;
   size_t flat_index_of_result_back = -1;
 
   // Submaps for which we have optimized poses.
+  std::vector<transform::Rigid3d> result;
   for (; flat_index != submap_states_.size(); ++flat_index) {
     const auto& state = submap_states_[flat_index];
-    if (state.trajectory != trajectory) {
+    if (state.id.trajectory_id != trajectory_id) {
       continue;
     }
-    const int trajectory_id = trajectory_ids_.at(trajectory);
     if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
         result.size() >= submap_transforms.at(trajectory_id).size()) {
       break;
@@ -456,7 +459,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
   // Extrapolate to the remaining submaps.
   for (; flat_index != submap_states_.size(); ++flat_index) {
     const auto& state = submap_states_[flat_index];
-    if (state.trajectory != trajectory) {
+    if (state.id.trajectory_id != trajectory_id) {
       continue;
     }
     if (result.empty()) {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -83,8 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                   const Eigen::Vector3d& angular_velocity);
 
   void RunFinalOptimization() override;
-  std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
+  std::vector<std::vector<int>> GetConnectedTrajectories() override;
-      override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(
       const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@@ -112,9 +111,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
     // which are finished.
     bool finished = false;
 
-    // The trajectory to which this SubmapState belongs.
-    const mapping::Submaps* trajectory = nullptr;
-
     mapping::SubmapId id;
   };
 
@@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
       GUARDED_BY(mutex_);
 
-  // Connectivity structure of our trajectories.
+  // Connectivity structure of our trajectories by IDs.
-  std::vector<std::vector<const mapping::Submaps*>> connected_components_;
+  std::vector<std::vector<int>> connected_components_;
-  // Trajectory to connected component ID.
+  // Trajectory ID to connected component ID.
-  std::map<const mapping::Submaps*, size_t> reverse_connected_components_;
+  std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
   //

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -78,8 +78,7 @@ void ConstraintBuilder::MaybeAddConstraint(
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, scan_index,
-                            nullptr, /* scan_trajectory */
+                            -1,      /* scan_trajectory_id */
-                            nullptr, /* submap_trajectory */
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
                             point_cloud, initial_relative_pose, constraint);
@@ -90,8 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const int scan_index, const mapping::Submaps* scan_trajectory,
+    const int scan_index, const int scan_trajectory_id,
-    const mapping::Submaps* submap_trajectory,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud) {
   common::MutexLocker locker(&mutex_);
@@ -102,8 +100,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
+        ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
-                          scan_trajectory, true, /* match_full_submap */
+                          true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid2d::Identity(), constraint);
         FinishComputation(current_computation);
@@ -168,8 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
 
 void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const int scan_index, const mapping::Submaps* scan_trajectory,
+    const int scan_index, const int scan_trajectory_id, bool match_full_submap,
-    const mapping::Submaps* submap_trajectory, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose,
@@ -194,7 +191,10 @@ void ConstraintBuilder::ComputeConstraint(
             filtered_point_cloud, options_.global_localization_min_score(),
             &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
-      trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
+      CHECK_GE(scan_trajectory_id, 0);
+      CHECK_GE(submap_id.trajectory_id, 0);
+      trajectory_connectivity->Connect(scan_trajectory_id,
+                                       submap_id.trajectory_id);
     } else {
       return;
     }

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -35,7 +35,7 @@
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
-#include "cartographer/mapping_2d/submaps.h"
+
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -86,16 +86,14 @@ class ConstraintBuilder {
   // 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
   // full-submap matching.
   //
-  // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
+  // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
-  // the 'submap' should be from 'submap_trajectory'. The
+  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
-  // 'trajectory_connectivity' is updated if the full-submap match succeeds.
   //
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const mapping::Submap* submap,
-      int scan_index, const mapping::Submaps* scan_trajectory,
+      int scan_index, int scan_trajectory_id,
-      const mapping::Submaps* submap_trajectory,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud);
 
@@ -134,13 +132,12 @@ class ConstraintBuilder {
   // Runs in a background thread and does computations for an additional
   // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
   // If 'match_full_submap' is true, and global localization succeeds, will
-  // connect 'scan_trajectory' and 'submap_trajectory' in
+  // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
       const mapping::SubmapId& submap_id, const mapping::Submap* submap,
-      int scan_index, const mapping::Submaps* scan_trajectory,
+      int scan_index, int scan_trajectory_id, bool match_full_submap,
-      const mapping::Submaps* submap_trajectory, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud,
       const transform::Rigid2d& initial_relative_pose,

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -97,6 +97,7 @@ void SparsePoseGraph::AddScan(
 
   common::MutexLocker locker(&mutex_);
   trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
+  const int trajectory_id = trajectory_ids_.at(trajectory);
   const int j = trajectory_nodes_.size();
   CHECK_LT(j, std::numeric_limits<int>::max());
 
@@ -106,16 +107,15 @@ void SparsePoseGraph::AddScan(
       transform::Rigid3d::Identity()});
   trajectory_nodes_.push_back(
       mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
-  trajectory_connectivity_.Add(trajectory);
+  trajectory_connectivity_.Add(trajectory_id);
 
   if (submap_indices_.count(insertion_submaps.back()) == 0) {
     submap_indices_.emplace(insertion_submaps.back(),
                             static_cast<int>(submap_indices_.size()));
     submap_states_.emplace_back();
     submap_states_.back().submap = insertion_submaps.back();
-    submap_states_.back().trajectory = trajectory;
     submap_states_.back().id = mapping::SubmapId{
-        trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
+        trajectory_id, num_submaps_in_trajectory_[trajectory]};
     ++num_submaps_in_trajectory_[trajectory];
     CHECK_EQ(submap_states_.size(), submap_indices_.size());
   }
@@ -172,23 +172,23 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
 
   const mapping::Submaps* const scan_trajectory =
       trajectory_nodes_[scan_index].constant_data->trajectory;
-  const mapping::Submaps* const submap_trajectory =
+  const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory);
-      submap_states_[submap_index].trajectory;
+  const int submap_trajectory_id =
+      submap_states_[submap_index].id.trajectory_id;
 
   // Only globally match against submaps not in this trajectory.
-  if (scan_trajectory != submap_trajectory &&
+  if (scan_trajectory_id != submap_trajectory_id &&
       global_localization_samplers_[scan_trajectory]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id, submap_states_[submap_index].submap, scan_index,
-        scan_trajectory, submap_trajectory, &trajectory_connectivity_,
+        scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);
-        trajectory_nodes_);
   } else {
     const bool scan_and_submap_trajectories_connected =
-        reverse_connected_components_.count(scan_trajectory) > 0 &&
+        reverse_connected_components_.count(scan_trajectory_id) > 0 &&
-        reverse_connected_components_.count(submap_trajectory) > 0 &&
+        reverse_connected_components_.count(submap_trajectory_id) > 0 &&
-        reverse_connected_components_.at(scan_trajectory) ==
+        reverse_connected_components_.at(scan_trajectory_id) ==
-            reverse_connected_components_.at(submap_trajectory);
+            reverse_connected_components_.at(submap_trajectory_id);
-    if (scan_trajectory == submap_trajectory ||
+    if (scan_trajectory_id == submap_trajectory_id ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_states_[submap_index].submap, scan_index,
@@ -379,8 +379,8 @@ void SparsePoseGraph::RunOptimization() {
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
   reverse_connected_components_.clear();
   for (size_t i = 0; i != connected_components_.size(); ++i) {
-    for (const auto* trajectory : connected_components_[i]) {
+    for (const int trajectory_id : connected_components_[i]) {
-      reverse_connected_components_.emplace(trajectory, i);
+      reverse_connected_components_.emplace(trajectory_id, i);
     }
   }
 }
@@ -410,8 +410,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
              .inverse();
 }
 
-std::vector<std::vector<const mapping::Submaps*>>
+std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-SparsePoseGraph::GetConnectedTrajectories() {
   common::MutexLocker locker(&mutex_);
   return connected_components_;
 }
@@ -426,17 +425,20 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
         submap_transforms,
     const mapping::Submaps* const trajectory) const {
-  std::vector<transform::Rigid3d> result;
+  if (trajectory_ids_.count(trajectory) == 0) {
+    return {transform::Rigid3d::Identity()};
+  }
+  const int trajectory_id = trajectory_ids_.at(trajectory);
   size_t flat_index = 0;
   size_t flat_index_of_result_back = -1;
 
   // Submaps for which we have optimized poses.
+  std::vector<transform::Rigid3d> result;
   for (; flat_index != submap_states_.size(); ++flat_index) {
     const auto& state = submap_states_[flat_index];
-    if (state.trajectory != trajectory) {
+    if (state.id.trajectory_id != trajectory_id) {
       continue;
     }
-    const int trajectory_id = trajectory_ids_.at(trajectory);
     if (static_cast<size_t>(trajectory_id) >= submap_transforms.size() ||
         result.size() >= submap_transforms.at(trajectory_id).size()) {
       break;
@@ -449,7 +451,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
   // Extrapolate to the remaining submaps.
   for (; flat_index != submap_states_.size(); ++flat_index) {
     const auto& state = submap_states_[flat_index];
-    if (state.trajectory != trajectory) {
+    if (state.id.trajectory_id != trajectory_id) {
       continue;
     }
     if (result.empty()) {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -85,8 +85,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                   const Eigen::Vector3d& angular_velocity);
 
   void RunFinalOptimization() override;
-  std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
+  std::vector<std::vector<int>> GetConnectedTrajectories() override;
-      override;
   std::vector<transform::Rigid3d> GetSubmapTransforms(
       const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
   transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
@@ -114,9 +113,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
     // which are finished.
     bool finished = false;
 
-    // The trajectory to which this SubmapState belongs.
-    const Submaps* trajectory = nullptr;
-
     mapping::SubmapId id;
   };
 
@@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
       GUARDED_BY(mutex_);
 
-  // Connectivity structure of our trajectories.
+  // Connectivity structure of our trajectories by IDs.
-  std::vector<std::vector<const mapping::Submaps*>> connected_components_;
+  std::vector<std::vector<int>> connected_components_;
-  // Trajectory to connected component ID.
+  // Trajectory ID to connected component ID.
-  std::map<const mapping::Submaps*, size_t> reverse_connected_components_;
+  std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
   //

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -100,8 +100,7 @@ void ConstraintBuilder::MaybeAddConstraint(
         submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
         [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, scan_index,
-                            nullptr, /* scan_trajectory */
+                            -1,      /* scan_trajectory_id */
-                            nullptr, /* submap_trajectory */
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
                             point_cloud, initial_relative_pose, constraint);
@@ -112,8 +111,7 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const int scan_index, const mapping::Submaps* scan_trajectory,
+    const int scan_index, const int scan_trajectory_id,
-    const mapping::Submaps* submap_trajectory,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
   const auto submap_nodes = ComputeSubmapNodes(
@@ -129,8 +127,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
       [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
+        ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
-                          scan_trajectory, true, /* match_full_submap */
+                          true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid3d::Identity(), constraint);
         FinishComputation(current_computation);
@@ -200,8 +198,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
 
 void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const int scan_index, const mapping::Submaps* scan_trajectory,
+    const int scan_index, const int scan_trajectory_id, bool match_full_submap,
-    const mapping::Submaps* submap_trajectory, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
     const transform::Rigid3d& initial_relative_pose,
@@ -227,7 +224,10 @@ void ConstraintBuilder::ComputeConstraint(
             initial_pose.rotation(), filtered_point_cloud, point_cloud,
             options_.global_localization_min_score(), &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
-      trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
+      CHECK_GE(scan_trajectory_id, 0);
+      CHECK_GE(submap_id.trajectory_id, 0);
+      trajectory_connectivity->Connect(scan_trajectory_id,
+                                       submap_id.trajectory_id);
     } else {
       return;
     }

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -82,16 +82,14 @@ class ConstraintBuilder {
   // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
   // 'scan_index'. This performs full-submap matching.
   //
-  // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
+  // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
-  // the 'submap' should be from 'submap_trajectory'. The
+  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
-  // 'trajectory_connectivity' is updated if the full-submap match succeeds.
   //
   // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
   // all computations are finished.
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
-      const mapping::Submaps* scan_trajectory,
+      int scan_trajectory_id,
-      const mapping::Submaps* submap_trajectory,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
 
@@ -133,13 +131,12 @@ class ConstraintBuilder {
   // Runs in a background thread and does computations for an additional
   // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
   // If 'match_full_submap' is true, and global localization succeeds, will
-  // connect 'scan_trajectory' and 'submap_trajectory' in
+  // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
-      const mapping::Submaps* scan_trajectory,
+      int scan_trajectory_id, bool match_full_submap,
-      const mapping::Submaps* submap_trajectory, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::CompressedPointCloud* const compressed_point_cloud,
       const transform::Rigid3d& initial_relative_pose,