LSD SLAM

• 1. Overview
• • 三大子產品
• 2. Pose tracking
• • 2.1 直接法
  • 2.2 LSD的直接法
• 3. Depth mapping
• • 3.1 Keyframe selection
  • 3.2 Depth Map Creation（Depth Map Propagation）
  • 3.3 Depth Map Refinement
  • • 3.3.1 參考圖像幀選取
    • 3.3.2 立體比對政策
    • 3.3.3 不确定性估計
    • • a. 幾何視差誤差（Geometric disparity error）
      • b. 光度視差誤差
      • c. 逆深度計算誤差
    • 3.3.4 深度觀測融合
• 4. Map Optimization
• • 4.1 Sim3求解
  • • 4.1.1 原理分析
    • • a. Sim3 圖像對齊
  • 4.2 雙向跟蹤檢測
• 參考

1. Overview

LSD SLAM 算法主要在以下兩篇論文中提出：

[1] 2013 Semi-dense Visual Odometry for a Monocular Camera

[2] 2014 LSD-SLAM: Large-Scale Direct Monocular SLAM

三大子產品

• Pose tracking： 目前幀與目前關鍵幀進行比對得到目前幀的位姿，比對方法為直接法;

• Depth mapping： 判斷目前幀與目前關鍵幀距離是否大于一定門檻值來決定是否将目前幀設為新的關鍵幀，

  若建構新的關鍵幀，則建構新的深度圖;

  若不建構，則更新目前關鍵幀的深度圖;

  這部分主要在論文

  [1]

  中詳細展開。
• Graph optimization： 搜尋閉環限制，進行圖優化.

2. Pose tracking

2.1 直接法

根據圖像灰階資訊，最小化光度誤差（Photometric error），計算相機運動。可以不用計算關鍵點和描述子，既避免了特征的計算時間，也避免了特征缺失情況。但對光照比較敏感。

P P P為相機1坐标系下的三維坐标， p 1 p_1 p1​和 p 2 p_2 p2​分别為 P P P在相機1和相機2圖像上的像素坐标。

由三維空間坐标通過相機内參矩陣投影到像素坐标。

p 1 = [ u 1 v 1 1 ] = 1 Z 1 K P , p_1 = \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} = \frac{1}{Z_1}KP, p1​=⎣⎡​u1​v1​1​⎦⎤​=Z1​1​KP,

p 2 = [ u 2 v 2 1 ] = 1 Z 2 K ( R P + t ) = 1 Z 2 K ( e x p ( [ ξ ] × ) P ) 1 : 3 . p_2 = \begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix} = \frac{1}{Z_2}K(RP + t) = \frac{1}{Z_2}K(exp([\xi]_{\times})P)_{1:3}. p2​=⎣⎡​u2​v2​1​⎦⎤​=Z2​1​K(RP+t)=Z2​1​K(exp([ξ]×​)P)1:3​.

直接法中沒有特征比對，通過估計的變換尋找p1對應的p2使p1與p2的亮度誤差最小：

m i n ξ J ( ξ ) = ∑ i = 1 N e i T e i ,      e i = I 1 ( p 1 , i ) − I 2 ( p 2 , i ) . min_{\xi} J(\xi) = \sum_{i = 1}^{N} e_i^T e_i, ~~~~ e_i = I_1(p_1, i) - I_2(p_2, i). minξ​J(ξ)=i=1∑N​eiT​ei​,    ei​=I1​(p1​,i)−I2​(p2​,i).

2.2 LSD的直接法

使用方差歸一化後的光度誤差（variance-normalized photometric error）：

(1) E p ( ξ j i ) = ∑ p ∈ Ω D i ∥ r p 2 ( p , ξ j i ) σ r p ( p , ξ j i ) 2 ∥ δ E_p(\mathbf\xi_{ji}) =\sum_{\mathbf{p}\in\Omega_{D_i}} \Biggl\|\frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2}\Biggr\|_\delta \tag{1} Ep​(ξji​)=p∈ΩDi​​∑​∥∥∥∥∥​σrp​(p,ξji​)2​rp2​(p,ξji​)​∥∥∥∥∥​δ​(1)

(2) r p ( p , ξ j i ) : = I i ( p ) − I j ( ω ( p , D i ( p ) , ξ j i ) ) r_p(\mathbf{p},{\mathbf\xi_{ji}}) := I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{2} rp​(p,ξji​):=Ii​(p)−Ij​(ω(p,Di​(p),ξji​))(2)

