Support varying numbers of residuals in autodiff.

This commit modifies the only function in autodiff that takes a
templated number of outputs (i.e. residuals) and makes that
template parameter a normal parameter. With that change, it
is a trivial matter to support a dynamic number of residuals.

The API for dynamic residuals is to pass a fake number of
residuals as the second template argument to
AutoDiffCostFunction, and to pass the real number of
parameters as a second constructor argument.

include/ceres/autodiff_cost_function.h

@@ -93,6 +93,20 @@
 // "MyScalarCostFunction", "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
+// runtime-determined number of residuals. For example:
+//
+//   CostFunction* cost_function
+//       = new AutoDiffCostFunction<MyScalarCostFunction, DYNAMIC, 2, 2>(
+//           new CostFunctionWithDynamicNumResiduals(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 6 independent
 // variables, and there is no limit on the dimensionality of each of them.
 //
@@ -115,18 +129,26 @@
 #include "ceres/internal/autodiff.h"
 #include "ceres/internal/scoped_ptr.h"
 #include "ceres/sized_cost_function.h"
+#include "ceres/types.h"
 
 namespace ceres {
 
-// A cost function which computes the derivative of the cost with respect to the
-// parameters (a.k.a. the jacobian) using an autodifferentiation framework. The
-// first template argument is the functor object, described in the header
-// comment. The second argument is the dimension of the residual, and subsequent
+// A cost function which computes the derivative of the cost with respect to
+// the parameters (a.k.a. the jacobian) using an autodifferentiation framework.
+// The first template argument is the functor object, described in the header
+// comment. The second argument is the dimension of the residual (or
+// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
 // arguments describe the size of the Nth parameter, one per parameter.
 //
-// The constructor, which takes a cost functor, takes ownership of the functor.
+// The constructors take ownership of the cost functor.
+//
+// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the
+// two-argument constructor must be used. The second constructor takes a number
+// of residuals (in addition to the templated number of residuals). This allows
+// for varying the number of residuals for a single autodiff cost function at
+// runtime.
 template <typename CostFunctor,
-          int M,        // Number of residuals.
+          int M,        // 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.
@@ -136,8 +158,25 @@ template <typename CostFunctor,
 class AutoDiffCostFunction :
   public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
  public:
-  // Takes ownership of functor.
-  explicit AutoDiffCostFunction(CostFunctor* functor) : functor_(functor) {}
+  // Takes ownership of functor. Uses the template-provided value for the
+  // number of residuals ("M").
+  explicit AutoDiffCostFunction(CostFunctor* functor)
+      : functor_(functor) {
+    CHECK_NE(M, 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
+  // residuals ("M") 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 "
+                          << "number of residuals is not ceres::DYNAMIC.";
+    SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::set_num_residuals(num_residuals);
+  }
 
   virtual ~AutoDiffCostFunction() {}
 
@@ -155,10 +194,12 @@ class AutoDiffCostFunction :
           ::Call(*functor_, parameters, residuals);
     }
     return internal::AutoDiff<CostFunctor, double,
-           M, N0, N1, N2, N3, N4, N5>::Differentiate(*functor_,
-                                                     parameters,
-                                                     residuals,
-                                                     jacobians);
+           N0, N1, N2, N3, N4, N5>::Differentiate(
+               *functor_,
+               parameters,
+               SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::num_residuals(),
+               residuals,
+               jacobians);
   }
 
  private:

include/ceres/internal/autodiff.h

@@ -144,6 +144,7 @@
 
 #include <glog/logging.h>
 #include "ceres/jet.h"
