123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436 |
- #ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_
- #define CERES_PUBLIC_CUBIC_INTERPOLATION_H_
- #include "Eigen/Core"
- #include "ceres/internal/export.h"
- #include "glog/logging.h"
- namespace ceres {
- 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) {
- using VType = Eigen::Matrix<double, kDataDimension, 1>;
- 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;
-
-
-
- if (f != nullptr) {
- Eigen::Map<VType>(f, kDataDimension) = d + x * (c + x * (b + x * a));
- }
-
- if (dfdx != nullptr) {
- Eigen::Map<VType>(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x);
- }
- }
- template <typename Grid>
- class CubicInterpolator {
- public:
- explicit CubicInterpolator(const Grid& grid) : grid_(grid) {
-
-
-
- CHECK_GE(+Grid::DATA_DIMENSION, 1);
- }
- 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);
- }
-
-
-
- void Evaluate(const double& x, double* f) const { Evaluate(x, f, nullptr); }
- 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;
- }
- }
- private:
- const Grid& grid_;
- };
- 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_;
- };
- template <typename Grid>
- class BiCubicInterpolator {
- public:
- explicit BiCubicInterpolator(const Grid& grid) : grid_(grid) {
-
-
-
- CHECK_GE(+Grid::DATA_DIMENSION, 1);
- }
-
-
-
- void Evaluate(
- double r, double c, double* f, double* dfdr, double* dfdc) const {
-
-
-
-
-
-
-
-
-
-
-
-
- const int row = std::floor(r);
- const int col = std::floor(c);
- Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> p0, p1, p2, p3;
-
-
- Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> f0, f1, f2, f3;
- Eigen::Matrix<double, Grid::DATA_DIMENSION, 1> df0dc, df1dc, df2dc, df3dc;
- 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());
-
-
- CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
- if (dfdc != nullptr) {
-
- CubicHermiteSpline<Grid::DATA_DIMENSION>(
- df0dc, df1dc, df2dc, df3dc, r - row, dfdc, nullptr);
- }
- }
-
-
-
- void Evaluate(const double& r, const double& c, double* f) const {
- Evaluate(r, c, f, nullptr, nullptr);
- }
- template <typename JetT>
- void Evaluate(const JetT& r, const JetT& c, JetT* f) const {
- 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;
- }
- }
- private:
- const Grid& grid_;
- };
- template <typename T,
- int kDataDimension = 1,
- bool kRowMajor = true,
- bool kInterleaved = true>
- struct Grid2D {
- public:
- enum { DATA_DIMENSION = kDataDimension };
- 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);
- }
- 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_ * 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 {
- for (int i = 0; i < kDataDimension; ++i) {
- f[i] = static_cast<double>(data_[i * num_values_ + n]);
- }
- }
- }
- 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_;
- };
- }
- #endif
|