SLAM中的塊矩陣與schur補

文章目錄

- SLAM中的塊矩陣與schur補
- - Schur補的另一種解釋
  - 對角塊矩陣的逆為各個塊的逆的組合

SLAM中的塊矩陣與schur補

Schur補的另一種解釋

Schur從概率角度來解釋是比較常見的一種推導，可以參考博客https://blog.csdn.net/weixin_41469272/article/details/121994485，此外也可以通過消元與回代的基本原理得到相同的結論。

首先設我們需要求解以下問題：
$\begin{bmatrix}A&B\\C&D\end{bmatrix}\begin{bmatrix}x_{1}\\x_{2}\end{bmatrix}=\begin{bmatrix}v\\w\end{bmatrix}$
通常對于SLAM問題，信息矩陣中的{ $B=C^T$ }， $A ， D$ 為可逆（對稱）方陣

可以得到：
$Ax_{1}+Bx_{2}=v\\Cx_{1}+Dx_{2}=w$
設我們需要marg掉 $x_2$ ，或者先求解 $x_1$ （ $H\Delta x=b$ 問題），

我們對第一行左右兩側均乘以 $BD^{-1}$ ，繼而可以得到：
$BD^{-1}Cx_{1}+BD^{-1}Dx_{2}=BD^{-1}w \Rightarrow Bx_{2}=BD^{-1}w-BD^{-1}Cx_{1}$

帶入 $Cx_{1}+Dx_{2}=w$ 得：
$A-BD^{-1}C)x_{1}=v-BD^{-1}w$

從而可以看出，我們通過回代同樣得到了Schur補的情況。從而，我們可以知道無論是邊緣化變量，還是更新參數的 $H\Delta x=b$ 的分塊求解問題，歸根到底都可以理解為變量的消元問題。

對角塊矩陣的逆為各個塊的逆的組合

對于給定的 ( $\times 9$ ) 矩陣：

$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix}$

其中 $A_1$ , $A_2$ , 和 $A_3$ 均為可逆的 $\times 3$ 矩陣，可以通過求解塊對角矩陣的逆來找到 $B$ 的逆。

1. 塊對角矩陣的逆

對于一個塊對角矩陣：

$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix}$

其逆矩陣也是一個塊對角矩陣，其形式為：

$B^{-1} = \begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix}$

2. 證明

設 $X$ 為 $B$ 的逆矩陣，即 $BX = I$ ，其中 $I$ 是 $\times 9$ 的單位矩陣。

考慮以下矩陣乘法：

$\begin{bmatrix} A_{1} & 0 & 0 \\ 0 & A_{2} & 0 \\ 0 & 0 & A_{3} \end{bmatrix} \begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix} = \begin{bmatrix} A_{1}A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}A_{3}^{-1} \end{bmatrix} = \begin{bmatrix} I_{3} & 0 & 0 \\ 0 & I_{3} & 0 \\ 0 & 0 & I_{3} \end{bmatrix}$

這里的 $I_{3}$ 是 $\times 3$ 的單位矩陣。

可以看到，右邊的矩陣確實是 $\times 9$ 的單位矩陣 $I$ 。因此，

$\begin{bmatrix} A_{1}^{-1} & 0 & 0 \\ 0 & A_{2}^{-1} & 0 \\ 0 & 0 & A_{3}^{-1} \end{bmatrix}$

是 B$ 的逆矩陣。

因而型如以下的SLAM求解問題，可以使用schur補+對角塊矩陣逆的特性，高效求解
$\left[\begin{matrix}B&E\\E^{T}&C\end{matrix}\right]\left[\begin{matrix}\Delta x_{c}\\\Delta x_{p}\end{matrix}\right]=\left[\begin{matrix}2\\20\end{matrix}\right]$
其中， $\Delta x_{c}$ 對應傳感器位姿變量的更新量； $\Delta x_{c}$ 特征點位置對應的更新量。SLAM問題中的信息矩陣的結構對應如下圖所示，其中關于特征點的部分(該矩陣中的C塊矩陣)為對角塊矩陣。

Schur補得到：
$\begin{bmatrix}B-EC^{-1}E^T&\mathbf{0}\\E^T&C\end{bmatrix}\left[\begin{array}{c}\Delta\boldsymbol{x}_c\\\Delta\boldsymbol{x}_p\end{array}\right]=\left[\begin{array}{c}\boldsymbol{v}-\boldsymbol{E}C^{-1}\boldsymbol{w}\\\\\boldsymbol{w}\end{array}\right].$

從而可以將求逆問題簡化為對角塊矩陣求逆的問題，先得到 $\Delta\boldsymbol{x}_c$
$[B-C^{-1}E^T]\Delta\boldsymbol{x}_c=\boldsymbol{v}-EC^{-1}\boldsymbol{w}.$

而后，再帶入 $B\Delta\boldsymbol{x}_c+E\Delta\boldsymbol{x}_p=\boldsymbol{v}$ 計算得到 $\Delta\boldsymbol{x}_p$

此外，也可以利用SAM的方法參考鏈接，經過因式分解，得到上三角陣，而后使用回代的方法進行求解。

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/web/43605.shtml
繁體地址，請注明出處：http://hk.pswp.cn/web/43605.shtml
英文地址，請注明出處：http://en.pswp.cn/web/43605.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！