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

Exclude trimmed submaps from the pose graph serialization. (#354) 

Related to #283.
master
Wolfgang Hess 2017-06-21 17:57:08 +02:00 committed by GitHub
parent 432cd5dcd3
commit 9fbb65084b
1 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
cartographer/mapping/sparse_pose_graph.cc
View File

@ -62,14 +62,15 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
proto::SparsePoseGraph proto;
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
std::map<SubmapId, SubmapId> submap_id_remapping; // Due to trimming.
const auto all_trajectory_nodes = GetTrajectoryNodes();
const auto all_submap_data = GetAllSubmapData();
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
++trajectory_id) {
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
auto* trajectory_proto = proto.add_trajectory();
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
for (size_t old_node_index = 0;
old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
const auto& node = single_trajectory_nodes[old_node_index];
@ -86,8 +87,16 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
}
}
if (!single_trajectory_nodes.empty()) {
for (const auto& submap_data : all_submap_data[trajectory_id]) {
const auto& single_trajectory_submap_data = all_submap_data[trajectory_id];
for (size_t old_submap_index = 0;
old_submap_index != single_trajectory_submap_data.size();
++old_submap_index) {
const auto& submap_data = single_trajectory_submap_data[old_submap_index];
if (submap_data.submap != nullptr) {
submap_id_remapping[SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(old_submap_index)}] =
SubmapId{static_cast<int>(trajectory_id),
static_cast<int>(trajectory_proto->submap_size())};
*trajectory_proto->add_submap()->mutable_pose() =
transform::ToProto(submap_data.pose);
}
@ -103,10 +112,12 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint_proto->set_translation_weight(
constraint.pose.translation_weight);
constraint_proto->set_rotation_weight(constraint.pose.rotation_weight);
const SubmapId submap_id = submap_id_remapping.at(constraint.submap_id);
constraint_proto->mutable_submap_id()->set_trajectory_id(
constraint.submap_id.trajectory_id);
submap_id.trajectory_id);
constraint_proto->mutable_submap_id()->set_submap_index(
constraint.submap_id.submap_index);
submap_id.submap_index);
const NodeId node_id = node_id_remapping.at(constraint.node_id);
constraint_proto->mutable_scan_id()->set_trajectory_id(