A refactor of the cubic interpolation code
1. Push the boundary handling logic into the underlying array
object. This has two very significant impacts:
a. The interpolation code becomes extremely simple to write
and to test.
b. The user has more flexibility in implementing how out of bounds
values are handled. We provide one default implementation.
Change-Id: Ic2f6cf9257ce7110c62e492688e5a6c8be1e7df2
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index b1c3a2b..408544d 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1846,75 +1846,70 @@
.. class:: CubicInterpolator
-Given as input a one dimensional array like object, which provides
-the following interface.
+Given as input an infinite one dimensional grid, which provides the
+following interface.
.. code::
- struct Array {
+ struct Grid1D {
enum { DATA_DIMENSION = 2; };
void GetValue(int n, double* f) const;
- int NumValues() const;
};
Where, ``GetValue`` gives us the value of a function :math:`f`
-(possibly vector valued) on the integers :code:`{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``.
+(possibly vector valued) for any integer :math:`n` and the enum
+``DATA_DIMENSION`` indicates the dimensionality of the function being
+interpolated. For example if you are interpolating rotations in
+axis-angle format over time, then ``DATA_DIMENSION = 3``.
:class:`CubicInterpolator` uses Cubic Hermite splines to produce a
smooth approximation to it that can be used to evaluate the
-:math:`f(x)` and :math:`f'(x)` at any real valued point in the
-interval :code:`[0, NumValues() - 1]`. For example, the following code
-interpolates an array of four numbers.
+:math:`f(x)` and :math:`f'(x)` at any point on the real number
+line. For example, the following code interpolates an array of four
+numbers.
.. code::
const double data[] = {1.0, 2.0, 5.0, 6.0};
- Array1D<double, 1> array(x, 4);
+ Grid1D<double, 1> array(x, 0, 4);
CubicInterpolator interpolator(array);
double f, dfdx;
- CHECK(interpolator.Evaluate(1.5, &f, &dfdx));
+ interpolator.Evaluate(1.5, &f, &dfdx);
-In the above code we use ``Array1D`` a templated helper class that
+In the above code we use ``Grid1D`` a templated helper class that
allows easy interfacing between ``C++`` arrays and
:class:`CubicInterpolator`.
-``Array1D`` supports vector valued functions where the various
+``Grid1D`` supports vector valued functions where the various
coordinates of the function can be interleaved or stacked. It also
allows the use of any numeric type as input, as long as it can be
safely cast to a double.
.. class:: BiCubicInterpolator
-Given as input a two dimensional array like object, which provides
-the following interface:
+Given as input an infinite two dimensional grid, which provides the
+following interface:
.. code::
- struct Array {
- enum { DATA_DIMENSION = 1 };
+ struct Grid2D {
+ enum { DATA_DIMENSION = 2 };
void GetValue(int row, int col, double* f) const;
- int NumRows() const;
- int NumCols() const;
};
Where, ``GetValue`` gives us the value of a function :math:`f`
-(possibly vector valued) on the integer grid :code:`{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``.
+(possibly vector valued) for any pair of integers :code:`row` and
+:code:`col` 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``.
:class:`BiCubicInterpolator` uses the cubic convolution interpolation
algorithm of R. Keys [Keys]_, to produce a smooth approximation to it
that can be used to evaluate the :math:`f(r,c)`, :math:`\frac{\partial
f(r,c)}{\partial r}` and :math:`\frac{\partial f(r,c)}{\partial c}` at
-any real valued point in the quad :code:`[0, NumRows() - 1] x [0,
-NumCols() - 1]`.
+any any point in the real plane.
For example the following code interpolates a two dimensional array.
@@ -1923,16 +1918,16 @@
const double data[] = {1.0, 3.0, -1.0, 4.0,
3.6, 2.1, 4.2, 2.0,
2.0, 1.0, 3.1, 5.2};
- Array2D<double, 1> array(data, 3, 4);
+ Grid2D<double, 1> array(data, 0, 3, 0, 4);
BiCubicInterpolator interpolator(array);
double f, dfdr, dfdc;
- CHECK(interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc));
+ interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);
-In the above code, the templated helper class ``Array2D`` is used to
+In the above code, the templated helper class ``Grid2D`` is used to
make a ``C++`` array look like a two dimensional table to
:class:`BiCubicInterpolator`.
-``Array2D`` supports row or column major layouts. It also supports
+``Grid2D`` supports row or column major layouts. It also supports
vector valued functions where the individual coordinates of the
function may be interleaved or stacked. It also allows the use of any
numeric type as input, as long as it can be safely cast to double.
diff --git a/examples/sampled_function.cc b/examples/sampled_function.cc
index ada8a4a..093276a 100644
--- a/examples/sampled_function.cc
+++ b/examples/sampled_function.cc
@@ -35,7 +35,7 @@
#include "ceres/cubic_interpolation.h"
#include "glog/logging.h"
-using ceres::Array1D;
+using ceres::Grid1D;
using ceres::CubicInterpolator;
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
@@ -47,22 +47,23 @@
// values with automatic differentiation.
struct InterpolatedCostFunctor {
explicit InterpolatedCostFunctor(
- const CubicInterpolator<Array1D<double> >& interpolator)
+ const CubicInterpolator<Grid1D<double> >& interpolator)
: interpolator_(interpolator) {
}
template<typename T> bool operator()(const T* x, T* residuals) const {
- return interpolator_.Evaluate(*x, residuals);
+ interpolator_.Evaluate(*x, residuals);
+ return true;
}
static CostFunction* Create(
- const CubicInterpolator<Array1D<double> >& interpolator) {
+ const CubicInterpolator<Grid1D<double> >& interpolator) {
return new AutoDiffCostFunction<InterpolatedCostFunctor, 1, 1>(
new InterpolatedCostFunctor(interpolator));
}
private:
- const CubicInterpolator<Array1D<double> >& interpolator_;
+ const CubicInterpolator<Grid1D<double> >& interpolator_;
};
int main(int argc, char** argv) {
@@ -75,8 +76,8 @@
values[i] = (i - 4.5) * (i - 4.5);
}
- Array1D<double> array(values, kNumSamples);
- CubicInterpolator<Array1D<double> > interpolator(array);
+ Grid1D<double> array(values, 0, kNumSamples);
+ CubicInterpolator<Grid1D<double> > interpolator(array);
double x = 1.0;
Problem problem;
diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h
index 9e63d01..41bf627 100644
--- a/include/ceres/cubic_interpolation.h
+++ b/include/ceres/cubic_interpolation.h
@@ -91,30 +91,25 @@
}
}
-// Given as input a one dimensional array like object, which provides
-// the following interface.
+// Given as input an infinite one dimensional grid, which provides the
+// following interface.
//
-// struct Array {
+// class Grid {
+// public:
// 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:
+// Here, GetValue gives the value of a function f (possibly vector
+// valued) for any integer n.
//
-// [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.
+// The enum DATA_DIMENSION indicates the dimensionality of the
+// function being interpolated. For example if you are interpolating
+// rotations in axis-angle format over time, 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].
+// at any point on the real number line.
//
// For more details on cubic interpolation see
//
@@ -123,117 +118,122 @@
// Example usage:
//
// const double data[] = {1.0, 2.0, 5.0, 6.0};
-// Array1D<double, 1> array(x, 4);
-// CubicInterpolator<Array1D<double, 1> > interpolator(array);
+// Grid1D<double, 1> grid(x, 0, 4);
+// CubicInterpolator<Grid1D<double, 1> > interpolator(grid);
// double f, dfdx;
-// CHECK(interpolator.Evaluator(1.5, &f, &dfdx));
-template<typename Array>
+// interpolator.Evaluator(1.5, &f, &dfdx);
+template<typename Grid>
class CERES_EXPORT CubicInterpolator {
public:
- explicit CubicInterpolator(const Array& array)
- : array_(array) {
- CHECK_GT(array.NumValues(), 1);
+ explicit CubicInterpolator(const Grid& grid)
+ : grid_(grid) {
// 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);
+ CHECK_GE(+Grid::DATA_DIMENSION, 1);
}
- 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<Array::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);
-
- return true;
+ void Evaluate(double x, double* f, double* dfdx) const {
+ const int n = std::floor(x);
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
+ grid_.GetValue(n - 1, p0.data());
+ grid_.GetValue(n, p1.data());
+ grid_.GetValue(n + 1, p2.data());
+ grid_.GetValue(n + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);
}
// The following two Evaluate overloads are needed for interfacing
// with automatic differentiation. The first is for when a scalar
// evaluation is done, and the second one is for when Jets are used.
- bool Evaluate(const double& x, double* f) const {
- return Evaluate(x, f, NULL);
+ void Evaluate(const double& x, double* f) const {
+ Evaluate(x, f, NULL);
}
- template<typename JetT> bool Evaluate(const JetT& x, JetT* f) const {
- double fx[Array::DATA_DIMENSION], dfdx[Array::DATA_DIMENSION];
- if (!Evaluate(x.a, fx, dfdx)) {
- return false;
- }
-
- for (int i = 0; i < Array::DATA_DIMENSION; ++i) {
+ template<typename JetT> void Evaluate(const JetT& x, JetT* f) const {
+ double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION];
+ Evaluate(x.a, fx, dfdx);
+ for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
f[i].a = fx[i];
f[i].v = dfdx[i] * x.v;
}
- return true;
}
- int NumValues() const { return array_.NumValues(); }
-
-private:
- const Array& array_;
+ private:
+ const Grid& grid_;
};
-// Given as input a two dimensional array like object, which provides
-// the following interface:
+// An object that implements an infinite one dimensional grid needed
+// by the CubicInterpolator where the source of the function values is
+// an array of type T on the interval
//
-// struct Array {
+// [begin, ..., end - 1]
+//
+// Since the input array is finite and the grid is infinite, values
+// outside this interval needs to be computed. Grid1D uses the value
+// from the nearest edge.
+//
+// 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 Grid1D {
+ public:
+ enum { DATA_DIMENSION = kDataDimension };
+
+ Grid1D(const T* data, const int begin, const int end)
+ : data_(data), begin_(begin), end_(end), num_values_(end - begin) {
+ CHECK_LT(begin, end);
+ }
+
+ EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {
+ const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;
+ if (kInterleaved) {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[kDataDimension * idx + i]);
+ }
+ } else {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[i * num_values_ + idx]);
+ }
+ }
+ }
+
+ private:
+ const T* data_;
+ const int begin_;
+ const int end_;
+ const int num_values_;
+};
+
+// Given as input an infinite two dimensional grid like object, which
+// provides the following interface:
+//
+// struct Grid {
// 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.
+// valued) for any pairs of integers (row, col), 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]
+// any point in the real plane.
//
// For more details on the algorithm used here see:
//
@@ -249,55 +249,29 @@
// const double data[] = {1.0, 3.0, -1.0, 4.0,
// 3.6, 2.1, 4.2, 2.0,
// 2.0, 1.0, 3.1, 5.2};
-// Array2D<double, 1> array(data, 3, 4);
-// BiCubicInterpolator<Array2D<double, 1> > interpolator(array);
+// Grid2D<double, 1> grid(data, 3, 4);
+// BiCubicInterpolator<Grid2D<double, 1> > interpolator(grid);
// double f, dfdr, dfdc;
-// CHECK(interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc));
+// interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);
-template<typename Array>
+template<typename Grid>
class CERES_EXPORT BiCubicInterpolator {
public:
- explicit BiCubicInterpolator(const Array& array)
- : array_(array) {
- CHECK_GT(array.NumRows(), 1);
- CHECK_GT(array.NumCols(), 1);
+ explicit BiCubicInterpolator(const Grid& grid)
+ : grid_(grid) {
// 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);
+ CHECK_GE(+Grid::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,
+ void Evaluate(double r, double c,
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.
+ // 4x4 grid of values.
//
// col
// p00 p01 p02 p03
@@ -308,200 +282,89 @@
// 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;
+ const int row = std::floor(r);
+ const int col = std::floor(c);
- 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 - p11;
- 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;
- }
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
// 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;
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;
+ Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
- CubicHermiteSpline<Array::DATA_DIMENSION>(p00, p01, p02, p03, c - col,
- f0.data(), df0dc.data());
- CubicHermiteSpline<Array::DATA_DIMENSION>(p10, p11, p12, p13, c - col,
- f1.data(), df1dc.data());
- CubicHermiteSpline<Array::DATA_DIMENSION>(p20, p21, p22, p23, c - col,
- f2.data(), df2dc.data());
- CubicHermiteSpline<Array::DATA_DIMENSION>(p30, p31, p32, p33, c - col,
- f3.data(), df3dc.data());
+ grid_.GetValue(row - 1, col - 1, p0.data());
+ grid_.GetValue(row - 1, col , p1.data());
+ grid_.GetValue(row - 1, col + 1, p2.data());
+ grid_.GetValue(row - 1, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f0.data(), df0dc.data());
+
+ grid_.GetValue(row, col - 1, p0.data());
+ grid_.GetValue(row, col , p1.data());
+ grid_.GetValue(row, col + 1, p2.data());
+ grid_.GetValue(row, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f1.data(), df1dc.data());
+
+ grid_.GetValue(row + 1, col - 1, p0.data());
+ grid_.GetValue(row + 1, col , p1.data());
+ grid_.GetValue(row + 1, col + 1, p2.data());
+ grid_.GetValue(row + 1, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f2.data(), df2dc.data());
+
+ grid_.GetValue(row + 2, col - 1, p0.data());
+ grid_.GetValue(row + 2, col , p1.data());
+ grid_.GetValue(row + 2, col + 1, p2.data());
+ grid_.GetValue(row + 2, col + 2, p3.data());
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
+ f3.data(), df3dc.data());
// Interpolate vertically the interpolated value from each row and
// compute the derivative along the columns.
- CubicHermiteSpline<Array::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
if (dfdc != NULL) {
// Interpolate vertically the derivative along the columns.
- CubicHermiteSpline<Array::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,
- r - row, dfdc, NULL);
+ CubicHermiteSpline<Grid::DATA_DIMENSION>(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
// evaluation is done, and the second one is for when Jets are used.
- bool Evaluate(const double& r, const double& c, double* f) const {
- return Evaluate(r, c, f, NULL, NULL);
+ void Evaluate(const double& r, const double& c, double* f) const {
+ Evaluate(r, c, f, NULL, NULL);
}
- template<typename JetT> bool Evaluate(const JetT& r,
+ template<typename JetT> void Evaluate(const JetT& r,
const JetT& c,
JetT* f) const {
- 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;
- }
-
- for (int i = 0; i < Array::DATA_DIMENSION; ++i) {
+ double frc[Grid::DATA_DIMENSION];
+ double dfdr[Grid::DATA_DIMENSION];
+ double dfdc[Grid::DATA_DIMENSION];
+ Evaluate(r.a, c.a, frc, dfdr, dfdc);
+ for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
f[i].a = frc[i];
f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;
}
-
- return true;
}
- int NumRows() const { return array_.NumRows(); }
- int NumCols() const { return array_.NumCols(); }
-
private:
- const Array& array_;
+ const Grid& grid_;
};
-// 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.
+// An object that implements an infinite two dimensional grid needed
+// by the BiCubicInterpolator where the source of the function values
+// is an grid of type T on the grid
//
-// 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
+// [(row_start, col_start), ..., (row_start, col_end - 1)]
+// [ ... ]
+// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]
//
-// 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 {
- DCHECK_GE(n, 0);
- DCHECK_LT(n, num_values_);
-
- 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.
+// Since the input grid is finite and the grid is infinite, values
+// outside this interval needs to be computed. Grid2D uses the value
+// from the nearest edge.
//
// The function being provided can be vector valued, in which case
// kDataDimension > 1. The data maybe stored in row or column major
@@ -522,37 +385,55 @@
int kDataDimension = 1,
bool kRowMajor = true,
bool kInterleaved = true>
-struct Array2D {
+struct Grid2D {
+ public:
enum { 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) {
+ Grid2D(const T* data,
+ const int row_begin, const int row_end,
+ const int col_begin, const int col_end)
+ : data_(data),
+ row_begin_(row_begin), row_end_(row_end),
+ col_begin_(col_begin), col_end_(col_end),
+ num_rows_(row_end - row_begin), num_cols_(col_end - col_begin),
+ num_values_(num_rows_ * num_cols_) {
CHECK_GE(kDataDimension, 1);
+ CHECK_LT(row_begin, row_end);
+ CHECK_LT(col_begin, col_end);
}
- void GetValue(const int r, const int c, double* f) const {
- DCHECK_GE(r, 0);
- DCHECK_LT(r, num_rows_);
- DCHECK_GE(c, 0);
- DCHECK_LT(c, num_cols_);
+ EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const {
+ const int row_idx =
+ std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_;
+ const int col_idx =
+ std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_;
- const int n = (kRowMajor) ? num_cols_ * r + c : num_rows_ * c + r;
- for (int i = 0; i < kDataDimension; ++i) {
- if (kInterleaved) {
+ const int n =
+ (kRowMajor)
+ ? num_cols_ * row_idx + col_idx
+ : num_rows_ * col_idx + row_idx;
+
+
+ if (kInterleaved) {
+ for (int i = 0; i < kDataDimension; ++i) {
f[i] = static_cast<double>(data_[kDataDimension * n + i]);
- } else {
- f[i] = static_cast<double>(data_[i * (num_rows_ * num_cols_) + n]);
+ }
+ } else {
+ for (int i = 0; i < kDataDimension; ++i) {
+ f[i] = static_cast<double>(data_[i * num_values_ + n]);
}
}
}
- int NumRows() const { return num_rows_; }
- int NumCols() const { return num_cols_; }
-
private:
const T* data_;
+ const int row_begin_;
+ const int row_end_;
+ const int col_begin_;
+ const int col_end_;
const int num_rows_;
const int num_cols_;
+ const int num_values_;
};
} // namespace ceres
diff --git a/internal/ceres/cubic_interpolation_test.cc b/internal/ceres/cubic_interpolation_test.cc
index b8ba45a..df43696 100644
--- a/internal/ceres/cubic_interpolation_test.cc
+++ b/internal/ceres/cubic_interpolation_test.cc
@@ -40,120 +40,169 @@
static const double kTolerance = 1e-12;
-TEST(Array1D, OneDataDimension) {
+TEST(Grid1D, OneDataDimension) {
int x[] = {1, 2, 3};
- Array1D<int, 1> array(x, 3);
+ Grid1D<int, 1> grid(x, 0, 3);
for (int i = 0; i < 3; ++i) {
double value;
- array.GetValue(i, &value);
+ grid.GetValue(i, &value);
EXPECT_EQ(value, static_cast<double>(i + 1));
}
}
-TEST(Array1D, TwoDataDimensionIntegerDataInterleaved) {
+TEST(Grid1D, OneDataDimensionOutOfBounds) {
+ int x[] = {1, 2, 3};
+ Grid1D<int, 1> grid(x, 0, 3);
+ double value;
+ grid.GetValue(-1, &value);
+ EXPECT_EQ(value, x[0]);
+ grid.GetValue(-2, &value);
+ EXPECT_EQ(value, x[0]);
+ grid.GetValue(3, &value);
+ EXPECT_EQ(value, x[2]);
+ grid.GetValue(4, &value);
+ EXPECT_EQ(value, x[2]);
+}
+
+TEST(Grid1D, TwoDataDimensionIntegerDataInterleaved) {
int x[] = {1, 5,
2, 6,
3, 7};
- Array1D<int, 2, true> array(x, 3);
+ Grid1D<int, 2, true> grid(x, 0, 3);
for (int i = 0; i < 3; ++i) {
double value[2];
- array.GetValue(i, value);
+ grid.GetValue(i, value);
EXPECT_EQ(value[0], static_cast<double>(i + 1));
EXPECT_EQ(value[1], static_cast<double>(i + 5));
}
}
-TEST(Array1D, TwoDataDimensionIntegerDataStacked) {
+
+TEST(Grid1D, TwoDataDimensionIntegerDataStacked) {
int x[] = {1, 2, 3,
5, 6, 7};
- Array1D<int, 2, false> array(x, 3);
+ Grid1D<int, 2, false> grid(x, 0, 3);
for (int i = 0; i < 3; ++i) {
double value[2];
- array.GetValue(i, value);
+ grid.GetValue(i, value);
EXPECT_EQ(value[0], static_cast<double>(i + 1));
EXPECT_EQ(value[1], static_cast<double>(i + 5));
}
}
-TEST(Array2D, OneDataDimensionRowMajor) {
+TEST(Grid2D, OneDataDimensionRowMajor) {
int x[] = {1, 2, 3,
2, 3, 4};
- Array2D<int, 1, true, true> array(x, 2, 3);
+ Grid2D<int, 1, true, true> grid(x, 0, 2, 0, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value;
- array.GetValue(r, c, &value);
+ grid.GetValue(r, c, &value);
EXPECT_EQ(value, static_cast<double>(r + c + 1));
}
}
}
-TEST(Array2D, TwoDataDimensionRowMajorInterleaved) {
+TEST(Grid2D, OneDataDimensionRowMajorOutOfBounds) {
+ int x[] = {1, 2, 3,
+ 2, 3, 4};
+ Grid2D<int, 1, true, true> grid(x, 0, 2, 0, 3);
+ double value;
+ grid.GetValue(-1, -1, &value);
+ EXPECT_EQ(value, x[0]);
+ grid.GetValue(-1, 0, &value);
+ EXPECT_EQ(value, x[0]);
+ grid.GetValue(-1, 1, &value);
+ EXPECT_EQ(value, x[1]);
+ grid.GetValue(-1, 2, &value);
+ EXPECT_EQ(value, x[2]);
+ grid.GetValue(-1, 3, &value);
+ EXPECT_EQ(value, x[2]);
+ grid.GetValue(0, 3, &value);
+ EXPECT_EQ(value, x[2]);
+ grid.GetValue(1, 3, &value);
+ EXPECT_EQ(value, x[5]);
+ grid.GetValue(2, 3, &value);
+ EXPECT_EQ(value, x[5]);
+ grid.GetValue(2, 2, &value);
+ EXPECT_EQ(value, x[5]);
+ grid.GetValue(2, 1, &value);
+ EXPECT_EQ(value, x[4]);
+ grid.GetValue(2, 0, &value);
+ EXPECT_EQ(value, x[3]);
+ grid.GetValue(2, -1, &value);
+ EXPECT_EQ(value, x[3]);
+ grid.GetValue(1, -1, &value);
+ EXPECT_EQ(value, x[3]);
+ grid.GetValue(0, -1, &value);
+ EXPECT_EQ(value, x[0]);
+}
+
+TEST(Grid2D, TwoDataDimensionRowMajorInterleaved) {
int x[] = {1, 4, 2, 8, 3, 12,
2, 8, 3, 12, 4, 16};
- Array2D<int, 2, true, true> array(x, 2, 3);
+ Grid2D<int, 2, true, true> grid(x, 0, 2, 0, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
- array.GetValue(r, c, value);
+ grid.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) {
+TEST(Grid2D, TwoDataDimensionRowMajorStacked) {
int x[] = {1, 2, 3,
2, 3, 4,
4, 8, 12,
8, 12, 16};
- Array2D<int, 2, true, false> array(x, 2, 3);
+ Grid2D<int, 2, true, false> grid(x, 0, 2, 0, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
- array.GetValue(r, c, value);
+ grid.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) {
+TEST(Grid2D, TwoDataDimensionColMajorInterleaved) {
int x[] = { 1, 4, 2, 8,
2, 8, 3, 12,
3, 12, 4, 16};
- Array2D<int, 2, false, true> array(x, 2, 3);
+ Grid2D<int, 2, false, true> grid(x, 0, 2, 0, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
- array.GetValue(r, c, value);
+ grid.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) {
+TEST(Grid2D, TwoDataDimensionColMajorStacked) {
int x[] = {1, 2,
2, 3,
3, 4,
4, 8,
8, 12,
12, 16};
- Array2D<int, 2, false, false> array(x, 2, 3);
+ Grid2D<int, 2, false, false> grid(x, 0, 2, 0, 3);
for (int r = 0; r < 2; ++r) {
for (int c = 0; c < 3; ++c) {
double value[2];
- array.GetValue(r, c, value);
+ grid.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)));
}
}
}
-
class CubicInterpolatorTest : public ::testing::Test {
public:
template <int kDataDimension>
@@ -170,8 +219,8 @@
}
}
- Array1D<double, kDataDimension> array(values_.get(), kNumSamples);
- CubicInterpolator<Array1D<double, kDataDimension> > interpolator(array);
+ Grid1D<double, kDataDimension> grid(values_.get(), 0, kNumSamples);
+ CubicInterpolator<Grid1D<double, kDataDimension> > interpolator(grid);
// Check values in the all the cells but the first and the last
// ones. In these cells, the interpolated function values should
@@ -191,7 +240,7 @@
expected_dfdx[dim] = (dim * dim + 1) * (3.0 * a * x * x + 2.0 * b * x + c);
}
- EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx));
+ interpolator.Evaluate(x, f, dfdx);
for (int dim = 0; dim < kDataDimension; ++dim) {
EXPECT_NEAR(f[dim], expected_f[dim], kTolerance)
<< "x: " << x << " dim: " << dim
@@ -233,12 +282,12 @@
TEST(CubicInterpolator, JetEvaluation) {
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);
+ Grid1D<double, 2, true> grid(values, 0, 4);
+ CubicInterpolator<Grid1D<double, 2, true> > interpolator(grid);
double f[2], dfdx[2];
const double x = 2.5;
- EXPECT_TRUE(interpolator.Evaluate(x, f, dfdx));
+ 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.
@@ -250,7 +299,7 @@
x_jet.v(3) = 1.3;
Jet<double, 4> f_jets[2];
- EXPECT_TRUE(interpolator.Evaluate(x_jet, f_jets));
+ interpolator.Evaluate(x_jet, f_jets);
// Check that the scalar part of the Jet is f(x).
EXPECT_EQ(f_jets[0].a, f[0]);
@@ -277,15 +326,15 @@
}
}
- Array2D<double, kDataDimension> array(values_.get(), kNumRows, kNumCols);
- BiCubicInterpolator<Array2D<double, kDataDimension> > interpolator(array);
+ Grid2D<double, kDataDimension> grid(values_.get(), 0, kNumRows, 0, kNumCols);
+ BiCubicInterpolator<Grid2D<double, kDataDimension> > interpolator(grid);
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;
double f[kDataDimension], dfdr[kDataDimension], dfdc[kDataDimension];
- EXPECT_TRUE(interpolator.Evaluate(r, c, f, dfdr, dfdc));
+ 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);
@@ -421,13 +470,13 @@
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);
+ Grid2D<double, 2> grid(values, 0, 2, 0, 4);
+ BiCubicInterpolator<Grid2D<double, 2> > interpolator(grid);
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));
+ 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.
@@ -446,7 +495,7 @@
c_jet.v(3) = 5.3;
Jet<double, 4> f_jets[2];
- EXPECT_TRUE(interpolator.Evaluate(r_jet, c_jet, f_jets));
+ 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(),
