Add argument to SerializeState for excluding unfinished submaps (#1352)

Adds a boolean argument to MapBuilderInterface::SerializeState() to indicate whether the caller wants to include unfinished submaps in the serialized state.

For cartographer_grpc this argument will be set to false since unfinished submaps do not have a Grid which would lead to a crash in the Submap::ToProto() function.

cartographer/cloud/client/map_builder_stub.cc

@@ -123,7 +123,10 @@ std::string MapBuilderStub::SubmapToProto(
   return client.response().error_msg();
 }
 
-void MapBuilderStub::SerializeState(io::ProtoStreamWriterInterface* writer) {
+void MapBuilderStub::SerializeState(bool include_unfinished_submaps,
+                                    io::ProtoStreamWriterInterface* writer) {
+  CHECK(!include_unfinished_submaps)
+      << "Writing of unfinished submaps is unsupported.";
   google::protobuf::Empty request;
   async_grpc::Client<handlers::WriteStateSignature> client(client_channel_);
   CHECK(client.Write(request));

cartographer/cloud/client/map_builder_stub.h

@@ -48,7 +48,8 @@ class MapBuilderStub : public mapping::MapBuilderInterface {
   std::string SubmapToProto(
       const mapping::SubmapId& submap_id,
       mapping::proto::SubmapQuery::Response* response) override;
-  void SerializeState(io::ProtoStreamWriterInterface* writer) override;
+  void SerializeState(bool include_unfinished_submaps,
+                      io::ProtoStreamWriterInterface* writer) override;
   std::map<int, int> LoadState(io::ProtoStreamReaderInterface* reader,
                                bool load_frozen_state) override;
   std::map<int, int> LoadStateFromFile(const std::string& filename) override;

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -202,7 +202,8 @@ PoseGraphStub::constraints() const {
   return mapping::FromProto(client.response().constraints());
 }
 
-mapping::proto::PoseGraph PoseGraphStub::ToProto() const {
+mapping::proto::PoseGraph PoseGraphStub::ToProto(
+    bool include_unfinished_submaps) const {
   LOG(FATAL) << "Not implemented";
 }
 

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -52,7 +52,8 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()
       const override;
   std::vector<Constraint> constraints() const override;
-  mapping::proto::PoseGraph ToProto() const override;
+  mapping::proto::PoseGraph ToProto(
+      bool include_unfinished_submaps) const override;
   void SetGlobalSlamOptimizationCallback(
       GlobalSlamOptimizationCallback callback) override;
 

cartographer/cloud/internal/client_server_test.cc

@@ -452,6 +452,25 @@ TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
   }
   WaitForLocalSlamResults(measurements.size());
   WaitForLocalSlamResultUploads(number_of_insertion_results_);
+
+  std::queue<std::unique_ptr<google::protobuf::Message>> chunks;
+  io::ForwardingProtoStreamWriter writer(
+      [&chunks](const google::protobuf::Message* proto) -> bool {
+        if (!proto) {
+          return true;
+        }
+        std::unique_ptr<google::protobuf::Message> p(proto->New());
+        p->CopyFrom(*proto);
+        chunks.push(std::move(p));
+        return true;
+      });
+  stub_->SerializeState(false, &writer);
+  CHECK(writer.Close());
+  // Ensure it can be read.
+  io::InMemoryProtoStreamReader reader(std::move(chunks));
+  io::ProtoStreamDeserializer deserializer(&reader);
+  EXPECT_EQ(deserializer.pose_graph().trajectory_size(), 1);
+
   stub_for_uploading_server_->FinishTrajectory(trajectory_id);
   EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
   EXPECT_NEAR(kTravelDistance,

cartographer/cloud/internal/handlers/write_state_handler.cc

@@ -49,7 +49,7 @@ void WriteStateHandler::OnRequest(const google::protobuf::Empty& request) {
         return true;
       });
   GetContext<MapBuilderContextInterface>()->map_builder().SerializeState(
-      &proto_stream_writer);
+      /*include_unfinished_submaps=*/false, &proto_stream_writer);
   proto_stream_writer.Close();
 }
 

cartographer/io/internal/mapping_state_serialization.cc

@@ -60,9 +60,10 @@ mapping::proto::SerializationHeader CreateHeader() {
   return header;
 }
 
-SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph) {
+SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph,
+                                  bool include_unfinished_submaps) {
   SerializedData proto;
-  *proto.mutable_pose_graph() = pose_graph.ToProto();
+  *proto.mutable_pose_graph() = pose_graph.ToProto(include_unfinished_submaps);
   return proto;
 }
 
@@ -79,9 +80,13 @@ SerializedData SerializeTrajectoryBuilderOptions(
 
 void SerializeSubmaps(
     const MapById<SubmapId, PoseGraphInterface::SubmapData>& submap_data,
-    ProtoStreamWriterInterface* const writer) {
+    bool include_unfinished_submaps, ProtoStreamWriterInterface* const writer) {
   // Next serialize all submaps.
   for (const auto& submap_id_data : submap_data) {
+    if (!include_unfinished_submaps &&
+        !submap_id_data.data.submap->finished()) {
+      continue;
+    }
     SerializedData proto;
     auto* const submap_proto = proto.mutable_submap();
     *submap_proto = submap_id_data.data.submap->ToProto(
@@ -209,14 +214,16 @@ void WritePbStream(
     const mapping::PoseGraph& pose_graph,
     const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
         trajectory_builder_options,
-    ProtoStreamWriterInterface* const writer) {
+    ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps) {
   writer->WriteProto(CreateHeader());
-  writer->WriteProto(SerializePoseGraph(pose_graph));
+  writer->WriteProto(
+      SerializePoseGraph(pose_graph, include_unfinished_submaps));
   writer->WriteProto(SerializeTrajectoryBuilderOptions(
       trajectory_builder_options,
       GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));
 
-  SerializeSubmaps(pose_graph.GetAllSubmapData(), writer);
+  SerializeSubmaps(pose_graph.GetAllSubmapData(), include_unfinished_submaps,
+                   writer);
   SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);
   SerializeTrajectoryData(pose_graph.GetTrajectoryData(), writer);
   SerializeImuData(pose_graph.GetImuData(), writer);

cartographer/io/internal/mapping_state_serialization.h

@@ -32,7 +32,7 @@ void WritePbStream(
     const mapping::PoseGraph& pose_graph,
     const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
         builder_options,
-    ProtoStreamWriterInterface* const writer);
+    ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps);
 
 }  // namespace io
 }  // namespace cartographer

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

@@ -100,7 +100,7 @@ TEST_F(PoseGraph3DTest, Empty) {
   EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty());
   proto::PoseGraph empty_proto;
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
-      pose_graph_->ToProto(), empty_proto));
+      pose_graph_->ToProto(/*include_unfinished_submaps=*/true), empty_proto));
 }
 
 TEST_F(PoseGraph3DTest, BasicSerialization) {
@@ -119,7 +119,8 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
       testing::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto);
   pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity());
   pose_graph_->WaitForAllComputations();
