A complete re-write of the cubic interpolation code. The key change is that there is a new layer of abstract, a Array object that the interpolator depends on. The Array provides a one dimension or two dimensional array like interface independent of the underlying representation of the data. Also included here is support for vector valued functions. Change-Id: Ica68f03778cf0d84192db00cd55653f8b4124d51
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h index 7e477c8..8b91f96 100644 --- a/include/ceres/cubic_interpolation.h +++ b/include/ceres/cubic_interpolation.h
@@ -28,25 +28,93 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include "ceres/internal/port.h" - #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 { -// This class takes as input a one dimensional array of values that is -// assumed to be integer valued samples from a function f(x), -// evaluated at x = 0, ... , n - 1 and 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 fractional point in the interval [0, -// n-1]. +// Given samples from a function sampled at four equally spaced points, // -// Besides this, the reason this class is included with Ceres is that -// the Evaluate method is overloaded so that the user can use it as -// part of their automatically differentiated CostFunction objects -// without worrying about the fact that they are working with a -// numerically interpolated object. +// 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) { + DCHECK_GE(x, 0.0); + DCHECK_LE(x, 1.0); + 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 a one dimensional array like object, which provides +// the following interface. +// +// struct Array { +// enum { DATA_DIMENSION = 2; }; +// void GetValue(int n, double* f) const; +// int NumValues() const; +// }; +// +// Where, GetValue gives us the value of a function f (possibly vector +// valued) on the integers: +// +// [0, ..., NumValues() - 1]. +// +// 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. +// +// 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 real valued point in the interval: +// +// [0, NumValues() - 1]. // // For more details on cubic interpolation see // @@ -55,20 +123,63 @@ // Example usage: // // const double x[] = {1.0, 2.0, 5.0, 6.0}; -// CubicInterpolator interpolator(x, 4); +// Array1D data(x, 4); +// CubicInterpolator interpolator(data); // double f, dfdx; // CHECK(interpolator.Evaluator(1.5, &f, &dfdx)); +template<typename Array> class CERES_EXPORT CubicInterpolator { public: - // values is an array containing the values of the function to be - // interpolated on the integer lattice [0, num_values - 1]. - // - // values should be a valid pointer for the lifetime of this object. - CubicInterpolator(const double* values, int num_values); + explicit CubicInterpolator(const Array& array) + : array_(array) { + CHECK_GT(array.NumValues(), 1); + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Array::DATA_DIMENSION, 1); + } - // Evaluate the interpolated function value and/or its - // derivative. Returns false if x is out of bounds. - bool Evaluate(double x, double* f, double* dfdx) const; + bool Evaluate(double x, double* f, double* dfdx) const { + const int num_values = array_.NumValues(); + if (x < 0 || x > num_values - 1) { + LOG(ERROR) << "x = " << x + << " is not in the interval [0, " << num_values - 1 << "]."; + return false; + } + + int n = floor(x); + // Deal with the case where the point sits exactly on the right + // boundary. + if (n == num_values - 1) { + n -= 1; + } + + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p0, p1, p2, p3; + + // The point being evaluated is now expected to lie in the + // internal corresponding to p1 and p2. + array_.GetValue(n, p1.data()); + array_.GetValue(n + 1, p2.data()); + + // If we are at n >=1, the choose the element at n - 1, otherwise + // linearly interpolate from p1 and p2. + if (n > 0) { + array_.GetValue(n - 1, p0.data()); + } else { + p0 = 2 * p1 - p2; + } + + // If we are at n < num_values_ - 2, then choose the element n + + // 2, otherwise linearly interpolate from p1 and p2. + if (n < num_values - 2) { + array_.GetValue(n + 2, p3.data()); + } else { + p3 = 2 * p2 - p1; + } + + CubicHermiteSpline(p0, p1, p2, p3, x - n, f, dfdx); + return true; + } // The following two Evaluate overloads are needed for interfacing // with automatic differentiation. The first is for when a scalar @@ -78,50 +189,223 @@ } template<typename JetT> bool Evaluate(const JetT& x, JetT* f) const { - double dfdx; - if (!Evaluate(x.a, &f->a, &dfdx)) { + double fx[Array::DATA_DIMENSION], dfdx[Array::DATA_DIMENSION]; + if (!Evaluate(x.a, fx, dfdx)) { return false; } - f->v = dfdx * x.v; + + for (int i = 0; i < Array::DATA_DIMENSION; ++i) { + f[i].a = fx[i]; + f[i].v = dfdx[i] * x.v; + } return true; } - int num_values() const { return num_values_; } + int NumValues() const { return array_.NumValues(); } - private: - const double* values_; - const int num_values_; +private: + const Array& array_; }; -// This class takes as input a row-major array of values that is -// assumed to be integer valued samples from a function f(x), -// evaluated on the integer lattice [0, num_rows - 1] x [0, num_cols - -// 1]; and 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 -// fractional point inside this lattice. +// Given as input a two dimensional array like object, which provides +// the following interface: // -// For more details on cubic interpolation see +// struct Array { +// enum { DATA_DIMENSION = 1 }; +// void GetValue(int row, int col, double* f) const; +// int NumRows() const; +// int NumCols() const; +// }; +// +// Where, GetValue gives us the value of a function f (possibly vector +// valued) on the integer grid: +// +// [0, ..., NumRows() - 1] x [0, ..., NumCols() - 1] +// +// 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 real valued point in the quad: +// +// [0, NumRows() - 1] x [0, NumCols() - 1] +// +// For more details on the algorithm used here see: // // "Cubic convolution interpolation for digital image processing". -// IEEE Transactions on Acoustics, Speech, and Signal Processing -// 29 (6): 1153–1160. +// 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 +template<typename Array> class CERES_EXPORT BiCubicInterpolator { public: - // values is a row-major array containing the values of the function - // to be interpolated on the integer lattice [0, num_rows - 1] x [0, - // num_cols - 1]; - // - // values should be a valid pointer for the lifetime of this object. - BiCubicInterpolator(const double* values, int num_rows, int num_cols); + BiCubicInterpolator(const Array& array) + : array_(array) { + CHECK_GT(array.NumRows(), 1); + CHECK_GT(array.NumCols(), 1); + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Array::DATA_DIMENSION, 1); + } // Evaluate the interpolated function value and/or its // derivative. Returns false if r or c is out of bounds. bool Evaluate(double r, double c, - double* f, double* dfdr, double* dfdc) const; + double* f, double* dfdr, double* dfdc) const { + const int num_rows = array_.NumRows(); + const int num_cols = array_.NumCols(); + + if (r < 0 || r > num_rows - 1 || c < 0 || c > num_cols - 1) { + LOG(ERROR) << "(r, c) = (" << r << ", " << c << ")" + << " is not in the square defined by [0, 0] " + << " and [" << num_rows - 1 << ", " << num_cols - 1 << "]"; + return false; + } + + int row = floor(r); + // Handle the case where the point sits exactly on the bottom + // boundary. + if (row == num_rows - 1) { + row -= 1; + } + + int col = floor(c); + // Handle the case where the point sits exactly on the right + // boundary. + if (col == num_cols - 1) { + col -= 1; + } + + // BiCubic interpolation requires 16 values around the point being + // evaluated. We will use pij, to indicate the elements of the + // 4x4 array 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. + + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p00, p01, p02, p03; + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p10, p11, p12, p13; + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p20, p21, p22, p23; + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p30, p31, p32, p33; + + array_.GetValue(row, col, p11.data()); + array_.GetValue(row, col + 1, p12.data()); + array_.GetValue(row + 1, col, p21.data()); + array_.GetValue(row + 1, col + 1, p22.data()); + + // If we are in rows >= 1, then choose the element from the row - 1, + // otherwise linearly interpolate from row and row + 1. + if (row > 0) { + array_.GetValue(row - 1, col, p01.data()); + array_.GetValue(row - 1, col + 1, p02.data()); + } else { + p01 = 2 * p11 - p21; + p02 = 2 * p12 - p22; + } + + // If we are in row < num_rows - 2, then pick the element from the + // row + 2, otherwise linearly interpolate from row and row + 1. + if (row < num_rows - 2) { + array_.GetValue(row + 2, col, p31.data()); + array_.GetValue(row + 2, col + 1, p32.data()); + } else { + p31 = 2 * p21 - p22; + p32 = 2 * p22 - p12; + } + + // Same logic as above, applies to the columns instead of rows. + if (col > 0) { + array_.GetValue(row, col - 1, p10.data()); + array_.GetValue(row + 1, col - 1, p20.data()); + } else { + p10 = 2 * p11 - p12; + p20 = 2 * p21 - p22; + } + + if (col < num_cols - 2) { + array_.GetValue(row, col + 2, p13.data()); + array_.GetValue(row + 1, col + 2, p23.data()); + } else { + p13 = 2 * p12 - p11; + p23 = 2 * p22 - p21; + } + + // The four corners of the block require a bit more care. Let us + // consider the evaluation of p00, the other three corners follow + // in the same manner. + // + // There are four cases in which we need to evaluate p00. + // + // row > 0, col > 0 : v(row, col) + // row = 0, col > 0 : Interpolate p10 & p20 + // row > 0, col = 0 : Interpolate p01 & p02 + // row = 0, col = 0 : Interpolate p10 & p20, or p01 & p02. + if (row > 0) { + if (col > 0) { + array_.GetValue(row - 1, col - 1, p00.data()); + } else { + p00 = 2 * p01 - p02; + } + + if (col < num_cols - 2) { + array_.GetValue(row - 1, col + 2, p03.data()); + } else { + p03 = 2 * p02 - p01; + } + } else { + p00 = 2 * p10 - p20; + p03 = 2 * p13 - p23; + } + + if (row < num_rows - 2) { + if (col > 0) { + array_.GetValue(row + 2, col - 1, p30.data()); + } else { + p30 = 2 * p31 - p32; + } + + if (col < num_cols - 2) { + array_.GetValue(row + 2, col + 2, p33.data()); + } else { + p33 = 2 * p32 - p31; + } + } else { + p30 = 2 * p20 - p10; + p33 = 2 * p23 - p13; + } + + // Interpolate along each of the four rows, evaluating the function + // value and the horizontal derivative in each row. + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> f0, f1, f2, f3; + Eigen::Matrix<double, Array::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc; + CubicHermiteSpline(p00, p01, p02, p03, c - col, f0.data(), df0dc.data()); + CubicHermiteSpline(p10, p11, p12, p13, c - col, f1.data(), df1dc.data()); + CubicHermiteSpline(p20, p21, p22, p23, c - col, f2.data(), df2dc.data()); + CubicHermiteSpline(p30, p31, p32, p33, c - col, f3.data(), df3dc.data()); + + // Interpolate vertically the interpolated value from each row and + // compute the derivative along the columns. + CubicHermiteSpline(f0, f1, f2, f3, r - row, f, dfdr); + if (dfdc != NULL) { + // Interpolate vertically the derivative along the columns. + CubicHermiteSpline(df0dc, df1dc, df2dc, df3dc, r - row, dfdc, NULL); + } + + return true; + } // The following two Evaluate overloads are needed for interfacing // with automatic differentiation. The first is for when a scalar @@ -133,19 +417,125 @@ template<typename JetT> bool Evaluate(const JetT& r, const JetT& c, JetT* f) const { - double dfdr, dfdc; - if (!Evaluate(r.a, c.a, &f->a, &dfdr, &dfdc)) { + double frc[Array::DATA_DIMENSION]; + double dfdr[Array::DATA_DIMENSION]; + double dfdc[Array::DATA_DIMENSION]; + if (!Evaluate(r.a, c.a, frc, dfdr, dfdc)) { return false; } - f->v = dfdr * r.v + dfdc * c.v; + + for (int i = 0; i < Array::DATA_DIMENSION; ++i) { + f[i].a = frc[i]; + f[i].v = dfdr[i] * r.v + dfdc[i] * c.v; + } + return true; } - int num_rows() const { return num_rows_; } - int num_cols() const { return num_cols_; } + int NumRows() const { return array_.NumRows(); } + int NumCols() const { return array_.NumCols(); } private: - const double* values_; + const Array& array_; +}; + +// An object that implements the one dimensional array like object +// needed by the CubicInterpolator where the source of the function +// values is an array of type T. +// +// 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 Array1D { + enum { DATA_DIMENSION = kDataDimension }; + + Array1D(const T* data, const int num_values) + : data_(data), num_values_(num_values) { + } + + void GetValue(const int n, double* f) const { + if (n < 0 || n > num_values_ - 1) { + LOG(FATAL) << "n = " << n + << " is not in the interval [0, " << num_values_ - 1 << "]."; + } + + for (int i = 0; i < kDataDimension; ++i) { + if (kInterleaved) { + f[i] = static_cast<double>(data_[kDataDimension * n + i]); + } else { + f[i] = static_cast<double>(data_[i * num_values_ + n]); + } + } + } + + int NumValues() const { return num_values_; } + + private: + const T* data_; + const int num_values_; +}; + +// An object that implements the two dimensional array like object +// needed by the BiCubicInterpolator where the source of the function +// values is an array of type T. +// +// 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 Array2D { + enum Foo { DATA_DIMENSION = kDataDimension }; + + Array2D(const T* data, const int num_rows, const int num_cols) + : data_(data), num_rows_(num_rows), num_cols_(num_cols) { + CHECK_GE(kDataDimension, 1); + } + + void GetValue(const int r, const int c, double* f) const { + if (r < 0 || r > num_rows_ - 1 || c < 0 || c > num_cols_ - 1) { + LOG(FATAL) << "(r, c) = (" << r << ", " << c << ")" + << " is not in the square defined by [0, 0] " + << " and [" << num_rows_ - 1 << ", " << num_cols_ - 1 << "]"; + } + + const int n = (kRowMajor) ? num_cols_ * r + c : num_rows_ * c + r; + for (int i = 0; i < kDataDimension; ++i) { + if (kInterleaved) { + f[i] = static_cast<double>(data_[kDataDimension * n + i]); + } else { + f[i] = static_cast<double>(data_[i * (num_rows_ * num_cols_) + n]); + } + } + } + + int NumRows() const { return num_rows_; } + int NumCols() const { return num_cols_; } + + private: + const T* data_; const int num_rows_; const int num_cols_; };
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index f2f8a85..a64ea52 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -53,7 +53,6 @@ corrector.cc covariance.cc covariance_impl.cc - cubic_interpolation.cc cxsparse.cc dense_normal_cholesky_solver.cc dense_qr_solver.cc
diff --git a/internal/ceres/cubic_interpolation.cc b/internal/ceres/cubic_interpolation.cc deleted file mode 100644 index 764b306..0000000 --- a/internal/ceres/cubic_interpolation.cc +++ /dev/null
@@ -1,258 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2014 Google Inc. All rights reserved. -// http://code.google.com/p/ceres-solver/ -// -// 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) - -#include "ceres/cubic_interpolation.h" - -#include <math.h> -#include "glog/logging.h" - -namespace ceres { -namespace { - -// 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 -inline void CubicHermiteSpline(const double p0, - const double p1, - const double p2, - const double p3, - const double x, - double* f, - double* dfdx) { - const double a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3); - const double b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3); - const double c = 0.5 * (-p0 + p2); - const double d = p1; - - // Use Horner's rule to evaluate the function value and its - // derivative. - - // f = ax^3 + bx^2 + cx + d - if (f != NULL) { - *f = d + x * (c + x * (b + x * a)); - } - - // dfdx = 3ax^2 + 2bx + c - if (dfdx != NULL) { - *dfdx = c + x * (2.0 * b + 3.0 * a * x); - } -} - -} // namespace - -CubicInterpolator::CubicInterpolator(const double* values, const int num_values) - : values_(CHECK_NOTNULL(values)), - num_values_(num_values) { - CHECK_GT(num_values, 1); -} - -bool CubicInterpolator::Evaluate(const double x, - double* f, - double* dfdx) const { - if (x < 0 || x > num_values_ - 1) { - LOG(ERROR) << "x = " << x - << " is not in the interval [0, " << num_values_ - 1 << "]."; - return false; - } - - int n = floor(x); - - // Handle the case where the point sits exactly on the right boundary. - if (n == num_values_ - 1) { - n -= 1; - } - - const double p1 = values_[n]; - const double p2 = values_[n + 1]; - const double p0 = (n > 0) ? values_[n - 1] : (2.0 * p1 - p2); - const double p3 = (n < (num_values_ - 2)) ? values_[n + 2] : (2.0 * p2 - p1); - CubicHermiteSpline(p0, p1, p2, p3, x - n, f, dfdx); - return true; -} - -BiCubicInterpolator::BiCubicInterpolator(const double* values, - const int num_rows, - const int num_cols) - : values_(CHECK_NOTNULL(values)), - num_rows_(num_rows), - num_cols_(num_cols) { - CHECK_GT(num_rows, 1); - CHECK_GT(num_cols, 1); -} - -bool BiCubicInterpolator::Evaluate(const double r, - const double c, - double* f, - double* dfdr, - double* dfdc) const { - if (r < 0 || r > num_rows_ - 1 || c < 0 || c > num_cols_ - 1) { - LOG(ERROR) << "(r, c) = " << r << ", " << c - << " is not in the square defined by [0, 0] " - << " and [" << num_rows_ - 1 << ", " << num_cols_ - 1 << "]"; - return false; - } - - int row = floor(r); - // Handle the case where the point sits exactly on the bottom - // boundary. - if (row == num_rows_ - 1) { - row -= 1; - } - - int col = floor(c); - // Handle the case where the point sits exactly on the right - // boundary. - if (col == num_cols_ - 1) { - col -= 1; - } - -#define v(n, m) values_[(n) * num_cols_ + m] - - // BiCubic interpolation requires 16 values around the point being - // evaluated. We will use pij, to indicate the elements of the 4x4 - // array 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. - - // These four entries are guaranteed to be in the values_ array. - const double p11 = v(row, col); - const double p12 = v(row, col + 1); - const double p21 = v(row + 1, col); - const double p22 = v(row + 1, col + 1); - - // If we are in rows >= 1, then choose the element from the row - 1, - // otherwise linearly interpolate from row and row + 1. - const double p01 = (row > 0) ? v(row - 1, col) : 2 * p11 - p21; - const double p02 = (row > 0) ? v(row - 1, col + 1) : 2 * p12 - p22; - - // If we are in row < num_rows_ - 2, then pick the element from the - // row + 2, otherwise linearly interpolate from row and row + 1. - const double p31 = (row < num_rows_ - 2) ? v(row + 2, col) : 2 * p21 - p11; - const double p32 = (row < num_rows_ - 2) ? v(row + 2, col + 1) : 2 * p22 - p12; // NOLINT - - // Same logic as above, applies to the columns instead of rows. - const double p10 = (col > 0) ? v(row, col - 1) : 2 * p11 - p12; - const double p20 = (col > 0) ? v(row + 1, col - 1) : 2 * p21 - p22; - const double p13 = (col < num_cols_ - 2) ? v(row, col + 2) : 2 * p12 - p11; - const double p23 = (col < num_cols_ - 2) ? v(row + 1, col + 2) : 2 * p22 - p21; // NOLINT - - // The four corners of the block require a bit more care. Let us - // consider the evaluation of p00, the other three corners follow in - // the same manner. - // - // There are four cases in which we need to evaluate p00. - // - // row > 0, col > 0 : v(row, col) - // row = 0, col > 1 : Interpolate p10 & p20 - // row > 1, col = 0 : Interpolate p01 & p02 - // row = 0, col = 0 : Interpolate p10 & p20, or p01 & p02. - double p00, p03; - if (row > 0) { - if (col > 0) { - p00 = v(row - 1, col - 1); - } else { - p00 = 2 * p01 - p02; - } - - if (col < num_cols_ - 2) { - p03 = v(row - 1, col + 2); - } else { - p03 = 2 * p02 - p01; - } - } else { - p00 = 2 * p10 - p20; - p03 = 2 * p13 - p23; - } - - double p30, p33; - if (row < num_rows_ - 2) { - if (col > 0) { - p30 = v(row + 2, col - 1); - } else { - p30 = 2 * p31 - p32; - } - - if (col < num_cols_ - 2) { - p33 = v(row + 2, col + 2); - } else { - p33 = 2 * p32 - p31; - } - } else { - p30 = 2 * p20 - p10; - p33 = 2 * p23 - p13; - } - - // Interpolate along each of the four rows, evaluating the function - // value and the horizontal derivative in each row. - double f0, f1, f2, f3; - double df0dc, df1dc, df2dc, df3dc; - CubicHermiteSpline(p00, p01, p02, p03, c - col, &f0, &df0dc); - CubicHermiteSpline(p10, p11, p12, p13, c - col, &f1, &df1dc); - CubicHermiteSpline(p20, p21, p22, p23, c - col, &f2, &df2dc); - CubicHermiteSpline(p30, p31, p32, p33, c - col, &f3, &df3dc); - - // Interpolate vertically the interpolated value from each row and - // compute the derivative along the columns. - CubicHermiteSpline(f0, f1, f2, f3, r - row, f, dfdr); - if (dfdc != NULL) { - // Interpolate vertically the derivative along the columns. - CubicHermiteSpline(df0dc, df1dc, df2dc, df3dc, r - row, dfdc, NULL); - } - - return true; -#undef v -} - -} // namespace ceres
diff --git a/internal/ceres/cubic_interpolation_test.cc b/internal/ceres/cubic_interpolation_test.cc index 0b4bb12..04a7b33 100644 --- a/internal/ceres/cubic_interpolation_test.cc +++ b/internal/ceres/cubic_interpolation_test.cc
@@ -31,31 +31,147 @@ #include "ceres/cubic_interpolation.h" #include "ceres/jet.h" +#include "ceres/internal/scoped_ptr.h" #include "glog/logging.h" #include "gtest/gtest.h" namespace ceres { namespace internal { -TEST(CubicInterpolator, NeedsAtleastTwoValues) { - double x[] = {1}; - EXPECT_DEATH_IF_SUPPORTED(CubicInterpolator c(x, 0), "num_values > 1"); - EXPECT_DEATH_IF_SUPPORTED(CubicInterpolator c(x, 1), "num_values > 1"); +static const double kTolerance = 1e-12; + +TEST(Array1D, OneDataDimension) { + int x[] = {1, 2, 3}; + Array1D<int, 1> array(x, 3); + for (int i = 0; i < 3; ++i) { + double value; + array.GetValue(i, &value); + EXPECT_EQ(value, static_cast<double>(i + 1)); + } } -static const double kTolerance = 1e-12; +TEST(Array1D, TwoDataDimensionIntegerDataInterleaved) { + int x[] = {1, 5, + 2, 6, + 3, 7}; + + Array1D<int, 2, true> array(x, 3); + for (int i = 0; i < 3; ++i) { + double value[2]; + array.GetValue(i, value); + EXPECT_EQ(value[0], static_cast<double>(i + 1)); + EXPECT_EQ(value[1], static_cast<double>(i + 5)); + } +} + +TEST(Array1D, TwoDataDimensionIntegerDataStacked) { + int x[] = {1, 2, 3, + 5, 6, 7}; + + Array1D<int, 2, false> array(x, 3); + for (int i = 0; i < 3; ++i) { + double value[2]; + array.GetValue(i, value); + EXPECT_EQ(value[0], static_cast<double>(i + 1)); + EXPECT_EQ(value[1], static_cast<double>(i + 5)); + } +} + +TEST(Array2D, OneDataDimensionRowMajor) { + int x[] = {1, 2, 3, + 2, 3, 4}; + Array2D<int, 1, true, true> array(x, 2, 3); + for (int r = 0; r < 2; ++r) { + for (int c = 0; c < 3; ++c) { + double value; + array.GetValue(r, c, &value); + EXPECT_EQ(value, static_cast<double>(r + c + 1)); + } + } +} + +TEST(Array2D, TwoDataDimensionRowMajorInterleaved) { + int x[] = {1, 4, 2, 8, 3, 12, + 2, 8, 3, 12, 4, 16}; + Array2D<int, 2, true, true> array(x, 2, 3); + for (int r = 0; r < 2; ++r) { + for (int c = 0; c < 3; ++c) { + double value[2]; + array.GetValue(r, c, value); + EXPECT_EQ(value[0], static_cast<double>(r + c + 1)); + EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1))); + } + } +} + +TEST(Array2D, TwoDataDimensionRowMajorStacked) { + int x[] = {1, 2, 3, + 2, 3, 4, + 4, 8, 12, + 8, 12, 16}; + Array2D<int, 2, true, false> array(x, 2, 3); + for (int r = 0; r < 2; ++r) { + for (int c = 0; c < 3; ++c) { + double value[2]; + array.GetValue(r, c, value); + EXPECT_EQ(value[0], static_cast<double>(r + c + 1)); + EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1))); + } + } +} + +TEST(Array2D, TwoDataDimensionColMajorInterleaved) { + int x[] = { 1, 4, 2, 8, + 2, 8, 3, 12, + 3, 12, 4, 16}; + Array2D<int, 2, false, true> array(x, 2, 3); + for (int r = 0; r < 2; ++r) { + for (int c = 0; c < 3; ++c) { + double value[2]; + array.GetValue(r, c, value); + EXPECT_EQ(value[0], static_cast<double>(r + c + 1)); + EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1))); + } + } +} + +TEST(Array2D, TwoDataDimensionColMajorStacked) { + int x[] = {1, 2, + 2, 3, + 3, 4, + 4, 8, + 8, 12, + 12, 16}; + Array2D<int, 2, false, false> array(x, 2, 3); + for (int r = 0; r < 2; ++r) { + for (int c = 0; c < 3; ++c) { + double value[2]; + array.GetValue(r, c, value); + EXPECT_EQ(value[0], static_cast<double>(r + c + 1)); + EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1))); + } + } +} + class CubicInterpolatorTest : public ::testing::Test { public: + template <int kDataDimension> void RunPolynomialInterpolationTest(const double a, const double b, const double c, const double d) { + values_.reset(new double[kDataDimension * kNumSamples]); + for (int x = 0; x < kNumSamples; ++x) { - values_[x] = a * x * x * x + b * x * x + c * x + d; + for (int dim = 0; dim < kDataDimension; ++dim) { + values_[x * kDataDimension + dim] = + (dim * dim + 1) * (a * x * x * x + b * x * x + c * x + d); + } } - CubicInterpolator interpolator(values_, kNumSamples); + Array1D<double, kDataDimension> array(values_.get(), kNumSamples); + CubicInterpolator<Array1D<double, kDataDimension> > interpolator(array); // Check values in the all the cells but the first and the last // ones. In these cells, the interpolated function values should @@ -66,46 +182,63 @@ // function values and its derivatives not to match. for (int j = 0; j < kNumTestSamples; ++j) { const double x = 1.0 + 7.0 / (kNumTestSamples - 1) * j; - const double expected_f = a * x * x * x + b * x * x + c * x + d; - const double expected_dfdx = 3.0 * a * x * x + 2.0 * b * x + c; - double f, dfdx; + double expected_f[kDataDimension], expected_dfdx[kDataDimension]; + double f[kDataDimension], dfdx[kDataDimension]; - EXPECT_TRUE(interpolator.Evaluate(x, &f, &dfdx)); - EXPECT_NEAR(f, expected_f, kTolerance) - << "x: " << x - << " actual f(x): " << expected_f - << " estimated f(x): " << f; - EXPECT_NEAR(dfdx, expected_dfdx, kTolerance) - << "x: " << x - << " actual df(x)/dx: " << expected_dfdx - << " estimated df(x)/dx: " << dfdx; + for (int dim = 0; dim < kDataDimension; ++dim) { + expected_f[dim] = + (dim * dim + 1) * (a * x * x * x + b * x * x + c * x + d); + expected_dfdx[dim] = (dim * dim + 1) * (3.0 * a * x * x + 2.0 * b * x + c); + } + + EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx)); + for (int dim = 0; dim < kDataDimension; ++dim) { + EXPECT_NEAR(f[dim], expected_f[dim], kTolerance) + << "x: " << x << " dim: " << dim + << " actual f(x): " << expected_f[dim] + << " estimated f(x): " << f[dim]; + EXPECT_NEAR(dfdx[dim], expected_dfdx[dim], kTolerance) + << "x: " << x << " dim: " << dim + << " actual df(x)/dx: " << expected_dfdx[dim] + << " estimated df(x)/dx: " << dfdx[dim]; + } } } private: static const int kNumSamples = 10; static const int kNumTestSamples = 100; - double values_[kNumSamples]; + scoped_array<double> values_; }; TEST_F(CubicInterpolatorTest, ConstantFunction) { - RunPolynomialInterpolationTest(0.0, 0.0, 0.0, 0.5); + RunPolynomialInterpolationTest<1>(0.0, 0.0, 0.0, 0.5); + RunPolynomialInterpolationTest<2>(0.0, 0.0, 0.0, 0.5); + RunPolynomialInterpolationTest<3>(0.0, 0.0, 0.0, 0.5); } TEST_F(CubicInterpolatorTest, LinearFunction) { - RunPolynomialInterpolationTest(0.0, 0.0, 1.0, 0.5); + RunPolynomialInterpolationTest<1>(0.0, 0.0, 1.0, 0.5); + RunPolynomialInterpolationTest<2>(0.0, 0.0, 1.0, 0.5); + RunPolynomialInterpolationTest<3>(0.0, 0.0, 1.0, 0.5); } TEST_F(CubicInterpolatorTest, QuadraticFunction) { - RunPolynomialInterpolationTest(0.0, 0.4, 1.0, 0.5); + RunPolynomialInterpolationTest<1>(0.0, 0.4, 1.0, 0.5); + RunPolynomialInterpolationTest<2>(0.0, 0.4, 1.0, 0.5); + RunPolynomialInterpolationTest<3>(0.0, 0.4, 1.0, 0.5); } + TEST(CubicInterpolator, JetEvaluation) { - const double values[] = {1.0, 2.0, 2.0, 3.0}; - CubicInterpolator interpolator(values, 4); - double f, dfdx; + const double values[] = {1.0, 2.0, 2.0, 5.0, 3.0, 9.0, 2.0, 7.0}; + + Array1D<double, 2, true> array(values, 4); + CubicInterpolator<Array1D<double, 2, true> > interpolator(array); + + double f[2], dfdx[2]; const double x = 2.5; - EXPECT_TRUE(interpolator.Evaluate(x, &f, &dfdx)); + EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx)); // Create a Jet with the same scalar part as x, so that the output // Jet will be evaluated at x. @@ -116,42 +249,48 @@ x_jet.v(2) = 1.2; x_jet.v(3) = 1.3; - Jet<double, 4> f_jet; - EXPECT_TRUE(interpolator.Evaluate(x_jet, &f_jet)); + Jet<double, 4> f_jets[2]; + EXPECT_TRUE(interpolator.Evaluate(x_jet, f_jets)); // Check that the scalar part of the Jet is f(x). - EXPECT_EQ(f_jet.a, f); + EXPECT_EQ(f_jets[0].a, f[0]); + EXPECT_EQ(f_jets[1].a, f[1]); // Check that the derivative part of the Jet is dfdx * x_jet.v // by the chain rule. - EXPECT_EQ((f_jet.v - dfdx * x_jet.v).norm(), 0.0); + EXPECT_NEAR((f_jets[0].v - dfdx[0] * x_jet.v).norm(), 0.0, kTolerance); + EXPECT_NEAR((f_jets[1].v - dfdx[1] * x_jet.v).norm(), 0.0, kTolerance); } class BiCubicInterpolatorTest : public ::testing::Test { public: + template <int kDataDimension> void RunPolynomialInterpolationTest(const Eigen::Matrix3d& coeff) { + values_.reset(new double[kNumRows * kNumCols * kDataDimension]); coeff_ = coeff; - double* v = values_; + double* v = values_.get(); for (int r = 0; r < kNumRows; ++r) { for (int c = 0; c < kNumCols; ++c) { - *v++ = EvaluateF(r, c); + for (int dim = 0; dim < kDataDimension; ++dim) { + *v++ = (dim * dim + 1) * EvaluateF(r, c); + } } } - BiCubicInterpolator interpolator(values_, kNumRows, kNumCols); + + Array2D<double, kDataDimension> array(values_.get(), kNumRows, kNumCols); + BiCubicInterpolator<Array2D<double, kDataDimension> > interpolator(array); for (int j = 0; j < kNumRowSamples; ++j) { const double r = 1.0 + 7.0 / (kNumRowSamples - 1) * j; for (int k = 0; k < kNumColSamples; ++k) { const double c = 1.0 + 7.0 / (kNumColSamples - 1) * k; - const double expected_f = EvaluateF(r, c); - const double expected_dfdr = EvaluatedFdr(r, c); - const double expected_dfdc = EvaluatedFdc(r, c); - double f, dfdr, dfdc; - - EXPECT_TRUE(interpolator.Evaluate(r, c, &f, &dfdr, &dfdc)); - EXPECT_NEAR(f, expected_f, kTolerance); - EXPECT_NEAR(dfdr, expected_dfdr, kTolerance); - EXPECT_NEAR(dfdc, expected_dfdc, kTolerance); + double f[kDataDimension], dfdr[kDataDimension], dfdc[kDataDimension]; + EXPECT_TRUE(interpolator.Evaluate(r, c, f, dfdr, dfdc)); + for (int dim = 0; dim < kDataDimension; ++dim) { + EXPECT_NEAR(f[dim], (dim * dim + 1) * EvaluateF(r, c), kTolerance); + EXPECT_NEAR(dfdr[dim], (dim * dim + 1) * EvaluatedFdr(r, c), kTolerance); + EXPECT_NEAR(dfdc[dim], (dim * dim + 1) * EvaluatedFdc(r, c), kTolerance); + } } } } @@ -187,18 +326,22 @@ static const int kNumCols = 10; static const int kNumRowSamples = 100; static const int kNumColSamples = 100; - double values_[kNumRows * kNumCols]; + scoped_array<double> values_; }; TEST_F(BiCubicInterpolatorTest, ZeroFunction) { Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree00Function) { Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); coeff(2, 2) = 1.0; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree01Function) { @@ -206,7 +349,9 @@ coeff(2, 2) = 1.0; coeff(0, 2) = 0.1; coeff(2, 0) = 0.1; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree10Function) { @@ -214,7 +359,9 @@ coeff(2, 2) = 1.0; coeff(0, 1) = 0.1; coeff(1, 0) = 0.1; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree11Function) { @@ -224,7 +371,9 @@ coeff(1, 0) = 0.1; coeff(0, 2) = 0.2; coeff(2, 0) = 0.2; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree12Function) { @@ -235,7 +384,9 @@ coeff(0, 2) = 0.2; coeff(2, 0) = 0.2; coeff(1, 1) = 0.3; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree21Function) { @@ -246,7 +397,9 @@ coeff(0, 2) = 0.2; coeff(2, 0) = 0.2; coeff(0, 0) = 0.3; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST_F(BiCubicInterpolatorTest, Degree22Function) { @@ -259,17 +412,22 @@ coeff(0, 0) = 0.3; coeff(0, 1) = -0.4; coeff(1, 0) = -0.4; - RunPolynomialInterpolationTest(coeff); + RunPolynomialInterpolationTest<1>(coeff); + RunPolynomialInterpolationTest<2>(coeff); + RunPolynomialInterpolationTest<3>(coeff); } TEST(BiCubicInterpolator, JetEvaluation) { - const double values[] = {1.0, 2.0, 2.0, 3.0, - 1.0, 2.0, 2.0, 3.0}; - BiCubicInterpolator interpolator(values, 2, 4); - double f, dfdr, dfdc; + const double values[] = {1.0, 5.0, 2.0, 10.0, 2.0, 6.0, 3.0, 5.0, + 1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 3.0, 1.0}; + + Array2D<double, 2> array(values, 2, 4); + BiCubicInterpolator<Array2D<double, 2> > interpolator(array); + + double f[2], dfdr[2], dfdc[2]; const double r = 0.5; const double c = 2.5; - EXPECT_TRUE(interpolator.Evaluate(r, c, &f, &dfdr, &dfdc)); + EXPECT_TRUE(interpolator.Evaluate(r, c, f, dfdr, dfdc)); // Create a Jet with the same scalar part as x, so that the output // Jet will be evaluated at x. @@ -287,10 +445,16 @@ c_jet.v(2) = 4.2; c_jet.v(3) = 5.3; - Jet<double, 4> f_jet; - EXPECT_TRUE(interpolator.Evaluate(r_jet, c_jet, &f_jet)); - EXPECT_EQ(f_jet.a, f); - EXPECT_EQ((f_jet.v - dfdr * r_jet.v - dfdc * c_jet.v).norm(), 0.0); + Jet<double, 4> f_jets[2]; + EXPECT_TRUE(interpolator.Evaluate(r_jet, c_jet, f_jets)); + EXPECT_EQ(f_jets[0].a, f[0]); + EXPECT_EQ(f_jets[1].a, f[1]); + EXPECT_NEAR((f_jets[0].v - dfdr[0] * r_jet.v - dfdc[0] * c_jet.v).norm(), + 0.0, + kTolerance); + EXPECT_NEAR((f_jets[1].v - dfdr[1] * r_jet.v - dfdc[1] * c_jet.v).norm(), + 0.0, + kTolerance); } } // namespace internal