text: // 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: mierle@gmail.com (Keir Mierle)
  //
  // WARNING WARNING WARNING
  // WARNING WARNING WARNING  Tiny solver is experimental and will change.
  // WARNING WARNING WARNING
  
  #ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
  #define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
  
  #include <memory>
  #include <type_traits>
  
  #include "Eigen/Core"
  #include "ceres/jet.h"
  #include "ceres/types.h"  // For kImpossibleValue.
  
  namespace ceres {
  
  // An adapter around autodiff-style CostFunctors to enable easier use of
  // TinySolver. See the example below showing how to use it:
  //
  //   // Example for cost functor with static residual size.
  //   // Same as an autodiff cost functor, but taking only 1 parameter.
  //   struct MyFunctor {
  //     template<typename T>
  //     bool operator()(const T* const parameters, T* residuals) const {
  //       const T& x = parameters[0];
  //       const T& y = parameters[1];
  //       const T& z = parameters[2];
  //       residuals[0] = x + 2.*y + 4.*z;
  //       residuals[1] = y * z;
  //       return true;
  //     }
  //   };
  //
  //   typedef TinySolverAutoDiffFunction<MyFunctor, 2, 3>
  //       AutoDiffFunction;
  //
  //   MyFunctor my_functor;
  //   AutoDiffFunction f(my_functor);
  //
  //   Vec3 x = ...;
  //   TinySolver<AutoDiffFunction> solver;
  //   solver.Solve(f, &x);
  //
  //   // Example for cost functor with dynamic residual size.
  //   // NumResiduals() supplies dynamic size of residuals.
  //   // Same functionality as in tiny_solver.h but with autodiff.
  //   struct MyFunctorWithDynamicResiduals {
  //     int NumResiduals() const {
  //       return 2;
  //     }
  //
  //     template<typename T>
  //     bool operator()(const T* const parameters, T* residuals) const {
  //       const T& x = parameters[0];
  //       const T& y = parameters[1];
  //       const T& z = parameters[2];
  //       residuals[0] = x + static_cast<T>(2.)*y + static_cast<T>(4.)*z;
  //       residuals[1] = y * z;
  //       return true;
  //     }
  //   };
  //
  //   typedef TinySolverAutoDiffFunction<MyFunctorWithDynamicResiduals,
  //                                      Eigen::Dynamic,
  //                                      3>
  //       AutoDiffFunctionWithDynamicResiduals;
  //
  //   MyFunctorWithDynamicResiduals my_functor_dyn;
  //   AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn);
  //
  //   Vec3 x = ...;
  //   TinySolver<AutoDiffFunctionWithDynamicResiduals> solver;
  //   solver.Solve(f, &x);
  //
  // WARNING: The cost function adapter is not thread safe.
  template <typename CostFunctor,
            int kNumResiduals,
            int kNumParameters,
            typename T = double>
  class TinySolverAutoDiffFunction {
   public:
    // This class needs to have an Eigen aligned operator new as it contains
    // as a member a Jet type, which itself has a fixed-size Eigen type as member.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
    TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
        : cost_functor_(cost_functor) {
      Initialize<kNumResiduals>(cost_functor);
    }
  
    typedef T Scalar;
    enum {
      NUM_PARAMETERS = kNumParameters,
      NUM_RESIDUALS = kNumResiduals,
    };
  
    // This is similar to AutoDifferentiate(), but since there is only one
    // parameter block it is easier to inline to avoid overhead.
    bool operator()(const T* parameters, T* residuals, T* jacobian) const {
      if (jacobian == NULL) {
        // No jacobian requested, so just directly call the cost function with
        // doubles, skipping jets and derivatives.
        return cost_functor_(parameters, residuals);
      }
      // Initialize the input jets with passed parameters.
      for (int i = 0; i < kNumParameters; ++i) {
        jet_parameters_[i].a = parameters[i];  // Scalar part.
        jet_parameters_[i].v.setZero();        // Derivative part.
        jet_parameters_[i].v[i] = T(1.0);
      }
  
      // Initialize the output jets such that we can detect user errors.
      for (int i = 0; i < num_residuals_; ++i) {
        jet_residuals_[i].a = kImpossibleValue;
        jet_residuals_[i].v.setConstant(kImpossibleValue);
      }
  
      // Execute the cost function, but with jets to find the derivative.
      if (!cost_functor_(jet_parameters_, jet_residuals_.data())) {
        return false;
      }
  
      // Copy the jacobian out of the derivative part of the residual jets.
      Eigen::Map<Eigen::Matrix<T, kNumResiduals, kNumParameters>> jacobian_matrix(
          jacobian, num_residuals_, kNumParameters);
      for (int r = 0; r < num_residuals_; ++r) {
        residuals[r] = jet_residuals_[r].a;
        // Note that while this looks like a fast vectorized write, in practice it
        // unfortunately thrashes the cache since the writes to the column-major
        // jacobian are strided (e.g. rows are non-contiguous).
        jacobian_matrix.row(r) = jet_residuals_[r].v;
      }
      return true;
    }
  
    int NumResiduals() const {
      return num_residuals_;  // Set by Initialize.
    }
  
   private:
    const CostFunctor& cost_functor_;
  
    // The number of residuals at runtime.
    // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic.
    int num_residuals_ = kNumResiduals;
  
    // To evaluate the cost function with jets, temporary storage is needed. These
    // are the buffers that are used during evaluation; parameters for the input,
    // and jet_residuals_ are where the final cost and derivatives end up.
    //
    // Since this buffer is used for evaluation, the adapter is not thread safe.
    using JetType = Jet<T, kNumParameters>;
    mutable JetType jet_parameters_[kNumParameters];
    // Eigen::Matrix serves as static or dynamic container.
    mutable Eigen::Matrix<JetType, kNumResiduals, 1> jet_residuals_;
  
    // The number of residuals is dynamically sized and the number of
    // parameters is statically sized.
    template <int R>
    typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize(
        const CostFunctor& function) {
      jet_residuals_.resize(function.NumResiduals());
      num_residuals_ = function.NumResiduals();
    }
  
    // The number of parameters and residuals are statically sized.
    template <int R>
    typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize(
        const CostFunctor& /* function */) {
      num_residuals_ = kNumResiduals;
    }
  };
  
  }  // namespace ceres
  
  #endif  // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_
  