Separate global optimization constraints for local SLAM and odometry (#1029)

Implements [RFC-0018](https://github.com/googlecartographer/rfcs/blob/master/text/0018-odometry-use-in-optimization-problem.md)

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc

@@ -273,12 +273,28 @@ void OptimizationProblem2D::Solve(
         continue;
       }
 
-      const transform::Rigid3d relative_pose =
+      // Add a relative pose constraint based on the odometry (if available).
-          ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
+      std::unique_ptr<transform::Rigid3d> relative_odometry =
+          CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
+                                        second_node_data);
+      if (relative_odometry != nullptr) {
+        problem.AddResidualBlock(
+            SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
+                *relative_odometry, options_.odometry_translation_weight(),
+                options_.odometry_rotation_weight()}),
+            nullptr /* loss function */, C_nodes.at(first_node_id).data(),
+            C_nodes.at(second_node_id).data());
+      }
+
+      // Add a relative pose constraint based on consecutive local SLAM poses.
+      const transform::Rigid3d relative_local_slam_pose =
+          transform::Embed3D(first_node_data.initial_pose.inverse() *
+                             second_node_data.initial_pose);
       problem.AddResidualBlock(
-          SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
+          SpaCostFunction2D::CreateAutoDiffCostFunction(
-              relative_pose, options_.consecutive_node_translation_weight(),
+              Constraint::Pose{relative_local_slam_pose,
-              options_.consecutive_node_rotation_weight()}),
+                               options_.local_slam_pose_translation_weight(),
+                               options_.local_slam_pose_rotation_weight()}),
           nullptr /* loss function */, C_nodes.at(first_node_id).data(),
           C_nodes.at(second_node_id).data());
     }
@@ -325,7 +341,8 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
           .transform);
 }
 
-transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
+std::unique_ptr<transform::Rigid3d>
+OptimizationProblem2D::CalculateOdometryBetweenNodes(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {
   if (odometry_data_.HasTrajectory(trajectory_id)) {
@@ -334,14 +351,15 @@ transform::Rigid3d OptimizationProblem2D::ComputeRelativePose(
     const std::unique_ptr<transform::Rigid3d> second_node_odometry =
         InterpolateOdometry(trajectory_id, second_node_data.time);
     if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
-      return transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
+      transform::Rigid3d relative_odometry =
-             first_node_odometry->inverse() * (*second_node_odometry) *
+          transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
-             transform::Rigid3d::Rotation(
+          first_node_odometry->inverse() * (*second_node_odometry) *
-                 second_node_data.gravity_alignment.inverse());
+          transform::Rigid3d::Rotation(
+              second_node_data.gravity_alignment.inverse());
+      return common::make_unique<transform::Rigid3d>(relative_odometry);
     }
   }
-  return transform::Embed3D(first_node_data.initial_pose.inverse() *
+  return nullptr;
-                            second_node_data.initial_pose);
 }
 
 }  // namespace pose_graph

cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h

@@ -103,8 +103,8 @@ class OptimizationProblem2D
  private:
   std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
       int trajectory_id, common::Time time) const;
-  // Uses odometry if available, otherwise the local SLAM results.
+  // Computes the relative pose between two nodes based on odometry data.
-  transform::Rigid3d ComputeRelativePose(
+  std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
       int trajectory_id, const NodeData2D& first_node_data,
       const NodeData2D& second_node_data) const;
 

cartographer/mapping/internal/2d/pose_graph_2d_test.cc

