Create AutoDiffCostFunction in cost functions. (#718)

Creating a ceres::AutoDiffCostFunction requires specifying numbers
of residuals and variables, so it is safer to implement this within
the cost functions, which know best.

cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h

@@ -33,16 +33,16 @@ namespace scan_matching {
 // at pixels with lower values.
 class OccupiedSpaceCostFunction {
  public:
-  OccupiedSpaceCostFunction(const double scaling_factor,
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
-                            const sensor::PointCloud& point_cloud,
+      const double scaling_factor, const sensor::PointCloud& point_cloud,
-                            const ProbabilityGrid& probability_grid)
+      const ProbabilityGrid& probability_grid) {
-      : scaling_factor_(scaling_factor),
+    return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
-        point_cloud_(point_cloud),
+                                           ceres::DYNAMIC /* residuals */,
-        probability_grid_(probability_grid) {}
+                                           3 /* pose variables */>(
-
+        new OccupiedSpaceCostFunction(scaling_factor, point_cloud,
-  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
+                                      probability_grid),
-  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
+        point_cloud.size());
-      delete;
+  }
 
   template <typename T>
   bool operator()(const T* const pose, T* residual) const {
@@ -105,6 +105,17 @@ class OccupiedSpaceCostFunction {
     const ProbabilityGrid& probability_grid_;
   };
 
+  OccupiedSpaceCostFunction(const double scaling_factor,
+                            const sensor::PointCloud& point_cloud,
+                            const ProbabilityGrid& probability_grid)
+      : scaling_factor_(scaling_factor),
+        point_cloud_(point_cloud),
+        probability_grid_(probability_grid) {}
+
+  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
+  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
+      delete;
+
   const double scaling_factor_;
   const sensor::PointCloud& point_cloud_;
   const ProbabilityGrid& probability_grid_;

cartographer/internal/mapping_3d/acceleration_cost_function.h

@@ -26,17 +26,19 @@ namespace mapping_3d {
 // Penalizes differences between IMU data and optimized accelerations.
 class AccelerationCostFunction {
  public:
-  AccelerationCostFunction(const double scaling_factor,
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const double scaling_factor,
       const Eigen::Vector3d& delta_velocity_imu_frame,
       const double first_delta_time_seconds,
-                           const double second_delta_time_seconds)
+      const double second_delta_time_seconds) {
-      : scaling_factor_(scaling_factor),
+    return new ceres::AutoDiffCostFunction<
-        delta_velocity_imu_frame_(delta_velocity_imu_frame),
+        AccelerationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
-        first_delta_time_seconds_(first_delta_time_seconds),
+        3 /* position variables */, 3 /* position variables */,
-        second_delta_time_seconds_(second_delta_time_seconds) {}
+        3 /* position variables */, 1 /* gravity variables */,
-
+        4 /* rotation variables */>(new AccelerationCostFunction(
-  AccelerationCostFunction(const AccelerationCostFunction&) = delete;
+        scaling_factor, delta_velocity_imu_frame, first_delta_time_seconds,
-  AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
+        second_delta_time_seconds));
+  }
 
   template <typename T>
   bool operator()(const T* const middle_rotation, const T* const start_position,
@@ -69,6 +71,18 @@ class AccelerationCostFunction {
   }
 
  private:
+  AccelerationCostFunction(const double scaling_factor,
+                           const Eigen::Vector3d& delta_velocity_imu_frame,
+                           const double first_delta_time_seconds,
+                           const double second_delta_time_seconds)
+      : scaling_factor_(scaling_factor),
+        delta_velocity_imu_frame_(delta_velocity_imu_frame),
+        first_delta_time_seconds_(first_delta_time_seconds),
+        second_delta_time_seconds_(second_delta_time_seconds) {}
+
+  AccelerationCostFunction(const AccelerationCostFunction&) = delete;
+  AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
+
   template <typename T>
   static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
     return Eigen::Quaternion<T>(quaternion[0], quaternion[1], quaternion[2],

cartographer/internal/mapping_3d/rotation_cost_function.h

@@ -26,13 +26,14 @@ namespace mapping_3d {
 // Penalizes differences between IMU data and optimized orientations.
 class RotationCostFunction {
  public:
-  RotationCostFunction(const double scaling_factor,
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
-                       const Eigen::Quaterniond& delta_rotation_imu_frame)
+      const double scaling_factor,
-      : scaling_factor_(scaling_factor),
+      const Eigen::Quaterniond& delta_rotation_imu_frame) {
-        delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
+    return new ceres::AutoDiffCostFunction<
-
+        RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
-  RotationCostFunction(const RotationCostFunction&) = delete;
+        4 /* rotation variables */, 4 /* rotation variables */
-  RotationCostFunction& operator=(const RotationCostFunction&) = delete;
+        >(new RotationCostFunction(scaling_factor, delta_rotation_imu_frame));
+  }
 
   template <typename T>
   bool operator()(const T* const start_rotation, const T* const end_rotation,
@@ -54,6 +55,14 @@ class RotationCostFunction {
   }
 
  private:
+  RotationCostFunction(const double scaling_factor,
+                       const Eigen::Quaterniond& delta_rotation_imu_frame)
+      : scaling_factor_(scaling_factor),
+        delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
+
+  RotationCostFunction(const RotationCostFunction&) = delete;
+  RotationCostFunction& operator=(const RotationCostFunction&) = delete;
+
   const double scaling_factor_;
   const Eigen::Quaterniond delta_rotation_imu_frame_;
 };

cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h

@@ -33,16 +33,15 @@ namespace scan_matching {
 // occupied space, i.e. at voxels with lower values.
 class OccupiedSpaceCostFunction {
  public:
-  OccupiedSpaceCostFunction(const double scaling_factor,
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
-                            const sensor::PointCloud& point_cloud,
+      const double scaling_factor, const sensor::PointCloud& point_cloud,
-                            const HybridGrid& hybrid_grid)
+      const HybridGrid& hybrid_grid) {
-      : scaling_factor_(scaling_factor),
+    return new ceres::AutoDiffCostFunction<
-        point_cloud_(point_cloud),
+        OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */,
-        interpolated_grid_(hybrid_grid) {}
+        3 /* translation variables */, 4 /* rotation variables */>(
-
+        new OccupiedSpaceCostFunction(scaling_factor, point_cloud, hybrid_grid),
-  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
+        point_cloud.size());
-  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
+  }
-      delete;
 
   template <typename T>
   bool operator()(const T* const translation, const T* const rotation,
@@ -54,6 +53,18 @@ class OccupiedSpaceCostFunction {
     return Evaluate(transform, residual);
   }
 
+ private:
+  OccupiedSpaceCostFunction(const double scaling_factor,
+                            const sensor::PointCloud& point_cloud,
+                            const HybridGrid& hybrid_grid)
+      : scaling_factor_(scaling_factor),
+        point_cloud_(point_cloud),
+        interpolated_grid_(hybrid_grid) {}
+
+  OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
+  OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
+      delete;
+
   template <typename T>
   bool Evaluate(const transform::Rigid3<T>& transform,
                 T* const residual) const {
@@ -67,7 +78,6 @@ class OccupiedSpaceCostFunction {
     return true;
   }
 
- private:
   const double scaling_factor_;
   const sensor::PointCloud& point_cloud_;
   const InterpolatedGrid interpolated_grid_;

cartographer/mapping_2d/pose_graph/optimization_problem.cc

@@ -152,8 +152,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     problem.AddResidualBlock(
-        new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
+        SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
-            new SpaCostFunction(constraint.pose)),
         // Only loop closure constraints should have a loss function.
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
@@ -187,10 +186,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       const transform::Rigid3d relative_pose =
           ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
       problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
+          SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
-              new SpaCostFunction(Constraint::Pose{
               relative_pose, options_.consecutive_node_translation_weight(),
-                  options_.consecutive_node_rotation_weight()})),
+              options_.consecutive_node_rotation_weight()}),
           nullptr /* loss function */, C_nodes.at(first_node_id).data(),
           C_nodes.at(second_node_id).data());
     }

cartographer/mapping_2d/pose_graph/spa_cost_function.h

@@ -36,6 +36,21 @@ class SpaCostFunction {
  public:
   using Constraint = mapping::PoseGraph::Constraint;
 
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const Constraint::Pose& pose) {
+    return new ceres::AutoDiffCostFunction<SpaCostFunction, 3 /* residuals */,
+                                           3 /* pose variables */,
+                                           3 /* pose variables */>(
+        new SpaCostFunction(pose));
+  }
+
+  template <typename T>
+  bool operator()(const T* const c_i, const T* const c_j, T* e) const {
+    ComputeScaledError(pose_, c_i, c_j, e);
+    return true;
+  }
+
+ private:
   explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
 
   // Computes the error between the node-to-submap alignment 'zbar_ij' and the
@@ -71,13 +86,6 @@ class SpaCostFunction {
     e[2] = e_ij[2] * T(pose.rotation_weight);
   }
 
-  template <typename T>
-  bool operator()(const T* const c_i, const T* const c_j, T* e) const {
-    ComputeScaledError(pose_, c_i, c_j, e);
-    return true;
-  }
-
- private:
   const Constraint::Pose pose_;
 };
 

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc

@@ -71,26 +71,21 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
   ceres::Problem problem;
   CHECK_GT(options_.occupied_space_weight(), 0.);
   problem.AddResidualBlock(
-      new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction, ceres::DYNAMIC,
+      OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
-                                      3>(
-          new OccupiedSpaceCostFunction(
           options_.occupied_space_weight() /
               std::sqrt(static_cast<double>(point_cloud.size())),
           point_cloud, probability_grid),
-          point_cloud.size()),
+      nullptr /* loss function */, ceres_pose_estimate);
-      nullptr, ceres_pose_estimate);
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
-      new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
+      TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
-          new TranslationDeltaCostFunctor(options_.translation_weight(),
+          options_.translation_weight(), target_translation),
-                                          target_translation)),
+      nullptr /* loss function */, ceres_pose_estimate);
-      nullptr, ceres_pose_estimate);
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(
-      new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1, 3>(
+      RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
-          new RotationDeltaCostFunctor(options_.rotation_weight(),
+          options_.rotation_weight(), ceres_pose_estimate[2]),
-                                       ceres_pose_estimate[2])),
+      nullptr /* loss function */, ceres_pose_estimate);
-      nullptr, ceres_pose_estimate);
 
   ceres::Solve(ceres_solver_options_, &problem, summary);
 

cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h

@@ -27,13 +27,12 @@ namespace scan_matching {
 // the solution's distance from 'target_angle'.
 class RotationDeltaCostFunctor {
  public:
-  // Constructs a new RotationDeltaCostFunctor for the given 'angle'.
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
-  explicit RotationDeltaCostFunctor(const double scaling_factor,
+      const double scaling_factor, const double target_angle) {
-                                    const double target_angle)
+    return new ceres::AutoDiffCostFunction<
-      : scaling_factor_(scaling_factor), angle_(target_angle) {}
+        RotationDeltaCostFunctor, 1 /* residuals */, 3 /* pose variables */>(
-
+        new RotationDeltaCostFunctor(scaling_factor, target_angle));
-  RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
+  }
-  RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
 
   template <typename T>
   bool operator()(const T* const pose, T* residual) const {
@@ -42,6 +41,13 @@ class RotationDeltaCostFunctor {
   }
 
  private:
+  explicit RotationDeltaCostFunctor(const double scaling_factor,
+                                    const double target_angle)
+      : scaling_factor_(scaling_factor), angle_(target_angle) {}
+
+  RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
+  RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
+
   const double scaling_factor_;
   const double angle_;
 };

cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h

@@ -27,6 +27,21 @@ namespace scan_matching {
 // Cost increases with the solution's distance from 'target_translation'.
 class TranslationDeltaCostFunctor {
  public:
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const double scaling_factor, const Eigen::Vector2d& target_translation) {
+    return new ceres::AutoDiffCostFunction<
+        TranslationDeltaCostFunctor, 2 /* residuals */, 3 /* pose variables */>(
+        new TranslationDeltaCostFunctor(scaling_factor, target_translation));
+  }
+
+  template <typename T>
+  bool operator()(const T* const pose, T* residual) const {
+    residual[0] = scaling_factor_ * (pose[0] - x_);
+    residual[1] = scaling_factor_ * (pose[1] - y_);
+    return true;
+  }
+
+ private:
   // Constructs a new TranslationDeltaCostFunctor from the given
   // 'target_translation' (x, y).
   explicit TranslationDeltaCostFunctor(
@@ -39,14 +54,6 @@ class TranslationDeltaCostFunctor {
   TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
       delete;
 
-  template <typename T>
-  bool operator()(const T* const pose, T* residual) const {
-    residual[0] = scaling_factor_ * (pose[0] - x_);
-    residual[1] = scaling_factor_ * (pose[1] - y_);
-    return true;
-  }
-
- private:
   const double scaling_factor_;
   const double x_;
   const double y_;

cartographer/mapping_3d/pose_graph/optimization_problem.cc

@@ -222,12 +222,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     problem.AddResidualBlock(
-        new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
+        SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
-            new SpaCostFunction(constraint.pose)),
         // Only loop closure constraints should have a loss function.
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
-            : nullptr,
+            : nullptr /* loss function */,
         C_submaps.at(constraint.submap_id).rotation(),
         C_submaps.at(constraint.submap_id).translation(),
         C_nodes.at(constraint.node_id).rotation(),
@@ -301,13 +300,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
                result_to_first_center.delta_rotation) *
               result_center_to_center.delta_velocity;
           problem.AddResidualBlock(
-              new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
+              AccelerationCostFunction::CreateAutoDiffCostFunction(
-                                              3, 3, 1, 4>(
-                  new AccelerationCostFunction(
                   options_.acceleration_weight(), delta_velocity,
                   common::ToSeconds(first_duration),
-                      common::ToSeconds(second_duration))),
+                  common::ToSeconds(second_duration)),
-              nullptr, C_nodes.at(second_node_id).rotation(),
+              nullptr /* loss function */,
+              C_nodes.at(second_node_id).rotation(),
               C_nodes.at(first_node_id).translation(),
               C_nodes.at(second_node_id).translation(),
               C_nodes.at(third_node_id).translation(),
@@ -315,10 +313,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
               trajectory_data.imu_calibration.data());
         }
         problem.AddResidualBlock(
-            new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
+            RotationCostFunction::CreateAutoDiffCostFunction(
-                new RotationCostFunction(options_.rotation_weight(),
+                options_.rotation_weight(), result.delta_rotation),
-                                         result.delta_rotation)),
+            nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
-            nullptr, C_nodes.at(first_node_id).rotation(),
             C_nodes.at(second_node_id).rotation(),
             trajectory_data.imu_calibration.data());
       }
