Fix writing state of trajectories with unfinished submaps. (#1286)

cartographer/io/internal/mapping_state_serialization.cc

@@ -88,8 +88,10 @@ void SerializeSubmaps(
         submap_id_data.id.trajectory_id);
     submap_proto->mutable_submap_id()->set_submap_index(
         submap_id_data.id.submap_index);
-    submap_id_data.data.submap->ToProto(submap_proto,
-                                        /*include_probability_grid_data=*/true);
+    submap_id_data.data.submap->ToProto(
+        submap_proto,
+        /*include_probability_grid_data=*/submap_id_data.data.submap
+            ->finished());
     writer->WriteProto(proto);
   }
 }

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

@@ -560,8 +560,11 @@ void PoseGraph2D::AddSubmapFromProto(
   // Immediately show the submap at the 'global_submap_pose'.
   data_.global_submap_poses_2d.Insert(
       submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
-  AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
-    data_.submap_data.at(submap_id).state = SubmapState::kFinished;
+  bool finished = submap.submap_2d().finished();
+  AddWorkItem(
+      [this, submap_id, global_submap_pose_2d, finished]() REQUIRES(mutex_) {
+        data_.submap_data.at(submap_id).state =
+            finished ? SubmapState::kFinished : SubmapState::kActive;
         optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
         return WorkItem::Result::kDoNotRunOptimization;
       });

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

@@ -650,9 +650,12 @@ void PoseGraph3D::AddSubmapFromProto(
     data_.global_submap_poses_3d.Insert(
         submap_id, optimization::SubmapSpec3D{global_submap_pose});
   }
-  AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) {
+  bool finished = submap.submap_3d().finished();
+  AddWorkItem(
+      [this, submap_id, global_submap_pose, finished]() EXCLUDES(mutex_) {
         common::MutexLocker locker(&mutex_);
-    data_.submap_data.at(submap_id).state = SubmapState::kFinished;
+        data_.submap_data.at(submap_id).state =
+            finished ? SubmapState::kFinished : SubmapState::kActive;
         optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
         return WorkItem::Result::kDoNotRunOptimization;
       });

cartographer/mapping/internal/testing/test_helpers.cc

@@ -85,6 +85,7 @@ proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) {
   proto.mutable_submap_3d()->set_num_range_data(1);
   *proto.mutable_submap_3d()->mutable_local_pose() =
       transform::ToProto(transform::Rigid3d::Identity());
+  proto.mutable_submap_3d()->set_finished(true);
   return proto;
 }
 

cartographer/mapping/map_builder_test.cc

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