Allow continuing after running a "final" optimization. (#109)

And a tiny cleanup of the submap visualization query.

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;
 

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() {

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() {

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() {
+  void Flush() { queue_.Flush(); }
-    queue_.Flush();
-  }
 
  private:
   // Queue keys are a pair of trajectory ID and sensor identifier.

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
+// sorted order. It will wait to see at least one value for each unfinished
-// dispatching the next time ordered value across all queues. This class is thread-compatible.
+// 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>)>;

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,
 }

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