目录

  1.原理详解

  2.算法解析

---

1.原理详解

Hector整体算法很直接，就是将激光点与已有的地图“对齐”，即扫描匹配。扫描匹配就是使用当前帧与已经有的地图数据构建误差函数，使用高斯牛顿法得到最优解和偏差量。其工作是实现激光点到栅格地图的转换，t时刻所有的激光点都能变换到栅格地图中，也就意味着匹配成功。

  具体流程为：

首先初始时刻激光自身的坐标系与栅格地图坐标系重合，即激光在地图中的初始位姿（estimate）已知，激光的第一帧扫描数据在地图中的坐标已知。接着，获取到第二帧激光扫描数据，第二帧数据在激光雷达坐标系下的坐标是可以测出的（根据激光雷达的range、angle便可得到在激光坐标系下的坐标），但是不知道与第一帧的相对位置关系。

下一步就是实现这两帧数据的匹配，我们假设两帧数据无限接近，即激光点在栅格地图占用值接近1（占用值越大，匹配的效果越好）。然后构造最小二乘法，对函数先对括号内部展开，然后对使误差偏导为0，求解高斯牛顿方程（其中有用到地图求偏导的方程（双线性插值法），带入即可求出位姿增量（在hector位姿增量用变量searchDir）。

 接下来就可求出第二帧激光在地图坐标系下的位姿（第一帧位姿加上位姿增量即可得到，estimate += searchDir），slam中定位完成。

接下来建图，后一帧的激光位姿求出，因为已知后一帧激光点在激光坐标系下的坐标，所以可根据后一帧激光位姿得后一帧激光点在地图中坐标，即映射到地图中，完成slam建图过程。

cartographer中的前端匹配使用了双三次线性插值＋ceres库求解非线性优化问题(构造最小二乘，优化匹配)，而hector slam中使用了双线性插值＋高斯牛顿求解非线性优化问题

hector存在的问题：
1、其中对于双线性差值，在理论上存在不连续的可能，Pm可能在计算的时候迭代的过程中跑出P00->P11围成的正方形。这个问题也被google的cartographer改进为三线性差值。

https://github.com.cnpmjs.org/googlecartographer/cartographer
2、没有对地图的修正能力，一旦地图出错，之后的匹配也都会出现问题。

2.算法解析

算法解析参考