(3) σ r p ( p , ξ j i ) 2 : = 2 σ I 2 + ( ∂ r p ( p , ξ j i ) ∂ D i ( p ) ) 2 V i ( p ) \sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2 := 2\sigma_I^2 + (\frac{\partial{r_p(\mathbf{p}, \mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}})^2V_i(\mathbf{p}) \tag{3} σrp​(p,ξji​)2​:=2σI2​+(∂Di​(p)∂rp​(p,ξji​)​)2Vi​(p)(3)

• ξ j i \xi_{ji} ξji​為從關鍵幀 i i i到目前幀 j j j的SE3變換，用李代數形式表示;

• p \mathbf{p} p是在參考幀 I i I_i Ii​觀測到的有深度資訊 ( p ∈ Ω D i ) (\mathbf{p} \in \Omega_{D_i}) (p∈ΩDi​​)的歸一化圖像點;

  p = [ X d Y d 1 ] . \mathbf{p} = \begin{bmatrix} Xd \\ Yd \\ 1 \end{bmatrix}. p=⎣⎡​XdYd1​⎦⎤​.

  其中 d = 1 Z d = \frac{1}{Z} d=Z1​為深度的逆。

• I : Ω → R I: \Omega \rightarrow \R I:Ω→R 每個像素坐标都對應一個光度值。
• ω \omega ω 是一個投影映射将 p \mathbf{p} p先通過 ξ j i \xi_{ji} ξji​從 i i i坐标系轉到 j j j坐标系，再通過相機内參矩陣投影至圖像得到像素坐标。
• D : Ω → R D: \Omega \rightarrow \R D:Ω→R 每個像素坐标對應一個逆深度， V : Ω → R V: \Omega \rightarrow \R V:Ω→R 每個像素坐标對應逆深度的方差
• σ I 2 \sigma_I^2 σI2​是圖像的高斯噪聲

• ∥ ∗ ∥ δ \|*\|_{\delta} ∥∗∥δ​為Huber-norm, 表示為：

  ∥ r 2 ∥ δ : = { r 2 2 δ if ∣ r ∣ ≤ δ ∣ r ∣ − δ 2 otherwise \|r^2\|_\delta:=\begin{cases} \frac{r^2}{2\delta} & \text{if}\quad |r|\le \delta \\[2ex] |r|-\frac{\delta}{2} & \text{otherwise} \end{cases} ∥r2∥δ​:=⎩⎨⎧​2δr2​∣r∣−2δ​​if∣r∣≤δotherwise​

論文中在估計兩幀間位姿變換的時候，把所有有逆深度假設的像素都用上了，但是每個逆深度的确定性不同，也就是有些逆深度比較準确有些不準确。準确與否展現在方差上，是以(1)用殘差除以了方差，相當于給每對殘差乘了個權重，方差大的則不準确權重小，方差小的較準确權重大。在論文中主要考慮兩個方面的方差，一個是由逆深度估計不準确引入的，一個是由圖像高斯噪聲引起的。是以(3)式前面一項是兩幅圖像的高斯噪聲，後一項為逆深度誤差造成的不确定性：

Σ f = J f Σ x J f T \Sigma_f = J_f \Sigma_x J_f^T Σf​=Jf​Σx​JfT​

3. Depth mapping

LSD-SLAM建構的是半稠密逆深度地圖（semi-dense inverse depth map），隻對有明顯梯度的像素位置進行深度估計，用逆深度表示，并且假設逆深度服從高斯分布。一旦一個圖像幀被選為關鍵幀，則用其跟蹤的參考幀的深度圖對其進行深度圖建構，之後跟蹤到該建立關鍵幀的圖像幀都會用來對其深度圖進行更新。當然，追溯到第一幀，肯定是沒有深度圖的，是以第一幀的深度圖是有明顯梯度區域随機生成的深度。

這部分主要是用Tracking跟蹤後的幀更新或建構深度圖，分兩種情況：

• 建構關鍵幀， 則建構新的深度圖(Depth Map Creation);
• 不建構關鍵幀， 則更新目前關鍵幀的深度圖(Depth Map Refinement).

3.1 Keyframe selection

論文中根據運動距離來确定，如果目前相機運動距離上一個關鍵幀達到一定門檻值， 則把目前幀作為關鍵幀。并給出了距離函數：

d i s t ( ξ j i ) : = ξ j i T W ξ j i . dist(\xi_{ji}) := \xi_{ji}^T \mathbf{W} \xi_{ji}. dist(ξji​):=ξjiT​Wξji​.

其中 W \mathbf{W} W是一個對角矩陣包含權重。 為了防止尺度漂移帶來的影響，作者統一将frame下的depth均值縮放到了1。這個時候門檻值标準就一樣了。

3.2 Depth Map Creation（Depth Map Propagation）

假設目前幀成為新的關鍵幀， 則将前一個關鍵幀的深度圖投影到目前幀來初始化新關鍵幀的深度圖。

将前一個關鍵幀的深度圖傳播到新關鍵幀的深度圖時，主要考慮逆深度誤差的傳播。假設兩幀之間的旋轉很小，新關鍵幀的逆深度就可以近似為：

d 1 ( d 0 ) = ( d 0 − 1 − t z ) − 1 . d_1(d_0) = (d_0^{-1} - t_z)^{-1}. d1​(d0​)=(d0−1​−tz​)−1.

這裡 t z t_z tz​是相機沿着光軸方向的位移， 在實際求逆深度的時候，是考慮旋轉的，把參考關鍵幀上的點通過SE3變換到目前新的關鍵幀上來，然後求逆深度。逆深度的方差：

σ d 1 2 = J d 1 σ d 0 2 J d 1 T + σ p 2 = ( d 1 d 0 ) 4 σ d 0 2 + σ p 2 \sigma_{d_1}^2 = J_{d_1}\sigma_{d_0}^2J_{d_1}^T + \sigma_p^2 = (\frac{d_1}{d_0})^4 \sigma_{d_0}^2 + \sigma_p^2 σd1​2​=Jd1​​σd0​2​Jd1​T​+σp2​=(d0​d1​​)4σd0​2​+σp2​

這裡 σ p 2 \sigma_p^2 σp2​為預測不确定性。

傳播完深度圖和逆深度之後， 進行一次正則化(regularization)及異常值去除(outlier removal)。

• Regularization: 每個像素深度用周圍像素對應的深度的權重平均來替代，這裡權值為各自方差。若兩個鄰接像素的深度內插補點大于門檻值 2 σ 2\sigma 2σ， 則不參與算平均。
• Outlier Removal: 在propagation過程中，如果光度發生明顯的變化，或者絕對圖像梯度低于一個門檻值時，視為異常值。

3.3 Depth Map Refinement

假設目前幀不作為新的關鍵幀， 在Tracking線程對目前幀的位姿進行估計後，目前幀就被送到建圖線程用于更新關鍵幀的深度圖。論文中成為基于立體比對的深度更新（Stereo-Based Depth Map Update）, 通過極線搜尋在圖像幀中找到與參考幀比對的圖像點，然後通過新比對的觀測點對逆深度進行更新。

3.3.1 參考圖像幀選取

這部分主要讨論如何選取與目前幀做極線比對是圖像幀。

注意

： 在論文和代碼中都把與幀做立體比對的圖像幀稱為參考幀(Reference Frame)。

考慮到準确性，立體比對時盡可能選取視察和觀測角小的兩幀圖像。當兩幀圖像間的基線過長，則會有很多錯誤比對出現（大基線比對點在圖像上運動較大）。小基線的兩個視圖對深度進行估計誤差往往比較大（小基線能恢複的深度範圍就小）。是以，在論文中提出一種自适應的方法。

在論文中，把離目前幀遠的圖像幀稱為“老的”， 那些最近擷取的圖像幀稱為“新的”。 使用觀察到像素的最老幀，其中視差搜尋範圍和觀察角度不超過某個門檻值。如下圖所示， 左上是目前幀， 下面兩行是之前的圖像幀， 下面是距離目前幀的時間，負的越多就說明該幀越老。

使用觀察到像素的最老幀，其中視差搜尋範圍和觀察角度不超過某個門檻值。如果視差搜尋不成功（即未找到良好比對），則像素的“年齡”會增加，這樣後續的視差搜尋将使用像素可能仍然可見的較新幀。

3.3.2 立體比對政策

上一步已經選取好了待比對的參考幀， 這部分主要介紹找到目前幀需要更新深度深度的像素點的對應點，在標明的參考幀中，對像素沿極線的強度進行了詳盡的搜尋，然後對比對視差進行亞像素精确定位。

基線

，

極線

如下所示。

假設 I 2 I_2 I2​為目前幀, 對于 p \mathbf{p} p在 I 2 I_2 I2​上的像素點， 我們在 I 1 I_1 I1​上所對應的極線上搜尋找對應點。

如果目前幀上的點已有逆深度的先驗假設 N ( d , σ d 2 ) N(d,\sigma_d^2) N(d,σd2​), 則設定的逆深度搜尋範圍為 d ± 2 σ d d \pm 2\sigma_d d±2σd​， 否則在整個範圍内搜尋。

論文中在極線的五個等距點上使用SSD（Sum of Squared Distance）誤差：雖然這顯著提高了高頻率圖像區域的魯棒性，但它不會改變這種搜尋的純一維性質。此外，它的計算效率很高，因為5個插值圖像值中的4個可用于每個SSD評估。

E S S D ( u ) = ∑ i [ I 1 ( x i + u ) − I 0 ( x i ) ] 2 E_{SSD}(u) = \sum_i [I_1 (x_i + u) - I_0(x_i)]^2 ESSD​(u)=i∑​[I1​(xi​+u)−I0​(xi​)]2

3.3.3 不确定性估計

通過立體比對找到參考幀上的對應點後，我們可以求得新的逆深度值。 此外，我們還需要求解逆深度的不确定性。通過對兩圖像 I 0 I_0 I0​, I 1 I_1 I1​立體比對求得的最佳比對點對應的逆深度可以表示為：

d ∗ = d ( I 0 , I 1 , ξ , π ) d^* = d(I_0, I_1, \xi, \pi) d∗=d(I0​,I1​,ξ,π)

這裡 ξ \xi ξ為兩幀間位姿變換， π \pi π為相機投影模型參數。 進而得到 d ∗ d^* d∗的誤差方差(error-variance)為：

σ d = J d Σ J d T \sigma_d = J_d \Sigma J_d^T σd​=Jd​ΣJdT​

這裡 J d J_d Jd​是對 d d d的雅克比， Σ \Sigma Σ為輸入誤差的方差。

計算逆深度主要分為3個步驟：

1. 計算在參考幀中的對極線;
2. 在對極線上找到最好的比對位置 λ ∗ ∈ R \lambda^* \in \R λ∗∈R(視差);
3. 通過 λ ∗ \lambda^* λ∗求出逆深度 d ∗ d^* d∗。

這三個步驟分别涉及三個誤差：

• 幾何視差誤差: ξ \xi ξ 和 π \pi π中的噪聲将影響第1步極線的位置， 進而導緻比對點位置的誤差;
• 光度視差誤差： 在圖像 I 0 I_0 I0​和 I 1 I_1 I1​上的噪聲将影響第2步比對位置的求取;
• 逆深度計算誤差: 逆深度誤差主要來源于兩個地方， 一是比對的像素位置， 另外就是兩個圖像間的基線長度。

接下來将對這幾個誤差如何模組化進行分析。

a. 幾何視差誤差（Geometric disparity error）

理論上可以計算準确的 ξ \xi ξ和 π \pi π中噪聲的方差， 但是在論文中指出這樣計算增加計算的複雜度但沒有等價地提升準确性， 是以論文中使用了一個簡單的近似。 定義極線段：

L : = { l 0 + λ ( l x l y ) ∣ λ ∈ S } L:=\begin{Bmatrix}l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\mid\lambda\in S\end{Bmatrix} L:={l0​+λ(lx​ly​​)∣λ∈S​}

這裡 λ \lambda λ是在搜尋範圍内的視差， ( l x , l y ) T (l_x, l_y)^T (lx​,ly​)T是歸一化的極線方向， l 0 l_0 l0​是極線上對應無窮遠點的圖像點， 假設 I 0 I_0 I0​受到各向同性高斯噪聲 ϵ l \epsilon_l ϵl​的影響。 論文指出， 當搜尋的極線段比較小， 并且旋轉誤差較小的情況下， 這個近似還是比較好的。

如圖所示， L是極線段， g , l g, l g,l分别是梯度方向和極線方向， 圓中心的點是一個真值， 由于各向同性高斯噪聲， 使得極線段有一個絕對偏差 ϵ l \epsilon_l ϵl​, 最後再通過灰階等值線（即黑色虛線）找到灰階相同的點， 就得到了這個點和真值點之間的幾何誤差 ϵ λ \epsilon_\lambda ϵλ​， 可以看出如果極線與圖像梯度平行，則極線上的定位誤差 ϵ l \epsilon_l ϵl​會導緻較小的視差誤差 ϵ λ \epsilon_\lambda ϵλ​，否則會導緻較大的視差誤差。

假設梯度局部不變， 則有：

l 0 + λ ( l x l y ) ＝ ! g 0 + γ ( − g y g x ) γ ∈ R l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\stackrel{!}＝g_0+\gamma\begin{pmatrix}-g_y\\g_x\end{pmatrix} \quad\quad \gamma\in\mathbb{R} l0​+λ(lx​ly​​)＝!g0​+γ(−gy​gx​​)γ∈R

這裡 g 0 g_0 g0​為等值線上的一點， g 0 g_0 g0​處的灰階值等于或接近待比對像素的灰階值。 g : = ( g x , g y ) g:=(g_x,g_y) g:=(gx​,gy​)表示歸一化梯度方向向量，則(−gy,gx)T表示歸一化等值線方向向量。 則 ( − g y , g x ) T (−g_y,g_x)^T (−gy​,gx​)T表示歸一化等值線方向向量。 故等式左邊是沿極線搜尋到的比對點坐标， 等式右邊表示該點位于等值線上。 由于下面會分析圖像噪聲的影響，是以這裡假設 g 0 g_0 g0​和 g g g不受噪聲影響。 等式兩邊同時與 g g g進行內積， 得：

(4) λ ∗ ( l 0 ) = ⟨ g , g 0 − l 0 ⟩ ⟨ g , l ⟩ \lambda^*(l_0)=\frac{\langle g,g_0-l_0 \rangle}{\langle g,l \rangle}\tag{4} λ∗(l0​)=⟨g,l⟩⟨g,g0​−l0​⟩​(4)

由協方差傳播可得：

(5) σ λ ( ξ , π ) 2 = J λ ∗ ( l 0 ) ( σ l 2 0 0 σ l 2 ) J λ ∗ ( l 0 ) T \sigma_{\lambda(\xi,\pi)}^2=J_{\lambda^*(l_0)}\begin{pmatrix}\sigma_l^2&0\\0&\sigma_l^2\end{pmatrix}J_{\lambda^*(l_0)}^{T}\tag{5} σλ(ξ,π)2​=Jλ∗(l0​)​(σl2​0​0σl2​​)Jλ∗(l0​)T​(5)

由式(4)得：

J λ ∗ ( l 0 ) = ∂ λ ∗ ( l 0 ) ∂ l 0 = − g T ⟨ g , l ⟩ J_{\lambda^*(l_0)}=\frac{\partial\lambda^*(l_0)}{\partial l_0}=-\frac{g^T}{\langle g,l \rangle} Jλ∗(l0​)​=∂l0​∂λ∗(l0​)​=−⟨g,l⟩gT​

代人式(5)得：

(6) σ λ ( ξ , π ) 2 = ⟨ g , g ⟩ ⋅ σ l 2 ⟨ g , l ⟩ 2 = σ l 2 ⟨ g , l ⟩ 2 \sigma_{\lambda(\xi,\pi)}^2=\frac{\langle g,g\rangle\cdot\sigma_l^2}{\langle g,l\rangle^2}=\frac{\sigma_l^2}{\langle g,l\rangle^2}\tag{6} σλ(ξ,π)2​=⟨g,l⟩2⟨g,g⟩⋅σl2​​=⟨g,l⟩2σl2​​(6)

其中， σ l 2 \sigma_l^2 σl2​為極線位置誤差 ϵ l \epsilon_l ϵl​的方差， 隻和相機位姿 ξ \xi ξ和相機參數 π \pi π相關。

從式(6)可以得出以下結論:

• 位姿擾動造成的 σ l 2 \sigma_l^2 σl2​越大， 則幾何視差誤差越大
• 梯度 g g g和極線 l l l夾角越小， 幾何視差誤差越小

b. 光度視差誤差

在極線搜尋的時候，我們找到的點滿足SSD誤差最小， 也就有：

(7) λ ∗ ＝ min ⁡ λ ( i r e f − I p ( λ ) ) 2 \lambda^*＝\min_{\lambda}(i_{ref}-I_p(\lambda))^2\tag{7} λ∗＝λmin​(iref​−Ip​(λ))2(7)

這裡的 i r e f i_{ref} iref​是目前幀的灰階， I p ( λ ) I_p(\lambda) Ip​(λ)是參考幀圖像極線在視差為 λ \lambda λ位置的灰階。 假設在極線上有過徹底的搜尋， 得到一個很好的初始位置 λ 0 \lambda_0 λ0​, 對式(7)中的 I p ( λ ) I_p(\lambda) Ip​(λ)做一階泰勒展開， 然後對增量 Δ λ \Delta\lambda Δλ求導并令式子等于0， 則可解出

(8) λ ∗ ( I ) = λ 0 + Δ λ = λ 0 + ( i r e f − I p ( λ 0 ) ) g p − 1 \lambda^*(I)=\lambda_0+\Delta\lambda=\lambda_0+(i_{ref}-I_p(\lambda_0))g_p^{-1}\tag{8} λ∗(I)=λ0​+Δλ=λ0​+(iref​−Ip​(λ0​))gp−1​(8)

這裡的 g p g_p gp​是圖像沿極線的梯度。 這裡同樣不考慮梯度的噪聲， 隻考慮兩個圖像的噪聲。 有：

(9) σ λ ( I ) 2 = J λ ∗ ( I ) ( σ i 2 0 0 σ i 2 ) J λ ∗ ( I ) T = 2 σ i 2 g p 2 \sigma_{\lambda(I)}^2=J_{\lambda^*(I)}\begin{pmatrix}\sigma_i^2&0\\0&\sigma_i^2\end{pmatrix}J_{\lambda^*(I)}^T=\frac{2\sigma_i^2}{g_p^2}\tag{9} σλ(I)2​=Jλ∗(I)​(σi2​0​0σi2​​)Jλ∗(I)T​=gp2​2σi2​​(9)

這裡由于同時對關鍵幀和參考幀的灰階都計算了噪聲，是以會有

２

這個系數，這個方程是一個線性方程，我們也可以這麼求：

σ λ ( I ) 2 = Var ( λ ∗ ( I ) ) = ( Var ( i r e f ) + Var ( I p ) ) g p − 2 = 2 σ i 2 g p 2 \sigma_{\lambda(I)}^2=\text{Var}(\lambda^*(I))=(\text{Var}(i_{ref})+\text{Var}(I_p))g_p^{-2}=\frac{2\sigma_i^2}{g_p^2} σλ(I)2​=Var(λ∗(I))=(Var(iref​)+Var(Ip​))gp−2​=gp2​2σi2​​

這裡的 σ i 2 σ_i^2 σi2​是圖像的高斯噪聲的方差，這裡的光度視差誤差和幾何光度誤差是獨立的。

$$

這裡的 σ i 2 σ_i^2 σi2​是圖像的高斯噪聲的方差，這裡的光度視差誤差和幾何光度誤差是獨立的。

如下圖所示，比較直覺。直線斜率的絕對值表示極線上梯度大小，當梯度值越大時，可以看出光度視差誤差越小。直接法因為是靠圖像梯度來不斷調整位姿的，是以梯度必須較大，這樣才能在優化中較快較好地收斂。

結合上述代數和幾何分析，得出結論：

• 圖像噪聲越大，光度視差誤差越大
• 極線方向上梯度越大，光度視差誤差越小

c. 逆深度計算誤差

論文中指出，當旋轉比較小的時候， 逆深度 d d d和視差 λ \lambda λ近似成一個比例關系（當旋轉較小時可近似為一個雙目模型， 視差 λ = u L − u R = f b z \lambda = u_L - u_R= \frac{fb}{z} λ=uL​−uR​=zfb​）, 而 λ \lambda λ誤差方差可以表現為幾何視差誤差和光度視差誤差方差之和：

(10) σ d , obs 2 = α 2 ( σ λ ( ξ , π ) 2 + σ λ ( I ) 2 ) \sigma_{d,\text{obs}}^2=\alpha^2\begin{pmatrix}\sigma_{\lambda(\xi,\pi)}^2+\sigma_{\lambda(I)}^2\end{pmatrix}\tag{10} σd,obs2​=α2(σλ(ξ,π)2​+σλ(I)2​​)(10)

其中 α \alpha α是權重， 其定義如下：

(11) α : = δ d δ λ \alpha:=\frac{\delta{d}}{\delta{\lambda}}\tag{11} α:=δλδd​(11)

式中， δ d \delta{d} δd為搜尋反深度間隔的長度， δ λ \delta\lambda δλ為搜尋極外線段的長度。雖然 α \alpha α在相機平移長度上是反線性的，但它也取決于平移方向和像素在圖像中的位置(這句沒看懂)。

如果一個小的極線上的變化會導緻深度變化大， 那就是說此時不确定性大，故權重大一些。

考慮到在極線上搜尋比對點的時候，是使用了多個點，是以實際上這裡是給出了逆深度誤差的上限：

(12) σ d , obs 2 ≤ α 2 ( min { σ λ ( ξ , π ) 2 } + min { σ λ ( I ) 2 } ) \sigma_{d,\text{obs}}^2\le\alpha^2\begin{pmatrix}\text{min}\{\sigma_{\lambda(\xi,\pi)}^2\}+\text{min}\{\sigma_{\lambda(I)}^2\}\end{pmatrix}\tag{12} σd,obs2​≤α2(min{σλ(ξ,π)2​}+min{σλ(I)2​}​)(12)

3.3.4 深度觀測融合

通過目前幀比對的像素為深度提供一個新的觀測值，然後就可以把目前觀測的深度融合到關鍵幀的深度地圖中去。這裡有兩種情況：當對應像素點沒有深度先驗時則由新的觀測值建構新的先驗；當已經有先驗值的話，則把新觀測值融合到先驗中去。在這個融合的過程，使用了兩個高斯分布乘法的方式：對于給定先驗 N ( d p , σ p 2 ) \mathcal{N}(d_p,\sigma_p^2) N(dp​,σp2​)以及有噪聲的觀測值 N ( d o , σ o 2 ) \mathcal{N}(d_o,\sigma_o^2) N(do​,σo2​)，給出後驗估計：

(13) N ( σ p 2 d o + σ o 2 d p σ p 2 + σ o 2 , σ p 2 σ o 2 σ p 2 + σ o 2 ) \mathcal{N}\begin{pmatrix}\frac{\sigma_p^2 d_o + \sigma_o^2d_p}{\sigma_p^2 + \sigma_o^2}, \frac{\sigma_p^2\sigma_o^2}{\sigma_p^2 + \sigma_o^2}\end{pmatrix}\tag{13} N(σp2​+σo2​σp2​do​+σo2​dp​​,σp2​+σo2​σp2​σo2​​​)(13)

4. Map Optimization

這部分在論文中叫做一緻性限制（constraint acquisition）， 是算法的核心（解決尺度問題）， 因為長距離會出現尺度漂移， 這部分主要分為閉環檢測和全局優化。 檢測閉環的方式是幀與幀之間做雙向跟蹤， 删選好候選幀就添加到pose graph中， 進行圖優化。

關于選擇候選幀，主要步驟：

• 視角和距離判别
• SE3跟蹤檢測
• Sim3跟蹤檢測

第一個步驟是根據每個關鍵幀求得的位姿判斷的， 把距離目前關鍵幀較遠的關鍵幀剔除。 第二步是通過SE3跟蹤選取與目前關鍵幀距離較近的候選關鍵幀。 第三步是核心， 把每一個候選幀都與目前關鍵幀做雙向的Sim3跟蹤， 如果都成功， 并且兩個 s i m ( 3 ) \mathfrak{sim}(3) sim(3)的馬氏距離足夠小，則任務是閉環，并向pose graph中添加邊。

接下來主要看一下Sim(3)求解以及雙向跟蹤檢測(reciprocal tracking check).

4.1 Sim3求解

4.1.1 原理分析

a. Sim3 圖像對齊

首先和 s e ( 3 ) \mathfrak{se}(3) se(3)變換一樣， 對于兩幀圖像上對應歸一化圖像點 p ′ p' p′和 p p p通過 s i m ( 3 ) \mathfrak{sim}(3) sim(3)變換有：

(14) p ′ = ( x ′ / z ′ y ′ / z ′ 1 ) ⎵ ω n with ( x ′ y ′ z ′ 1 ) : = exp s i m ( 3 ) ( ξ ) ( p x / d p y / d 1 / d 1 ) ⎵ w s \underbrace{\mathbf{p}' = \begin{pmatrix}x'/z'\\y'/z'\\1\end{pmatrix}}_{\omega_n} \quad\text{with}\quad \underbrace{\begin{pmatrix}x'\\y'\\z'\\1\end{pmatrix} := \text{exp}_{\mathfrak{sim}(3)}(\mathbf\xi)\begin{pmatrix}\mathbf{p}_x/d\\\mathbf{p}_y/d\\1/d\\1\end{pmatrix}}_{w_s} \tag{14} ωn​

p′=⎝⎛​x′/z′y′/z′1​⎠⎞​​​withws​

⎝⎜⎜⎛​x′y′z′1​⎠⎟⎟⎞​:=expsim(3)​(ξ)⎝⎜⎜⎛​px​/dpy​/d1/d1​⎠⎟⎟⎞​​​(14)

上式的代價函數和SE(3)相比多了一項尺度項， 并且依舊使用歸一化方差：

(15) E ( ξ j i ) = ∑ p ∈ Ω D i ∥ r p 2 ( p , ξ j i ) σ r p ( p , ξ j i ) 2 + r d 2 ( p , ξ j i ) σ r d ( p , ξ j i ) 2 ∥ δ E(\mathbf\xi_{ji}) =\sum_{\mathbf{p}\in\Omega_{D_i}}\Biggl\| \frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2} + \frac{r_d^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2} \Biggr\|_\delta \tag{15} E(ξji​)=p∈ΩDi​​∑​∥∥∥∥∥​σrp​(p,ξji​)2​rp2​(p,ξji​)​+σrd​(p,ξji​)2​rd2​(p,ξji​)​∥∥∥∥∥​δ​(15)

這裡的光度殘差和方差的定義和SE3跟蹤是一樣的:

(16) r p ( p , ξ j i ) : = I i ( p ) − I j ( ω ( p , D i ( p ) , ξ j i ) ) r_p(\mathbf{p},{\mathbf\xi_{ji}}) := I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{16} rp​(p,ξji​):=Ii​(p)−Ij​(ω(p,Di​(p),ξji​))(16)

(17) σ r p ( p , ξ j i ) 2 : = 2 σ I 2 + ( ∂ r p ( p , ξ j i ) ∂ D i ( p ) ) 2 V i ( p ) \sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2 := 2\sigma_I^2 + (\frac{\partial{r_p(\mathbf{p}, \mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}})^2V_i(\mathbf{p}) \tag{17} σrp​(p,ξji​)2​:=2σI2​+(∂Di​(p)∂rp​(p,ξji​)​)2Vi​(p)(17)

以及深度殘差和方差定義如下：

(18) r d ( p , ξ j i ) = [ p ′ ] 3 − D j ( [ p ′ ] 1 , 2 ) r_d(\mathbf{p},\mathbf\xi_{ji}) = [\mathbf{p}']_3-D_j([\mathbf{p}']_{1,2}) \tag{18} rd​(p,ξji​)=[p′]3​−Dj​([p′]1,2​)(18)

(19) σ r d ( p , ξ j i ) 2 = V j ( [ p ′ ] 1 , 2 ) ( ∂ r d ( p , ξ j i ) ∂ D j ( [ p ′ ] 1 , 2 ) ) + V i ( p ) ( ∂ r d ( p , ξ j i ) ∂ D i ( p ) ) \sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2 = V_j([\mathbf{p}']_{1,2})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_j([\mathbf{p}']_{1,2})}\end{pmatrix} + V_i(\mathbf{p})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_i(\mathbf{p})}\end{pmatrix}\tag{19} σrd​(p,ξji​)2​=Vj​([p′]1,2​)(∂Dj​([p′]1,2​)∂rd​(p,ξji​)​​)+Vi​(p)(∂Di​(p)∂rd​(p,ξji​)​​)(19)

