orbslam2的運動估計簡介
orbslam2中的運動估計核心方法就是3d-2d的pnp，而在跟蹤過程主要分為三種型別：
無運動模型的跟蹤，即基於參考幀的跟蹤；
基於勻速運動模型的跟蹤；
重定位；
上述三種方案，我們只介紹前兩種，重定位由於需要用到回環檢測，我們會在之後講解。
pnp運動估計
在介紹orbslam2的跟蹤策略之前，我們先了解一下他所用的運動估計方法——pnp。
pnp是一種將匹配點從三維空間投影到像平面並與觀測資料計算誤差來估計相機運動的方法，我們也管這種方法叫重投影誤差。基於解析的pnp方法只採用了少量的匹配對即可估計相對運動，然而，在slam問題當中，通常約束條件較多，因此基於解析的方法通常不能更好地利用約束條件。另外，若將錯誤匹配對納入解析方程中，會錯誤估計相機運動。儘管可以通過隨機選取多組匹配對進行估算多個相機運動並根據一定條件進行篩選出最合適的相機運動，但是筆者更傾向於用pnp解析解估算出乙個初值，隨後通過非線性優化的方法來優化相機運動。不過，值得一提的是，orbslam2並不用pnp求初值，而是直接用參考幀的運動作為當前幀的運動初值進行優化。因此，本文只介紹基於非線性優化的pnp方法。
其中，空間點 $p$ 齊次座標為 $p=[x,y,z,1]^$，其在影象 $i_1$ 中投影的畫素座標為 $p_1=[u_1,v_1]^$，在影象 $i_2$ 中的重投影畫素座標為 $p_2^=[u_2^,v_2^]^$，而空間點 $p$ 在影象 $i_2$ 中的觀測值為 $p_2=[u_2,v_2]^$ ,$e=p_2-p_2^$ 表示重投影誤差。
圖中的理想重投影過程用公式$(1)$表達如下：
\begin
s_2u_2=kexp(\xi^)p
\end
其中，$s_2$ 表示空間點 $p$ 在影象 $i_2$ 所在相機座標系的深度。$k$ 表示相機內引數，$exp( \xi^ )$ 表示相機從影象 $i_1$ 到影象 $i_2$ 的姿態變換陣，也可以用 $t$ 表示，$\xi$ 表示 $t$ 對應的李代數。
然而，由於相機姿態初值估計不夠好，因此重投影時通常會與真實值存在一定的誤差，這個誤差我們定義如下：
\begin
e_2 = u_2 - \frackexp(\xi^)p
\end
乙個相機姿態下觀測到的特徵點通常不止乙個，假設我們有 $n$ 個特徵點，則構成求相機姿態 $\xi$ 的最小二乘問題：
\begin
\xi^ = \mathop_\frac\sum_^\left\|e_i\right\|_2^ = \mathop_\frac\sum_^\left\|u_i - \frackexp(\xi^)p_i\right\|_2^
\end
為了滿足所有重投影誤差最小，我們通過不斷調整$\xi$的值，使得上述誤差不斷下降，最終得到乙個對當前匹配對最優的相機姿態。但是 $\xi$ 應該怎麼調整？我們知道最小二乘優化是通過不斷調整增量，使得整體的誤差不斷下降，直到收斂的過程。在這個過程中，我們需要選擇增量的方向和步長。根據優化理論知識我們可知，求出誤差相對於位姿的雅克比，這個優化問題我們基本上就算解決了。如果用了牛頓法，我們還需要再求個海塞陣，但通常我們都會用高斯牛頓法來替代牛頓法，避免計算海塞陣。
通過鏈式法則，我們可以求出誤差相對於位姿的雅克比：
\begin
\frac=\frac}\frac}
\end
其中，$p^$ 表示 $p$ 位於影象 $i_2$ 處的相機座標系下的座標。
上述雅克比為：
\begin
\frac = -\frac}} & 0 & -\fracx^}} & -\fracx^y^}} & f_+\fracx^}} & -\fracy^}} \\
0 & \frac}} & -\fracy^}} & -f_-\fracy^}} & \fracx^y^}} & \fracx^}} \end \right]}
\end
得到雅克比以後我們就可以通過g2o或者ceres來求解公式 $(3)$ 的最小二乘問題了。(具體原始碼，之後筆者會更新在github上，並將在此附上鏈結)
那麼，到這裡，基於非線性優化的pnp方法就差不多介紹完了。如果我們公式 $(3)$ 拓展一下，將參考幀的位姿和所有共同觀測點作為優化變數加入優化函式中，我們就得到了完整的bundle adjustment，可以同時優化兩個姿態和三維空間點。
對基於pnp解析方法比較感興趣的同學可以參考：
塗金戈同學的部落格： ；
或者參考高翔博士的《視覺slam十四講》。
稍微加個插曲：
在優化演算法上，筆者根據自己的理解寫出部分優化演算法的優缺點(這也是視覺slam十四講中的課後思考題)：
實際上，由於梯度下降法過於貪心，通常會導致鋸齒狀下降，導致收斂速度緩慢；
而牛頓法儘管使用了二階近似，提高了下降速度，但對於slam問題而言，求解海塞陣的計算量太大，非常不適用；
基於牛頓法改進的高斯牛頓法，通過二次近似二階泰勒展開，利用雅克比的二次方 $j^j$ 來近似海塞陣，在計算量和下降速度上可以較好滿足slam的需求，但存在的隱患是增量的範圍超出泰勒展開時限定的微小鄰域，導致近似失敗；
列文伯格-馬誇爾特法是基於高斯牛頓法進一步改進的，通過限定搜尋區域，防止出現近似失敗的情況，lm方法也是slam中最常用的優化方法。
基於參考幀的跟蹤
當沒有運動模型時，orbslam2採用跟蹤參考幀的方式估計相機的相對運動。
在orbslam2中，參考關鍵幀的大部分特徵點在地圖中都有對應三維點。輸入當前幀後，通過上一講所介紹的特徵匹配後，我們確定了當前幀和參考關鍵幀之間的二維特徵匹配關係。已知參考關鍵幀特徵點對應的地圖三維點，我們很容易就可以得到匹配的三維空間點與當前幀特徵點的對應關係。即我們有多對匹配的3d-2d點，於是我們很容易想到利用前面我們介紹的pnp方法來優化位姿。
在前面我們提到過，orbslam2並沒有用解析解的方法求解乙個相機位姿作為初值。在基於參考幀的跟蹤中，orbslam2用上一幀的相機姿態作為初值進行迭代優化。在優化階段，有一些細節需要稍微注意一下：
1. 初始化階段，設定當前幀相機位姿作為節點。空間三維點都只作為觀測資訊，在此階段不對空間三維點進行優化；
2. 優化過程迴圈了4次，前兩次優化呼叫了魯棒核，防止誤差值太過發散，更主要的原因是抑制錯誤匹配對的影響。後兩次關閉了魯棒核，認為前兩次的優化基本上抑制了錯誤匹配的影響。每次優化迭代詩詞；
3. 每次優化結束後，根據預設誤差閾值判斷每乙個特徵點是內點還是外點。
優化結束後，根據判斷條件，將所有匹配的外點全部刪除，統計內點(觀測次數大於0)數量。若內點數量大於10，則認為跟蹤成功，否則跟蹤失敗。
基於勻速運動模型的跟蹤
orbslam2中每次跟蹤成功後，都會將兩幀間的相對運動記錄下來作為運動模型。在估計下一幀運動時，將前一幀的姿態，乘上這個運動模型，就得到了當前幀的姿態初值。能用公式的地方，我們堅決不含糊，畢竟數學公式是沒有歧義性的，因此先上公式：
假設前一幀的相機姿態是 $t_$ ，表示從世界座標系到前一幀相機座標系的相對變換。運動模型為 $t_$ ，表示從前一幀到當前幀的相對變換。則當前幀的相機姿態初值可以描述為：
\begin
t_ = t_t_
\end
確定了姿態初值，我們還需要查詢匹配對。由於我們根據前一幀的相機姿態加上運動模型得到了當前幀的姿態，所以我們在需要在前一幀中查詢匹配對。
查詢匹配對的方法非常有意思，有很值得大家借鑑：
1. 從相機姿態中取出前一幀和當前幀對應的旋轉和位移；
2. 逐個取出前一幀特徵點 $f_$ 對應的地圖點 $pm_$，並將其投影到當前幀的像平面中；
3. 根據設定的搜尋區域閾值，確定搜尋視窗半徑 $r$；
4. 獲取當前幀中處於搜尋視窗區域的所有候選特徵點 $\left\^, f_^, \dots, f_^\right\}$；
5. 將前一幀的特徵點 $f_$ 與上述所有候選特徵點進行匹配，並根據閾值條件篩選最優匹配；
6. 同樣統計匹配對的特徵主方向角度差值，取數量最多的前三個角度區間對應的匹配對作為最終匹配對；
7. 若匹配對數量低於20，擴大搜尋區域，重複步驟4-6，若仍然小於20，則跟蹤失敗。
除了上述姿態初值的選擇方式和匹配對的選擇方式不同之外，兩種跟蹤方法在優化相機位姿時採用的步驟是一致的。因此，優化的方法就不再贅述了。(後續筆者會實現這部分功能，屆時會將原始碼公布在github上，並在此提供原始碼鏈結)
總結：
涉及的運動估計方法主要是pnp，因此我們詳細介紹了利用非線性優化的pnp方法；
此外，我們還分析了兩種跟蹤方案的不同特點，大家可以根據實際需要選擇跟蹤方案，亦或是像orbslam2一樣，二者結合。
區域性優化：我們通過擴大關鍵幀的數目，將當前幀關聯的所有關鍵幀以及對應的所有觀測點全部包含進來優化問題中，構成了更大的優化問題。利用同樣的優化策略，我們就實現了區域性的優化，防止相機姿態隨著時間變化而漂移。
[1] 視覺slam十四講
[2] 機械人學中的狀態估計
ps：
我的github鏈結是：
ORB SLAM2詳解（四）跟蹤
這一部分是orb slam系統中最基本的一步，會對每一幀影象進行跟蹤計算。tracking執行緒執行在主線程中，主要思路是在當前幀和 區域性 地圖之間尋找盡可能多的對應關係，來優化當前幀的位姿。每次新採集到一幀影象，就是用下列介面將影象傳入slam系統就行處理。該 位於主程式中 pass the i...
H 261簡要介紹 1 運動估計
1.3 h.261 簡要介紹 運動估計 me 我們經常可以碰到這樣的情況，兩個不同的產商，他們都支援h.261，設定的位元速率也一樣，可是實際跑起來的時候，兩個產商的編碼效果差別卻很大，這裡面，主要的原因就是運動估計演算法不同。h.261協議裡面其實並沒有定義如何運動估計，因為，協議一般只定 碼器，...
DeMoN 深度與運動估計的網路學習
demon depth and motion network for learning monocular stereo taylor guo,2017年05月20日 本文將運動恢復結構問題表示為乙個深度學習問題。我們訓練了乙個端到端的卷積神經網路從連續的無約束的影象對中計算深度和相機運動。這個架構...