Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h
new file mode 100644
index 0000000..88a3ebf
--- /dev/null
+++ b/include/ceres/cubic_interpolation.h
@@ -0,0 +1,439 @@
+// 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 <typename T,
+ 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 <typename T,
+ 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_