視覺SLAM十四講|第10講 後端1
鹹魚了好久。。。。起來接著學習。
上來就又被先驗後驗搞蒙了，重新理解一下：
先驗概率：根據以往經驗和分析得到的概率，它往往作為「由因求果」問題中的「因」出現。
後驗概率：指在得到「結果」的信息後重新修正的概率，是「執果尋因」問題中的「因」。
最大的區別就在於結果/當前觀測在後驗概率中已知，在先驗概率中未知。
1. 後端要做什麼？
前端能夠根據相鄰的兩幅圖像判斷出此時此刻的位姿，是暫時的；那麼後端需要對前端測量以及計算的結果進行矯正，不僅用過去的信息，也用未來的信息更新自己，希望能夠得到一個長時間的正確狀態。
前面有講過，在運動方程和觀測方程中，如果把位姿和路標看成隨機變數，就可以把問題變為已知觀測數據和運動數據情況下，如何確定狀態量的分佈問題，是一個狀態估計問題。假設雜訊和狀態量服從高斯分佈，只需要估計它的均值和方差，即可確定狀態量的分佈。
完成後端優化可以使用濾波器，也可以使用非線性優化，本講對兩種方法都做了推導。
2. 濾波器推導
先做一些符號的定義和鋪墊：
把位姿和路標寫在一起，記為： ，用新符號寫運動方程和觀測方程，為：
。
需要求解的是後驗概率問題，即在已知0時刻的狀態、1:k時刻的觀測下，k時刻的狀態分佈，寫為 ，根據貝葉斯法則，將後驗概率展開為似然和先驗概率的乘積，寫為
。
先驗概率中k時刻的狀態 受到前面0:k-1時刻狀態的影響，對先驗概率進行展開，可以寫為
，在進行濾波器模型推導時，假設馬爾可夫性（k時刻狀態只與k-1時刻狀態有關，與之前狀態無關）。可以根據馬爾可夫性對先驗概率的展開式進行化簡，積分中第一個概率化簡為
，第二個概率化簡為
（運動方程中k時刻狀態只受到k-1時刻的狀態和k時刻的運動數據相關）。
2.1 卡爾曼濾波器（線性高斯系統）
在線性高斯系統中，用線性方程描述運動方程和觀測方程，寫為 ，記雜訊服從零均值高斯分佈：
，令
表示後驗，
表示先驗。
因為各隨機變數服從高斯分佈，誤差服從零均值高斯分佈，根據高斯分佈的特性（後面解釋）和觀測方程、運動方程，可以分別寫出似然和先驗的分佈：
把先驗均值和方差記為
根據後驗、似然、先驗的關係，有：
高斯分佈底數為e，比較等式兩邊的指數項（不考慮常數項）有，
比較兩邊的係數，經過一系列推導可以得到後驗均值和方差的表達式。
整個卡爾曼濾波的過程為：
在第一步預測中，使用k-1時刻的後驗分佈估計k時刻的先驗分佈，這一步不確定性變大，第二步使用k時刻的先驗分佈估計k時刻的後驗分佈，對於結果進行修正，縮小不確定性。
2.2 拓展卡爾曼濾波器EKF（非線性系統）
將線性系統拓展到非線性系統，通常會在某個點處對方程進行展開，保留一階項，按照線性系統進行推導。
對於k時刻的運動方程，展開有： ，記偏導數為
。
對於k時刻的觀測方程，展開有： ，記偏導數為
。
利用和線性系統類似的方法，推導得到預測步驟（沒看明白怎麼推的，如有大神請幫忙解釋一下）：
卡爾曼增益：
更新步驟：
EKF的侷限性：馬爾可夫假設、一次線性化，這兩個數學上的處理化簡了問題，但是偏離了實際情況。記錄均值、方差、路標等信息，存儲量大。
3. 非線性優化（BA與圖優化）
Bundle Adjustment，是指從視覺重建中提煉出最優的3D模型和相機參數（內參數和外參數）。
3.1 優化目標
回顧整個投影過程，假設已知相機的外參數（R，t）和世界坐標系的p點坐標，求解p點的像素坐標過程。
世界坐標轉化為相機坐標：
相機坐標投影到歸一化平面：
去畸變（徑向畸變）：
計算像素坐標：
整個過程中，用到了相機的位姿（R，t）和路標p的世界坐標，得到了路標的像素坐標，對應到觀測方程z=h(x,y)中，觀測z就是像素坐標 ，相機位姿x用李代數表示
，路標y用三維點p的坐標。那麼通過降低觀測到的像素坐標z和估計到的像素坐標
之間的誤差，就可以得到最優的x和y的解。用最小二乘表示整體的代價函數為：
採用非線性優化更新增量的方式求解，令 ，
。
引入增量後，目標函數變為：
表示f(x)對於相機位姿的偏導數，
表示f(x)對於路標點位置的偏導數。
接下來使用第六章的非線性優化方法進行求解，在求解時不免要計算 ，利用H矩陣的稀疏性能夠加速求解。
3.2 稀疏性
在高斯牛頓法中 ，在列文伯格——馬誇爾特方法中
，所以H的稀疏性主要是由J引起的。
對於代價函數中的 ，描述的是在
看到
，只與這兩個量相關，所以
由於 ，J只在i和j處有非零塊，所以矩陣H中只有(i,i),(i,j),(j,i),(j,j)處有非零塊，其中i是相機部分，j是路標部分。
舉例說明，假設有2個相機位姿 和6個路標
，那麼相機所對應的變數有
，路標對應變數有
。如圖10-4所示，連線表示相機可以看到路標。
圖10-4的場景，用目標函數表示應該是：
其中 描述的是相機
觀測到了
，把所有變數按照
的順序擺放，則雅各比矩陣為
，依次推類可以得到目標函數每一項的雅各比矩陣，疊放在一起，用方塊表示非零塊，則矩陣J和H的非零值如圖10-6所示。
每個非零塊都對應著相機和路標點的關係，拓展開來，可以把H矩陣劃分成四個區域，如圖10-9所示。
其中BC是對角塊矩陣，B的對角塊與相機位姿的維度相同，C的對角塊與路標維度相同，E的數值和具體的觀測相關。
3.3 邊緣化（Marginalization）
把相機位姿變數放到一起，記為 ，把空間點的變數放在一起，記為
。
在非線性優化中，需要根據 求解增量，用分塊矩陣表示H後變為：
，需要想辦法計算兩個增量。
先消去右上角的E： ，
整理得到 ，
用第一行計算出 的值，再帶入第二行，求解
，這個過程稱為Marginalization。
3.4 魯棒核函數
當輸入的數據存在無匹配時，誤差會很大，在二範式中產生的梯度也會很大，從而導致增量朝著錯誤的方向變化。選用一個具有光滑性質並且變化較小的函數可以使得系統更穩健，這種函數稱為魯棒核函數，比如Huber核： 。
附錄A.3
高斯函數性質：
對於隨機變數 ，若有
，其中A，b為線性變數的稀疏矩陣核偏移量，w為雜訊項，且
，有
。
參考文獻
[1] 《視覺SLAM十四講從理論到實踐》 高翔，張濤
推薦閱讀：