从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析

本文主要是介绍从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

这篇文章将带领大家推导一下hector slam论文中的公式．之后再对这部分公式对应的代码进行讲解下．

markdown打公式太费劲了，所以我用手写了．（懒）　然后csdn又限制了图片文件大小，我是照完照片又截图才传上来的，所以图片有点不清晰．

1 高斯牛顿法

首先借用 <视觉SLAM十四讲> 说明一下高斯牛顿法的思路．

高斯牛顿法首先将目标函数进行一阶泰勒展开，再经过公式推导，可以获得最优解的公式为 $\Delta x=H^{-1}\ast g$　，并且hessian矩阵可以通过 $H=J^T\ast J$ 的形式进行求解，免去了计算hessian矩阵的较大计算量．

以上图片引自 <<视觉SLAM十四讲>> ，如有侵权请联系我进行删除．

2 公式推导

2.1 目标函数推导

目标函数的形式为

${\xi }^{\ast }=argmin({\sum }_{i=1}^N[1-M(S_i(\xi )){]}^2)$ ，

代表当前这帧雷达数据占据地图的概率，$M(S_i(\xi ))$ 最大为1，最大值不好求，所以通过 $1-M(S_i(\xi ))$ 的方式求最小值．

首先，通过右加一个小量 $\Delta \xi$ , 来使得公式趋近于零．

但是由于目标函数是非线性的，不能直接求导，所以根据高斯牛顿法的思路，先进行一阶泰勒展开，然后再进行求导．
之后，进行左右项的变换．求得 $\Delta \xi =H^{-1}\ast g$ ．

2.2 求解

所以，接下来只需要求出 $J(\xi )$ 与 $f(\xi )$ 就可以了．

只需分别求出 栅格值 $M(S_i(\xi ))$ ， 栅格的导数 $\nabla M(S_i(\xi ))$， 激光点转换函数的导数$\nabla S_i(\xi )$，即可．

2.2.1 求激光点转换函数的导数

首先求 激光点转换函数的导数．

激光点转换函数Ｓ是个2*1维的矩阵，第一行是x坐标的转换公式，第二行是y坐标的转换公式．

所以求导时候 分别对第一行第二行，对 $p_x$ , $p_y$ , $\psi$ 三个变量进行求导，将得到一个2*3的矩阵．

2.2.2 求栅格值 $M(S_i(\xi ))$

点(x,y)的栅格值可以通过 这点周围相邻的4个栅格的栅格值 进行插值来表示．

公式是直接从论文里摘下来的，是通过 拉格朗日2次插值 来求的.

2.2.3 求栅格值的导数 $\nabla M(S_i(\xi ))$

栅格值的导数是一个1*2的矩阵，是分别对x方向，y方向的导数.

公式是直接从论文里摘下来了，是通过栅格值分别对x与y求偏导得到的。

其中 y1-y0 = x1-x0=1，所以是如下形式。

2.2.4 求 $J(\xi )$ 与 $f(\xi )$

图片里求 $J(\xi )$ 的最后一步时，对结果进行转置了， $J(\xi )$ 是一个 1*3 的矩阵.

2.2.5 求$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、bfor (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再进行研究了。

接下来的文章将开始使用里程计，首先简单讲解下里程计，之后对激光雷达数据进行畸变校正处理。

这篇关于从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---