Improve the sparse pose graph optimization. (#165)

Removes the loss function for intra-submap constraints in 3D SLAM,
as it is already done in 2D. Also removes a duplicate call to the
solver in 2D, and code cleanup.

cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt

@@ -53,7 +53,6 @@ google_library(mapping_2d_sparse_pose_graph_optimization_problem
   DEPENDS
     common_ceres_solver_options
     common_histogram
-    common_lua_parameter_dictionary
     common_math
     common_port
     mapping_2d_sparse_pose_graph_spa_cost_function

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -191,9 +191,6 @@ void OptimizationProblem::Solve(
     LOG(INFO) << "Inter-submap theta^2 residual histogram:\n"
               << inter_submap_theta_residuals.ToString(10);
   }
-  ceres::Solve(
-      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
-      &problem, &summary);
 
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -23,7 +23,6 @@
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
-#include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -323,15 +323,7 @@ void SparsePoseGraph::RunFinalOptimization() {
 
 void SparsePoseGraph::RunOptimization() {
   if (!submap_transforms_.empty()) {
-    transform::Rigid3d submap_0_pose;
+    optimization_problem_.Solve(constraints_, &submap_transforms_);
-    {
-      common::MutexLocker locker(&mutex_);
-      CHECK(!submap_states_.empty());
-      submap_0_pose = submap_states_.front().submap->local_pose();
-    }
-
-    optimization_problem_.Solve(constraints_, submap_0_pose,
-                                &submap_transforms_);
     common::MutexLocker locker(&mutex_);
     has_new_optimized_poses_ = true;
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -99,7 +99,6 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
 
 void OptimizationProblem::Solve(
     const std::vector<Constraint>& constraints,
-    const transform::Rigid3d& submap_0_transform,
     std::vector<transform::Rigid3d>* const submap_transforms) {
   if (node_data_.empty()) {
     // Nothing to optimize.
@@ -146,7 +145,7 @@ void OptimizationProblem::Solve(
         common::make_unique<ceres::QuaternionParameterization>(), &problem);
   }
 
-  // Add cost functions for the loop closing constraints.
+  // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     CHECK_GE(constraint.i, 0);
     CHECK_LT(constraint.i, submap_transforms->size());
@@ -155,7 +154,10 @@ void OptimizationProblem::Solve(
     problem.AddResidualBlock(
         new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
             new SpaCostFunction(constraint.pose)),
-        new ceres::HuberLoss(options_.huber_scale()),
+        // Only loop closure constraints should have a loss function.
+        constraint.tag == Constraint::INTER_SUBMAP
+            ? new ceres::HuberLoss(options_.huber_scale())
+            : nullptr,
         C_submaps[constraint.i].rotation(),
         C_submaps[constraint.i].translation(),
         C_point_clouds[constraint.j].rotation(),
@@ -228,9 +230,9 @@ void OptimizationProblem::Solve(
 
   // Solve.
   ceres::Solver::Summary summary;
-  ceres::Solver::Options ceres_solver_options =
+  ceres::Solve(
-      common::CreateCeresSolverOptions(options_.ceres_solver_options());
+      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
-  ceres::Solve(ceres_solver_options, &problem, &summary);
+      &problem, &summary);
 
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -68,7 +68,6 @@ class OptimizationProblem {
 
   // Computes the optimized poses.
   void Solve(const std::vector<Constraint>& constraints,
-             const transform::Rigid3d& submap_0_transform,
              std::vector<transform::Rigid3d>* submap_transforms);
 
   const std::vector<NodeData>& node_data() const;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -166,8 +166,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
                             node_data[j].point_cloud_pose);
   }
 
-  optimization_problem_.Solve(constraints, kSubmap0Transform,
+  optimization_problem_.Solve(constraints, &submap_transforms);
-                              &submap_transforms);
 
   double translation_error_after = 0.;
   double rotation_error_after = 0.;

configuration_files/sparse_pose_graph.lua

@@ -66,7 +66,7 @@ SPARSE_POSE_GRAPH = {
   },
   optimization_problem = {
     huber_scale = 1e1,
-    acceleration_weight = 7e4,
+    acceleration_weight = 1e4,
     rotation_weight = 3e6,
     consecutive_scan_translation_penalty_factor = 1e5,
     consecutive_scan_rotation_penalty_factor = 1e5,