先看这个匹配的原理
ceres中的类 BiCubicInterpolator
输入无限的二维grid
1 2 3 4
| struct Grid2D { enum { DATA_DIMENSION = 2 }; void GetValue(int row, int col, double* f) const; };
|
GetValue
函数返回函数
f
(可能是向量)的值, 枚举
DATA_DIMENSION
表示所插值函数的维度,比如对一个图片(红绿蓝)进行插值,那么
DATA_DIMENSION = 3
BiCubicInterpolator
使用三次卷积算法生成平滑估计,或者说双三次插值法,用来在真实曲线的任一点评价
对二维数组进行插值
1 2 3 4 5 6 7 8
| 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};
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
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
|
ceres::CostFunction* CreateOccupiedSpaceCostFunction2D( const double scaling_factor, const sensor::PointCloud& point_cloud, const Grid2D& grid) { return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D, ceres::DYNAMIC, 3 > ( new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid), point_cloud.size() ); }
|
OccupiedSpaceCostFunction2D
的构造函数只有参数赋值
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
| 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.); const GridArrayAdapter adapter(grid_); ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter); const MapLimits& limits = grid_.limits(); for (size_t i = 0; i < point_cloud_.size(); ++i) { 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]; } return true; }
|
占用栅格中原本存储的就是栅格空闲的概率,而这里GetValue
查询出来的概率其实就是 ,令其最小化就对了
GridArrayAdapter
是cartographer定义的,使用适配器模式,interpolator
构造函数的参数需要的是模板里的类型。重要函数的是GetValue
,调用的地方在interpolator.Evaluate
里面。根源还是BiCubicInterpolator
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
| 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_; }
|
参考: ceres 三次插值
cubic_interpolation.h