Allow using TolerantLoss for INS in pose graph optimization (#1700)

This PR adds the possibility to use the TolerantLoss function for INS in pose graph optimization. This is currently switched off, and so there's not functional change. 

The ceres TolerantLoss function (see [description](http://ceres-solver.org/nnls_modeling.html), and [implementation](https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/loss_function.cc)) has the following property

- for large values of x it approaches a quadratic loss ("null loss") with the specified weight (fixed_frame_pose_translation_weight)
- for small values of x it approaches a quadratic loss with a smaller weight
- there's a crossover at some value x_c. The function is convex everywhere.

cartographer/mapping/internal/2d/pose_graph_2d_test.cc

@@ -142,6 +142,9 @@ class PoseGraph2DTest : public ::testing::Test {
               odometry_rotation_weight = 0.,
               fixed_frame_pose_translation_weight = 1e1,
               fixed_frame_pose_rotation_weight = 1e2,
+              fixed_frame_pose_use_tolerant_loss = false,
+              fixed_frame_pose_tolerant_loss_param_a = 1,
+              fixed_frame_pose_tolerant_loss_param_b = 1,
               log_solver_summary = true,
               use_online_imu_extrinsics_in_3d = true,
               fix_z_in_3d = false,

cartographer/mapping/internal/optimization/optimization_problem_3d.cc

@@ -548,7 +548,10 @@ void OptimizationProblem3D::Solve(
 
       problem.AddResidualBlock(
           SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose),
-          nullptr /* loss function */,
+          options_.fixed_frame_pose_use_tolerant_loss() ?
+              new ceres::TolerantLoss(
+            options_.fixed_frame_pose_tolerant_loss_param_a(),
+            options_.fixed_frame_pose_tolerant_loss_param_b()) : nullptr,
           C_fixed_frames.at(trajectory_id).rotation(),
           C_fixed_frames.at(trajectory_id).translation(),
           C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());

cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc

@@ -48,6 +48,9 @@ class OptimizationProblem3DTest : public ::testing::Test {
           odometry_rotation_weight = 1e-2,
           fixed_frame_pose_translation_weight = 1e1,
           fixed_frame_pose_rotation_weight = 1e2,
+          fixed_frame_pose_use_tolerant_loss = false,
+          fixed_frame_pose_tolerant_loss_param_a = 1,
+          fixed_frame_pose_tolerant_loss_param_b = 1,
           log_solver_summary = true,
           use_online_imu_extrinsics_in_3d = true,
           fix_z_in_3d = false,

cartographer/mapping/internal/optimization/optimization_problem_options.cc

@@ -42,6 +42,12 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
       parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight"));
   options.set_fixed_frame_pose_rotation_weight(
       parameter_dictionary->GetDouble("fixed_frame_pose_rotation_weight"));
+  options.set_fixed_frame_pose_use_tolerant_loss(
+      parameter_dictionary->GetBool("fixed_frame_pose_use_tolerant_loss"));
+  options.set_fixed_frame_pose_tolerant_loss_param_a(
+      parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_a"));
+  options.set_fixed_frame_pose_tolerant_loss_param_b(
+      parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_b"));
   options.set_log_solver_summary(
       parameter_dictionary->GetBool("log_solver_summary"));
   options.set_use_online_imu_extrinsics_in_3d(

cartographer/mapping/proto/pose_graph/optimization_problem_options.proto

@@ -18,8 +18,9 @@ package cartographer.mapping.optimization.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
-// NEXT ID: 19
+// NEXT ID: 26
 message OptimizationProblemOptions {
+  reserved 20 to 22; // For visual constraints.
   // Scaling parameter for Huber loss function.
   double huber_scale = 1;
 
@@ -45,12 +46,20 @@ message OptimizationProblemOptions {
   // odometry.
   double odometry_rotation_weight = 17;
 
-  // Scaling parameter for the FixedFramePose translation.
+  // Scaling parameter for the FixedFramePose translation. Unit: 1/meters.
   double fixed_frame_pose_translation_weight = 11;
 
   // Scaling parameter for the FixedFramePose rotation.
   double fixed_frame_pose_rotation_weight = 12;
 
+  bool fixed_frame_pose_use_tolerant_loss = 23;
+  // The following parameters are used only if fixed_frame_pose_use_tolerant_loss is true.
+  // See http://ceres-solver.org/nnls_modeling.html.
+  // For large values of s, the tolerant loss function approaches a null loss
+  // with fixed_frame_pose_translation_weight.
+  double fixed_frame_pose_tolerant_loss_param_a = 24;
+  double fixed_frame_pose_tolerant_loss_param_b = 25;
+
   // 3D only: fix Z.
   bool fix_z_in_3d = 13;
 

configuration_files/pose_graph.lua

@@ -71,6 +71,9 @@ POSE_GRAPH = {
     odometry_rotation_weight = 1e5,
     fixed_frame_pose_translation_weight = 1e1,
     fixed_frame_pose_rotation_weight = 1e2,
+    fixed_frame_pose_use_tolerant_loss = false,
+    fixed_frame_pose_tolerant_loss_param_a = 1,
+    fixed_frame_pose_tolerant_loss_param_b = 1,
     log_solver_summary = false,
     use_online_imu_extrinsics_in_3d = true,
     fix_z_in_3d = false,