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

Serialize range data. (#395) 

We compress 2D range data before serialization to reduce
output size.

Related to #253.
master
Wolfgang Hess 2017-07-07 15:39:55 +02:00 committed by GitHub
parent 94a848cd46
commit 5378ee2adc
2 changed files with 55 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cartographer/mapping/map_builder.cc
View File

@ -141,9 +141,11 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
// We serialize the pose graph followed by all the data referenced in it.
writer->WriteProto(sparse_pose_graph_->ToProto());
// Next we serialize all submap data.
{
const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) {
trajectory_id != static_cast<int>(submap_data.size());
++trajectory_id) {
for (int submap_index = 0;
submap_index != static_cast<int>(submap_data[trajectory_id].size());
++submap_index) {
@ -158,9 +160,36 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
writer->WriteProto(proto);
}
}
// TODO(whess): Serialize range data ("scan") for each trajectory node.
}
// Next we serialize all range data.
{
const auto node_data = sparse_pose_graph_->GetTrajectoryNodes();
for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
for (int node_index = 0;
node_index != static_cast<int>(node_data[trajectory_id].size());
++node_index) {
proto::SerializedData proto;
auto* const range_data_proto = proto.mutable_range_data();
// TODO(whess): Handle trimmed data.
range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
range_data_proto->mutable_node_id()->set_node_index(node_index);
const auto& data = *node_data[trajectory_id][node_index].constant_data;
if (!data.range_data_2d.returns.empty()) {
*range_data_proto->mutable_range_data_2d() =
sensor::ToProto(sensor::Compress(data.range_data_2d));
} else {
*range_data_proto->mutable_range_data_3d() =
sensor::ToProto(data.range_data_3d);
}
// TODO(whess): Only enable optionally? Resulting pbstream files will be
// a lot larger now.
writer->WriteProto(proto);
}
}
// TODO(whess): Serialize additional sensor data: IMU, odometry.
}
}
void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
proto::SparsePoseGraph pose_graph;

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

@ -18,6 +18,7 @@ package cartographer.mapping.proto;
import "cartographer/mapping/proto/sparse_pose_graph.proto";
import "cartographer/mapping/proto/submap.proto";
import "cartographer/sensor/proto/sensor.proto";
message Submap {
optional SubmapId submap_id = 1;
@ -25,7 +26,14 @@ message Submap {
optional Submap3D submap_3d = 3;
}
message RangeData {
optional NodeId node_id = 1;
optional sensor.proto.CompressedRangeData range_data_2d = 2;
optional sensor.proto.CompressedRangeData range_data_3d = 3;
}
message SerializedData {
optional Submap submap = 1;
// TODO(whess): Add range data, IMU data, odometry.
optional RangeData range_data = 2;
// TODO(whess): Add IMU data, odometry.
}