這裡 p ′ : = ω s ( ( p ) , D i ( p ) , ξ j i ) \mathbf{p}' := \omega_s(\mathbf(p), D_i(\mathbf{p}), \xi_{ji}) p′:=ωs​((p),Di​(p),ξji​)， 其中 w s w_s ws​是用了sim3的變換。

求解部分用疊代變權重高斯牛頓算法，關于雅克閉矩陣的推導以後再推。

4.2 雙向跟蹤檢測

由于論文中采用的是直接法，雖然代碼中有fabmap檢測閉環的部分，但是其預設檢測閉環的方式是幀與幀之間做雙向跟蹤（Reciprocal tracking check）。首先搜尋最近的10個關鍵幀及一些外觀較像的幀作為候選幀，對每一個候選幀都計算其與目前關鍵幀彼此跟蹤的???(3)，然後計算兩者間的馬氏距離的平方：

(20) e ( ξ j k i , ξ i j k ) : = ( ξ j k i ∘ ξ i j k ) T ( Σ j k i + Adj j k i Σ i j k Adj j k i T ) − 1 ( ξ j k i ∘ ξ i j k ) e\left(\mathbf\xi_{j_k i}, \mathbf\xi_{i j_k}\right) := \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)^T \left(\mathbf\Sigma_{j_k i} + \text{Adj}_{j_k i}\mathbf\Sigma_{i j_k}\text{Adj}_{j_k i}^T\right)^{-1} \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)\tag{20} e(ξjk​i​,ξijk​​):=(ξjk​i​∘ξijk​​)T(Σjk​i​+Adjjk​i​Σijk​​Adjjk​iT​)−1(ξjk​i​∘ξijk​​)(20)

