(De)serialize trajectory data from the optimization problem (#915)

* Write/Read the trajectory data (gravity, imu calibration, and fixed frame origin) into the serialized state proto

cartographer/mapping/map_builder.cc

@@ -266,6 +266,28 @@ void MapBuilder::SerializeState(io::ProtoStreamWriterInterface* const writer) {
       }
     }
   }
+  // Next we serialize all trajectory data.
+  {
+    const auto all_trajectory_data = pose_graph_->GetTrajectoryData();
+    for (const auto& trajectory_data : all_trajectory_data) {
+      proto::SerializedData proto;
+      auto* const trajectory_data_proto = proto.mutable_trajectory_data();
+      trajectory_data_proto->set_trajectory_id(trajectory_data.first);
+      trajectory_data_proto->set_gravity_constant(
+          trajectory_data.second.gravity_constant);
+      *trajectory_data_proto->mutable_imu_calibration() = transform::ToProto(
+          Eigen::Quaterniond(trajectory_data.second.imu_calibration[0],
+                             trajectory_data.second.imu_calibration[1],
+                             trajectory_data.second.imu_calibration[2],
+                             trajectory_data.second.imu_calibration[3]));
+      if (trajectory_data.second.fixed_frame_origin_in_map.has_value()) {
+        *trajectory_data_proto->mutable_fixed_frame_origin_in_map() =
+            transform::ToProto(
+                trajectory_data.second.fixed_frame_origin_in_map.value());
+      }
+      writer->WriteProto(proto);
+    }
+  }
 }
 
 void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
@@ -333,6 +355,11 @@ void MapBuilder::LoadMap(io::ProtoStreamReaderInterface* const reader) {
                                    proto.submap().submap_id().submap_index()});
       pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
     }
+    if (proto.has_trajectory_data()) {
+      proto.mutable_trajectory_data()->set_trajectory_id(
+          trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
+      pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
+    }
     // TODO(ojura): Deserialize IMU, odometry and fixed frame pose data when
     // loading unfrozen trajectories.
   }

cartographer/mapping/pose_graph.h

@@ -91,6 +91,10 @@ class PoseGraph : public PoseGraphInterface {
   virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
                                 const proto::Node& node) = 0;
 
+  // Sets the trajectory data from a proto.
+  virtual void SetTrajectoryDataFromProto(
+      const mapping::proto::TrajectoryData& data) = 0;
+
   // Adds information that 'node_id' was inserted into 'submap_id'. The submap
   // has to be deserialized first.
   virtual void AddNodeToSubmap(const NodeId& node_id,
@@ -121,6 +125,9 @@ class PoseGraph : public PoseGraphInterface {
   // Returns the odometry data.
   virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0;
 
+  // Returns the trajectory data.
+  virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0;
+
   // Returns the fixed frame pose data.
   virtual sensor::MapByTime<sensor::FixedFramePoseData>
   GetFixedFramePoseData() = 0;

cartographer/mapping/pose_graph_interface.h

@@ -73,6 +73,12 @@ class PoseGraphInterface {
     transform::Rigid3d pose;
   };
 
+  struct TrajectoryData {
+    double gravity_constant = 9.8;
+    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
+    common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
+  };
+
   PoseGraphInterface() {}
   virtual ~PoseGraphInterface() {}
 

cartographer/mapping/proto/serialization.proto

@@ -20,6 +20,7 @@ import "cartographer/mapping/proto/pose_graph.proto";
 import "cartographer/mapping/proto/submap.proto";
 import "cartographer/mapping/proto/trajectory_node_data.proto";
 import "cartographer/sensor/proto/sensor.proto";
+import "cartographer/transform/proto/transform.proto";
 
 message Submap {
   SubmapId submap_id = 1;
@@ -47,12 +48,20 @@ message FixedFramePoseData {
   sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
 }
 
+message TrajectoryData {
+  int32 trajectory_id = 1;
+  double gravity_constant = 2;
+  transform.proto.Quaterniond imu_calibration = 3;
+  transform.proto.Rigid3d fixed_frame_origin_in_map = 4;
+}
+
 message SerializedData {
   Submap submap = 1;
   Node node = 2;
   ImuData imu_data = 3;
   OdometryData odometry_data = 4;
   FixedFramePoseData fixed_frame_pose_data = 5;
+  TrajectoryData trajectory_data = 6;
 }
 
 message LocalSlamResultData {

cartographer/mapping_2d/pose_graph.cc

@@ -474,6 +474,11 @@ void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
   });
 }
 
+void PoseGraph::SetTrajectoryDataFromProto(
+    const mapping::proto::TrajectoryData& data) {
+  // Not implemented yet in 2D.
+}
+
 void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
                                 const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
@@ -629,6 +634,11 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
   return optimization_problem_.odometry_data();
 }
 
