Serialize landmark observations. (#1006)

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

@@ -635,6 +635,12 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
   return optimization_problem_->odometry_data();
 }
 
+std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+PoseGraph2D::GetLandmarkNodes() {
+  common::MutexLocker locker(&mutex_);
+  return landmark_nodes_;
+}
+
 std::map<int, PoseGraphInterface::TrajectoryData>
 PoseGraph2D::GetTrajectoryData() {
   return {};  // Not implemented yet in 2D.

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

@@ -126,6 +126,8 @@ class PoseGraph2D : public PoseGraph {
       EXCLUDES(mutex_);
   sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
       EXCLUDES(mutex_);
+  std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+  GetLandmarkNodes() override EXCLUDES(mutex_);
   std::map<int, TrajectoryData> GetTrajectoryData() override EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
   void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,

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

@@ -684,6 +684,12 @@ PoseGraph3D::GetFixedFramePoseData() {
   return optimization_problem_->fixed_frame_pose_data();
 }
 
+std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+PoseGraph3D::GetLandmarkNodes() {
+  common::MutexLocker locker(&mutex_);
+  return landmark_nodes_;
+}
+
 std::map<int, PoseGraphInterface::TrajectoryData>
 PoseGraph3D::GetTrajectoryData() {
   common::MutexLocker locker(&mutex_);

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

@@ -125,6 +125,8 @@ class PoseGraph3D : public PoseGraph {
       EXCLUDES(mutex_);
   sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
       EXCLUDES(mutex_);
+  std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+  GetLandmarkNodes() override EXCLUDES(mutex_);
   std::map<int, TrajectoryData> GetTrajectoryData() override;
 
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);

cartographer/mapping/map_builder.cc

@@ -286,7 +286,29 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
       writer->WriteProto(proto);
     }
   }
-  // TODO(pifon2a, ojura): serialize landmarks
+  // Next we serialize all landmark data.
+  {
+    const std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+        all_landmark_nodes = pose_graph_->GetLandmarkNodes();
+    for (const auto& node : all_landmark_nodes) {
+      for (const auto& observation : node.second.landmark_observations) {
+        proto::SerializedData proto;
+        auto* landmark_data_proto = proto.mutable_landmark_data();
+        landmark_data_proto->set_trajectory_id(observation.trajectory_id);
+        landmark_data_proto->mutable_landmark_data()->set_timestamp(
+            common::ToUniversal(observation.time));
+        auto* observation_proto = landmark_data_proto->mutable_landmark_data()
+                                      ->add_landmark_observations();
+        observation_proto->set_id(node.first);
+        *observation_proto->mutable_landmark_to_tracking_transform() =
+            transform::ToProto(observation.landmark_to_tracking_transform);
+        observation_proto->set_translation_weight(
+            observation.translation_weight);
+        observation_proto->set_rotation_weight(observation.rotation_weight);
+        writer->WriteProto(proto);
+      }
+    }
+  }
 }
 
 void MapBuilder::LoadState(io::ProtoStreamReaderInterface* const reader,

cartographer/mapping/pose_graph.h

@@ -129,6 +129,10 @@ class PoseGraph : public PoseGraphInterface {
   virtual sensor::MapByTime<sensor::FixedFramePoseData>
   GetFixedFramePoseData() = 0;
 
+  // Returns the landmark data.
+  virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+  GetLandmarkNodes() = 0;
+
   // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
   // respect to 'to_trajectory_id' at time 'time'.
   virtual void SetInitialTrajectoryPose(int from_trajectory_id,