From cae9c02e0589fb3d7ae16372c9803a0f47b3829f Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Wed, 4 Oct 2017 16:47:26 +0200
Subject: [PATCH] Follow #547 for 3D. (#564)

And changes to 3D following 2D regarding trimmed data.
---
 cartographer/mapping_2d/sparse_pose_graph.cc | 1 +
 cartographer/mapping_3d/sparse_pose_graph.cc | 6 +++++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 438dd0a..7dad55b 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -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()) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 32e0294..4964085 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -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()) &&