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
diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h index b192867..5605e0b 100644 --- a/include/ceres/autodiff_cost_function.h +++ b/include/ceres/autodiff_cost_function.h
@@ -191,7 +191,7 @@ return internal::VariadicEvaluate<ParameterDims>( *functor_, parameters, residuals); } - return internal::AutoDifferentiate<ParameterDims>( + return internal::AutoDifferentiate<kNumResiduals, ParameterDims>( *functor_, parameters, SizedCostFunction<kNumResiduals, Ns...>::num_residuals(),
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h index 9d59a46..d694376 100644 --- a/include/ceres/autodiff_local_parameterization.h +++ b/include/ceres/autodiff_local_parameterization.h
@@ -135,6 +135,7 @@ 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); }
diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h index ef7cfea..72b8e37 100644 --- a/include/ceres/internal/autodiff.h +++ b/include/ceres/internal/autodiff.h
@@ -193,12 +193,14 @@ 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 @@ 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 @@ 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 @@ Make1stOrderPerturbations<Parameters>::Apply(parameters, x.data()); - if (!VariadicEvaluate<ParameterDims>(functor, unpacked_parameters.data(), - output)) { + if (!VariadicEvaluate<ParameterDims>( + functor, unpacked_parameters.data(), output)) { return false; }
diff --git a/internal/ceres/autodiff_test.cc b/internal/ceres/autodiff_test.cc index 04a77ea..2d56400 100644 --- a/internal/ceres/autodiff_test.cc +++ b/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 @@ // 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 @@ 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 @@ 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 @@ } // 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 @@ // 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 @@ 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 @@ 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 @@ // 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 @@ // 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 @@ // 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 @@ 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 @@ }; 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 @@ 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 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ { 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 @@ { 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 @@ { 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 @@ { 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 @@ { 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 @@ { 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 @@ { 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 @@ { 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 @@ 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 @@ { 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) {
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index 336474c..9334287 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/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 @@ 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 @@ 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 @@ } 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 @@ // 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 @@ } }; -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 @@ 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 @@ 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 @@ 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 @@ // 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 @@ // 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 @@ 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 @@ // 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 @@ 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 @@ 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 @@ 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 @@ param1->GlobalSize() + param2->GlobalSize()); } - TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) { LocalParameterization* param1 = param1_.release(); LocalParameterization* param2 = param2_.release(); @@ -648,15 +631,11 @@ 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 @@ 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 @@ 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();