非濾波單目slam基礎
直接法 特徵點法 混合法區別與聯繫
按照 2D−2D 數據關聯方式的不同 ,視覺定位方法可以分爲直接法、非直接法和混合法
1. 直接法假設幀間光度值具有不變性 , 即相機運動前後特徵點的灰度值是相同的 .
數據關聯時 , 根據灰度值對特徵點進行匹配，通過最小化光度誤差，來優化匹配.
直接法使用了簡單的成像模型 ,
適用於幀間運動較小的情形 , 但在場景的照明發生變化時容易失敗 .
直接法中是直接對普通的像素點(DTAM),或者灰度梯度大的點(lsd-slam)進行直接法匹配。
直接法起源是光流法，由於使用了圖像中大部分的信息，
對紋理差的部分魯棒性比直接法好，但是計算量也加大，要並行化計算。
基本原理是亮度一致性約束，
J(x,y) = I(x + u(x,y) + v(x,y)) ，x，y是圖像的像素座標，u,v是同一場景下的兩幅圖像I,J的對應點的像素偏移。
2. 非直接法 , 又稱爲特徵法 , 該方法提取圖像中的特徵進行匹配 ,
最小化重投影誤差得到位姿 . 圖像中的特徵點以及對應描述子用於數據關聯 ,
通過特徵描述子的匹配 ,完成初始化中 2D−2D 以及之後的 3D−2D 的數據關聯 .
例如 ORB (Oriented FAST and rotatedBRIEF， ORBSLAM中 ) 、
FAST (Features from accelerated seg-ment test) 、
BRISK (Binary robust invariant scalable keypoints) 、
SURF (Speeded up robustfeatures) ,
或者直接的灰度塊(PTAM中， 使用fast角點+灰度快匹配)
可用於完成幀間點匹配。
3. 混合法，又稱爲半直接法，結合直接法和特徵點法
使用特徵點法中的特徵點提取部分，而特徵點匹配不使用 特徵描述子進行匹配，
而使用直接法進行匹配，利用最小化光度誤差，來優化特徵點的匹配，
直接法中是直接對普通的像素點(DTAM),或者灰度梯度大的點(lsd-slam)進行直接法匹配。
1. 單目初始化
單目系統初始化時完成運動估計常用的方法主要有兩種 :
1) 將當前場景視爲一個平面場景 , 估計單應矩陣並分解得到運動估計 ,
使用這種方法的有 SVO 、 PTAM 等 .
2) 使用極線約束關係 , 估計基礎矩陣或者本質矩陣 , 分解得到運動估計 ,
使用這種方法的有 DT SLAM 等 .
初始化中遇到的普遍問題是雙視幾何中的退化問題 .
當特徵共面或相機發生純旋轉的時候 , 解出的基礎矩陣的自由度下降 ,
如果繼續求解基礎矩陣 , 那麼多出來的自由度主要由噪聲決定 .
3）根據情況旋轉兩張中間最好的。
爲了避免退化現象造成的影響 , 一些VO 系統同時估計基礎矩陣和單應矩陣 ,
例如 ORB-SLAM 和 DPPTAM,
使用一個懲罰函數 , 判斷當前的情形 , 選擇重投影誤差比較小的一方作爲運動估計結果 .
初始化 一般的初始化流程：
主要是爲找到跟第一幅圖像有足夠數量的特徵匹配點，視差足夠大的第二幅圖像
1. PTAM初始化要求用戶兩次輸入，來獲取地圖中的前2個關鍵幀;
需要用戶在第一個和第二個關鍵幀之間,做與場景平行的、緩慢平滑相對明顯的平移運動。
2. SVO使用單應進行初始化,但SVO不需要用戶輸入,算法使用系統啓動時的第一個關鍵幀;
提取FAST特徵,用圖像間的KLT算法跟蹤特徵，視差足夠的時候就選擇作爲第二幅圖像，然後計算單應矩陣，只能是2維平面
3. SD-SLAM從第1個圖像隨機初始化場景的深度,通過隨後的圖像進行優化。
LSD-SLAM初始化方法不需要使用兩視圖幾何。
不像其他SLAM系統跟蹤兩視圖特徵,LSD-SLAM只用單個圖像進行初始化
4. ORB-SLAM 並行計算基本矩陣和單應矩陣,根據對稱轉移誤差(多視圖幾何)懲罰不同的模型,最終選擇最合適的模型
單目深度估計
單目系統在初始化中還要完成像素點的深度估計 , 單目系統無法直接從單張圖像中恢復深度 ,
因此需要一個初始估計 .
解決該問題的：
一種辦法是跟蹤一個已知的結構(先驗知識)；
另外一種方法是初始化點爲具有較大誤差的逆深度,
在之後過程中優化(隨機過程優化，多高斯分佈(卡爾曼濾波器？？？)，
高斯分佈+均勻分佈(貝葉斯濾波器 svo))直到收斂至真值 .
像素點的深度估計方法有濾波器方法和非線性優化方法 .
其中 SVO 、 DSO 將深度建模爲一 個 類 高 斯 模 型 , 然 後 使 用 濾 波 器 估 計 .
另 外 一種 方 法 對 深 度 圖 構 建 一 個 能 量 函 數 , 例 如 LSD-SLAM 、DTAM 、 DPPTAM 等 ,
然後使用非線性優化方法最小化能量函數 .
該函數包括一個光度值誤差項以及一個正則項 , 用來平滑所得結果.
特徵點匹配
在講解恢復R,T前，稍微提一下特徵點匹配的方法。
常見的有如下兩種方式：
1. 計算特徵點，然後計算特徵描述子，通過描述子來進行匹配，優點準確度高，缺點是描述子計算量大。
2. 光流法：在第一幅圖中檢測特徵點，使用光流法(Lucas Kanade method)對這些特徵點進行跟蹤，
得到這些特徵點在第二幅圖像中的位置，得到的位置可能和真實特徵點所對應的位置有偏差。
所以通常的做法是對第二幅圖也檢測特徵點，如果檢測到的特徵點位置和光流法預測的位置靠近，
那就認爲這個特徵點和第一幅圖中的對應。
在相鄰時刻光照條件幾乎不變的條件下（特別是單目slam的情形），
光流法匹配是個不錯的選擇，它不需要計算特徵描述子，計算量更小。
2. 位姿估計
通用的位姿估計流程
這個先驗信息可能是恆速運動模型，可能是假設運動量不大，使用上一時刻的位姿。
1. 恆速運動模型，使用上幀位姿*速度得到當前幀位姿初始值
PTAM,DT-SLAM,ORB-SLAM,DPPTAM都假設相機做平滑運動採用恆定速度運動模型,
用跟蹤到的之前兩幅圖像的位姿變化估計作爲當前圖像的先驗知識(用於數據關聯的圖像位姿的要求和限制)。
但是,在相機運動方向上有猛烈移動時,這樣的模型就容易失效
2. 靜止不動模型，使用上幀位姿作爲當前幀位姿的初始值
LSD-SLAM和SVO都假設在隨後的圖像,相機位姿沒有明顯改變,因此,這種情況下都是用高幀率相機。
直接和間接方式都是通過最小化圖像間的測量誤差，優化估計的相機位姿;
直接方法測量光度誤差(像素值誤差),
間接方法通過最小化從圖像上一位姿的地圖中獲得的路標的重投影到當前幀的重投影誤差(幾何位置誤差)。
單應矩陣H 恢復變換矩陣 R, t
p2 = H12 * p1 4對點 A*h = 0 奇異值分解 A 得到 單元矩陣 H ， T = K 逆 * H21*K
展開成矩陣形式：
u2 h1 h2 h3 u1
v2 = h4 h5 h6 * v1
1 h7 h8 h9 1
按矩陣乘法展開：
u2 = (h1*u1 + h2*v1 + h3) /( h7*u1 + h8*v1 + h9)
v2 = (h4*u1 + h5*v1 + h6) /( h7*u1 + h8*v1 + h9)
將分母移到另一邊，兩邊再做減法
-((h4*u1 + h5*v1 + h6) - ( h7*u1*v2 + h8*v1*v2 + h9*v2))=0 式子爲0 左側加 - 號不變
h1*u1 + h2*v1 + h3 - ( h7*u1*u2 + h8*v1*u2 + h9*u2)=0
寫成關於 H的矩陣形式：
0 0 0 0 -u1 -v1 -1 u1*v2 v1*v2 v2
u1 v1 1 0 0 0 0 -u1*u2 -v1*u2 -u2 * (h1 h2 h3 h4 h5 h6 h7 h8 h9)轉置 = 0
h1~h9 9個變量一個尺度因子，相當於8個自由變量
一對點 2個約束
4對點 8個約束 求解8個變量
A*h = 0 奇異值分解 A 得到 單元矩陣 H
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// 奇異值分解
H = vt.row(8).reshape(0, 3);// v的最後一列
單應矩陣恢復 旋轉矩陣 R 和平移向量t
p2 = H21 * p1 = H21 * KP
p2 = K( RP + t) = KTP = H21 * KP
T = K 逆 * H21*K 本質矩陣F求解 變換矩陣[R t] p2轉置 * F * p1 = 0
基本矩陣的獲得
空間點 P 兩相機 像素點對 p1 p2 兩相機 歸一化平面上的點對 x1 x2 與P點對應
p1 = KP
p2 = K( RP + t)
x1 = K逆* p1 = P
x2 = K逆* p2 = ( RP + t) = R * x1 + t
消去t(同一個變量和自己叉乘得到0向量)
t 叉乘 x2 = t 叉乘 R * x1
再消去等式右邊
x2轉置 * t 叉乘 x2 = 0 = x2轉置 * t 叉乘 R * x1
得到 ：
x2轉置 * t 叉乘 R * x1 = x2轉置 * E * x1 = 0 ， E爲本質矩陣
也可以寫成：
p2轉置 * K 轉置逆 * t 叉乘 R * K逆 * p1 = p2轉置 * F * p1 = 0 ， F爲基本矩陣幾何知識參考
對極幾何原理：
兩個攝像機的光心C0、C1，三維空間中一點P，在兩幅圖像中的位置爲p0、p1（相當於上面的 x1, x2）。
像素點 u1，u2
p0 = inv(K) * u0
p1 = inv(K) * u1
x0 (u0x - cx) / fx
p0 = y0 = (u0y - cy) / fy
1 1
x1 (u1x - cx) / fx
p1 = y1 = (u1y - cy) / fy
1 1
如下圖所示：
由於C0、C1、P三點共面，得到：
這時，由共面得到的向量方程可寫成：
p0 *(t 叉乘 R * p1)
其中，
t是兩個攝像機光心的平移量;
R是從座標系C1到座標系C0的旋轉變換，
p1左乘旋轉矩陣R的目的是把向量p1從C1座標系下旋轉到C0座標系下(統一表示)。
一個向量a叉乘一個向量b，
可以表示爲一個反對稱矩陣乘以向量b的形式，
這時由向量a=(a1,a2,a3) 表示的反對稱矩陣(skew symmetric matrix)如下:
0 -a3 a2
ax = a3 0 -a1
-a2 a1 0
所以把括號去掉的話，p0 需要變成 1*3的行向量形式
纔可以與 3*3的 反對稱矩陣相乘
p0轉置 * t叉乘 * R * P1
我們把 t叉乘 * R = E 寫成
p0轉置 * E * P1
本徵矩陣E的性質：
一個3x3的矩陣是本徵矩陣的充要條件是對它奇異值分解後，
它有兩個相等的奇異值，並且第三個奇異值爲0。
牢記這個性質，它在實際求解本徵矩陣時有很重要的意義 .
計算本徵矩陣E、尺度scale的來由:
將上述矩陣相乘的形式拆開得到 ：
上面這個方程左邊進行任意縮放都不會影響方程的解：
(x0x1 x0y1 x0 y0x1 y0y1 y0 x1 y1 1)*E33(E11/E33 ... E32/E33 1) = 0
所以E雖然有9個未知數，但是有一個變量E33可以看做是縮放因子，
因此實際只有8個未知量，這裏就是尺度scale的來由，後面會進一步分析這個尺度。
AX=0，x有8個未知量，需要A的秩等於8，所以至少需要8對匹配點,應爲有可能有兩個約束是可以變成一個的，
點可能在同一條線上，或者所有點在同一個面上的情況，這時候就存在多解,得到的值可能不對。
對矩陣A進行奇異值SVD分解，可以得到A
#### p2轉置 * F * p1 = 0 8點對8個約束求解得到F
* f1 f2 f3 u1
* (u2 v2 1) * f4 f5 f6 * v1 = 0
* f7 f8 f9 1
按照矩陣乘法展開：
a1 = f1*u2 + f4*v2 + f7;
b1 = f2*u2 + f5*v2 + f8;
c1 = f3*u2 + f6*v2 + f9;
得到：
a1*u1+ b1*v1 + c1= 0
展開：
f1*u2*u1 + f2*u2*v1 + f3*u2 + f4*v2*u1 + f5*v2*v1 + f6*v2 + f7*u1 + f8*v1 + f9*1 = 0
寫成矩陣形式：
[u1*u2 v1*u2 u2 u1*v2 v1*v2 v2 u1 v1 1]*[f1 f2 f3 f4 f5 f6 f7 f8 f9]轉置 = 0
f 9個變量，1個尺度因子，相當於8個變量
一個點對，得到一個約束方程
需要8個點對，得到8個約束方程，來求解8個變量
A*f = 0
所以F雖然有9個未知數，但是有一個變量f9可以看做是縮放因子，
因此實際只有8個未知量，這裏就是尺度scale的來由，後面會進一步分析這個尺度。
上面這個方程的解就是矩陣A進行SVD分解A=UΣV轉置 後，V矩陣是最右邊那一列的值f。
另外如果這些匹配點都在一個平面上那就會出現A的秩小於8的情況，這時會出現多解，會讓你計算的E/F可能是錯誤的。
A * f = 0 求 f
奇異值分解F 基礎矩陣 且其秩爲2
需要再奇異值分解 後 取對角矩陣 秩爲2 後在合成F
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);// A = w * u * vt
cv::Mat Fpre = vt.row(8).reshape(0, 3);// F 基礎矩陣的秩爲2 需要在分解 後 取對角矩陣 秩爲2 在合成F
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0;// 基礎矩陣的秩爲2，重要的約束條件
F = u * cv::Mat::diag(w) * vt;// 在合成F
從基本矩陣恢復 旋轉矩陣R 和 平移向量t
F = K轉置逆 * E * K逆
本質矩陣 E = K轉置 * F * K = t 叉乘 R
從本質矩陣恢復 旋轉矩陣R 和 平移向量t
恢復時有四種假設 並驗證得到其中一個可行的解.
本質矩陣E = t 叉乘 R 恢復變換矩陣R,t的時候，有四種情況，但是隻有一種是正確的。
而判斷正確的標準，
就是按照這個R,t 計算出來的深度值(兩個座標系下的三維座標的Z值)都是正值，
因爲相機正前方爲Z軸正方向。
三角變換計算三維座標可按後面的方法計算。
本徵矩陣的性質：
一個3x3的矩陣是本徵矩陣的充要條件是對它奇異值分解後，
它有兩個相等的奇異值，
並且第三個奇異值爲0。
牢記這個性質，它在實際求解本徵矩陣時有很重要的意義。
計算本徵矩陣E的八點法，大家也可以去看看wiki的詳細說明 有本質矩陣E 恢復 R,t
從R,T的計算公式中可以看到R,T都有兩種情況，
組合起來R,T有4種組合方式。
由於一組R,T就決定了攝像機光心座標系C的位姿，
所以選擇正確R、T的方式就是，把所有特徵點的深度計算出來，
看深度值是不是都大於0，深度都大於0的那組R,T就是正確的。
以上的解法 在 orbslam2中有很好的代碼解法
3. 地圖生成
地圖生成的一般流程
地圖生成模塊將世界表示成稠密(直接)或稀疏(間接)的點雲。
系統將2D興趣點三角化成3D路標,並持續跟蹤3D座標,然後定位相機,這就是量度地圖(pose + point)。
但是,相機在大場景運行時,量度地圖的大小就會無限增大,最終導致系統失效
拓撲地圖(pose-graph)可以減少這一弊端,它儘量將地圖中的量度信息最小化,
減少幾何信息(尺度,距離和方向)而採用連接信息。
視覺SLAM中,拓撲地圖是一個無向圖,節點通常表示關鍵幀,關鍵幀通過邊連接,節點之間存在相同的數據關聯
拓撲地圖與大場景的尺度比較吻合,爲了估計相機位姿,也需要量度信息;
從拓撲地圖到量度地圖的變化並不是一件容易的事情,因此,最近的視覺 SLAM 系統都採用混合地圖,局部量度地圖和全局拓撲地圖。
地圖製作過程會處理新路標將其添加到地圖中,還檢測和處理離羣點。
圖標點的添加可以通過三角化，或者濾波實現的位置估計(一般是逆深度，加上一個概率分佈)，
收斂到一定的程度就可以加入到地圖中。
單目極線搜索匹配點三角化計算初始深度值
用單應變換矩陣H或者本質矩陣E 求解得到相鄰兩幀的變換矩陣R,t後就可是使用類似雙目的
三角測距原理來得到深度。
上圖中的物理物理中的點 P =[X,Y,Z,1]
在兩相機歸一化平面下的點 x1 x2 [x,y,z]
在兩相機像素平面上的點 p1, p2，匹配點對 [u,v,1]
p1 = k × [R1 t1] × P
左乘 k逆 × p1 = [R1 t1] × P
得到 x1 = T1 × P
計算 x1叉乘x1 = x1叉乘T1 × P = 0
這裏 T1 = [I, 0 0 0] 爲單位矩陣。
p2 = k × [R2 t2] × P
左乘 k逆 × p2 = [R2 t2] × P
得到 x2 = T2 × P
消去 x2叉乘x2 = x2叉乘T2 × P = 0
式中：
x1 = k逆 × p1 ，
x2 = k逆 × p2 ，
T2= [R, t]爲幀1變換到幀2的 已知
得到兩個方程
x1叉乘T1 × P = 0
x2叉乘T2 × P = 0
寫成矩陣形式 A*P = 0
A = [x1叉乘T1; x2叉乘T2]
這又是一個要用最小二乘求解的線性方程方程組 ,和求本徵矩陣一樣，
計算矩陣A的SVD分解，然後奇異值最小的那個奇異向量就是三維座標P的解。
P是3維齊次座標，需要除以第四個尺度因子 歸一化.
[U,D,V] = svd(A);
P = V[:,4] 最後一列
P = P/P(4)；// 歸一化
以上也是由本質矩陣E = t 叉乘 R 恢復變換矩陣R,t的時候，有四種情況，但是隻有一種是正確的。
而判斷正確的標準，就是按照這個R,t 計算出來的深度值都是正值，因爲相機正前方爲Z軸正方向。
概率方法更新矯正 深度值
我們知道通過兩幀圖像的匹配點就可以計算出這一點的深度值，
如果有多幅匹配的圖像，那就能計算出這一點的多個深度值。
一幅圖像對應另外n幅圖像，可以看作爲n個深度傳感器，
把得到點的深度信息的問題看作是有噪聲的多傳感器數據融合的問題，
使用Gaussian Uniform mixture model(高斯均值混合模型)來對這一問題進行建模，
假設好的測量值符合正態分佈，好的測量值的比例爲pi,
噪聲符合均勻分佈，噪聲比例爲1-pi 使用貝葉斯方法不斷更新這個模型，
讓點的深度（也就是正態分佈中的均值參數）和好的測量值的比例pi收斂到真實值。
好的測量值符合正態分佈
噪聲符合均勻分佈，
使用貝葉斯方法(最大後驗概率)更新，這是SVO中深度濾波的部分。
好的測量值和噪聲均符合高斯分佈，使用卡爾曼濾波來進行深度測量值的濾波
這就像對同一個狀態變量我們進行了多次測量，
因此，可以用貝葉斯估計來對多個測量值進行融合，
使得估計的不確定性縮小。
如下圖所示：
一開始深度估計的不確定性較大(淺綠色部分)，
通過三角化得到一個深度估計值以後，
能夠極大的縮小這個不確定性(墨綠色部分)。
4. 地圖維護
地圖維護通過捆集調整(Bundle Adjustment)或位姿圖優化(Pose-Graph)來優化地圖。
地圖擴展的過程中,新的3D路標基於相機位姿估計進行三角化。經過一段時間的運行,由於相
機累積誤差增加,相機位姿錯誤,系統出現漂移(scale-drift)。
位姿圖優化相比全局捆集調整返回較差的結果。
原因是位姿圖優化只用於關鍵幀位姿優化,
(要是路標相對於的是關鍵幀表示，而不是世界下的表示，會相應地調整路標的3D結構);
全局捆集調整都優化關鍵幀位姿和3D結構。
地圖維護也負責檢測和刪除地圖中的由於噪聲和錯誤特徵匹配的離羣點。
丟失後閉環檢測
視覺詞袋BOVM(orb-slam2)
或者 高斯小圖 塊匹配(ptam)
思考
感覺一般的slam按照上面的8個部分，想一遍要能想通，原理上面一般就算是可以了。就剩下代碼實踐了。
系統有沒有特定的要求，大範圍的可以嗎，室內還是室外，
是直接法還是間接法，對應的就是稀疏地圖還是(半)稠密地圖，優化的是幾何誤差還是光度誤差。
需不需要初始化，需要人工的干預，非2維平面可以嗎，對應的就是計算本徵矩陣還是單應矩陣。
還是基於濾波的方法，不需要初始化，後續優化就可以。
然後就是tracking，因爲視覺slam是一個非凸函數，需要一個初始值，
對應的就是你的prior是什麼，恆速運動模型還是高速相機，把前一幀的位姿當做初始值。
追蹤的過程的優化函數是什麼，是光度不變，還是幾何誤差，就是g2o中邊，頂點的構造。
當然還要維護地圖，地圖中的點是怎麼表示，基於逆深度，還是歐式幾何表示，camera-anchor的表示。
地圖維護中要進行BA，還是Pose-Graph的優化，還是雜交。
然後就是閉環的檢測，基於視覺詞帶，還是隨機取一些幀進行Image Alignment。
當然這其中還有無數細節，運動快，模糊是怎麼處理的(加大搜索範圍，從金字塔coarse-fine的搜索)，
Tracking lost在什麼情況下發生，怎麼進行重定位。
最小二乘法要做幾次，怎麼時候收斂，要不要給他設個時間約束，權重怎麼設置等等。