From b38b1cefa02c3fb07df7a81c51fe5743c4552547 Mon Sep 17 00:00:00 2001 From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com> Date: Mon, 15 Jun 2020 10:56:23 +0200 Subject: [PATCH] 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. --- .../mapping/internal/2d/pose_graph_2d_test.cc | 3 +++ .../optimization/optimization_problem_3d.cc | 5 ++++- .../optimization/optimization_problem_3d_test.cc | 3 +++ .../optimization/optimization_problem_options.cc | 6 ++++++ .../pose_graph/optimization_problem_options.proto | 13 +++++++++++-- configuration_files/pose_graph.lua | 3 +++ 6 files changed, 30 insertions(+), 3 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 965ff0a..924f36e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/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, diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 1b8f632..83d1777 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/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()); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 4e3a129..05bfac5 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/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, diff --git a/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc index f329217..a17088a 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_options.cc +++ b/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( diff --git a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto index 6374fb6..efc61c1 100644 --- a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto +++ b/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; diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index 797e407..572cd1a 100644 --- a/configuration_files/pose_graph.lua +++ b/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,