Introduce TrajectoryConnectivityState. (#513)

* Introduce TrajectoryConnectivityState.

This class will be used to track the connectivity state (including the
last connection time) between pairs of trajectories.

cartographer/mapping/connected_components.cc

@@ -60,7 +60,7 @@ int ConnectedComponents::FindSet(const int trajectory_id) {
 }
 
 bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
-                                                   const int trajectory_id_b) {
+                                                const int trajectory_id_b) {
   if (trajectory_id_a == trajectory_id_b) {
     return true;
   }
@@ -91,8 +91,19 @@ std::vector<std::vector<int>> ConnectedComponents::Components() {
   return result;
 }
 
+std::vector<int> ConnectedComponents::GetComponent(const int trajectory_id) {
+  const int set_id = FindSet(trajectory_id);
+  std::vector<int> trajectory_ids;
+  for (const auto& entry : forest_) {
+    if (FindSet(entry.first) == set_id) {
+      trajectory_ids.push_back(entry.first);
+    }
+  }
+  return trajectory_ids;
+}
+
 int ConnectedComponents::ConnectionCount(const int trajectory_id_a,
-                                            const int trajectory_id_b) {
+                                         const int trajectory_id_b) {
   common::MutexLocker locker(&lock_);
   const auto it =
       connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));

cartographer/mapping/connected_components.h