+std::map<int, mapping::PoseGraphInterface::TrajectoryData>
+PoseGraph::GetTrajectoryData() {
+  return {};  // Not implemented yet in 2D.
+}
+
 sensor::MapByTime<sensor::FixedFramePoseData>
 PoseGraph::GetFixedFramePoseData() {
   return {};  // Not implemented yet in 2D.

cartographer/mapping_2d/pose_graph.h

@@ -97,6 +97,8 @@ class PoseGraph : public mapping::PoseGraph {
                           const mapping::proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
+  void SetTrajectoryDataFromProto(
+      const mapping::proto::TrajectoryData& data) override;
   void AddNodeToSubmap(const mapping::NodeId& node_id,
                        const mapping::SubmapId& submap_id) override;
   void AddSerializedConstraints(
@@ -123,6 +125,7 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
   sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() 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,
                                 const transform::Rigid3d& pose,

cartographer/mapping_3d/pose_graph.cc

@@ -33,6 +33,7 @@
 #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
 #include "cartographer/sensor/voxel_filter.h"
+#include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -484,6 +485,25 @@ void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
   });
 }
 
+void PoseGraph::SetTrajectoryDataFromProto(
+    const mapping::proto::TrajectoryData& data) {
+  TrajectoryData trajectory_data;
+  trajectory_data.gravity_constant = data.gravity_constant();
+  trajectory_data.imu_calibration = {
+      {data.imu_calibration().w(), data.imu_calibration().x(),
+       data.imu_calibration().y(), data.imu_calibration().z()}};
+  if (data.has_fixed_frame_origin_in_map()) {
+    trajectory_data.fixed_frame_origin_in_map =
+        transform::ToRigid3(data.fixed_frame_origin_in_map());
+  }
+
+  const int trajectory_id = data.trajectory_id();
+  common::MutexLocker locker(&mutex_);
+  AddWorkItem([this, trajectory_id, trajectory_data]() REQUIRES(mutex_) {
+    optimization_problem_.SetTrajectoryData(trajectory_id, trajectory_data);
+  });
+}
+
 void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
                                 const mapping::SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
@@ -665,6 +685,12 @@ PoseGraph::GetFixedFramePoseData() {
   return optimization_problem_.fixed_frame_pose_data();
 }
 
+std::map<int, mapping::PoseGraphInterface::TrajectoryData>
+PoseGraph::GetTrajectoryData() {
+  common::MutexLocker locker(&mutex_);
+  return optimization_problem_.trajectory_data();
+}
+
 std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_3d/pose_graph.h

@@ -96,6 +96,8 @@ class PoseGraph : public mapping::PoseGraph {
                           const mapping::proto::Submap& submap) override;
   void AddNodeFromProto(const transform::Rigid3d& global_pose,
                         const mapping::proto::Node& node) override;
