如何上傳文件到自己的網(wǎng)站googleplay官方下載
文章目錄
- 09 后端 2
- 9.1 滑動(dòng)窗口濾波和優(yōu)化
- 9.1.1 實(shí)際環(huán)境下的 BA 結(jié)構(gòu)
- 9.1.2 滑動(dòng)窗口法
- 9.2 位姿圖
- 9.2.1 位姿圖的意義
- 9.2.2 位姿圖優(yōu)化
09 后端 2
9.1 滑動(dòng)窗口濾波和優(yōu)化
9.1.1 實(shí)際環(huán)境下的 BA 結(jié)構(gòu)
由于計(jì)算機(jī)算力的限制,我們必須控制 BA 的規(guī)模,一種簡(jiǎn)單的思路是僅保留離當(dāng)前時(shí)刻最近的 N N N 個(gè)關(guān)鍵幀,去掉時(shí)間上更早的關(guān)鍵幀。于是,BA 被固定在一個(gè)時(shí)間窗口內(nèi),離開這個(gè)窗口的即被拋棄,稱為 滑動(dòng)窗口法。
或者像 ORB-SLAM 2 那樣,定義一種稱為 共視圖 的結(jié)構(gòu),即與當(dāng)前相機(jī)存在共同觀測(cè)的關(guān)鍵幀構(gòu)成的圖。在 BA 優(yōu)化時(shí),按照某些原則在共視圖內(nèi)取一些關(guān)鍵幀和路標(biāo)進(jìn)行優(yōu)化。
9.1.2 滑動(dòng)窗口法
(1)現(xiàn)在考慮一個(gè)滑動(dòng)窗口,假設(shè)窗口內(nèi)有 N N N 個(gè)關(guān)鍵幀,他們的位姿表達(dá)為(李代數(shù)形式):
x 1 , x 2 , . . . , x N \boldsymbol{x}_1, \boldsymbol{x}_2,...,\boldsymbol{x}_N x1?,x2?,...,xN?
假設(shè)這個(gè)滑動(dòng)窗口中還有 M M M 個(gè)路標(biāo)點(diǎn) y 1 , y 2 , . . . , y M \boldsymbol{y}_1, \boldsymbol{y}_2,...,\boldsymbol{y}_M y1?,y2?,...,yM?,用上一講中的 BA 方法來處理這個(gè)滑動(dòng)窗口,包括建立圖優(yōu)化模型,構(gòu)建海森矩陣,在邊緣化所有路標(biāo)點(diǎn)來加速求解。邊緣化時(shí),考慮關(guān)鍵幀的位姿:
[ x 1 , … , x N ] T ~ N ( [ μ 1 , … , μ N ] T , Σ ) (9-1) \left[\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right]^{\mathrm{T}} \sim N\left(\left[\boldsymbol{\mu}_{1}, \ldots, \boldsymbol{\mu}_{N}\right]^{\mathrm{T}}, \boldsymbol{\Sigma}\right) \tag{9-1} [x1?,…,xN?]T~N([μ1?,…,μN?]T,Σ)(9-1)
其中 μ k \boldsymbol{\mu}_k μk? 為第 k k k 個(gè)關(guān)鍵幀的位姿均值, Σ \boldsymbol{\Sigma} Σ 為所有關(guān)鍵幀的協(xié)方差矩陣。顯然,均值部分就是 BA 迭代之后的結(jié)果, Σ \boldsymbol{\Sigma} Σ 是對(duì)整個(gè) BA 的 H \boldsymbol{H} H 矩陣進(jìn)行邊緣化之后的結(jié)果。
(2)當(dāng)窗口結(jié)構(gòu)改變時(shí):
① 先在窗口中新增一個(gè)關(guān)鍵幀,以及觀測(cè)到的路標(biāo)點(diǎn);
② 把窗口中一個(gè)舊的關(guān)鍵幀刪除,可能會(huì)刪除他觀測(cè)到的路標(biāo)點(diǎn)。
- 新增一個(gè)關(guān)鍵幀和路標(biāo)點(diǎn)
將新的關(guān)鍵幀 x N + 1 \boldsymbol{x}_{N+1} xN+1? 按照正常的 BA 流程處理即可。
- 刪除一個(gè)舊的關(guān)鍵幀
刪除舊的關(guān)鍵幀時(shí),就比較麻煩。比如刪除 x 1 \boldsymbol{x}_1 x1? ,但 x 1 \boldsymbol{x}_1 x1? 并不是孤立的,它會(huì)和其它幀觀測(cè)到同樣的路標(biāo),將 x 1 \boldsymbol{x}_1 x1? 邊緣化后將導(dǎo)致整個(gè)問題不再稀疏(破壞了路標(biāo)部分的對(duì)角塊結(jié)構(gòu))。
(3)滑動(dòng)窗口法適合 VO 系統(tǒng),而不適合大規(guī)模建圖的系統(tǒng)。
9.2 位姿圖
9.2.1 位姿圖的意義
(1)隨著時(shí)間的流逝,機(jī)器人的運(yùn)動(dòng)軌跡會(huì)越來越長(zhǎng),地圖規(guī)模也會(huì)越來越大,BA 的計(jì)算效率就會(huì)下降。同時(shí)我們發(fā)現(xiàn),經(jīng)過若干次迭代后,收斂的特征點(diǎn)位置變化很小,發(fā)散的外點(diǎn)則已被剔除,因此在后續(xù)優(yōu)化中沒有必要再將收斂點(diǎn)考慮進(jìn)來,而是只把他們當(dāng)做位姿估計(jì)的約束。
(2)放開思路,我們完全可以構(gòu)建一個(gè)只有軌跡的圖優(yōu)化,而位姿節(jié)點(diǎn)的邊,可以由兩個(gè)關(guān)鍵幀之間通過特征匹配之后得到的運(yùn)動(dòng)估計(jì)來給定初始值。一旦初始估計(jì)完成,就不再優(yōu)化那些路標(biāo)點(diǎn)的位置,而只關(guān)心相機(jī)位姿之間的聯(lián)系。這樣的方式,省去了大量特征點(diǎn)優(yōu)化的計(jì)算,只保留了關(guān)鍵幀的軌跡,構(gòu)建了所謂的 位姿圖。
通過舍棄對(duì)路標(biāo)點(diǎn)的優(yōu)化,提高計(jì)算效率。
9.2.2 位姿圖優(yōu)化
位姿圖中的節(jié)點(diǎn)表示相機(jī)位姿,用 T 1 , T 2 , . . . , T n , \boldsymbol{T}_1,\boldsymbol{T}_2,...,\boldsymbol{T}_n, T1?,T2?,...,Tn?, 表示,邊則是兩個(gè)位姿節(jié)點(diǎn)之間相對(duì)運(yùn)動(dòng)的估計(jì),這個(gè)估計(jì)可以通過特征點(diǎn)法或直接法得到。假設(shè)我們估計(jì)了 T i \boldsymbol{T}_i Ti? 和 T j \boldsymbol{T}_j Tj? 之間的相對(duì)運(yùn)動(dòng) T i j \boldsymbol{T}_{ij} Tij?,則有
T i T i j = T j \boldsymbol{T}_i \boldsymbol{T}_{ij}=\boldsymbol{T}_j Ti?Tij?=Tj?
也即
T i j = T i ? 1 T j (9-2) \boldsymbol{T}_{ij}=\boldsymbol{T}_i^{-1}\boldsymbol{T}_j \tag{9-2} Tij?=Ti?1?Tj?(9-2)
寫成李代數(shù)形式
ξ i j = ξ i ? 1 ° ξ j = ln ? ( T i ? 1 T j ) ∨ (9-3) \boldsymbol{\xi}_{i j}=\boldsymbol{\xi}_{i}^{-1} \circ \boldsymbol{\xi}_{j}=\ln \left(\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee} \tag{9-3} ξij?=ξi?1?°ξj?=ln(Ti?1?Tj?)∨(9-3)
將式(9-2)中的 Δ T i j \Delta \boldsymbol{T}_{ij} ΔTij? 移至方程右側(cè),則
T i j ? 1 T i ? 1 T j = I (9-4) \boldsymbol{T}_{ij}^{-1}\boldsymbol{T}_i^{-1}\boldsymbol{T}_j=\boldsymbol{I} \tag{9-4} Tij?1?Ti?1?Tj?=I(9-4)
但實(shí)際上,并不可能完全相等。定義
e i j = ln ? ( T i j ? 1 T i ? 1 T j ) ∨ (9-5) \boldsymbol{e}_{ij}=\ln( \boldsymbol{T}_{ij}^{-1}\boldsymbol{T}_i^{-1}\boldsymbol{T}_j)^{\vee} \tag{9-5} eij?=ln(Tij?1?Ti?1?Tj?)∨(9-5)
我們需要優(yōu)化的是 T i \boldsymbol{T}_i Ti? 和 T j \boldsymbol{T}_j Tj?,也即 ξ i \boldsymbol{\xi}_i ξi? 和 ξ j \boldsymbol{\xi}_j ξj?,因此需要求這兩個(gè)變量關(guān)于 e i j \boldsymbol{e}_{ij} eij? 的導(dǎo)數(shù)。分別左乘一個(gè)左擾動(dòng): δ ξ i \boldsymbol{\delta \xi}_i δξi? 和 δ ξ j \boldsymbol{\delta \xi}_j δξj?
e ^ i j = ln ? ( T i j ? 1 T i ? 1 exp ? ( ( ? δ ξ i ) ∧ exp ? ( ( δ ξ j ) ∧ T j ) ∨ (9-6) \boldsymbol{\hat{e}}_{ij}=\ln( \boldsymbol{T}_{ij}^{-1}\boldsymbol{T}_i^{-1} \exp((-\boldsymbol{\delta \xi}_i)^{\wedge}\exp((\boldsymbol{\delta \xi}_j)^{\wedge} \boldsymbol{T}_j)^{\vee} \tag{9-6} e^ij?=ln(Tij?1?Ti?1?exp((?δξi?)∧exp((δξj?)∧Tj?)∨(9-6)
根據(jù)伴隨矩陣的性質(zhì):
exp ? ( ( Ad ? ( T ) ξ ) ∧ ) = T exp ? ( ξ ∧ ) T ? 1 (9-7) \exp \left((\operatorname{Ad}(\boldsymbol{T}) \boldsymbol{\xi})^{\wedge}\right)=\boldsymbol{T} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{T}^{-1} \tag{9-7} exp((Ad(T)ξ)∧)=Texp(ξ∧)T?1(9-7)
稍作改變(把 Ad ? ( T ) \operatorname{Ad}(\boldsymbol{T}) Ad(T) 移到右側(cè),$ \boldsymbol{T}^{-1}$ 移到左側(cè))
exp ? ( ξ ∧ ) T = T exp ? ( ( Ad ? ( T ? 1 ) ξ ) ∧ ) (9-9) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{T}=\boldsymbol{T} \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}^{-1}\right) \boldsymbol{\xi}\right)^{\wedge}\right) \tag{9-9} exp(ξ∧)T=Texp((Ad(T?1)ξ)∧)(9-9)
那么,式(9-6)可寫為(從右往左化簡(jiǎn))
e ^ i j = ln ? ( T i j ? 1 T i ? 1 exp ? ( ( ? δ ξ i ) ∧ ) exp ? ( δ ξ j ∧ ) T j  ̄ ) ∨ = ln ? ( T i j ? 1 T i ? 1 exp ? ( ( ? δ ξ i ) ∧ ) T j  ̄ exp ? ( ( Ad ? ( T j ? 1 ) δ ξ j ) ∧ ) ) ∨ = ln ? ( T i j ? 1 T i ? 1 T j exp ? ( ( ? Ad ? ( T j ? 1 ) δ ξ i ) ∧ ) exp ? ( ( Ad ? ( T j ? 1 ) δ ξ j ) ∧ ) ) ∨ ≈ ln ? ( T i j ? 1 T i ? 1 T j [ I ? ( Ad ? ( T j ? 1 ) δ ξ i ) ∧ + ( Ad ? ( T j ? 1 ) δ ξ j ) ∧ ] ) ∨ ≈ e i j + ? e i j ? δ ξ i δ ξ i + ? e i j ? δ ξ j δ ξ j (9-10) \begin{aligned} \hat{\boldsymbol{e}}_{i j} &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \underline{\exp \left(\delta \boldsymbol{\xi}_{j}^{\wedge}\right) \boldsymbol{T}_{j}}\right)^{\vee} \\ &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \underline{\exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \boldsymbol{T}_{j}} \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{j}\right)^{\wedge}\right)\right)^{\vee} \\ &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j} \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{j}\right)^{\wedge}\right)\right)^{\vee} \\ & \approx \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\left[\boldsymbol{I}-\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{j}\right)^{\wedge}\right]\right)^{\vee} \\ & \approx \boldsymbol{e}_{i j}+\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{i}} \boldsymbol{\delta} \boldsymbol{\xi}_{i}+\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{j}} \boldsymbol{\delta} \boldsymbol{\xi}_{j} \end{aligned} \tag{9-10} e^ij??=ln(Tij?1?Ti?1?exp((?δξi?)∧)exp(δξj∧?)Tj??)∨=ln(Tij?1?Ti?1?exp((?δξi?)∧)Tj??exp((Ad(Tj?1?)δξj?)∧))∨=ln(Tij?1?Ti?1?Tj?exp((?Ad(Tj?1?)δξi?)∧)exp((Ad(Tj?1?)δξj?)∧))∨≈ln(Tij?1?Ti?1?Tj?[I?(Ad(Tj?1?)δξi?)∧+(Ad(Tj?1?)δξj?)∧])∨≈eij?+?δξi??eij??δξi?+?δξj??eij??δξj??(9-10)
其中,第四步將兩個(gè)指數(shù)一階泰勒展開,相乘后舍去二次項(xiàng);第四步到第五步則使用了 BCH 近似。
按照李代數(shù)上的求導(dǎo)法則,我們得到了誤差關(guān)于兩個(gè)位姿的雅克比矩陣,即
? e i j ? δ ξ i = ? J r ? 1 ( e i j ) Ad ? ( T j ? 1 ) \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{i}}=-\mathcal{J}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) ?δξi??eij??=?Jr?1?(eij?)Ad(Tj?1?)
? e i j ? δ ξ j = J r ? 1 ( e i j ) Ad ? ( T j ? 1 ) (9-11) \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{j}}=\mathcal{J}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \tag{9-11} ?δξj??eij??=Jr?1?(eij?)Ad(Tj?1?)(9-11)
J r \mathcal{J}_{r} Jr? 的形式比較復(fù)雜,通常取它的近似,
J r ? 1 ( e i j ) ≈ I + 1 2 [ ? e ∧ ρ e ∧ 0 ? e ∧ ] (9-12) \mathcal{J}_{r}^{-1}\left(e_{i j}\right) \approx \boldsymbol{I}+\frac{1}{2}\left[\begin{array}{cc} \phi_{e}^{\wedge} & \rho_{e}^{\wedge} \\ 0 & \phi_{e}^{\wedge} \end{array}\right] \tag{9-12} Jr?1?(eij?)≈I+21?[?e∧?0?ρe∧??e∧??](9-12)
了解雅克比求導(dǎo)后,剩下的部分就是普通的圖優(yōu)化。 記所有的邊(也就是位姿)為 E \mathcal{E} E,則總體目標(biāo)函數(shù)為
min ? 1 2 ∑ i , j ∈ E e i j T Σ i j ? 1 e i j (9-13) \min \frac{1}{2} \sum_{i, j \in \mathcal{E}} \boldsymbol{e}_{i j}^{\mathrm{T}} \boldsymbol{\Sigma}_{i j}^{-1} \boldsymbol{e}_{i j} \tag{9-13} min21?i,j∈E∑?eijT?Σij?1?eij?(9-13)
然后再用高斯牛頓法或 L-M 法優(yōu)化求解。