Remove reverse_connected_components. (#507)

Remove reverse_connected_components from SparsePoseGraphs and update
TrajectoryConnectivity to return "connected" for the reflexive case even
if trajectories are unknown.

cartographer/mapping/trajectory_connectivity.cc

@@ -61,6 +61,10 @@ int TrajectoryConnectivity::FindSet(const int trajectory_id) {
 
 bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
                                                    const int trajectory_id_b) {
+  if (trajectory_id_a == trajectory_id_b) {
+    return true;
+  }
+
   common::MutexLocker locker(&lock_);
 
   if (forest_.count(trajectory_id_a) == 0 ||

cartographer/mapping/trajectory_connectivity.h

@@ -40,7 +40,7 @@ class TrajectoryConnectivity {
   TrajectoryConnectivity(const TrajectoryConnectivity&) = delete;
   TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
 
-  // Add a trajectory which is initially connected to nothing.
+  // Add a trajectory which is initially connected to only itself.
   void Add(int trajectory_id) EXCLUDES(lock_);
 
   // Connect two trajectories. If either trajectory is untracked, it will be
@@ -49,8 +49,9 @@ class TrajectoryConnectivity {
   void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
 
   // Determines if two trajectories have been (transitively) connected. If
-  // either trajectory is not being tracked, returns false. This function is
+  // either trajectory is not being tracked, returns false, except when it is
-  // invariant to the order of its arguments.
+  // the same trajectory, where it returns true. This function is invariant to
+  // the order of its arguments.
   bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
       EXCLUDES(lock_);
 

cartographer/mapping/trajectory_connectivity_test.cc

@@ -35,8 +35,9 @@ TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
   for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) {
     for (int trajectory_b = 0; trajectory_b < kNumTrajectories;
          ++trajectory_b) {
-      EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
+      EXPECT_EQ(trajectory_a == trajectory_b,
-                                                                 trajectory_b));
+                trajectory_connectivity.TransitivelyConnected(trajectory_a,
+                                                              trajectory_b));
     }
   }
 
@@ -102,6 +103,15 @@ TEST(TrajectoryConnectivityTest, ConnectionCount) {
   }
 }
 
+TEST(TrajectoryConnectivityTest, ReflexiveConnectivity) {
+  TrajectoryConnectivity trajectory_connectivity;
+  EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0));
+  EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0));
+  trajectory_connectivity.Add(0);
+  EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0));
+  EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0));
+}
+
 }  // namespace
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -180,13 +180,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
         trajectory_nodes_.at(node_id).constant_data.get());
   } else {
-    const bool scan_and_submap_trajectories_connected =
+    if (trajectory_connectivity_.TransitivelyConnected(
-        reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
+            node_id.trajectory_id, submap_id.trajectory_id)) {
-        reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
-        reverse_connected_components_.at(node_id.trajectory_id) ==
-            reverse_connected_components_.at(submap_id.trajectory_id);
-    if (node_id.trajectory_id == submap_id.trajectory_id ||
-        scan_and_submap_trajectories_connected) {
       const transform::Rigid2d initial_relative_pose =
           optimization_problem_.submap_data()
               .at(submap_id.trajectory_id)
@@ -471,12 +466,6 @@ void SparsePoseGraph::RunOptimization() {
   }
   optimized_submap_transforms_ = submap_data;
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
-  reverse_connected_components_.clear();
-  for (size_t i = 0; i != connected_components_.size(); ++i) {
-    for (const int trajectory_id : connected_components_[i]) {
-      reverse_connected_components_.emplace(trajectory_id, i);
-    }
-  }
 
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -196,8 +196,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Connectivity structure of our trajectories by IDs.
   std::vector<std::vector<int>> connected_components_;
-  // Trajectory ID to connected component ID.
-  std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -212,13 +212,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
         initial_relative_pose.rotation());
   } else {
-    const bool scan_and_submap_trajectories_connected =
+    if (trajectory_connectivity_.TransitivelyConnected(
-        reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
+            node_id.trajectory_id, submap_id.trajectory_id)) {
-        reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
-        reverse_connected_components_.at(node_id.trajectory_id) ==
-            reverse_connected_components_.at(submap_id.trajectory_id);
-    if (node_id.trajectory_id == submap_id.trajectory_id ||
-        scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
           trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
@@ -509,12 +504,6 @@ void SparsePoseGraph::RunOptimization() {
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
-  reverse_connected_components_.clear();
-  for (size_t i = 0; i != connected_components_.size(); ++i) {
-    for (const int trajectory_id : connected_components_[i]) {
-      reverse_connected_components_.emplace(trajectory_id, i);
-    }
-  }
 
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -201,8 +201,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Connectivity structure of our trajectories by IDs.
   std::vector<std::vector<int>> connected_components_;
-  // Trajectory ID to connected component ID.
-  std::map<int, size_t> reverse_connected_components_;
 
   // Data that are currently being shown.
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>