mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Keir Mierle
7492b0d8de
Since Ceres is moving to using GitHub for issues, and the Google Code URL in the current copyright header will soon become invalid, update all the headers. Change-Id: I1fce70375d1bcf098591f07b4d8f01a5c1e0789c
678 lines
20 KiB
C++
678 lines
20 KiB
C++
// Ceres Solver - A fast non-linear least squares minimizer
|
|
// Copyright 2015 Google Inc. All rights reserved.
|
|
// http://ceres-solver.org/
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistributions of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
// * Neither the name of Google Inc. nor the names of its contributors may be
|
|
// used to endorse or promote products derived from this software without
|
|
// specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
// POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Author: keir@google.com (Keir Mierle)
|
|
|
|
#include "ceres/internal/autodiff.h"
|
|
|
|
#include "gtest/gtest.h"
|
|
#include "ceres/random.h"
|
|
|
|
namespace ceres {
|
|
namespace internal {
|
|
|
|
template <typename T> inline
|
|
T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
|
|
return base[cols * i + j];
|
|
}
|
|
|
|
// Do (symmetric) finite differencing using the given function object 'b' of
|
|
// type 'B' and scalar type 'T' with step size 'del'.
|
|
//
|
|
// The type B should have a signature
|
|
//
|
|
// 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.
|
|
if (!b(par, fun)) {
|
|
return false;
|
|
}
|
|
|
|
// Temporary parameter vector.
|
|
T tmp_par[N];
|
|
for (int j = 0; j < N; ++j) {
|
|
tmp_par[j] = par[j];
|
|
}
|
|
|
|
// For each dimension, we do one forward step and one backward step in
|
|
// parameter space, and store the output vector vectors in these vectors.
|
|
T fwd_fun[M];
|
|
T bwd_fun[M];
|
|
|
|
for (int j = 0; j < N; ++j) {
|
|
// Forward step.
|
|
tmp_par[j] = par[j] + del;
|
|
if (!b(tmp_par, fwd_fun)) {
|
|
return false;
|
|
}
|
|
|
|
// Backward step.
|
|
tmp_par[j] = par[j] - del;
|
|
if (!b(tmp_par, bwd_fun)) {
|
|
return false;
|
|
}
|
|
|
|
// Symmetric differencing:
|
|
// f'(a) = (f(a + h) - f(a - h)) / (2 h)
|
|
for (int i = 0; i < M; ++i) {
|
|
RowMajorAccess(jac, M, N, i, j) =
|
|
(fwd_fun[i] - bwd_fun[i]) / (T(2) * del);
|
|
}
|
|
|
|
// Restore our temporary vector.
|
|
tmp_par[j] = par[j];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
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];
|
|
A c = q[2];
|
|
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;
|
|
#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
|
|
#undef R
|
|
}
|
|
|
|
// A structure for projecting a 3x4 camera matrix and a
|
|
// homogeneous 3D point, to a 2D inhomogeneous point.
|
|
struct Projective {
|
|
// Function that takes P and X as separate vectors:
|
|
// P, X -> x
|
|
template <typename A>
|
|
bool operator()(A const P[12], A const X[4], A x[2]) const {
|
|
A PX[3];
|
|
for (int i = 0; i < 3; ++i) {
|
|
PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
|
|
RowMajorAccess(P, 3, 4, i, 1) * X[1] +
|
|
RowMajorAccess(P, 3, 4, i, 2) * X[2] +
|
|
RowMajorAccess(P, 3, 4, i, 3) * X[3];
|
|
}
|
|
if (PX[2] != 0.0) {
|
|
x[0] = PX[0] / PX[2];
|
|
x[1] = PX[1] / PX[2];
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// Version that takes P and X packed in one vector:
|
|
//
|
|
// (P, X) -> x
|
|
//
|
|
template <typename A>
|
|
bool operator()(A const P_X[12 + 4], A x[2]) const {
|
|
return operator()(P_X + 0, P_X + 12, x);
|
|
}
|
|
};
|
|
|
|
// Test projective camera model projector.
|
|
TEST(AutoDiff, ProjectiveCameraModel) {
|
|
srand(5);
|
|
double const tol = 1e-10; // floating-point tolerance.
|
|
double const del = 1e-4; // finite-difference step.
|
|
double const err = 1e-6; // finite-difference tolerance.
|
|
|
|
Projective b;
|
|
|
|
// Make random P and X, in a single vector.
|
|
double PX[12 + 4];
|
|
for (int i = 0; i < 12 + 4; ++i) {
|
|
PX[i] = RandDouble();
|
|
}
|
|
|
|
// Handy names for the P and X parts.
|
|
double *P = PX + 0;
|
|
double *X = PX + 12;
|
|
|
|
// Apply the mapping, to get image point b_x.
|
|
double b_x[2];
|
|
b(P, X, b_x);
|
|
|
|
// 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)));
|
|
|
|
for (int i = 0; i < 2; ++i) {
|
|
ASSERT_EQ(fd_x[i], b_x[i]);
|
|
}
|
|
|
|
// Use automatic differentiation to compute the Jacobian.
|
|
double ad_x1[2];
|
|
double J_PX[2 * (12 + 4)];
|
|
{
|
|
double *parameters[] = { PX };
|
|
double *jacobians[] = { J_PX };
|
|
ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
|
|
b, parameters, 2, ad_x1, jacobians)));
|
|
|
|
for (int i = 0; i < 2; ++i) {
|
|
ASSERT_NEAR(ad_x1[i], b_x[i], tol);
|
|
}
|
|
}
|
|
|
|
// Use automatic differentiation (again), with two arguments.
|
|
{
|
|
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((AutoDiff<Projective, double, 12, 4>::Differentiate(
|
|
b, parameters, 2, ad_x2, jacobians)));
|
|
|
|
for (int i = 0; i < 2; ++i) {
|
|
ASSERT_NEAR(ad_x2[i], b_x[i], tol);
|
|
}
|
|
|
|
// Now compare the jacobians we got.
|
|
for (int i = 0; i < 2; ++i) {
|
|
for (int j = 0; j < 12 + 4; ++j) {
|
|
ASSERT_NEAR(J_PX[(12 + 4) * i + j], fd_J[(12 + 4) * i + j], err);
|
|
}
|
|
|
|
for (int j = 0; j < 12; ++j) {
|
|
ASSERT_NEAR(J_PX[(12 + 4) * i + j], J_P[12 * i + j], tol);
|
|
}
|
|
for (int j = 0; j < 4; ++j) {
|
|
ASSERT_NEAR(J_PX[(12 + 4) * i + 12 + j], J_X[4 * i + j], tol);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Object to implement the projection by a calibrated camera.
|
|
struct Metric {
|
|
// The mapping is
|
|
//
|
|
// q, c, X -> x = dehomg(R(q) (X - c))
|
|
//
|
|
// where q is a quaternion and c is the center of projection.
|
|
//
|
|
// This function takes three input vectors.
|
|
template <typename A>
|
|
bool operator()(A const q[4], A const c[3], A const X[3], A x[2]) const {
|
|
A R[3 * 3];
|
|
QuaternionToScaledRotation(q, R);
|
|
|
|
// Convert the quaternion mapping all the way to projective matrix.
|
|
A P[3 * 4];
|
|
|
|
// Set P(:, 1:3) = R
|
|
for (int i = 0; i < 3; ++i) {
|
|
for (int j = 0; j < 3; ++j) {
|
|
RowMajorAccess(P, 3, 4, i, j) = RowMajorAccess(R, 3, 3, i, j);
|
|
}
|
|
}
|
|
|
|
// Set P(:, 4) = - R c
|
|
for (int i = 0; i < 3; ++i) {
|
|
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) };
|
|
Projective p;
|
|
return p(P, X1, x);
|
|
}
|
|
|
|
// A version that takes a single vector.
|
|
template <typename A>
|
|
bool operator()(A const q_c_X[4 + 3 + 3], A x[2]) const {
|
|
return operator()(q_c_X, q_c_X + 4, q_c_X + 4 + 3, x);
|
|
}
|
|
};
|
|
|
|
// This test is similar in structure to the previous one.
|
|
TEST(AutoDiff, Metric) {
|
|
srand(5);
|
|
double const tol = 1e-10; // floating-point tolerance.
|
|
double const del = 1e-4; // finite-difference step.
|
|
double const err = 1e-5; // finite-difference tolerance.
|
|
|
|
Metric b;
|
|
|
|
// Make random parameter vector.
|
|
double qcX[4 + 3 + 3];
|
|
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;
|
|
|
|
// Compute projection, b_x.
|
|
double b_x[2];
|
|
ASSERT_TRUE(b(q, c, X, b_x));
|
|
|
|
// 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)));
|
|
|
|
for (int i = 0; i < 2; ++i) {
|
|
ASSERT_NEAR(fd_x[i], b_x[i], tol);
|
|
}
|
|
|
|
// Automatic differentiation.
|
|
double ad_x[2];
|
|
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((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
|
|
b, parameters, 2, ad_x, jacobians)));
|
|
|
|
for (int i = 0; i < 2; ++i) {
|
|
ASSERT_NEAR(ad_x[i], b_x[i], tol);
|
|
}
|
|
|
|
// Compare the pieces.
|
|
for (int i = 0; i < 2; ++i) {
|
|
for (int j = 0; j < 4; ++j) {
|
|
ASSERT_NEAR(J_q[4 * i + j], fd_J[(4 + 3 + 3) * i + j], err);
|
|
}
|
|
for (int j = 0; j < 3; ++j) {
|
|
ASSERT_NEAR(J_c[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4], err);
|
|
}
|
|
for (int j = 0; j < 3; ++j) {
|
|
ASSERT_NEAR(J_X[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4 + 3], err);
|
|
}
|
|
}
|
|
}
|
|
|
|
struct VaryingResidualFunctor {
|
|
template <typename T>
|
|
bool operator()(const T x[2], T* y) const {
|
|
for (int i = 0; i < num_residuals; ++i) {
|
|
y[i] = T(i) * x[0] * x[1] * x[1];
|
|
}
|
|
return true;
|
|
}
|
|
|
|
int num_residuals;
|
|
};
|
|
|
|
TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
|
|
double x[2] = { 1.0, 5.5 };
|
|
double *parameters[] = { x };
|
|
const int kMaxResiduals = 10;
|
|
double J_x[2 * kMaxResiduals];
|
|
double residuals[kMaxResiduals];
|
|
double *jacobians[] = { J_x };
|
|
|
|
// Use a single functor, but tweak it to produce different numbers of
|
|
// residuals.
|
|
VaryingResidualFunctor functor;
|
|
|
|
for (int num_residuals = 1; num_residuals < kMaxResiduals; ++num_residuals) {
|
|
// Tweak the number of residuals to produce.
|
|
functor.num_residuals = num_residuals;
|
|
|
|
// Run autodiff with the new number of residuals.
|
|
ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
|
|
functor, parameters, num_residuals, residuals, jacobians)));
|
|
|
|
const double kTolerance = 1e-14;
|
|
for (int i = 0; i < num_residuals; ++i) {
|
|
EXPECT_NEAR(J_x[2 * i + 0], i * x[1] * x[1], kTolerance) << "i: " << i;
|
|
EXPECT_NEAR(J_x[2 * i + 1], 2 * i * x[0] * x[1], kTolerance)
|
|
<< "i: " << i;
|
|
}
|
|
}
|
|
}
|
|
|
|
struct Residual1Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0, T* y) const {
|
|
y[0] = *x0;
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual2Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0, const T* x1, T* y) const {
|
|
y[0] = *x0 + pow(*x1, 2);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual3Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0, const T* x1, const T* x2, T* y) const {
|
|
y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual4Param {
|
|
template <typename T>
|
|
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;
|
|
}
|
|
};
|
|
|
|
struct Residual5Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
T* y) const {
|
|
y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual6Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
const T* x5,
|
|
T* y) const {
|
|
y[0] = *x0 + pow(*x1, 2) + pow(*x2, 3) + pow(*x3, 4) + pow(*x4, 5) +
|
|
pow(*x5, 6);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual7Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
const T* x5,
|
|
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);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual8Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
const T* x5,
|
|
const T* x6,
|
|
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);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual9Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
const T* x5,
|
|
const T* x6,
|
|
const T* x7,
|
|
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);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
struct Residual10Param {
|
|
template <typename T>
|
|
bool operator()(const T* x0,
|
|
const T* x1,
|
|
const T* x2,
|
|
const T* x3,
|
|
const T* x4,
|
|
const T* x5,
|
|
const T* x6,
|
|
const T* x7,
|
|
const T* x8,
|
|
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);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
TEST(AutoDiff, VariadicAutoDiff) {
|
|
double x[10];
|
|
double residual = 0;
|
|
double* parameters[10];
|
|
double jacobian_values[10];
|
|
double* jacobians[10];
|
|
|
|
for (int i = 0; i < 10; ++i) {
|
|
x[i] = 2.0;
|
|
parameters[i] = x + i;
|
|
jacobians[i] = jacobian_values + i;
|
|
}
|
|
|
|
{
|
|
Residual1Param functor;
|
|
int num_variables = 1;
|
|
EXPECT_TRUE((AutoDiff<Residual1Param, double, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual2Param functor;
|
|
int num_variables = 2;
|
|
EXPECT_TRUE((AutoDiff<Residual2Param, double, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual3Param functor;
|
|
int num_variables = 3;
|
|
EXPECT_TRUE((AutoDiff<Residual3Param, double, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual4Param functor;
|
|
int num_variables = 4;
|
|
EXPECT_TRUE((AutoDiff<Residual4Param, double, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual5Param functor;
|
|
int num_variables = 5;
|
|
EXPECT_TRUE((AutoDiff<Residual5Param, double, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual6Param functor;
|
|
int num_variables = 6;
|
|
EXPECT_TRUE((AutoDiff<Residual6Param,
|
|
double,
|
|
1, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual7Param functor;
|
|
int num_variables = 7;
|
|
EXPECT_TRUE((AutoDiff<Residual7Param,
|
|
double,
|
|
1, 1, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual8Param functor;
|
|
int num_variables = 8;
|
|
EXPECT_TRUE((AutoDiff<
|
|
Residual8Param,
|
|
double, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual9Param functor;
|
|
int num_variables = 9;
|
|
EXPECT_TRUE((AutoDiff<
|
|
Residual9Param,
|
|
double,
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
|
|
{
|
|
Residual10Param functor;
|
|
int num_variables = 10;
|
|
EXPECT_TRUE((AutoDiff<
|
|
Residual10Param,
|
|
double,
|
|
1, 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
|
|
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));
|
|
}
|
|
}
|
|
}
|
|
|
|
// This is fragile test that triggers the alignment bug on
|
|
// i686-apple-darwin10-llvm-g++-4.2 (GCC) 4.2.1. It is quite possible,
|
|
// that other combinations of operating system + compiler will
|
|
// re-arrange the operations in this test.
|
|
//
|
|
// But this is the best (and only) way we know of to trigger this
|
|
// problem for now. A more robust solution that guarantees the
|
|
// alignment of Eigen types used for automatic differentiation would
|
|
// be nice.
|
|
TEST(AutoDiff, AlignedAllocationTest) {
|
|
// This int is needed to allocate 16 bits on the stack, so that the
|
|
// next allocation is not aligned by default.
|
|
char y = 0;
|
|
|
|
// This is needed to prevent the compiler from optimizing y out of
|
|
// this function.
|
|
y += 1;
|
|
|
|
typedef Jet<double, 2> JetT;
|
|
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(3);
|
|
|
|
// Need this to makes sure that x does not get optimized out.
|
|
x[0] = x[0] + JetT(1.0);
|
|
}
|
|
|
|
} // namespace internal
|
|
} // namespace ceres
|