Fix multi-trajectory loop closure attempts. (#131)

Before, old scans with new submaps were always matched as if they
belong to connected trajectories.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan(
   }
 
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(j, submaps, matching_submap, insertion_submaps,
+    ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
                               finished_submap, pose, covariance);
   });
 }
@@ -140,6 +140,39 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   }
 }
 
+void SparsePoseGraph::ComputeConstraint(const int scan_index,
+                                        const int submap_index) {
+  const transform::Rigid2d relative_pose =
+      submap_transforms_[submap_index].inverse() *
+      point_cloud_poses_[scan_index];
+
+  const mapping::Submaps* const scan_trajectory =
+      trajectory_nodes_[scan_index].constant_data->trajectory;
+  const mapping::Submaps* const submap_trajectory =
+      submap_states_[submap_index].trajectory;
+  // Only globally match against submaps not in this trajectory.
+  if (scan_trajectory != submap_trajectory &&
+      global_localization_samplers_[scan_trajectory]->Pulse()) {
+    constraint_builder_.MaybeAddGlobalConstraint(
+        submap_index, submap_states_[submap_index].submap, scan_index,
+        scan_trajectory, submap_trajectory, &trajectory_connectivity_,
+        &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
+  } else {
+    const bool scan_and_submap_trajectories_connected =
+        reverse_connected_components_.count(scan_trajectory) > 0 &&
+        reverse_connected_components_.count(submap_trajectory) > 0 &&
+        reverse_connected_components_.at(scan_trajectory) ==
+            reverse_connected_components_.at(submap_trajectory);
+    if (scan_trajectory == submap_trajectory ||
+        scan_and_submap_trajectories_connected) {
+      constraint_builder_.MaybeAddConstraint(
+          submap_index, submap_states_[submap_index].submap, scan_index,
+          &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
+          relative_pose);
+    }
+  }
+}
+
 void SparsePoseGraph::ComputeConstraintsForOldScans(
     const mapping::Submap* submap) {
   const int submap_index = GetSubmapIndex(submap);
@@ -148,20 +181,13 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
   const int num_nodes = point_cloud_poses_.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
     if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
-      const transform::Rigid2d relative_pose =
+      ComputeConstraint(scan_index, submap_index);
-          submap_transforms_[submap_index].inverse() *
-          point_cloud_poses_[scan_index];
-      constraint_builder_.MaybeAddConstraint(
-          submap_index, submap, scan_index,
-          &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
-          relative_pose);
     }
   }
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    int scan_index, const mapping::Submaps* scan_trajectory,
+    int scan_index, const mapping::Submap* matching_submap,
-    const mapping::Submap* matching_submap,
     std::vector<const mapping::Submap*> insertion_submaps,
     const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
     const kalman_filter::Pose2DCovariance& covariance) {
@@ -194,42 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         Constraint::INTRA_SUBMAP});
   }
 
-  // Determine if this scan should be globally localized.
-  const bool run_global_localization =
-      global_localization_samplers_[scan_trajectory]->Pulse();
-
   CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
   const int num_submaps = submap_states_.size();
   for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
     if (submap_states_[submap_index].finished) {
       CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
-      const transform::Rigid2d relative_pose =
+      ComputeConstraint(scan_index, submap_index);
-          submap_transforms_[submap_index].inverse() *
-          point_cloud_poses_[scan_index];
-
-      const auto* submap_trajectory = submap_states_[submap_index].trajectory;
-
-      // Only globally match against submaps not in this trajectory.
-      if (run_global_localization && scan_trajectory != submap_trajectory) {
-        constraint_builder_.MaybeAddGlobalConstraint(
-            submap_index, submap_states_[submap_index].submap, scan_index,
-            scan_trajectory, submap_trajectory, &trajectory_connectivity_,
-            &trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
-      } else {
-        const bool scan_and_submap_trajectories_connected =
-            reverse_connected_components_.count(scan_trajectory) > 0 &&
-            reverse_connected_components_.count(submap_trajectory) > 0 &&
-            reverse_connected_components_.at(scan_trajectory) ==
-                reverse_connected_components_.at(submap_trajectory);
-        if (scan_trajectory == submap_trajectory ||
-            scan_and_submap_trajectories_connected) {
-          constraint_builder_.MaybeAddConstraint(
-              submap_index, submap_states_[submap_index].submap, scan_index,
-              &trajectory_nodes_[scan_index]
-                   .constant_data->laser_fan_2d.returns,
-              relative_pose);
-        }
-      }
     }
   }
 

