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
|