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