Serialize IMU data. (#548)

cartographer/mapping/map_builder.cc

@@ -179,8 +179,21 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
           ToProto(*node_id_data.data.constant_data);
       writer->WriteProto(proto);
     }
-    // TODO(whess): Serialize additional sensor data: IMU, odometry.
   }
+  // Next we serialize IMU data from the pose graph.
+  {
+    const auto all_imu_data = sparse_pose_graph_->GetImuData();
+    for (const int trajectory_id : all_imu_data.trajectory_ids()) {
+      for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) {
+        proto::SerializedData proto;
+        auto* const imu_data_proto = proto.mutable_imu_data();
+        imu_data_proto->set_trajectory_id(trajectory_id);
+        *imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data);
+        writer->WriteProto(proto);
+      }
+    }
+  }
+  // TODO(whess): Serialize additional sensor data: odometry.
 }
 
 void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
@@ -238,6 +251,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
                                    proto.submap().submap_id().submap_index()});
       sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
     }
+    // TODO(ojura): Deserialize IMU data when loading unfrozen trajectories.
   }
 
   // Add information about which nodes belong to which submap.

cartographer/mapping/proto/serialization.proto

@@ -19,6 +19,7 @@ package cartographer.mapping.proto;
 import "cartographer/mapping/proto/sparse_pose_graph.proto";
 import "cartographer/mapping/proto/submap.proto";
 import "cartographer/mapping/proto/trajectory_node_data.proto";
+import "cartographer/sensor/proto/sensor.proto";
 
 message Submap {
   optional SubmapId submap_id = 1;
@@ -31,8 +32,14 @@ message Node {
   optional TrajectoryNodeData node_data = 5;
 }
 
+message ImuData {
+  optional int32 trajectory_id = 1;
+  optional sensor.proto.ImuData imu_data = 2;
+}
+
 message SerializedData {
   optional Submap submap = 1;
   optional Node node = 2;
-  // TODO(whess): Add IMU data, odometry.
+  optional ImuData imu_data = 3;
+  // TODO(whess): Add odometry.
 }

cartographer/mapping/sparse_pose_graph.h

@@ -31,6 +31,8 @@
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping/trajectory_node.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/map_by_time.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
@@ -80,6 +82,10 @@ class SparsePoseGraph {
   SparsePoseGraph(const SparsePoseGraph&) = delete;
   SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
 
+  // Inserts an IMU measurement.
+  virtual void AddImuData(int trajectory_id,
+                          const sensor::ImuData& imu_data) = 0;
+
   // Finishes the given trajectory.
   virtual void FinishTrajectory(int trajectory_id) = 0;
 
@@ -135,6 +141,9 @@ class SparsePoseGraph {
   // Serializes the constraints and trajectories.
   proto::SparsePoseGraph ToProto();
 
+  // Returns the IMU data.
+  virtual sensor::MapByTime<sensor::ImuData> GetImuData() = 0;
+
   // Returns the collection of constraints.
   virtual std::vector<Constraint> constraints() = 0;
 

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -544,6 +544,11 @@ SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
+  common::MutexLocker locker(&mutex_);
+  return optimization_problem_.imu_data();
+}
+
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   std::vector<Constraint> result;
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
+      EXCLUDES(mutex_);
   void AddOdometerData(int trajectory_id,
                        const sensor::OdometryData& odometry_data);
   void AddFixedFramePoseData(
@@ -104,6 +105,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::ImuData> GetImuData() 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_2d/sparse_pose_graph/optimization_problem.cc

@@ -238,6 +238,11 @@ OptimizationProblem::submap_data() const {
   return submap_data_;
 }
 
+const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
+    const {
+  return imu_data_;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_2d
 }  // namespace cartographer

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -88,6 +88,7 @@ class OptimizationProblem {
 
   const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
+  const sensor::MapByTime<sensor::ImuData>& imu_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -573,6 +573,11 @@ SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_;
 }
 
+sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
+  common::MutexLocker locker(&mutex_);
+  return optimization_problem_.imu_data();
+}
+
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_3d/sparse_pose_graph.h

@@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
+      EXCLUDES(mutex_);
   void AddOdometerData(int trajectory_id,
                        const sensor::OdometryData& odometry_data);
   void AddFixedFramePoseData(
@@ -104,6 +105,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
+  sensor::MapByTime<sensor::ImuData> GetImuData() 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/sparse_pose_graph/optimization_problem.cc

@@ -438,6 +438,11 @@ OptimizationProblem::submap_data() const {
   return submap_data_;
 }
 
+const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
+    const {
+  return imu_data_;
+}
+
 }  // namespace sparse_pose_graph
 }  // namespace mapping_3d
 }  // namespace cartographer

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -92,6 +92,7 @@ class OptimizationProblem {
 
   const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
   const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
+  const sensor::MapByTime<sensor::ImuData>& imu_data() const;
 
  private:
   struct TrajectoryData {