|  | @@ -35,6 +35,8 @@
 | 
	
		
			
				|  |  |  #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"
 | 
	
	
		
			
				|  | @@ -45,6 +47,7 @@ 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>
 | 
	
	
		
			
				|  | @@ -68,6 +71,37 @@ namespace ceres {
 | 
	
		
			
				|  |  |  //   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,
 | 
	
	
		
			
				|  | @@ -75,8 +109,10 @@ template<typename CostFunctor,
 | 
	
		
			
				|  |  |           typename T = double>
 | 
	
		
			
				|  |  |  class TinySolverAutoDiffFunction {
 | 
	
		
			
				|  |  |   public:
 | 
	
		
			
				|  |  | -   TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
 | 
	
		
			
				|  |  | -     : cost_functor_(cost_functor) {}
 | 
	
		
			
				|  |  | +  TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
 | 
	
		
			
				|  |  | +      : cost_functor_(cost_functor) {
 | 
	
		
			
				|  |  | +    Initialize<kNumResiduals>(cost_functor);
 | 
	
		
			
				|  |  | +  }
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |    typedef T Scalar;
 | 
	
		
			
				|  |  |    enum {
 | 
	
	
		
			
				|  | @@ -102,21 +138,22 @@ class TinySolverAutoDiffFunction {
 | 
	
		
			
				|  |  |      }
 | 
	
		
			
				|  |  |  
 | 
	
		
			
				|  |  |      // Initialize the output jets such that we can detect user errors.
 | 
	
		
			
				|  |  | -    for (int i = 0; i < kNumResiduals; ++i) {
 | 
	
		
			
				|  |  | +    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_)) {
 | 
	
		
			
				|  |  | +    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);
 | 
	
		
			
				|  |  | -    for (int r = 0; r < kNumResiduals; ++r) {
 | 
	
		
			
				|  |  | +    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
 | 
	
	
		
			
				|  | @@ -126,16 +163,42 @@ class TinySolverAutoDiffFunction {
 | 
	
		
			
				|  |  |      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.
 | 
	
		
			
				|  |  | -  mutable Jet<T, kNumParameters> jet_parameters_[kNumParameters];
 | 
	
		
			
				|  |  | -  mutable Jet<T, kNumParameters> jet_residuals_[kNumResiduals];
 | 
	
		
			
				|  |  | +  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
 |