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

Serialize odometry data (#666) 

Replaces #550.
master
Juraj Oršulić 2017-11-14 15:18:39 +01:00 committed by Wally B. Feed
parent a7680f60d4
commit 5496cbdc0c
11 changed files with 63 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cartographer/mapping/map_builder.cc
View File

@ -193,7 +193,22 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
}
}
}
// TODO(whess): Serialize additional sensor data: odometry.
// Next we serialize odometry data from the pose graph.
{
const auto all_odometry_data = sparse_pose_graph_->GetOdometryData();
for (const int trajectory_id : all_odometry_data.trajectory_ids()) {
for (const auto& odometry_data :
all_odometry_data.trajectory(trajectory_id)) {
proto::SerializedData proto;
auto* const odometry_data_proto = proto.mutable_odometry_data();
odometry_data_proto->set_trajectory_id(trajectory_id);
*odometry_data_proto->mutable_odometry_data() =
sensor::ToProto(odometry_data);
writer->WriteProto(proto);
}
}
}
// TODO(whess): Serialize additional sensor data: fixed frame pose data.
}
void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
@ -251,7 +266,8 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto.submap().submap_id().submap_index()});
sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap());
}
// TODO(ojura): Deserialize IMU data when loading unfrozen trajectories.
// TODO(ojura): Deserialize IMU and odometry data when loading unfrozen
// trajectories.
}
// Add information about which nodes belong to which submap.

8
cartographer/mapping/proto/serialization.proto
View File

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

8
cartographer/mapping/sparse_pose_graph.h
View File

@ -33,6 +33,7 @@
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
@ -86,6 +87,10 @@ class SparsePoseGraph {
virtual void AddImuData(int trajectory_id,
const sensor::ImuData& imu_data) = 0;
// Inserts an odometry measurement.
virtual void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data) = 0;
// Finishes the given trajectory.
virtual void FinishTrajectory(int trajectory_id) = 0;
@ -144,6 +149,9 @@ class SparsePoseGraph {
// Returns the IMU data.
virtual sensor::MapByTime<sensor::ImuData> GetImuData() = 0;
// Returns the odometry data.
virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() = 0;
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;

5
cartographer/mapping_2d/sparse_pose_graph.cc
View File

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

5
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data);
const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_);
void AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
@ -106,6 +107,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,

5
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -232,6 +232,11 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem::odometry_data() const {
return odometry_data_;
}
std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
const int trajectory_id, const common::Time time) const {
const auto it = odometry_data_.lower_bound(trajectory_id, time);

1
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -90,6 +90,7 @@ class OptimizationProblem {
const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
const sensor::MapByTime<sensor::OdometryData>& odometry_data() const;
private:
std::unique_ptr<transform::Rigid3d> InterpolateOdometry(

5
cartographer/mapping_3d/sparse_pose_graph.cc
View File

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

5
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -79,7 +79,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_);
void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data);
const sensor::OdometryData& odometry_data) override
EXCLUDES(mutex_);
void AddFixedFramePoseData(
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
@ -106,6 +107,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
sensor::MapByTime<sensor::OdometryData> GetOdometryData() override
EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -465,6 +465,11 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>&
OptimizationProblem::odometry_data() const {
return odometry_data_;
}
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {

1
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

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