先看这个匹配的原理

ceres中的类 BiCubicInterpolator

输入无限的二维grid

struct Grid2D {
  enum { DATA_DIMENSION = 2 };
  void GetValue(int row, int col, double* f) const;
};

GetValue函数返回函数f(可能是向量)的值， 枚举DATA_DIMENSION表示所插值函数的维度，比如对一个图片(红绿蓝)进行插值，那么DATA_DIMENSION = 3

BiCubicInterpolator使用三次卷积算法生成平滑估计，或者说双三次插值法，用来在真实曲线的任一点评价 $f(r,c), \frac{\partial f(r,c)}{\partial r}, \frac{\partial f(r,c)}{\partial c}$

对二维数组进行插值

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};
// 生成 BiCubicInterpolator 需要的二维数组
Grid2D<double, 1>  array(data, 0, 3, 0, 4);
BiCubicInterpolator interpolator(array);
double f, dfdr, dfdc;
interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);

函数void Evaluate(double r, double c, double* f, double* dfdr, double* dfdc)，残差会对应第3个参数f。 Evaluate the interpolated function value and/or its 导数. Returns false 如果r 或者 c越界

这里的用法没看懂，怎么获得插值后的结果, GetValue获得还是之前的值

OccupiedSpaceCostFunction2D

这个Occupied Space Cost Function的模型和 Real time correlative scan matching 的思路基本上一致，只是求解方法变成了最小二乘问题的求解。

将点云中所有点的坐标映射到栅格坐标系， 假如点对应的空闲概率最小，说明对应栅格几乎被占据，点确实是hit点，此时的变换为最优变换。 出于精度考虑使用了ceres提供的双三线性插值。 还有地图大小限制的问题，即一旦点云变换后存在部分脱离地图范围的点，这些点的代价值需要定义。cartographer中的做法是在地图周围增加一个巨大的边框(kPadding)，并且通过一个地图适配器定义点落在边框中的代价值。

先看创建代价函数 CreateOccupiedSpaceCostFunction2D

// 创建代价函数 for matching the 'point_cloud' to the 'grid' with a 'pose'
//  'point_cloud' 是 filtered_gravity_aligned_point_cloud
// 'grid' 是 matching_submap->grid()
//  grid 和 point observation  匹配越差，代价越大
//  比如 points falling into less occupied space
ceres::CostFunction* CreateOccupiedSpaceCostFunction2D(
    const double scaling_factor, const sensor::PointCloud& point_cloud,
    const Grid2D& grid)
{
  return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
           ceres::DYNAMIC,   /* residuals 残差维度未知 */
           3  /* pose variables */>
  (
    new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
    point_cloud.size() );
}

OccupiedSpaceCostFunction2D的构造函数只有参数赋值

template <typename T>
bool operator()(const T* const pose, T* residual) const
{
    Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
    Eigen::Rotation2D<T> rotation(pose[2]);
    Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
    Eigen::Matrix<T, 3, 3> transform;
    transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
    // 构造二阶线性插值类， 成员变量只有 grid_
    // 功能主要是在概率栅格图对应的 index 中取出相应的概率值
    const GridArrayAdapter  adapter(grid_);
    // 使用Ceres提供的双三次插值迭代器
    ceres::BiCubicInterpolator<GridArrayAdapter>  interpolator(adapter);
    const MapLimits&  limits = grid_.limits();
    // 遍历点云（当前 scan）中的所有点，计算该点匹配残差： 1-Smooth(Tp)
    for (size_t i = 0; i < point_cloud_.size(); ++i)
    {
        // 这是2D点，第三个元素是 scaling factor
        const Eigen::Matrix<T, 3, 1> point(
        (  T(point_cloud_[i].position.x()) ),
        (  T(point_cloud_[i].position.y()) ),
           T(1.)   );

        // 将点转换到 local map 坐标系下
        const Eigen::Matrix<T, 3, 1> world = transform * point;
        /* 计算  1-Smooth(T * point)
        前两个参数用来描述x y轴索引， 第三个参数用于记录插值后的结果
        xy索引的计算是通过 GridArrayAdapter::GetValue 获取栅格数据 
        取出每个点对应的栅格地图空闲概率（双三次插值之后的） p */
        interpolator.Evaluate(
        // 传进来的时候x和y都分别加了  kPadding
        // max的x y减掉真实的xy得到  cell_index
        /* kPadding是为了解决有些点可能跑到地图外面去的情况，所以加了一个超大的值
          即将地图上下左右边界分别扩大 kPadding，这是为了照顾ceres的Evaluate函数 */

        // 将坐标转换为栅格坐标，与子图坐标方向相反，双三次插值器自动计算中对应坐标的value
        (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];
    }
    return true;
}

占用栅格中原本存储的就是栅格空闲的概率，而这里GetValue查询出来的概率其实就是 $1-M_{smooth}\ \ (T_{\xi} H_k)$，令其最小化就对了

$$每个残差权重=\frac{参数 occupied\ space\ weight} {\sqrt{点云数}}$$

GridArrayAdapter是cartographer定义的，使用适配器模式，interpolator构造函数的参数需要的是模板里的类型。重要函数的是GetValue，调用的地方在interpolator.Evaluate里面。根源还是BiCubicInterpolator

class GridArrayAdapter
{
   public:
    enum { DATA_DIMENSION = 1 };  //被插值的向量维度

    explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}
    // 返回空闲概率， kPadding 是个很大的数
    void GetValue(const int row, const int column, double* const value) const
    {
      // 处于地图外部时，赋予最大的free值
      if(row < kPadding || column < kPadding || 
      	row >= NumRows() - kPadding || column >= NumCols() - kPadding)
        	*value = kMaxCorrespondenceCost;
     // 在地图里取空闲概率，这里需要减掉kPadding，因为在传进来的时候，已经加了kPadding
      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_;
}

参考: ceres 三次插值
cubic_interpolation.h