@@ -63,6 +63,10 @@ class ConnectedComponents {
   // The trajectory IDs, grouped by connectivity.
   std::vector<std::vector<int>> Components() EXCLUDES(lock_);
 
+  // The list of trajectory IDs that belong to the same connected component as
+  // 'trajectory_id'.
+  std::vector<int> GetComponent(int trajectory_id) EXCLUDES(lock_);
+
  private:
   // Find the representative and compresses the path to it.
   int FindSet(int trajectory_id) REQUIRES(lock_);

cartographer/mapping/trajectory_connectivity_state.cc

@@ -0,0 +1,74 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/trajectory_connectivity_state.h"
+
+namespace cartographer {
+namespace mapping {
+
+void TrajectoryConnectivityState::Add(const int trajectory_id) {
+  connected_components_.Add(trajectory_id);
+}
+
+void TrajectoryConnectivityState::Connect(const int trajectory_id_a,
+                                          const int trajectory_id_b,
+                                          const common::Time time) {
+  if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) {
+    // The trajectories are transitively connected, i.e. they belong to the same
+    // connected component. In this case we only update the last connection time
+    // of those two trajectories.
+    auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
+    if (last_connection_time_map_[sorted_pair] < time) {
+      last_connection_time_map_[sorted_pair] = time;
+    }
+  } else {
+    // The connection between these two trajectories is about to join to
+    // connected components. Here we update all bipartite trajectory pairs for
+    // the two connected components with the connection time. This is to quickly
+    // change to a more efficient loop closure search (by constraining the
+    // search window) when connected components are joined.
+    std::vector<int> component_a = connected_components_.GetComponent(trajectory_id_a);
+    std::vector<int> component_b = connected_components_.GetComponent(trajectory_id_b);
+    for (const auto id_a : component_a) {
+      for (const auto id_b : component_b) {
+        auto id_pair = std::minmax(id_a, id_b);
+        last_connection_time_map_[id_pair] = time;
+      }
+    }
+  }
+  connected_components_.Connect(trajectory_id_a, trajectory_id_b);
+}
+
+bool TrajectoryConnectivityState::TransitivelyConnected(
+    const int trajectory_id_a,
+    const int trajectory_id_b) {
+  return connected_components_.TransitivelyConnected(trajectory_id_a,
+                                                     trajectory_id_b);
+}
+
+std::vector<std::vector<int>> TrajectoryConnectivityState::Components() {
+  return connected_components_.Components();
+}
+
+common::Time TrajectoryConnectivityState::LastConnectionTime(
+    const int trajectory_id_a,
+    const int trajectory_id_b) {
+  auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
+  return last_connection_time_map_[sorted_pair];
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/trajectory_connectivity_state.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_
+#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_
+
+#include "cartographer/common/time.h"
+#include "cartographer/mapping/connected_components.h"
+
+namespace cartographer {
+namespace mapping {
+
+// A class that tracks the connectivity state between trajectories. Compared to
+// ConnectedComponents it tracks additionally the last time that a global
+// constraint connected to trajectories.
+//
+// This class is thread-compatible.
+class TrajectoryConnectivityState {
+ public:
+  TrajectoryConnectivityState() {}
+
+  TrajectoryConnectivityState(const TrajectoryConnectivityState&) = delete;
+  TrajectoryConnectivityState& operator=(const TrajectoryConnectivityState&) =
+      delete;
+
+  // Add a trajectory which is initially connected to only itself.
+  void Add(int trajectory_id);
+
+  // 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 and update the last
+  // connected time.
+  void Connect(int trajectory_id_a,
+               int trajectory_id_b,
+               common::Time time);
+
+  // Determines if two trajectories have been (transitively) connected. If
+  // either trajectory is not being tracked, returns false, except when it is
+  // 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);
+
+  // The trajectory IDs, grouped by connectivity.
+  std::vector<std::vector<int>> Components();
+
+  // Return the last connection count between the two trajectories. If either of
+  // the trajectories is untracked or they have never been connected returns the
+  // beginning of time.
+  common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b);
+
+ private:
+  ConnectedComponents connected_components_;
+
+  // Tracks the last time a direct connection between two trajectories has
+  // been added. The exception is when a connection between two trajectories
+  // connects two formerly unconnected connected components. In this case all
+  // bipartite trajectories entries for these components are updated with the
+  // new connection time.
+  std::map<std::pair<int, int>, common::Time> last_connection_time_map_;
+
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_

cartographer/mapping/trajectory_connectivity_state_test.cc

@@ -0,0 +1,91 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/trajectory_connectivity_state.h"
+
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+
+TEST(TrajectoryConnectivityStateTest, UnknownTrajectory) {
+  TrajectoryConnectivityState state;
+  state.Add(0);
+
+  // Nothing is transitively connected to an unknown trajectory.
+  EXPECT_FALSE(state.TransitivelyConnected(0, 1));
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time());
+}
+
+TEST(TrajectoryConnectivityStateTest, NotConnected) {
+  TrajectoryConnectivityState state;
+  state.Add(0);
+  state.Add(1);
+  EXPECT_FALSE(state.TransitivelyConnected(0, 1));
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time());
+}
+
+TEST(TrajectoryConnectivityStateTest, Connected) {
+  TrajectoryConnectivityState state;
+  state.Add(0);
+  state.Add(1);
+  state.Connect(0, 1, common::FromUniversal(123456));
+  EXPECT_TRUE(state.TransitivelyConnected(0, 1));
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456));
+}
+
+TEST(TrajectoryConnectivityStateTest, UpdateConnectionTime) {
+  TrajectoryConnectivityState state;
+  state.Add(0);
+  state.Add(1);
+  state.Connect(0, 1, common::FromUniversal(123456));
+  state.Connect(0, 1, common::FromUniversal(234567));
+  EXPECT_TRUE(state.TransitivelyConnected(0, 1));
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567));
+
+  // Connections with an earlier connection time do not update the last
+  // connection time.
+  state.Connect(0, 1, common::FromUniversal(123456));
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567));
+}
+
+TEST(TrajectoryConnectivityStateTest, JoinTwoComponents) {
+  TrajectoryConnectivityState state;
+  state.Add(0);
+  state.Add(1);
+  state.Add(2);
+  state.Add(3);
+  state.Connect(0, 1, common::FromUniversal(123456));
+  state.Connect(2, 3, common::FromUniversal(123456));
+
+  // Connect the two disjoint connected components.
+  state.Connect(0, 2, common::FromUniversal(234567));
+  EXPECT_TRUE(state.TransitivelyConnected(0, 2));
+  EXPECT_TRUE(state.TransitivelyConnected(1, 3));
+
+  // All bipartite trajectory pairs between the two connected components should
+  // have the updated connection time.
+  EXPECT_EQ(state.LastConnectionTime(0, 2), common::FromUniversal(234567));
+  EXPECT_EQ(state.LastConnectionTime(0, 3), common::FromUniversal(234567));
+  EXPECT_EQ(state.LastConnectionTime(1, 3), common::FromUniversal(234567));
+
+  // A pair of trajectory IDs belonging to the same connected component should
+  // be unaffected.
+  EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456));
+}
+
+}  // namespace cartographer
+}  // namespace mapping

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan(
   trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
   ++num_trajectory_nodes_;
-  connected_components_.Add(trajectory_id);
+  trajectory_connectivity_state_.Add(trajectory_id);
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
   if (trajectory_id >= submap_data_.num_trajectories() ||
@@ -180,7 +180,7 @@ 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 {
-    if (connected_components_.TransitivelyConnected(
+    if (trajectory_connectivity_state_.TransitivelyConnected(
             node_id.trajectory_id, submap_id.trajectory_id)) {
       const transform::Rigid2d initial_relative_pose =
           optimization_problem_.submap_data()
@@ -300,6 +300,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   }
 }
 
+void SparsePoseGraph::UpdateTrajectoryConnectivity(
+    const sparse_pose_graph::ConstraintBuilder::Result& result) {
+  for (const Constraint& constraint : result) {
+    CHECK_EQ(constraint.tag,
+             mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
+    mapping::NodeId last_submap_node_id =
+        *submap_data_.at(constraint.submap_id).node_ids.rbegin();
+    common::Time time =
+        std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
+                 trajectory_nodes_.at(last_submap_node_id).constant_data->time);
+    trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
+                                           constraint.submap_id.trajectory_id,
+                                           time);
+  }
+}
+
 void SparsePoseGraph::HandleWorkQueue() {
   constraint_builder_.WhenDone(
       [this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
@@ -308,15 +324,7 @@ void SparsePoseGraph::HandleWorkQueue() {
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
         RunOptimization();
-
+        UpdateTrajectoryConnectivity(result);
-        // Update the trajectory connectivity structure with the new
-        // constraints.
-        for (const Constraint& constraint : result) {
-          CHECK_EQ(constraint.tag,
-                   mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
-          connected_components_.Connect(constraint.node_id.trajectory_id,
-                                           constraint.submap_id.trajectory_id);
-        }
 
         common::MutexLocker locker(&mutex_);
         num_scans_since_last_loop_closure_ = 0;
@@ -503,7 +511,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-  return connected_components_.Components();
+  return trajectory_connectivity_state_.Components();
 }
 
 int SparsePoseGraph::num_submaps(const int trajectory_id) {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -32,9 +32,9 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/connected_components.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
+#include "cartographer/mapping/trajectory_connectivity_state.h"
 #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
@@ -153,6 +153,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);
 
+  // Updates the trajectory connectivity structure with the new constraints.
+  void UpdateTrajectoryConnectivity(
+      const sparse_pose_graph::ConstraintBuilder::Result& result);
+
   // Computes the local to global frame transform based on the given optimized
   // 'submap_transforms'.
   transform::Rigid3d ComputeLocalToGlobalTransform(
@@ -172,7 +176,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       GUARDED_BY(mutex_);
 
   // How our various trajectories are related.
-  mapping::ConnectedComponents connected_components_;
+  mapping::TrajectoryConnectivityState trajectory_connectivity_state_;
 
   // We globally localize a fraction of the scans from each trajectory.
   std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan(
   trajectory_nodes_.Append(
       trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
   ++num_trajectory_nodes_;
-  connected_components_.Add(trajectory_id);
+  trajectory_connectivity_state_.Add(trajectory_id);
 
   // Test if the 'insertion_submap.back()' is one we never saw before.
   if (trajectory_id >= submap_data_.num_trajectories() ||
@@ -213,7 +213,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
         initial_relative_pose.rotation());
   } else {
-    if (connected_components_.TransitivelyConnected(
+    if (trajectory_connectivity_state_.TransitivelyConnected(
             node_id.trajectory_id, submap_id.trajectory_id)) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@@ -320,6 +320,22 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   }
 }
 
+void SparsePoseGraph::UpdateTrajectoryConnectivity(
+    const sparse_pose_graph::ConstraintBuilder::Result& result) {
+  for (const Constraint& constraint : result) {
+    CHECK_EQ(constraint.tag,
+             mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
+    const mapping::NodeId last_submap_node_id =
+        *submap_data_.at(constraint.submap_id).node_ids.rbegin();
+    const common::Time time =
+        std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
+                 trajectory_nodes_.at(last_submap_node_id).constant_data->time);
+    trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
+                                           constraint.submap_id.trajectory_id,
+                                           time);
+  }
+}
+
 void SparsePoseGraph::HandleWorkQueue() {
   constraint_builder_.WhenDone(
       [this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
@@ -328,15 +344,7 @@ void SparsePoseGraph::HandleWorkQueue() {
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
         RunOptimization();
-
+        UpdateTrajectoryConnectivity(result);
-        // Update the trajectory connectivity structure with the new
-        // constraints.
-        for (const Constraint& constraint : result) {
-          CHECK_EQ(constraint.tag,
-                   mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
-          connected_components_.Connect(constraint.node_id.trajectory_id,
-                                           constraint.submap_id.trajectory_id);
-        }
 
         common::MutexLocker locker(&mutex_);
         num_scans_since_last_loop_closure_ = 0;
@@ -542,7 +550,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-  return connected_components_.Components();
+  return trajectory_connectivity_state_.Components();
 }
 
 int SparsePoseGraph::num_submaps(const int trajectory_id) {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -32,9 +32,9 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
-#include "cartographer/mapping/connected_components.h"
 #include "cartographer/mapping/pose_graph_trimmer.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
+#include "cartographer/mapping/trajectory_connectivity_state.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
 #include "cartographer/mapping_3d/submaps.h"
@@ -168,6 +168,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // poses.
   void LogResidualHistograms() REQUIRES(mutex_);
 
+  // Updates the trajectory connectivity structure with the new constraints.
+  void UpdateTrajectoryConnectivity(
+      const sparse_pose_graph::ConstraintBuilder::Result& result);
+
   const mapping::proto::SparsePoseGraphOptions options_;
   common::Mutex mutex_;
 
@@ -177,7 +181,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       GUARDED_BY(mutex_);
 
   // How our various trajectories are related.
-  mapping::ConnectedComponents connected_components_;
+  mapping::TrajectoryConnectivityState trajectory_connectivity_state_;
 
   // We globally localize a fraction of the scans from each trajectory.
   std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>