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

63
cartographer/mapping/map_builder.cc
View File

@ -141,25 +141,54 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
// We serialize the pose graph followed by all the data referenced in it. // We serialize the pose graph followed by all the data referenced in it.
writer->WriteProto(sparse_pose_graph_->ToProto()); writer->WriteProto(sparse_pose_graph_->ToProto());
// Next we serialize all submap data. // Next we serialize all submap data.
const auto submap_data = sparse_pose_graph_->GetAllSubmapData(); {
for (int trajectory_id = 0; const auto submap_data = sparse_pose_graph_->GetAllSubmapData();
trajectory_id != static_cast<int>(submap_data.size()); ++trajectory_id) { for (int trajectory_id = 0;
for (int submap_index = 0; trajectory_id != static_cast<int>(submap_data.size());
submap_index != static_cast<int>(submap_data[trajectory_id].size()); ++trajectory_id) {
++submap_index) { for (int submap_index = 0;
proto::SerializedData proto; submap_index != static_cast<int>(submap_data[trajectory_id].size());
auto* const submap_proto = proto.mutable_submap(); ++submap_index) {
// TODO(whess): Handle trimmed data. proto::SerializedData proto;
submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id); auto* const submap_proto = proto.mutable_submap();
submap_proto->mutable_submap_id()->set_submap_index(submap_index); // TODO(whess): Handle trimmed data.
submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto); submap_proto->mutable_submap_id()->set_trajectory_id(trajectory_id);
// TODO(whess): Only enable optionally? Resulting pbstream files will be submap_proto->mutable_submap_id()->set_submap_index(submap_index);
// a lot larger now. submap_data[trajectory_id][submap_index].submap->ToProto(submap_proto);
writer->WriteProto(proto); // TODO(whess): Only enable optionally? Resulting pbstream files will be
// a lot larger now.
writer->WriteProto(proto);
}
} }
} }
// TODO(whess): Serialize range data ("scan") for each trajectory node. // Next we serialize all range data.
// TODO(whess): Serialize additional sensor data: IMU, odometry. {
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) { void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {

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/sparse_pose_graph.proto";
import "cartographer/mapping/proto/submap.proto"; import "cartographer/mapping/proto/submap.proto";
import "cartographer/sensor/proto/sensor.proto";
message Submap { message Submap {
optional SubmapId submap_id = 1; optional SubmapId submap_id = 1;
@ -25,7 +26,14 @@ message Submap {
optional Submap3D submap_3d = 3; 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 { message SerializedData {
optional Submap submap = 1; optional Submap submap = 1;
// TODO(whess): Add range data, IMU data, odometry. optional RangeData range_data = 2;
// TODO(whess): Add IMU data, odometry.
} }