@@ -351,11 +348,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
         const transform::Rigid3d relative_pose = ComputeRelativePose(
             trajectory_id, first_node_data, second_node_data);
         problem.AddResidualBlock(
-            new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
+            SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
-                new SpaCostFunction(Constraint::Pose{
+                relative_pose, options_.consecutive_node_translation_weight(),
-                    relative_pose,
+                options_.consecutive_node_rotation_weight()}),
-                    options_.consecutive_node_translation_weight(),
-                    options_.consecutive_node_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(),
@@ -406,9 +401,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       }
 
       problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
+          SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
-              new SpaCostFunction(constraint_pose)),
+          nullptr /* loss function */, C_fixed_frames.back().rotation(),
-          nullptr, C_fixed_frames.back().rotation(),
           C_fixed_frames.back().translation(), C_nodes.at(node_id).rotation(),
           C_nodes.at(node_id).translation());
     }

cartographer/mapping_3d/pose_graph/spa_cost_function.h

@@ -36,6 +36,24 @@ class SpaCostFunction {
  public:
   using Constraint = mapping::PoseGraph::Constraint;
 
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const Constraint::Pose& pose) {
+    return new ceres::AutoDiffCostFunction<
+        SpaCostFunction, 6 /* residuals */, 4 /* rotation variables */,
+        3 /* translation variables */, 4 /* rotation variables */,
+        3 /* translation variables */>(new SpaCostFunction(pose));
+  }
+
+  template <typename T>
+  bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
+                  const T* const c_j_rotation, const T* const c_j_translation,
+                  T* const e) const {
+    ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
+                       c_j_translation, e);
+    return true;
+  }
+
+ private:
   explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
 
   // Computes the error between the node-to-submap alignment 'zbar_ij' and the
