Serialize fixed frame pose data. (#689)

Similar to #666 and #548.
master
Juraj Oršulić 2017-11-28 09:44:35 +01:00 committed by Wally B. Feed
parent 1c7183d5c4
commit bebe021b04
9 changed files with 60 additions and 6 deletions

View File

@ -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) { void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
@ -273,8 +288,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto.submap().submap_id().submap_index()}); proto.submap().submap_id().submap_index()});
pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
} }
// TODO(ojura): Deserialize IMU and odometry data when loading unfrozen // TODO(ojura): Deserialize IMU, odometry and fixed frame pose data when
// trajectories. // loading unfrozen trajectories.
} }
// Add information about which nodes belong to which submap. // Add information about which nodes belong to which submap.

View File

@ -31,6 +31,7 @@
#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -91,6 +92,11 @@ class PoseGraph {
virtual void AddOdometryData(int trajectory_id, virtual void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) = 0; 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. // Finishes the given trajectory.
virtual void FinishTrajectory(int trajectory_id) = 0; virtual void FinishTrajectory(int trajectory_id) = 0;
@ -152,6 +158,10 @@ class PoseGraph {
// Returns the odometry data. // Returns the odometry data.
virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0; virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0;
// Returns the fixed frame pose data.
virtual sensor::MapByTime<sensor::FixedFramePoseData>
GetFixedFramePoseData() = 0;
// Returns the collection of constraints. // Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0; virtual std::vector<Constraint> constraints() = 0;

View File

@ -42,10 +42,15 @@ message OdometryData {
sensor.proto.OdometryData odometry_data = 2; sensor.proto.OdometryData odometry_data = 2;
} }
message FixedFramePoseData {
int32 trajectory_id = 1;
sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
}
message SerializedData { message SerializedData {
Submap submap = 1; Submap submap = 1;
Node node = 2; Node node = 2;
ImuData imu_data = 3; ImuData imu_data = 3;
OdometryData odometry_data = 4; OdometryData odometry_data = 4;
// TODO(whess): Add fixed frame pose data. FixedFramePoseData fixed_frame_pose_data = 5;
} }

View File

@ -555,6 +555,11 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
return optimization_problem_.odometry_data(); return optimization_problem_.odometry_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph::GetFixedFramePoseData() {
return {}; // Not implemented yet in 2D.
}
std::vector<PoseGraph::Constraint> PoseGraph::constraints() { std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
std::vector<Constraint> result; std::vector<Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);

View File

@ -83,7 +83,8 @@ class PoseGraph : public mapping::PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
void AddFixedFramePoseData( void AddFixedFramePoseData(
int trajectory_id, 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 FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
@ -109,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_); sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_); std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,

View File

@ -584,6 +584,12 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
return optimization_problem_.odometry_data(); return optimization_problem_.odometry_data();
} }
sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph::GetFixedFramePoseData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.fixed_frame_pose_data();
}
std::vector<PoseGraph::Constraint> PoseGraph::constraints() { std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return constraints_; return constraints_;

View File

@ -83,7 +83,8 @@ class PoseGraph : public mapping::PoseGraph {
EXCLUDES(mutex_); EXCLUDES(mutex_);
void AddFixedFramePoseData( void AddFixedFramePoseData(
int trajectory_id, 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 FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
@ -109,6 +110,8 @@ class PoseGraph : public mapping::PoseGraph {
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_); sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_); std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,

View File

@ -469,6 +469,11 @@ OptimizationProblem::odometry_data() const {
return odometry_data_; return odometry_data_;
} }
const sensor::MapByTime<sensor::FixedFramePoseData>&
OptimizationProblem::fixed_frame_pose_data() const {
return fixed_frame_pose_data_;
}
transform::Rigid3d OptimizationProblem::ComputeRelativePose( transform::Rigid3d OptimizationProblem::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data, const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const { const NodeData& second_node_data) const {

View File

@ -94,6 +94,8 @@ class OptimizationProblem {
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const; const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const; const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const; const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
const sensor::MapByTime<sensor::FixedFramePoseData>& fixed_frame_pose_data()
const;
private: private:
// Uses odometry if available, otherwise the local SLAM results. // Uses odometry if available, otherwise the local SLAM results.