Serialize IMU data. (#548)

master
Juraj Oršulić 2017-11-09 15:32:54 +01:00 committed by Wolfgang Hess
parent 3bdee588bd
commit e21fc9f253
11 changed files with 60 additions and 4 deletions

View File

@ -179,8 +179,21 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
ToProto(*node_id_data.data.constant_data); ToProto(*node_id_data.data.constant_data);
writer->WriteProto(proto); writer->WriteProto(proto);
} }
// TODO(whess): Serialize additional sensor data: IMU, odometry.
} }
// Next we serialize IMU data from the pose graph.
{
const auto all_imu_data = sparse_pose_graph_->GetImuData();
for (const int trajectory_id : all_imu_data.trajectory_ids()) {
for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) {
proto::SerializedData proto;
auto* const imu_data_proto = proto.mutable_imu_data();
imu_data_proto->set_trajectory_id(trajectory_id);
*imu_data_proto->mutable_imu_data() = sensor::ToProto(imu_data);
writer->WriteProto(proto);
}
}
}
// TODO(whess): Serialize additional sensor data: odometry.
} }
void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
@ -238,6 +251,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto.submap().submap_id().submap_index()}); proto.submap().submap_id().submap_index()});
sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
} }
// TODO(ojura): Deserialize IMU data when loading unfrozen trajectories.
} }
// Add information about which nodes belong to which submap. // Add information about which nodes belong to which submap.

View File

@ -19,6 +19,7 @@ package cartographer.mapping.proto;
import "cartographer/mapping/proto/sparse_pose_graph.proto"; import "cartographer/mapping/proto/sparse_pose_graph.proto";
import "cartographer/mapping/proto/submap.proto"; import "cartographer/mapping/proto/submap.proto";
import "cartographer/mapping/proto/trajectory_node_data.proto"; import "cartographer/mapping/proto/trajectory_node_data.proto";
import "cartographer/sensor/proto/sensor.proto";
message Submap { message Submap {
optional SubmapId submap_id = 1; optional SubmapId submap_id = 1;
@ -31,8 +32,14 @@ message Node {
optional TrajectoryNodeData node_data = 5; optional TrajectoryNodeData node_data = 5;
} }
message ImuData {
optional int32 trajectory_id = 1;
optional sensor.proto.ImuData imu_data = 2;
}
message SerializedData { message SerializedData {
optional Submap submap = 1; optional Submap submap = 1;
optional Node node = 2; optional Node node = 2;
// TODO(whess): Add IMU data, odometry. optional ImuData imu_data = 3;
// TODO(whess): Add odometry.
} }

View File

@ -31,6 +31,8 @@
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.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/imu_data.h"
#include "cartographer/sensor/map_by_time.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
namespace cartographer { namespace cartographer {
@ -80,6 +82,10 @@ class SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Inserts an IMU measurement.
virtual void AddImuData(int trajectory_id,
const sensor::ImuData& imu_data) = 0;
// Finishes the given trajectory. // Finishes the given trajectory.
virtual void FinishTrajectory(int trajectory_id) = 0; virtual void FinishTrajectory(int trajectory_id) = 0;
@ -135,6 +141,9 @@ class SparsePoseGraph {
// Serializes the constraints and trajectories. // Serializes the constraints and trajectories.
proto::SparsePoseGraph ToProto(); proto::SparsePoseGraph ToProto();
// Returns the IMU data.
virtual sensor::MapByTime<sensor::ImuData> GetImuData() = 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

@ -544,6 +544,11 @@ SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_; return trajectory_nodes_;
} }
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
std::vector<Constraint> result; std::vector<Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);

View File

@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_); EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
void AddOdometerData(int trajectory_id, void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data); const sensor::OdometryData& odometry_data);
void AddFixedFramePoseData( void AddFixedFramePoseData(
@ -104,6 +105,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_); GetTrajectoryNodes() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() 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

@ -238,6 +238,11 @@ OptimizationProblem::submap_data() const {
return submap_data_; return submap_data_;
} }
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
const {
return imu_data_;
}
} // namespace sparse_pose_graph } // namespace sparse_pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

View File

@ -88,6 +88,7 @@ class OptimizationProblem {
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const; const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
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;
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;

View File

@ -573,6 +573,11 @@ SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_; return trajectory_nodes_;
} }
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return constraints_; return constraints_;

View File

@ -76,7 +76,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
EXCLUDES(mutex_); EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
void AddOdometerData(int trajectory_id, void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data); const sensor::OdometryData& odometry_data);
void AddFixedFramePoseData( void AddFixedFramePoseData(
@ -104,6 +105,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_); GetTrajectoryNodes() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() 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

@ -438,6 +438,11 @@ OptimizationProblem::submap_data() const {
return submap_data_; return submap_data_;
} }
const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
const {
return imu_data_;
}
} // namespace sparse_pose_graph } // namespace sparse_pose_graph
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer

View File

@ -92,6 +92,7 @@ class OptimizationProblem {
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const; const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
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;
private: private:
struct TrajectoryData { struct TrajectoryData {