Add support for up to 10 parameters in autodiff.

Supporting only 6 parameters in autodiff was enough for most
cases, but 6 was not always sufficient. This extends the
current implementation to work with up to 10 parameters.
This also increases the number of parameters supported in
SizedCostFunction to 10.

Change-Id: Ic783602f93e6ddf4af24fa34eff37c0a4b775dc1

include/ceres/autodiff_cost_function.h

@@ -154,16 +154,20 @@ template <typename CostFunctor,
           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 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 AutoDiffCostFunction :
-  public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> {
+  public SizedCostFunction<M, 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").
   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.";
+                         << "number of residuals is set to ceres::DYNAMIC.";
   }
 
   // Takes ownership of functor. Ignores the template-provided number of
@@ -174,8 +178,9 @@ class AutoDiffCostFunction :
   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);
+                         << "number of residuals is not ceres::DYNAMIC.";
+    SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
+        ::set_num_residuals(num_residuals);
   }
 
   virtual ~AutoDiffCostFunction() {}
@@ -190,14 +195,15 @@ class AutoDiffCostFunction :
                         double** jacobians) const {
     if (!jacobians) {
       return internal::VariadicEvaluate<
-          CostFunctor, double, N0, N1, N2, N3, N4, N5>
+          CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
           ::Call(*functor_, parameters, residuals);
     }
     return internal::AutoDiff<CostFunctor, double,
-           N0, N1, N2, N3, N4, N5>::Differentiate(
+           N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
                *functor_,
                parameters,
-               SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::num_residuals(),
+               SizedCostFunction<M, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
+                   ::num_residuals(),
                residuals,
                jacobians);
   }

include/ceres/internal/autodiff.h

@@ -203,8 +203,8 @@ inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
 // Supporting variadic functions is the primary source of complexity in the
 // autodiff implementation.
 
-template<typename Functor, typename T,
-         int N0, int N1, int N2, int N3, int N4, int N5>
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8, int N9>
 struct VariadicEvaluate {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
@@ -213,13 +213,78 @@ struct VariadicEvaluate {
                    input[3],
                    input[4],
                    input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   input[9],
                    output);
   }
 };
 
-template<typename Functor, typename T,
-         int N0, int N1, int N2, int N3, int N4>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0> {
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7, int N8>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   input[8],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6, int N7>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   input[7],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5, int N6>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   input[6],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
+         int N5>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
+  static bool Call(const Functor& functor, T const *const *input, T* output) {
+    return functor(input[0],
+                   input[1],
+                   input[2],
+                   input[3],
+                   input[4],
+                   input[5],
+                   output);
+  }
+};
+
+template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
                    input[1],
@@ -230,9 +295,8 @@ struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0> {
   }
 };
 
-template<typename Functor, typename T,
-         int N0, int N1, int N2, int N3>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0> {
+template<typename Functor, typename T, int N0, int N1, int N2, int N3>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
                    input[1],
@@ -242,9 +306,8 @@ struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0> {
   }
 };
 
-template<typename Functor, typename T,
-         int N0, int N1, int N2>
-struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0> {
+template<typename Functor, typename T, int N0, int N1, int N2>
+struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
                    input[1],
@@ -253,9 +316,8 @@ struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0> {
   }
 };
 
-template<typename Functor, typename T,
-         int N0, int N1>
-struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0> {
+template<typename Functor, typename T, int N0, int N1>
+struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
                    input[1],
@@ -264,7 +326,7 @@ struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0> {
 };
 
 template<typename Functor, typename T, int N0>
-struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0> {
+struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
   static bool Call(const Functor& functor, T const *const *input, T* output) {
     return functor(input[0],
                    output);
@@ -275,48 +337,58 @@ struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0> {
 // 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 N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5=0>
+          int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
+          int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 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;
-
-    DCHECK_GT(N0, 0)
-        << "Cost functions must have at least one parameter block.";
-    DCHECK((!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) ||
-           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) ||
-           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0)))
+    // This block breaks the 80 column rule to keep it somewhat readable.
+    DCHECK_GT(num_outputs, 0);
+    CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
+          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)))
         << "Zero block cannot precede a non-zero block. Block sizes are "
         << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
