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();
