1.回环检测

2.图优化

一句话说清楚：

        图优化就是把 SLAM 里机器人位姿和路标点作为图顶点，把里程计约束（如imu预积分计算帧间相对位姿约束，激光里程计通过两帧的点云匹配计算出的帧间约束，回环检测得到闭环后通过点云配准得到回环约束）、观测（视觉slam的观测点约束）作为约束边， 通过最小二乘全局迭代微调所有位姿，消除里程计累积漂移，同时优化轨迹和地图的后端优化方法。最重要的是回环检测约束，它是让图优化起作用的机制，没有回环检测图优化就没有任何作用。

imu预积分与起始的位姿解绑，不会牵一发而动全身，只计算相邻点云帧的相对关系，输出相对旋转，相对平移，相对速度。可作为图优化中的帧间约束，补偿激光的运动畸变。

视频讲解：图优化SLAM_哔哩哔哩_bilibili

第一帧的点云都是正确的，左侧是实际环境图，右侧是将局部测量的点云地图和位姿关系形成pose graph（位姿图）

​

第二帧点云，灰色的车子是由于轮式里程计等里程计的累积误差等误差导致的错误的位姿估算

​

连接两帧之间的edge(边)，边的长度取决于系统对里程计的信任度，信任度越强，长度越固定，否则长度可变化，每一帧的pose成为node(节点)。

​

第三帧点云

​​

一直记录并连接edge,不断填充，从而形成位姿图，直到当前帧与过去的错误地图帧很相似，比如当前帧与第一帧有相同的特征（即通过回环检测可得），根据点云配准（icp）得到精确的相对位姿关系，往位姿图中加一条回环约束边（在该例子中边的长度为0），即可将该位姿图闭合。

​

​​

这样蓝色的边想要靠近，红色的边想要保持不动，故这两个位姿就会自动的靠近过去（实现方式：通过计算机器人每一帧当前位置和所有约束的误差，来不断的迭代优化），所以回环检测是让图优化起作用的机制。

​​

只有一个回环检测，得到的纠正还不够准确，接着机器人继续走，得到另一个闭环，就会继续纠正其他地方的点云位姿。

​​

随着闭环的增多，会越来越准确。但是，如果有错误的闭环，后续就会一步错步步错，很难清除错误的闭环造成的影响。

​