Add a one dimensional cubic interpolator.
Add a cubic interpolator based on the Catmull-Rom spline,
with support for automatic differentiation.
Change-Id: I02ae4c4ea37805ff1f717b05ea805989b474bd59
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h
new file mode 100644
index 0000000..5d18b59
--- /dev/null
+++ b/include/ceres/cubic_interpolation.h
@@ -0,0 +1,89 @@
+// 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/internal/port.h"
+
+#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_
+#define CERES_PUBLIC_CUBIC_INTERPOLATION_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 Catmull-Rom 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].
+//
+// 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.
+//
+// For more details on cubic interpolation see
+//
+// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
+// http://www.paulinternet.nl/?page=bicubic
+//
+// Example usage:
+//
+// const double x[] = {1.0, 2.0, 5.0, 6.0};
+// CubicInterpolator interpolator(x, 4);
+// double f, dfdx;
+// CHECK(interpolator.Evaluator(1.5, &f, &dfdx));
+class CERES_EXPORT CubicInterpolator {
+ public:
+ // values should be a valid pointer for the lifetime of this object.
+ CubicInterpolator(const double* values, int num_values);
+
+ // 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;
+
+ // Overload for Jets, which automatically accounts for the chain rule.
+ template<typename JetT> bool Evaluate(const JetT& x, JetT* f) const {
+ double dfdx;
+ if (!Evaluate(x.a, &f->a, &dfdx)) {
+ return false;
+ }
+ f->v = dfdx * x.v;
+ return true;
+ }
+
+ int num_values() const { return num_values_; }
+
+ private:
+ const double* values_;
+ const int num_values_;
+};
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 7c1ed5b..f2f8a85 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -53,6 +53,7 @@
corrector.cc
covariance.cc
covariance_impl.cc
+ cubic_interpolation.cc
cxsparse.cc
dense_normal_cholesky_solver.cc
dense_qr_solver.cc
@@ -250,6 +251,7 @@
CERES_TEST(corrector)
CERES_TEST(cost_function_to_functor)
CERES_TEST(covariance)
+ CERES_TEST(cubic_interpolation)
CERES_TEST(dense_sparse_matrix)
CERES_TEST(dynamic_autodiff_cost_function)
CERES_TEST(dynamic_compressed_row_sparse_matrix)
diff --git a/internal/ceres/cubic_interpolation.cc b/internal/ceres/cubic_interpolation.cc
new file mode 100644
index 0000000..9dd6194
--- /dev/null
+++ b/internal/ceres/cubic_interpolation.cc
@@ -0,0 +1,98 @@
+// 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)
+//
+// This implementation was inspired by the description at
+// http://www.paulinternet.nl/?page=bicubic
+
+#include "ceres/cubic_interpolation.h"
+
+#include <math.h>
+#include "glog/logging.h"
+
+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) {
+ 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) {
+ 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);
+ CatmullRomSpline(p0, p1, p2, p3, x - n, f, dfdx);
+ return true;
+}
+
+} // namespace ceres
diff --git a/internal/ceres/cubic_interpolation_test.cc b/internal/ceres/cubic_interpolation_test.cc
new file mode 100644
index 0000000..04aacc0
--- /dev/null
+++ b/internal/ceres/cubic_interpolation_test.cc
@@ -0,0 +1,131 @@
+// 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 "ceres/jet.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;
+
+class CubicInterpolatorTest : public ::testing::Test {
+ public:
+
+ void RunPolynomialInterpolationTest(const double a,
+ const double b,
+ const double c,
+ const double d) {
+ for (int x = 0; x < kNumSamples; ++x) {
+ values_[x] = a * x * x * x + b * x * x + c * x + d;
+ }
+
+ CubicInterpolator interpolator(values_, kNumSamples);
+
+ // Check values in the all the cells but the first and the last
+ // ones. In these cells, the interpolated function values should
+ // match exactly the values of the function being interpolated.
+ //
+ // On the boundary, we extrapolate the values of the function on
+ // the basis of its first derivative, so we do not expect the
+ // 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;
+
+ 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;
+ }
+ }
+
+ static const int kNumSamples = 10;
+ static const int kNumTestSamples = 100;
+ double values_[kNumSamples];
+};
+
+TEST_F(CubicInterpolatorTest, ConstantFunction) {
+ RunPolynomialInterpolationTest(0.0, 0.0, 0.0, 0.5);
+}
+
+TEST_F(CubicInterpolatorTest, LinearFunction) {
+ RunPolynomialInterpolationTest(0.0, 0.0, 1.0, 0.5);
+}
+
+TEST_F(CubicInterpolatorTest, QuadraticFunction) {
+ RunPolynomialInterpolationTest(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 x = 2.5;
+ EXPECT_TRUE(interpolator.Evaluate(x, &f, &dfdx));
+
+ // 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> output_jet;
+ EXPECT_TRUE(interpolator.Evaluate(input_jet, &output_jet));
+
+ // Check that the scalar part of the Jet is f(x).
+ EXPECT_EQ(output_jet.a, f);
+
+ // Check that the derivative part of the Jet is dfdx * input_jet.v
+ // by the chain rule.
+ EXPECT_EQ((output_jet.v - dfdx * input_jet.v).norm(), 0.0);
+}
+
+} // namespace internal
+} // namespace ceres
