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

Delete data of trimmed submaps. (#353) 

This is related to #283.

PAIR=SirVer
master
Wolfgang Hess 2017-06-21 16:36:54 +02:00 committed by GitHub
parent 0c7b3ee596
commit 432cd5dcd3
2 changed files with 13 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping/map_builder.cc
View File

@ -123,7 +123,11 @@ string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
} }
const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id); const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id);
CHECK(submap_data.submap != nullptr); if (submap_data.submap == nullptr) {
return "Requested submap " + std::to_string(submap_id.submap_index) +
" from trajectory " + std::to_string(submap_id.trajectory_id) +
" but it has been trimmed.";
}
submap_data.submap->ToResponseProto(submap_data.pose, response); submap_data.submap->ToResponseProto(submap_data.pose, response);
return ""; return "";
} }

11
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -494,6 +494,9 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
return {};
}
auto submap = submap_data_.at(submap_id).submap; auto submap = submap_data_.at(submap_id).submap;
// We already have an optimized pose. // We already have an optimized pose.
if (submap_id.trajectory_id < if (submap_id.trajectory_id <
@ -565,10 +568,12 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
} }
// Mark the submap with 'submap_id' as trimmed and remove its data. // Mark the submap with 'submap_id' as trimmed and remove its data.
parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed; auto& submap_data = parent_->submap_data_.at(submap_id);
CHECK(submap_data.state == SubmapState::kFinished);
submap_data.state = SubmapState::kTrimmed;
CHECK(submap_data.submap != nullptr);
submap_data.submap.reset();
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
// TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
// there.
// Mark the 'nodes_to_remove' as trimmed and remove their data. // Mark the 'nodes_to_remove' as trimmed and remove their data.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {