diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index adbaed5..bd39221 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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. diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index 33bc11d..78b56c3 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/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. } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 6a381c2..6f0cd32 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/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 GetImuData() = 0; + // Returns the collection of constraints. virtual std::vector constraints() = 0; diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 94903e9..f8b075c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -544,6 +544,11 @@ SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +sensor::MapByTime SparsePoseGraph::GetImuData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.imu_data(); +} + std::vector SparsePoseGraph::constraints() { std::vector result; common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 8fb99b3..2286482 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& 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 GetTrajectoryNodes() override EXCLUDES(mutex_); + sensor::MapByTime GetImuData() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, const transform::Rigid3d& pose, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 3844c7c..1dfda4c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -238,6 +238,11 @@ OptimizationProblem::submap_data() const { return submap_data_; } +const sensor::MapByTime& OptimizationProblem::imu_data() + const { + return imu_data_; +} + } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 9a3e3e2..834ced0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -88,6 +88,7 @@ class OptimizationProblem { const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; + const sensor::MapByTime& imu_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f3729d5..b864d21 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -573,6 +573,11 @@ SparsePoseGraph::GetTrajectoryNodes() { return trajectory_nodes_; } +sensor::MapByTime SparsePoseGraph::GetImuData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.imu_data(); +} + std::vector SparsePoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 9ef7d80..88b1671 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& 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 GetTrajectoryNodes() override EXCLUDES(mutex_); + sensor::MapByTime GetImuData() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, const transform::Rigid3d& pose, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 9597e11..5292029 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -438,6 +438,11 @@ OptimizationProblem::submap_data() const { return submap_data_; } +const sensor::MapByTime& OptimizationProblem::imu_data() + const { + return imu_data_; +} + } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index c3a9d77..92c0ceb 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -92,6 +92,7 @@ class OptimizationProblem { const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; + const sensor::MapByTime& imu_data() const; private: struct TrajectoryData {