@@ -116,8 +116,10 @@ class PoseGraph2DTest : public ::testing::Test {
               acceleration_weight = 1.,
               rotation_weight = 1e2,
               huber_scale = 1.,
-              consecutive_node_translation_weight = 0.,
+              local_slam_pose_translation_weight = 0.,
-              consecutive_node_rotation_weight = 0.,
+              local_slam_pose_rotation_weight = 0.,
+              odometry_translation_weight = 0.,
+              odometry_rotation_weight = 0.,
               fixed_frame_pose_translation_weight = 1e1,
               fixed_frame_pose_rotation_weight = 1e2,
               log_solver_summary = true,

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc

@@ -437,8 +437,8 @@ void OptimizationProblem3D::Solve(
   }
 
   if (options_.fix_z_in_3d()) {
-    // Add penalties for violating odometry or changes between consecutive nodes
+    // Add penalties for violating odometry (if available) and changes between
-    // if odometry is not available.
+    // consecutive nodes.
     for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
       const int trajectory_id = node_it->id.trajectory_id;
       const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
@@ -459,12 +459,29 @@ void OptimizationProblem3D::Solve(
           continue;
         }
 
-        const transform::Rigid3d relative_pose = ComputeRelativePose(
+        // Add a relative pose constraint based on the odometry (if available).
-            trajectory_id, first_node_data, second_node_data);
+        const std::unique_ptr<transform::Rigid3d> relative_odometry =
+            CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
+                                          second_node_data);
+        if (relative_odometry != nullptr) {
+          problem.AddResidualBlock(
+              SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
+                  *relative_odometry, options_.odometry_translation_weight(),
+                  options_.odometry_rotation_weight()}),
+              nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
+              C_nodes.at(first_node_id).translation(),
+              C_nodes.at(second_node_id).rotation(),
+              C_nodes.at(second_node_id).translation());
+        }
+
+        // Add a relative pose constraint based on consecutive local SLAM poses.
+        const transform::Rigid3d relative_local_slam_pose =
+            first_node_data.local_pose.inverse() * second_node_data.local_pose;
         problem.AddResidualBlock(
-            SpaCostFunction3D::CreateAutoDiffCostFunction(Constraint::Pose{
+            SpaCostFunction3D::CreateAutoDiffCostFunction(
-                relative_pose, options_.consecutive_node_translation_weight(),
+                Constraint::Pose{relative_local_slam_pose,
-                options_.consecutive_node_rotation_weight()}),
+                                 options_.local_slam_pose_translation_weight(),
+                                 options_.local_slam_pose_rotation_weight()}),
             nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
             C_nodes.at(first_node_id).translation(),
             C_nodes.at(second_node_id).rotation(),
@@ -572,7 +589,8 @@ void OptimizationProblem3D::Solve(
   }
 }
 
-transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
+std::unique_ptr<transform::Rigid3d>
+OptimizationProblem3D::CalculateOdometryBetweenNodes(
     const int trajectory_id, const NodeData& first_node_data,
     const NodeData& second_node_data) const {
   if (odometry_data_.HasTrajectory(trajectory_id)) {
@@ -581,10 +599,12 @@ transform::Rigid3d OptimizationProblem3D::ComputeRelativePose(
     const std::unique_ptr<transform::Rigid3d> second_node_odometry =
         Interpolate(odometry_data_, trajectory_id, second_node_data.time);
     if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
-      return first_node_odometry->inverse() * (*second_node_odometry);
+      const transform::Rigid3d relative_odometry =
+          first_node_odometry->inverse() * (*second_node_odometry);
+      return common::make_unique<transform::Rigid3d>(relative_odometry);
     }
   }
-  return first_node_data.local_pose.inverse() * second_node_data.local_pose;
+  return nullptr;
 }
 
 }  // namespace pose_graph

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h

@@ -116,8 +116,8 @@ class OptimizationProblem3D
   }
 
  private:
-  // Uses odometry if available, otherwise the local SLAM results.
+  // Computes the relative pose between two nodes based on odometry data.
-  transform::Rigid3d ComputeRelativePose(
+  std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
       int trajectory_id, const NodeData3D& first_node_data,
       const NodeData3D& second_node_data) const;
 

cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc

@@ -42,8 +42,10 @@ class OptimizationProblem3DTest : public ::testing::Test {
           acceleration_weight = 1e-4,
           rotation_weight = 1e-2,
           huber_scale = 1.,
-          consecutive_node_translation_weight = 1e-2,
+          local_slam_pose_translation_weight = 1e-2,
-          consecutive_node_rotation_weight = 1e-2,
+          local_slam_pose_rotation_weight = 1e-2,
+          odometry_translation_weight = 1e-2,
+          odometry_rotation_weight = 1e-2,
           fixed_frame_pose_translation_weight = 1e1,
           fixed_frame_pose_rotation_weight = 1e2,
           log_solver_summary = true,

cartographer/mapping/internal/pose_graph/optimization_problem_options.cc

@@ -30,10 +30,14 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
       parameter_dictionary->GetDouble("acceleration_weight"));
   options.set_rotation_weight(
       parameter_dictionary->GetDouble("rotation_weight"));
