Serialize and deserialize trajectory builder options (#859)

cartographer/mapping/map_builder.cc

@@ -129,12 +129,24 @@ int MapBuilder::AddTrajectoryBuilder(
         transform::ToRigid3(initial_trajectory_pose.relative_pose()),
         common::FromUniversal(initial_trajectory_pose.timestamp()));
   }
+  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
+  for (const auto& sensor_id : expected_sensor_ids) {
+    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
+  }
+  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
+      trajectory_options;
+  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
+  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
   return trajectory_id;
 }
 
-int MapBuilder::AddTrajectoryForDeserialization() {
+int MapBuilder::AddTrajectoryForDeserialization(
+    const proto::TrajectoryBuilderOptionsWithSensorIds&
+        options_with_sensor_ids_proto) {
   const int trajectory_id = trajectory_builders_.size();
   trajectory_builders_.emplace_back();
+  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
+  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
   return trajectory_id;
 }
 
@@ -171,6 +183,18 @@ std::string MapBuilder::SubmapToProto(
 void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
   // We serialize the pose graph followed by all the data referenced in it.
   writer->WriteProto(pose_graph_->ToProto());
+  // Serialize trajectory builder options.
+  {
+    proto::AllTrajectoryBuilderOptions all_builder_options_proto;
+    for (const auto& options_with_sensor_ids :
+         all_trajectory_builder_options_) {
+      *all_builder_options_proto.add_options_with_sensor_ids() =
+          options_with_sensor_ids;
+    }
+    CHECK_EQ(all_trajectory_builder_options_.size(),
+             all_builder_options_proto.options_with_sensor_ids_size());
+    writer->WriteProto(all_builder_options_proto);
+  }
   // Next we serialize all submap data.
   {
     for (const auto& submap_id_data : pose_graph_->GetAllSubmapData()) {
@@ -245,12 +269,20 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
 }
 
 void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
-  proto::PoseGraph pose_graph;
+  proto::PoseGraph pose_graph_proto;
-  CHECK(reader->ReadProto(&pose_graph));
+  CHECK(reader->ReadProto(&pose_graph_proto));
+  proto::AllTrajectoryBuilderOptions all_builder_options_proto;
+  CHECK(reader->ReadProto(&all_builder_options_proto));
+  CHECK_EQ(pose_graph_proto.trajectory_size(),
+           all_builder_options_proto.options_with_sensor_ids_size());
 
   std::map<int, int> trajectory_remapping;
-  for (auto& trajectory_proto : *pose_graph.mutable_trajectory()) {
+  for (auto& trajectory_proto : *pose_graph_proto.mutable_trajectory()) {
-    const int new_trajectory_id = AddTrajectoryForDeserialization();
+    const auto& options_with_sensor_ids_proto =
+        all_builder_options_proto.options_with_sensor_ids(
+            trajectory_proto.trajectory_id());
+    const int new_trajectory_id =
+        AddTrajectoryForDeserialization(options_with_sensor_ids_proto);
     CHECK(trajectory_remapping
               .emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
               .second)
@@ -260,7 +292,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
   }
 
   MapById<SubmapId, transform::Rigid3d> submap_poses;
-  for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
+  for (const proto::Trajectory& trajectory_proto :
+       pose_graph_proto.trajectory()) {
     for (const proto::Trajectory::Submap& submap_proto :
          trajectory_proto.submap()) {
       submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
@@ -270,7 +303,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
   }
 
   MapById<NodeId, transform::Rigid3d> node_poses;
-  for (const proto::Trajectory& trajectory_proto : pose_graph.trajectory()) {
+  for (const proto::Trajectory& trajectory_proto :
+       pose_graph_proto.trajectory()) {
     for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) {
       node_poses.Insert(
           NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()},
@@ -305,7 +339,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
 
   // Add information about which nodes belong to which submap.
   for (const proto::PoseGraph::Constraint& constraint_proto :
-       pose_graph.constraint()) {
+       pose_graph_proto.constraint()) {
     if (constraint_proto.tag() !=
         mapping::proto::PoseGraph::Constraint::INTRA_SUBMAP) {
       continue;
@@ -327,5 +361,10 @@ int MapBuilder::num_trajectory_builders() const {
 
 PoseGraphInterface* MapBuilder::pose_graph() { return pose_graph_; }
 
+const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
+MapBuilder::GetAllTrajectoryBuilderOptions() const {
+  return all_trajectory_builder_options_;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/map_builder.h

@@ -50,7 +50,9 @@ class MapBuilder : public MapBuilderInterface {
       const proto::TrajectoryBuilderOptions& trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) override;
 
-  int AddTrajectoryForDeserialization() override;
+  int AddTrajectoryForDeserialization(
+      const proto::TrajectoryBuilderOptionsWithSensorIds&
+          options_with_sensor_ids_proto) override;
 
   mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
       int trajectory_id) const override;
@@ -68,6 +70,9 @@ class MapBuilder : public MapBuilderInterface {
 
   mapping::PoseGraphInterface* pose_graph() override;
 
+  const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
+  GetAllTrajectoryBuilderOptions() const override;
+
  private:
   const proto::MapBuilderOptions options_;
   common::ThreadPool thread_pool_;
@@ -79,6 +84,8 @@ class MapBuilder : public MapBuilderInterface {
   std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
   std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
       trajectory_builders_;
+  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
+      all_trajectory_builder_options_;
 };
 
 }  // namespace mapping

cartographer/mapping/map_builder_interface.h

@@ -58,7 +58,9 @@ class MapBuilderInterface {
 
   // Creates a new trajectory and returns its index. Querying the trajectory
   // builder for it will return 'nullptr'.
-  virtual int AddTrajectoryForDeserialization() = 0;
+  virtual int AddTrajectoryForDeserialization(
+      const proto::TrajectoryBuilderOptionsWithSensorIds&
+          options_with_sensor_ids_proto) = 0;
 
   // Returns the 'TrajectoryBuilderInterface' corresponding to the specified
   // 'trajectory_id' or 'nullptr' if the trajectory has no corresponding
@@ -84,6 +86,9 @@ class MapBuilderInterface {
   virtual int num_trajectory_builders() const = 0;
 
   virtual mapping::PoseGraphInterface* pose_graph() = 0;
+
+  virtual const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
+  GetAllTrajectoryBuilderOptions() const = 0;
 };
 
 }  // namespace mapping

cartographer/mapping/proto/trajectory_builder_options.proto

@@ -34,3 +34,26 @@ message TrajectoryBuilderOptions {
   bool pure_localization = 3;
   InitialTrajectoryPose initial_trajectory_pose = 4;
 }
+
+message SensorId {
+  enum SensorType {
+    RANGE = 0;
+    IMU = 1;
+    ODOMETRY = 2;
+    FIXED_FRAME_POSE = 3;
+    LANDMARK = 4;
+    LOCAL_SLAM_RESULT = 5;
+  }
+
+  SensorType type = 1;
+  string id = 2;
+}
+
+message TrajectoryBuilderOptionsWithSensorIds {
+  repeated SensorId sensor_id = 1;
+  TrajectoryBuilderOptions trajectory_builder_options = 2;
+}
+
+message AllTrajectoryBuilderOptions {
+  repeated TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids = 1;
+}

cartographer/mapping/trajectory_builder_interface.cc

@@ -37,5 +37,66 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
   return options;
 }
 
+proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id) {
+  proto::SensorId sensor_id_proto;
+  switch (sensor_id.type) {
+    case TrajectoryBuilderInterface::SensorId::SensorType::RANGE:
+      sensor_id_proto.set_type(proto::SensorId::RANGE);
+      break;
+    case TrajectoryBuilderInterface::SensorId::SensorType::IMU:
+      sensor_id_proto.set_type(proto::SensorId::IMU);
+      break;
+    case TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY:
+      sensor_id_proto.set_type(proto::SensorId::ODOMETRY);
+      break;
+    case TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE:
+      sensor_id_proto.set_type(proto::SensorId::FIXED_FRAME_POSE);
+      break;
+    case TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK:
+      sensor_id_proto.set_type(proto::SensorId::LANDMARK);
+      break;
+    case TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT:
+      sensor_id_proto.set_type(proto::SensorId::LOCAL_SLAM_RESULT);
+      break;
+    default:
+      LOG(FATAL) << "Unsupported sensor type.";
+  }
+  sensor_id_proto.set_id(sensor_id.id);
+  return sensor_id_proto;
+}
+
+TrajectoryBuilderInterface::SensorId FromProto(
+    const proto::SensorId& sensor_id_proto) {
+  TrajectoryBuilderInterface::SensorId sensor_id;
+  switch (sensor_id_proto.type()) {
+    case proto::SensorId::RANGE:
+      sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::RANGE;
+      break;
+    case proto::SensorId::IMU:
+      sensor_id.type = TrajectoryBuilderInterface::SensorId::SensorType::IMU;
+      break;
+    case proto::SensorId::ODOMETRY:
+      sensor_id.type =
+          TrajectoryBuilderInterface::SensorId::SensorType::ODOMETRY;
+      break;
+    case proto::SensorId::FIXED_FRAME_POSE:
+      sensor_id.type =
+          TrajectoryBuilderInterface::SensorId::SensorType::FIXED_FRAME_POSE;
+      break;
+    case proto::SensorId::LANDMARK:
+      sensor_id.type =
+          TrajectoryBuilderInterface::SensorId::SensorType::LANDMARK;
+      break;
+    case proto::SensorId::LOCAL_SLAM_RESULT:
+      sensor_id.type =
+          TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT;
+      break;
+    default:
+      LOG(FATAL) << "Unsupported sensor type.";
+  }
+  sensor_id.id = sensor_id_proto.id();
+  return sensor_id;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/trajectory_builder_interface.h

@@ -112,6 +112,10 @@ class TrajectoryBuilderInterface {
       std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
 };
 
+proto::SensorId ToProto(const TrajectoryBuilderInterface::SensorId& sensor_id);
+TrajectoryBuilderInterface::SensorId FromProto(
+    const proto::SensorId& sensor_id_proto);
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer_grpc/mapping/map_builder_stub.cc

@@ -59,7 +59,9 @@ int MapBuilderStub::AddTrajectoryBuilder(
   return client.response().trajectory_id();
 }
 
-int MapBuilderStub::AddTrajectoryForDeserialization() {
+int MapBuilderStub::AddTrajectoryForDeserialization(
+    const cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds&
+        options_with_sensor_ids_proto) {
   LOG(FATAL) << "Not implemented";
 }
 
@@ -128,5 +130,11 @@ cartographer::mapping::PoseGraphInterface* MapBuilderStub::pose_graph() {
   return &pose_graph_stub_;
 }
 
+const std::vector<
+    cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
+MapBuilderStub::GetAllTrajectoryBuilderOptions() const {
+  LOG(FATAL) << "Not implemented";
+}
+
 }  // namespace mapping
 }  // namespace cartographer_grpc

cartographer_grpc/mapping/map_builder_stub.h

@@ -39,7 +39,9 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
       const cartographer::mapping::proto::TrajectoryBuilderOptions&
           trajectory_options,
       LocalSlamResultCallback local_slam_result_callback) override;
-  int AddTrajectoryForDeserialization() override;
+  int AddTrajectoryForDeserialization(
+      const cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds&
+          options_with_sensor_ids_proto) override;
   cartographer::mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
       int trajectory_id) const override;
   void FinishTrajectory(int trajectory_id) override;
@@ -51,6 +53,9 @@ class MapBuilderStub : public cartographer::mapping::MapBuilderInterface {
   void LoadMap(cartographer::io::ProtoStreamReaderInterface* reader) override;
   int num_trajectory_builders() const override;
   cartographer::mapping::PoseGraphInterface* pose_graph() override;
+  const std::vector<
+      cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&
+  GetAllTrajectoryBuilderOptions() const override;
 
  private:
   std::shared_ptr<grpc::Channel> client_channel_;

cartographer_grpc/testing/mock_map_builder.h

@@ -37,7 +37,10 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
               &trajectory_options,
           cartographer::mapping::MapBuilderInterface::LocalSlamResultCallback
               local_slam_result_callback));
-  MOCK_METHOD0(AddTrajectoryForDeserialization, int());
+  MOCK_METHOD1(AddTrajectoryForDeserialization,
+               int(const cartographer::mapping::proto::
+                       TrajectoryBuilderOptionsWithSensorIds
+                           &options_with_sensor_ids_proto));
   MOCK_CONST_METHOD1(
       GetTrajectoryBuilder,
       cartographer::mapping::TrajectoryBuilderInterface *(int trajectory_id));
@@ -51,6 +54,11 @@ class MockMapBuilder : public cartographer::mapping::MapBuilderInterface {
   MOCK_METHOD1(LoadMap, void(cartographer::io::ProtoStreamReaderInterface *));
   MOCK_CONST_METHOD0(num_trajectory_builders, int());
   MOCK_METHOD0(pose_graph, cartographer::mapping::PoseGraphInterface *());
+  MOCK_CONST_METHOD0(
+      GetAllTrajectoryBuilderOptions,
+      const std::vector<
+          cartographer::mapping::proto::TrajectoryBuilderOptionsWithSensorIds>
+          &());
 };
 
 }  // namespace testing