DSO演算法初探
筆者出於基於特徵點的SLAM無法構建出讓人看得懂的點雲地圖轉而研究基於直接法的SLAM。
DSO(Direct Sparse Odometry)是慕尼黑工業大學Engel於2016年釋出在arXiv上的一種基於直接法，構建稀疏點雲地圖的視覺里程計。
採用direct和sparse相結合的原因：
- 直接法中並不要求點特徵需要識別它自己，從而可以允許更精細的幾何表示。
- 可以對所有畫素點進行均勻取樣，構建更完善的模型，在少紋理特徵區域更具魯棒性。
- 增加幾何先驗資訊的缺點是引入了幾何引數之間的關聯，造成Hessian矩陣不稀疏，如下圖所示。
- 當前得到的先驗資訊複雜度有限，儘管區域性更精確，視覺效果更好，但是作者發現先驗資訊會引入一個bias，降低系統在長時間、大尺度工作時的精度。
與其他直接法的不同：
- 將光度模型、集合模型結合起來，對相機內參、相機姿態、畫素點的深度等引數做聯合估計；
- 由於引入了光度模型，演算法忽略了其他直接法採用的對影象進行平滑的預處理，而是將相機看做一個感測器，對影象畫素進行均勻取樣，進行計算。
- 提出的模型集成了完整的光度校準，考慮了曝光時間、透鏡漸暈和非線性相應函式。
- 通過實驗驗證，該演算法在跟蹤精度和魯棒性方面效能超過了state-of-the-art的演算法水平。
模型：
直接法對影象成像的過程進行了全面的建模，包含幾何模型、光度模型。注意光度模型在直接法中很重要，直接法直接利用畫素上的光強資訊進行運算。而特徵點法由於對光強改變有很好的魯棒性，所以光度模型對基於特徵點的SLAM系統用處不大。
幾何相機標定（徑向畸變在預處理時被矯正）：
投影函式：
反投影函式：
光度相機標定：
非線性相應函式：
透鏡衰減：
輻射度（irradiance）: 
曝光時間：t
光度模型：
光度校正：
光度誤差：
定義參考幀 
其中，
影象幀之間的轉換關係如下：
作者有意識地將影象中梯度大的畫素點對應的權值縮小。筆者的理解是作者希望最終的能量函式能夠反映大多數畫素點的光度誤差，而不是個別梯度大的畫素點的誤差。
為了使演算法可應用於曝光時間未知的影象序列，作者增加了仿射光度公式 
在所有影象幀和點中對應的光度誤差：
滑動視窗法優化：
Gauss-Newton優化：
非線性優化方法Gauss-Newton法的基本公示推導可參考部落格。DSO中對Jacobian矩陣進行了加權處理，即
由於DSO對畫素點採用了畫素塊的表達方式，每個點對應有8個光度殘差。
第k個點對應的殘差：
其中，
Jacobian矩陣 ： 
DSO採用GN法迭代6次進行優化，是因為初始值選的很好的關係（如何選初始值？）。
注意：在計算Jacobian時，應當使用“First Estimate Jacobians”。
Marginalization會marg掉可能影響H矩陣稀疏性的殘差項。以保證Hessian矩陣的稀疏性，保證矩陣維數維持在合理範圍內。具體過程如下：
利用Schur complement可以marg想消去的量，具體可以參考部落格。marg消去的量的資訊會作為先驗資訊加入未marg的量當中，會改變保留項的概率分佈，如STATE ESTIMATION FOR ROBOTICS P18對應的內容。
視覺里程計前端：
整體思路同ORBSLAM很像，並沒有太大的不同。
- 決定使用哪些點、影象幀，以及點和影象幀之間的關係。
- 提供初始值。
滑動視窗內包含7幀關鍵幀，幀1為最新的關鍵幀，幀7為最舊的關鍵幀。關鍵幀marg的策略如下：
- 始終保持最新的兩幀。
- 若舊關鍵幀中的點不足5%為新關鍵幀觀察到，則舊關鍵幀被marg。
- 若滿足保留條件的關鍵幀多於7, 對除了最新兩幀的關鍵幀計算"distance score"，消去得分高的關鍵幀以保持所有關鍵幀在三維空間中有個較好的分佈。
distance score（
備用點的選取：
直接法選取的點較特徵點法有所不同，直接法允許在所有的可得資料中進行取樣，包括弱紋理區域及重複區域。為了滿足實時性的要求，DSO僅優化2000個地圖點及7個關鍵幀，又出於備用點需足夠的角度，在每一關鍵幀中選擇2000個點作為備用。