| // 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: sergey.vfx@gmail.com (Sergey Sharybin) | |
| // mierle@gmail.com (Keir Mierle) | |
| // sameeragarwal@google.com (Sameer Agarwal) | |
| namespace ceres { | |
| // WARNING: LocalParameterizations are deprecated, so is | |
| // AutoDiffLocalParameterization. They will be removed from Ceres Solver in | |
| // version 2.2.0. Please use Manifolds and AutoDiffManifold instead. | |
| // Create local parameterization with Jacobians computed via automatic | |
| // differentiation. For more information on local parameterizations, | |
| // see include/ceres/local_parameterization.h | |
| // | |
| // To get an auto differentiated local parameterization, you must define | |
| // a class with a templated operator() (a functor) that computes | |
| // | |
| // x_plus_delta = Plus(x, delta); | |
| // | |
| // the template parameter T. The autodiff framework substitutes appropriate | |
| // "Jet" objects for T in order to compute the derivative when necessary, but | |
| // this is hidden, and you should write the function as if T were a scalar type | |
| // (e.g. a double-precision floating point number). | |
| // | |
| // The function must write the computed value in the last argument (the only | |
| // non-const one) and return true to indicate success. | |
| // | |
| // For example, Quaternions have a three dimensional local | |
| // parameterization. It's plus operation can be implemented as (taken | |
| // from internal/ceres/auto_diff_local_parameterization_test.cc) | |
| // | |
| // struct QuaternionPlus { | |
| // template<typename T> | |
| // bool operator()(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; | |
| // } | |
| // }; | |
| // | |
| // Then given this struct, the auto differentiated local | |
| // parameterization can now be constructed as | |
| // | |
| // LocalParameterization* local_parameterization = | |
| // new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; | |
| // | | | |
| // Global Size ---------------+ | | |
| // Local Size -------------------+ | |
| // | |
| // WARNING: Since the functor will get instantiated with different types for | |
| // T, you must to convert from other numeric types to T before mixing | |
| // computations with other variables of type T. In the example above, this is | |
| // seen where instead of using k_ directly, k_ is wrapped with T(k_). | |
| template <typename Functor, int kGlobalSize, int kLocalSize> | |
| class CERES_DEPRECATED_WITH_MSG("Use AutoDiffManifold instead.") | |
| AutoDiffLocalParameterization : public LocalParameterization { | |
| public: | |
| AutoDiffLocalParameterization() : functor_(new Functor()) {} | |
| // Takes ownership of functor. | |
| explicit AutoDiffLocalParameterization(Functor* functor) | |
| : functor_(functor) {} | |
| bool Plus(const double* x, | |
| const double* delta, | |
| double* x_plus_delta) const override { | |
| return (*functor_)(x, delta, x_plus_delta); | |
| } | |
| bool ComputeJacobian(const double* x, double* jacobian) const override { | |
| double zero_delta[kLocalSize]; | |
| for (int i = 0; i < kLocalSize; ++i) { | |
| zero_delta[i] = 0.0; | |
| } | |
| double x_plus_delta[kGlobalSize]; | |
| for (int i = 0; i < kGlobalSize; ++i) { | |
| x_plus_delta[i] = 0.0; | |
| } | |
| const double* parameter_ptrs[2] = {x, zero_delta}; | |
| double* jacobian_ptrs[2] = {nullptr, jacobian}; | |
| return internal::AutoDifferentiate< | |
| kGlobalSize, | |
| internal::StaticParameterDims<kGlobalSize, kLocalSize>>( | |
| *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); | |
| } | |
| int GlobalSize() const override { return kGlobalSize; } | |
| int LocalSize() const override { return kLocalSize; } | |
| const Functor& functor() const { return *functor_; } | |
| private: | |
| std::unique_ptr<Functor> functor_; | |
| }; | |
| } // namespace ceres | |