// Ceres Solver - A fast non-linear least squares minimizer

// Copyright 2015 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)

//         dgossow@google.com (David Gossow)

#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_

#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_

#include <numeric>

#include <vector>

#include "ceres/dynamic_cost_function.h"

#include "ceres/internal/fixed_array.h"

#include "ceres/internal/port.h"

#include "ceres/internal/scoped_ptr.h"

namespace ceres {

// DynamicCostFunctionToFunctor allows users to use CostFunction

// objects in templated functors which are to be used for automatic

// differentiation. It works similar to CostFunctionToFunctor, with the

// difference that it allows you to wrap a cost function with dynamic numbers

// of parameters and residuals.

//

// For example, let us assume that

//

//  class IntrinsicProjection : public CostFunction {

//    public:

//      IntrinsicProjection(const double* observation);

//      virtual bool Evaluate(double const* const* parameters,

//                            double* residuals,

//                            double** jacobians) const;

//  };

//

// is a cost function that implements the projection of a point in its

// local coordinate system onto its image plane and subtracts it from

// the observed point projection. It can compute its residual and

// either via analytic or numerical differentiation can compute its

// jacobians. The intrinsics are passed in as parameters[0] and the point as

// parameters[1].

//

// Now we would like to compose the action of this CostFunction with

// the action of camera extrinsics, i.e., rotation and

// translation. Say we have a templated function

//

//   template<typename T>

//   void RotateAndTranslatePoint(double const* const* parameters,

//                                double* residuals);

//

// Then we can now do the following,

//

// struct CameraProjection {

//   CameraProjection(const double* observation)

//       : intrinsic_projection_.(new IntrinsicProjection(observation)) {

//   }

//   template <typename T>

//   bool operator()(T const* const* parameters,

//                   T* residual) const {

//     const T* rotation = parameters[0];

//     const T* translation = parameters[1];

//     const T* intrinsics = parameters[2];

//     const T* point = parameters[3];

//     T transformed_point[3];

//     RotateAndTranslatePoint(rotation, translation, point, transformed_point);

//

//     // Note that we call intrinsic_projection_, just like it was

//     // any other templated functor.

//     const T* projection_parameters[2];

//     projection_parameters[0] = intrinsics;

//     projection_parameters[1] = transformed_point;

//     return intrinsic_projection_(projection_parameters, residual);

//   }

//

//  private:

//   DynamicCostFunctionToFunctor intrinsic_projection_;

// };

class DynamicCostFunctionToFunctor {

 public:

  // Takes ownership of cost_function.

  explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)

      : cost_function_(cost_function) {

    CHECK_NOTNULL(cost_function);

  }

  bool operator()(double const* const* parameters, double* residuals) const {

    return cost_function_->Evaluate(parameters, residuals, NULL);

  }

  template <typename JetT>

  bool operator()(JetT const* const* inputs, JetT* output) const {

    const std::vector<int32>& parameter_block_sizes =

        cost_function_->parameter_block_sizes();

    const int num_parameter_blocks =

        static_cast<int>(parameter_block_sizes.size());

    const int num_residuals = cost_function_->num_residuals();

    const int num_parameters = std::accumulate(parameter_block_sizes.begin(),

                                               parameter_block_sizes.end(), 0);

    internal::FixedArray<double> parameters(num_parameters);

    internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);

    internal::FixedArray<double> jacobians(num_residuals * num_parameters);

    internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);

    internal::FixedArray<double> residuals(num_residuals);

    // Build a set of arrays to get the residuals and jacobians from

    // the CostFunction wrapped by this functor.

    double* parameter_ptr = parameters.get();

    double* jacobian_ptr = jacobians.get();

    for (int i = 0; i < num_parameter_blocks; ++i) {

      parameter_blocks[i] = parameter_ptr;

      jacobian_blocks[i] = jacobian_ptr;

      for (int j = 0; j < parameter_block_sizes[i]; ++j) {

        *parameter_ptr++ = inputs[i][j].a;

      }

      jacobian_ptr += num_residuals * parameter_block_sizes[i];

    }

    if (!cost_function_->Evaluate(parameter_blocks.get(),

                                  residuals.get(),

                                  jacobian_blocks.get())) {

      return false;

    }

    // Now that we have the incoming Jets, which are carrying the

    // partial derivatives of each of the inputs w.r.t to some other

    // underlying parameters. The derivative of the outputs of the

    // cost function w.r.t to the same underlying parameters can now

    // be computed by applying the chain rule.

    //

    //  d output[i]               d output[i]   d input[j]

    //  --------------  = sum_j   ----------- * ------------

    //  d parameter[k]            d input[j]    d parameter[k]

    //

    // d input[j]

    // --------------  = inputs[j], so

    // d parameter[k]

    //

    //  outputJet[i]  = sum_k jacobian[i][k] * inputJet[k]

    //

    // The following loop, iterates over the residuals, computing one

    // output jet at a time.

    for (int i = 0; i < num_residuals; ++i) {

      output[i].a = residuals[i];

      output[i].v.setZero();

      for (int j = 0; j < num_parameter_blocks; ++j) {

        const int32 block_size = parameter_block_sizes[j];

        for (int k = 0; k < parameter_block_sizes[j]; ++k) {

          output[i].v +=

              jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;

        }

      }

    }

    return true;

  }

 private:

  internal::scoped_ptr<CostFunction> cost_function_;

};

}  // namespace ceres

#endif  // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_