1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of Google Inc. nor the names of its contributors may be
14 // used to endorse or promote products derived from this software without
15 // specific prior written permission.
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
29 // Author: sameeragarwal@google.com (Sameer Agarwal)
31 #ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_
32 #define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
34 #include "ceres/internal/port.h"
36 #include "glog/logging.h"
40 // Given samples from a function sampled at four equally spaced points,
47 // Evaluate the cubic Hermite spline (also known as the Catmull-Rom
48 // spline) at a point x that lies in the interval [0, 1].
50 // This is also the interpolation kernel (for the case of a = 0.5) as
51 // proposed by R. Keys, in:
53 // "Cubic convolution interpolation for digital image processing".
54 // IEEE Transactions on Acoustics, Speech, and Signal Processing
57 // For more details see
59 // http://en.wikipedia.org/wiki/Cubic_Hermite_spline
60 // http://en.wikipedia.org/wiki/Bicubic_interpolation
62 // f if not NULL will contain the interpolated function values.
63 // dfdx if not NULL will contain the interpolated derivative values.
64 template <int kDataDimension>
65 void CubicHermiteSpline(const Eigen::Matrix<double, kDataDimension, 1>& p0,
66 const Eigen::Matrix<double, kDataDimension, 1>& p1,
67 const Eigen::Matrix<double, kDataDimension, 1>& p2,
68 const Eigen::Matrix<double, kDataDimension, 1>& p3,
72 typedef Eigen::Matrix<double, kDataDimension, 1> VType;
73 const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3);
74 const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3);
75 const VType c = 0.5 * (-p0 + p2);
78 // Use Horner's rule to evaluate the function value and its
81 // f = ax^3 + bx^2 + cx + d
83 Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
86 // dfdx = 3ax^2 + 2bx + c
88 Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
92 // Given as input an infinite one dimensional grid, which provides the
93 // following interface.
97 // enum { DATA_DIMENSION = 2; };
98 // void GetValue(int n, double* f) const;
101 // Here, GetValue gives the value of a function f (possibly vector
102 // valued) for any integer n.
104 // The enum DATA_DIMENSION indicates the dimensionality of the
105 // function being interpolated. For example if you are interpolating
106 // rotations in axis-angle format over time, then DATA_DIMENSION = 3.
108 // CubicInterpolator uses cubic Hermite splines to produce a smooth
109 // approximation to it that can be used to evaluate the f(x) and f'(x)
110 // at any point on the real number line.
112 // For more details on cubic interpolation see
114 // http://en.wikipedia.org/wiki/Cubic_Hermite_spline
118 // const double data[] = {1.0, 2.0, 5.0, 6.0};
119 // Grid1D<double, 1> grid(x, 0, 4);
120 // CubicInterpolator<Grid1D<double, 1> > interpolator(grid);
122 // interpolator.Evaluator(1.5, &f, &dfdx);
123 template<typename Grid>
124 class CubicInterpolator {
126 explicit CubicInterpolator(const Grid& grid)
128 // The + casts the enum into an int before doing the
129 // comparison. It is needed to prevent
130 // "-Wunnamed-type-template-args" related errors.
131 CHECK_GE(+Grid::DATA_DIMENSION, 1);
134 void Evaluate(double x, double* f, double* dfdx) const {
135 const int n = std::floor(x);
136 Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
137 grid_.GetValue(n - 1, p0.data());
138 grid_.GetValue(n, p1.data());
139 grid_.GetValue(n + 1, p2.data());
140 grid_.GetValue(n + 2, p3.data());
141 CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, x - n, f, dfdx);
144 // The following two Evaluate overloads are needed for interfacing
145 // with automatic differentiation. The first is for when a scalar
146 // evaluation is done, and the second one is for when Jets are used.
147 void Evaluate(const double& x, double* f) const {
148 Evaluate(x, f, NULL);
151 template<typename JetT> void Evaluate(const JetT& x, JetT* f) const {
152 double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION];
153 Evaluate(x.a, fx, dfdx);
154 for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
156 f[i].v = dfdx[i] * x.v;
164 // An object that implements an infinite one dimensional grid needed
165 // by the CubicInterpolator where the source of the function values is
166 // an array of type T on the interval
168 // [begin, ..., end - 1]
170 // Since the input array is finite and the grid is infinite, values
171 // outside this interval needs to be computed. Grid1D uses the value
172 // from the nearest edge.
174 // The function being provided can be vector valued, in which case
175 // kDataDimension > 1. The dimensional slices of the function maybe
176 // interleaved, or they maybe stacked, i.e, if the function has
177 // kDataDimension = 2, if kInterleaved = true, then it is stored as
179 // f01, f02, f11, f12 ....
181 // and if kInterleaved = false, then it is stored as
183 // f01, f11, .. fn1, f02, f12, .. , fn2
185 template <typename T,
186 int kDataDimension = 1,
187 bool kInterleaved = true>
190 enum { DATA_DIMENSION = kDataDimension };
192 Grid1D(const T* data, const int begin, const int end)
193 : data_(data), begin_(begin), end_(end), num_values_(end - begin) {
194 CHECK_LT(begin, end);
197 EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const {
198 const int idx = std::min(std::max(begin_, n), end_ - 1) - begin_;
200 for (int i = 0; i < kDataDimension; ++i) {
201 f[i] = static_cast<double>(data_[kDataDimension * idx + i]);
204 for (int i = 0; i < kDataDimension; ++i) {
205 f[i] = static_cast<double>(data_[i * num_values_ + idx]);
214 const int num_values_;
217 // Given as input an infinite two dimensional grid like object, which
218 // provides the following interface:
221 // enum { DATA_DIMENSION = 1 };
222 // void GetValue(int row, int col, double* f) const;
225 // Where, GetValue gives us the value of a function f (possibly vector
226 // valued) for any pairs of integers (row, col), and the enum
227 // DATA_DIMENSION indicates the dimensionality of the function being
228 // interpolated. For example if you are interpolating a color image
229 // with three channels (Red, Green & Blue), then DATA_DIMENSION = 3.
231 // BiCubicInterpolator uses the cubic convolution interpolation
232 // algorithm of R. Keys, to produce a smooth approximation to it that
233 // can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at
234 // any point in the real plane.
236 // For more details on the algorithm used here see:
238 // "Cubic convolution interpolation for digital image processing".
239 // Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal
240 // Processing 29 (6): 1153–1160, 1981.
242 // http://en.wikipedia.org/wiki/Cubic_Hermite_spline
243 // http://en.wikipedia.org/wiki/Bicubic_interpolation
247 // const double data[] = {1.0, 3.0, -1.0, 4.0,
248 // 3.6, 2.1, 4.2, 2.0,
249 // 2.0, 1.0, 3.1, 5.2};
250 // Grid2D<double, 1> grid(data, 3, 4);
251 // BiCubicInterpolator<Grid2D<double, 1> > interpolator(grid);
252 // double f, dfdr, dfdc;
253 // interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);
255 template<typename Grid>
256 class BiCubicInterpolator {
258 explicit BiCubicInterpolator(const Grid& grid)
260 // The + casts the enum into an int before doing the
261 // comparison. It is needed to prevent
262 // "-Wunnamed-type-template-args" related errors.
263 CHECK_GE(+Grid::DATA_DIMENSION, 1);
266 // Evaluate the interpolated function value and/or its
267 // derivative. Returns false if r or c is out of bounds.
268 void Evaluate(double r, double c,
269 double* f, double* dfdr, double* dfdc) const {
270 // BiCubic interpolation requires 16 values around the point being
271 // evaluated. We will use pij, to indicate the elements of the
272 // 4x4 grid of values.
276 // row p10 p11 p12 p13
280 // The point (r,c) being evaluated is assumed to lie in the square
281 // defined by p11, p12, p22 and p21.
283 const int row = std::floor(r);
284 const int col = std::floor(c);
286 Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
288 // Interpolate along each of the four rows, evaluating the function
289 // value and the horizontal derivative in each row.
290 Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;
291 Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
293 grid_.GetValue(row - 1, col - 1, p0.data());
294 grid_.GetValue(row - 1, col , p1.data());
295 grid_.GetValue(row - 1, col + 1, p2.data());
296 grid_.GetValue(row - 1, col + 2, p3.data());
297 CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
298 f0.data(), df0dc.data());
300 grid_.GetValue(row, col - 1, p0.data());
301 grid_.GetValue(row, col , p1.data());
302 grid_.GetValue(row, col + 1, p2.data());
303 grid_.GetValue(row, col + 2, p3.data());
304 CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
305 f1.data(), df1dc.data());
307 grid_.GetValue(row + 1, col - 1, p0.data());
308 grid_.GetValue(row + 1, col , p1.data());
309 grid_.GetValue(row + 1, col + 1, p2.data());
310 grid_.GetValue(row + 1, col + 2, p3.data());
311 CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
312 f2.data(), df2dc.data());
314 grid_.GetValue(row + 2, col - 1, p0.data());
315 grid_.GetValue(row + 2, col , p1.data());
316 grid_.GetValue(row + 2, col + 1, p2.data());
317 grid_.GetValue(row + 2, col + 2, p3.data());
318 CubicHermiteSpline<Grid::DATA_DIMENSION>(p0, p1, p2, p3, c - col,
319 f3.data(), df3dc.data());
321 // Interpolate vertically the interpolated value from each row and
322 // compute the derivative along the columns.
323 CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
325 // Interpolate vertically the derivative along the columns.
326 CubicHermiteSpline<Grid::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,
327 r - row, dfdc, NULL);
331 // The following two Evaluate overloads are needed for interfacing
332 // with automatic differentiation. The first is for when a scalar
333 // evaluation is done, and the second one is for when Jets are used.
334 void Evaluate(const double& r, const double& c, double* f) const {
335 Evaluate(r, c, f, NULL, NULL);
338 template<typename JetT> void Evaluate(const JetT& r,
341 double frc[Grid::DATA_DIMENSION];
342 double dfdr[Grid::DATA_DIMENSION];
343 double dfdc[Grid::DATA_DIMENSION];
344 Evaluate(r.a, c.a, frc, dfdr, dfdc);
345 for (int i = 0; i < Grid::DATA_DIMENSION; ++i) {
347 f[i].v = dfdr[i] * r.v + dfdc[i] * c.v;
355 // An object that implements an infinite two dimensional grid needed
356 // by the BiCubicInterpolator where the source of the function values
357 // is an grid of type T on the grid
359 // [(row_start, col_start), ..., (row_start, col_end - 1)]
361 // [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]
363 // Since the input grid is finite and the grid is infinite, values
364 // outside this interval needs to be computed. Grid2D uses the value
365 // from the nearest edge.
367 // The function being provided can be vector valued, in which case
368 // kDataDimension > 1. The data maybe stored in row or column major
369 // format and the various dimensional slices of the function maybe
370 // interleaved, or they maybe stacked, i.e, if the function has
371 // kDataDimension = 2, is stored in row-major format and if
372 // kInterleaved = true, then it is stored as
374 // f001, f002, f011, f012, ...
376 // A commonly occuring example are color images (RGB) where the three
377 // channels are stored interleaved.
379 // If kInterleaved = false, then it is stored as
381 // f001, f011, ..., fnm1, f002, f012, ...
382 template <typename T,
383 int kDataDimension = 1,
384 bool kRowMajor = true,
385 bool kInterleaved = true>
388 enum { DATA_DIMENSION = kDataDimension };
390 Grid2D(const T* data,
391 const int row_begin, const int row_end,
392 const int col_begin, const int col_end)
394 row_begin_(row_begin), row_end_(row_end),
395 col_begin_(col_begin), col_end_(col_end),
396 num_rows_(row_end - row_begin), num_cols_(col_end - col_begin),
397 num_values_(num_rows_ * num_cols_) {
398 CHECK_GE(kDataDimension, 1);
399 CHECK_LT(row_begin, row_end);
400 CHECK_LT(col_begin, col_end);
403 EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const {
405 std::min(std::max(row_begin_, r), row_end_ - 1) - row_begin_;
407 std::min(std::max(col_begin_, c), col_end_ - 1) - col_begin_;
411 ? num_cols_ * row_idx + col_idx
412 : num_rows_ * col_idx + row_idx;
416 for (int i = 0; i < kDataDimension; ++i) {
417 f[i] = static_cast<double>(data_[kDataDimension * n + i]);
420 for (int i = 0; i < kDataDimension; ++i) {
421 f[i] = static_cast<double>(data_[i * num_values_ + n]);
428 const int row_begin_;
430 const int col_begin_;
434 const int num_values_;
439 #endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_