diff --git a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc index 656dadc..349e45d 100644 --- a/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc +++ b/cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc @@ -36,6 +36,10 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( options.set_consecutive_scan_rotation_penalty_factor( parameter_dictionary->GetDouble( "consecutive_scan_rotation_penalty_factor")); + options.set_fixed_frame_pose_translation_weight( + 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_log_solver_summary( parameter_dictionary->GetBool("log_solver_summary")); *options.mutable_ceres_solver_options() = diff --git a/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto index 7dbcca2..578ebef 100644 --- a/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto @@ -18,7 +18,7 @@ package cartographer.mapping.sparse_pose_graph.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 11 +// NEXT ID: 13 message OptimizationProblemOptions { // Scaling parameter for Huber loss function. optional double huber_scale = 1; @@ -29,10 +29,18 @@ message OptimizationProblemOptions { // Scaling parameter for the IMU rotation term. optional double rotation_weight = 9; - // Penalty factors for changes to the relative pose between consecutive scans. + // Penalty factors for translation changes to the relative pose between consecutive scans. optional double consecutive_scan_translation_penalty_factor = 2; + + // Penalty factors for rotation changes to the relative pose between consecutive scans. optional double consecutive_scan_rotation_penalty_factor = 3; + // Scaling parameter for the FixedFramePose translation. + optional double fixed_frame_pose_translation_weight = 11; + + // Scaling parameter for the FixedFramePose rotation. + optional double fixed_frame_pose_rotation_weight = 12; + // If true, the Ceres solver summary will be logged for every optimization. optional bool log_solver_summary = 5; diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index fee364d..5f58885 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -134,6 +134,8 @@ class SparsePoseGraphTest : public ::testing::Test { huber_scale = 1., consecutive_scan_translation_penalty_factor = 0., consecutive_scan_rotation_penalty_factor = 0., + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = true, ceres_solver_options = { use_nonmonotonic_steps = false, diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 28b756e..df2e2ad 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -46,6 +46,8 @@ class OptimizationProblemTest : public ::testing::Test { huber_scale = 1., consecutive_scan_translation_penalty_factor = 1e-2, consecutive_scan_rotation_penalty_factor = 1e-2, + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = true, ceres_solver_options = { use_nonmonotonic_steps = false, diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index b8fffb9..e7d5ce7 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -83,6 +83,8 @@ SPARSE_POSE_GRAPH = { rotation_weight = 3e5, consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5, + fixed_frame_pose_translation_weight = 1e1, + fixed_frame_pose_rotation_weight = 1e2, log_solver_summary = false, ceres_solver_options = { use_nonmonotonic_steps = false, diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index a6c677b..c0b1d92 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -156,10 +156,16 @@ double rotation_weight Scaling parameter for the IMU rotation term. double consecutive_scan_translation_penalty_factor - Penalty factors for changes to the relative pose between consecutive scans. + Penalty factors for translation changes to the relative pose between consecutive scans. double consecutive_scan_rotation_penalty_factor - Not yet documented. + Penalty factors for rotation changes to the relative pose between consecutive scans. + +double fixed_frame_pose_translation_weight + Scaling parameter for the FixedFramePose translation. + +double fixed_frame_pose_rotation_weight + Scaling parameter for the FixedFramePose rotation. bool log_solver_summary If true, the Ceres solver summary will be logged for every optimization.