internal/ceres/cubic_interpolation.cc - ceres-solver - Git at Google

 // 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 <math.h>
 #include "glog/logging.h"

 namespace ceres {
 namespace {

 // 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 (for the case of a = 0.5) as
 // 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 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);
   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) {
     LOG(ERROR) << "x =  " << x
                << " is not in the interval [0, " << 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);
   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 three 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
	// 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 <math.h>
	#include "glog/logging.h"

	namespace ceres {
	namespace {

	// 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 (for the case of a = 0.5) as
	// 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 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);
	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) {
	LOG(ERROR) << "x = " << x
	<< " is not in the interval [0, " << 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);
	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 three 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