orbslam2特徵點三角化簡介
插入關鍵幀以後，我們還需要插入新的地圖點。為了確保新插入的地圖點是足夠魯棒的，進行嚴格的檢查是必要的。orbslam2在插入地圖點的時候也十分仔細，上一講我們提到了地圖的更新策略，唯獨三角化沒有細講，倒不是因為它不重要而不提，而是因為三言兩語說不清楚，所以才需要單獨用一講來說說這個方法。
接下來，筆者將會從一下兩個方面來介紹本講的內容：
1. 線性三角化方法；
2. 建立新地圖點的方法提綱；
線性三角化方法
同之前一樣，筆者會介紹建立新地圖點所必要的技術細節，然後再講方法提綱。
在orbslam2原始碼中，建立新的地圖點採用的是線性三角化方法，當然有些點並不是用這個方法建立的，但是這一小節我們不提，留到方法提綱那一節再介紹。
ok，我們就直接進入主題吧。
orbslam2採用的是針孔模型的相機，而不是魚眼或者事件相機之類的。因此這類相機的投影模型，大家應該都是比較熟悉的。筆者就不贅述了。
假設三維空間點為 $p_ = [ x, y, z, 1 ]^$，參考幀的相機位姿為 $t_ = [ r_ | t_ ]$，表示從世界座標系變換到參考幀座標系，同理，當前幀的相機位姿為 $t_ = [ r_ | t_ ]$。點 $p_$ 在參考幀歸一化平面裡的位置為 $p_ = [ x_/z_, y_/z_, 1 ]^$，深度為 $z_$，在當前幀歸一化平面裡的位置為 $p_ = [ x_/z_, y_/z_, 1 ]^$，深度為 $z_$。根據位置變換關係有：
\begin
z_p_ = t_p_
\end
\begin
z_p_ = t_p_
\end
對公式 $(1)$ 和公式 $(2)$ 分別叉乘對應的歸一化平面的點，得到：
\begin
p_\times z_p_ = p_\times t_p_ = 0
\end
\begin
p_\times z_p_ = p_\times t_p_ = 0
\end
\begin
\begin p_\times t_ \\p_\times t_ \end p_ = ax = 0
\end
即通過構建乙個齊次方程來求解空間點 $p_$。其中，矩陣 $a \in r^$， 因此它是乙個超定方程，所以我們的目標函式是：
\begin
j(x) = \min\|ax\|
\end
顯然 $x = 0$ 是很自然的乙個解，但我們想要的是非0解，因此我們假設 $x$ 滿足 $\|x\| = 1$，於是我們用奇異值分解的方法來求解。
通過svd分解，我們可以得到 $a = udv^$，其中，$u \in r^$，$d \in r^$，$v \in r^$，則原方程可以寫成：
\begin
j(x) = \min\|ax\| = \min\|udv^x\| = \min\|dv^x\|
\end
因為 u 和 v 都是正交矩陣，行列式為1。因此有 $\|v^x\| = \|x\|$。
令 $y = v^x$，則原問題進一步簡化為：
\begin
j(y) = j(v^x) = \min\|dy\|
\end
為什麼說進一步簡化了呢？因為原始矩陣 a 是乙個無序矩陣，到了公式 $(8)$ 的時候，矩陣 a 變成了對角陣 d。且由於對角陣 d 是矩陣 a 的奇異值從大到小降序排列而成，因此 $j(y)$ 的最小值在 d 矩陣奇異值最小的地方取到。由於 d 是 $6\times 4$ 維的矩陣， 則原代價函式最小值在 $y = [ 0, 0, 0, 1 ]^$ 時取到。於是我們得到：
\begin
y = v^x \longrightarrow x = vy
\end
於是，$x$ 的解就變成了正交矩陣 v 的最後一列的列向量。到此，我們就通過上面一系列方法，求出了空間點 $x = p_$，但是由於它是乙個模長為1的齊次向量，所以我們進行歸一化，即：
\begin
\begin x \\ y \\ z \\ w \end \longrightarrow \begin x/w \\ y/w \\ z/w \\ w/w \end = \begin x \\ y \\ z \\1 \end
\end
另外，值得注意的是，orbslam2這裡並不是利用兩個點構建乙個 $6\times 4$ 維的 a 矩陣，而是通過刪除其中兩行，來生成乙個 $4\times 4$ 的 a 矩陣。筆者認為二者並無本質區別，不過可能在計算上更加方便。(筆者後面復現這方面**時，會做一下對比，到時再回來補充。先立個flag，push自己去做！)
建立新地圖點的方法提綱
筆者還是照往常一樣，為大家羅列orbslam2建立新地圖點的方法提綱。
1. 建立新的地圖點需要有足夠視差的兩個匹配點才能進行三角化，所以首先將當前幀共檢視裡所有關鍵幀全部包含進來；
2. 計算當前幀和共檢視關鍵幀的基本矩陣；
3. 在共檢視關鍵幀中，為當前幀所有特徵點查詢最好的匹配點，構建匹配關係；
note：在匹配階段，利用bow向量進行篩選加速，計算描述子匹配分數，確定最優匹配，這裡還要檢查是否滿足對極幾何關係。
4. 計算匹配對的歸一化座標，並計算射線的夾角 $\theta_$；
5. 計算當前幀和共檢視關鍵幀左右兩檢視觀測該地圖點形成的射線的夾角 $\theta_$ 和 $\theta_$；
6. 比較三個夾角的大小，若步驟5中的任意乙個夾角大於 $\theta_$，則直接用該幀的深度資訊建立新的地圖點，否則使用上面介紹的線性三角化方法生成新的地圖點；
7. 確認新的地圖點位置是否在兩個相機的前面，這裡主要通過位姿變換確認深度值為正數即可；
8. 計算重投影誤差，即將該地圖點分別投影回兩幀影象中，與投影點計算誤差，確認其在閾值範圍內；
9. 最後就是確認尺度資訊，即確定匹配對的尺度誤差不能太大；
10. 剩餘的就是關聯觀測資訊還有插入地圖點，然後計算法向量，描述子這些東西了，前面已經介紹過的。
ok，提綱已經基本介紹完了。那麼特徵點的三角化這一講就結束拉。
總結：
按照慣例，我們還是總結一下。
這一講，我們主要介紹了orbslam2中特徵點的三角化，主要包括其線性三角化方法，其利用了反對稱矩陣的性質以及svd分解的方法來求解三維空間點。
此外，我們還羅列了orbslam2中具體的建立新的地圖點的主要提綱，把用到的步驟全部都精煉呈現給大家。有需要的小夥伴可以按需查**。
[1] 視覺slam十四講
[2]
[3]
[4]
[5] 矩陣分析
ps：
我的github鏈結是：
（七）ORBSLAM特徵點的三角化
orbslam2特徵點三角化簡介 插入關鍵幀以後，我們還需要插入新的地圖點。為了確保新插入的地圖點是足夠魯棒的，進行嚴格的檢查是必要的。orbslam2在插入地圖點的時候也十分仔細，上一講我們提到了地圖的更新策略，唯獨三角化沒有細講，倒不是因為它不重要而不提，而是因為三言兩語說不清楚，所以才需要單獨...
酉三角化和實正交三角化
介紹乙個非常有用的定理 任何復方陣 a 與以 a 的特徵值作為對角元素的乙個三角矩陣酉相似,以及總可以通過實正交相似將矩陣化為乙個實的擬三角型並作了相應的推廣.證明 設 u 1 x quad u 2 quad cdots quad u n 是任意乙個第一列為 x 的酉矩陣，比方說利用 qr 分解 中...
貪婪投影三角化演算法對有向點雲進行三角化
貪婪投影法 先將有向點雲投影到某一區域性座標平面內，再在座標平面內進行平面內的三角化，根據平面內三位點的拓撲關係獲得乙個三角網格曲面模型。include include include include include include include include include void main...