env/ceres-solver/include/ceres/tiny_solver_autodiff_function.h

// 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

  explicit TinySolverAutoDiffFunction(const CostFunctor& cost_functor)
      : cost_functor_(cost_functor) {
    Initialize<kNumResiduals>(cost_functor);
  }

  using Scalar = T;
  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 == nullptr) {
      // 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 overridden 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_