从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析
这篇文章将带领大家推导一下hector slam论文中的公式．之后再对这部分公式对应的代码进行讲解下．
markdown打公式太费劲了，所以我用手写了．（懒） 然后csdn又限制了图片文件大小，我是照完照片又截图才传上来的，所以图片有点不清晰．
1 高斯牛顿法
首先借用 <视觉SLAM十四讲> 说明一下高斯牛顿法的思路．
高斯牛顿法首先将目标函数进行一阶泰勒展开，再经过公式推导，可以获得最优解的公式为 Δ x = H − 1 ∗ g \Delta x = H^{-1} * g Δx=H−1∗g ，并且hessian矩阵可以通过 H = J T ∗ J H=J^T * J H=JT∗J 的形式进行求解，免去了计算hessian矩阵的较大计算量．
以上图片引自 <<视觉SLAM十四讲>> ，如有侵权请联系我进行删除．
2 公式推导
2.1 目标函数推导
目标函数的形式为
ξ ∗ = a r g m i n ( ∑ i = 1 N [ 1 − M ( S i ( ξ ) ) ] 2 ) \xi^*=argmin(\sum_{i=1}^N[1-M( S_i(\xi) ) ]^2) ξ∗=argmin(∑i=1N[1−M(Si(ξ))]2) ，
代表当前这帧雷达数据占据地图的概率， M ( S i ( ξ ) ) M( S_i(\xi) ) M(Si(ξ)) 最大为1，最大值不好求，所以通过 1 − M ( S i ( ξ ) ) 1-M( S_i(\xi) ) 1−M(Si(ξ)) 的方式求最小值．
首先，通过右加一个小量 Δ ξ \Delta\xi Δξ , 来使得公式趋近于零．
但是由于目标函数是非线性的，不能直接求导，所以根据高斯牛顿法的思路，先进行一阶泰勒展开，然后再进行求导．
之后，进行左右项的变换．求得 Δ ξ = H − 1 ∗ g \Delta\xi = H^{-1} * g Δξ=H−1∗g ．
2.2 求解
所以，接下来只需要求出 J ( ξ ) J(\xi) J(ξ) 与 f ( ξ ) f(\xi) f(ξ) 就可以了．
只需分别求出 栅格值 M ( S i ( ξ ) ) M( S_i(\xi)) M(Si(ξ)) ， 栅格的导数 ∇ M ( S i ( ξ ) ) \nabla M( S_i(\xi)) ∇M(Si(ξ))， 激光点转换函数的导数 ∇ S i ( ξ ) \nabla S_i(\xi) ∇Si(ξ)，即可．
2.2.1 求激光点转换函数的导数
首先求 激光点转换函数的导数．
激光点转换函数Ｓ是个2*1维的矩阵，第一行是x坐标的转换公式，第二行是y坐标的转换公式．
所以求导时候 分别对第一行第二行，对 p x p_x px , p y p_y py , ψ \psi ψ 三个变量进行求导，将得到一个2*3的矩阵．
2.2.2 求栅格值 M ( S i ( ξ ) ) M( S_i(\xi)) M(Si(ξ))
点(x,y)的栅格值可以通过 这点周围相邻的4个栅格的栅格值 进行插值来表示．
公式是直接从论文里摘下来的，是通过 拉格朗日2次插值 来求的.
2.2.3 求栅格值的导数 ∇ M ( S i ( ξ ) ) \nabla M( S_i(\xi)) ∇M(Si(ξ))
栅格值的导数是一个1*2的矩阵，是分别对x方向，y方向的导数.
公式是直接从论文里摘下来了，是通过栅格值分别对x与y求偏导得到的。
其中 y1-y0 = x1-x0=1，所以是如下形式。
2.2.4 求 J ( ξ ) J(\xi) J(ξ) 与 f ( ξ ) f(\xi) f(ξ)
图片里求 J ( ξ ) J(\xi) J(ξ) 的最后一步时，对结果进行转置了， J ( ξ ) J(\xi) J(ξ) 是一个 1*3 的矩阵.
2.2.5 求 H e s s i a n Hessian Hessian矩阵
3 相关代码讲解
3.1 getCompleteHessianDerivs()
这个函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/OccGridMapUtil.h 内．
代码的实现和公式一模一样，唯一的区别就是公式里的求和符合在代码里变成了累加。
/**
* 使用当前pose投影dataPoints到地图，计算出 H 矩阵 b列向量， 理论部分详见Hector论文： 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》.
* @param pose 地图系上的位姿
* @param dataPoints 已转换为地图尺度的激光点数据
* @param H 需要计算的 H矩阵
* @param dTr 需要计算的 g列向量
*/
void getCompleteHessianDerivs(const Eigen::Vector3f &pose,
const DataContainer &dataPoints,
Eigen::Matrix3f &H,
Eigen::Vector3f &dTr)
{
int size = dataPoints.getSize();
// 获取变换矩阵
Eigen::Affine2f transform(getTransformForState(pose));
float sinRot = sin(pose[2]);
float cosRot = cos(pose[2]);
H = Eigen::Matrix3f::Zero();
dTr = Eigen::Vector3f::Zero();
// 按照公式计算H、b
for (int i = 0; i < size; ++i)
{
// 地图尺度下的激光坐标系下的激光点坐标
const Eigen::Vector2f &currPoint(dataPoints.getVecEntry(i));
// 将激光点坐标转换到地图系上, 通过双线性插值计算栅格概率
// transformedPointData[0]--通过插值得到的栅格值
// transformedPointData[1]--栅格值x方向的梯度
// transformedPointData[2]--栅格值y方向的梯度
Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));
// 目标函数f(x) (1-M(Pm))
float funVal = 1.0f - transformedPointData[0];
// 计算g列向量的 x 与 y 方向的值
dTr[0] += transformedPointData[1] * funVal;
dTr[1] += transformedPointData[2] * funVal;
// 根据公式计算
float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] +
(cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);
// 计算g列向量的 角度 的值
dTr[2] += rotDeriv * funVal;
// 计算 hessian 矩阵
H(0, 0) += util::sqr(transformedPointData[1]);
H(1, 1) += util::sqr(transformedPointData[2]);
H(2, 2) += util::sqr(rotDeriv);
H(0, 1) += transformedPointData[1] * transformedPointData[2];
H(0, 2) += transformedPointData[1] * rotDeriv;
H(1, 2) += transformedPointData[2] * rotDeriv;
}
// H是对称矩阵，只算上三角就行， 减少计算量。
H(1, 0) = H(0, 1);
H(2, 0) = H(0, 2);
H(2, 1) = H(1, 2);
}
3.2 interpMapValueWithDerivatives()
这个函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/map/OccGridMapUtil.h 内．
取相邻4点的栅格值时用了一个cache函数，这块感觉实现的不好，可能这里就是导致地图变大时hector计算时间变长的原因。
/**
* 双线性插值计算网格中任一点的得分（占据概率）以及该点处的梯度
* @param coords 激光点地图坐标
* @return ret(0) 是网格值 ， ret(1) 是栅格值在x方向的导数 ， ret(2)是栅格值在y方向的导数
*/
Eigen::Vector3f interpMapValueWithDerivatives(const Eigen::Vector2f &coords)
{
// 检查coords坐标是否是在地图坐标范围内
if (concreteGridMap->pointOutOfMapBounds(coords))
{
return Eigen::Vector3f(0.0f, 0.0f, 0.0f);
}
// 对坐标进行向下取整，即得到坐标(x0,y0)
Eigen::Vector2i indMin(coords.cast<int>());
// 得到双线性插值的因子
// | x | | x0 | | x-x0 |
// | | - | | = | |
// | y | | y0 | | y-y0 |
Eigen::Vector2f factors(coords - indMin.cast<float>());
// 获得地图的X方向最大边界
int sizeX = concreteGridMap->getSizeX();
// 计算(x0, y0)点的网格索引值
int index = indMin[1] * sizeX + indMin[0];
// 下边这取4个点的栅格值，感觉就是导致hector大地图后计算变慢的原因
/** 首先判断cache中该地图点在本次scan中是否被访问过，若有则直接取值；没有则立马计算概率值并更新到cache **/
/** 这个cache的作用是，避免单次scan重复访问同一网格时带来的重复概率计算。地图更新后，网格logocc改变，cache数据就会无效。 **/
/** 但是这种方式内存开销太大..相当于同时维护两份地图，使用 hash map 是不是会更合适些 **/
if (!cacheMethod.containsCachedData(index, intensities[0]))
{
intensities[0] = getUnfilteredGridPoint(index); // 得到M(P00),P00(x0,y0)
cacheMethod.cacheData(index, intensities[0]);
}
++index;
if (!cacheMethod.containsCachedData(index, intensities[1]))
{
intensities[1] = getUnfilteredGridPoint(index);
cacheMethod.cacheData(index, intensities[1]);
}
index += sizeX - 1;
if (!cacheMethod.containsCachedData(index, intensities[2]))
{
intensities[2] = getUnfilteredGridPoint(index);
cacheMethod.cacheData(index, intensities[2]);
}
++index;
if (!cacheMethod.containsCachedData(index, intensities[3]))
{
intensities[3] = getUnfilteredGridPoint(index);
cacheMethod.cacheData(index, intensities[3]);
}
float dx1 = intensities[0] - intensities[1]; // 求得(M(P00) - M(P10))的值
float dx2 = intensities[2] - intensities[3]; // 求得(M(P01) - M(P11))的值
float dy1 = intensities[0] - intensities[2]; // 求得(M(P00) - M(P01))的值
float dy2 = intensities[1] - intensities[3]; // 求得(M(P10) - M(P11))的值
// 得到双线性插值的因子,注意x0+1=x1,y0+1=y1,则
// | x-x0 | | 1-x+x0 | | x1-x |
// 1 - | | = | | = | |
// | y-y0 | | 1-y+y0 | | y1-x |
float xFacInv = (1.0f - factors[0]); // 求得(x1-x)的值
float yFacInv = (1.0f - factors[1]); // 求得(y1-y)的值
// y-y0 | x-x0 x1-x | y1-y | x-x0 x1-x |
// M(Pm) = ------|------ M(P11) + ------ M(P01)| + ------|------ M(P10) + ------ M(P00)|
// y1-y0| x1-x0 x1-x0 | y1-y0| x1-x0 x1-x0 |
// 注意：此处y1-y0=x1-x0=1,那么对应函数返回值，可以写成
// M(Pm) = (M(P00) * (x1-x) + M(P10) * (x-x0)) * (y1-y) + (M(P01) * (x1-x) + M(P11) * (x-x0)) * (y-y0)
// d(M(Pm)) y-y0 | | y1-y | |
// ---------- = ------| M(P11) - M(P01)| + ------| M(P10) - M(P00)|
// dx y1-y0| | y1-y0| |
// 同理，化简可得 d(M(Pm))/dx = -((M(P00) - M(P10)) * (y1-y) + (M(P01) - M(P11)) * (y-y0))
// 同样地，也有 d(M(Pm))/dy = -((M(P00) - M(P01)) * (x1-x) + (M(P10) - M(P11)) * (x-x0))
// 计算网格值，计算梯度 --- 原版这里的dx、dy的计算有错误，已经改过来了
return Eigen::Vector3f(
((intensities[0] * xFacInv + intensities[1] * factors[0]) * (yFacInv)) +
((intensities[2] * xFacInv + intensities[3] * factors[0]) * (factors[1])),
-((dx1 * yFacInv) + (dx2 * factors[1])),
-((dy1 * xFacInv) + (dy2 * factors[0]))
// -((dx1 * xFacInv) + (dx2 * factors[0])), // 应该为： -((dx1 * yFacInv) + (dx2 * factors[1]))
// -((dy1 * yFacInv) + (dy2 * factors[1])) // 应该为： -((dy1 * xFacInv) + (dy2 * factors[0]))
);
}
4 实验
将 2.2 节中的 cacheMethod 相关语句都注释掉，发现同样跑完数据包时，注释掉 cacheMethod 后最终 odom_hector 的频率是9.249hz ，而没有注释时最终 odom_hector 的频率是8.571hz ，可见这块确实会导致时间增长．
原因猜测是本来只需要计算4次网格值就行了，现在还有在cache中进行查找，如果数据量太大的情况下查找的时间将比直接计算4次网格值的时间要大．
虽然频率确实下降的少了，但是还是下降了，应该是还有别的原因导致计算时间变长．
5 总结与思考
5.1 总结
通过三篇文章对hector进行了讲解。
第一篇文章讲了hector的地图是如何构建的，第二篇文章讲了hector是如何进行slam的，第三篇文章讲了hector论文的公式推导以及公式对应的代码的讲解。
虽然成功的构建出了地图，但是还是有几点不足的。
- 首先就是hector的计算太慢了，只进行扫描匹配的操作就要花费大概0.1秒钟，这就导致即使雷达频率再高，hector也处理不过来
- 还有就是hector的地图不能自动更改大小，地图的大小在初始化之后始终是固定的
- hector中获取栅格点时使用了cache，这块实现的不是特别好
- hector中维护的始终是一张地图，有时会出现地图被错误更新之后再也恢复不了的情况
5.2 思考
gmapping与hector维护地图的方式不同.
gmapping的维护地图是通过保存下来的位姿与相应的雷达数据，每次发布的地图都是新生成的地图，将所有的保存的雷达数据写成地图。
hector的维护地图是在初始化阶段时只初始化一张地图，然后使用新的雷达数据对这张地图进行更新与维护。
这就导致，如果某时刻的雷达点出现了偏差，将未知区域更新成空闲区域了，而如果之后不出现偏差的话，这块空闲地图再也不会被更新了，也不会变成未知区域了。
也就是上一篇地图中走廊两边的墙内区域还有一些白色区域，这块的白色区域就不会被更新，也就是不会消失，不会再变成未知区域。
这点就是始终维护一张地图不好的地方。但是，如果这帧偏离的激光数据被gmapping保存下来了，那接下来gmapping生成的地图也始终会存在这一块区域。
6 NEXT
hector其实还可以做很多改进，如将里程计或者imu加进来做位姿预测，地图大小可以动态更新，计算地图这可以使用ceres库等等。
github上有人对hector的高斯牛顿这的代码做了改进，使用ceres库进行计算，感兴趣的可以在github上搜索 hector_slam_Ceres 查看代码。
由于后边还有karto与cartographer这些更优秀的项目等着我，所以我就不对hector再进行研究了。
接下来的文章将开始使用里程计，首先简单讲解下里程计，之后对激光雷达数据进行畸变校正处理。