Pass kNumResiduals to Autodiff

The compile-time constant kNumResiduals is now passed to the
autodiff functions as a template parameter. This will be used
by future patches to optimize autodiff performance.

Change-Id: Ia2b2cc99b88752e8f12f4ce2542b1963bda552f5

include/ceres/autodiff_cost_function.h

@@ -191,7 +191,7 @@ class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, Ns...> {
       return internal::VariadicEvaluate<ParameterDims>(
           *functor_, parameters, residuals);
     }
-    return internal::AutoDifferentiate<ParameterDims>(
+    return internal::AutoDifferentiate<kNumResiduals, ParameterDims>(
         *functor_,
         parameters,
         SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),

include/ceres/autodiff_local_parameterization.h

@@ -135,6 +135,7 @@ class AutoDiffLocalParameterization : public LocalParameterization {
     const double* parameter_ptrs[2] = {x, zero_delta};
     double* jacobian_ptrs[2] = {NULL, jacobian};
     return internal::AutoDifferentiate<
+        kGlobalSize,
         internal::StaticParameterDims<kGlobalSize, kLocalSize>>(
         *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs);
   }

include/ceres/internal/autodiff.h

@@ -193,12 +193,14 @@ template <typename Seq, int ParameterIdx = 0, int Offset = 0>
 struct Make1stOrderPerturbations;
 
 template <int N, int... Ns, int ParameterIdx, int Offset>
-struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>, ParameterIdx,
+struct Make1stOrderPerturbations<integer_sequence<int, N, Ns...>,
+                                 ParameterIdx,
                                  Offset> {
   template <typename T, typename JetT>
   static void Apply(T const* const* parameters, JetT* x) {
     Make1stOrderPerturbation<Offset, N>(parameters[ParameterIdx], x + Offset);
-    Make1stOrderPerturbations<integer_sequence<int, Ns...>, ParameterIdx + 1,
+    Make1stOrderPerturbations<integer_sequence<int, Ns...>,
+                              ParameterIdx + 1,
                               Offset + N>::Apply(parameters, x);
   }
 };
@@ -253,14 +255,16 @@ template <typename Seq, int ParameterIdx = 0, int Offset = 0>
 struct Take1stOrderParts;
 
 template <int N, int... Ns, int ParameterIdx, int Offset>
-struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx,
+struct Take1stOrderParts<integer_sequence<int, N, Ns...>,
+                         ParameterIdx,
                          Offset> {
   template <typename JetT, typename T>
   static void Apply(int num_outputs, JetT* output, T** jacobians) {
     if (jacobians[ParameterIdx]) {
       Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
     }
-    Take1stOrderParts<integer_sequence<int, Ns...>, ParameterIdx + 1,
+    Take1stOrderParts<integer_sequence<int, Ns...>,
+                      ParameterIdx + 1,
                       Offset + N>::Apply(num_outputs, output, jacobians);
   }
 };
@@ -269,13 +273,17 @@ struct Take1stOrderParts<integer_sequence<int, N, Ns...>, ParameterIdx,
 template <int ParameterIdx, int Offset>
 struct Take1stOrderParts<integer_sequence<int>, ParameterIdx, Offset> {
   template <typename T, typename JetT>
-  static void Apply(int /* NOT USED*/, JetT* /* NOT USED*/,
+  static void Apply(int /* NOT USED*/,
+                    JetT* /* NOT USED*/,
                     T** /* NOT USED */) {}
 };
 
-template <typename ParameterDims, typename Functor, typename T>
+template <int kNumResiduals,
+          typename ParameterDims,
+          typename Functor,
+          typename T>
 inline bool AutoDifferentiate(const Functor& functor,
-                              T const *const *parameters,
+                              T const* const* parameters,
                               int num_outputs,
                               T* function_value,
                               T** jacobians) {
@@ -301,8 +309,8 @@ inline bool AutoDifferentiate(const Functor& functor,
 
   Make1stOrderPerturbations<Parameters>::Apply(parameters, x.data());
 
-  if (!VariadicEvaluate<ParameterDims>(functor, unpacked_parameters.data(),
-                                       output)) {
+  if (!VariadicEvaluate<ParameterDims>(
+          functor, unpacked_parameters.data(), output)) {
     return false;
   }
 

internal/ceres/autodiff_test.cc

@@ -30,14 +30,14 @@
 
 #include "ceres/internal/autodiff.h"
 
-#include "gtest/gtest.h"
 #include "ceres/random.h"
+#include "gtest/gtest.h"
 
 namespace ceres {
 namespace internal {
 
-template <typename T> inline
-T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
+template <typename T>
+inline T& RowMajorAccess(T* base, int rows, int cols, int i, int j) {
   return base[cols * i + j];
 }
 
@@ -49,12 +49,12 @@ T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
 //   bool operator()(T const *, T *) const;
 //
 // which maps a vector of parameters to a vector of outputs.
-template <typename B, typename T, int M, int N> inline
-bool SymmetricDiff(const B& b,
-                   const T par[N],
-                   T del,           // step size.
-                   T fun[M],
-                   T jac[M * N]) {  // row-major.
+template <typename B, typename T, int M, int N>
+inline bool SymmetricDiff(const B& b,
+                          const T par[N],
+                          T del,  // step size.
+                          T fun[M],
+                          T jac[M * N]) {  // row-major.
   if (!b(par, fun)) {
     return false;
   }
@@ -97,8 +97,8 @@ bool SymmetricDiff(const B& b,
   return true;
 }
 
-template <typename A> inline
-void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
+template <typename A>
+inline void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
   // Make convenient names for elements of q.
   A a = q[0];
   A b = q[1];
@@ -106,20 +106,26 @@ void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
   A d = q[3];
   // This is not to eliminate common sub-expression, but to
   // make the lines shorter so that they fit in 80 columns!
-  A aa = a*a;
-  A ab = a*b;
-  A ac = a*c;
-  A ad = a*d;
-  A bb = b*b;
-  A bc = b*c;
-  A bd = b*d;
-  A cc = c*c;
-  A cd = c*d;
-  A dd = d*d;
+  A aa = a * a;
+  A ab = a * b;
+  A ac = a * c;
+  A ad = a * d;
+  A bb = b * b;
+  A bc = b * c;
+  A bd = b * d;
+  A cc = c * c;
+  A cd = c * d;
+  A dd = d * d;
 #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
+  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
 #undef R
 }
 
@@ -171,8 +177,8 @@ TEST(AutoDiff, ProjectiveCameraModel) {
   }
 
   // Handy names for the P and X parts.
-  double *P = PX + 0;
-  double *X = PX + 12;
+  double* P = PX + 0;
+  double* X = PX + 12;
 
   // Apply the mapping, to get image point b_x.
   double b_x[2];
@@ -181,8 +187,8 @@ TEST(AutoDiff, ProjectiveCameraModel) {
   // Use finite differencing to estimate the Jacobian.
   double fd_x[2];
   double fd_J[2 * (12 + 4)];
-  ASSERT_TRUE((SymmetricDiff<Projective, double, 2, 12 + 4>(b, PX, del,
-                                                            fd_x, fd_J)));
+  ASSERT_TRUE(
+      (SymmetricDiff<Projective, double, 2, 12 + 4>(b, PX, del, fd_x, fd_J)));
 
   for (int i = 0; i < 2; ++i) {
     ASSERT_NEAR(fd_x[i], b_x[i], tol);
@@ -192,9 +198,9 @@ TEST(AutoDiff, ProjectiveCameraModel) {
   double ad_x1[2];
   double J_PX[2 * (12 + 4)];
   {
-    double *parameters[] = { PX };
-    double *jacobians[] = { J_PX };
-    ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12 + 4>>(
+    double* parameters[] = {PX};
+    double* jacobians[] = {J_PX};
+    ASSERT_TRUE((AutoDifferentiate<2, StaticParameterDims<12 + 4>>(
         b, parameters, 2, ad_x1, jacobians)));
 
     for (int i = 0; i < 2; ++i) {
@@ -207,9 +213,9 @@ TEST(AutoDiff, ProjectiveCameraModel) {
     double ad_x2[2];
     double J_P[2 * 12];
     double J_X[2 * 4];
-    double *parameters[] = { P, X };
-    double *jacobians[] = { J_P, J_X };
-    ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12, 4>>(
+    double* parameters[] = {P, X};
+    double* jacobians[] = {J_P, J_X};
+    ASSERT_TRUE((AutoDifferentiate<2, StaticParameterDims<12, 4>>(
         b, parameters, 2, ad_x2, jacobians)));
 
     for (int i = 0; i < 2; ++i) {
@@ -258,13 +264,12 @@ struct Metric {
 
     // Set P(:, 4) = - R c
     for (int i = 0; i < 3; ++i) {
-      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]);
+      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) };
+    A X1[4] = {X[0], X[1], X[2], A(1)};
     Projective p;
     return p(P, X1, x);
   }
@@ -287,13 +292,12 @@ TEST(AutoDiff, Metric) {
 
   // Make random parameter vector.
   double qcX[4 + 3 + 3];
-  for (int i = 0; i < 4 + 3 + 3; ++i)
-    qcX[i] = RandDouble();
+  for (int i = 0; i < 4 + 3 + 3; ++i) qcX[i] = RandDouble();
 
   // Handy names.
-  double *q = qcX;
-  double *c = qcX + 4;
-  double *X = qcX + 4 + 3;
+  double* q = qcX;
+  double* c = qcX + 4;
+  double* X = qcX + 4 + 3;
 
   // Compute projection, b_x.
   double b_x[2];
@@ -302,8 +306,8 @@ TEST(AutoDiff, Metric) {
   // Finite differencing estimate of Jacobian.
   double fd_x[2];
   double fd_J[2 * (4 + 3 + 3)];
-  ASSERT_TRUE((SymmetricDiff<Metric, double, 2, 4 + 3 + 3>(b, qcX, del,
-                                                           fd_x, fd_J)));
+  ASSERT_TRUE(
+      (SymmetricDiff<Metric, double, 2, 4 + 3 + 3>(b, qcX, del, fd_x, fd_J)));
 
   for (int i = 0; i < 2; ++i) {
     ASSERT_NEAR(fd_x[i], b_x[i], tol);
@@ -314,9 +318,9 @@ TEST(AutoDiff, Metric) {
   double J_q[2 * 4];
   double J_c[2 * 3];
   double J_X[2 * 3];
-  double *parameters[] = { q, c, X };
-  double *jacobians[] = { J_q, J_c, J_X };
-  ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<4, 3, 3>>(
+  double* parameters[] = {q, c, X};
+  double* jacobians[] = {J_q, J_c, J_X};
+  ASSERT_TRUE((AutoDifferentiate<2, StaticParameterDims<4, 3, 3>>(
       b, parameters, 2, ad_x, jacobians)));
 
   for (int i = 0; i < 2; ++i) {
@@ -350,12 +354,12 @@ struct VaryingResidualFunctor {
 };
 
 TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
-  double x[2] = { 1.0, 5.5 };
-  double *parameters[] = { x };
+  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 };
+  double* jacobians[] = {J_x};
 
   // Use a single functor, but tweak it to produce different numbers of
   // residuals.
@@ -366,7 +370,7 @@ TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
     functor.num_residuals = num_residuals;
 
     // Run autodiff with the new number of residuals.
-    ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<2>>(
+    ASSERT_TRUE((AutoDifferentiate<DYNAMIC, StaticParameterDims<2>>(
         functor, parameters, num_residuals, residuals, jacobians)));
 
     const double kTolerance = 1e-14;
@@ -404,11 +408,8 @@ struct Residual3Param {
 
 struct Residual4Param {
   template <typename T>
-  bool operator()(const T* x0,
-                  const T* x1,
-                  const T* x2,
-                  const T* x3,
-                  T* y) const {
+  bool operator()(
+      const T* x0, const T* x1, const T* x2, const T* x3, T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4);
     return true;
   }
@@ -437,7 +438,7 @@ struct Residual6Param {
                   const T* x5,
                   T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
-        pow(*x5, 6);
+           pow(*x5, 6);
     return true;
   }
 };
@@ -453,7 +454,7 @@ struct Residual7Param {
                   const T* x6,
                   T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
-        pow(*x5, 6) + pow(*x6, 7);
+           pow(*x5, 6) + pow(*x6, 7);
     return true;
   }
 };
@@ -470,7 +471,7 @@ struct Residual8Param {
                   const T* x7,
                   T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
-        pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8);
+           pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8);
     return true;
   }
 };
@@ -488,7 +489,7 @@ struct Residual9Param {
                   const T* x8,
                   T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
-        pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8) + pow(*x8, 9);
+           pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8) + pow(*x8, 9);
     return true;
   }
 };
@@ -507,7 +508,7 @@ struct Residual10Param {
                   const T* x9,
                   T* y) const {
     y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
-        pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8) + pow(*x8, 9) + pow(*x9, 10);
+           pow(*x5, 6) + pow(*x6, 7) + pow(*x7, 8) + pow(*x8, 9) + pow(*x9, 10);
     return true;
   }
 };
@@ -528,7 +529,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual1Param functor;
     int num_variables = 1;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -539,7 +540,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual2Param functor;
     int num_variables = 2;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -550,7 +551,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual3Param functor;
     int num_variables = 3;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -561,7 +562,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual4Param functor;
     int num_variables = 4;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -572,7 +573,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual5Param functor;
     int num_variables = 5;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -583,7 +584,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual6Param functor;
     int num_variables = 6;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -594,7 +595,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual7Param functor;
     int num_variables = 7;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>(
+    EXPECT_TRUE((AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>(
         functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -605,8 +606,9 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual8Param functor;
     int num_variables = 8;
-    EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>(
-        functor, parameters, 1, &residual, jacobians)));
+    EXPECT_TRUE(
+        (AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>(
+            functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
       EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -617,7 +619,7 @@ TEST(AutoDiff, VariadicAutoDiff) {
     Residual9Param functor;
     int num_variables = 9;
     EXPECT_TRUE(
-        (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>(
+        (AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>(
             functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {
@@ -628,8 +630,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
   {
     Residual10Param functor;
     int num_variables = 10;
-    EXPECT_TRUE(
-        (AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>(
+    EXPECT_TRUE((
+        AutoDifferentiate<1, StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>(
             functor, parameters, 1, &residual, jacobians)));
     EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
     for (int i = 0; i < num_variables; ++i) {

internal/ceres/local_parameterization_test.cc

@@ -28,6 +28,8 @@
 //
 // Author: sameeragarwal@google.com (Sameer Agarwal)
 
+#include "ceres/local_parameterization.h"
+
 #include <cmath>
 #include <limits>
 #include <memory>
@@ -37,7 +39,6 @@
 #include "ceres/householder_vector.h"
 #include "ceres/internal/autodiff.h"
 #include "ceres/internal/eigen.h"
-#include "ceres/local_parameterization.h"
 #include "ceres/random.h"
 #include "ceres/rotation.h"
 #include "gtest/gtest.h"
@@ -69,14 +70,11 @@ TEST(IdentityParameterization, EverythingTest) {
 
   Matrix global_matrix = Matrix::Ones(10, 3);
   Matrix local_matrix = Matrix::Zero(10, 3);
-  parameterization.MultiplyByJacobian(x,
-                                      10,
-                                      global_matrix.data(),
-                                      local_matrix.data());
+  parameterization.MultiplyByJacobian(
+      x, 10, global_matrix.data(), local_matrix.data());
   EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0);
 }
 
-
 TEST(SubsetParameterization, NegativeParameterIndexDeathTest) {
   std::vector<int> constant_parameters;
   constant_parameters.push_back(-1);
@@ -161,7 +159,7 @@ TEST(SubsetParameterization, NormalFunctionTest) {
     parameterization.Plus(x, delta, x_plus_delta);
     int k = 0;
     for (int j = 0; j < kGlobalSize; ++j) {
-      if (j == i)  {
+      if (j == i) {
         EXPECT_EQ(x_plus_delta[j], x[j]);
       } else {
         EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);
@@ -193,10 +191,8 @@ TEST(SubsetParameterization, NormalFunctionTest) {
     }
 
     Matrix local_matrix = Matrix::Zero(10, kLocalSize);
-    parameterization.MultiplyByJacobian(x,
-                                        10,
-                                        global_matrix.data(),
-                                        local_matrix.data());
+    parameterization.MultiplyByJacobian(
+        x, 10, global_matrix.data(), local_matrix.data());
     Matrix expected_local_matrix =
         global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
     EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);
@@ -206,7 +202,7 @@ TEST(SubsetParameterization, NormalFunctionTest) {
 // Functor needed to implement automatically differentiated Plus for
 // quaternions.
 struct QuaternionPlus {
-  template<typename T>
+  template <typename T>
   bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
     const T squared_norm_delta =
         delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
@@ -235,10 +231,10 @@ struct QuaternionPlus {
   }
 };
 
-template<typename Parameterization, typename Plus>
-void QuaternionParameterizationTestHelper(
-    const double* x, const double* delta,
-    const double* x_plus_delta_ref) {
+template <typename Parameterization, typename Plus>
+void QuaternionParameterizationTestHelper(const double* x,
+                                          const double* delta,
+                                          const double* x_plus_delta_ref) {
   const int kGlobalSize = 4;
   const int kLocalSize = 3;
 
@@ -251,34 +247,28 @@ void QuaternionParameterizationTestHelper(
     EXPECT_NEAR(x_plus_delta[i], x_plus_delta[i], kTolerance);
   }
 
-  const double x_plus_delta_norm =
-      sqrt(x_plus_delta[0] * x_plus_delta[0] +
-           x_plus_delta[1] * x_plus_delta[1] +
-           x_plus_delta[2] * x_plus_delta[2] +
-           x_plus_delta[3] * x_plus_delta[3]);
+  const double x_plus_delta_norm = sqrt(
+      x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] +
+      x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]);
 
   EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance);
 
   double jacobian_ref[12];
   double zero_delta[kLocalSize] = {0.0, 0.0, 0.0};
   const double* parameters[2] = {x, zero_delta};
-  double* jacobian_array[2] = { NULL, jacobian_ref };
+  double* jacobian_array[2] = {NULL, jacobian_ref};
 
   // Autodiff jacobian at delta_x = 0.
-  internal::AutoDifferentiate<StaticParameterDims<kGlobalSize, kLocalSize>>(
-      Plus(),
-      parameters,
-      kGlobalSize,
-      x_plus_delta,
-      jacobian_array);
+  internal::AutoDifferentiate<kGlobalSize,
+                              StaticParameterDims<kGlobalSize, kLocalSize>>(
+      Plus(), parameters, kGlobalSize, x_plus_delta, jacobian_array);
 
   double jacobian[12];
   parameterization.ComputeJacobian(x, jacobian);
   for (int i = 0; i < 12; ++i) {
     EXPECT_TRUE(IsFinite(jacobian[i]));
     EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance)
-        << "Jacobian mismatch: i = " << i
-        << "\n Expected \n"
+        << "Jacobian mismatch: i = " << i << "\n Expected \n"
         << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize)
         << "\n Actual \n"
         << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize);
@@ -286,10 +276,8 @@ void QuaternionParameterizationTestHelper(
 
   Matrix global_matrix = Matrix::Random(10, kGlobalSize);
   Matrix local_matrix = Matrix::Zero(10, kLocalSize);
-  parameterization.MultiplyByJacobian(x,
-                                      10,
-                                      global_matrix.data(),
-                                      local_matrix.data());
+  parameterization.MultiplyByJacobian(
+      x, 10, global_matrix.data(), local_matrix.data());
   Matrix expected_local_matrix =
       global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
   EXPECT_NEAR((local_matrix - expected_local_matrix).norm(),
@@ -338,9 +326,8 @@ TEST(QuaternionParameterization, AwayFromZeroTest) {
   Normalize<4>(x);
 
   double delta[3] = {0.24, 0.15, 0.10};
-  const double delta_norm = sqrt(delta[0] * delta[0] +
-                                 delta[1] * delta[1] +
-                                 delta[2] * delta[2]);
+  const double delta_norm =
+      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
   double q_delta[4];
   q_delta[0] = cos(delta_norm);
   q_delta[1] = sin(delta_norm) / delta_norm * delta[0];
@@ -356,7 +343,7 @@ TEST(QuaternionParameterization, AwayFromZeroTest) {
 // Functor needed to implement automatically differentiated Plus for
 // Eigen's quaternion.
 struct EigenQuaternionPlus {
-  template<typename T>
+  template <typename T>
   bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
     const T norm_delta =
         sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
@@ -372,7 +359,7 @@ struct EigenQuaternionPlus {
       // constant and when used for automatic differentiation will
       // lead to a zero derivative. Instead we take a first order
       // approximation and evaluate it at zero.
-      q_delta.coeffs() <<  delta[0], delta[1], delta[2], T(1.0);
+      q_delta.coeffs() << delta[0], delta[1], delta[2], T(1.0);
     }
 
     Eigen::Map<Eigen::Quaternion<T>> x_plus_delta_ref(x_plus_delta);
@@ -415,9 +402,8 @@ TEST(EigenQuaternionParameterization, AwayFromZeroTest) {
   x.normalize();
 
   double delta[3] = {0.24, 0.15, 0.10};
-  const double delta_norm = sqrt(delta[0] * delta[0] +
-                                 delta[1] * delta[1] +
-                                 delta[2] * delta[2]);
+  const double delta_norm =
+      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
 
   // Note: w is first in the constructor.
   Eigen::Quaterniond q_delta(cos(delta_norm),
@@ -434,8 +420,9 @@ TEST(EigenQuaternionParameterization, AwayFromZeroTest) {
 // Functor needed to implement automatically differentiated Plus for
 // homogeneous vectors. Note this explicitly defined for vectors of size 4.
 struct HomogeneousVectorParameterizationPlus {
-  template<typename Scalar>
-  bool operator()(const Scalar* p_x, const Scalar* p_delta,
+  template <typename Scalar>
+  bool operator()(const Scalar* p_x,
+                  const Scalar* p_delta,
                   Scalar* p_x_plus_delta) const {
     Eigen::Map<const Eigen::Matrix<Scalar, 4, 1>> x(p_x);
     Eigen::Map<const Eigen::Matrix<Scalar, 3, 1>> delta(p_delta);
@@ -449,8 +436,8 @@ struct HomogeneousVectorParameterizationPlus {
     if (squared_norm_delta > Scalar(0.0)) {
       Scalar norm_delta = sqrt(squared_norm_delta);
       Scalar norm_delta_div_2 = 0.5 * norm_delta;
-      const Scalar sin_delta_by_delta = sin(norm_delta_div_2) /
-          norm_delta_div_2;
+      const Scalar sin_delta_by_delta =
+          sin(norm_delta_div_2) / norm_delta_div_2;
       y[0] = sin_delta_by_delta * delta[0] * one_half;
       y[1] = sin_delta_by_delta * delta[1] * one_half;
       y[2] = sin_delta_by_delta * delta[2] * one_half;
@@ -487,14 +474,12 @@ static void HomogeneousVectorParameterizationHelper(const double* x,
   double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
   homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta);
 
-  const double x_plus_delta_norm =
-      sqrt(x_plus_delta[0] * x_plus_delta[0] +
-           x_plus_delta[1] * x_plus_delta[1] +
-           x_plus_delta[2] * x_plus_delta[2] +
-           x_plus_delta[3] * x_plus_delta[3]);
+  const double x_plus_delta_norm = sqrt(
+      x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] +
+      x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]);
 
-  const double x_norm = sqrt(x[0] * x[0] + x[1] * x[1] +
-                             x[2] * x[2] + x[3] * x[3]);
+  const double x_norm =
+      sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]);
 
   EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance);
 
@@ -580,34 +565,33 @@ TEST(HomogeneousVectorParameterization, DeathTests) {
   EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");
 }
 
-
 class ProductParameterizationTest : public ::testing::Test {
- protected :
+ protected:
   void SetUp() final {
     const int global_size1 = 5;
     std::vector<int> constant_parameters1;
     constant_parameters1.push_back(2);
-    param1_.reset(new SubsetParameterization(global_size1,
-                                             constant_parameters1));
+    param1_.reset(
+        new SubsetParameterization(global_size1, constant_parameters1));
 
     const int global_size2 = 3;
     std::vector<int> constant_parameters2;
     constant_parameters2.push_back(0);
     constant_parameters2.push_back(1);
-    param2_.reset(new SubsetParameterization(global_size2,
-                                             constant_parameters2));
+    param2_.reset(
+        new SubsetParameterization(global_size2, constant_parameters2));
 
     const int global_size3 = 4;
     std::vector<int> constant_parameters3;
     constant_parameters3.push_back(1);
-    param3_.reset(new SubsetParameterization(global_size3,
-                                             constant_parameters3));
+    param3_.reset(
+        new SubsetParameterization(global_size3, constant_parameters3));
 
     const int global_size4 = 2;
     std::vector<int> constant_parameters4;
     constant_parameters4.push_back(1);
-    param4_.reset(new SubsetParameterization(global_size4,
-                                             constant_parameters4));
+    param4_.reset(
+        new SubsetParameterization(global_size4, constant_parameters4));
   }
 
   std::unique_ptr<LocalParameterization> param1_;
@@ -627,7 +611,6 @@ TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) {
             param1->GlobalSize() + param2->GlobalSize());
 }
 
-
 TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) {
   LocalParameterization* param1 = param1_.release();
   LocalParameterization* param2 = param2_.release();
@@ -648,15 +631,11 @@ TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) {
 
   ProductParameterization product_param(param1, param2, param3, param4);
   EXPECT_EQ(product_param.LocalSize(),
-            param1->LocalSize() +
-            param2->LocalSize() +
-            param3->LocalSize() +
-            param4->LocalSize());
+            param1->LocalSize() + param2->LocalSize() + param3->LocalSize() +
+                param4->LocalSize());
   EXPECT_EQ(product_param.GlobalSize(),
-            param1->GlobalSize() +
-            param2->GlobalSize() +
-            param3->GlobalSize() +
-            param4->GlobalSize());
+            param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize() +
+                param4->GlobalSize());
 }
 
 TEST_F(ProductParameterizationTest, Plus) {
@@ -683,27 +662,23 @@ TEST_F(ProductParameterizationTest, Plus) {
   int x_cursor = 0;
   int delta_cursor = 0;
 
-  EXPECT_TRUE(param1->Plus(&x[x_cursor],
-                           &delta[delta_cursor],
-                           &x_plus_delta[x_cursor]));
+  EXPECT_TRUE(param1->Plus(
+      &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
   x_cursor += param1->GlobalSize();
   delta_cursor += param1->LocalSize();
 
-  EXPECT_TRUE(param2->Plus(&x[x_cursor],
-                           &delta[delta_cursor],
-                           &x_plus_delta[x_cursor]));
+  EXPECT_TRUE(param2->Plus(
+      &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
   x_cursor += param2->GlobalSize();
   delta_cursor += param2->LocalSize();
 
-  EXPECT_TRUE(param3->Plus(&x[x_cursor],
-                           &delta[delta_cursor],
-                           &x_plus_delta[x_cursor]));
+  EXPECT_TRUE(param3->Plus(
+      &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
   x_cursor += param3->GlobalSize();
   delta_cursor += param3->LocalSize();
 
-  EXPECT_TRUE(param4->Plus(&x[x_cursor],
-                           &delta[delta_cursor],
-                           &x_plus_delta[x_cursor]));
+  EXPECT_TRUE(param4->Plus(
+      &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
   x_cursor += param4->GlobalSize();
   delta_cursor += param4->LocalSize();
 
@@ -725,45 +700,41 @@ TEST_F(ProductParameterizationTest, ComputeJacobian) {
     x[i] = RandNormal();
   }
 
-  Matrix jacobian = Matrix::Random(product_param.GlobalSize(),
-                                   product_param.LocalSize());
+  Matrix jacobian =
+      Matrix::Random(product_param.GlobalSize(), product_param.LocalSize());
   EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data()));
   int x_cursor = 0;
   int delta_cursor = 0;
 
   Matrix jacobian1(param1->GlobalSize(), param1->LocalSize());
   EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data()));
-  jacobian.block(x_cursor, delta_cursor,
-                 param1->GlobalSize(),
-                 param1->LocalSize())
-      -= jacobian1;
+  jacobian.block(
+      x_cursor, delta_cursor, param1->GlobalSize(), param1->LocalSize()) -=
+      jacobian1;
   x_cursor += param1->GlobalSize();
   delta_cursor += param1->LocalSize();
 
   Matrix jacobian2(param2->GlobalSize(), param2->LocalSize());
   EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data()));
-  jacobian.block(x_cursor, delta_cursor,
-                 param2->GlobalSize(),
-                 param2->LocalSize())
-      -= jacobian2;
+  jacobian.block(
+      x_cursor, delta_cursor, param2->GlobalSize(), param2->LocalSize()) -=
+      jacobian2;
   x_cursor += param2->GlobalSize();
   delta_cursor += param2->LocalSize();
 
   Matrix jacobian3(param3->GlobalSize(), param3->LocalSize());
   EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data()));
-  jacobian.block(x_cursor, delta_cursor,
-                 param3->GlobalSize(),
-                 param3->LocalSize())
-      -= jacobian3;
+  jacobian.block(
+      x_cursor, delta_cursor, param3->GlobalSize(), param3->LocalSize()) -=
+      jacobian3;
   x_cursor += param3->GlobalSize();
   delta_cursor += param3->LocalSize();
 
   Matrix jacobian4(param4->GlobalSize(), param4->LocalSize());
   EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data()));
-  jacobian.block(x_cursor, delta_cursor,
-                 param4->GlobalSize(),
-                 param4->LocalSize())
-      -= jacobian4;
+  jacobian.block(
+      x_cursor, delta_cursor, param4->GlobalSize(), param4->LocalSize()) -=
+      jacobian4;
   x_cursor += param4->GlobalSize();
   delta_cursor += param4->LocalSize();