ORBSLAM2特征點三角化簡介
插入關鍵幀以后，我們還需要插入新的地圖點。為了確保新插入的地圖點是足夠魯棒的，進行嚴格的檢查是必要的。ORBSLAM2在插入地圖點的時候也十分仔細，上一講我們提到了地圖的更新策略，唯獨三角化沒有細講，倒不是因為它不重要而不提，而是因為三言兩語說不清楚，所以才需要單獨用一講來說說這個方法。
接下來，筆者將會從一下兩個方面來介紹本講的內容：
1. 線性三角化方法；
2. 創建新地圖點的方法提綱；
線性三角化方法
同之前一樣，筆者會介紹創建新地圖點所必要的技術細節，然后再講方法提綱。
在ORBSLAM2源碼中，創建新的地圖點采用的是線性三角化方法，當然有些點並不是用這個方法創建的，但是這一小節我們不提，留到方法提綱那一節再介紹。
OK，我們就直接進入主題吧。
ORBSLAM2采用的是針孔模型的相機，而不是魚眼或者事件相機之類的。因此這類相機的投影模型，大家應該都是比較熟悉的。筆者就不贅述了。
假設三維空間點為 $P_{w} = [ X, Y, Z, 1 ]^{T}$，參考幀的相機位姿為 $T_{rw} = [ R_{rw} | t_{rw} ]$，表示從世界坐標系變換到參考幀坐標系，同理，當前幀的相機位姿為 $T_{cw} = [ R_{cw} | t_{cw} ]$。點 $P_{w}$ 在參考幀歸一化平面里的位置為 $p_{r} = [ x_{r}/z_{r}, y_{r}/z_{r}, 1 ]^{T}$，深度為 $z_{r}$，在當前幀歸一化平面里的位置為 $p_{c} = [ x_{c}/z_{c}, y_{c}/z_{c}, 1 ]^{T}$，深度為 $z_{c}$。根據位置變換關系有：
\begin{equation}
z_{r}p_{r} = T_{rw}P_{w}
\end{equation}
\begin{equation}
z_{c}p_{c} = T_{cw}P_{w}
\end{equation}
對公式 $(1)$ 和公式 $(2)$ 分別叉乘對應的歸一化平面的點，得到：
\begin{equation}
p_{r}\times z_{r}p_{r} = p_{r}\times T_{rw}P_{w} = 0
\end{equation}
\begin{equation}
p_{c}\times z_{c}p_{c} = p_{c}\times T_{cw}P_{w} = 0
\end{equation}
我們知道，一個三維向量 $t$，叉乘自己 $t\times t = 0$。於是整理上面的式子，可以得到：
\begin{equation}
\begin{bmatrix} p_{r}\times T_{rw} \\p_{c}\times T_{cw} \end{bmatrix} P_{w} = Ax = 0
\end{equation}
即通過構建一個齊次方程來求解空間點 $P_{w}$。其中，矩陣 $A \in R^{6\times 4}$， 因此它是一個超定方程，所以我們的目標函數是：
\begin{equation}
J(x) = \min\|Ax\|
\end{equation}
顯然 $x = 0$ 是很自然的一個解，但我們想要的是非0解，因此我們假設 $x$ 滿足 $\|x\| = 1$，於是我們用奇異值分解的方法來求解。
通過SVD分解，我們可以得到 $A = UDV^{T}$，其中，$U \in R^{6\times 6}$，$D \in R^{6\times 4}$，$V \in R^{4\times 4}$，則原方程可以寫成：
\begin{equation}
J(x) = \min\|Ax\| = \min\|UDV^{T}x\| = \min\|DV^{T}x\|
\end{equation}
因為 U 和 V 都是正交矩陣，行列式為1。因此有 $\|V^{T}x\| = \|x\|$。
令 $y = V^{T}x$，則原問題進一步簡化為：
\begin{equation}
J(y) = J(V^{T}x) = \min\|Dy\|
\end{equation}
為什么說進一步簡化了呢？因為原始矩陣 A 是一個無序矩陣，到了公式 $(8)$ 的時候，矩陣 A 變成了對角陣 D。且由於對角陣 D 是矩陣 A 的奇異值從大到小降序排列而成，因此 $J(y)$ 的最小值在 D 矩陣奇異值最小的地方取到。由於 D 是 $6\times 4$ 維的矩陣， 則原代價函數最小值在 $y = [ 0, 0, 0, 1 ]^{T}$ 時取到。於是我們得到：
\begin{equation}
y = V^{T}x \Longrightarrow x = Vy
\end{equation}
於是，$x$ 的解就變成了正交矩陣 V 的最后一列的列向量。到此，我們就通過上面一系列方法，求出了空間點 $x = P_{w}$，但是由於它是一個模長為1的齊次向量，所以我們進行歸一化，即：
\begin{equation}
\begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix} \Longrightarrow \begin{bmatrix} x/w \\ y/w \\ z/w \\ w/w \end{bmatrix} = \begin{bmatrix} X \\ Y \\ Z \\1 \end{bmatrix}
\end{equation}
至此，我們的線性三角化方法就全部介紹完了。當然，如果感興趣的話，大家也可以查閱相關的文檔，另外，也可以看看奇異值分解是怎么用在非線性方程求解問題上的。
另外，值得注意的是，ORBSLAM2這里並不是利用兩個點構建一個 $6\times 4$ 維的 A 矩陣，而是通過刪除其中兩行，來生成一個 $4\times 4$ 的 A 矩陣。筆者認為二者並無本質區別，不過可能在計算上更加方便。（筆者后面復現這方面代碼時，會做一下對比，到時再回來補充。先立個Flag，Push自己去做！）
創建新地圖點的方法提綱
筆者還是照往常一樣，為大家羅列ORBSLAM2創建新地圖點的方法提綱。
1. 創建新的地圖點需要有足夠視差的兩個匹配點才能進行三角化，所以首先將當前幀共視圖里所有關鍵幀全部包含進來；
2. 計算當前幀和共視圖關鍵幀的基本矩陣；
3. 在共視圖關鍵幀中，為當前幀所有特征點查找最好的匹配點，構建匹配關系；
Note：在匹配階段，利用BOW向量進行篩選加速，計算描述子匹配分數，確定最優匹配，這里還要檢查是否滿足對極幾何關系。
4. 計算匹配對的歸一化坐標，並計算射線的夾角 $\theta_{n}$；
5. 計算當前幀和共視圖關鍵幀左右兩視圖觀測該地圖點形成的射線的夾角 $\theta_{c}$ 和 $\theta_{g}$；
6. 比較三個夾角的大小，若步驟5中的任意一個夾角大於 $\theta_{n}$，則直接用該幀的深度信息創建新的地圖點，否則使用上面介紹的線性三角化方法生成新的地圖點；
7. 確認新的地圖點位置是否在兩個相機的前面，這里主要通過位姿變換確認深度值為正數即可；
8. 計算重投影誤差，即將該地圖點分別投影回兩幀圖像中，與投影點計算誤差，確認其在閾值范圍內；
9. 最后就是確認尺度信息，即確定匹配對的尺度誤差不能太大；
10. 剩余的就是關聯觀測信息還有插入地圖點，然后計算法向量，描述子這些東西了，前面已經介紹過的。
OK，提綱已經基本介紹完了。那么特征點的三角化這一講就結束拉。
總結：
按照慣例，我們還是總結一下。
這一講，我們主要介紹了ORBSLAM2中特征點的三角化，主要包括其線性三角化方法，其利用了反對稱矩陣的性質以及SVD分解的方法來求解三維空間點。
此外，我們還羅列了ORBSLAM2中具體的創建新的地圖點的主要提綱，把用到的步驟全部都精煉呈現給大家。有需要的小伙伴可以按需查代碼。
下一講，筆者將為大家介紹SLAM中一個非常重要的模塊——回環檢測。但是由於回環檢測篇幅較大，所以分成兩講。下一講，先為大家介紹回環檢測里面的位置識別。
參考文獻：
[1] 視覺SLAM十四講
[2] https://blog.csdn.net/kokerf/article/details/72844455
[3] https://blog.csdn.net/heyijia0327/article/details/50774104
[4] https://blog.csdn.net/MyArrow/article/details/53780972
[5] 矩陣分析
PS：
如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請注明出處鏈接。
對本文有任何問題可以在留言區進行評論，也可以在泡泡機器人論壇：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技術交流模塊發帖提問。
我的github鏈接是：https://github.com/yepeichu123/orbslam2_learn。