NumericDiffCostFunction supports dynamic number of residuals.

1. Update AutoDiffCostFunction template parameters to be consistent
with NumericDiffCostFunction.

2. Update the documentation for NumericDiffCostFunction and
AutoDiffCostFunction.

Change-Id: I113038abb5bedebb0f6f326f2a4ac31480d785fc

docs/source/building.rst

@@ -1,8 +1,8 @@
 .. _chapter-building:
 
-=====================
+============
-Building Ceres Solver
+Installation
-=====================
+============
 
 Stable Ceres Solver releases are available for download at
 `code.google.com <http://code.google.com/p/ceres-solver/>`_. For the

docs/source/modeling.rst

@@ -164,7 +164,7 @@ residuals and their derivatives. This is done using
    .. code-block:: c++
 
      template <typename CostFunctor,
-            int M,        // Number of residuals, or ceres::DYNAMIC.
+            int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
             int N0,       // Number of parameters in block 0.
             int N1 = 0,   // Number of parameters in block 1.
             int N2 = 0,   // Number of parameters in block 2.
@@ -176,7 +176,7 @@ residuals and their derivatives. This is done using
             int N8 = 0,   // Number of parameters in block 8.
             int N9 = 0>   // Number of parameters in block 9.
      class AutoDiffCostFunction : public
-     SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
+     SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
      };
 
    To get an auto differentiated cost function, you must define a
@@ -254,6 +254,22 @@ residuals and their derivatives. This is done using
    computing a 1-dimensional output from two arguments, both
    2-dimensional.
 
+   :class:`AutoDiffCostFunction` also supports cost functions with a
+   runtime-determined number of residuals. For example:
+
+   .. code-block:: c++
+
+     CostFunction* cost_function
+         = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
+             new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
+             runtime_number_of_residuals); <----+           |     |  |
+                                                |           |     |  |
+                                                |           |     |  |
+               Actual number of residuals ------+           |     |  |
+               Indicate dynamic number of residuals --------+     |  |
+               Dimension of x ------------------------------------+  |
+               Dimension of y ---------------------------------------+
+
    The framework can currently accommodate cost functions of up to 10
    independent variables, and there is no limit on the dimensionality
    of each of them.
@@ -342,12 +358,21 @@ residuals and their derivatives. This is done using
 
     .. code-block:: c++
 
-      template <typename CostFunctionNoJacobian,
+      template <typename CostFunctor,
-                NumericDiffMethod method = CENTRAL, int M = 0,
+                NumericDiffMethod method = CENTRAL,
-                int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
+                int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
-                int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
+                int N0,       // Number of parameters in block 0.
+                int N1 = 0,   // Number of parameters in block 1.
+                int N2 = 0,   // Number of parameters in block 2.
+                int N3 = 0,   // Number of parameters in block 3.
+                int N4 = 0,   // Number of parameters in block 4.
+                int N5 = 0,   // Number of parameters in block 5.
+                int N6 = 0,   // Number of parameters in block 6.
+                int N7 = 0,   // Number of parameters in block 7.
+                int N8 = 0,   // Number of parameters in block 8.
+                int N9 = 0>   // Number of parameters in block 9.
       class NumericDiffCostFunction
-        : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
+        : public SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
       };
 
    To get a numerically differentiated :class:`CostFunction`, you must
@@ -426,6 +451,24 @@ residuals and their derivatives. This is done using
    computing a 1-dimensional output from two arguments, both
    2-dimensional.
 
+   NumericDiffCostFunction also supports cost functions with a
+   runtime-determined number of residuals. For example:
+
+   .. code-block:: c++
+
+     CostFunction* cost_function
+         = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
+             new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
+             TAKE_OWNERSHIP,                                |     |  |
+             runtime_number_of_residuals); <----+           |     |  |
+                                                |           |     |  |
+                                                |           |     |  |
+               Actual number of residuals ------+           |     |  |
+               Indicate dynamic number of residuals --------+     |  |
+               Dimension of x ------------------------------------+  |
+               Dimension of y ---------------------------------------+
+
+
    The framework can currently accommodate cost functions of up to 10
    independent variables, and there is no limit on the dimensionality
    of each of them.