@@ -90,16 +108,6 @@ class SpaCostFunction {
     }
   }
 
-  template <typename T>
-  bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
-                  const T* const c_j_rotation, const T* const c_j_translation,
-                  T* const e) const {
-    ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
-                       c_j_translation, e);
-    return true;
-  }
-
- private:
   const Constraint::Pose pose_;
 };
 

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc

@@ -93,27 +93,23 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
         *point_clouds_and_hybrid_grids[i].first;
     const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
     problem.AddResidualBlock(
-        new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
+        OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
-                                        ceres::DYNAMIC, 3, 4>(
-            new OccupiedSpaceCostFunction(
             options_.occupied_space_weight(i) /
                 std::sqrt(static_cast<double>(point_cloud.size())),
             point_cloud, hybrid_grid),
-            point_cloud.size()),
+        nullptr /* loss function */, ceres_pose.translation(),
-        nullptr, ceres_pose.translation(), ceres_pose.rotation());
+        ceres_pose.rotation());
   }
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
-      new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>(
+      TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
-          new TranslationDeltaCostFunctor(options_.translation_weight(),
+          options_.translation_weight(), target_translation),
-                                          target_translation)),
+      nullptr /* loss function */, ceres_pose.translation());
-      nullptr, ceres_pose.translation());
   CHECK_GT(options_.rotation_weight(), 0.);
   problem.AddResidualBlock(
-      new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 3, 4>(
+      RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
-          new RotationDeltaCostFunctor(options_.rotation_weight(),
+          options_.rotation_weight(), initial_pose_estimate.rotation()),
-                                       initial_pose_estimate.rotation())),
+      nullptr /* loss function */, ceres_pose.rotation());
-      nullptr, ceres_pose.rotation());
 
   ceres::Solve(ceres_solver_options_, &problem, summary);
 

cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h

@@ -30,19 +30,14 @@ namespace scan_matching {
 // Cost increases with the solution's distance from 'target_rotation'.
 class RotationDeltaCostFunctor {
  public:
-  // Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
-  explicit RotationDeltaCostFunctor(const double scaling_factor,
+      const double scaling_factor, const Eigen::Quaterniond& target_rotation) {
-                                    const Eigen::Quaterniond& target_rotation)
+    return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor,
-      : scaling_factor_(scaling_factor) {
+                                           3 /* residuals */,
-    target_rotation_inverse_[0] = target_rotation.w();
+                                           4 /* rotation variables */>(
-    target_rotation_inverse_[1] = -target_rotation.x();
+        new RotationDeltaCostFunctor(scaling_factor, target_rotation));
-    target_rotation_inverse_[2] = -target_rotation.y();
-    target_rotation_inverse_[3] = -target_rotation.z();
   }
 
-  RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
-  RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
-
   template <typename T>
   bool operator()(const T* const rotation_quaternion, T* residual) const {
     T delta[4];
@@ -60,6 +55,19 @@ class RotationDeltaCostFunctor {
   }
 
  private:
+  // Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
+  explicit RotationDeltaCostFunctor(const double scaling_factor,
+                                    const Eigen::Quaterniond& target_rotation)
+      : scaling_factor_(scaling_factor) {
+    target_rotation_inverse_[0] = target_rotation.w();
+    target_rotation_inverse_[1] = -target_rotation.x();
+    target_rotation_inverse_[2] = -target_rotation.y();
+    target_rotation_inverse_[3] = -target_rotation.z();
+  }
+
+  RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
+  RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
+
   const double scaling_factor_;
   double target_rotation_inverse_[4];
 };

cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h

@@ -27,6 +27,23 @@ namespace scan_matching {
 // Cost increases with the solution's distance from 'target_translation'.
 class TranslationDeltaCostFunctor {
  public:
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const double scaling_factor, const Eigen::Vector3d& target_translation) {
+    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor,
+                                           3 /* residuals */,
+                                           3 /* translation variables */>(
+        new TranslationDeltaCostFunctor(scaling_factor, target_translation));
+  }
+
+  template <typename T>
+  bool operator()(const T* const translation, T* residual) const {
+    residual[0] = scaling_factor_ * (translation[0] - x_);
+    residual[1] = scaling_factor_ * (translation[1] - y_);
+    residual[2] = scaling_factor_ * (translation[2] - z_);
+    return true;
+  }
+
+ private:
   // Constructs a new TranslationDeltaCostFunctor from the given
   // 'target_translation'.
   explicit TranslationDeltaCostFunctor(
@@ -40,15 +57,6 @@ class TranslationDeltaCostFunctor {
   TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
       delete;
 
-  template <typename T>
-  bool operator()(const T* const translation, T* residual) const {
-    residual[0] = scaling_factor_ * (translation[0] - x_);
-    residual[1] = scaling_factor_ * (translation[1] - y_);
-    residual[2] = scaling_factor_ * (translation[2] - z_);
-    return true;
-  }
-
- private:
   const double scaling_factor_;
   const double x_;
   const double y_;