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。

（九）ORBSLAM回環檢測之幾何驗證