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

Follow #547 for 3D. (#564) 

And changes to 3D following 2D regarding trimmed data.
master
Wolfgang Hess 2017-10-04 16:47:26 +02:00 committed by GitHub
parent 51f5a18462
commit cae9c02e05
2 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -70,6 +70,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
return {submap_id};
}
CHECK_EQ(2, insertion_submaps.size());
CHECK(!submap_data.at(trajectory_id).empty());
const mapping::SubmapId last_submap_id{
trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -239,6 +239,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
for (const auto& index_node_data : node_data[trajectory_id]) {
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
index_node_data.first};
CHECK(!trajectory_nodes_.at(node_id).trimmed());
if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
}
@ -373,13 +374,13 @@ void SparsePoseGraph::HandleWorkQueue() {
run_loop_closure_ = false;
while (!run_loop_closure_) {
if (work_queue_->empty()) {
LOG(INFO) << "We caught up. Hooray!";
work_queue_.reset();
return;
}
work_queue_->front()();
work_queue_->pop_front();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
// We have to optimize again.
HandleWorkQueue();
});
@ -625,6 +626,9 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
if (submap_data_.at(submap_id).state == SubmapState::kTrimmed) {
return {};
}
auto submap = submap_data_.at(submap_id).submap;
if (submap_id.trajectory_id <
static_cast<int>(optimized_submap_transforms_.size()) &&