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/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
