diff options
Diffstat (limited to 'extern/ceres/include/ceres/autodiff_manifold.h')
| -rw-r--r-- | extern/ceres/include/ceres/autodiff_manifold.h | 259 |
1 files changed, 259 insertions, 0 deletions
diff --git a/extern/ceres/include/ceres/autodiff_manifold.h b/extern/ceres/include/ceres/autodiff_manifold.h new file mode 100644 index 00000000000..3063e19e802 --- /dev/null +++ b/extern/ceres/include/ceres/autodiff_manifold.h @@ -0,0 +1,259 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ +#define CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ + +#include <memory> + +#include "ceres/internal/autodiff.h" +#include "ceres/manifold.h" + +namespace ceres { + +// Create a Manifold with Jacobians computed via automatic differentiation. For +// more information on manifolds, see include/ceres/manifold.h +// +// To get an auto differentiated manifold, you must define a class/struct with +// templated Plus and Minus functions that compute +// +// x_plus_delta = Plus(x, delta); +// y_minus_x = Minus(y, x); +// +// Where, x, y and x_plus_y are vectors on the manifold in the ambient space (so +// they are kAmbientSize vectors) and delta, y_minus_x are vectors in the +// tangent space (so they are kTangentSize vectors). +// +// The Functor should have the signature: +// +// struct Functor { +// template <typename T> +// bool Plus(const T* x, const T* delta, T* x_plus_delta) const; +// +// template <typename T> +// bool Minus(const T* y, const T* x, T* y_minus_x) const; +// }; +// +// Observe that the Plus and Minus operations are templated on the parameter T. +// The autodiff framework substitutes appropriate "Jet" objects for T in order +// to compute the derivative when necessary. This is the same mechanism that is +// used to compute derivatives when using AutoDiffCostFunction. +// +// Plus and Minus should return true if the computation is successful and false +// otherwise, in which case the result will not be used. +// +// Given this Functor, the corresponding Manifold can be constructed as: +// +// AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold; +// +// As a concrete example consider the case of Quaternions. Quaternions form a +// three dimensional manifold embedded in R^4, i.e. they have an ambient +// dimension of 4 and their tangent space has dimension 3. The following Functor +// (taken from autodiff_manifold_test.cc) defines the Plus and Minus operations +// on the Quaternion manifold: +// +// NOTE: The following is only used for illustration purposes. Ceres Solver +// ships with optimized production grade QuaternionManifold implementation. See +// manifold.h. +// +// This functor assumes that the quaternions are laid out as [w,x,y,z] in +// memory, i.e. the real or scalar part is the first coordinate. +// +// struct QuaternionFunctor { +// template <typename T> +// bool Plus(const T* x, const T* delta, T* x_plus_delta) const { +// const T squared_norm_delta = +// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; +// +// T q_delta[4]; +// if (squared_norm_delta > T(0.0)) { +// T norm_delta = sqrt(squared_norm_delta); +// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; +// q_delta[0] = cos(norm_delta); +// q_delta[1] = sin_delta_by_delta * delta[0]; +// q_delta[2] = sin_delta_by_delta * delta[1]; +// q_delta[3] = sin_delta_by_delta * delta[2]; +// } else { +// // We do not just use q_delta = [1,0,0,0] here because that is a +// // constant and when used for automatic differentiation will +// // lead to a zero derivative. Instead we take a first order +// // approximation and evaluate it at zero. +// q_delta[0] = T(1.0); +// q_delta[1] = delta[0]; +// q_delta[2] = delta[1]; +// q_delta[3] = delta[2]; +// } +// +// QuaternionProduct(q_delta, x, x_plus_delta); +// return true; +// } +// +// template <typename T> +// bool Minus(const T* y, const T* x, T* y_minus_x) const { +// T minus_x[4] = {x[0], -x[1], -x[2], -x[3]}; +// T ambient_y_minus_x[4]; +// QuaternionProduct(y, minus_x, ambient_y_minus_x); +// T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] + +// ambient_y_minus_x[2] * ambient_y_minus_x[2] + +// ambient_y_minus_x[3] * ambient_y_minus_x[3]); +// if (u_norm > 0.0) { +// T theta = atan2(u_norm, ambient_y_minus_x[0]); +// y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm; +// y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm; +// y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm; +// } else { +// // We do not use [0,0,0] here because even though the value part is +// // a constant, the derivative part is not. +// y_minus_x[0] = ambient_y_minus_x[1]; +// y_minus_x[1] = ambient_y_minus_x[2]; +// y_minus_x[2] = ambient_y_minus_x[3]; +// } +// return true; +// } +// }; +// +// Then given this struct, the auto differentiated Quaternion Manifold can now +// be constructed as +// +// Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>; + +template <typename Functor, int kAmbientSize, int kTangentSize> +class AutoDiffManifold final : public Manifold { + public: + AutoDiffManifold() : functor_(std::make_unique<Functor>()) {} + + // Takes ownership of functor. + explicit AutoDiffManifold(Functor* functor) : functor_(functor) {} + + int AmbientSize() const override { return kAmbientSize; } + int TangentSize() const override { return kTangentSize; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return functor_->Plus(x, delta, x_plus_delta); + } + + bool PlusJacobian(const double* x, double* jacobian) const override; + + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override { + return functor_->Minus(y, x, y_minus_x); + } + + bool MinusJacobian(const double* x, double* jacobian) const override; + + const Functor& functor() const { return *functor_; } + + private: + std::unique_ptr<Functor> functor_; +}; + +namespace internal { + +// The following two helper structs are needed to interface the Plus and Minus +// methods of the ManifoldFunctor with the automatic differentiation which +// expects a Functor with operator(). +template <typename Functor> +struct PlusWrapper { + explicit PlusWrapper(const Functor& functor) : functor(functor) {} + template <typename T> + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + return functor.Plus(x, delta, x_plus_delta); + } + const Functor& functor; +}; + +template <typename Functor> +struct MinusWrapper { + explicit MinusWrapper(const Functor& functor) : functor(functor) {} + template <typename T> + bool operator()(const T* y, const T* x, T* y_minus_x) const { + return functor.Minus(y, x, y_minus_x); + } + const Functor& functor; +}; +} // namespace internal + +template <typename Functor, int kAmbientSize, int kTangentSize> +bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::PlusJacobian( + const double* x, double* jacobian) const { + double zero_delta[kTangentSize]; + for (int i = 0; i < kTangentSize; ++i) { + zero_delta[i] = 0.0; + } + + double x_plus_delta[kAmbientSize]; + for (int i = 0; i < kAmbientSize; ++i) { + x_plus_delta[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, zero_delta}; + + // PlusJacobian is D_2 Plus(x,0) so we only need to compute the Jacobian + // w.r.t. the second argument. + double* jacobian_ptrs[2] = {nullptr, jacobian}; + return internal::AutoDifferentiate< + kAmbientSize, + internal::StaticParameterDims<kAmbientSize, kTangentSize>>( + internal::PlusWrapper<Functor>(*functor_), + parameter_ptrs, + kAmbientSize, + x_plus_delta, + jacobian_ptrs); +} + +template <typename Functor, int kAmbientSize, int kTangentSize> +bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::MinusJacobian( + const double* x, double* jacobian) const { + double y_minus_x[kTangentSize]; + for (int i = 0; i < kTangentSize; ++i) { + y_minus_x[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, x}; + + // MinusJacobian is D_1 Minus(x,x), so we only need to compute the Jacobian + // w.r.t. the first argument. + double* jacobian_ptrs[2] = {jacobian, nullptr}; + return internal::AutoDifferentiate< + kTangentSize, + internal::StaticParameterDims<kAmbientSize, kAmbientSize>>( + internal::MinusWrapper<Functor>(*functor_), + parameter_ptrs, + kTangentSize, + y_minus_x, + jacobian_ptrs); +} + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ |