saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

A complete re-write of the cubic interpolation code.

Browse Source 
The key change is that there is a new layer of abstract,
a Array object that the interpolator depends on.

The Array provides a one dimension or two dimensional
array like interface independent of the underlying representation
of the data.

Also included here is support for vector valued functions.

Change-Id: Ica68f03778cf0d84192db00cd55653f8b4124d51
This commit is contained in:
Sameer Agarwal
2015-02-05 06:29:27 -08:00
parent 264bf456df
commit 3125c23228
4 changed files with 667 additions and 372 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/cubic_interpolation.h
+442 -52
View File
@@ -28,25 +28,93 @@
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
#include "ceres/internal/port.h"
#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_
#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
#include "ceres/internal/port.h"
#include "Eigen/Core"
#include "glog/logging.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 cubic Hermite splines to
// produce a smooth approximation to it that can be used to evaluate
// the f(x) and f'(x) at any fractional point in the interval [0,
// n-1].
// Given samples from a function sampled at four equally spaced points,
//
// 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.
// 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
//
// f if not NULL will contain the interpolated function values.
// dfdx if not NULL will contain the interpolated derivative values.
template <int kDataDimension>
void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0,
const Eigen::Matrix<double, kDataDimension, 1>& p1,
const Eigen::Matrix<double, kDataDimension, 1>& p2,
const Eigen::Matrix<double, kDataDimension, 1>& p3,
const double x,
double* f,
double* dfdx) {
DCHECK_GE(x, 0.0);
DCHECK_LE(x, 1.0);
typedef Eigen::Matrix<double, kDataDimension, 1> VType;
const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3);
const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3);
const VType c = 0.5 * (-p0 + p2);
const VType d = p1;
// Use Horner's rule to evaluate the function value and its
// derivative.
// f = ax^3 + bx^2 + cx + d
if (f != NULL) {
Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
}
// dfdx = 3ax^2 + 2bx + c
if (dfdx != NULL) {
Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
}
}
// Given as input a one dimensional array like object, which provides
// the following interface.
//
// struct Array {
// enum { DATA_DIMENSION = 2; };
// void GetValue(int n, double* f) const;
// int NumValues() const;
// };
//
// Where, GetValue gives us the value of a function f (possibly vector
// valued) on the integers:
//
// [0, ..., NumValues() - 1].
//
// and the enum DATA_DIMENSION indicates the dimensionality of the
// function being interpolated. For example if you are interpolating a
// color image with three channels (Red, Green & Blue), then
// DATA_DIMENSION = 3.
//
// CubicInterpolator 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 real valued point in the interval:
//
// [0, NumValues() - 1].
//
// For more details on cubic interpolation see
//
@@ -55,20 +123,63 @@ namespace ceres {
// Example usage:
//
// const double x[] = {1.0, 2.0, 5.0, 6.0};
// CubicInterpolator interpolator(x, 4);
// Array1D data(x, 4);
// CubicInterpolator interpolator(data);
// double f, dfdx;
// CHECK(interpolator.Evaluator(1.5, &f, &dfdx));
template<typename Array>
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);
explicit CubicInterpolator(const Array& array)
: array_(array) {
CHECK_GT(array.NumValues(), 1);
// The + casts the enum into an int before doing the
// comparison. It is needed to prevent
// "-Wunnamed-type-template-args" related errors.
CHECK_GE(+Array::DATA_DIMENSION, 1);
}
// 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;
bool Evaluate(double x, double* f, double* dfdx) const {
const int num_values = array_.NumValues();
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);
// Deal with the case where the point sits exactly on the right
// boundary.
if (n == num_values - 1) {
n -= 1;
}
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p0, p1, p2, p3;
// The point being evaluated is now expected to lie in the
// internal corresponding to p1 and p2.
array_.GetValue(n, p1.data());
array_.GetValue(n + 1, p2.data());
// If we are at n >=1, the choose the element at n - 1, otherwise
// linearly interpolate from p1 and p2.
if (n > 0) {
array_.GetValue(n - 1, p0.data());
} else {
p0 = 2 * p1 - p2;
}
// If we are at n < num_values_ - 2, then choose the element n +
// 2, otherwise linearly interpolate from p1 and p2.
if (n < num_values - 2) {
array_.GetValue(n + 2, p3.data());
} else {
p3 = 2 * p2 - p1;
}
CubicHermiteSpline(p0, p1, p2, p3, x - n, f, dfdx);
return true;
}
// The following two Evaluate overloads are needed for interfacing
// with automatic differentiation. The first is for when a scalar
@@ -78,50 +189,223 @@ class CERES_EXPORT CubicInterpolator {
}
template<typename JetT> bool Evaluate(const JetT& x, JetT* f) const {
double dfdx;
if (!Evaluate(x.a, &f->a, &dfdx)) {
double fx[Array::DATA_DIMENSION], dfdx[Array::DATA_DIMENSION];
if (!Evaluate(x.a, fx, dfdx)) {
return false;
}
f->v = dfdx * x.v;
for (int i = 0; i < Array::DATA_DIMENSION; ++i) {
f[i].a = fx[i];
f[i].v = dfdx[i] * x.v;
}
return true;
}
int num_values() const { return num_values_; }
int NumValues() const { return array_.NumValues(); }
private:
const double* values_;
const int num_values_;
private:
const Array& array_;
};
// 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.
// Given as input a two dimensional array like object, which provides
// the following interface:
//
// For more details on cubic interpolation see
// struct Array {
// enum { DATA_DIMENSION = 1 };
// void GetValue(int row, int col, double* f) const;
// int NumRows() const;
// int NumCols() const;
// };
//
// Where, GetValue gives us the value of a function f (possibly vector
// valued) on the integer grid:
//
// [0, ..., NumRows() - 1] x [0, ..., NumCols() - 1]
//
// and the enum DATA_DIMENSION indicates the dimensionality of the
// function being interpolated. For example if you are interpolating a
// color image with three channels (Red, Green & Blue), then
// DATA_DIMENSION = 3.
//
// BiCubicInterpolator 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 real valued point in the quad:
//
// [0, NumRows() - 1] x [0, NumCols() - 1]
//
// For more details on the algorithm used here see:
//
// "Cubic convolution interpolation for digital image processing".
// IEEE Transactions on Acoustics, Speech, and Signal Processing
// 29 (6): 1153–1160.
// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal
// Processing 29 (6): 1153–1160, 1981.
//
// http://en.wikipedia.org/wiki/Cubic_Hermite_spline
// http://en.wikipedia.org/wiki/Bicubic_interpolation
template<typename Array>
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);
BiCubicInterpolator(const Array& array)
: array_(array) {
CHECK_GT(array.NumRows(), 1);
CHECK_GT(array.NumCols(), 1);
// The + casts the enum into an int before doing the
// comparison. It is needed to prevent
// "-Wunnamed-type-template-args" related errors.
CHECK_GE(+Array::DATA_DIMENSION, 1);
}
// 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;
double* f, double* dfdr, double* dfdc) const {
const int num_rows = array_.NumRows();
const int num_cols = array_.NumCols();
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;
}
// 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.
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p00, p01, p02, p03;
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p10, p11, p12, p13;
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p20, p21, p22, p23;
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> p30, p31, p32, p33;
array_.GetValue(row, col, p11.data());
array_.GetValue(row, col + 1, p12.data());
array_.GetValue(row + 1, col, p21.data());
array_.GetValue(row + 1, col + 1, p22.data());
// If we are in rows >= 1, then choose the element from the row - 1,
// otherwise linearly interpolate from row and row + 1.
if (row > 0) {
array_.GetValue(row - 1, col, p01.data());
array_.GetValue(row - 1, col + 1, p02.data());
} else {
p01 = 2 * p11 - p21;
p02 = 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.
if (row < num_rows - 2) {
array_.GetValue(row + 2, col, p31.data());
array_.GetValue(row + 2, col + 1, p32.data());
} else {
p31 = 2 * p21 - p22;
p32 = 2 * p22 - p12;
}
// Same logic as above, applies to the columns instead of rows.
if (col > 0) {
array_.GetValue(row, col - 1, p10.data());
array_.GetValue(row + 1, col - 1, p20.data());
} else {
p10 = 2 * p11 - p12;
p20 = 2 * p21 - p22;
}
if (col < num_cols - 2) {
array_.GetValue(row, col + 2, p13.data());
array_.GetValue(row + 1, col + 2, p23.data());
} else {
p13 = 2 * p12 - p11;
p23 = 2 * p22 - p21;
}
// 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 > 0 : Interpolate p10 & p20
// row > 0, col = 0 : Interpolate p01 & p02
// row = 0, col = 0 : Interpolate p10 & p20, or p01 & p02.
if (row > 0) {
if (col > 0) {
array_.GetValue(row - 1, col - 1, p00.data());
} else {
p00 = 2 * p01 - p02;
}
if (col < num_cols - 2) {
array_.GetValue(row - 1, col + 2, p03.data());
} else {
p03 = 2 * p02 - p01;
}
} else {
p00 = 2 * p10 - p20;
p03 = 2 * p13 - p23;
}
if (row < num_rows - 2) {
if (col > 0) {
array_.GetValue(row + 2, col - 1, p30.data());
} else {
p30 = 2 * p31 - p32;
}
if (col < num_cols - 2) {
array_.GetValue(row + 2, col + 2, p33.data());
} 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.
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> f0, f1, f2, f3;
Eigen::Matrix<double, Array::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
CubicHermiteSpline(p00, p01, p02, p03, c - col, f0.data(), df0dc.data());
CubicHermiteSpline(p10, p11, p12, p13, c - col, f1.data(), df1dc.data());
CubicHermiteSpline(p20, p21, p22, p23, c - col, f2.data(), df2dc.data());
CubicHermiteSpline(p30, p31, p32, p33, c - col, f3.data(), df3dc.data());
// 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;
}
// The following two Evaluate overloads are needed for interfacing
// with automatic differentiation. The first is for when a scalar
@@ -133,19 +417,125 @@ class CERES_EXPORT BiCubicInterpolator {
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)) {
double frc[Array::DATA_DIMENSION];
double dfdr[Array::DATA_DIMENSION];
double dfdc[Array::DATA_DIMENSION];
if (!Evaluate(r.a, c.a, frc, dfdr, dfdc)) {
return false;
}
f->v = dfdr * r.v + dfdc * c.v;
for (int i = 0; i < Array::DATA_DIMENSION; ++i) {
f[i].a = frc[i];
f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;
}
return true;
}
int num_rows() const { return num_rows_; }
int num_cols() const { return num_cols_; }
int NumRows() const { return array_.NumRows(); }
int NumCols() const { return array_.NumCols(); }
private:
const double* values_;
const Array& array_;
};
// An object that implements the one dimensional array like object
// needed by the CubicInterpolator where the source of the function
// values is an array of type T.
//
// The function being provided can be vector valued, in which case
// kDataDimension > 1. The dimensional slices of the function maybe
// interleaved, or they maybe stacked, i.e, if the function has
// kDataDimension = 2, if kInterleaved = true, then it is stored as
//
// f01, f02, f11, f12 ....
//
// and if kInterleaved = false, then it is stored as
//
// f01, f11, .. fn1, f02, f12, .. , fn2
template <typename T, int kDataDimension = 1, bool kInterleaved = true>
struct Array1D {
enum { DATA_DIMENSION = kDataDimension };
Array1D(const T* data, const int num_values)
: data_(data), num_values_(num_values) {
}
void GetValue(const int n, double* f) const {
if (n < 0 || n > num_values_ - 1) {
LOG(FATAL) << "n = " << n
<< " is not in the interval [0, " << num_values_ - 1 << "].";
}
for (int i = 0; i < kDataDimension; ++i) {
if (kInterleaved) {
f[i] = static_cast<double>(data_[kDataDimension * n + i]);
} else {
f[i] = static_cast<double>(data_[i * num_values_ + n]);
}
}
}
int NumValues() const { return num_values_; }
private:
const T* data_;
const int num_values_;
};
// An object that implements the two dimensional array like object
// needed by the BiCubicInterpolator where the source of the function
// values is an array of type T.
//
// The function being provided can be vector valued, in which case
// kDataDimension > 1. The data maybe stored in row or column major
// format and the various dimensional slices of the function maybe
// interleaved, or they maybe stacked, i.e, if the function has
// kDataDimension = 2, is stored in row-major format and if
// kInterleaved = true, then it is stored as
//
// f001, f002, f011, f012, ...
//
// A commonly occuring example are color images (RGB) where the three
// channels are stored interleaved.
//
// If kInterleaved = false, then it is stored as
//
// f001, f011, ..., fnm1, f002, f012, ...
template <typename T,
int kDataDimension = 1,
bool kRowMajor = true,
bool kInterleaved = true>
struct Array2D {
enum Foo { DATA_DIMENSION = kDataDimension };
Array2D(const T* data, const int num_rows, const int num_cols)
: data_(data), num_rows_(num_rows), num_cols_(num_cols) {
CHECK_GE(kDataDimension, 1);
}
void GetValue(const int r, const int c, double* f) const {
if (r < 0 || r > num_rows_ - 1 || c < 0 || c > num_cols_ - 1) {
LOG(FATAL) << "(r, c) = (" << r << ", " << c << ")"
<< " is not in the square defined by [0, 0] "
<< " and [" << num_rows_ - 1 << ", " << num_cols_ - 1 << "]";
}
const int n = (kRowMajor) ? num_cols_ * r + c : num_rows_ * c + r;
for (int i = 0; i < kDataDimension; ++i) {
if (kInterleaved) {
f[i] = static_cast<double>(data_[kDataDimension * n + i]);
} else {
f[i] = static_cast<double>(data_[i * (num_rows_ * num_cols_) + n]);
}
}
}
int NumRows() const { return num_rows_; }
int NumCols() const { return num_cols_; }
private:
const T* data_;
const int num_rows_;
const int num_cols_;
};
internal/ceres/CMakeLists.txt
-1
View File
@@ -53,7 +53,6 @@ SET(CERES_INTERNAL_SRC
corrector.cc
covariance.cc
covariance_impl.cc
cubic_interpolation.cc
cxsparse.cc
dense_normal_cholesky_solver.cc
dense_qr_solver.cc
internal/ceres/cubic_interpolation.cc
-258
View File
@@ -1,258 +0,0 @@
// 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
internal/ceres/cubic_interpolation_test.cc
+225 -61
View File
@@ -31,31 +31,147 @@
#include "ceres/cubic_interpolation.h"
#include "ceres/jet.h"
#include "ceres/internal/scoped_ptr.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;
TEST(Array1D, OneDataDimension) {
int x[] = {1, 2, 3};
Array1D<int, 1> array(x, 3);
for (int i = 0; i < 3; ++i) {
double value;
array.GetValue(i, &value);
EXPECT_EQ(value, static_cast<double>(i + 1));
}
}
TEST(Array1D, TwoDataDimensionIntegerDataInterleaved) {
int x[] = {1, 5,
2, 6,
3, 7};
Array1D<int, 2, true> array(x, 3);
for (int i = 0; i < 3; ++i) {
double value[2];
array.GetValue(i, value);
EXPECT_EQ(value[0], static_cast<double>(i + 1));
EXPECT_EQ(value[1], static_cast<double>(i + 5));
}
}
TEST(Array1D, TwoDataDimensionIntegerDataStacked) {
int x[] = {1, 2, 3,
5, 6, 7};
Array1D<int, 2, false> array(x, 3);
for (int i = 0; i < 3; ++i) {
double value[2];
array.GetValue(i, value);
EXPECT_EQ(value[0], static_cast<double>(i + 1));
EXPECT_EQ(value[1], static_cast<double>(i + 5));
}
}
TEST(Array2D, OneDataDimensionRowMajor) {
int x[] = {1, 2, 3,
2, 3, 4};
Array2D<int, 1, true, true> array(x, 2, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value;
array.GetValue(r, c, &value);
EXPECT_EQ(value, static_cast<double>(r + c + 1));
}
}
}
TEST(Array2D, TwoDataDimensionRowMajorInterleaved) {
int x[] = {1, 4, 2, 8, 3, 12,
2, 8, 3, 12, 4, 16};
Array2D<int, 2, true, true> array(x, 2, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
array.GetValue(r, c, value);
EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
}
}
}
TEST(Array2D, TwoDataDimensionRowMajorStacked) {
int x[] = {1, 2, 3,
2, 3, 4,
4, 8, 12,
8, 12, 16};
Array2D<int, 2, true, false> array(x, 2, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
array.GetValue(r, c, value);
EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
}
}
}
TEST(Array2D, TwoDataDimensionColMajorInterleaved) {
int x[] = { 1, 4, 2, 8,
2, 8, 3, 12,
3, 12, 4, 16};
Array2D<int, 2, false, true> array(x, 2, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
array.GetValue(r, c, value);
EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
}
}
}
TEST(Array2D, TwoDataDimensionColMajorStacked) {
int x[] = {1, 2,
2, 3,
3, 4,
4, 8,
8, 12,
12, 16};
Array2D<int, 2, false, false> array(x, 2, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
array.GetValue(r, c, value);
EXPECT_EQ(value[0], static_cast<double>(r + c + 1));
EXPECT_EQ(value[1], static_cast<double>(4 *(r + c + 1)));
}
}
}
static const double kTolerance = 1e-12;
class CubicInterpolatorTest : public ::testing::Test {
public:
template <int kDataDimension>
void RunPolynomialInterpolationTest(const double a,
const double b,
const double c,
const double d) {
values_.reset(new double[kDataDimension * kNumSamples]);
for (int x = 0; x < kNumSamples; ++x) {
values_[x] = a * x * x * x + b * x * x + c * x + d;
for (int dim = 0; dim < kDataDimension; ++dim) {
values_[x * kDataDimension + dim] =
(dim * dim + 1) * (a * x * x * x + b * x * x + c * x + d);
}
}
CubicInterpolator interpolator(values_, kNumSamples);
Array1D<double, kDataDimension> array(values_.get(), kNumSamples);
CubicInterpolator<Array1D<double, kDataDimension> > interpolator(array);
// Check values in the all the cells but the first and the last
// ones. In these cells, the interpolated function values should
@@ -66,46 +182,63 @@ class CubicInterpolatorTest : public ::testing::Test {
// 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;
double expected_f[kDataDimension], expected_dfdx[kDataDimension];
double f[kDataDimension], dfdx[kDataDimension];
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;
for (int dim = 0; dim < kDataDimension; ++dim) {
expected_f[dim] =
(dim * dim + 1) * (a * x * x * x + b * x * x + c * x + d);
expected_dfdx[dim] = (dim * dim + 1) * (3.0 * a * x * x + 2.0 * b * x + c);
}
EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx));
for (int dim = 0; dim < kDataDimension; ++dim) {
EXPECT_NEAR(f[dim], expected_f[dim], kTolerance)
<< "x: " << x << " dim: " << dim
<< " actual f(x): " << expected_f[dim]
<< " estimated f(x): " << f[dim];
EXPECT_NEAR(dfdx[dim], expected_dfdx[dim], kTolerance)
<< "x: " << x << " dim: " << dim
<< " actual df(x)/dx: " << expected_dfdx[dim]
<< " estimated df(x)/dx: " << dfdx[dim];
}
}
}
private:
static const int kNumSamples = 10;
static const int kNumTestSamples = 100;
double values_[kNumSamples];
scoped_array<double> values_;
};
TEST_F(CubicInterpolatorTest, ConstantFunction) {
RunPolynomialInterpolationTest(0.0, 0.0, 0.0, 0.5);
RunPolynomialInterpolationTest<1>(0.0, 0.0, 0.0, 0.5);
RunPolynomialInterpolationTest<2>(0.0, 0.0, 0.0, 0.5);
RunPolynomialInterpolationTest<3>(0.0, 0.0, 0.0, 0.5);
}
TEST_F(CubicInterpolatorTest, LinearFunction) {
RunPolynomialInterpolationTest(0.0, 0.0, 1.0, 0.5);
RunPolynomialInterpolationTest<1>(0.0, 0.0, 1.0, 0.5);
RunPolynomialInterpolationTest<2>(0.0, 0.0, 1.0, 0.5);
RunPolynomialInterpolationTest<3>(0.0, 0.0, 1.0, 0.5);
}
TEST_F(CubicInterpolatorTest, QuadraticFunction) {
RunPolynomialInterpolationTest(0.0, 0.4, 1.0, 0.5);
RunPolynomialInterpolationTest<1>(0.0, 0.4, 1.0, 0.5);
RunPolynomialInterpolationTest<2>(0.0, 0.4, 1.0, 0.5);
RunPolynomialInterpolationTest<3>(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 values[] = {1.0, 2.0, 2.0, 5.0, 3.0, 9.0, 2.0, 7.0};
Array1D<double, 2, true> array(values, 4);
CubicInterpolator<Array1D<double, 2, true> > interpolator(array);
double f[2], dfdx[2];
const double x = 2.5;
EXPECT_TRUE(interpolator.Evaluate(x, &f, &dfdx));
EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx));
// Create a Jet with the same scalar part as x, so that the output
// Jet will be evaluated at x.
@@ -116,42 +249,48 @@ TEST(CubicInterpolator, JetEvaluation) {
x_jet.v(2) = 1.2;
x_jet.v(3) = 1.3;
Jet<double, 4> f_jet;
EXPECT_TRUE(interpolator.Evaluate(x_jet, &f_jet));
Jet<double, 4> f_jets[2];
EXPECT_TRUE(interpolator.Evaluate(x_jet, f_jets));
// Check that the scalar part of the Jet is f(x).
EXPECT_EQ(f_jet.a, f);
EXPECT_EQ(f_jets[0].a, f[0]);
EXPECT_EQ(f_jets[1].a, f[1]);
// Check that the derivative part of the Jet is dfdx * x_jet.v
// by the chain rule.
EXPECT_EQ((f_jet.v - dfdx * x_jet.v).norm(), 0.0);
EXPECT_NEAR((f_jets[0].v - dfdx[0] * x_jet.v).norm(), 0.0, kTolerance);
EXPECT_NEAR((f_jets[1].v - dfdx[1] * x_jet.v).norm(), 0.0, kTolerance);
}
class BiCubicInterpolatorTest : public ::testing::Test {
public:
template <int kDataDimension>
void RunPolynomialInterpolationTest(const Eigen::Matrix3d& coeff) {
values_.reset(new double[kNumRows * kNumCols * kDataDimension]);
coeff_ = coeff;
double* v = values_;
double* v = values_.get();
for (int r = 0; r < kNumRows; ++r) {
for (int c = 0; c < kNumCols; ++c) {
*v++ = EvaluateF(r, c);
for (int dim = 0; dim < kDataDimension; ++dim) {
*v++ = (dim * dim + 1) * EvaluateF(r, c);
}
}
}
BiCubicInterpolator interpolator(values_, kNumRows, kNumCols);
Array2D<double, kDataDimension> array(values_.get(), kNumRows, kNumCols);
BiCubicInterpolator<Array2D<double, kDataDimension> > interpolator(array);
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);
double f[kDataDimension], dfdr[kDataDimension], dfdc[kDataDimension];
EXPECT_TRUE(interpolator.Evaluate(r, c, f, dfdr, dfdc));
for (int dim = 0; dim < kDataDimension; ++dim) {
EXPECT_NEAR(f[dim], (dim * dim + 1) * EvaluateF(r, c), kTolerance);
EXPECT_NEAR(dfdr[dim], (dim * dim + 1) * EvaluatedFdr(r, c), kTolerance);
EXPECT_NEAR(dfdc[dim], (dim * dim + 1) * EvaluatedFdc(r, c), kTolerance);
}
}
}
}
@@ -187,18 +326,22 @@ class BiCubicInterpolatorTest : public ::testing::Test {
static const int kNumCols = 10;
static const int kNumRowSamples = 100;
static const int kNumColSamples = 100;
double values_[kNumRows * kNumCols];
scoped_array<double> values_;
};
TEST_F(BiCubicInterpolatorTest, ZeroFunction) {
Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree00Function) {
Eigen::Matrix3d coeff = Eigen::Matrix3d::Zero();
coeff(2, 2) = 1.0;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree01Function) {
@@ -206,7 +349,9 @@ TEST_F(BiCubicInterpolatorTest, Degree01Function) {
coeff(2, 2) = 1.0;
coeff(0, 2) = 0.1;
coeff(2, 0) = 0.1;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree10Function) {
@@ -214,7 +359,9 @@ TEST_F(BiCubicInterpolatorTest, Degree10Function) {
coeff(2, 2) = 1.0;
coeff(0, 1) = 0.1;
coeff(1, 0) = 0.1;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree11Function) {
@@ -224,7 +371,9 @@ TEST_F(BiCubicInterpolatorTest, Degree11Function) {
coeff(1, 0) = 0.1;
coeff(0, 2) = 0.2;
coeff(2, 0) = 0.2;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree12Function) {
@@ -235,7 +384,9 @@ TEST_F(BiCubicInterpolatorTest, Degree12Function) {
coeff(0, 2) = 0.2;
coeff(2, 0) = 0.2;
coeff(1, 1) = 0.3;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree21Function) {
@@ -246,7 +397,9 @@ TEST_F(BiCubicInterpolatorTest, Degree21Function) {
coeff(0, 2) = 0.2;
coeff(2, 0) = 0.2;
coeff(0, 0) = 0.3;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(coeff);
}
TEST_F(BiCubicInterpolatorTest, Degree22Function) {
@@ -259,17 +412,22 @@ TEST_F(BiCubicInterpolatorTest, Degree22Function) {
coeff(0, 0) = 0.3;
coeff(0, 1) = -0.4;
coeff(1, 0) = -0.4;
RunPolynomialInterpolationTest(coeff);
RunPolynomialInterpolationTest<1>(coeff);
RunPolynomialInterpolationTest<2>(coeff);
RunPolynomialInterpolationTest<3>(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 values[] = {1.0, 5.0, 2.0, 10.0, 2.0, 6.0, 3.0, 5.0,
1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 3.0, 1.0};
Array2D<double, 2> array(values, 2, 4);
BiCubicInterpolator<Array2D<double, 2> > interpolator(array);
double f[2], dfdr[2], dfdc[2];
const double r = 0.5;
const double c = 2.5;
EXPECT_TRUE(interpolator.Evaluate(r, c, &f, &dfdr, &dfdc));
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 evaluated at x.
@@ -287,10 +445,16 @@ TEST(BiCubicInterpolator, JetEvaluation) {
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);
Jet<double, 4> f_jets[2];
EXPECT_TRUE(interpolator.Evaluate(r_jet, c_jet, f_jets));
EXPECT_EQ(f_jets[0].a, f[0]);
EXPECT_EQ(f_jets[1].a, f[1]);
EXPECT_NEAR((f_jets[0].v - dfdr[0] * r_jet.v - dfdc[0] * c_jet.v).norm(),
0.0,
kTolerance);
EXPECT_NEAR((f_jets[1].v - dfdr[1] * r_jet.v - dfdc[1] * c_jet.v).norm(),
0.0,
kTolerance);
}
} // namespace internal