From 5496cbdc0c70b09818e9ac3c047b69a23272649d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 14 Nov 2017 15:18:39 +0100 Subject: [PATCH] Serialize odometry data (#666) Replaces #550. --- cartographer/mapping/map_builder.cc | 20 +++++++++++++++++-- .../mapping/proto/serialization.proto | 8 +++++++- cartographer/mapping/sparse_pose_graph.h | 8 ++++++++ cartographer/mapping_2d/sparse_pose_graph.cc | 5 +++++ cartographer/mapping_2d/sparse_pose_graph.h | 5 ++++- .../sparse_pose_graph/optimization_problem.cc | 5 +++++ .../sparse_pose_graph/optimization_problem.h | 1 + cartographer/mapping_3d/sparse_pose_graph.cc | 5 +++++ cartographer/mapping_3d/sparse_pose_graph.h | 5 ++++- .../sparse_pose_graph/optimization_problem.cc | 5 +++++ .../sparse_pose_graph/optimization_problem.h | 1 + 11 files changed, 63 insertions(+), 5 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index bd39221..975d82f 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -193,7 +193,22 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } } } - // TODO(whess): Serialize additional sensor data: odometry. + // Next we serialize odometry data from the pose graph. + { + const auto all_odometry_data = sparse_pose_graph_->GetOdometryData(); + for (const int trajectory_id : all_odometry_data.trajectory_ids()) { + for (const auto& odometry_data : + all_odometry_data.trajectory(trajectory_id)) { + proto::SerializedData proto; + auto* const odometry_data_proto = proto.mutable_odometry_data(); + odometry_data_proto->set_trajectory_id(trajectory_id); + *odometry_data_proto->mutable_odometry_data() = + sensor::ToProto(odometry_data); + writer->WriteProto(proto); + } + } + } + // TODO(whess): Serialize additional sensor data: fixed frame pose data. } void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { @@ -251,7 +266,8 @@ 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. + // TODO(ojura): Deserialize IMU and odometry 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 73c0a10..fb5936f 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/cartographer/mapping/proto/serialization.proto @@ -37,9 +37,15 @@ message ImuData { sensor.proto.ImuData imu_data = 2; } +message OdometryData { + int32 trajectory_id = 1; + sensor.proto.OdometryData odometry_data = 2; +} + message SerializedData { Submap submap = 1; Node node = 2; ImuData imu_data = 3; - // TODO(whess): Add odometry. + OdometryData odometry_data = 4; + // TODO(whess): Add fixed frame pose data. } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 6f0cd32..2111cf1 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -33,6 +33,7 @@ #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/map_by_time.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -86,6 +87,10 @@ class SparsePoseGraph { virtual void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) = 0; + // Inserts an odometry measurement. + virtual void AddOdometerData(int trajectory_id, + const sensor::OdometryData& odometry_data) = 0; + // Finishes the given trajectory. virtual void FinishTrajectory(int trajectory_id) = 0; @@ -144,6 +149,9 @@ class SparsePoseGraph { // Returns the IMU data. virtual sensor::MapByTime GetImuData() = 0; + // Returns the odometry data. + virtual sensor::MapByTime GetOdometryData() = 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 6d35878..84dd32b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -557,6 +557,11 @@ sensor::MapByTime SparsePoseGraph::GetImuData() { return optimization_problem_.imu_data(); } +sensor::MapByTime SparsePoseGraph::GetOdometryData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.odometry_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 35543ac..4854838 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override EXCLUDES(mutex_); void AddOdometerData(int trajectory_id, - const sensor::OdometryData& odometry_data); + const sensor::OdometryData& odometry_data) override + EXCLUDES(mutex_); void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); @@ -106,6 +107,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); + sensor::MapByTime GetOdometryData() 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 5073b75..f592b5b 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -232,6 +232,11 @@ const sensor::MapByTime& OptimizationProblem::imu_data() return imu_data_; } +const sensor::MapByTime& +OptimizationProblem::odometry_data() const { + return odometry_data_; +} + std::unique_ptr OptimizationProblem::InterpolateOdometry( const int trajectory_id, const common::Time time) const { const auto it = odometry_data_.lower_bound(trajectory_id, time); diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 2859a17..5caa48f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -90,6 +90,7 @@ class OptimizationProblem { const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; const sensor::MapByTime& imu_data() const; + const sensor::MapByTime& odometry_data() const; private: std::unique_ptr InterpolateOdometry( diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 8d4d367..8038817 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -583,6 +583,11 @@ sensor::MapByTime SparsePoseGraph::GetImuData() { return optimization_problem_.imu_data(); } +sensor::MapByTime SparsePoseGraph::GetOdometryData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.odometry_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 b1e3433..bc749ec 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override EXCLUDES(mutex_); void AddOdometerData(int trajectory_id, - const sensor::OdometryData& odometry_data); + const sensor::OdometryData& odometry_data) override + EXCLUDES(mutex_); void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); @@ -106,6 +107,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); sensor::MapByTime GetImuData() override EXCLUDES(mutex_); + sensor::MapByTime GetOdometryData() 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 f0c5c8d..1006972 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -465,6 +465,11 @@ const sensor::MapByTime& OptimizationProblem::imu_data() return imu_data_; } +const sensor::MapByTime& +OptimizationProblem::odometry_data() const { + return odometry_data_; +} + transform::Rigid3d OptimizationProblem::ComputeRelativePose( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 6b1ffeb..ef90bb6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -94,6 +94,7 @@ class OptimizationProblem { const mapping::MapById& node_data() const; const mapping::MapById& submap_data() const; const sensor::MapByTime& imu_data() const; + const sensor::MapByTime& odometry_data() const; private: // Uses odometry if available, otherwise the local SLAM results.