From a5b81ff159d302441b33a51585506e02b137a95f Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Thu, 27 Oct 2016 15:25:57 +0200
Subject: [PATCH] Allow continuing after running a "final" optimization. (#109)

And a tiny cleanup of the submap visualization query.
---
 cartographer/mapping/proto/submap_visualization.proto | 5 +----
 cartographer/mapping_2d/sparse_pose_graph.cc          | 4 ++++
 cartographer/mapping_3d/sparse_pose_graph.cc          | 4 ++++
 cartographer/sensor/collator.h                        | 4 +---
 cartographer/sensor/ordered_multi_queue.h             | 6 ++++--
 configuration_files/sparse_pose_graph.lua             | 2 +-
 scripts/update_cmakelists.py                          | 1 -
 7 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/cartographer/mapping/proto/submap_visualization.proto b/cartographer/mapping/proto/submap_visualization.proto
index f98a4d2..e3742a1 100644
--- a/cartographer/mapping/proto/submap_visualization.proto
+++ b/cartographer/mapping/proto/submap_visualization.proto
@@ -34,15 +34,12 @@ message SubmapList {
 message SubmapQuery {
   message Request {
     // Index into 'SubmapList.trajectory(trajectory_id).submap'.
-    optional int32 submap_id = 1;
+    optional int32 submap_index = 1;
     // Index into 'TrajectoryList.trajectory'.
     optional int32 trajectory_id = 2;
   }
 
   message Response {
-    // ID of the Submap.
-    optional int32 submap_id = 1;
-
     // Version of the given submap, higher means newer.
     optional int32 submap_version = 2;
 
diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index dad9dd0..2ecf4d8 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -310,6 +310,10 @@ void SparsePoseGraph::RunFinalOptimization() {
   optimization_problem_.SetMaxNumIterations(
       options_.max_num_final_iterations());
   RunOptimization();
+  optimization_problem_.SetMaxNumIterations(
+      options_.optimization_problem_options()
+          .ceres_solver_options()
+          .max_num_iterations());
 }
 
 void SparsePoseGraph::RunOptimization() {
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 995f7c9..fc60bd8 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -311,6 +311,10 @@ void SparsePoseGraph::RunFinalOptimization() {
   optimization_problem_.SetMaxNumIterations(
       options_.max_num_final_iterations());
   RunOptimization();
+  optimization_problem_.SetMaxNumIterations(
+      options_.optimization_problem_options()
+          .ceres_solver_options()
+          .max_num_iterations());
 }
 
 void SparsePoseGraph::RunOptimization() {
diff --git a/cartographer/sensor/collator.h b/cartographer/sensor/collator.h
index b258aed..d28e898 100644
--- a/cartographer/sensor/collator.h
+++ b/cartographer/sensor/collator.h
@@ -75,9 +75,7 @@ class Collator {
 
   // Dispatches all queued sensor packets. May only be called once.
   // AddSensorData may not be called after Flush.
-  void Flush() {
-    queue_.Flush();
-  }
+  void Flush() { queue_.Flush(); }
 
  private:
   // Queue keys are a pair of trajectory ID and sensor identifier.
diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h
index 80a501a..2a7cc29 100644
--- a/cartographer/sensor/ordered_multi_queue.h
+++ b/cartographer/sensor/ordered_multi_queue.h
@@ -50,8 +50,10 @@ inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) {
 }
 
 // Maintains multiple queues of sorted sensor data and dispatches it in merge
-// sorted order. It will wait to see at least one value for each unfinished queue before
-// dispatching the next time ordered value across all queues. This class is thread-compatible.
+// sorted order. It will wait to see at least one value for each unfinished
+// queue before dispatching the next time ordered value across all queues.
+//
+// This class is thread-compatible.
 class OrderedMultiQueue {
  public:
   using Callback = std::function<void(std::unique_ptr<Data>)>;
diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua
index feb9193..4b2b805 100644
--- a/configuration_files/sparse_pose_graph.lua
+++ b/configuration_files/sparse_pose_graph.lua
@@ -79,5 +79,5 @@ SPARSE_POSE_GRAPH = {
     },
   },
   max_num_final_iterations = 200,
-  global_sampling_ratio = 0.01,
+  global_sampling_ratio = 0.003,
 }
diff --git a/scripts/update_cmakelists.py b/scripts/update_cmakelists.py
index 6d5820c..9fd3f83 100755
--- a/scripts/update_cmakelists.py
+++ b/scripts/update_cmakelists.py
@@ -14,7 +14,6 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
-
 """A dumb CMakeLists.txt generator that relies on source name conventions."""
 
 from os import path