diff options
Diffstat (limited to 'extern/ceres/include/ceres/internal/autodiff.h')
| -rw-r--r-- | extern/ceres/include/ceres/internal/autodiff.h | 291 |
1 files changed, 169 insertions, 122 deletions
diff --git a/extern/ceres/include/ceres/internal/autodiff.h b/extern/ceres/include/ceres/internal/autodiff.h index 136152a36cd..cb7b1aca5b9 100644 --- a/extern/ceres/include/ceres/internal/autodiff.h +++ b/extern/ceres/include/ceres/internal/autodiff.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -30,10 +30,10 @@ // // 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 +// 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 AutoDiff::Differentiate() computes the jacobian of +// The helper wrapper AutoDifferentiate() computes the jacobian of // functors with templated operator() taking this form: // // struct F { @@ -57,7 +57,7 @@ // [ * ] // // 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 +// 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>. // @@ -142,16 +142,33 @@ #include <stddef.h> -#include "ceres/jet.h" +#include <array> +#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 pertubation for every dimension and puts it in +// 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 @@ -165,21 +182,62 @@ namespace internal { // // 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); +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) { +inline void Take0thOrderPart(int M, const JetT* src, T dst) { DCHECK(src); for (int i = 0; i < M; ++i) { dst[i] = src[i].a; @@ -188,128 +246,117 @@ inline void Take0thOrderPart(int M, const JetT *src, T dst) { // 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) { +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) = + 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; +// 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; - 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); +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); + } +}; - // 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; +// 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 */) {} +}; - 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, - }; +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) { + typedef Jet<T, ParameterDims::kNumParameters> JetT; + using Parameters = typename ParameterDims::Parameters; - JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; + if (kNumResiduals != DYNAMIC) { + DCHECK_EQ(kNumResiduals, dynamic_num_outputs); + } -#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 + ArraySelector<JetT, + ParameterDims::kNumParameters, + CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK> + parameters_as_jets(ParameterDims::kNumParameters); - if (!VariadicEvaluate<Functor, JetT, - N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( - functor, unpacked_parameters, output)) { - return false; - } + // Pointers to the beginning of each parameter block + std::array<JetT*, ParameterDims::kNumParameterBlocks> unpacked_parameters = + ParameterDims::GetUnpackedParameters(parameters_as_jets.data()); - internal::Take0thOrderPart(num_outputs, output, function_value); + // 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); -#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; + 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 |