From bebe021b048549314970ef4692effd97ebf43751 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 28 Nov 2017 09:44:35 +0100 Subject: [PATCH] Serialize fixed frame pose data. (#689) Similar to #666 and #548. --- cartographer/mapping/map_builder.cc | 21 ++++++++++++++++--- cartographer/mapping/pose_graph.h | 10 +++++++++ .../mapping/proto/serialization.proto | 7 ++++++- cartographer/mapping_2d/pose_graph.cc | 5 +++++ cartographer/mapping_2d/pose_graph.h | 5 ++++- cartographer/mapping_3d/pose_graph.cc | 6 ++++++ cartographer/mapping_3d/pose_graph.h | 5 ++++- .../pose_graph/optimization_problem.cc | 5 +++++ .../pose_graph/optimization_problem.h | 2 ++ 9 files changed, 60 insertions(+), 6 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index e5ab746..cc362ae 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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. diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index 0fff128..33b3108 100644 --- a/cartographer/mapping/pose_graph.h +++ b/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 GetOdometryData() = 0; + // Returns the fixed frame pose data. + virtual sensor::MapByTime + GetFixedFramePoseData() = 0; + // Returns the collection of constraints. virtual std::vector constraints() = 0; diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index dd1a796..f3fe698 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/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; } diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index 4c6efe4..60ac1ff 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/cartographer/mapping_2d/pose_graph.cc @@ -555,6 +555,11 @@ sensor::MapByTime PoseGraph::GetOdometryData() { return optimization_problem_.odometry_data(); } +sensor::MapByTime +PoseGraph::GetFixedFramePoseData() { + return {}; // Not implemented yet in 2D. +} + std::vector PoseGraph::constraints() { std::vector result; common::MutexLocker locker(&mutex_); diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index 35edf19..6d500e1 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/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 GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); + sensor::MapByTime GetFixedFramePoseData() 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/pose_graph.cc b/cartographer/mapping_3d/pose_graph.cc index 5bf9219..0a070b3 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/cartographer/mapping_3d/pose_graph.cc @@ -584,6 +584,12 @@ sensor::MapByTime PoseGraph::GetOdometryData() { return optimization_problem_.odometry_data(); } +sensor::MapByTime +PoseGraph::GetFixedFramePoseData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.fixed_frame_pose_data(); +} + std::vector PoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; diff --git a/cartographer/mapping_3d/pose_graph.h b/cartographer/mapping_3d/pose_graph.h index c30b460..e9c1efd 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/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 GetImuData() override EXCLUDES(mutex_); sensor::MapByTime GetOdometryData() override EXCLUDES(mutex_); + sensor::MapByTime GetFixedFramePoseData() 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/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index 093a0a5..6d38809 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -469,6 +469,11 @@ OptimizationProblem::odometry_data() const { return odometry_data_; } +const sensor::MapByTime& +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 { diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h index 8ab9e6b..580d691 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h @@ -94,6 +94,8 @@ class OptimizationProblem { const mapping::MapById& submap_data() const; const sensor::MapByTime& imu_data() const; const sensor::MapByTime& odometry_data() const; + const sensor::MapByTime& fixed_frame_pose_data() + const; private: // Uses odometry if available, otherwise the local SLAM results.