回环检测回环校正（二）：回环校正原理

回环检测回环校正（二）：回环校正原理

一、回环校正概述

参考：[1]徐宽. 融合IMU信息的双目视觉SLAM研究[D].哈尔滨工业大学,2018.

回环校正所要做的事是如何利用回环信息消除累计的误差。回环校正主要分为两个步骤：

1、首先，进行回环融合，回环融合就是将回环检测匹配到的特征点所相关联的地图点融合，将重复的地图点剔除。
2、然后进行当前帧位姿校正，利用回环检测得到的基础矩阵算出当前帧和回环帧之间的位姿转换关系，将当前帧的位姿校正。
3、最后进行所有帧位姿校正，利用 BA（Bundle Adjustment，光束平差法）将回环内图像的所有位姿进行校正。

二、几种图

1、共视图（Covisibility Graph）

共视图是一种无向加权图。图的每个顶点表示某个时刻相机的状态，即位姿。图的边连接两个顶点，表示这两个状态下的相机位姿的变换联系。当两个相机看到相似的空间点时，它们对应的位姿就会产生联系（我们就可以根据这些空间点在照片上的投影计算两个相机间的运动）。根据观测到的空间点的数量，给这个边加上一个权值，度量这个边的可信程度。如图是一个共视图，其中蓝色的顶点表示相机状态点，绿色的线连接两个蓝色顶点，表示这两个时刻的相机位姿的变换关系。

2、本质图（Essential Graph）

本质图是ORB-SLAM提出的概念，其尽可能地减少边数，从而减小全局回环的计算量。因为，如果共视图中的两个顶点能够共同观测到更多地图点（根据共同地图点评估运动），说明它们有更强的连接关系。所以，利用这个特点，可以对共视图进行一些简化，得到本质图，具体过程如下：

设置一个合适的阈值，如果两个顶点能够观测到的地图点数量大于这个阈值，则它们之间的边会被保存，否则将被剔除。但是，若如果某一组全部顶点和另一组顶点的边全部都被剔除了，那么优化的时候这两组顶点就会被分开优化，不包含回环信息的那组相机的位姿将不会被得到优化，这是所不希望看到的。所以，在剔除弱连接关系的边之前，先建立一个原来共视图的生成树，在此基础上在将一些不需要的边剔除，这样最终得到的也是一个原来共视图的生成树，并且顶点之间大多能够观测到足够多数量的地图点。回环校正便是在本质图上进行的（ORB-SLAM）。

三、BA（Bundle Adjustment）

参考：Bundle Adjustment—即最小化重投影误差

BA，Bundle Adjustment，译为光束平差法，其本质是一个优化模型，目的是最小化重投影误差。

光束

光束指的是三维空间中的点投影到像平面上的光束，而重投影误差正是利用这些光束来构建的，因此称为光束法，强调光束也正是描述其优化模型是如何建立的。

平差

在测量时，为了尽可能减少测量误差，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。平差采用的原理就是“最小二乘法”。

重投影误差

重投影也就是指的第二次投影：

第一次投影：指的就是相机在拍照的时候真实三维空间点在图像平面上的投影点；
第二次投影（重投影）：我们利用图像对一些特征点先通过对极几何计算出两帧图片之间相机的运动。在得到相机运动之后，再通过三角测量估计特征点的空间位置。然后利用我们计算得到的三维点的坐标（注意不是真实的）和我们计算得到的相机位姿（当然也不是真实的）进行第二次投影，也就是重投影。

真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值，就是重投影误差。

BA

1、构建误差函数

BA是一个图优化模型，如下：

2、非线性优化

非线性优化的过程是用算法优化相机的位姿使误差函数最小
BA现在基本都是利用LM（Levenberg-Marquardt）算法并在此基础上利用BA模型的稀疏性质来进行计算的，LM算法是最速下降法（梯度下降法）和Gauss-Newton的结合体。

3、解方程（略）