Add use_online_imu_extrinsics_in_3d parameter and set it to true by default. (#1217)

Add the parameter `OptimizationProblemOptions.use_online_imu_extrinsincs_in_3d`. Set it to true by default to not change existing behavior.

Using online IMU extrinsics is not always desirable, and particularly during localization, we have determined that it is preferable to turn this feature off.

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

@@ -129,6 +129,7 @@ class PoseGraph2DTest : public ::testing::Test {
               fixed_frame_pose_translation_weight = 1e1,
               fixed_frame_pose_rotation_weight = 1e2,
               log_solver_summary = true,
+              use_online_imu_extrinsics_in_3d = true,
               ceres_solver_options = {
                 use_nonmonotonic_steps = false,
                 max_num_iterations = 200,

cartographer/mapping/internal/optimization/optimization_problem_3d.cc

@@ -367,6 +367,10 @@ void OptimizationProblem3D::Solve(
 
       problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                                 new ceres::QuaternionParameterization());
+      if (!options_.use_online_imu_extrinsics_in_3d()) {
+        problem.SetParameterBlockConstant(
+            trajectory_data.imu_calibration.data());
+      }
       CHECK(imu_data_.HasTrajectory(trajectory_id));
       const auto imu_data = imu_data_.trajectory(trajectory_id);
       CHECK(imu_data.begin() != imu_data.end());

cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc

@@ -49,6 +49,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
           fixed_frame_pose_translation_weight = 1e1,
           fixed_frame_pose_rotation_weight = 1e2,
           log_solver_summary = true,
+          use_online_imu_extrinsics_in_3d = true,
           ceres_solver_options = {
             use_nonmonotonic_steps = false,
             max_num_iterations = 200,

cartographer/mapping/internal/optimization/optimization_problem_options.cc

@@ -44,6 +44,8 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
       parameter_dictionary->GetDouble("fixed_frame_pose_rotation_weight"));
   options.set_log_solver_summary(
       parameter_dictionary->GetBool("log_solver_summary"));
+  options.set_use_online_imu_extrinsics_in_3d(
+      parameter_dictionary->GetBool("use_online_imu_extrinsics_in_3d"));
   *options.mutable_ceres_solver_options() =
       common::CreateCeresSolverOptionsProto(
           parameter_dictionary->GetDictionary("ceres_solver_options").get());

cartographer/mapping/proto/pose_graph/optimization_problem_options.proto

@@ -18,7 +18,7 @@ package cartographer.mapping.optimization.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
-// NEXT ID: 18
+// NEXT ID: 19
 message OptimizationProblemOptions {
   // Scaling parameter for Huber loss function.
   double huber_scale = 1;
@@ -54,6 +54,9 @@ message OptimizationProblemOptions {
   // 3D only: fix Z.
   bool fix_z_in_3d = 13;
 
+  // 3D only: activate online IMU extrinsics.
+  bool use_online_imu_extrinsics_in_3d = 18;
+
   // If true, the Ceres solver summary will be logged for every optimization.
   bool log_solver_summary = 5;
 

configuration_files/pose_graph.lua

@@ -72,6 +72,7 @@ POSE_GRAPH = {
     fixed_frame_pose_translation_weight = 1e1,
     fixed_frame_pose_rotation_weight = 1e2,
     log_solver_summary = false,
+    use_online_imu_extrinsics_in_3d = true,
     ceres_solver_options = {
       use_nonmonotonic_steps = false,
       max_num_iterations = 50,