Skip constraint search against unfinished submaps. (#1362)

The uplink server only receives the grid content of a submap after
that submap is finished for efficiency. Therefore, constraint
searches against that submap need to be skipped.
Also add checks to avoid this in the future.

FIXES=#1360

Author: gaschler

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

@@ -242,6 +242,11 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
   {
     common::MutexLocker locker(&mutex_);
     CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
+    if (!data_.submap_data.at(submap_id).submap->finished()) {
+      // Uplink server only receives grids when they are finished, so skip
+      // constraint search before that.
+      return;
+    }
 
     const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
     const common::Time last_connection_time =

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

@@ -256,6 +256,11 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
   {
     common::MutexLocker locker(&mutex_);
     CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
+    if (!data_.submap_data.at(submap_id).submap->finished()) {
+      // Uplink server only receives grids when they are finished, so skip
+      // constraint search before that.
+      return;
+    }
 
     for (const NodeId& submap_node_id :
          data_.submap_data.at(submap_id).node_ids) {

cartographer/mapping/internal/constraints/constraint_builder_2d.cc

@@ -158,6 +158,7 @@ void ConstraintBuilder2D::WhenDone(
 const ConstraintBuilder2D::SubmapScanMatcher*
 ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                      const Grid2D* const grid) {
+  CHECK(grid);
   if (submap_scan_matchers_.count(submap_id) != 0) {
     return &submap_scan_matchers_.at(submap_id);
   }
@@ -183,6 +184,7 @@ void ConstraintBuilder2D::ComputeConstraint(
     const transform::Rigid2d& initial_relative_pose,
     const SubmapScanMatcher& submap_scan_matcher,
     std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
+  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
   const transform::Rigid2d initial_pose =
       ComputeSubmapPose(*submap) * initial_relative_pose;
 

cartographer/mapping/internal/constraints/constraint_builder_2d.h

@@ -107,7 +107,7 @@ class ConstraintBuilder2D {
 
  private:
   struct SubmapScanMatcher {
-    const Grid2D* grid;
+    const Grid2D* grid = nullptr;
     std::unique_ptr<scan_matching::FastCorrelativeScanMatcher2D>
         fast_correlative_scan_matcher;
     std::weak_ptr<common::Task> creation_task_handle;

cartographer/mapping/internal/constraints/constraint_builder_3d.cc

@@ -199,6 +199,7 @@ void ConstraintBuilder3D::ComputeConstraint(
     const transform::Rigid3d& global_submap_pose,
     const SubmapScanMatcher& submap_scan_matcher,
     std::unique_ptr<Constraint>* constraint) {
+  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
   // The 'constraint_transform' (submap i <- node j) is computed from:
   // - a 'high_resolution_point_cloud' in node j and
   // - the initial guess 'initial_pose' (submap i <- node j).

cartographer/mapping/internal/constraints/constraint_builder_3d.h

@@ -116,8 +116,8 @@ class ConstraintBuilder3D {
 
  private:
   struct SubmapScanMatcher {
-    const HybridGrid* high_resolution_hybrid_grid;
-    const HybridGrid* low_resolution_hybrid_grid;
+    const HybridGrid* high_resolution_hybrid_grid = nullptr;
+    const HybridGrid* low_resolution_hybrid_grid = nullptr;
     std::unique_ptr<scan_matching::FastCorrelativeScanMatcher3D>
         fast_correlative_scan_matcher;
     std::weak_ptr<common::Task> creation_task_handle;

cartographer/mapping/map_builder_test.cc

@@ -443,7 +443,7 @@ TEST_P(MapBuilderTestByGridType, LocalizationOnFrozenTrajectory2D) {
       ++num_cross_trajectory_constraints;
     }
   }
-  EXPECT_GT(num_cross_trajectory_constraints, 3);
+  EXPECT_GE(num_cross_trajectory_constraints, 3);
   // TODO(gaschler): Subscribe global slam callback, verify that all nodes are
   // optimized.
   EXPECT_THAT(constraints, ::testing::Contains(::testing::Field(