Add fixed frame pose weight into optimization problem options. (#477)

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() =

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;
 

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,

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,

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,

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.