ORBSLAM2的運動估計簡介
ORBSLAM2中的運動估計核心方法就是3D-2D的PNP，而在跟蹤過程主要分為三種類型：
- 無運動模型的跟蹤，即基於參考幀的跟蹤；
- 基於勻速運動模型的跟蹤；
- 重定位；
上述三種方案，我們只介紹前兩種，重定位由於需要用到回環檢測，我們會在之后講解。
PNP運動估計
在介紹ORBSLAM2的跟蹤策略之前，我們先了解一下他所用的運動估計方法——PNP。
PNP是一種將匹配點從三維空間投影到像平面並與觀測數據計算誤差來估計相機運動的方法，我們也管這種方法叫重投影誤差。基於解析的PNP方法只采用了少量的匹配對即可估計相對運動，然而，在SLAM問題當中，通常約束條件較多，因此基於解析的方法通常不能更好地利用約束條件。另外，若將錯誤匹配對納入解析方程中，會錯誤估計相機運動。盡管可以通過隨機選取多組匹配對進行估算多個相機運動並根據一定條件進行篩選出最合適的相機運動，但是筆者更傾向於用PNP解析解估算出一個初值，隨后通過非線性優化的方法來優化相機運動。不過，值得一提的是，ORBSLAM2並不用PNP求初值，而是直接用參考幀的運動作為當前幀的運動初值進行優化。因此，本文只介紹基於非線性優化的PNP方法。
與之前一樣，為了讓大家更直觀的看到問題的描述，筆者為大家提供了一個圖：
其中，空間點 $P$ 齊次坐標為 $P=[X,Y,Z,1]^{T}$，其在圖像 $I_1$ 中投影的像素坐標為 $p_1=[u_1,v_1]^{T}$，在圖像 $I_2$ 中的重投影像素坐標為 $p_2^{'}=[u_2^{'},v_2^{'}]^{T}$，而空間點 $P$ 在圖像 $I_2$ 中的觀測值為 $p_2=[u_2,v_2]^{T}$ ,$e=p_2-p_2^{'}$ 表示重投影誤差。
圖中的理想重投影過程用公式$(1)$表達如下：
\begin{equation}
s_2u_2=Kexp(\xi^{\land})P
\end{equation}
其中，$s_2$ 表示空間點 $P$ 在圖像 $I_2$ 所在相機坐標系的深度。$K$ 表示相機內參數，$exp( \xi^{\land} )$ 表示相機從圖像 $I_1$ 到圖像 $I_2$ 的姿態變換陣，也可以用 $T$ 表示，$\xi$ 表示 $T$ 對應的李代數。
然而，由於相機姿態初值估計不夠好，因此重投影時通常會與真實值存在一定的誤差，這個誤差我們定義如下：
\begin{equation}
e_2 = u_2 - \frac{1}{s_2}Kexp(\xi^{\land})P
\end{equation}
一個相機姿態下觀測到的特征點通常不止一個，假設我們有 $N$ 個特征點，則構成求相機姿態 $\xi$ 的最小二乘問題：
\begin{equation}
\xi^{\ast} = \mathop{\arg\min}_{\xi}\frac{1}{2}\sum_{i=1}^{N}\left\|e_i\right\|_2^{2} = \mathop{\arg\min}_{\xi}\frac{1}{2}\sum_{i=1}^{N}\left\|u_i - \frac{1}{s_i}Kexp(\xi^{\land})P_i\right\|_2^{2}
\end{equation}
為了滿足所有重投影誤差最小，我們通過不斷調整$\xi$的值，使得上述誤差不斷下降，最終得到一個對當前匹配對最優的相機姿態。但是 $\xi$ 應該怎么調整？我們知道最小二乘優化是通過不斷調整增量，使得整體的誤差不斷下降，直到收斂的過程。在這個過程中，我們需要選擇增量的方向和步長。根據優化理論知識我們可知，求出誤差相對於位姿的雅克比，這個優化問題我們基本上就算解決了。如果用了牛頓法，我們還需要再求個海塞陣，但通常我們都會用高斯牛頓法來替代牛頓法，避免計算海塞陣。
通過鏈式法則，我們可以求出誤差相對於位姿的雅克比：
\begin{equation}
\frac{\partial e}{\partial \delta\xi}=\frac{\partial e }{\partial P^{'}}\frac{\partial P^{'}}{\partial\delta\xi}
\end{equation}
其中，$P^{'}$ 表示 $P$ 位於圖像 $I_2$ 處的相機坐標系下的坐標。
上述雅克比為：
\begin{equation}
\frac{\partial e}{\partial \delta\xi} = -{\left[ \begin{array}{cccccc}\frac{f_{x}}{Z^{'}} & 0 & -\frac{f_{x}X^{'}}{Z^{'2}} & -\frac{f_{x}X^{'}Y^{'}}{Z^{'2}} & f_{x}+\frac{f_{x}X^{'2}}{Z^{'2}} & -\frac{f_{x}Y^{'}}{Z^{'}} \\
0 & \frac{f_{y}}{Z^{'}} & -\frac{f_{y}Y^{'}}{Z^{'2}} & -f_{y}-\frac{f_{y}Y^{'2}}{Z^{'2}} & \frac{f_{y}X^{'}Y^{'}}{Z^{'2}} & \frac{f_{y}X^{'}}{Z^{'}} \end{array} \right]}
\end{equation}
得到雅克比以后我們就可以通過g2o或者ceres來求解公式 $(3)$ 的最小二乘問題了。（具體源碼，之后筆者會更新在github上，並將在此附上鏈接）
那么，到這里，基於非線性優化的PNP方法就差不多介紹完了。如果我們公式 $(3)$ 拓展一下，將參考幀的位姿和所有共同觀測點作為優化變量加入優化函數中，我們就得到了完整的bundle adjustment，可以同時優化兩個姿態和三維空間點。
對基於PNP解析方法比較感興趣的同學可以參考：
塗金戈同學的博客：https://www.cnblogs.com/JingeTU/p/10646322.html ；
或者參考高翔博士的《視覺SLAM十四講》。
--------------------------------------------------------------
稍微加個插曲：
在優化算法上，筆者根據自己的理解寫出部分優化算法的優缺點（這也是視覺SLAM十四講中的課后思考題）：
實際上，由於梯度下降法過於貪心，通常會導致鋸齒狀下降，導致收斂速度緩慢；
而牛頓法盡管使用了二階近似，提高了下降速度，但對於SLAM問題而言，求解海塞陣的計算量太大，非常不適用；
基於牛頓法改進的高斯牛頓法，通過二次近似二階泰勒展開，利用雅克比的二次方 $J^{T}J$ 來近似海塞陣，在計算量和下降速度上可以較好滿足SLAM的需求，但存在的隱患是增量的范圍超出泰勒展開時限定的微小鄰域，導致近似失敗；
列文伯格-馬誇爾特法是基於高斯牛頓法進一步改進的，通過限定搜索區域，防止出現近似失敗的情況，LM方法也是SLAM中最常用的優化方法。
基於參考幀的跟蹤
當沒有運動模型時，ORBSLAM2采用跟蹤參考幀的方式估計相機的相對運動。
在ORBSLAM2中，參考關鍵幀的大部分特征點在地圖中都有對應三維點。輸入當前幀后，通過上一講所介紹的特征匹配后，我們確定了當前幀和參考關鍵幀之間的二維特征匹配關系。已知參考關鍵幀特征點對應的地圖三維點，我們很容易就可以得到匹配的三維空間點與當前幀特征點的對應關系。即我們有多對匹配的3D-2D點，於是我們很容易想到利用前面我們介紹的PNP方法來優化位姿。
在前面我們提到過，ORBSLAM2並沒有用解析解的方法求解一個相機位姿作為初值。在基於參考幀的跟蹤中，ORBSLAM2用上一幀的相機姿態作為初值進行迭代優化。在優化階段，有一些細節需要稍微注意一下：
1. 初始化階段，設置當前幀相機位姿作為節點。空間三維點都只作為觀測信息，在此階段不對空間三維點進行優化；
2. 優化過程循環了4次，前兩次優化調用了魯棒核，防止誤差值太過發散，更主要的原因是抑制錯誤匹配對的影響。后兩次關閉了魯棒核，認為前兩次的優化基本上抑制了錯誤匹配的影響。每次優化迭代詩詞；
3. 每次優化結束后，根據預設誤差閾值判斷每一個特征點是內點還是外點。
優化結束后，根據判斷條件，將所有匹配的外點全部刪除，統計內點（觀測次數大於0）數量。若內點數量大於10，則認為跟蹤成功，否則跟蹤失敗。
基於勻速運動模型的跟蹤
ORBSLAM2中每次跟蹤成功后，都會將兩幀間的相對運動記錄下來作為運動模型。在估計下一幀運動時，將前一幀的姿態，乘上這個運動模型，就得到了當前幀的姿態初值。能用公式的地方，我們堅決不含糊，畢竟數學公式是沒有歧義性的，因此先上公式：
假設前一幀的相機姿態是 $T_{lw}$ ，表示從世界坐標系到前一幀相機坐標系的相對變換。運動模型為 $T_{cl}$ ，表示從前一幀到當前幀的相對變換。則當前幀的相機姿態初值可以描述為：
\begin{equation}
T_{cw} = T_{cl}T_{lw}
\end{equation}
確定了姿態初值，我們還需要查找匹配對。由於我們根據前一幀的相機姿態加上運動模型得到了當前幀的姿態，所以我們在需要在前一幀中查找匹配對。
查找匹配對的方法非常有意思，有很值得大家借鑒：
1. 從相機姿態中取出前一幀和當前幀對應的旋轉和位移；
2. 逐個取出前一幀特征點 $f_{i}$ 對應的地圖點 $pM_{i}$，並將其投影到當前幀的像平面中；
3. 根據設置的搜索區域閾值，確定搜索窗口半徑 $r$；
4. 獲取當前幀中處於搜索窗口區域的所有候選特征點 $\left\{ f_{c}^{1}, f_{c}^{2}, \dots, f_{c}^{m}\right\}$；
5. 將前一幀的特征點 $f_{i}$ 與上述所有候選特征點進行匹配，並根據閾值條件篩選最優匹配；
6. 同樣統計匹配對的特征主方向角度差值，取數量最多的前三個角度區間對應的匹配對作為最終匹配對；
7. 若匹配對數量低於20，擴大搜索區域，重復步驟4-6，若仍然小於20，則跟蹤失敗。
除了上述姿態初值的選擇方式和匹配對的選擇方式不同之外，兩種跟蹤方法在優化相機位姿時采用的步驟是一致的。因此，優化的方法就不再贅述了。（后續筆者會實現這部分功能，屆時會將源碼公布在github上，並在此提供源碼鏈接）
總結：
本文主要介紹了ORBSLAM2中的跟蹤方案：
涉及的運動估計方法主要是PNP，因此我們詳細介紹了利用非線性優化的PNP方法；
此外，我們還分析了兩種跟蹤方案的不同特點，大家可以根據實際需要選擇跟蹤方案，亦或是像ORBSLAM2一樣，二者結合。
另外，由於本文中已經涉及了PNP的非線性優化方法，局部優化我們就不單獨寫一講了，詳細的優化內容在本文中查看。之后介紹閉環時，我們也會細致地分析相關優化內容。
局部優化：我們通過擴大關鍵幀的數目，將當前幀關聯的所有關鍵幀以及對應的所有觀測點全部包含進來優化問題中，構成了更大的優化問題。利用同樣的優化策略，我們就實現了局部的優化，防止相機姿態隨着時間變化而漂移。
下一講，筆者將為大家介紹前端中一個很重要的模塊：關鍵幀的篩選和插入。
參考文獻：
[1] 視覺SLAM十四講
[2] 機器人學中的狀態估計
PS：
如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請注明出處鏈接。
對本文有任何問題可以在留言區進行評論，也可以在泡泡機器人論壇：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技術交流模塊發帖提問。
我的github鏈接是：https://github.com/yepeichu123/orbslam2_learn。