先看这个匹配的原理
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