-        << N3 << ", " << N4 << ", " << N5;
-
-    DCHECK_GT(num_outputs, 0);
+        << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
+        << N8 << ", " << N9;
 
+    typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
     FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
-        N0 + N1 + N2 + N3 + N4 + N5 + num_outputs);
+        N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
 
-    // It's ugly, but it works.
-    const int jet0 = 0;
-    const int jet1 = N0;
-    const int jet2 = N0 + N1;
-    const int jet3 = N0 + N1 + N2;
-    const int jet4 = N0 + N1 + N2 + N3;
-    const int jet5 = N0 + N1 + N2 + N3 + N4;
-    const int jet6 = N0 + N1 + N2 + N3 + N4 + N5;
+    // These are the positions of the respective jets in the fixed array x.
+    const int jet0  = 0;
+    const int jet1  = N0;
+    const int jet2  = N0 + N1;
+    const int jet3  = N0 + N1 + N2;
+    const int jet4  = N0 + N1 + N2 + N3;
+    const int jet5  = N0 + N1 + N2 + N3 + N4;
+    const int jet6  = N0 + N1 + N2 + N3 + N4 + N5;
+    const int jet7  = N0 + N1 + N2 + N3 + N4 + N5 + N6;
+    const int jet8  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
+    const int jet9  = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
 
-    const JetT *unpacked_parameters[6] = {
+    const JetT *unpacked_parameters[10] = {
         x.get() + jet0,
         x.get() + jet1,
         x.get() + jet2,
         x.get() + jet3,
         x.get() + jet4,
         x.get() + jet5,
+        x.get() + jet6,
+        x.get() + jet7,
+        x.get() + jet8,
+        x.get() + jet9,
     };
     JetT *output = x.get() + jet6;
 
@@ -333,10 +405,14 @@ struct AutoDiff {
     CERES_MAKE_1ST_ORDER_PERTURBATION(3);
     CERES_MAKE_1ST_ORDER_PERTURBATION(4);
     CERES_MAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_MAKE_1ST_ORDER_PERTURBATION(9);
 #undef CERES_MAKE_1ST_ORDER_PERTURBATION
 
     if (!VariadicEvaluate<Functor, JetT,
-                          N0, N1, N2, N3, N4, N5>::Call(
+                          N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
         functor, unpacked_parameters, output)) {
       return false;
     }
@@ -359,6 +435,10 @@ struct AutoDiff {
     CERES_TAKE_1ST_ORDER_PERTURBATION(3);
     CERES_TAKE_1ST_ORDER_PERTURBATION(4);
     CERES_TAKE_1ST_ORDER_PERTURBATION(5);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(6);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(7);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(8);
+    CERES_TAKE_1ST_ORDER_PERTURBATION(9);
 #undef CERES_TAKE_1ST_ORDER_PERTURBATION
     return true;
   }

include/ceres/sized_cost_function.h

@@ -45,25 +45,29 @@
 namespace ceres {
 
 template<int kNumResiduals,
-         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0>
+         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
+         int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
 class SizedCostFunction : public CostFunction {
  public:
   SizedCostFunction() {
-    // Sanity checking.
     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.";
-    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) ||
-           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) ||
-           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0)))
+    // This block breaks the 80 column rule to keep it somewhat readable.
+    CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
+           ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)))
         << "Zero block cannot precede a non-zero block. Block sizes are "
         << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
-        << N3 << ", " << N4 << ", " << N5;
+        << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
+        << N8 << ", " << N9;
 
     set_num_residuals(kNumResiduals);
 
