滑窗優化

在SLAM問題中，狀態估計問題被建模為最大后驗概率問題，在假設為高斯分布的情況下即為最小化損失函數的一個最小二乘問題，并通過泰勒展開轉化為增量迭代求解問題：
$$\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系統的運行，狀態變量規模不斷增大。使用滑窗優化是限制計算量的常用手段。

邊緣化

對于滑窗外的狀態，我們不去進行優化，但也不能直接丟掉，這樣會破壞原有的約束關系，損失約束信息。采用邊緣化的技巧，將約束信息轉化為待優化變量的先驗分布，實際上是一個從聯合分布中獲得變量子集概率分布的問題。
定義待邊緣化變量$x_m$,和該變量有約束的待優化變量$x_b$,剩余待優化變量$x_r$，$x_m$和$x_b$間的約束為$z_m$,$x_b$間的約束為$z_b$,剩余約束為$z_r$，將優化問題拆解為兩部分
$$\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$$其中第一部分進行邊緣化處理，將約束封裝為$x_b$的先驗$\mathcal{N}(\widehat{x_b},{\Lambda }_t^{?1})$,這樣優化問題轉為：
$$\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$$邊緣化采用的方式為舒爾補(schur complement)

證明

概率角度

狀態變量滿足高斯聯合分布(區別于噪聲的高斯分布，狀態變量本身也是真值附近的高斯分布)
$$\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])$$
對a進行邊緣化，將聯合概率分布分解為條件概率和邊緣概率，其中條件概率為：

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

使用不進行處理的變量表示待邊緣化變量

通過使用不被邊緣化的狀態代替待邊緣化狀態，可以求解出最小值時使用了舒爾補形式。

FEJ(First Estimiated Jacobian)

執行邊緣化過程中，我們需要不斷迭代計算H矩陣和殘差b，而迭代過程中，狀態變量會被不斷更新，計算邊緣化相關的雅克比時需要注意固定線性化點。也就是計算雅克比時求導變量的值要固定，而不是用每次迭代更新以后的x去求雅克比，這就是FEJ（First Estimate Jacobians）。也被稱為系統的一致性問題。

解釋一：

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

解釋二：系統能觀性

使用兩個線性化點不確定性的東西變得確定了，專業的術語叫不可觀的狀態變量變得可觀了，說明我們人為的引入了錯誤的信息。
能觀性是系統的本身屬性，不受估計方式而改變。如VIO的不可觀維度為4。采用不同的線性化點，導致能觀性矩陣零空間維度發生改變，給系統引入了虛假的能觀信息，使得原本系統不可觀的部分可觀。導致優化的系統與實際的系統不一致。
如何理解SLAM中的FEJ 知乎

解釋三：

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

fill-in

要marg那些不被其他幀觀測到的特征點
（待補）

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。