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; }