(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
master
danielsievers 2018-02-19 16:30:32 +01:00 committed by Wally B. Feed
parent 3211e75957
commit c38bb60407
10 changed files with 114 additions and 10 deletions

View File

@ -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.
}

View File

@ -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<sensor::OdometryData> GetOdometryData() = 0;
// Returns the trajectory data.
virtual std::map<int, TrajectoryData> GetTrajectoryData() = 0;
// Returns the fixed frame pose data.
virtual sensor::MapByTime<sensor::FixedFramePoseData>
GetFixedFramePoseData() = 0;

View File

@ -73,6 +73,12 @@ class PoseGraphInterface {
transform::Rigid3d pose;
};
struct TrajectoryData {
double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
};
PoseGraphInterface() {}
virtual ~PoseGraphInterface() {}

View File

@ -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 {

View File

@ -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<sensor::OdometryData> PoseGraph::GetOdometryData() {
return optimization_problem_.odometry_data();
}
std::map<int, mapping::PoseGraphInterface::TrajectoryData>
PoseGraph::GetTrajectoryData() {
return {}; // Not implemented yet in 2D.
}
sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph::GetFixedFramePoseData() {
return {}; // Not implemented yet in 2D.

View File

@ -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<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,

View File

@ -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<int, mapping::PoseGraphInterface::TrajectoryData>
PoseGraph::GetTrajectoryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.trajectory_data();
}
std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;

View File

@ -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<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::map<int, TrajectoryData> GetTrajectoryData() override;
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,

View File

@ -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<transform::Rigid3d> 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<int, TrajectoryData>& 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 {

View File

@ -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<sensor::OdometryData>& odometry_data() const;
const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
const;
const std::map<int, mapping::PoseGraphInterface::TrajectoryData>&
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<double, 4> imu_calibration{{1., 0., 0., 0.}};
common::optional<transform::Rigid3d> fixed_frame;
};
mapping::pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
@ -120,7 +119,7 @@ class OptimizationProblem {
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::map<int, TrajectoryData> trajectory_data_;
std::map<int, mapping::PoseGraphInterface::TrajectoryData> trajectory_data_;
};
} // namespace pose_graph