From 95dbcfdf9e1b8817500e1276d3067999732e8e0f Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Wed, 7 Feb 2018 10:47:39 +0100
Subject: [PATCH] Serialize landmarks. (#889)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
---
 cartographer/mapping/pose_graph.cc          | 7 +++++++
 cartographer/mapping/proto/pose_graph.proto | 6 ++++++
 2 files changed, 13 insertions(+)

diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc
index d36c93a..65bda7f 100644
--- a/cartographer/mapping/pose_graph.cc
+++ b/cartographer/mapping/pose_graph.cc
@@ -154,6 +154,13 @@ proto::PoseGraph PoseGraph::ToProto() {
     *proto.add_constraint() = cartographer::mapping::ToProto(constraint);
   }
 
+  auto landmarks_copy = GetLandmarkPoses();
+  proto.mutable_landmarks()->Reserve(landmarks_copy.size());
+  for (const auto& id_pose : landmarks_copy) {
+    auto* landmark_proto = proto.add_landmarks();
+    landmark_proto->set_landmark_id(id_pose.first);
+    *landmark_proto->mutable_global_pose() = transform::ToProto(id_pose.second);
+  }
   return proto;
 }
 
diff --git a/cartographer/mapping/proto/pose_graph.proto b/cartographer/mapping/proto/pose_graph.proto
index 4b4bd58..d34ee1d 100644
--- a/cartographer/mapping/proto/pose_graph.proto
+++ b/cartographer/mapping/proto/pose_graph.proto
@@ -51,6 +51,12 @@ message PoseGraph {
     Tag tag = 5;
   }
 
+  message Landmark {
+    string landmark_id = 1;
+    transform.proto.Rigid3d global_pose = 2;
+  }
+
   repeated Constraint constraint = 2;
   repeated Trajectory trajectory = 4;
+  repeated Landmark landmarks = 5;
 }