Handle multiple frozen trajectories as one connected component. (#1610)

This fixes local constraint search in frozen maps built from multiple trajectories.

Local constraint search from a localization trajectory to a set of frozen trajectories
(i.e. the map) is done only among trajectories that are transitively connected in the
same connected component. If we set an initial pose, we create such a connection
to one frozen trajectory, but we were so far not able to do a local constraint search
w.r.t. to other frozen trajectories because they're not connected among each other.
Any constraints between them are not loaded (because they’re frozen).

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -649,6 +649,22 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
   AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
     absl::MutexLock locker(&mutex_);
     CHECK(!IsTrajectoryFrozen(trajectory_id));
+    // Connect multiple frozen trajectories among each other.
+    // This is required for localization against multiple frozen trajectories
+    // because we lose inter-trajectory constraints when freezing.
+    for (const auto& entry : data_.trajectories_state) {
+      const int other_trajectory_id = entry.first;
+      if (!IsTrajectoryFrozen(other_trajectory_id)) {
+        continue;
+      }
+      if (data_.trajectory_connectivity_state.TransitivelyConnected(
+              trajectory_id, other_trajectory_id)) {
+        // Already connected, nothing to do.
+        continue;
+      }
+      data_.trajectory_connectivity_state.Connect(
+          trajectory_id, other_trajectory_id, common::FromUniversal(0));
+    }
     data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
     return WorkItem::Result::kDoNotRunOptimization;
   });

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -643,6 +643,22 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
   AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
     absl::MutexLock locker(&mutex_);
     CHECK(!IsTrajectoryFrozen(trajectory_id));
+    // Connect multiple frozen trajectories among each other.
+    // This is required for localization against multiple frozen trajectories
+    // because we lose inter-trajectory constraints when freezing.
+    for (const auto& entry : data_.trajectories_state) {
+      const int other_trajectory_id = entry.first;
+      if (!IsTrajectoryFrozen(other_trajectory_id)) {
+        continue;
+      }
+      if (data_.trajectory_connectivity_state.TransitivelyConnected(
+              trajectory_id, other_trajectory_id)) {
+        // Already connected, nothing to do.
+        continue;
+      }
+      data_.trajectory_connectivity_state.Connect(
+          trajectory_id, other_trajectory_id, common::FromUniversal(0));
+    }
     data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
     return WorkItem::Result::kDoNotRunOptimization;
   });