[modernize] Modernize internal autodiff and numeric_diff logic

Refactor the core automatic and numeric differentiation internal helpers
to utilize C++17 features for improved readability and maintainability:

- Added IntegerSequenceTraits to provide a uniform compile-time interface
  for accessing head and tail of std::integer_sequence.
- Eliminated recursive template meta-programming in autodiff.h, replacing
  it with fold expressions and std::index_sequence.
- Simplified core helper functions like Make1stOrderPerturbation into
  unrolled loops.
- Refactored EvaluateJacobianForParameterBlocks from a recursive struct
  to a recursive function template using if constexpr.
- Updated NumericDiffCostFunction and NumericDiffFirstOrderFunction
  to use the modernized internal helper API.

Change-Id: I934034f7434fc05a8855565b2b534d325f920584

include/ceres/internal/autodiff.h

@@ -177,64 +177,35 @@ namespace ceres::internal {
 //
 // is what would get put in dst if N was 3, offset was 3, and the jet type JetT
 // was 8-dimensional.
-template <int j, int N, int Offset, typename T, typename JetT>
-struct Make1stOrderPerturbation {
- public:
-  inline static void Apply(const T* src, JetT* dst) {
-    if (j == 0) {
-      DCHECK(src);
-      DCHECK(dst);
-    }
-    dst[j] = JetT(src[j], j + Offset);
-    Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst);
-  }
-};
-
 template <int N, int Offset, typename T, typename JetT>
