? ? ? ? VINS中的滑窗優化策略，將滑出窗外的幀與滑窗內的幀的約束使用邊緣化的形式保存為先驗誤差進行后續非線性優化，以保留約束信息。本文對具體的方案進行記錄。
? ? ? ? 緊耦合VIO處理的兩種誤差分別為IMU誤差與圖像誤差，采用LM、GN、Dogleg等方式迭代求解待優化參數增量，以求得最大似然估計，最小化非線性化誤差。
? ? ? ? 迭代優化的核心即求解增量方程：
$$H\delta x=b$$? ? ? ? 其中$H=J^{T}J,b=J^{T}e,e=(J^T{)}^{+}b$.此處的$({)}^{+}$為偽逆，$e$為殘差，$J$為殘差對于狀態的Jacobian。
? ? ? ? 由于引入滑窗后，對于滑到窗口之外的幀不再優化其參數，但其與滑窗內的數據仍然存在約束，直接丟掉這些窗外幀會造成約束信息丟失，所以要將其封裝成先驗信息，加入到大的非線性優化問題中作為一部分誤差，這一步即為邊緣化。假設要邊緣化的狀態為$x_2$，要保留的狀態為$x_1$，對于上述增量方程，變為：$$\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]$$? ? ? ? 邊緣化的方法為舒爾補：$$\begin{gathered} \left[\begin{array}{cc}I& ?H_{12}{H}_{22}^{?1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{cc}I& ?H_{12}{H}_{22}^{?1}\\ 0& I\end{array}\right]\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right] \\ \left[\begin{array}{cc}{H}_{11}?H_{12}{H}_{22}^{?1}{H}_{21}& 0\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1?H_{12}{H}_{22}^{?1}{b}_2\\ b_2\end{array}\right] \end{gathered}$$? ? ? ? 這樣只需要求解第一個方程，即$$(H_{11}?H_{12}{H}_{22}^{?1}{H}_{21})\delta x_1=b_1?H_{12}{H}_{22}^{?1}{b}_2,$$? ? ? ? 這樣既沒有損失約束信息，又不需要求解$\delta x_2$,把$H_{11}?H_{12}{H}_{22}^{?1}{H}_{21}$記為$H_0^{?}$，$b_1?H_{12}{H}_{22}^{?1}{b}_2$記為$b_0^{?}$。
? ? ? ? 上式為邊緣化后待求解的增量方程，我們要根據這個增量方程恢復出邊緣化封裝的先驗誤差的具體形式$e_p$
? ? ? ? 隨著迭代的過程，狀態量被不斷更新。但在邊緣化時，被邊緣化的狀態值固定，舒爾補時使用的雅克比即為當時泰勒展開使用該固定的值(x線性化點)求得的雅克比，都需要固定。在VINS中，線性化點處的參數值x0被保存為keep_block_data。此即為EFJ，更多解釋詳看滑窗優化、邊緣化、舒爾補、FEJ及fill-in問題。但在上述方程中，由于$b=J^{T}e$，其中的$e$隨著狀態的更新而變化，因此$b$也需隨之變化。
? ? ? ? 狀態的更新（此處$\delta x$表示當前參數值相對$x_0$的變化）：$$x^{\prime }=Exp(\delta x)?x_0$$? ? ? ? 此時b的更新可以表示為$$\begin{gathered} b=b_0+\frac{?b}{?x}{|}_{x_0}\delta x \\ =b_0+J^T\frac{?e}{?x}{|}_{x_0}\delta x \\ =b_0+J^{T}J\delta x=b_0+H\delta x \end{gathered}$$? ? ? ? 有$$\begin{gathered} \left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]=\left[\begin{array}{c}{b}_{1,0}\\ b_{2,0}\end{array}\right]+\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right] \\ {b}_1=b_{1,0}+H_{11}\delta x_1+H_{12}\delta x_2 \\ {b}_2=b_{2,0}+H_{21}\delta x_1+H_{22}\delta x_2 \end{gathered}$$? ? ? ? 聯立$$b^{?}=b_1?H_{12}{H}_{22}^{?1}{b}_2$$? ? ? ? 有$$\begin{gathered} b^{?}=b_1?H_{12}{H}_{22}^{?1}{b}_2 \\ =(b_{1,0}+H_{11}\delta x_1+H_{12}\delta x_2)?H_{12}{H}_{22}^{?1}(b_{2,0}+H_{21}\delta x_1+H_{22}\delta x_2) \\ =b_{1,0}?H_{12}{H}_{22}^{?1}{b}_{2,0}+(H_{11}?H_{12}{H}_{22}^{?1}{H}_{21})\delta x_1 \\ =b_0^{?}+H_0^{?}\delta x_1 \end{gathered}$$? ? ? ? 由上述推導，原增量方程變為$$\begin{gathered} H_0^{?}\delta x=J_l^T{J}_l\delta x=b^{?}=b_0^{?}+H_0^{?}dx \\ =b_0^{?}+J_l^T{J}_{l}dx=J_l^T(J_l^T{)}^{+}{b}_0^{?}+J_l^T{J}_{l}dx=J_l^T((J_l^T{)}^{+}{b}_0^{?}+J_{l}dx) \end{gathered}$$? ? ? ? 其中$J_l$為$H_0^{?}$分解出的等價雅克比，$\delta x$為每次參數更新增量，$dx$為當前參數值與線性化點$x_0$的差值。
? ? ? ? 因此，邊緣化后等價的先驗誤差$$\begin{gathered} e_p=(J_l^T{)}^{+}{b}^{?} \\ =(J_l^T{)}^{+}{b}_0^{?}+J_{l}dx \end{gathered}$$至此，邊緣化的步驟可以總結為：

舒爾補求解$H_0^{?}=H_{11}?H_{12}{H}_{22}^{?1}{H}_{21},b_0^{?}=b_{1,0}?H_{12}{H}_{22}^{?1}{b}_{2,0}$

分解$H_0^{?}$求解$J_l、J_l^T、(J_l^T{)}^{+}$，求解$e_0=(J_l^T{)}^{+}{b}_0^{?}$，保存$J_l,e_0$

求解$\delta x$由$H_0^{?}\delta x=b_0^{?}$（ceres完成）

使用$\delta x$更新$x$,計算當前狀態$x$與$x_0$的差值$dx$,更新$e$:$e_p=e_0+J_{l}dx$（costfunction定義residuals，
同時定義costfunction需要的jacobians，把$J_l$由localsize轉為globalsize）
ceres自動更新$b^{?}$:$b^{?}=J_l^T{e}_p$:

求解$\delta x$由$H_0^{?}\delta x=b^{?}$（ceres完成）

循環4,5

附錄：

? ? ? ? 使用H分解J的方式如下：
$$H=US{U}^T=J^{T}J$$? ? ? ? 對H進行奇異值分解，由于H為半正定實矩陣，S也即為特征值矩陣，這樣$$\begin{gathered} J=S^{\frac{1}{2}}{U}^T \\ (J^T{)}^{+}=S^{?\frac{1}{2}}{U}^T \end{gathered}$$

總結

以上是生活随笔為你收集整理的VINS-MONO边缘化策略的全部內容，希望文章能夠幫你解決所遇到的問題。

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