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

#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_

#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_

#include "ceres/internal/port.h"

#include "Eigen/Core"

#include "glog/logging.h"

namespace ceres {

// Given samples from a function sampled at four equally spaced points,

//

//   p0 = f(-1)

//   p1 = f(0)

//   p2 = f(1)

//   p3 = f(2)

//

// Evaluate the cubic Hermite spline (also known as the Catmull-Rom

// spline) at a point x that lies in the interval [0, 1].

//

// This is also the interpolation kernel (for the case of a = 0.5) as

// proposed by R. Keys, in:

//

// "Cubic convolution interpolation for digital image processing".

// IEEE Transactions on Acoustics, Speech, and Signal Processing

// 29 (6): 1153–1160.

//

// For more details see

//

// http://en.wikipedia.org/wiki/Cubic_Hermite_spline

// http://en.wikipedia.org/wiki/Bicubic_interpolation

//

// f if not NULL will contain the interpolated function values.

// dfdx if not NULL will contain the interpolated derivative values.

template <int kDataDimension>

void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0,

                        const Eigen::Matrix<double, kDataDimension, 1>& p1,

                        const Eigen::Matrix<double, kDataDimension, 1>& p2,

                        const Eigen::Matrix<double, kDataDimension, 1>& p3,

                        const double x,

                        double* f,

                        double* dfdx) {

  typedef Eigen::Matrix<double, kDataDimension, 1> VType;

  const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3);

  const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3);

  const VType c = 0.5 * (-p0 + p2);

  const VType d = p1;

  // Use Horner's rule to evaluate the function value and its

  // derivative.

  // f = ax^3 + bx^2 + cx + d

  if (f != NULL) {

    Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));

  }

  // dfdx = 3ax^2 + 2bx + c

  if (dfdx != NULL) {

    Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);

  }

}

// Given as input an infinite one dimensional grid, which provides the

// following interface.

//

//   class Grid {

//    public:

//     enum { DATA_DIMENSION = 2; };

//     void GetValue(int n, double* f) const;

//   };

//

// Here, GetValue gives the value of a function f (possibly vector

// valued) for any integer n.

//

// The enum DATA_DIMENSION indicates the dimensionality of the

// function being interpolated. For example if you are interpolating

// rotations in axis-angle format over time, then DATA_DIMENSION = 3.

//

// CubicInterpolator uses cubic Hermite splines to produce a smooth

// approximation to it that can be used to evaluate the f(x) and f'(x)

// at any point on the real number line.

//

// For more details on cubic interpolation see

//

// http://en.wikipedia.org/wiki/Cubic_Hermite_spline

//

// Example usage:

//

//  const double data[] = {1.0, 2.0, 5.0, 6.0};

//  Grid1D<double, 1> grid(x, 0, 4);

//  CubicInterpolator<Grid1D<double, 1> > interpolator(grid);

//  double f, dfdx;

//  interpolator.Evaluator(1.5, &f, &dfdx);

template<typename Grid>

class CubicInterpolator {

 public:

  explicit CubicInterpolator(const Grid& grid)

      : grid_(grid) {

    // The + casts the enum into an int before doing the

    // comparison. It is needed to prevent

    // "-Wunnamed-type-template-args" related errors.

    CHECK_GE(+Grid::DATA_DIMENSION, 1);

  }

  void Evaluate(double x, double* f, double* dfdx) const {

    const int n = std::floor(x);

    Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;

    grid_.GetValue(n - 1, p0.data());

    grid_.GetValue(n,     p1.data());

    grid_.GetValue(n + 1, p2.data());

    grid_.GetValue(n + 2, p3.data());

    CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);

  }

  // The following two Evaluate overloads are needed for interfacing

  // with automatic differentiation. The first is for when a scalar

  // evaluation is done, and the second one is for when Jets are used.

  void Evaluate(const double& x, double* f) const {

    Evaluate(x, f, NULL);

  }

  template<typename JetT> void Evaluate(const JetT& x, JetT* f) const {

    double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION];

    Evaluate(x.a, fx, dfdx);

    for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {

      f[i].a = fx[i];

      f[i].v = dfdx[i] * x.v;

    }

  }

 private:

  const Grid& grid_;

};

// An object that implements an infinite one dimensional grid needed

// by the CubicInterpolator where the source of the function values is

// an array of type T on the interval