cartographer/mapping_2d/sparse_pose_graph.h

@@ -127,12 +127,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
-      int scan_index, const mapping::Submaps* scan_trajectory,
+      int scan_index, const mapping::Submap* matching_submap,
-      const mapping::Submap* matching_submap,
       std::vector<const mapping::Submap*> insertion_submaps,
       const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
       const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
 
+  // Computes constraints for a scan and submap pair.
+  void ComputeConstraint(const int scan_index, const int submap_index)
+      REQUIRES(mutex_);
+
   // Adds constraints for older scans whenever a new submap is finished.
   void ComputeConstraintsForOldScans(const mapping::Submap* submap)
       REQUIRES(mutex_);

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -196,23 +196,25 @@ void ConstraintBuilder::ComputeConstraint(
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             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);
     } else {
       return;
     }
   } else {
-    if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
+    if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
             initial_pose, filtered_point_cloud, options_.min_score(), &score,
             &pose_estimate)) {
-      return;
-    }
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
+    } else {
+      return;
+    }
+  }
   {
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);
   }
-  }
 
   // Use the CSM estimate as both the initial and previous pose. This has the
   // effect that, in the absence of better information, we prefer the original

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -122,9 +122,8 @@ int SparsePoseGraph::AddScan(
   }
 
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(time, j, submaps, matching_submap,
+    ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
-                              insertion_submaps, finished_submap, pose,
+                              finished_submap, pose, covariance);
-                              covariance);
   });
   return j;
 }
@@ -147,6 +146,38 @@ void SparsePoseGraph::AddImuData(common::Time time,
   });
 }
 
+void SparsePoseGraph::ComputeConstraint(const int scan_index,
+                                        const int submap_index) {
+  const transform::Rigid3d relative_pose =
+      submap_transforms_[submap_index].inverse() *
+      optimization_problem_.node_data().at(scan_index).point_cloud_pose;
+
+  const mapping::Submaps* const scan_trajectory =
+      trajectory_nodes_[scan_index].constant_data->trajectory;
+  const mapping::Submaps* const submap_trajectory =
+      submap_states_[submap_index].trajectory;
+  // Only globally match against submaps not in this trajectory.
+  if (scan_trajectory != submap_trajectory &&
+      global_localization_samplers_[scan_trajectory]->Pulse()) {
+    constraint_builder_.MaybeAddGlobalConstraint(
+        submap_index, submap_states_[submap_index].submap, scan_index,
+        scan_trajectory, submap_trajectory, &trajectory_connectivity_,
+        trajectory_nodes_);
+  } else {
+    const bool scan_and_submap_trajectories_connected =
+        reverse_connected_components_.count(scan_trajectory) > 0 &&
+        reverse_connected_components_.count(submap_trajectory) > 0 &&
+        reverse_connected_components_.at(scan_trajectory) ==
+            reverse_connected_components_.at(submap_trajectory);
+    if (scan_trajectory == submap_trajectory ||
+        scan_and_submap_trajectories_connected) {
+      constraint_builder_.MaybeAddConstraint(
+          submap_index, submap_states_[submap_index].submap, scan_index,
+          trajectory_nodes_, relative_pose);
+    }
+  }
+}
+
 void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
   const int submap_index = GetSubmapIndex(submap);
   const auto& node_data = optimization_problem_.node_data();
@@ -155,20 +186,15 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
   const int num_nodes = node_data.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
     if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
-      const transform::Rigid3d relative_pose =
+      ComputeConstraint(scan_index, submap_index);
-          submap_transforms_[submap_index].inverse() *
-          node_data[scan_index].point_cloud_pose;
-      constraint_builder_.MaybeAddConstraint(submap_index, submap, scan_index,
-                                             trajectory_nodes_, relative_pose);
     }
   }
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
     const common::Time time, const int scan_index,
