Fix serialization for deleted trajectories. (#1214)

* update TrajectoryState of trajectories that got 'trimmed' away to be
 deleted in the PureLocalizationTrimmer
 * update serialization to only serialize 'undeleted' trajectories and
 corresponding options.

#1111

cartographer/io/internal/mapping_state_serialization.cc

@@ -31,16 +31,29 @@ using mapping::proto::SerializedData;
 mapping::proto::AllTrajectoryBuilderOptions
 CreateAllTrajectoryBuilderOptionsProto(
     const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
-        all_options_with_sensor_ids) {
+        all_options_with_sensor_ids,
+    const std::vector<int>& trajectory_ids_to_serialize) {
   mapping::proto::AllTrajectoryBuilderOptions all_options_proto;
-  for (const auto& options_with_sensor_ids : all_options_with_sensor_ids) {
+  for (auto id : trajectory_ids_to_serialize) {
-    *all_options_proto.add_options_with_sensor_ids() = options_with_sensor_ids;
+    *all_options_proto.add_options_with_sensor_ids() =
+        all_options_with_sensor_ids[id];
   }
-  CHECK_EQ(all_options_with_sensor_ids.size(),
-           all_options_proto.options_with_sensor_ids_size());
   return all_options_proto;
 }
 
+// Will return all trajectory ids, that have `state != DELETED`.
+std::vector<int> GetValidTrajectoryIds(
+    const std::map<int, PoseGraphInterface::TrajectoryState>&
+        trajectory_states) {
+  std::vector<int> valid_trajectories;
+  for (const auto& t : trajectory_states) {
+    if (t.second != PoseGraphInterface::TrajectoryState::DELETED) {
+      valid_trajectories.push_back(t.first);
+    }
+  }
+  return valid_trajectories;
+}
+
 mapping::proto::SerializationHeader CreateHeader() {
   mapping::proto::SerializationHeader header;
   header.set_format_version(kMappingStateSerializationFormatVersion);
@@ -53,12 +66,14 @@ SerializedData SerializePoseGraph(const mapping::PoseGraph& pose_graph) {
   return proto;
 }
 
-SerializedData SerializeAllTrajectoryBuilderOptions(
+SerializedData SerializeTrajectoryBuilderOptions(
     const std::vector<mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
-        trajectory_builder_options) {
+        trajectory_builder_options,
+    const std::vector<int>& trajectory_ids_to_serialize) {
   SerializedData proto;
   *proto.mutable_all_trajectory_builder_options() =
-      CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options);
+      CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_options,
+                                             trajectory_ids_to_serialize);
   return proto;
 }
 
@@ -197,8 +212,9 @@ void WritePbStream(
     ProtoStreamWriterInterface* const writer) {
   writer->WriteProto(CreateHeader());
   writer->WriteProto(SerializePoseGraph(pose_graph));
-  writer->WriteProto(
+  writer->WriteProto(SerializeTrajectoryBuilderOptions(
-      SerializeAllTrajectoryBuilderOptions(trajectory_builder_options));
+      trajectory_builder_options,
+      GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));
 
   SerializeSubmaps(pose_graph.GetAllSubmapData(), writer);
   SerializeTrajectoryNodes(pose_graph.GetTrajectoryNodes(), writer);

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

@@ -986,6 +986,11 @@ bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
   return parent_->IsTrajectoryFinished(trajectory_id);
 }
 
+void PoseGraph2D::TrimmingHandle::SetTrajectoryState(int trajectory_id,
+                                                     TrajectoryState state) {
+  parent_->data_.trajectories_state[trajectory_id].state = state;
+}
+
 void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   // TODO(hrapp): We have to make sure that the trajectory has been finished
   // if we want to delete the last submaps.

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

@@ -264,6 +264,8 @@ class PoseGraph2D : public PoseGraph {
     void TrimSubmap(const SubmapId& submap_id)
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);
+    void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
+        REQUIRES(parent_->mutex_);
 
    private:
     PoseGraph2D* const parent_;

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

@@ -1012,6 +1012,11 @@ bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const {
   return parent_->IsTrajectoryFinished(trajectory_id);
 }
 
+void PoseGraph3D::TrimmingHandle::SetTrajectoryState(int trajectory_id,
+                                                     TrajectoryState state) {
+  parent_->data_.trajectories_state[trajectory_id].state = state;
+}
+
 void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
   // TODO(hrapp): We have to make sure that the trajectory has been finished
   // if we want to delete the last submaps.

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

@@ -268,6 +268,9 @@ class PoseGraph3D : public PoseGraph {
         REQUIRES(parent_->mutex_) override;
     bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);
 
+    void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
+        REQUIRES(parent_->mutex_);
+
    private:
     PoseGraph3D* const parent_;
   };

cartographer/mapping/internal/testing/fake_trimmable.h

@@ -96,6 +96,10 @@ class FakeTrimmable : public Trimmable {
 
   bool IsFinished(const int trajectory_id) const override { return false; }
 
+  void SetTrajectoryState(
+      int /*trajectory_id*/,
+      PoseGraphInterface::TrajectoryState /*state*/) override {}
+
   std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
 
  private:

cartographer/mapping/pose_graph_trimmer.cc

@@ -39,6 +39,8 @@ void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
 
   if (num_submaps_to_keep_ == 0) {
     finished_ = true;
+    pose_graph->SetTrajectoryState(
+        trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED);
   }
 }
 

cartographer/mapping/pose_graph_trimmer.h

@@ -46,6 +46,10 @@ class Trimmable {
 
   // Checks if the given trajectory is finished or not.
   virtual bool IsFinished(int trajectory_id) const = 0;
+
+  // Sets the state for a specific trajectory.
+  virtual void SetTrajectoryState(
+      int trajectory_id, PoseGraphInterface::TrajectoryState state) = 0;
 };
 
 // An interface to implement algorithms that choose how to trim the pose graph.