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