@@ -75,6 +79,10 @@ class SizedCostFunction : public CostFunction {
     ADD_PARAMETER_BLOCK(N3);
     ADD_PARAMETER_BLOCK(N4);
     ADD_PARAMETER_BLOCK(N5);
+    ADD_PARAMETER_BLOCK(N6);
+    ADD_PARAMETER_BLOCK(N7);
+    ADD_PARAMETER_BLOCK(N8);
+    ADD_PARAMETER_BLOCK(N9);
 #undef ADD_PARAMETER_BLOCK
   }
 

internal/ceres/autodiff_cost_function_test.cc

@@ -51,7 +51,7 @@ class BinaryScalarCost {
   double a_;
 };
 
-TEST(AutoDiffResidualAndJacobian, BilinearDifferentiationTest) {
+TEST(AutodiffCostFunction, BilinearDifferentiationTest) {
   CostFunction* cost_function  =
     new AutoDiffCostFunction<BinaryScalarCost, 1, 2, 2>(
         new BinaryScalarCost(1.0));
@@ -73,20 +73,72 @@ TEST(AutoDiffResidualAndJacobian, BilinearDifferentiationTest) {
   double residuals = 0.0;
 
   cost_function->Evaluate(parameters, &residuals, NULL);
-  EXPECT_EQ(residuals, 10);
+  EXPECT_EQ(10.0, residuals);
   cost_function->Evaluate(parameters, &residuals, jacobians);
 
-  EXPECT_EQ(jacobians[0][0], 3);
-  EXPECT_EQ(jacobians[0][1], 4);
-  EXPECT_EQ(jacobians[1][0], 1);
-  EXPECT_EQ(jacobians[1][1], 2);
-
-  delete []jacobians[0];
-  delete []jacobians[1];
-  delete []parameters[0];
-  delete []parameters[1];
-  delete []jacobians;
-  delete []parameters;
+  EXPECT_EQ(3, jacobians[0][0]);
+  EXPECT_EQ(4, jacobians[0][1]);
+  EXPECT_EQ(1, jacobians[1][0]);
+  EXPECT_EQ(2, jacobians[1][1]);
+
+  delete[] jacobians[0];
+  delete[] jacobians[1];
+  delete[] parameters[0];
+  delete[] parameters[1];
+  delete[] jacobians;
+  delete[] parameters;
+  delete cost_function;
+}
+
+struct TenParameterCost {
+  template <typename T>
+  bool operator()(const T* const x0,
+                  const T* const x1,
+                  const T* const x2,
+                  const T* const x3,
+                  const T* const x4,
+                  const T* const x5,
+                  const T* const x6,
+                  const T* const x7,
+                  const T* const x8,
+                  const T* const x9,
+                  T* cost) const {
+    cost[0] = *x0 + *x1 + *x2 + *x3 + *x4 + *x5 + *x6 + *x7 + *x8 + *x9;
+    return true;
+  }
+};
+
+TEST(AutodiffCostFunction, ManyParameterAutodiffInstantiates) {
+  CostFunction* cost_function  =
+      new AutoDiffCostFunction<
+          TenParameterCost, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1>(
+              new TenParameterCost);
+
+  double** parameters = new double*[10];
+  double** jacobians = new double*[10];
+  for (int i = 0; i < 10; ++i) {
+    parameters[i] = new double[1];
+    parameters[i][0] = i;
+    jacobians[i] = new double[1];
+  }
+
+  double residuals = 0.0;
+
+  cost_function->Evaluate(parameters, &residuals, NULL);
+  EXPECT_EQ(45.0, residuals);
+
+  cost_function->Evaluate(parameters, &residuals, jacobians);
+  EXPECT_EQ(residuals, 45.0);
+  for (int i = 0; i < 10; ++i) {
+    EXPECT_EQ(1.0, jacobians[i][0]);
+  }
+
+  for (int i = 0; i < 10; ++i) {
+    delete[] jacobians[i];
+    delete[] parameters[i];
+  }
+  delete[] jacobians;
+  delete[] parameters;
   delete cost_function;
 }