+#include "ceres/internal/eigen.h"
 #include "ceres/internal/fixed_array.h"
 
 namespace ceres {
@@ -185,18 +186,12 @@ inline void Take0thOrderPart(int M, const JetT *src, T dst) {
 
 // Takes N 1st order parts, starting at index N0, and puts them in the M x N
 // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
-template <typename JetT, typename T, int M, int N0, int N>
-inline void Take1stOrderPart(const JetT *src, T *dst) {
+template <typename JetT, typename T, int N0, int N>
+inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
   DCHECK(src);
   DCHECK(dst);
-  // TODO(keir): Change Jet to use a single array, where v[0] is the
-  // non-infinitesimal part rather than "a". That way it's possible to use a
-  // single memcpy or eigen operation, rather than the explicit loop. The loop
-  // doesn't exploit any SSE or other intrinsics.
   for (int i = 0; i < M; ++i) {
-    for (int j = 0; j < N; ++j) {
-      dst[N * i + j] = src[i].v[N0 + j];
-    }
+    Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = src[i].v.template segment<N>(N0);
   }
 }
 
@@ -279,11 +274,12 @@ struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0> {
 // This is in a struct because default template parameters on a function are not
 // supported in C++03 (though it is available in C++0x). N0 through N5 are the
 // dimension of the input arguments to the user supplied functor.
-template <typename Functor, typename T, int kNumOutputs,
+template <typename Functor, typename T,
           int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5=0>
 struct AutoDiff {
   static bool Differentiate(const Functor& functor,
                             T const *const *parameters,
+                            int num_outputs,
                             T *function_value,
                             T **jacobians) {
     typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5> JetT;
@@ -300,10 +296,10 @@ struct AutoDiff {
         << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
         << N3 << ", " << N4 << ", " << N5;
 
-    DCHECK_GT(kNumOutputs, 0);
+    DCHECK_GT(num_outputs, 0);
 
     FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
-        N0 + N1 + N2 + N3 + N4 + N5 + kNumOutputs);
+        N0 + N1 + N2 + N3 + N4 + N5 + num_outputs);
 
     // It's ugly, but it works.
     const int jet0 = 0;
@@ -345,16 +341,16 @@ struct AutoDiff {
       return false;
     }
 
-    internal::Take0thOrderPart(kNumOutputs, output, function_value);
+    internal::Take0thOrderPart(num_outputs, output, function_value);
 
 #define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
     if (N ## i) { \
       if (jacobians[i]) { \
         internal::Take1stOrderPart<JetT, T, \
-                                   kNumOutputs, \
                                    jet ## i, \
-                                   N ## i>(output, \
-                                          jacobians[i]); \
+                                   N ## i>(num_outputs, \
+                                           output, \
+                                           jacobians[i]); \
       } \
     }
     CERES_TAKE_1ST_ORDER_PERTURBATION(0);

include/ceres/sized_cost_function.h

@@ -30,11 +30,16 @@
 //
 // A convenience class for cost functions which are statically sized.
 // Compared to the dynamically-sized base class, this reduces boilerplate.
+//
+// The kNumResiduals template parameter can be a constant such as 2 or 5, or it
+// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses
+// are responsible for calling set_num_residuals() at runtime.
 
 #ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_
 #define CERES_PUBLIC_SIZED_COST_FUNCTION_H_
 
 #include <glog/logging.h>
+#include "ceres/types.h"
 #include "ceres/cost_function.h"
 
 namespace ceres {
@@ -45,11 +50,12 @@ class SizedCostFunction : public CostFunction {
  public:
   SizedCostFunction() {
     // Sanity checking.
-    DCHECK_GT(kNumResiduals, 0) << "Cost functions must have at least "
-                                << "one residual block.";
-    DCHECK_GT(N0, 0)
+    CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC)
+        << "Cost functions must have at least one residual block.";
+
+    CHECK_GT(N0, 0)
         << "Cost functions must have at least one parameter block.";
-    DCHECK((!N1 && !N2 && !N3 && !N4 && !N5) ||
+    CHECK((!N1 && !N2 && !N3 && !N4 && !N5) ||
            ((N1 > 0) && !N2 && !N3 && !N4 && !N5) ||
            ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) ||
            ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) ||

include/ceres/types.h

@@ -237,6 +237,13 @@ enum DumpFormatType {
   TEXTFILE
 };
 
+// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be specified for
+// the number of residuals. If specified, then the number of residuas for that
+// cost function can vary at runtime.
+enum DimensionType {
+  DYNAMIC = -1
+};
+
 const char* LinearSolverTypeToString(LinearSolverType type);
 const char* PreconditionerTypeToString(PreconditionerType type);
 const char* LinearSolverTerminationTypeToString(

internal/ceres/autodiff_test.cc

@@ -37,7 +37,7 @@ namespace ceres {
 namespace internal {
 
 template <typename T> inline
-T &RowMajor(T *base, int rows, int cols, int i, int j) {
+T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
   return base[cols * i + j];
 }
 
@@ -86,7 +86,7 @@ bool SymmetricDiff(const B& b,
     // Symmetric differencing:
     //   f'(a) = (f(a + h) - f(a - h)) / (2 h)
     for (int i = 0; i < M; ++i) {
-      RowMajor(jac, M, N, i, j) =
+      RowMajorAccess(jac, M, N, i, j) =
           (fwd_fun[i] - bwd_fun[i]) / (T(2) * del);
     }
 
@@ -116,7 +116,7 @@ void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
   A cc = c*c;
   A cd = c*d;
   A dd = d*d;
-#define R(i, j) RowMajor(R, 3, 3, (i), (j))
+#define R(i, j) RowMajorAccess(R, 3, 3, (i), (j))
   R(0, 0) =  aa+bb-cc-dd; R(0, 1) = A(2)*(bc-ad); R(0, 2) = A(2)*(ac+bd);  // NOLINT
   R(1, 0) = A(2)*(ad+bc); R(1, 1) =  aa-bb+cc-dd; R(1, 2) = A(2)*(cd-ab);  // NOLINT
   R(2, 0) = A(2)*(bd-ac); R(2, 1) = A(2)*(ab+cd); R(2, 2) =  aa-bb-cc+dd;  // NOLINT
@@ -132,10 +132,10 @@ struct Projective {
   bool operator()(A const P[12], A const X[4], A x[2]) const {
     A PX[3];
     for (int i = 0; i < 3; ++i) {
-      PX[i] = RowMajor(P, 3, 4, i, 0) * X[0] +
-              RowMajor(P, 3, 4, i, 1) * X[1] +
-              RowMajor(P, 3, 4, i, 2) * X[2] +
-              RowMajor(P, 3, 4, i, 3) * X[3];
+      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
+              RowMajorAccess(P, 3, 4, i, 1) * X[1] +
+              RowMajorAccess(P, 3, 4, i, 2) * X[2] +
+              RowMajorAccess(P, 3, 4, i, 3) * X[3];
     }
     if (PX[2] != 0.0) {
       x[0] = PX[0] / PX[2];
@@ -194,8 +194,8 @@ TEST(AutoDiff, ProjectiveCameraModel) {
   {
     double *parameters[] = { PX };
     double *jacobians[] = { J_PX };
-    ASSERT_TRUE((AutoDiff<Projective, double, 2, 12 + 4>::Differentiate(
-        b, parameters, ad_x1, jacobians)));
+    ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
+        b, parameters, 2, ad_x1, jacobians)));
 
     for (int i = 0; i < 2; ++i) {
       ASSERT_NEAR(ad_x1[i], b_x[i], tol);
@@ -209,8 +209,8 @@ TEST(AutoDiff, ProjectiveCameraModel) {
     double J_X[2 * 4];
     double *parameters[] = { P, X };
     double *jacobians[] = { J_P, J_X };
-    ASSERT_TRUE((AutoDiff<Projective, double, 2, 12, 4>::Differentiate(
-        b, parameters, ad_x2, jacobians)));
+    ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
+        b, parameters, 2, ad_x2, jacobians)));
 
     for (int i = 0; i < 2; ++i) {
       ASSERT_NEAR(ad_x2[i], b_x[i], tol);
@@ -252,16 +252,16 @@ struct Metric {
     // Set P(:, 1:3) = R
     for (int i = 0; i < 3; ++i) {
       for (int j = 0; j < 3; ++j) {
-        RowMajor(P, 3, 4, i, j) = RowMajor(R, 3, 3, i, j);
+        RowMajorAccess(P, 3, 4, i, j) = RowMajorAccess(R, 3, 3, i, j);
       }
     }
 
     // Set P(:, 4) = - R c
     for (int i = 0; i < 3; ++i) {
-      RowMajor(P, 3, 4, i, 3) =
-        - (RowMajor(R, 3, 3, i, 0) * c[0] +
-           RowMajor(R, 3, 3, i, 1) * c[1] +
-           RowMajor(R, 3, 3, i, 2) * c[2]);
+      RowMajorAccess(P, 3, 4, i, 3) =
+        - (RowMajorAccess(R, 3, 3, i, 0) * c[0] +
+           RowMajorAccess(R, 3, 3, i, 1) * c[1] +
+           RowMajorAccess(R, 3, 3, i, 2) * c[2]);
     }
 
     A X1[4] = { X[0], X[1], X[2], A(1) };
@@ -316,8 +316,8 @@ TEST(AutoDiff, Metric) {
   double J_X[2 * 3];
   double *parameters[] = { q, c, X };
   double *jacobians[] = { J_q, J_c, J_X };
-  ASSERT_TRUE((AutoDiff<Metric, double, 2, 4, 3, 3>::Differentiate(
-      b, parameters, ad_x, jacobians)));
+  ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
+      b, parameters, 2, ad_x, jacobians)));
 
   for (int i = 0; i < 2; ++i) {
     ASSERT_NEAR(ad_x[i], b_x[i], tol);
@@ -337,5 +337,45 @@ TEST(AutoDiff, Metric) {
   }
 }
 
+struct VaryingResidualFunctor {
+  template <typename T>
+  bool operator()(const T x[2], T* y) const {
+    for (int i = 0; i < num_residuals; ++i) {
+      y[i] = T(i) * x[0] * x[1] * x[1];
+    }
+    return true;
+  }
+
+  int num_residuals;
+};
+
+TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
+  double x[2] = { 1.0, 5.5 };
+  double *parameters[] = { x };
+  const int kMaxResiduals = 10;
+  double J_x[2 * kMaxResiduals];
+  double residuals[kMaxResiduals];
+  double *jacobians[] = { J_x };
+
+  // Use a single functor, but tweak it to produce different numbers of
+  // residuals.
+  VaryingResidualFunctor functor;
+
+  for (int num_residuals = 0; num_residuals < kMaxResiduals; ++num_residuals) {
+    // Tweak the number of residuals to produce.
+    functor.num_residuals = num_residuals;
+
+    // Run autodiff with the new number of residuals.
+    ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
+        functor, parameters, num_residuals, residuals, jacobians)));
+
+    const double kTolerance = 1e-14;
+    for (int i = 0; i < num_residuals; ++i) {
+      EXPECT_NEAR(J_x[2 * i + 0], i * x[1] * x[1], kTolerance) << "i: " << i;
+      EXPECT_NEAR(J_x[2 * i + 1], 2 * i * x[0] * x[1], kTolerance) << "i: " << i;
+    }
+  }
+}
+
 }  // namespace internal
 }  // namespace ceres

internal/ceres/local_parameterization_test.cc

@@ -176,8 +176,8 @@ void QuaternionParameterizationTestHelper(const double* x,
   double* jacobian_array[2] = { NULL, jacobian_ref };
 
   // Autodiff jacobian at delta_x = 0.
-  internal::AutoDiff<QuaternionPlus, double, 4, 4, 3>::Differentiate(
-      QuaternionPlus(), parameters, x_plus_delta, jacobian_array);
+  internal::AutoDiff<QuaternionPlus, double, 4, 3>::Differentiate(
+      QuaternionPlus(), parameters, 4, x_plus_delta, jacobian_array);
 
   double jacobian[12];
   param.ComputeJacobian(x, jacobian);