mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Sameer Agarwal
2dd907744d
ResidualBlock evaluation has logic to ensure that CostFunction should always fill out the residual and jacobian arrays completely by using a special value to pre-populate these arrays. This works for CostFunctions with analytical Jacobians but not for AutoDiffCostFunction and NumericDiffCostFunction Jacobians. There is no way to fix this for NumericDiffCostFunctions without introducing significant performance penalties but the residual evaluation fails, which should be enough to catch such errors. For AutoDiffCostFunction the way the Jets are default initialized was sidestepping this check. So now, the Jet that is used to capture the output residuals is now initialized with kImpossibleValue, which will ensure that if the user forgets to fill all output fields, it triggers an evaluation error. This change required that ceres::internal::kImpossibleValue be moved out of array_utils.h/cc to types.h. Change-Id: I35bb0946cf0785a5d43c7b5459a2272848fb2a9b
325 lines
14 KiB
C++
325 lines
14 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)
|
|
//
|
|
// Computation of the Jacobian matrix for vector-valued functions of multiple
|
|
// variables, using automatic differentiation based on the implementation of
|
|
// dual numbers in jet.h. Before reading the rest of this file, it is adivsable
|
|
// to read jet.h's header comment in detail.
|
|
//
|
|
// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
|
|
// functors with templated operator() taking this form:
|
|
//
|
|
// struct F {
|
|
// template<typename T>
|
|
// bool operator()(const T *x, const T *y, ..., T *z) {
|
|
// // Compute z[] based on x[], y[], ...
|
|
// // return true if computation succeeded, false otherwise.
|
|
// }
|
|
// };
|
|
//
|
|
// All inputs and outputs may be vector-valued.
|
|
//
|
|
// To understand how jets are used to compute the jacobian, a
|
|
// picture may help. Consider a vector-valued function, F, returning 3
|
|
// dimensions and taking a vector-valued parameter of 4 dimensions:
|
|
//
|
|
// y x
|
|
// [ * ] F [ * ]
|
|
// [ * ] <--- [ * ]
|
|
// [ * ] [ * ]
|
|
// [ * ]
|
|
//
|
|
// Similar to the 2-parameter example for f described in jet.h, computing the
|
|
// jacobian dy/dx is done by substutiting a suitable jet object for x and all
|
|
// intermediate steps of the computation of F. Since x is has 4 dimensions, use
|
|
// a Jet<double, 4>.
|
|
//
|
|
// Before substituting a jet object for x, the dual components are set
|
|
// appropriately for each dimension of x:
|
|
//
|
|
// y x
|
|
// [ * | * * * * ] f [ * | 1 0 0 0 ] x0
|
|
// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1
|
|
// [ * | * * * * ] [ * | 0 0 1 0 ] x2
|
|
// ---+--- [ * | 0 0 0 1 ] x3
|
|
// | ^ ^ ^ ^
|
|
// dy/dx | | | +----- infinitesimal for x3
|
|
// | | +------- infinitesimal for x2
|
|
// | +--------- infinitesimal for x1
|
|
// +----------- infinitesimal for x0
|
|
//
|
|
// The reason to set the internal 4x4 submatrix to the identity is that we wish
|
|
// to take the derivative of y separately with respect to each dimension of x.
|
|
// Each column of the 4x4 identity is therefore for a single component of the
|
|
// independent variable x.
|
|
//
|
|
// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
|
|
// extended y vector, indicated in the above diagram.
|
|
//
|
|
// Functors with multiple parameters
|
|
// ---------------------------------
|
|
// In practice, it is often convenient to use a function f of two or more
|
|
// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
|
|
// framework is designed for a single-parameter vector-valued input. The wrapper
|
|
// in this file addresses this issue adding support for functions with one or
|
|
// more parameter vectors.
|
|
//
|
|
// To support multiple parameters, all the parameter vectors are concatenated
|
|
// into one and treated as a single parameter vector, except that since the
|
|
// functor expects different inputs, we need to construct the jets as if they
|
|
// were part of a single parameter vector. The extended jets are passed
|
|
// separately for each parameter.
|
|
//
|
|
// For example, consider a functor F taking two vector parameters, p[2] and
|
|
// q[3], and producing an output y[4]:
|
|
//
|
|
// struct F {
|
|
// template<typename T>
|
|
// bool operator()(const T *p, const T *q, T *z) {
|
|
// // ...
|
|
// }
|
|
// };
|
|
//
|
|
// In this case, the necessary jet type is Jet<double, 5>. Here is a
|
|
// visualization of the jet objects in this case:
|
|
//
|
|
// Dual components for p ----+
|
|
// |
|
|
// -+-
|
|
// y [ * | 1 0 | 0 0 0 ] --- p[0]
|
|
// [ * | 0 1 | 0 0 0 ] --- p[1]
|
|
// [ * | . . | + + + ] |
|
|
// [ * | . . | + + + ] v
|
|
// [ * | . . | + + + ] <--- F(p, q)
|
|
// [ * | . . | + + + ] ^
|
|
// ^^^ ^^^^^ |
|
|
// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0]
|
|
// [ * | 0 0 | 0 1 0 ] --- q[1]
|
|
// [ * | 0 0 | 0 0 1 ] --- q[2]
|
|
// --+--
|
|
// |
|
|
// Dual components for q --------------+
|
|
//
|
|
// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
|
|
// of y in the above diagram are the derivatives of y with respect to p and q
|
|
// respectively. This is how autodiff works for functors taking multiple vector
|
|
// valued arguments (up to 6).
|
|
//
|
|
// Jacobian NULL pointers
|
|
// ----------------------
|
|
// In general, the functions below will accept NULL pointers for all or some of
|
|
// the Jacobian parameters, meaning that those Jacobians will not be computed.
|
|
|
|
#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
|
|
#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
|
|
|
|
#include <stddef.h>
|
|
|
|
#include "ceres/jet.h"
|
|
#include "ceres/internal/eigen.h"
|
|
#include "ceres/internal/fixed_array.h"
|
|
#include "ceres/internal/variadic_evaluate.h"
|
|
#include "glog/logging.h"
|
|
|
|
namespace ceres {
|
|
namespace internal {
|
|
|
|
// Extends src by a 1st order pertubation for every dimension and puts it in
|
|
// dst. The size of src is N. Since this is also used for perturbations in
|
|
// blocked arrays, offset is used to shift which part of the jet the
|
|
// perturbation occurs. This is used to set up the extended x augmented by an
|
|
// identity matrix. The JetT type should be a Jet type, and T should be a
|
|
// numeric type (e.g. double). For example,
|
|
//
|
|
// 0 1 2 3 4 5 6 7 8
|
|
// dst[0] [ * | . . | 1 0 0 | . . . ]
|
|
// dst[1] [ * | . . | 0 1 0 | . . . ]
|
|
// dst[2] [ * | . . | 0 0 1 | . . . ]
|
|
//
|
|
// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
|
|
// was 8-dimensional.
|
|
template <typename JetT, typename T, int N>
|
|
inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
|
|
DCHECK(src);
|
|
DCHECK(dst);
|
|
for (int j = 0; j < N; ++j) {
|
|
dst[j].a = src[j];
|
|
dst[j].v.setZero();
|
|
dst[j].v[offset + j] = T(1.0);
|
|
}
|
|
}
|
|
|
|
// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
|
|
// dst. This is used to pick out the "vector" part of the extended y.
|
|
template <typename JetT, typename T>
|
|
inline void Take0thOrderPart(int M, const JetT *src, T dst) {
|
|
DCHECK(src);
|
|
for (int i = 0; i < M; ++i) {
|
|
dst[i] = src[i].a;
|
|
}
|
|
}
|
|
|
|
// Takes N 1st order parts, starting at index N0, and puts them in the M x N
|
|
// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
|
|
template <typename JetT, typename T, int N0, int N>
|
|
inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
|
|
DCHECK(src);
|
|
DCHECK(dst);
|
|
for (int i = 0; i < M; ++i) {
|
|
Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
|
|
src[i].v.template segment<N>(N0);
|
|
}
|
|
}
|
|
|
|
// This is in a struct because default template parameters on a
|
|
// function are not supported in C++03 (though it is available in
|
|
// C++0x). N0 through N5 are the dimension of the input arguments to
|
|
// the user supplied functor.
|
|
template <typename Functor, typename T,
|
|
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
|
|
int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
|
|
struct AutoDiff {
|
|
static bool Differentiate(const Functor& functor,
|
|
T const *const *parameters,
|
|
int num_outputs,
|
|
T *function_value,
|
|
T **jacobians) {
|
|
// This block breaks the 80 column rule to keep it somewhat readable.
|
|
DCHECK_GT(num_outputs, 0);
|
|
DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
|
|
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
|
|
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || // NOLINT
|
|
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) // NOLINT
|
|
<< "Zero block cannot precede a non-zero block. Block sizes are "
|
|
<< "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
|
|
<< N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
|
|
<< N8 << ", " << N9;
|
|
|
|
typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
|
|
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
|
|
N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
|
|
|
|
// These are the positions of the respective jets in the fixed array x.
|
|
const int jet0 = 0;
|
|
const int jet1 = N0;
|
|
const int jet2 = N0 + N1;
|
|
const int jet3 = N0 + N1 + N2;
|
|
const int jet4 = N0 + N1 + N2 + N3;
|
|
const int jet5 = N0 + N1 + N2 + N3 + N4;
|
|
const int jet6 = N0 + N1 + N2 + N3 + N4 + N5;
|
|
const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6;
|
|
const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
|
|
const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
|
|
|
|
const JetT *unpacked_parameters[10] = {
|
|
x.get() + jet0,
|
|
x.get() + jet1,
|
|
x.get() + jet2,
|
|
x.get() + jet3,
|
|
x.get() + jet4,
|
|
x.get() + jet5,
|
|
x.get() + jet6,
|
|
x.get() + jet7,
|
|
x.get() + jet8,
|
|
x.get() + jet9,
|
|
};
|
|
|
|
JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
|
|
|
|
// Invalidate the output Jets, so that we can detect if the user
|
|
// did not assign values to all of them.
|
|
for (int i = 0; i < num_outputs; ++i) {
|
|
output[i].a = kImpossibleValue;
|
|
output[i].v.setConstant(kImpossibleValue);
|
|
}
|
|
|
|
#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \
|
|
if (N ## i) { \
|
|
internal::Make1stOrderPerturbation<JetT, T, N ## i>( \
|
|
jet ## i, \
|
|
parameters[i], \
|
|
x.get() + jet ## i); \
|
|
}
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(0);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(1);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(2);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(3);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(4);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(5);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(6);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(7);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(8);
|
|
CERES_MAKE_1ST_ORDER_PERTURBATION(9);
|
|
#undef CERES_MAKE_1ST_ORDER_PERTURBATION
|
|
|
|
if (!VariadicEvaluate<Functor, JetT,
|
|
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
|
|
functor, unpacked_parameters, output)) {
|
|
return false;
|
|
}
|
|
|
|
internal::Take0thOrderPart(num_outputs, output, function_value);
|
|
|
|
#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
|
|
if (N ## i) { \
|
|
if (jacobians[i]) { \
|
|
internal::Take1stOrderPart<JetT, T, \
|
|
jet ## i, \
|
|
N ## i>(num_outputs, \
|
|
output, \
|
|
jacobians[i]); \
|
|
} \
|
|
}
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(0);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(1);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(2);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(3);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(4);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(5);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(6);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(7);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(8);
|
|
CERES_TAKE_1ST_ORDER_PERTURBATION(9);
|
|
#undef CERES_TAKE_1ST_ORDER_PERTURBATION
|
|
return true;
|
|
}
|
|
};
|
|
|
|
} // namespace internal
|
|
} // namespace ceres
|
|
|
|
#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_
|