Let NumericDiffFirstOrderFunction take a dynamically sized parameter vector

Also fix a template naming lint along the way.

Change-Id: Iabb98aeec2ff9609a19c3778b9ea2da37771c985

include/ceres/dynamic_numeric_diff_cost_function.h

@@ -76,7 +76,7 @@ namespace ceres {
 //   cost_function.AddParameterBlock(5);
 //   cost_function.AddParameterBlock(10);
 //   cost_function.SetNumResiduals(21);
-template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>
+template <typename CostFunctor, NumericDiffMethodType kMethod = CENTRAL>
 class DynamicNumericDiffCostFunction final : public DynamicCostFunction {
  public:
   explicit DynamicNumericDiffCostFunction(
@@ -134,7 +134,7 @@ class DynamicNumericDiffCostFunction final : public DynamicCostFunction {
     for (size_t block = 0; block < block_sizes.size(); ++block) {
       if (jacobians[block] != nullptr &&
           !NumericDiff<CostFunctor,
-                       method,
+                       kMethod,
                        ceres::DYNAMIC,
                        internal::DynamicParameterDims,
                        ceres::DYNAMIC,

include/ceres/internal/variadic_evaluate.h

@@ -106,6 +106,29 @@ inline bool VariadicEvaluate(const Functor& functor,
   return VariadicEvaluateImpl<ParameterDims>(functor, input, output, &functor);
 }
 
+// When differentiating dynamically sized CostFunctions, VariadicEvaluate
+// expects a functor with the signature:
+//
+// bool operator()(double const* const* parameters, double* cost) const
+//
+// However for NumericDiffFirstOrderFunction, the functor has the signature
+//
+// bool operator()(double const* parameters, double* cost) const
+//
+// This thin wrapper adapts the latter to the former.
+template <typename Functor>
+class FirstOrderFunctorAdapter {
+ public:
+  explicit FirstOrderFunctorAdapter(const Functor& functor)
+      : functor_(functor) {}
+  bool operator()(double const* const* parameters, double* cost) const {
+    return functor_(*parameters, cost);
+  }
+
+ private:
+  const Functor& functor_;
+};
+
 }  // namespace ceres::internal
 
 #endif  // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_

include/ceres/numeric_diff_cost_function.h

@@ -176,7 +176,7 @@
 namespace ceres {
 
 template <typename CostFunctor,
-          NumericDiffMethodType method = CENTRAL,
+          NumericDiffMethodType kMethod = CENTRAL,
           int kNumResiduals = 0,  // Number of residuals, or ceres::DYNAMIC
           int... Ns>              // Parameters dimensions for each block.
 class NumericDiffCostFunction final
@@ -236,7 +236,7 @@ class NumericDiffCostFunction final
     }
 
     internal::EvaluateJacobianForParameterBlocks<ParameterDims>::
-        template Apply<method, kNumResiduals>(
+        template Apply<kMethod, kNumResiduals>(
             functor_.get(),
             residuals,
             options_,

include/ceres/numeric_diff_first_order_function.h

@@ -42,6 +42,7 @@
 #include "ceres/internal/variadic_evaluate.h"
 #include "ceres/numeric_diff_options.h"
 #include "ceres/types.h"
+#include "glog/logging.h"
 
 namespace ceres {
 
@@ -99,19 +100,55 @@ namespace ceres {
 // "QuadraticCostFunctor", "CENTRAL, 4", describe the finite
 // differencing scheme as "central differencing" and the functor as
 // computing its cost from a 4 dimensional input.
+//
+// If the size of the parameter vector is not known at compile time, then an
+// alternate construction syntax can be used:
+//
+//   FirstOrderFunction* function
+//       = new NumericDiffFirstOrderFunction<MyScalarCostFunctor, CENTRAL>(
+//           new QuadraticCostFunctor(1.0), 4);
+//
+// Note that instead of passing 4 as a template argument, it is now passed as
+// the second argument to the constructor.
 template <typename FirstOrderFunctor,
-          NumericDiffMethodType method,
-          int kNumParameters>
+          NumericDiffMethodType kMethod,
+          int kNumParameters = DYNAMIC>
 class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
  public:
+  // Constructor for the case where the parameter size is known at compile time.
   explicit NumericDiffFirstOrderFunction(
       FirstOrderFunctor* functor,
       Ownership ownership = TAKE_OWNERSHIP,
       const NumericDiffOptions& options = NumericDiffOptions())
-      : functor_(functor), ownership_(ownership), options_(options) {
+      : functor_(functor),
+        num_parameters_(kNumParameters),
+        ownership_(ownership),
+        options_(options) {
+    static_assert(kNumParameters != DYNAMIC,
+                  "Number of parameters must be static when defined via the "
+                  "template parameter. Use the other constructor for "
+                  "dynamically sized functions.");
     static_assert(kNumParameters > 0, "kNumParameters must be positive");
   }
 
+  // Constructor for the case where the parameter size is specified at run time.
+  explicit NumericDiffFirstOrderFunction(
+      FirstOrderFunctor* functor,
+      int num_parameters,
+      Ownership ownership = TAKE_OWNERSHIP,
+      const NumericDiffOptions& options = NumericDiffOptions())
+      : functor_(functor),
+        num_parameters_(num_parameters),
+        ownership_(ownership),
+        options_(options) {
+    static_assert(
+        kNumParameters == DYNAMIC,
+        "Template parameter must be DYNAMIC when using this constructor. If "
+        "you want to provide the number of parameters statically use the other "
+        "constructor.");
+    CHECK_GT(num_parameters, 0);
+  }
+
   ~NumericDiffFirstOrderFunction() override {
     if (ownership_ != TAKE_OWNERSHIP) {
       functor_.release();
@@ -121,12 +158,8 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
   bool Evaluate(const double* const parameters,
                 double* cost,
                 double* gradient) const override {
-    using ParameterDims = internal::StaticParameterDims<kNumParameters>;
-    constexpr int kNumResiduals = 1;
-
     // Get the function value (cost) at the the point to evaluate.
-    if (!internal::VariadicEvaluate<ParameterDims>(
-            *functor_, &parameters, cost)) {
+    if (!(*functor_)(parameters, cost)) {
       return false;
     }
 
@@ -135,27 +168,47 @@ class NumericDiffFirstOrderFunction final : public FirstOrderFunction {
     }
 
     // Create a copy of the parameters which will get mutated.
-    internal::FixedArray<double, 32> parameters_copy(kNumParameters);
-    std::copy_n(parameters, kNumParameters, parameters_copy.data());
+    internal::FixedArray<double, 32> parameters_copy(num_parameters_);
+    std::copy_n(parameters, num_parameters_, parameters_copy.data());
     double* parameters_ptr = parameters_copy.data();
-    internal::EvaluateJacobianForParameterBlocks<
-        ParameterDims>::template Apply<method, kNumResiduals>(functor_.get(),
-                                                              cost,
-                                                              options_,
-                                                              kNumResiduals,
-                                                              &parameters_ptr,
-                                                              &gradient);
-    return true;
+    constexpr int kNumResiduals = 1;
+    if constexpr (kNumParameters == DYNAMIC) {
+      internal::FirstOrderFunctorAdapter<FirstOrderFunctor> fofa(*functor_);
+      return internal::NumericDiff<
+          internal::FirstOrderFunctorAdapter<FirstOrderFunctor>,
+          kMethod,
+          kNumResiduals,
+          internal::DynamicParameterDims,
+          0,
+          DYNAMIC>::EvaluateJacobianForParameterBlock(&fofa,
+                                                      cost,
+                                                      options_,
+                                                      kNumResiduals,
+                                                      0,
+                                                      num_parameters_,
+                                                      &parameters_ptr,
+                                                      gradient);
+    } else {
+      return internal::EvaluateJacobianForParameterBlocks<
+          internal::StaticParameterDims<kNumParameters>>::
+          template Apply<kMethod, 1>(functor_.get(),
+                                     cost,
+                                     options_,
+                                     kNumResiduals,
+                                     &parameters_ptr,
+                                     &gradient);
+    }
   }
 
-  int NumParameters() const override { return kNumParameters; }
+  int NumParameters() const override { return num_parameters_; }
 
   const FirstOrderFunctor& functor() const { return *functor_; }
 
  private:
   std::unique_ptr<FirstOrderFunctor> functor_;
-  Ownership ownership_;
-  NumericDiffOptions options_;
+  const int num_parameters_;
+  const Ownership ownership_;
+  const NumericDiffOptions options_;
 };
 
 }  // namespace ceres