要适配的对象，目标是实现栅格插值，通过传入模板Grid来初始化，我们要构造一个适配器类来使用我们的数据（使用占用概率地图数据）适配这个Grid模板类，主要是实现GetValue这个函数。
template<typename Grid>
class BiCubicInterpolator {
public:
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(+Grid::DATA_DIMENSION, 1);
}
// Evaluate the interpolated function value and/or its
// derivative. Returns false if r or c is out of bounds.
void Evaluate(double r, double c,
double* f, double* dfdr, double* dfdc) const {
// BiCubic interpolation requires 16 values around the point being
// evaluated. We will use pij, to indicate the elements of the
// 4x4 grid 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.
const int row = std::floor(r);
const int col = std::floor(c);
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, 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());
// Interpolate vertically the interpolated value from each row and
// compute the derivative along the columns.
CubicHermiteSpline<Grid::DATA_DIMENSION>(f0, f1, f2, f3, r - row, f, dfdr);
if (dfdc != NULL) {
// Interpolate vertically the derivative along the columns.
CubicHermiteSpline<Grid::DATA_DIMENSION>(df0dc, df1dc, df2dc, df3dc,
r - row, dfdc, NULL);
}
}
// 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.
void Evaluate(const double& r, const double& c, double* f) const {
Evaluate(r, c, f, NULL, NULL);
}
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_;
};
// 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
//
// [(row_start, col_start), ..., (row_start, col_end - 1)]
// [ ... ]
// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)]
//
// 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
// 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 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_;
};
OK，我们的适配器类出场了，它使用了grid_数据并实现了GetValue接口，接下来可以使用适配了。
class GridArrayAdapter {
public:
enum {
DATA_DIMENSION = 1 };
explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {
}
void GetValue(const int row, const int column, double* const value) const {
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
column >= NumCols() - kPadding) {
*value = kMaxCorrespondenceCost;
} else {
*value = static_cast<double>(grid_.GetCorrespondenceCost(
Eigen::Array2i(column - kPadding, row - kPadding)));
}
}
int NumRows() const {
return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
}
int NumCols() const {
return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
}
private:
const Grid2D& grid_;
};
这样使用
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = grid_.limits();
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// Note that this is a 2D point. The third component is a scaling factor.
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
(T(point_cloud_[i].position.y())),
T(1.));
const Eigen::Matrix<T, 3, 1> world = transform * point;
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
&residual[i]);
residual[i] = scaling_factor_ * residual[i];
}