From 0ee06ba5611c736e36c1416928aafaf1df88d25c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Tue, 17 Jul 2018 08:39:50 +0200
Subject: [PATCH] Fix writing state of trajectories with unfinished submaps.
 (#1286)

---
 .../io/internal/mapping_state_serialization.cc    |  6 ++++--
 cartographer/mapping/internal/2d/pose_graph_2d.cc | 13 ++++++++-----
 cartographer/mapping/internal/3d/pose_graph_3d.cc | 15 +++++++++------
 .../mapping/internal/testing/test_helpers.cc      |  1 +
 cartographer/mapping/map_builder_test.cc          |  2 +-
 5 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc
index 5b6208a..2160c54 100644
--- a/cartographer/io/internal/mapping_state_serialization.cc
+++ b/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);
   }
 }
diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc
index de95104..d728666 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d.cc
+++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc
@@ -560,11 +560,14 @@ 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;
-    optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
-    return WorkItem::Result::kDoNotRunOptimization;
-  });
+  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;
+      });
 }
 
 void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc
index 3622ebc..a1431b6 100644
--- a/cartographer/mapping/internal/3d/pose_graph_3d.cc
+++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc
@@ -650,12 +650,15 @@ 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_) {
-    common::MutexLocker locker(&mutex_);
-    data_.submap_data.at(submap_id).state = SubmapState::kFinished;
-    optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
-    return WorkItem::Result::kDoNotRunOptimization;
-  });
+  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 =
+            finished ? SubmapState::kFinished : SubmapState::kActive;
+        optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
+        return WorkItem::Result::kDoNotRunOptimization;
+      });
 }
 
 void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc
index 18389be..d5413ab 100644
--- a/cartographer/mapping/internal/testing/test_helpers.cc
+++ b/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;
 }
 
diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc
index 6bf9190..cf3dc10 100644
--- a/cartographer/mapping/map_builder_test.cc
+++ b/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(