（九）ORBSLAM回环检测之几何验证

---

 ORBSLAM2回环检测之几何验证简介

　　回环检测的目的是找到当前场景在历史中是否出现过，如果出现过，那会给我们提供一个非常强的约束条件，把我们偏离很多的轨迹一下子修正到正确的位置上。当然，这么好的东西，有利自然就有弊。万一我们检测出来的回环不是真正的回环，也就是说我们认错了地方，这种时候提供的回环约束会导致轨迹被错误地“修正”了，结果就是估计的轨迹跟真实的轨迹相差十万八千里，这显然是不可接受的。

　　因此，回环检测的正确性就显得非常重要。我们会宁愿不要回环约束，也不要一个错误的回环约束。所以ORBSLAM2中对回环帧的要求非常高，在外观验证/位置识别阶段的筛选就有好几轮，这我们在上一讲列的提纲里介绍过。而完成外观验证后，还要进一步进行几何验证，才能最终确定是否要回环帧。这很像一个公司里面要找一个领导，找到对的人能让公司发展步入正确的轨道，万一遇到不合适的人可能公司就会直接崩溃掉。

　　几何验证很直白的意思，就是用几何约束来判断候选帧是否满足条件，这一讲，我们主要分为以下几个部分来介绍它：

　　1. RANSAC方法；

　　2. 双向优化确定内点；

　　3. ORBSLAM2几何验证方法提纲；

　　4. 重定位；

---

 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