From c38bb60407209f46c2f5d5cb599728c2c2cee93b Mon Sep 17 00:00:00 2001 From: danielsievers <35999903+danielsievers@users.noreply.github.com> Date: Mon, 19 Feb 2018 16:30:32 +0100 Subject: [PATCH] (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 | 27 +++++++++++++++++++ cartographer/mapping/pose_graph.h | 7 +++++ cartographer/mapping/pose_graph_interface.h | 6 +++++ .../mapping/proto/serialization.proto | 9 +++++++ cartographer/mapping_2d/pose_graph.cc | 10 +++++++ cartographer/mapping_2d/pose_graph.h | 3 +++ cartographer/mapping_3d/pose_graph.cc | 26 ++++++++++++++++++ cartographer/mapping_3d/pose_graph.h | 4 +++ .../pose_graph/optimization_problem.cc | 19 ++++++++++--- .../pose_graph/optimization_problem.h | 13 +++++---- 10 files changed, 114 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 477a725..cd00345 100644 --- a/cartographer/mapping/map_builder.cc +++ b/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. } diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index e445ae0..24e18a8 100644 --- a/cartographer/mapping/pose_graph.h +++ b/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 GetOdometryData() = 0; + // Returns the trajectory data. + virtual std::map GetTrajectoryData() = 0; + // Returns the fixed frame pose data. virtual sensor::MapByTime GetFixedFramePoseData() = 0; diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 85f4148..db36e3a 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -73,6 +73,12 @@ class PoseGraphInterface { transform::Rigid3d pose; }; + struct TrajectoryData { + double gravity_constant = 9.8; + std::array imu_calibration{{1., 0., 0., 0.}}; + common::optional fixed_frame_origin_in_map; + }; + PoseGraphInterface() {} virtual ~PoseGraphInterface() {} diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index 228c9f1..62f20e0 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/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 { diff --git a/cartographer/mapping_2d/pose_graph.cc b/cartographer/mapping_2d/pose_graph.cc index a4c3878..8134d79 100644 --- a/cartographer/mapping_2d/pose_graph.cc +++ b/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 PoseGraph::GetOdometryData() { return optimization_problem_.odometry_data(); } +std::map +PoseGraph::GetTrajectoryData() { + return {}; // Not implemented yet in 2D. +} + sensor::MapByTime PoseGraph::GetFixedFramePoseData() { return {}; // Not implemented yet in 2D. diff --git a/cartographer/mapping_2d/pose_graph.h b/cartographer/mapping_2d/pose_graph.h index fd074f0..9ef4600 100644 --- a/cartographer/mapping_2d/pose_graph.h +++ b/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 GetFixedFramePoseData() override EXCLUDES(mutex_); + std::map GetTrajectoryData() 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 77851ac..d6ba996 100644 --- a/cartographer/mapping_3d/pose_graph.cc +++ b/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 +PoseGraph::GetTrajectoryData() { + common::MutexLocker locker(&mutex_); + return optimization_problem_.trajectory_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 0cd71f0..76d4ff8 100644 --- a/cartographer/mapping_3d/pose_graph.h +++ b/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 GetFixedFramePoseData() override EXCLUDES(mutex_); + std::map GetTrajectoryData() override; + 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 4428c06..c1f7748 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/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 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()) { - fixed_frame_pose_in_map = trajectory_data.fixed_frame.value(); + if (trajectory_data.fixed_frame_origin_in_map.has_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& 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 { diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h index 0445921..582bc7f 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/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& odometry_data() const; const sensor::MapByTime& fixed_frame_pose_data() const; + const std::map& + 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 imu_calibration{{1., 0., 0., 0.}}; - common::optional fixed_frame; - }; - mapping::pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; mapping::MapById node_data_; @@ -120,7 +119,7 @@ class OptimizationProblem { sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; sensor::MapByTime fixed_frame_pose_data_; - std::map trajectory_data_; + std::map trajectory_data_; }; } // namespace pose_graph