12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

(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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping/map_builder.cc
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.
}

7
cartographer/mapping/pose_graph.h
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;

6
cartographer/mapping/pose_graph_interface.h
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() {}

9
cartographer/mapping/proto/serialization.proto
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 {

10
cartographer/mapping_2d/pose_graph.cc
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.

3
cartographer/mapping_2d/pose_graph.h
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,

26
cartographer/mapping_3d/pose_graph.cc
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_;

4
cartographer/mapping_3d/pose_graph.h
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,

19
cartographer/mapping_3d/pose_graph/optimization_problem.cc
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 {

13
cartographer/mapping_3d/pose_graph/optimization_problem.h
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