+  void SetTrajectoryDataFromProto(
+      const mapping::proto::TrajectoryData& data) override;
   void AddNodeToSubmap(const mapping::NodeId& node_id,
                        const mapping::SubmapId& submap_id) override;
   void AddSerializedConstraints(
@@ -122,6 +124,8 @@ class PoseGraph : public mapping::PoseGraph {
       EXCLUDES(mutex_);
   sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
       EXCLUDES(mutex_);
+  std::map<int, TrajectoryData> GetTrajectoryData() override;
+
   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

@@ -51,6 +51,8 @@ namespace {
 
 using ::cartographer::mapping::pose_graph::CeresPose;
 using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
+using TrajectoryData =
+    ::cartographer::mapping::PoseGraphInterface::TrajectoryData;
 
 // For odometry.
 std::unique_ptr<transform::Rigid3d> Interpolate(
@@ -195,6 +197,11 @@ void OptimizationProblem::AddTrajectoryNode(
   trajectory_data_[trajectory_id];
 }
 
+void OptimizationProblem::SetTrajectoryData(
+    int trajectory_id, const TrajectoryData& trajectory_data) {
+  trajectory_data_[trajectory_id] = trajectory_data;
+}
+
 void OptimizationProblem::InsertTrajectoryNode(
     const mapping::NodeId& node_id, const common::Time time,
     const transform::Rigid3d& local_pose,
@@ -475,8 +482,9 @@ void OptimizationProblem::Solve(
 
       if (!fixed_frame_pose_initialized) {
         transform::Rigid3d fixed_frame_pose_in_map;
-        if (trajectory_data.fixed_frame.has_value()) {
+        if (trajectory_data.fixed_frame_origin_in_map.has_value()) {
-          fixed_frame_pose_in_map = trajectory_data.fixed_frame.value();
+          fixed_frame_pose_in_map =
+              trajectory_data.fixed_frame_origin_in_map.value();
         } else {
           fixed_frame_pose_in_map =
               node_data.global_pose * constraint_pose.zbar_ij.inverse();
@@ -537,7 +545,7 @@ void OptimizationProblem::Solve(
         C_node_id_data.data.ToRigid();
   }
   for (const auto& C_fixed_frame : C_fixed_frames) {
-    trajectory_data_.at(C_fixed_frame.first).fixed_frame =
+    trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
         C_fixed_frame.second.ToRigid();
   }
   for (const auto& C_landmark : C_landmarks) {
@@ -575,6 +583,11 @@ OptimizationProblem::fixed_frame_pose_data() const {
   return fixed_frame_pose_data_;
 }
 
+const std::map<int, TrajectoryData>& OptimizationProblem::trajectory_data()
+    const {
+  return trajectory_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

@@ -75,6 +75,9 @@ class OptimizationProblem {
   void AddTrajectoryNode(int trajectory_id, common::Time time,
                          const transform::Rigid3d& local_pose,
                          const transform::Rigid3d& global_pose);
+  void SetTrajectoryData(
+      int trajectory_id,
+      const mapping::PoseGraphInterface::TrajectoryData& trajectory_data);
   void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
                             const transform::Rigid3d& local_pose,
                             const transform::Rigid3d& global_pose);
@@ -99,6 +102,8 @@ class OptimizationProblem {
   const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
   const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
       const;
+  const std::map<int, mapping::PoseGraphInterface::TrajectoryData>&
+  trajectory_data() const;
 
  private:
   // Uses odometry if available, otherwise the local SLAM results.
@@ -106,12 +111,6 @@ class OptimizationProblem {
       int trajectory_id, const NodeData& first_node_data,
       const NodeData& second_node_data) const;
 
-  struct TrajectoryData {
-    double gravity_constant = 9.8;
-    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
-    common::optional<transform::Rigid3d> fixed_frame;
-  };
-
   mapping::pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
   mapping::MapById<mapping::NodeId, NodeData> node_data_;
@@ -120,7 +119,7 @@ class OptimizationProblem {
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
   sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
-  std::map<int, TrajectoryData> trajectory_data_;
+  std::map<int, mapping::PoseGraphInterface::TrajectoryData> trajectory_data_;
 };
 
 }  // namespace pose_graph