滑窗優(yōu)化

在SLAM問題中，狀態(tài)估計(jì)問題被建模為最大后驗(yàn)概率問題，在假設(shè)為高斯分布的情況下即為最小化損失函數(shù)的一個(gè)最小二乘問題，并通過泰勒展開轉(zhuǎn)化為增量迭代求解問題：
$$\begin{gathered} \hat{x}={\mathrm{argmax}}_{x}p(z|x)={\mathrm{argmax}}_x\Pi p(z_{ij}|x_i,x_j) \\ ?{\mathrm{argmin}}_x\frac{1}{2}\Sigma \Vert z_{ij}?h_{ij}(x_i,x_j){\Vert }_{{\Lambda }_{ij}}^2 \\ ?\delta x={\mathrm{argmin}}_{\delta x}\frac{1}{2}\Sigma \Vert z_{ij}?h_{ij}({\widehat{x_i}}^{(k)},{\widehat{x_j}}^{(k)})?J_{ij}^{(k)}\delta x{\Vert }_{{\Lambda }_{ij}}^2 \end{gathered}$$隨著SLAM系統(tǒng)的運(yùn)行，狀態(tài)變量規(guī)模不斷增大。使用滑窗優(yōu)化是限制計(jì)算量的常用手段。

邊緣化

對(duì)于滑窗外的狀態(tài)，我們不去進(jìn)行優(yōu)化，但也不能直接丟掉，這樣會(huì)破壞原有的約束關(guān)系，損失約束信息。采用邊緣化的技巧，將約束信息轉(zhuǎn)化為待優(yōu)化變量的先驗(yàn)分布，實(shí)際上是一個(gè)從聯(lián)合分布中獲得變量子集概率分布的問題。
定義待邊緣化變量$x_m$,和該變量有約束的待優(yōu)化變量$x_b$,剩余待優(yōu)化變量$x_r$，$x_m$和$x_b$間的約束為$z_m$,$x_b$間的約束為$z_b$,剩余約束為$z_r$，將優(yōu)化問題拆解為兩部分
$$\hat{x}={\mathrm{argmin}}_{x_m,x_b}\frac{1}{2}\sum _{(i,j)\in z_m}\Vert z_{ij}?h_{ij}(x_i,x_j){\Vert }_{{\Lambda }_{ij}}^2+{\mathrm{argmin}}_{x_b,x_r}\frac{1}{2}\sum _{(i,j)\in z_b,z_r}\Vert z_{ij}?h_{ij}(x_i,x_j){\Vert }_{{\Lambda }_{ij}}^2$$其中第一部分進(jìn)行邊緣化處理，將約束封裝為$x_b$的先驗(yàn)$\mathcal{N}(\widehat{x_b},{\Lambda }_t^{?1})$,這樣優(yōu)化問題轉(zhuǎn)為：
$$\hat{x}={\mathrm{argmin}}_x\frac{1}{2}\Vert \widehat{x_b}?x_b{\Vert }_{{\Lambda }_t}^2+\frac{1}{2}\sum _{(i,j)\in z_b,z_r}\Vert z_{ij}?h_{ij}(x_i,x_j){\Vert }_{{\Lambda }_{ij}}^2$$邊緣化采用的方式為舒爾補(bǔ)(schur complement)

證明

概率角度

狀態(tài)變量滿足高斯聯(lián)合分布(區(qū)別于噪聲的高斯分布，狀態(tài)變量本身也是真值附近的高斯分布)
$$\left[\begin{array}{c}a\\ b\end{array}\right]～\mathcal{N}(\mu =\left[\begin{array}{c}{\mu }_a\\ {\mu }_b\end{array}\right],\Sigma =\left[\begin{array}{cc}A& C^T\\ C& B\end{array}\right])$$
對(duì)a進(jìn)行邊緣化，將聯(lián)合概率分布分解為條件概率和邊緣概率，其中條件概率為：

這樣就去掉了a變量，得到了只含b變量的邊際分布，其中a的信息固定，以概率為1的形式保留，不再影響b，對(duì)應(yīng)b的信息矩陣進(jìn)行了舒爾補(bǔ)（原本關(guān)于待邊緣化變量條件獨(dú)立的變量由于邊緣化變量的固定而具有相關(guān)性，因子圖兩兩之間建立約束，fill-in）。

使用不進(jìn)行處理的變量表示待邊緣化變量

通過使用不被邊緣化的狀態(tài)代替待邊緣化狀態(tài)，可以求解出最小值時(shí)使用了舒爾補(bǔ)形式。

FEJ(First Estimiated Jacobian)

執(zhí)行邊緣化過程中，我們需要不斷迭代計(jì)算H矩陣和殘差b，而迭代過程中，狀態(tài)變量會(huì)被不斷更新，計(jì)算邊緣化相關(guān)的雅克比時(shí)需要注意固定線性化點(diǎn)。也就是計(jì)算雅克比時(shí)求導(dǎo)變量的值要固定，而不是用每次迭代更新以后的x去求雅克比，這就是FEJ（First Estimate Jacobians）。也被稱為系統(tǒng)的一致性問題。

解釋一：

以上證明了使用舒爾補(bǔ)進(jìn)行邊緣化可以得到無信息損失的近似優(yōu)化函數(shù)，同時(shí)，在使用一開始泰勒展開的線性化點(diǎn)計(jì)算的雅克比時(shí)（邊緣化時(shí)的使用的線性化點(diǎn)），此時(shí)$g_t$項(xiàng)為0，最大似然估計(jì)才等價(jià)于上述我們假定條件概率符合高斯分布構(gòu)造的最小二乘問題。否則就會(huì)引入了人為的偽造信息，系統(tǒng)慢慢破壞。

解釋二：系統(tǒng)能觀性

使用兩個(gè)線性化點(diǎn)不確定性的東西變得確定了，專業(yè)的術(shù)語叫不可觀的狀態(tài)變量變得可觀了，說明我們?nèi)藶榈囊肓隋e(cuò)誤的信息。
能觀性是系統(tǒng)的本身屬性，不受估計(jì)方式而改變。如VIO的不可觀維度為4。采用不同的線性化點(diǎn)，導(dǎo)致能觀性矩陣零空間維度發(fā)生改變，給系統(tǒng)引入了虛假的能觀信息，使得原本系統(tǒng)不可觀的部分可觀。導(dǎo)致優(yōu)化的系統(tǒng)與實(shí)際的系統(tǒng)不一致。
如何理解SLAM中的FEJ 知乎

解釋三：

被邊緣化的點(diǎn)固定：邊緣化的操作，把舊信息以先驗(yàn)的形式保留了下來，被邊緣化的狀態(tài)固定并且不再更新。因此其線性化點(diǎn)也就固定了以此來保證一致性。

fill-in

要marg那些不被其他幀觀測(cè)到的特征點(diǎn)
（待補(bǔ)）

總結(jié)

以上是生活随笔為你收集整理的滑窗优化、边缘化、舒尔补、FEJ及fill-in问题的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。