-  proto::PoseGraph actual_proto = pose_graph_->ToProto();
+  proto::PoseGraph actual_proto =
+      pose_graph_->ToProto(/*include_unfinished_submaps=*/true);
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
       proto.constraint(0), actual_proto.constraint(0)));
   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
@@ -134,6 +135,59 @@ TEST_F(PoseGraph3DTest, BasicSerialization) {
       google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
 }
 
+TEST_F(PoseGraph3DTest, SerializationWithUnfinishedSubmaps) {
+  BuildPoseGraph();
+  proto::PoseGraph proto;
+
+  // Create three nodes.
+  auto fake_node_1 = testing::CreateFakeNode(1, 1);
+  testing::AddToProtoGraph(fake_node_1, &proto);
+  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_1);
+  auto fake_node_2 = testing::CreateFakeNode(1, 2);
+  testing::AddToProtoGraph(fake_node_2, &proto);
+  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_2);
+  auto fake_node_3 = testing::CreateFakeNode(1, 3);
+  testing::AddToProtoGraph(fake_node_3, &proto);
+  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_3);
+
+  // Create two submaps: one finished, the other not.
+  auto fake_submap_1 = testing::CreateFakeSubmap3D(1, 1, true);
+  testing::AddToProtoGraph(fake_submap_1, &proto);
+  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_1);
+  auto fake_submap_2 = testing::CreateFakeSubmap3D(1, 2, false);
+  testing::AddToProtoGraph(fake_submap_2, &proto);
+  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_2);
+
+  // Connect node 1 to submap 1, node 2 to submaps 1 and 2, node 3 to submap 2.
+  testing::AddToProtoGraph(
+      testing::CreateFakeConstraint(fake_node_1, fake_submap_1), &proto);
+  testing::AddToProtoGraph(
+      testing::CreateFakeConstraint(fake_node_2, fake_submap_1), &proto);
+  testing::AddToProtoGraph(
+      testing::CreateFakeConstraint(fake_node_2, fake_submap_2), &proto);
+  testing::AddToProtoGraph(
+      testing::CreateFakeConstraint(fake_node_3, fake_submap_2), &proto);
+  pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
+
+  pose_graph_->WaitForAllComputations();
+  proto::PoseGraph actual_proto =
+      pose_graph_->ToProto(/*include_unfinished_submaps=*/false);
+  EXPECT_EQ(actual_proto.constraint_size(), 2);
+  EXPECT_EQ(actual_proto.trajectory_size(), 1);
+  EXPECT_EQ(actual_proto.trajectory(0).node_size(), 2);
+  EXPECT_EQ(actual_proto.trajectory(0).submap_size(), 1);
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.constraint(0), actual_proto.constraint(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.constraint(1), actual_proto.constraint(1)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0).node(1), actual_proto.trajectory(0).node(1)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
+}
+
 TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
   BuildPoseGraphWithFakeOptimization();
   const int trajectory_id = 2;

