Add BiCubic interpolation. This bi-cubic interpolation implementation is based on the cubic convolution algorithm of keys, which allows us to implement a bi-cubic spline like interpolation scheme using five one dimensional cubic spline operations. Change-Id: I116aa8036191c3e654af788323fc8298ae8252a6
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h index 5d18b59..2ade679 100644 --- a/include/ceres/cubic_interpolation.h +++ b/include/ceres/cubic_interpolation.h
@@ -37,9 +37,10 @@ // 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 Catmull-Rom splines to +// 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 point in the interval [0, n-1]. +// the f(x) and f'(x) at any fractional point in the interval [0, +// n-1]. // // 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 @@ -50,7 +51,6 @@ // For more details on cubic interpolation see // // http://en.wikipedia.org/wiki/Cubic_Hermite_spline -// http://www.paulinternet.nl/?page=bicubic // // Example usage: // @@ -60,6 +60,9 @@ // CHECK(interpolator.Evaluator(1.5, &f, &dfdx)); 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); @@ -84,6 +87,57 @@ const int num_values_; }; +// 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. +// +// For more details on cubic interpolation see +// +// "Cubic convolution interpolation for digital image processing". +// IEEE Transactions on Acoustics, Speech, and Signal Processing +// 29 (6): 1153–1160. +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// http://en.wikipedia.org/wiki/Bicubic_interpolation +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); + + // 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; + + // Overload for Jets, which automatically accounts for the chain rule. + 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)) { + return false; + } + f->v = dfdr * r.v + dfdc * c.v; + return true; + } + + int num_rows() const { return num_rows_; } + int num_cols() const { return num_cols_; } + + private: + const double* values_; + const int num_rows_; + const int num_cols_; +}; + } // namespace ceres #endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_
diff --git a/internal/ceres/cubic_interpolation.cc b/internal/ceres/cubic_interpolation.cc index 9dd6194..b202579 100644 --- a/internal/ceres/cubic_interpolation.cc +++ b/internal/ceres/cubic_interpolation.cc
@@ -27,9 +27,6 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: sameeragarwal@google.com (Sameer Agarwal) -// -// This implementation was inspired by the description at -// http://www.paulinternet.nl/?page=bicubic #include "ceres/cubic_interpolation.h" @@ -39,13 +36,35 @@ namespace ceres { namespace { -inline void CatmullRomSpline(const double p0, - const double p1, - const double p2, - const double p3, - const double x, - double* f, - double* dfdx) { +// 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 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 the case of a = -0.5. +// +// 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); @@ -77,6 +96,8 @@ 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; } @@ -91,8 +112,148 @@ 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); - CatmullRomSpline(p0, p1, p2, p3, x - n, f, dfdx); + 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 four 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 04aacc0..d3b5b49 100644 --- a/internal/ceres/cubic_interpolation_test.cc +++ b/internal/ceres/cubic_interpolation_test.cc
@@ -47,7 +47,6 @@ class CubicInterpolatorTest : public ::testing::Test { public: - void RunPolynomialInterpolationTest(const double a, const double b, const double c, @@ -83,6 +82,7 @@ } } + private: static const int kNumSamples = 10; static const int kNumTestSamples = 100; double values_[kNumSamples]; @@ -109,22 +109,188 @@ // Create a Jet with the same scalar part as x, so that the output // Jet will be evaluate at x. - Jet<double, 4> input_jet; - input_jet.a = x; - input_jet.v(0) = 1.0; - input_jet.v(1) = 1.1; - input_jet.v(2) = 1.2; - input_jet.v(3) = 1.3; + Jet<double, 4> x_jet; + x_jet.a = x; + x_jet.v(0) = 1.0; + x_jet.v(1) = 1.1; + x_jet.v(2) = 1.2; + x_jet.v(3) = 1.3; - Jet<double, 4> output_jet; - EXPECT_TRUE(interpolator.Evaluate(input_jet, &output_jet)); + Jet<double, 4> f_jet; + EXPECT_TRUE(interpolator.Evaluate(x_jet, &f_jet)); // Check that the scalar part of the Jet is f(x). - EXPECT_EQ(output_jet.a, f); + EXPECT_EQ(f_jet.a, f); - // Check that the derivative part of the Jet is dfdx * input_jet.v + // Check that the derivative part of the Jet is dfdx * x_jet.v // by the chain rule. - EXPECT_EQ((output_jet.v - dfdx * input_jet.v).norm(), 0.0); + EXPECT_EQ((f_jet.v - dfdx * x_jet.v).norm(), 0.0); +} + +class BiCubicInterpolatorTest : public ::testing::Test { + public: + void RunPolynomialInterpolationTest(const Eigen::Matrix3d& coeff) { + coeff_ = coeff; + double* v = values_; + for (int r = 0; r < kNumRows; ++r) { + for (int c = 0; c < kNumCols; ++c) { + *v++ = EvaluateF(r, c); + } + } + BiCubicInterpolator interpolator(values_, kNumRows, kNumCols); + + 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); + } + } + } + + private: + double EvaluateF(double r, double c) { + Eigen::Vector3d x; + x(0) = r; + x(1) = c; + x(2) = 1; + return x.transpose() * coeff_ * x; + } + + double EvaluatedFdr(double r, double c) { + Eigen::Vector3d x; + x(0) = r; + x(1) = c; + x(2) = 1; + return (coeff_.row(0) + coeff_.col(0).transpose()) * x; + } + + double EvaluatedFdc(double r, double c) { + Eigen::Vector3d x; + x(0) = r; + x(1) = c; + x(2) = 1; + return (coeff_.row(1) + coeff_.col(1).transpose()) * x; + } + + + Eigen::Matrix3d coeff_; + static const int kNumRows = 10; + static const int kNumCols = 10; + static const int kNumRowSamples = 100; + static const int kNumColSamples = 100; + double values_[kNumRows * kNumCols]; +}; + +TEST_F(BiCubicInterpolatorTest, ZeroFunction) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree00Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree01Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 2) = 0.1; + coeff(2, 0) = 0.1; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree10Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 1) = 0.1; + coeff(1, 0) = 0.1; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree11Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 1) = 0.1; + coeff(1, 0) = 0.1; + coeff(0, 2) = 0.2; + coeff(2, 0) = 0.2; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree12Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 1) = 0.1; + coeff(1, 0) = 0.1; + coeff(0, 2) = 0.2; + coeff(2, 0) = 0.2; + coeff(1, 1) = 0.3; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree21Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 1) = 0.1; + coeff(1, 0) = 0.1; + coeff(0, 2) = 0.2; + coeff(2, 0) = 0.2; + coeff(0, 0) = 0.3; + RunPolynomialInterpolationTest(coeff); +} + +TEST_F(BiCubicInterpolatorTest, Degree22Function) { + Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero(); + coeff(2, 2) = 1.0; + coeff(0, 1) = 0.1; + coeff(1, 0) = 0.1; + coeff(0, 2) = 0.2; + coeff(2, 0) = 0.2; + coeff(0, 0) = 0.3; + coeff(0, 1) = -0.4; + coeff(1, 0) = -0.4; + RunPolynomialInterpolationTest(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 r = 0.5; + const double c = 2.5; + 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 evaluate at x. + Jet<double, 4> r_jet; + r_jet.a = r; + r_jet.v(0) = 1.0; + r_jet.v(1) = 1.1; + r_jet.v(2) = 1.2; + r_jet.v(3) = 1.3; + + Jet<double, 4> c_jet; + c_jet.a = c; + c_jet.v(0) = 2.0; + c_jet.v(1) = 3.1; + 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); } } // namespace internal