//

//   [begin, ..., end - 1]

//

// Since the input array is finite and the grid is infinite, values

// outside this interval needs to be computed. Grid1D uses the value

// from the nearest edge.

//

// The function being provided can be vector valued, in which case

// kDataDimension > 1. The dimensional slices of the function maybe

// interleaved, or they maybe stacked, i.e, if the function has

// kDataDimension = 2, if kInterleaved = true, then it is stored as

//

//   f01, f02, f11, f12 ....

//

// and if kInterleaved = false, then it is stored as

//

//  f01, f11, .. fn1, f02, f12, .. , fn2

//

template 

          int kDataDimension = 1,

          bool kInterleaved = true>

struct Grid1D {

 public:

  enum { DATA_DIMENSION = kDataDimension };

  Grid1D(const T* data, const int begin, const int end)

      : data_(data), begin_(begin), end_(end), num_values_(end - begin) {

    CHECK_LT(begin, end);

  }

  EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {

    const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;

    if (kInterleaved) {

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

        f[i] = static_cast<double>(data_[kDataDimension * idx + i]);

      }

    } else {

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

        f[i] = static_cast<double>(data_[i * num_values_ + idx]);

      }

    }

  }

 private:

  const T* data_;

  const int begin_;

  const int end_;

  const int num_values_;

};

// Given as input an infinite two dimensional grid like object, which

// provides the following interface:

//

//   struct Grid {

//     enum { DATA_DIMENSION = 1 };

//     void GetValue(int row, int col, double* f) const;

//   };

//

// Where, GetValue gives us the value of a function f (possibly vector

// valued) for any pairs of integers (row, col), and the enum

// DATA_DIMENSION indicates the dimensionality of the function being

// interpolated. For example if you are interpolating a color image

// with three channels (Red, Green & Blue), then DATA_DIMENSION = 3.

//

// BiCubicInterpolator uses the cubic convolution interpolation

// algorithm of R. Keys, to produce a smooth approximation to it that

// can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at

// any point in the real plane.

//

// For more details on the algorithm used here see:

//

// "Cubic convolution interpolation for digital image processing".

// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal

// Processing 29 (6): 1153–1160, 1981.

//

// http://en.wikipedia.org/wiki/Cubic_Hermite_spline

// http://en.wikipedia.org/wiki/Bicubic_interpolation

//

// Example usage:

//

// const double data[] = {1.0, 3.0, -1.0, 4.0,

//                         3.6, 2.1,  4.2, 2.0,

//                        2.0, 1.0,  3.1, 5.2};

//  Grid2D<double, 1>  grid(data, 3, 4);

//  BiCubicInterpolator<Grid2D<double, 1> > interpolator(grid);

//  double f, dfdr, dfdc;

//  interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);

template<typename Grid>

class BiCubicInterpolator {

 public:

  explicit BiCubicInterpolator(const Grid& grid)

      : grid_(grid) {

    // The + casts the enum into an int before doing the

    // comparison. It is needed to prevent

    // "-Wunnamed-type-template-args" related errors.

    CHECK_GE(+Grid::DATA_DIMENSION, 1);

  }

  // Evaluate the interpolated function value and/or its

  // derivative. Returns false if r or c is out of bounds.

  void Evaluate(double r, double c,

                double* f, double* dfdr, double* dfdc) const {

    // BiCubic interpolation requires 16 values around the point being

    // evaluated.  We will use pij, to indicate the elements of the

    // 4x4 grid of values.

    //

    //          col

    //      p00 p01 p02 p03

    // row  p10 p11 p12 p13

    //      p20 p21 p22 p23

    //      p30 p31 p32 p33

    //

    // The point (r,c) being evaluated is assumed to lie in the square

    // defined by p11, p12, p22 and p21.

    const int row = std::floor(r);

    const int col = std::floor(c);

    Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;

    // Interpolate along each of the four rows, evaluating the function

    // value and the horizontal derivative in each row.

    Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;

    Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;

    grid_.GetValue(row - 1, col - 1, p0.data());

    grid_.GetValue(row - 1, col    , p1.data());

    grid_.GetValue(row - 1, col + 1, p2.data());

    grid_.GetValue(row - 1, col + 2, p3.data());

    CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,

                                             f0.data(), df0dc.data());

    grid_.GetValue(row, col - 1, p0.data());