cartographer/mapping/internal/testing/mock_map_builder.h

@@ -47,7 +47,7 @@ class MockMapBuilder : public mapping::MapBuilderInterface {
   MOCK_METHOD2(SubmapToProto,
                std::string(const mapping::SubmapId &,
                            mapping::proto::SubmapQuery::Response *));
-  MOCK_METHOD1(SerializeState, void(io::ProtoStreamWriterInterface *));
+  MOCK_METHOD2(SerializeState, void(bool, io::ProtoStreamWriterInterface *));
   MOCK_METHOD2(LoadState,
                std::map<int, int>(io::ProtoStreamReaderInterface *, bool));
   MOCK_METHOD1(LoadStateFromFile, std::map<int, int>(const std::string &));

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -57,7 +57,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
       GetTrajectoryData,
       std::map<int, mapping::PoseGraphInterface::TrajectoryData>());
   MOCK_CONST_METHOD0(constraints, std::vector<Constraint>());
-  MOCK_CONST_METHOD0(ToProto, mapping::proto::PoseGraph());
+  MOCK_CONST_METHOD1(ToProto, mapping::proto::PoseGraph(bool));
   MOCK_METHOD1(SetGlobalSlamOptimizationCallback,
                void(GlobalSlamOptimizationCallback callback));
 };

cartographer/mapping/internal/testing/test_helpers.cc

@@ -78,13 +78,15 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
   return measurements;
 }
 
-proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) {
+proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index,
+                                 bool finished) {
   proto::Submap proto;
   proto.mutable_submap_id()->set_trajectory_id(trajectory_id);
   proto.mutable_submap_id()->set_submap_index(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(finished);
   return proto;
 }
 

cartographer/mapping/internal/testing/test_helpers.h

@@ -39,7 +39,8 @@ GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
                               double duration, double time_step,
                               const transform::Rigid3f& local_to_global);
 
-proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1);
+proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1,
+                                 bool finished = true);
 
 proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1);
 

cartographer/mapping/map_builder.cc

@@ -219,8 +219,10 @@ std::string MapBuilder::SubmapToProto(
   return "";
 }
 
-void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
-  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer);
+void MapBuilder::SerializeState(bool include_unfinished_submaps,
+                                io::ProtoStreamWriterInterface* const writer) {
+  io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer,
+                    include_unfinished_submaps);
 }
 
 std::map<int, int> MapBuilder::LoadState(

cartographer/mapping/map_builder.h

@@ -56,7 +56,8 @@ class MapBuilder : public MapBuilderInterface {
   std::string SubmapToProto(const SubmapId &submap_id,
                             proto::SubmapQuery::Response *response) override;
 
-  void SerializeState(io::ProtoStreamWriterInterface *writer) override;
+  void SerializeState(bool include_unfinished_submaps,
+                      io::ProtoStreamWriterInterface *writer) override;
 
   std::map<int, int> LoadState(io::ProtoStreamReaderInterface *reader,
                                bool load_frozen_state) override;

cartographer/mapping/map_builder_interface.h

@@ -77,8 +77,12 @@ class MapBuilderInterface {
   virtual std::string SubmapToProto(const SubmapId& submap_id,
                                     proto::SubmapQuery::Response* response) = 0;
 
-  // Serializes the current state to a proto stream.
-  virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0;
+  // Serializes the current state to a proto stream. If
+  // 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
+  // submaps that have not yet received all rangefinder data insertions, will
+  // be included in the serialized state.
+  virtual void SerializeState(bool include_unfinished_submaps,
+                              io::ProtoStreamWriterInterface* writer) = 0;
 
   // Loads the SLAM state from a proto stream. Returns the remapping of new
   // trajectory_ids.

cartographer/mapping/map_builder_test.cc

@@ -342,7 +342,7 @@ TEST_F(MapBuilderTest, SaveLoadState) {
     // TODO(gaschler): Consider using in-memory to avoid side effects.
     const std::string filename = "temp-SaveLoadState.pbstream";
     io::ProtoStreamWriter writer(filename);
-    map_builder_->SerializeState(&writer);
+    map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
     writer.Close();
 
     // Reset 'map_builder_'.
@@ -373,7 +373,7 @@ TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) {
       0);
   const std::string filename = "temp-LocalizationOnFrozenTrajectory2D.pbstream";
   io::ProtoStreamWriter writer(filename);
-  map_builder_->SerializeState(&writer);
+  map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
   writer.Close();
 
   // Reset 'map_builder_'.

cartographer/mapping/pose_graph.cc

@@ -115,7 +115,7 @@ proto::PoseGraph::Constraint ToProto(const PoseGraph::Constraint& constraint) {
   return constraint_proto;
 }
 
-proto::PoseGraph PoseGraph::ToProto() const {
+proto::PoseGraph PoseGraph::ToProto(bool include_unfinished_submaps) const {
   proto::PoseGraph proto;
 
   std::map<int, proto::Trajectory* const> trajectory_protos;
@@ -129,30 +129,62 @@ proto::PoseGraph PoseGraph::ToProto() const {
     return trajectory_protos.at(trajectory_id);
   };
 
-  for (const auto& node_id_data : GetTrajectoryNodes()) {
-    CHECK(node_id_data.data.constant_data != nullptr);
-    auto* const node_proto =
-        trajectory(node_id_data.id.trajectory_id)->add_node();
-    node_proto->set_node_index(node_id_data.id.node_index);
-    node_proto->set_timestamp(
-        common::ToUniversal(node_id_data.data.constant_data->time));
-    *node_proto->mutable_pose() =
-        transform::ToProto(node_id_data.data.global_pose);
-  }
-
+  std::set<mapping::SubmapId> unfinished_submaps;
   for (const auto& submap_id_data : GetAllSubmapData()) {
+    proto::Trajectory* trajectory_proto =
+        trajectory(submap_id_data.id.trajectory_id);
+    if (!include_unfinished_submaps &&
+        !submap_id_data.data.submap->finished()) {
+      // Collect IDs of all unfinished submaps and skip them.
+      unfinished_submaps.insert(submap_id_data.id);
+      continue;
+    }
     CHECK(submap_id_data.data.submap != nullptr);
-    auto* const submap_proto =
-        trajectory(submap_id_data.id.trajectory_id)->add_submap();
+    auto* const submap_proto = trajectory_proto->add_submap();
     submap_proto->set_submap_index(submap_id_data.id.submap_index);
     *submap_proto->mutable_pose() =
         transform::ToProto(submap_id_data.data.pose);
   }
 
   auto constraints_copy = constraints();
+  std::set<mapping::NodeId> orphaned_nodes;
   proto.mutable_constraint()->Reserve(constraints_copy.size());
+  for (auto it = constraints_copy.begin(); it != constraints_copy.end();) {
+    if (!include_unfinished_submaps &&
+        unfinished_submaps.count(it->submap_id) > 0) {
+      // Skip all those constraints that refer to unfinished submaps and
+      // remember the corresponding trajectory nodes as potentially orphaned.
+      orphaned_nodes.insert(it->node_id);
+      it = constraints_copy.erase(it);
+      continue;
+    }
+    *proto.add_constraint() = cartographer::mapping::ToProto(*it);
+    ++it;
+  }
+
+  if (!include_unfinished_submaps) {
+    // Iterate over all constraints and remove trajectory nodes from
+    // 'orphaned_nodes' that are not actually orphaned.
     for (const auto& constraint : constraints_copy) {
-    *proto.add_constraint() = cartographer::mapping::ToProto(constraint);
+      orphaned_nodes.erase(constraint.node_id);
+    }
+  }
+
+  for (const auto& node_id_data : GetTrajectoryNodes()) {
+    proto::Trajectory* trajectory_proto =
+        trajectory(node_id_data.id.trajectory_id);
+    if (!include_unfinished_submaps &&
+        orphaned_nodes.count(node_id_data.id) > 0) {
+      // Skip orphaned trajectory nodes.
+      continue;
+    }
+    CHECK(node_id_data.data.constant_data != nullptr);
+    auto* const node_proto = trajectory_proto->add_node();
+    node_proto->set_node_index(node_id_data.id.node_index);
+    node_proto->set_timestamp(
+        common::ToUniversal(node_id_data.data.constant_data->time));
+    *node_proto->mutable_pose() =
+        transform::ToProto(node_id_data.data.global_pose);
   }
 
   auto landmarks_copy = GetLandmarkPoses();

cartographer/mapping/pose_graph.h

@@ -117,7 +117,7 @@ class PoseGraph : public PoseGraphInterface {
   // not exist (anymore).
   virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;
 
-  proto::PoseGraph ToProto() const override;
+  proto::PoseGraph ToProto(bool include_unfinished_submaps) const override;
 
   // Returns the IMU data.
   virtual sensor::MapByTime<sensor::ImuData> GetImuData() const = 0;

cartographer/mapping/pose_graph_interface.h

@@ -140,8 +140,11 @@ class PoseGraphInterface {
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() const = 0;
 
-  // Serializes the constraints and trajectories.
-  virtual proto::PoseGraph ToProto() const = 0;
+  // Serializes the constraints and trajectories. If
+  // 'include_unfinished_submaps' is set to 'true', unfinished submps, i.e.
+  // submaps that have not yet received all rangefinder data insertions, will
+  // be included, otherwise not.
+  virtual proto::PoseGraph ToProto(bool include_unfinished_submaps) const = 0;
 
   // Sets the callback function that is invoked whenever the global optimization
   // problem is solved.