diff options
Diffstat (limited to 'extern/ceres/include/ceres/cubic_interpolation.h')
| -rw-r--r-- | extern/ceres/include/ceres/cubic_interpolation.h | 436 |
1 files changed, 436 insertions, 0 deletions
diff --git a/extern/ceres/include/ceres/cubic_interpolation.h b/extern/ceres/include/ceres/cubic_interpolation.h new file mode 100644 index 00000000000..9b9ea4a942c --- /dev/null +++ b/extern/ceres/include/ceres/cubic_interpolation.h @@ -0,0 +1,436 @@ +// 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: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_ +#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_ + +#include "Eigen/Core" +#include "ceres/internal/port.h" +#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(data, 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. Uses the nearest point on the grid boundary 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_ |