ORBSLAM2回环检测之几何验证简介
回环检测的目的是找到当前场景在历史中是否出现过，如果出现过，那会给我们提供一个非常强的约束条件，把我们偏离很多的轨迹一下子修正到正确的位置上。当然，这么好的东西，有利自然就有弊。万一我们检测出来的回环不是真正的回环，也就是说我们认错了地方，这种时候提供的回环约束会导致轨迹被错误地“修正”了，结果就是估计的轨迹跟真实的轨迹相差十万八千里，这显然是不可接受的。
因此，回环检测的正确性就显得非常重要。我们会宁愿不要回环约束，也不要一个错误的回环约束。所以ORBSLAM2中对回环帧的要求非常高，在外观验证/位置识别阶段的筛选就有好几轮，这我们在上一讲列的提纲里介绍过。而完成外观验证后，还要进一步进行几何验证，才能最终确定是否要回环帧。这很像一个公司里面要找一个领导，找到对的人能让公司发展步入正确的轨道，万一遇到不合适的人可能公司就会直接崩溃掉。
几何验证很直白的意思，就是用几何约束来判断候选帧是否满足条件，这一讲，我们主要分为以下几个部分来介绍它：
1. RANSAC方法；
2. 双向优化确定内点；
3. ORBSLAM2几何验证方法提纲；
RANSAC方法
ORBSLAM2中采用的RANSAC方法中，求解相对位姿的方法是Berthold K. P. Horn在1987年发表的文章”Closed-form solution of absolute orientation using unit quaternions“中所提出的。这是一篇非常厉害的文章，笔者在读这篇文章的时候都感叹作者的水平是如此之高。Horn通过三维匹配点构建优化方程，但是却不需要用迭代优化的方法，直接使用闭解的方法求出了相对旋转、相对位移和尺度因子。接下来，笔者将详细介绍一下这篇文章的主要方法。
假设我们当前有 $n$ 对已匹配的三维点，记为 $P_{l} = \{ p_{l,1}, p_{l,2}, \dots, p_{l,n}\}$ 和 $P_{r} = \{ p_{r,1}, p_{r,2}, \dots, p_{r,n}\}$。
我们需要求解质心，这里我们用左相机和右相机表示两个位置，SLAM问题中通常表示一个相机在两个位置：
左相机三维点：$\bar{P}_{l} = \frac{1}{n}\sum\limits_{i=1}^{n}p_{l,i}$ 并对左相机每个三维点构建新坐标：$p_{l,i}^{'} = p_{l,i} - \bar{p}_{l}$
右相机三维点：$\bar{P}_{r} = \frac{1}{n}\sum\limits_{i=1}^{n}p_{r,i}$ 并对右相机每个三维点构建新坐标：$p_{r,i}^{'} = p_{r,i} - \bar{p}_{r}$
则误差项可以写成：
\begin{equation}
e_{i} = p_{r,i}^{'} - sR_{rl}p_{l,i}^{'} - t_{rl}^{'}
\end{equation}
其中 $t_{rl}^{'} = t_{rl} - \bar{P}_{r} + sR_{rl}\bar{P}_{l}$
则代价函数则可以写成：
\begin{equation}
J = \sum\limits_{i=1}^{n} \|p_{r,i}^{'} - sR_{rl}p_{l,i}^{'} - t_{rl}^{'}\|^{2}
\end{equation}
将代价函数展开，我们得到：
\begin{equation}
J = \sum\limits_{i=1}^{n} \| p_{r,i}^{'} - sR_{rl}p_{l,i}^{'} \|^{2} - 2t_{rl}^{'}\cdot \sum\limits_{i=1}^{n}[ p_{r,i}^{'} - sR_{rl}p_{l,i}^{'}] + n\|t_{rl}^{'}\|^{2}
\end{equation}
从质心的定义可知，上式中间项为0，则只剩下 $J = \sum\limits_{i=1}^{n} \| p_{r,i}^{'} - sR_{rl}p_{l,i}^{'} \|^{2} + n\|t_{rl}^{'}\|^{2}$ 两项。
我们分析一下剩余的这两项，第一项与尺度和旋转都有关系，比较复杂，而第二项只是位移的二次方，显然两项均不小于0。理想的情况自然是令第二项为0，而优化第一项。
那么我们就有 $t_{rl}^{'} = 0$，由前面的关系我们知道 $t_{rl}^{'} = t_{rl} - \bar{P}_{r} + sR_{rl}\bar{P}_{l} = 0$，即可求位移： $t_{rl} = \bar{P}_{r} - sR_{rl}\bar{P}_{l}$。
也就是说，我们知道了尺度和旋转，我们就可以用一个减法直接得到平移了，所以我们的任务从求位姿简化成求旋转和尺度因子。也就是说我们的代价函数已经进一步简化成求解下列代价函数的最小值：
\begin{equation}
J = \sum\limits_{i=1}^{n} \| p_{r,i}^{'} - sR_{rl}p_{l,i}^{'} \|^{2}
\end{equation}
由于向量的模长并不受旋转的影响，所以 $\|R_{rl}p_{l,i}^{'}\|^{2} = \|p_{l,i}^{'}\|^{2}$，展开代价函数，我们有：
\begin{equation}
J = \sum\limits_{i=1}^{n} \| p_{r,i}^{'} \|^{2} - 2s\sum\limits_{i=1}^{n}p_{r,i}^{'}\cdot R_{rl}p_{l,i}^{'} + s^{2}\sum\limits_{i=1}^{n}\|p_{l,i}^{'}\|^{2}
\end{equation}
我们将上面的式子进行代换，可以得到：
\begin{equation}
J = S_{r} - 2sD + s^{2}S_{l} = (s\sqrt{S_{l}} - \frac{D}{\sqrt{S_{l}}})^{2} + \frac{(S_{r}S_{l} - D^{2})}{S_{l}}
\end{equation}
显然，令二次项为0，则该代价函数取得最小值，这时，我们得到了尺度因子：
\begin{equation}
s = D/S_{l} = \sum\limits_{i=1}^{n} p_{r,i}^{'}\cdot R_{rl}p_{l,i}^{'} / \sum\limits_{i=1}^{n}\|p_{l,i}^{'}\|^{2}
\end{equation}
于是，我们将尺度因子表示成了与旋转有关的式子。当然，该文还提供了尺度的对称求解方法，这种方法的尺度因子最终表示为：
\begin{equation}
s = (\sum\limits_{i=1}^{n} \|p_{r,i}^{'}\|^{2} / \sum\limits_{i=1}^{n}\|p_{l,i}^{'}\|^{2})^{1/2}
\end{equation}
值得注意的是，公式 $(8)$ 的这种解法，不依赖于旋转就可以求出尺度因子。不过ORBSLAM2求尺度因子用的是第一种方法，因此我们就没有详细介绍这种对称求解的方法了。
不论是用哪种方法求出的尺度因子，都对旋转没有影响。而在代价函数的剩余部分，当 $D$ 越大，代价值就越小。于是，我们的目标就变成了使下面的式子越大越好：
\begin{equation}
J_{new} = \sum\limits_{i=1}^{n} p_{r,i}^{'}\cdot R_{rl}p_{l,i}^{'}
\end{equation}
到这里，我们先小结一下求解位姿的步骤：
1. 将求解位姿的问题拆分成求解旋转、平移和尺度因子；
2. 将位移表示成旋转和尺度因子，即只需要一个减法就能求出平移；
3. 将尺度因子表示成旋转，只需要求解一个简单的累加和除法即可求出尺度因子；
4. 最终的目标就只需要求出旋转。
所以，我们的目标很明确，就是先把旋转求出来。尺度因子和平移自然就知道了。
利用四元数表示旋转 $R_{rl}$，另外，我们用一个四元数表示三维点（纯虚数，实数项为0，虚数项为X,Y,Z），而两个四元数叉乘的结果不一定是纯虚数，因此作者利用 $\mathring{q}\mathring{p_{l,i}^{'}}\mathring{q^{*}}$ 来表示 $R_{rl}p_{l,i}^{'}$。于是代价函数 $J_{new}$ 可以表示成：
\begin{equation}
J_{new} = \sum\limits_{i=1}^{n} (\mathring{q}\mathring{p_{l,i}^{'}}\mathring{q^{*}})\cdot \mathring{p_{r,i}^{'}} = \sum\limits_{i=1}^{n} (\mathring{q}\mathring{p_{l,i}^{'}})\cdot (\mathring{p_{r,i}^{'}}\mathring{q})
\end{equation}
进一步，我们有：
\begin{equation}
J_{new} = \sum\limits_{i=1}^{n}( \bar{R_{l,i}}\mathring{q})\cdot ({R_{r,i}}\mathring{q}) = \sum\limits_{i=1}^{n} \mathring{q}^{T}\bar{R_{l,i}}^{T}R_{r,i}\mathring{q} = \mathring{q}^{T}\lgroup \sum\limits_{i=1}^{n}\bar{R_{l,i}^{T}}R_{r,i} \rgroup\mathring{q} = \mathring{q}^{T}N\mathring{q}
\end{equation}
于是，我们有 $N = \sum\limits_{i=1}^{n}N_{i} = \sum\limits_{i=1}^{n}\bar{R_{l,i}^{T}R_{r,i}}$。
更确切的计算如下：
$N = \begin{bmatrix}
(S_{xx} + S_{yy} + S_{zz}) & S_{yz} - S_{zy} & S_{zx} - S_{xz} & S_{xy} - S_{yx} \\
S_{yz} - S_{zy} & (S_{xx} - S_{yy} - S_{zz}) & S_{xy} + S_{yx} & S_{zx} + S_{xz} \\
S_{zx} - S_{xz} & S_{xy} + S_{yx} & (-S_{xx} + S_{yy} - S_{zz}) & S_{yz} + S_{zy} \\
S_{xy} - S_{yx} & S_{zx} + S_{xz} & S_{yz} + S_{zy} & (-S_{xx} - S_{yy} + S_{zz})
\end{bmatrix}$
为了计算 $N$，我们需要构造 $M$ 矩阵。$M$ 矩阵的组成为：
$M = \begin{bmatrix}
S_{xx} & S_{xy} & S_{xz} \\
S_{yx} & S_{yy} & S_{yz} \\
S_{zx} & S_{zy} & S_{zz}
\end{bmatrix}$
其中，$p_{l,i}^{'} = (x_{l,i}^{'}, y_{l,i}^{'}, z_{l,i}^{'})^{T}$，$p_{r,i}^{'} = (x_{r,i}^{'}, y_{r,i}^{'}, z_{r,i}^{'})^{T}$。$S_{xx} = \sum\limits_{i=1}^{n}x_{l,i}^{'}x_{r,i}^{'}$，$S_{xy} = \sum\limits_{i=1}^{n}x_{l,i}^{'}y_{r,i}^{'}$，并以此类推其他项。
而表示旋转的四元数即是矩阵 $N$ 的最大特征值对应的特征向量。求解特征值和特征向量有许多工具，这里笔者就不过多解释了。
至此，我们求解得到了位姿中的旋转，通过旋转我们可以求得尺度因子，最后得到平移，从而求出完整的位姿。于是，这篇文章的内容，基本介绍完了。
------------------------------
啊，花了很大的篇幅介绍了Horn的这篇文章，现在才刚刚进入RANSAC的具体实施中。实际上，前面的求位姿的方法介绍完了，RANSAC的内容只有一点点。
我们通过当前帧和回环候选帧构建匹配关系，生成三维地图点的匹配关系。通过我们上面介绍的求位姿闭解的方法求出这两帧的相对变换关系。
假设当前帧到回环帧的变换为 $T_{lc}$，逆变换为 $T_{cl}$，当前帧坐标系下的三维点 $P_{c}$ 对应的像素为 $p_{c}$，同理，回环候选帧坐标系下与 $P_{c}$ 匹配的三维点 $P_{l}$ 及其对应的像素为 $p_{l}$，则重投影误差为：
\begin{equation}
e_{lc} = p_{l} - T_{lc}P_{c}
\end{equation}
\begin{equation}
e_{cl} = p_{c} - T_{cl}P_{l}
\end{equation}
预设阈值分别为 $E_{1}$ 和 $E_{2}$，若 $e_{lc} < E_{1}$ 且 $e_{cl} < E_{2}$ 则说明该匹配对构建的三维点是内点。当内点数量满足要求时，ORBSLAM2会返回一个相对姿态，用于进一步做筛选。否则继续计算下一个回环候选帧与当前帧的内点数量，方法不变。
所以，RANSAC的内容基本上就这么多啦。
双向优化确定内点
在介绍完RANSAC以后，有个优化方法也需要再讲一讲。
在我们先前介绍估计相对运动的时候提到过，我们是用PNP，即重投影误差的方式来算相对运动。不过，我们当时用的只是单向的投影，怎么理解呢？就是我们把第一帧看到的三维点，投影到第二帧的像平面中，与匹配的像素点计算误差，不断优化相机位姿来使得所有这些投影误差最小。
在几何验证阶段的这个误差函数被稍稍拓展了一下，变成了需要向两边投影，算重投影误差。对单个点而言，实际上就是公式 $(12)$ 和 $(13)$ 所表示的形式。于是，我们对于 $n$ 个匹配对，我们可以构建如下的代价函数：
\begin{equation}
J = \sum\limits_{i=1}^{n}\|e_{cl,i}\|^{2} + \|e_{lc,i}\|^{2} = \sum\limits_{i=1}^{n}\|(p_{c,i} - T_{cl}P_{l,i})\|^{2}+ \|(p_{l,i} - T_{lc}P_{c,i})\|^{2}
\end{equation}
这个代价函数的优化方法跟我们在第四讲：https://www.cnblogs.com/yepeichu/p/10746952.html 所用的方法并无二致，所以笔者就不再赘述了。
优化后，每一个点的误差项，即公式 $(12)$ 和公式 $(13)$，同时满足误差阈值条件时，确定为内点，否则剔除。
在第一次优化后，我们确定了内点。根据这些内点，再用同样的方法对内点执行一次优化，最后再根据阈值条件统计内点，确定最终内点数量。
ORBSLAM2几何验证方法提纲
介绍完主要技术，笔者再为大家罗列一下ORBSLAM2几何验证的提纲：
1. 遍历位置识别中选择的回环候选帧；
2. 对当前帧与回环候选帧进行特征匹配，确定匹配数量和对应地图点；
3. 对当前帧和回环候选帧的匹配对进行RANSAC，用我们前面所介绍的方法计算相对位姿，然后提出外点；
4. 若RANSAC阶段内点数满足要求，会返回一个相对位姿，基于相对位姿进行重投影搜索匹配，这个我们在前面介绍过：https://www.cnblogs.com/yepeichu/p/10746952.html；
5. 根据搜索结果，我们执行前面所介绍的双向优化，根据阈值条件筛选内点；
6. 基于步骤5的内点，ORBSLAM2再次执行了多次迭代的优化，从而最终确定内点数量；
7. 根据内点数量与阈值条件，确定最终的回环帧；
8. 利用回环帧的共视图构建局部地图点，通过估算的相对位姿进行重投影匹配，确定匹配数量；
9. 根据匹配数量，确定回环检测的结果。
至此，回环检测模块就全部介绍了，真是个漫长的过程啊。
总结：
本文主要介绍了ORBSLAM2中回环检测模块里的几何验证方法：
涉及的技术包括：RANSAC方法和双向优化。实际上，双向优化也没有多特殊，只是在我们前面所介绍的优化方法中，加了个反向投影而已。因此，本文主要的篇幅都是在介绍RANSAC方法。
此外，我们还罗列了ORBSLAM2中几何验证模块的方法提纲，供大家参考。
下一讲，笔者基于回环检测的结果将为大家介绍闭环模块。
参考文献：
[1] 视觉SLAM十四讲
[2] 机器人学中的状态估计
[3] Closed-form solution of absolute orientation using unit quaternions
PS：
如果您觉得我的博客对您有所帮助，欢迎关注我的博客。此外，欢迎转载我的文章，但请注明出处链接。
对本文有任何问题可以在留言区进行评论，也可以在泡泡机器人论坛：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技术交流模块发帖提问。
我的github链接是：https://github.com/yepeichu123/orbslam2_learn。