    grid_.GetValue(row, col    , p1.data());

    grid_.GetValue(row, col + 1, p2.data());

    grid_.GetValue(row, col + 2, p3.data());

    CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,

                                             f1.data(), df1dc.data());

    grid_.GetValue(row + 1, col - 1, p0.data());

    grid_.GetValue(row + 1, col    , p1.data());

    grid_.GetValue(row + 1, col + 1, p2.data());

    grid_.GetValue(row + 1, col + 2, p3.data());

    CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,

                                             f2.data(), df2dc.data());

    grid_.GetValue(row + 2, col - 1, p0.data());

    grid_.GetValue(row + 2, col    , p1.data());

    grid_.GetValue(row + 2, col + 1, p2.data());

    grid_.GetValue(row + 2, col + 2, p3.data());

    CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,

                                             f3.data(), df3dc.data());

    // Interpolate vertically the interpolated value from each row and

    // compute the derivative along the columns.

    CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);

    if (dfdc != NULL) {

      // Interpolate vertically the derivative along the columns.

      CubicHermiteSpline<Grid::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,

                                               r - row, dfdc, NULL);

    }

  }

  // The following two Evaluate overloads are needed for interfacing

  // with automatic differentiation. The first is for when a scalar

  // evaluation is done, and the second one is for when Jets are used.

  void Evaluate(const double& r, const double& c, double* f) const {

    Evaluate(r, c, f, NULL, NULL);

  }

  template<typename JetT> void Evaluate(const JetT& r,

                                        const JetT& c,

                                        JetT* f) const {

    double frc[Grid::DATA_DIMENSION];

    double dfdr[Grid::DATA_DIMENSION];

    double dfdc[Grid::DATA_DIMENSION];

    Evaluate(r.a, c.a, frc, dfdr, dfdc);

    for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {

      f[i].a = frc[i];

      f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;

    }

  }

 private:

  const Grid& grid_;

};

// An object that implements an infinite two dimensional grid needed

// by the BiCubicInterpolator where the source of the function values

// is an grid of type T on the grid

//

//   [(row_start,   col_start), ..., (row_start,   col_end - 1)]

//   [                          ...                            ]

//   [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]

//

// Since the input grid is finite and the grid is infinite, values

// outside this interval needs to be computed. Grid2D uses the value

// from the nearest edge.

//

// The function being provided can be vector valued, in which case

// kDataDimension > 1. The data maybe stored in row or column major

// format and the various dimensional slices of the function maybe

// interleaved, or they maybe stacked, i.e, if the function has

// kDataDimension = 2, is stored in row-major format and if

// kInterleaved = true, then it is stored as

//

//   f001, f002, f011, f012, ...

//

// A commonly occuring example are color images (RGB) where the three

// channels are stored interleaved.

//

// If kInterleaved = false, then it is stored as

//

//  f001, f011, ..., fnm1, f002, f012, ...

template 

          int kDataDimension = 1,

          bool kRowMajor = true,

          bool kInterleaved = true>

struct Grid2D {

 public:

  enum { DATA_DIMENSION = kDataDimension };

  Grid2D(const T* data,

         const int row_begin, const int row_end,

         const int col_begin, const int col_end)

      : data_(data),

        row_begin_(row_begin), row_end_(row_end),

        col_begin_(col_begin), col_end_(col_end),

        num_rows_(row_end - row_begin), num_cols_(col_end - col_begin),

        num_values_(num_rows_ * num_cols_) {

    CHECK_GE(kDataDimension, 1);

    CHECK_LT(row_begin, row_end);

    CHECK_LT(col_begin, col_end);

  }

  EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const {

    const int row_idx =

        std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_;

    const int col_idx =

        std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_;

    const int n =

        (kRowMajor)

        ? num_cols_ * row_idx + col_idx

        : num_rows_ * col_idx + row_idx;

    if (kInterleaved) {

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

        f[i] = static_cast<double>(data_[kDataDimension * n + i]);

      }

    } else {

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

        f[i] = static_cast<double>(data_[i * num_values_ + n]);

      }

    }

  }

 private:

  const T* data_;

  const int row_begin_;

  const int row_end_;

  const int col_begin_;

  const int col_end_;

  const int num_rows_;

  const int num_cols_;

  const int num_values_;

};

}  // namespace ceres

#endif  // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_