當距離足夠小時表明相似度較高，就将這一幀插入全局地圖中。最後執行圖優化（g2o中的pose graph optimization），邊為連接配接關系，節點為關鍵幀，即優化：

(21) E ( ξ W 1 ⋯ ξ W n ) = ∑ ( ξ j i , Σ ) ∈ ε ( ξ j i ∘ ξ W i − 1 ∘ ξ W j ) T Σ j i − 1 ( ξ j i ∘ ξ W i − 1 ∘ ξ W j ) E(\xi_{W1}\cdots\xi_{Wn})=\sum_{(\xi_{ji},\Sigma)\in\varepsilon}(\xi_{ji}\circ\xi_{Wi}^{-1}\circ\xi_{Wj})^T\Sigma_{ji}^{-1}(\xi_{ji}\circ\xi_{Wi}^{-1}\circ\xi_{Wj}) \tag{21} E(ξW1​⋯ξWn​)=(ξji​,Σ)∈ε∑​(ξji​∘ξWi−1​∘ξWj​)TΣji−1​(ξji​∘ξWi−1​∘ξWj​)(21)

參考

• LSDSLAM算法解析
• LSD-SLAM筆記之SE3Tracking
• LSD-SLAM筆記之DepthMap
• LSD-SLAM筆記之一緻性限制
• SVO & LSD_SLAM(賀一家)視訊
• SLAM 14講