blob: fe64e824f9aeb19986b4b5f39518678a2058046e [file] [log] [blame]
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2019 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 advisable
// to read jet.h's header comment in detail.
//
// The helper wrapper AutoDifferentiate() 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 substituting 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 (nullptr)
// --------------------------------
// In general, the functions below will accept nullptr 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 <array>
#include <cstddef>
#include <utility>
#include "ceres/internal/array_selector.h"
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/parameter_dims.h"
#include "ceres/internal/variadic_evaluate.h"
#include "ceres/jet.h"
#include "ceres/types.h"
#include "glog/logging.h"
// If the number of parameters exceeds this values, the corresponding jets are
// placed on the heap. This will reduce performance by a factor of 2-5 on
// current compilers.
#ifndef CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK
#define CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK 50
#endif
#ifndef CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK
#define CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK 20
#endif
namespace ceres {
namespace internal {
// Extends src by a 1st order perturbation 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 <int j, int N, int Offset, typename T, typename JetT>
struct Make1stOrderPerturbation {
public:
inline static void Apply(const T* src, JetT* dst) {
if (j == 0) {
DCHECK(src);
DCHECK(dst);
}
dst[j] = JetT(src[j], j + Offset);
Make1stOrderPerturbation<j + 1, N, Offset, T, JetT>::Apply(src, dst);
}
};
template <int N, int Offset, typename T, typename JetT>
struct Make1stOrderPerturbation<N, N, Offset, T, JetT> {
public:
static void Apply(const T* src, JetT* dst) {}
};
// Calls Make1stOrderPerturbation for every parameter block.
//
// Example:
// If one having three parameter blocks with dimensions (3, 2, 4), the call
// Make1stOrderPerturbations<integer_sequence<3, 2, 4>::Apply(params, x);
// will result in the following calls to Make1stOrderPerturbation:
// Make1stOrderPerturbation<0, 3, 0>::Apply(params[0], x + 0);
// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3);
// Make1stOrderPerturbation<0, 4, 5>::Apply(params[2], x + 5);
template <typename Seq, int ParameterIdx = 0, int Offset = 0>
struct Make1stOrderPerturbations;
template <int N, int... Ns, int ParameterIdx, int Offset>
struct Make1stOrderPerturbations<std::integer_sequence<int, N, Ns...>,
ParameterIdx,
Offset> {
template <typename T, typename JetT>
inline static void Apply(T const* const* parameters, JetT* x) {
Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply(
parameters[ParameterIdx], x + Offset);
Make1stOrderPerturbations<std::integer_sequence<int, Ns...>,
ParameterIdx + 1,
Offset + N>::Apply(parameters, x);
}
};
// End of 'recursion'. Nothing more to do.
template <int ParameterIdx, int Total>
struct Make1stOrderPerturbations<std::integer_sequence<int>,
ParameterIdx,
Total> {
template <typename T, typename JetT>
static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {}
};
// 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 <int N0, int N, typename JetT, typename T>
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);
}
}
// Calls Take1stOrderPart for every parameter block.
//
// Example:
// If one having three parameter blocks with dimensions (3, 2, 4), the call
// Take1stOrderParts<integer_sequence<3, 2, 4>::Apply(num_outputs,
// output,
// jacobians);
// will result in the following calls to Take1stOrderPart:
// if (jacobians[0]) {
// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]);
// }
// if (jacobians[1]) {
// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]);
// }
// if (jacobians[2]) {
// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]);
// }
template <typename Seq, int ParameterIdx = 0, int Offset = 0>
struct Take1stOrderParts;
template <int N, int... Ns, int ParameterIdx, int Offset>
struct Take1stOrderParts<std::integer_sequence<int, N, Ns...>,
ParameterIdx,
Offset> {
template <typename JetT, typename T>
inline static void Apply(int num_outputs, JetT* output, T** jacobians) {
if (jacobians[ParameterIdx]) {
Take1stOrderPart<Offset, N>(num_outputs, output, jacobians[ParameterIdx]);
}
Take1stOrderParts<std::integer_sequence<int, Ns...>,
ParameterIdx + 1,
Offset + N>::Apply(num_outputs, output, jacobians);
}
};
// End of 'recursion'. Nothing more to do.
template <int ParameterIdx, int Offset>
struct Take1stOrderParts<std::integer_sequence<int>, ParameterIdx, Offset> {
template <typename T, typename JetT>
static void Apply(int /* NOT USED*/,
JetT* /* NOT USED*/,
T** /* NOT USED */) {}
};
template <int kNumResiduals,
typename ParameterDims,
typename Functor,
typename T>
inline bool AutoDifferentiate(const Functor& functor,
T const* const* parameters,
int dynamic_num_outputs,
T* function_value,
T** jacobians) {
using JetT = Jet<T, ParameterDims::kNumParameters>;
using Parameters = typename ParameterDims::Parameters;
if (kNumResiduals != DYNAMIC) {
DCHECK_EQ(kNumResiduals, dynamic_num_outputs);
}
ArraySelector<JetT,
ParameterDims::kNumParameters,
CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK>
parameters_as_jets(ParameterDims::kNumParameters);
// Pointers to the beginning of each parameter block
std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters =
ParameterDims::GetUnpackedParameters(parameters_as_jets.data());
// If the number of residuals is fixed, we use the template argument as the
// number of outputs. Otherwise we use the num_outputs parameter. Note: The
// ?-operator here is compile-time evaluated, therefore num_outputs is also
// a compile-time constant for functors with fixed residuals.
const int num_outputs =
kNumResiduals == DYNAMIC ? dynamic_num_outputs : kNumResiduals;
DCHECK_GT(num_outputs, 0);
ArraySelector<JetT, kNumResiduals, CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK>
residuals_as_jets(num_outputs);
// 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) {
residuals_as_jets[i].a = kImpossibleValue;
residuals_as_jets[i].v.setConstant(kImpossibleValue);
}
Make1stOrderPerturbations<Parameters>::Apply(parameters,
parameters_as_jets.data());
if (!VariadicEvaluate<ParameterDims>(
functor, unpacked_parameters.data(), residuals_as_jets.data())) {
return false;
}
Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value);
Take1stOrderParts<Parameters>::Apply(
num_outputs, residuals_as_jets.data(), jacobians);
return true;
}
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_