-struct Make1stOrderPerturbation<N, N, Offset, T, JetT> {
- public:
-  static void Apply(const T* /* NOT USED */, JetT* /* NOT USED */) {}
-};
-
-// Calls Make1stOrderPerturbation for every parameter block.
-//
-// Example:
-// If one having three parameter blocks with dimensions (3, 2, 4), the call
-// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x);
-// will result in the following calls to Make1stOrderPerturbation:
-// Make1stOrderPerturbation<0, 3, 0>::Apply(params[0], x + 0);
-// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3);
-// Make1stOrderPerturbation<0, 4, 5>::Apply(params[2], x + 5);
-template <typename Seq, int ParameterIdx = 0, int Offset = 0>
-struct Make1stOrderPerturbations;
-
-template <int N, int... Ns, int ParameterIdx, int Offset>
-struct Make1stOrderPerturbations<std::integer_sequence<int, N, Ns...>,
-                                 ParameterIdx,
-                                 Offset> {
-  template <typename T, typename JetT>
-  inline static void Apply(T const* const* parameters, JetT* x) {
-    Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply(
-        parameters[ParameterIdx], x + Offset);
-    Make1stOrderPerturbations<std::integer_sequence<int, Ns...>,
-                              ParameterIdx + 1,
-                              Offset + N>::Apply(parameters, x);
+inline void Make1stOrderPerturbation(const T* src, JetT* dst) {
+  DCHECK(src);
+  DCHECK(dst);
+  for (int i = 0; i < N; ++i) {
+    dst[i] = JetT(src[i], i + Offset);
   }
-};
+}
 
-// End of 'recursion'. Nothing more to do.
-template <int ParameterIdx, int Total>
-struct Make1stOrderPerturbations<std::integer_sequence<int>,
-                                 ParameterIdx,
-                                 Total> {
-  template <typename T, typename JetT>
-  static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {}
-};
+// Internal non-recursive implementation of Make1stOrderPerturbations.
+template <int... Ns,
+          int... Offsets,
+          std::size_t... BlockIdx,
+          typename T,
+          typename JetT>
+inline void Make1stOrderPerturbationsImpl(
+    T const* const* parameters,
+    JetT* x,
+    std::integer_sequence<int, Ns...>,
+    std::integer_sequence<int, Offsets...>,
+    std::index_sequence<BlockIdx...>) {
+  ((Make1stOrderPerturbation<Ns, Offsets>(parameters[BlockIdx], x + Offsets)),
+   ...);
+}
 
 // Takes the 0th order part of src, assumed to be a Jet type, and puts it in
 // dst. This is used to pick out the "vector" part of the extended y.
 template <typename JetT, typename T>
-inline void Take0thOrderPart(int M, const JetT* src, T dst) {
+inline void Take0thOrderPart(int M, const JetT* src, T* dst) {
   DCHECK(src);
   for (int i = 0; i < M; ++i) {
     dst[i] = src[i].a;
@@ -253,49 +224,24 @@ inline void Take1stOrderPart(const int M, const JetT* src, T* dst) {
   }
 }
 
-// Calls Take1stOrderPart for every parameter block.
-//
-// Example:
-// If one having three parameter blocks with dimensions (3, 2, 4), the call
-// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs,
-//                                                    output,
-//                                                    jacobians);
-// will result in the following calls to Take1stOrderPart:
-// if (jacobians[0]) {
-//   Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]);
-// }
-// if (jacobians[1]) {
-//   Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]);
-// }
-// if (jacobians[2]) {
-//   Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]);
-// }
-template <typename Seq, int ParameterIdx = 0, int Offset = 0>
-struct Take1stOrderParts;
-
-template <int N, int... Ns, int ParameterIdx, int Offset>
-struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>,
-                         ParameterIdx,
-                         Offset> {
-  template <typename JetT, typename T>
-  inline static void Apply(int num_outputs, JetT* output, T** jacobians) {
-    if (jacobians[ParameterIdx]) {
-      Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
-    }
-    Take1stOrderParts<std::integer_sequence<int, Ns...>,
-                      ParameterIdx + 1,
-                      Offset + N>::Apply(num_outputs, output, jacobians);
-  }
-};
-
-// End of 'recursion'. Nothing more to do.
-template <int ParameterIdx, int Offset>
-struct Take1stOrderParts<std::integer_sequence<int>, ParameterIdx, Offset> {
-  template <typename T, typename JetT>
-  static void Apply(int /* NOT USED*/,
-                    JetT* /* NOT USED*/,
-                    T** /* NOT USED */) {}
-};
+// Internal non-recursive implementation of Take1stOrderParts.
+template <int... Ns,
+          int... Offsets,
+          std::size_t... BlockIdx,
+          typename JetT,
+          typename T>
+inline void Take1stOrderPartsImpl(int num_outputs,
+                                  JetT* output,
+                                  T** jacobians,
+                                  std::integer_sequence<int, Ns...>,
+                                  std::integer_sequence<int, Offsets...>,
+                                  std::index_sequence<BlockIdx...>) {
+  ((void)(jacobians[BlockIdx] != nullptr &&
+          (Take1stOrderPart<Offsets, Ns>(
+               num_outputs, output, jacobians[BlockIdx]),
+           true)),
+   ...);
+}
 
 template <int kNumResiduals,
           typename ParameterDims,
@@ -333,15 +279,18 @@ inline bool AutoDifferentiate(const Functor& functor,
   ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK>
       residuals_as_jets(num_outputs);
 
-  // Invalidate the output Jets, so that we can detect if the user
-  // did not assign values to all of them.
   for (int i = 0; i < num_outputs; ++i) {
     residuals_as_jets[i].a = kImpossibleValue;
     residuals_as_jets[i].v.setConstant(kImpossibleValue);
   }
 
-  Make1stOrderPerturbations<Parameters>::Apply(parameters,
-                                               parameters_as_jets.data());
+  using Offsets = ExclusiveScan<Parameters>;
+  Make1stOrderPerturbationsImpl(
+      parameters,
+      parameters_as_jets.data(),
+      Parameters{},
+      Offsets{},
+      std::make_index_sequence<ParameterDims::kNumParameterBlocks>{});
 
   if (!VariadicEvaluate<ParameterDims>(
           functor, unpacked_parameters.data(), residuals_as_jets.data())) {
@@ -349,8 +298,13 @@ inline bool AutoDifferentiate(const Functor& functor,
   }
 
   Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value);
-  Take1stOrderParts<Parameters>::Apply(
-      num_outputs, residuals_as_jets.data(), jacobians);
+  Take1stOrderPartsImpl(
+      num_outputs,
+      residuals_as_jets.data(),
+      jacobians,
+      Parameters{},
+      Offsets{},
+      std::make_index_sequence<ParameterDims::kNumParameterBlocks>{});
 
   return true;
 }

include/ceres/internal/numeric_diff.h

@@ -341,11 +341,12 @@ struct NumericDiff {
                              options.ridders_step_shrink_factor;
 
         // Compute the difference between the previous value and the current.
-        double candidate_error = (std::max)(
-            (current_candidates->col(k) - current_candidates->col(k - 1))
-                .norm(),
-            (current_candidates->col(k) - previous_candidates->col(k - 1))
-                .norm());
+        double candidate_error = (std::max)((current_candidates->col(k) -
+                                             current_candidates->col(k - 1))
+                                                .norm(),
+                                            (current_candidates->col(k) -
+                                             previous_candidates->col(k - 1))
+                                                .norm());
 
         // If the error has decreased, update results.
         if (candidate_error <= norm_error) {
@@ -434,57 +435,64 @@ struct NumericDiff {
 //     return false;
 //   }
 // }
-template <typename ParameterDims,
-          typename Parameters = typename ParameterDims::Parameters,
-          int ParameterIdx = 0>
-struct EvaluateJacobianForParameterBlocks;
+// Internal non-recursive implementation of EvaluateJacobianForParameterBlocks.
+template <NumericDiffMethodType method,
+          int kNumResiduals,
+          typename ParameterDims,
+          int... Ns,
+          std::size_t... BlockIdx,
+          typename CostFunctor>
+inline bool EvaluateJacobianForParameterBlocksImpl(
+    const CostFunctor* functor,
+    const double* residuals_at_eval_point,
+    const NumericDiffOptions& options,
+    int num_residuals,
+    double** parameters,
+    double** jacobians,
+    std::integer_sequence<int, Ns...>,
+    std::index_sequence<BlockIdx...>) {
+  return (... &&
+          (jacobians[BlockIdx] == nullptr ||
+           NumericDiff<
+               CostFunctor,
+               method,
+               kNumResiduals,
+               ParameterDims,
+               BlockIdx,
+               Ns>::EvaluateJacobianForParameterBlock(functor,
+                                                      residuals_at_eval_point,
+                                                      options,
+                                                      num_residuals,
+                                                      BlockIdx,
+                                                      Ns,
+                                                      parameters,
+                                                      jacobians[BlockIdx])));
+}
 
-template <typename ParameterDims, int N, int... Ns, int ParameterIdx>
-struct EvaluateJacobianForParameterBlocks<ParameterDims,
-                                          std::integer_sequence<int, N, Ns...>,
-                                          ParameterIdx> {
-  template <NumericDiffMethodType method,
-            int kNumResiduals,
-            typename CostFunctor>
-  static bool Apply(const CostFunctor* functor,
-                    const double* residuals_at_eval_point,
-                    const NumericDiffOptions& options,
-                    int num_residuals,
-                    double** parameters,
-                    double** jacobians) {
-    if (jacobians[ParameterIdx] != nullptr) {
-      if (!NumericDiff<CostFunctor,
-                       method,
-                       kNumResiduals,
-                       ParameterDims,
-                       ParameterIdx,
-                       N>::
-              EvaluateJacobianForParameterBlock(functor,
-                                                residuals_at_eval_point,
-                                                options,
-                                                num_residuals,
-                                                ParameterIdx,
-                                                N,
-                                                parameters,
-                                                jacobians[ParameterIdx])) {
-        return false;
-      }
-    }
-
-    if constexpr (sizeof...(Ns) > 0) {
-      return EvaluateJacobianForParameterBlocks<
-          ParameterDims,
-          std::integer_sequence<int, Ns...>,
-          ParameterIdx + 1>::template Apply<method, kNumResiduals>(functor,
-                                                                  residuals_at_eval_point,
-                                                                  options,
-                                                                  num_residuals,
-                                                                  parameters,
-                                                                  jacobians);
-    }
-    return true;
-  }
-};
+template <NumericDiffMethodType method,
+          int kNumResiduals,
+          typename ParameterDims,
+          typename Seq = typename ParameterDims::Parameters,
+          typename CostFunctor>
+inline bool EvaluateJacobianForParameterBlocks(
+    const CostFunctor* functor,
+    const double* residuals_at_eval_point,
+    const NumericDiffOptions& options,
+    int num_residuals,
+    double** parameters,
+    double** jacobians) {
+  return EvaluateJacobianForParameterBlocksImpl<method,
+                                                kNumResiduals,
+                                                ParameterDims>(
+      functor,
+      residuals_at_eval_point,
+      options,
+      num_residuals,
+      parameters,
+      jacobians,
+      Seq{},
+      std::make_index_sequence<ParameterDims::kNumParameterBlocks>{});
+}
 
 }  // namespace ceres::internal
 

include/ceres/numeric_diff_cost_function.h

@@ -266,14 +266,15 @@ class NumericDiffCostFunction final
              sizeof(double) * ParameterDims::GetDim(block));
     }
 
-    internal::EvaluateJacobianForParameterBlocks<ParameterDims>::
-        template Apply<kMethod, kNumResiduals>(
-            functor_.get(),
-            residuals,
-            options_,
-            this->num_residuals(),
-            parameters_reference_copy.data(),
-            jacobians);
+    internal::EvaluateJacobianForParameterBlocks<kMethod,
+                                                 kNumResiduals,
+                                                 ParameterDims>(
+        functor_.get(),
+        residuals,
+        options_,
+        this->num_residuals(),
+        parameters_reference_copy.data(),
+        jacobians);
 
     return true;
   }

include/ceres/numeric_diff_first_order_function.h

@@ -148,9 +148,10 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
 
   explicit NumericDiffFirstOrderFunction(FirstOrderFunctor* functor,
                                          Ownership ownership = TAKE_OWNERSHIP)
-      : NumericDiffFirstOrderFunction(std::unique_ptr<FirstOrderFunctor>(functor),
-                                   kNumParameters,
-                                   ownership) {
+      : NumericDiffFirstOrderFunction(
+            std::unique_ptr<FirstOrderFunctor>(functor),
+            kNumParameters,
+            ownership) {
     static_assert(kNumParameters != DYNAMIC,
                   "When kNumParameters is DYNAMIC, the number of parameters "
                   "must be provided as a constructor argument.");
@@ -209,14 +210,16 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
                                                       &parameters_ptr,
                                                       gradient);
     } else {
-      return internal::EvaluateJacobianForParameterBlocks<
-          internal::StaticParameterDims<kNumParameters>>::
-          template Apply<kMethod, 1>(functor_.get(),
-                                     cost,
-                                     options_,
-                                     kNumResiduals,
-                                     &parameters_ptr,
-                                     &gradient);
+      using ParameterDims = internal::StaticParameterDims<kNumParameters>;
+      return internal::EvaluateJacobianForParameterBlocks<kMethod,
+                                                          kNumResiduals,
+                                                          ParameterDims>(
+          functor_.get(),
+          cost,
+          options_,
+          kNumResiduals,
+          &parameters_ptr,
+          &gradient);
     }
   }
 
@@ -225,10 +228,11 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
   const FirstOrderFunctor& functor() const { return *functor_; }
 
  private:
-  explicit NumericDiffFirstOrderFunction(std::unique_ptr<FirstOrderFunctor> functor,
-                                         Ownership ownership,
-                                         int num_parameters,
-                                         const NumericDiffOptions& options)
+  explicit NumericDiffFirstOrderFunction(
+      std::unique_ptr<FirstOrderFunctor> functor,
+      Ownership ownership,
+      int num_parameters,
+      const NumericDiffOptions& options)
       : functor_(std::move(functor)),
         num_parameters_(num_parameters),
         ownership_(ownership),