From 7fcc4576a81100c67c1bb49700daf0754240a64e Mon Sep 17 00:00:00 2001
From: Arno Fleck <38908098+CodeArno@users.noreply.github.com>
Date: Mon, 2 Jul 2018 13:40:21 +0200
Subject: [PATCH] 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       | 1 +
 .../mapping/internal/optimization/optimization_problem_3d.cc | 4 ++++
 .../internal/optimization/optimization_problem_3d_test.cc    | 1 +
 .../internal/optimization/optimization_problem_options.cc    | 2 ++
 .../proto/pose_graph/optimization_problem_options.proto      | 5 ++++-
 configuration_files/pose_graph.lua                           | 1 +
 6 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc
index bceb588..ad009be 100644
--- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc
+++ b/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,
diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc
index ad12f31..0b525b7 100644
--- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc
+++ b/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());
diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
index 68b39bd..1231f1f 100644
--- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
+++ b/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,
diff --git a/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc
index 360e4be..fb2e443 100644
--- a/cartographer/mapping/internal/optimization/optimization_problem_options.cc
+++ b/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());
diff --git a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto
index 9994adb..6374fb6 100644
--- a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto
+++ b/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;
 
diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua
index 11a073a..eead726 100644
--- a/configuration_files/pose_graph.lua
+++ b/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,