Serialize fixed frame pose data. (#689)

Similar to #666 and #548.

cartographer/mapping/map_builder.cc

@@ -215,7 +215,22 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
       }
     }
   }
-  // TODO(whess): Serialize additional sensor data: fixed frame pose data.
+  // Next we serialize all fixed frame pose data from the pose graph.
+  {
+    const auto all_fixed_frame_pose_data = pose_graph_->GetFixedFramePoseData();
+    for (const int trajectory_id : all_fixed_frame_pose_data.trajectory_ids()) {
+      for (const auto& fixed_frame_pose_data :
+           all_fixed_frame_pose_data.trajectory(trajectory_id)) {
+        proto::SerializedData proto;
+        auto* const fixed_frame_pose_data_proto =
+            proto.mutable_fixed_frame_pose_data();
+        fixed_frame_pose_data_proto->set_trajectory_id(trajectory_id);
+        *fixed_frame_pose_data_proto->mutable_fixed_frame_pose_data() =
+            sensor::ToProto(fixed_frame_pose_data);
+        writer->WriteProto(proto);
+      }
+    }
+  }
 }
 
 void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
@@ -273,8 +288,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
                                    proto.submap().submap_id().submap_index()});
       pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
     }
-    // TODO(ojura): Deserialize IMU and odometry data when loading unfrozen
-    // trajectories.
+    // TODO(ojura): Deserialize IMU, odometry and fixed frame pose data when
+    // loading unfrozen trajectories.
   }
 
   // Add information about which nodes belong to which submap.

cartographer/mapping/pose_graph.h

@@ -31,6 +31,7 @@
 #include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/sensor/fixed_frame_pose_data.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/map_by_time.h"
 #include "cartographer/sensor/odometry_data.h"
@@ -91,6 +92,11 @@ class PoseGraph {
   virtual void AddOdometryData(int trajectory_id,
                                const sensor::OdometryData& odometry_data) = 0;
 
+  // Inserts a fixed frame pose measurement.
+  virtual void AddFixedFramePoseData(
+      int trajectory_id,
+      const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0;
+
   // Finishes the given trajectory.
   virtual void FinishTrajectory(int trajectory_id) = 0;
 
@@ -152,6 +158,10 @@ class PoseGraph {
   // Returns the odometry data.
   virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0;
 
+  // Returns the fixed frame pose data.
+  virtual sensor::MapByTime<sensor::FixedFramePoseData>
+  GetFixedFramePoseData() = 0;
+
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() = 0;
 

cartographer/mapping/proto/serialization.proto

@@ -42,10 +42,15 @@ message OdometryData {
   sensor.proto.OdometryData odometry_data = 2;
 }
 
+message FixedFramePoseData {
+  int32 trajectory_id = 1;
+  sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
+}
+
 message SerializedData {
   Submap submap = 1;
   Node node = 2;
   ImuData imu_data = 3;
   OdometryData odometry_data = 4;
-  // TODO(whess): Add fixed frame pose data.
+  FixedFramePoseData fixed_frame_pose_data = 5;
 }

cartographer/mapping_2d/pose_graph.cc

@@ -555,6 +555,11 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
   return optimization_problem_.odometry_data();
 }
 
+sensor::MapByTime<sensor::FixedFramePoseData>
+PoseGraph::GetFixedFramePoseData() {
+  return {};  // Not implemented yet in 2D.
+}
+
 std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
   std::vector<Constraint> result;
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/pose_graph.h

@@ -83,7 +83,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
   void AddFixedFramePoseData(
       int trajectory_id,
-      const sensor::FixedFramePoseData& fixed_frame_pose_data);
+      const sensor::FixedFramePoseData& fixed_frame_pose_data) override
+      EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
@@ -109,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
+      EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
   void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
                                 const transform::Rigid3d& pose,

cartographer/mapping_3d/pose_graph.cc

@@ -584,6 +584,12 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
   return optimization_problem_.odometry_data();
 }
 
+sensor::MapByTime<sensor::FixedFramePoseData>
+PoseGraph::GetFixedFramePoseData() {
+  common::MutexLocker locker(&mutex_);
+  return optimization_problem_.fixed_frame_pose_data();
+}
+
 std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_3d/pose_graph.h

@@ -83,7 +83,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
   void AddFixedFramePoseData(
       int trajectory_id,
-      const sensor::FixedFramePoseData& fixed_frame_pose_data);
+      const sensor::FixedFramePoseData& fixed_frame_pose_data) override
+      EXCLUDES(mutex_);
 
   void FinishTrajectory(int trajectory_id) override;
   void FreezeTrajectory(int trajectory_id) override;
@@ -109,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
   sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
   sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
       EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
+      EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
   void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
                                 const transform::Rigid3d& pose,

cartographer/mapping_3d/pose_graph/optimization_problem.cc

@@ -469,6 +469,11 @@ OptimizationProblem::odometry_data() const {
   return odometry_data_;
 }
 
+const sensor::MapByTime<sensor::FixedFramePoseData>&
+OptimizationProblem::fixed_frame_pose_data() const {
+  return fixed_frame_pose_data_;
+}
+
 transform::Rigid3d OptimizationProblem::ComputeRelativePose(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {

cartographer/mapping_3d/pose_graph/optimization_problem.h

@@ -94,6 +94,8 @@ class OptimizationProblem {
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
   const sensor::MapByTime<sensor::ImuData>& imu_data() const;
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
+  const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
+      const;
 
  private:
   // Uses odometry if available, otherwise the local SLAM results.