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