include/ceres/autodiff_cost_function.h

@@ -96,7 +96,7 @@
 // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
 // 1-dimensional output from two arguments, both 2-dimensional.
 //
-// The autodiff cost function also supports cost functions with a
+// AutoDiffCostFunction also supports cost functions with a
 // runtime-determined number of residuals. For example:
 //
 //   CostFunction* cost_function
@@ -110,8 +110,9 @@
 //             Dimension of x ------------------------------------+  |
 //             Dimension of y ---------------------------------------+
 //
-// The framework can currently accommodate cost functions of up to 6 independent
+// The framework can currently accommodate cost functions of up to 10
-// variables, and there is no limit on the dimensionality of each of them.
+// independent variables, and there is no limit on the dimensionality
+// of each of them.
 //
 // WARNING #1: Since the functor will get instantiated with different types for
 // T, you must to convert from other numeric types to T before mixing
@@ -145,13 +146,13 @@ namespace ceres {
 //
 // The constructors take ownership of the cost functor.
 //
-// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the
+// If the number of residuals (argument kNumResiduals below) is
-// two-argument constructor must be used. The second constructor takes a number
+// ceres::DYNAMIC, then the two-argument constructor must be used. The
-// of residuals (in addition to the templated number of residuals). This allows
+// second constructor takes a number of residuals (in addition to the
-// for varying the number of residuals for a single autodiff cost function at
+// templated number of residuals). This allows for varying the number
-// runtime.
+// of residuals for a single autodiff cost function at runtime.
 template <typename CostFunctor,
-          int M,        // Number of residuals, or ceres::DYNAMIC.
+          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
           int N0,       // Number of parameters in block 0.
           int N1 = 0,   // Number of parameters in block 1.
           int N2 = 0,   // Number of parameters in block 2.
@@ -162,28 +163,30 @@ template <typename CostFunctor,
           int N7 = 0,   // Number of parameters in block 7.
           int N8 = 0,   // Number of parameters in block 8.
           int N9 = 0>   // Number of parameters in block 9.
-class AutoDiffCostFunction : public SizedCostFunction<M,
+class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
                                                       N0, N1, N2, N3, N4,
                                                       N5, N6, N7, N8, N9> {
  public:
   // Takes ownership of functor. Uses the template-provided value for the
-  // number of residuals ("M").
+  // number of residuals ("kNumResiduals").
   explicit AutoDiffCostFunction(CostFunctor* functor)
       : functor_(functor) {
-    CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the "
+    CHECK_NE(kNumResiduals, DYNAMIC)
-                         << "number of residuals is set to ceres::DYNAMIC.";
+        << "Can't run the fixed-size constructor if the "
+        << "number of residuals is set to ceres::DYNAMIC.";
   }
 
-  // Takes ownership of functor. Ignores the template-provided number of
+  // Takes ownership of functor. Ignores the template-provided
-  // residuals ("M") in favor of the "num_residuals" argument provided.
+  // kNumResiduals in favor of the "num_residuals" argument provided.
   //
   // This allows for having autodiff cost functions which return varying
   // numbers of residuals at runtime.
   AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
       : functor_(functor) {
-    CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the "
+    CHECK_EQ(kNumResiduals, DYNAMIC)
-                         << "number of residuals is not ceres::DYNAMIC.";
+        << "Can't run the dynamic-size constructor if the "
-    SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
+        << "number of residuals is not ceres::DYNAMIC.";
+    SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
         ::set_num_residuals(num_residuals);
   }
 
@@ -206,7 +209,7 @@ class AutoDiffCostFunction : public SizedCostFunction<M,
            N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
                *functor_,
                parameters,
-               SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
+               SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
                    ::num_residuals(),
                residuals,
                jacobians);

include/ceres/internal/numeric_diff.h

@@ -90,6 +90,7 @@ struct NumericDiff {
       const CostFunctor* functor,
       double const* residuals_at_eval_point,
       const double relative_step_size,
+      int num_residuals,
       double **parameters,
       double *jacobian) {
     using Eigen::Map;
@@ -97,15 +98,21 @@ struct NumericDiff {
     using Eigen::RowMajor;
     using Eigen::ColMajor;
 
+    const int NUM_RESIDUALS =
+        (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals);
+
     typedef Matrix<double, kNumResiduals, 1> ResidualVector;
     typedef Matrix<double, kParameterBlockSize, 1> ParameterVector;
-    typedef Matrix<double, kNumResiduals, kParameterBlockSize,
+    typedef Matrix<double,
+                   kNumResiduals,
+                   kParameterBlockSize,
                    (kParameterBlockSize == 1 &&
-                    kNumResiduals > 1) ? ColMajor : RowMajor> JacobianMatrix;
+                    kNumResiduals > 1) ? ColMajor : RowMajor>
+        JacobianMatrix;
 
 
     Map<JacobianMatrix> parameter_jacobian(jacobian,
-                                           kNumResiduals,
+                                           NUM_RESIDUALS,
                                            kParameterBlockSize);
 
     // Mutate 1 element at a time and then restore.
@@ -125,16 +132,16 @@ struct NumericDiff {
 
     // For each parameter in the parameter block, use finite differences to
     // compute the derivative for that parameter.
+
+    ResidualVector residuals(NUM_RESIDUALS);
     for (int j = 0; j < kParameterBlockSize; ++j) {
       const double delta =
           (step_size(j) == 0.0) ? fallback_step_size : step_size(j);
 
       x_plus_delta(j) = x(j) + delta;
 
-      double residuals[kNumResiduals];  // NOLINT
-
       if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
-              functor, parameters, residuals, functor)) {
+              functor, parameters, residuals.data(), functor)) {
         return false;
       }
 
@@ -142,8 +149,7 @@ struct NumericDiff {
       // 1. Store residuals for the forward part.
       // 2. Subtract residuals for the backward (or 0) part.
       // 3. Divide out the run.
-      parameter_jacobian.col(j) =
+      parameter_jacobian.col(j) = residuals;
-          Map<const ResidualVector>(residuals, kNumResiduals);
 
       double one_over_delta = 1.0 / delta;
       if (kMethod == CENTRAL) {
@@ -151,17 +157,16 @@ struct NumericDiff {
         x_plus_delta(j) = x(j) - delta;
 
         if (!EvaluateImpl<CostFunctor, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>(
-                functor, parameters, residuals, functor)) {
+                functor, parameters, residuals.data(), functor)) {
           return false;
         }
 
-        parameter_jacobian.col(j) -=
+        parameter_jacobian.col(j) -= residuals;
-            Map<ResidualVector>(residuals, kNumResiduals, 1);
         one_over_delta /= 2;
       } else {
         // Forward difference only; reuse existing residuals evaluation.
         parameter_jacobian.col(j) -=
-            Map<const ResidualVector>(residuals_at_eval_point, kNumResiduals);
+            Map<const ResidualVector>(residuals_at_eval_point, NUM_RESIDUALS);
       }
       x_plus_delta(j) = x(j);  // Restore x_plus_delta.
 
@@ -186,6 +191,7 @@ struct NumericDiff<CostFunctor, kMethod, kNumResiduals,
       const CostFunctor* functor,
       double const* residuals_at_eval_point,
       const double relative_step_size,
+      const int num_residuals,
       double **parameters,
       double *jacobian) {
     LOG(FATAL) << "Control should never reach here.";

include/ceres/numeric_diff_cost_function.h

@@ -95,6 +95,21 @@
 // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing
 // a 1-dimensional output from two arguments, both 2-dimensional.
 //
+// NumericDiffCostFunction also supports cost functions with a
+// runtime-determined number of residuals. For example:
+//
+//   CostFunction* cost_function
+//       = new NumericDiffCostFunction<MyScalarCostFunctor, CENTRAL, DYNAMIC, 2, 2>(
+//           new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
+//           TAKE_OWNERSHIP,                                |     |  |
+//           runtime_number_of_residuals); <----+           |     |  |
+//                                              |           |     |  |
+//                                              |           |     |  |
+//             Actual number of residuals ------+           |     |  |
+//             Indicate dynamic number of residuals --------+     |  |
+//             Dimension of x ------------------------------------+  |
+//             Dimension of y ---------------------------------------+
+//
 // The framework can currently accommodate cost functions of up to 10
 // independent variables, and there is no limit on the dimensionality
 // of each of them.
@@ -104,8 +119,6 @@
 // central differences begin with, and only after that works, trying forward
 // difference to improve performance.
 //
-// TODO(sameeragarwal): Add support for dynamic number of residuals.
-//
 // WARNING #1: A common beginner's error when first using
 // NumericDiffCostFunction is to get the sizing wrong. In particular,
 // there is a tendency to set the template parameters to (dimension of
@@ -177,17 +190,17 @@ class NumericDiffCostFunction
                                N5, N6, N7, N8, N9> {
  public:
   NumericDiffCostFunction(CostFunctor* functor,
+                          Ownership ownership = TAKE_OWNERSHIP,
+                          int num_residuals = kNumResiduals,
                           const double relative_step_size = 1e-6)
       :functor_(functor),
-       ownership_(TAKE_OWNERSHIP),
+       ownership_(ownership),
-       relative_step_size_(relative_step_size) {}
+       relative_step_size_(relative_step_size) {
-
+    if (kNumResiduals == DYNAMIC) {
-  NumericDiffCostFunction(CostFunctor* functor,
+      SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
-                          Ownership ownership,
+          ::set_num_residuals(num_residuals);
-                          const double relative_step_size = 1e-6)
+    }
-      : functor_(functor),
+  }
-        ownership_(ownership),
-        relative_step_size_(relative_step_size) {}
 
   ~NumericDiffCostFunction() {
     if (ownership_ != TAKE_OWNERSHIP) {
@@ -216,7 +229,7 @@ class NumericDiffCostFunction
       return false;
     }
 
-    if (!jacobians) {
+    if (jacobians == NULL) {
       return true;
     }
 
@@ -264,6 +277,7 @@ class NumericDiffCostFunction
                            functor_.get(),                              \
                            residuals,                                   \
                            relative_step_size_,                         \
+                           SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::num_residuals(), \
                            parameters_reference_copy.get(),             \
                            jacobians[block])) {                         \
         return false;                                                   \

internal/ceres/numeric_diff_cost_function_test.cc

@@ -184,5 +184,18 @@ TEST(NumericDiffCostFunction, EigenRowMajorColMajorTest) {
           new SizeTestingCostFunction<2,2>, ceres::TAKE_OWNERSHIP));
 }
 
+TEST(NumericDiffCostFunction, EasyCaseFunctorCentralDifferencesAndDynamicNumResiduals) {
+  internal::scoped_ptr<CostFunction> cost_function;
+  cost_function.reset(
+      new NumericDiffCostFunction<EasyFunctor,
+                                  CENTRAL,
+                                  ceres::DYNAMIC,
+                                  5,  /* size of x1 */
+                                  5   /* size of x2 */>(
+                                      new EasyFunctor, TAKE_OWNERSHIP, 3));
+  EasyFunctor functor;
+  functor.ExpectCostFunctionEvaluationIsNearlyCorrect(*cost_function, CENTRAL);
+}
+
 }  // namespace internal
 }  // namespace ceres