-  options.set_consecutive_node_translation_weight(
+  options.set_odometry_translation_weight(
-      parameter_dictionary->GetDouble("consecutive_node_translation_weight"));
+      parameter_dictionary->GetDouble("odometry_translation_weight"));
-  options.set_consecutive_node_rotation_weight(
+  options.set_odometry_rotation_weight(
-      parameter_dictionary->GetDouble("consecutive_node_rotation_weight"));
+      parameter_dictionary->GetDouble("odometry_rotation_weight"));
+  options.set_local_slam_pose_translation_weight(
+      parameter_dictionary->GetDouble("local_slam_pose_translation_weight"));
+  options.set_local_slam_pose_rotation_weight(
+      parameter_dictionary->GetDouble("local_slam_pose_rotation_weight"));
   options.set_fixed_frame_pose_translation_weight(
       parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight"));
   options.set_fixed_frame_pose_rotation_weight(

cartographer/mapping/pose_graph/proto/optimization_problem_options.proto

@@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
-// NEXT ID: 14
+// NEXT ID: 18
 message OptimizationProblemOptions {
   // Scaling parameter for Huber loss function.
   double huber_scale = 1;
@@ -29,11 +29,21 @@ message OptimizationProblemOptions {
   // Scaling parameter for the IMU rotation term.
   double rotation_weight = 9;
 
-  // Scaling parameter for translation between consecutive nodes.
+  // Scaling parameter for translation between consecutive nodes based on the
-  double consecutive_node_translation_weight = 2;
+  // local SLAM pose.
+  double local_slam_pose_translation_weight = 14;
 
-  // Scaling parameter for rotation between consecutive nodes.
+  // Scaling parameter for rotation between consecutive nodes based on the
-  double consecutive_node_rotation_weight = 3;
+  // local SLAM pose.
+  double local_slam_pose_rotation_weight = 15;
+
+  // Scaling parameter for translation between consecutive nodes based on the
+  // odometry.
+  double odometry_translation_weight = 16;
+
+  // Scaling parameter for rotation between consecutive nodes based on the
+  // odometry.
+  double odometry_rotation_weight = 17;
 
   // Scaling parameter for the FixedFramePose translation.
   double fixed_frame_pose_translation_weight = 11;

configuration_files/pose_graph.lua

@@ -65,8 +65,10 @@ POSE_GRAPH = {
     huber_scale = 1e1,
     acceleration_weight = 1e3,
     rotation_weight = 3e5,
-    consecutive_node_translation_weight = 1e5,
+    local_slam_pose_translation_weight = 1e5,
-    consecutive_node_rotation_weight = 1e5,
+    local_slam_pose_rotation_weight = 1e5,
+    odometry_translation_weight = 1e5,
+    odometry_rotation_weight = 1e5,
     fixed_frame_pose_translation_weight = 1e1,
     fixed_frame_pose_rotation_weight = 1e2,
     log_solver_summary = false,

docs/source/configuration.rst

@@ -86,11 +86,17 @@ double acceleration_weight
 double rotation_weight
   Scaling parameter for the IMU rotation term.
 
-double consecutive_node_translation_weight
+double local_slam_pose_translation_weight
-  Scaling parameter for translation between consecutive nodes.
+  Scaling parameter for translation between consecutive nodes based on the local SLAM pose.
 
-double consecutive_node_rotation_weight
+double local_slam_pose_rotation_weight
-  Scaling parameter for rotation between consecutive nodes.
+  Scaling parameter for rotation between consecutive nodes based on the local SLAM pose.
+
+double odometry_translation_weight
+  Scaling parameter for translation between consecutive nodes based on the odometry.
+
+double odometry_rotation_weight
+  Scaling parameter for rotation between consecutive nodes based on the odometry.
 
 double fixed_frame_pose_translation_weight
   Scaling parameter for the FixedFramePose translation.