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,