-    const Submaps* scan_trajectory, const Submap* matching_submap,
+    const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
-    std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
+    const Submap* finished_submap, const transform::Rigid3d& pose,
-    const transform::Rigid3d& pose,
     const kalman_filter::PoseCovariance& covariance) {
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const int matching_index = GetSubmapIndex(matching_submap);
@@ -194,39 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                    Constraint::INTRA_SUBMAP});
   }
 
-  // Determine if this scan should be globally localized.
-  const bool run_global_localization =
-      global_localization_samplers_[scan_trajectory]->Pulse();
-
   CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
   const int num_submaps = submap_states_.size();
   for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
     if (submap_states_[submap_index].finished) {
       CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
-      const transform::Rigid3d relative_pose =
+      ComputeConstraint(scan_index, submap_index);
-          submap_transforms_[submap_index].inverse() * optimized_pose;
-
-      const auto* submap_trajectory = submap_states_[submap_index].trajectory;
-
-      // Only globally match against submaps not in this trajectory.
-      if (run_global_localization && scan_trajectory != submap_trajectory) {
-        constraint_builder_.MaybeAddGlobalConstraint(
-            submap_index, submap_states_[submap_index].submap, scan_index,
-            scan_trajectory, submap_trajectory, &trajectory_connectivity_,
-            trajectory_nodes_);
-      } else {
-        const bool scan_and_submap_trajectories_connected =
-            reverse_connected_components_.count(scan_trajectory) > 0 &&
-            reverse_connected_components_.count(submap_trajectory) > 0 &&
-            reverse_connected_components_.at(scan_trajectory) ==
-                reverse_connected_components_.at(submap_trajectory);
-        if (scan_trajectory == submap_trajectory ||
-            scan_and_submap_trajectories_connected) {
-          constraint_builder_.MaybeAddConstraint(
-              submap_index, submap_states_[submap_index].submap, scan_index,
-              trajectory_nodes_, relative_pose);
-        }
-      }
     }
   }
 

cartographer/mapping_3d/sparse_pose_graph.h

@@ -129,12 +129,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Adds constraints for a scan, and starts scan matching in the background.
   void ComputeConstraintsForScan(
-      common::Time time, int scan_index, const Submaps* scan_trajectory,
+      common::Time time, int scan_index, const Submap* matching_submap,
-      const Submap* matching_submap,
       std::vector<const Submap*> insertion_submaps,
       const Submap* finished_submap, const transform::Rigid3d& pose,
       const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
 
+  // Computes constraints for a scan and submap pair.
+  void ComputeConstraint(const int scan_index, const int submap_index)
+      REQUIRES(mutex_);
+
   // Adds constraints for older scans whenever a new submap is finished.
   void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
 
@@ -167,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
 
   // We globally localize a fraction of the scans from each trajectory.
-  std::unordered_map<const Submaps*, std::unique_ptr<common::FixedRatioSampler>>
+  std::unordered_map<const mapping::Submaps*,
+                     std::unique_ptr<common::FixedRatioSampler>>
       global_localization_samplers_ GUARDED_BY(mutex_);
 
   // Number of scans added since last loop closure.

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -146,7 +146,8 @@ void ConstraintBuilder::WhenDone(
     const std::function<void(const ConstraintBuilder::Result&)> callback) {
   common::MutexLocker locker(&mutex_);
   CHECK(when_done_ == nullptr);
-  when_done_.reset(new std::function<void(const Result&)>(callback));
+  when_done_ =
+      common::make_unique<std::function<void(const Result&)>>(callback);
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
   thread_pool_->Schedule(
@@ -223,13 +224,14 @@ void ConstraintBuilder::ComputeConstraint(
 
   CHECK(!match_full_submap) << "match_full_submap not supported for 3D.";
 
-  if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
+  if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
           initial_pose, filtered_point_cloud, point_cloud, options_.min_score(),
           &score, &pose_estimate)) {
-    return;
-  }
     // We've reported a successful local match.
     CHECK_GT(score, options_.min_score());
+  } else {
+    return;
+  }
   {
     common::MutexLocker locker(&mutex_);
     score_histogram_.Add(score);