因子圖相關理論彙總

本文主要是學習《Factor Graphs for Robot Perception》一書是記錄的筆記，耗時較長，篇幅較大，後續再做細緻的分節以及補充相關知識。

問題表達

Measurement predictions and noise models are the core elements of a generative model, which is well matched with the Bayesian network framework.

轉換為貝葉斯網路：

聯合概率密度：

\[\begin{aligned} p(X,Y) &= p(x_1)p(x_2|x_1)p(x_3|x_2) \\ &\times p(l_1)p(l_2) \\ &\times p(z_1|x_1) \\ &\times p(z_2|x_1,l_1)p(z_3|x_2,l_1)p(z_4|x_3,l_2) \end{aligned} \]

squared Mahalanobis distance:

\[\parallel \theta - \mu \parallel ^{2}_{\Sigma} \triangleq (\theta - \mu)^\top\Sigma^{-1}(\theta - \mu) \]

多維高斯分佈：

\[\begin{aligned} \mathcal{N}(\theta;\mu,\Sigma) &= \frac {1}{\sqrt{2\pi|\Sigma|}}\operatorname{exp}(-\frac{1}{2}(\theta - \mu)^T\Sigma^{-1}(\theta - \mu)) \\ &= \frac {1}{\sqrt{2\pi|\Sigma|}}\operatorname{exp}(-\frac{1}{2}\parallel \theta - \mu \parallel ^{2}_{\Sigma}) \end{aligned} \]

a bearing measurement from a given pose x to a given landmark l would be modeled as

\[z = h(x,l)+\eta \\ \eta \sim \mathcal{N}(\theta;0,\Sigma) \]

\[p(z|x,l) = \mathcal{N}(z;h(x,l),R) \]

最大後驗概率密度
maximizes the posterior density p(X|Z) of the states X given the measurements Z

\[\begin{aligned} X^{MAP} &= \underset{x}{\text{argmax}}\: p(X|Z) \\ &= \underset{x}{\text{argmax}}\: \frac{p(Z|X)p(X)}{p(Z)} \\ &= \underset{x}{\text{argmax}}\: l(X;Z)p(X) \end{aligned} \]

\(l(X; Z)\) is the likelihood of the states X given the measurements Z.

\[l(X;Z) \propto p(Z|X) \]

通過條件化感測器資料，貝葉斯網路可以很容易得轉換為因子圖。

使用因子圖的原因：

• the distinct division between states X and measurements Z
• the fact that we are more interested in the non-Gaussian likelihood functions, which are not proper probability densities.

因此：

\[\begin{aligned} p(X|Z) &\propto p(x_1)p(x_2|x_1)p(x_3|x_2) \\ &\times p(l_1)p(l_2) \\ &\times l(x_1;z_1) \\ &\times l(x_1,l_1;z_1)l(x_2,l_1;z_3)l(x_3,l_2:z_4) \end{aligned} \]

與貝葉斯網路不同測量值不在明確的顯示在圖中， 也不再將每個節點與相應的條件概率密度關聯起來，而是使用因子（factors）來表示後驗概率密度\(p(X|Z)\)。上圖中，每個小黑點都是一個因子，只與其所連線的變數有關，例如似然（likelihood）因子\(l(x_3,l_2;z_4)\)只與變數節點\(x_3,l_2\)有關。

每一個因子圖都是一個二分圖：\(F=(\mathcal{U}, \mathcal{V}, \mathcal{\varepsilon})\)，有兩種型別節點 factors \(\phi_i \in \mathcal{U}\)， variables \(x_j \in \mathcal{V}\).edges \(e_{ij} \in \mathcal{\varepsilon}\)
在factor與variable之間。The set of variable nodes \(X_i\) adjacent to a factor \(\phi(i)\) is written as \(\mathcal{N}(\phi(i))\).a factor graph \(\mathcal{F}\) defines the factorization of a global function \(\phi(X)\) as:

\[\phi(X) = \prod_{i}\phi_{i}(X_i) \]

每個因子代表未歸一化的後驗概率\(\phi(X) \propto P(X|Z)\)。

factor graph factorization:

\[\begin{aligned} \phi(l_1,l_2,x_1,x_2,x_3) &= \phi_1(x_1)\phi_2(x_2,x_1)\phi_3(x_3,x_2) \\ &\times \phi_4(l_1)\phi_5(l_2) \\ &\times \phi_6(x_1) \\ &\times \phi_7(x_1,l_1)\phi_8(x_2,l_1)\phi_9(x_3,l_2) \end{aligned} \]

應該注意到因子與原先概率密度的關係，比如\(\phi_7(x_1,l_1)=l(x_1,l_1;z_1) \propto p(z_1|x_1,l_1)\)。

對於任意的因子圖，MAP問題轉換成了最大化因子乘積：

\[\begin{aligned} X^{MAP} &= \underset{X}{\text{argmax}} \: \phi(X) \\ &= \underset{X}{\text{argmax}} \: \prod_i \phi_i(X_i) \end{aligned} \]

假設所有的因子服從零均值高斯分佈(zero-mean normal distribution)，則

\[\phi_i(X_i) \propto exp(-\frac{1}{2}\parallel h_i(X_i) - z_i \parallel ^{2}_{\Sigma_i}) \]

取負對數，並去除係數\(1/2\)轉為非線性最小二乘問題

\[X^{MAP} = \underset{X}{\text{argmin}} \: \Sigma_i\parallel h_i(X_i) - z_i \parallel ^{2}_{\Sigma_i} \]

線性化求解非線性問題：

令\(h(X_i)=h_i(X^0_i + \Delta_i) \approx h_i(X^0_i) + H_i\Delta_i\)

其中\(H_i\)為測量雅可比矩陣

\[H_i = \left.\begin{matrix}\frac{\partial h(X_i)}{\partial X_i} \end{matrix}\right|_{X^0_i} \\ \Delta_i \triangleq X_i - X^0_i \]

\[\parallel e \parallel ^{2}_{\Sigma} \triangleq e^\top\Sigma^{-1}e = (\Sigma^{-1/2}e)^\top(\Sigma^{-1/2}e) = \parallel \Sigma^{-1/2}e \parallel^2_2 \]

上面的過程也叫作白化，誤差除以測量標準差，消除了量綱，這樣可以將不同型別的誤差放在一起處理。
代替最小二乘問題：

\[\begin{aligned} \Delta^* &= \underset{\Delta}{\text{argmin}} \: \sum_i\parallel H_i\Delta_i - \{ z_i - h_i(X^0_i) \} \parallel ^{2}_{\Sigma_i} \\ &= \underset{\Delta}{\text{argmin}} \: \sum_i\parallel \underbrace{A_i}_{\Sigma_i^{-1/2}H_i}\Delta_i - \underbrace{b_i}_{\Sigma_i^{-1/2}(z_i - h_i(X^0_i))} \parallel ^{2}_{2} \\ &= \underset{\Delta}{\text{argmin}} \: \parallel A\Delta - b \parallel \end{aligned} \]

假設矩陣\(A_{m \times n}\)列滿秩，通常情況下\(m \ge n\)，那麼最小二乘問題有唯一解：

\[A^{\top}A \Delta = A^{\top} \mathbf{b} \]

此時\(A^{\top}A\)正定。

cholesky法求解

其中\(A^{\top}A\)是實對稱正定矩陣，Cholesky factorization後資訊矩陣 \(\Lambda \triangleq A^{\top}A = R^{\top}R\)，其中\(R\)是上三角矩陣。

\[A^{\top}A \Delta = A^{\top} \mathbf{b} \rightarrow \\ R^{\top}R \Delta = A^{\top} \mathbf{b} \rightarrow \\ R^{\top} \mathbf{y} = A^{\top} \mathbf{b} \]

先求解\(\mathbf{y}\)再求解\(\Delta\)

QR法求解

\[H_{n} \ldots H_{2} H_{1} A = Q_{m \times m}^{\top} A_{m \times n} = \left[\begin{array}{l} R_{n \times n} \\ 0_{(m -n) \times n} \end{array}\right] \]

QR Decomposition with Householder Reflections

與Gram-Schmidt正交化操作相比，Householder reflections在QR分解中更常用。Householder reflections是另外一種正交變換(orthogonal transformation)，它可以將一個向量\(\mathbf{x}\)變換到一個與之平行的向量\(\mathbf{y}\)。

\[H = I - 2vv^T \]

\[\|A \Delta - b \|_{2}^{2} = \left \| Q^{\top} A \Delta - Q^{\top} b \right \|_{2}^{2} = \|R \Delta - d \|_{2}^{2} + \| e \|_{2}^{2} \]

其中

\[Q^{\top} b = \left[\begin{array}{l} d \\ e \end{array}\right] \]

\(\|e\|^{2}\)是QR分解的殘差，與\(\Delta\)無關。

\(\Lambda\)被稱作資訊矩陣可以和另一種無向圖模型（undirected graphical model）馬科夫隨機場聯絡起來。\(\Lambda\)是\(G\)的伴隨矩陣。後續不採用MRF而是使用因子圖，是因為因子圖可以表達更細粒度的因子化，更貼近於原始問題方程。

Elimination Algorithm

邊緣化marginalization
one solution is to remove older variables without removing information

消除演算法可以將因子圖轉換回貝葉斯網路(有向無環圖)，但是隻關注未知變數\(X\)。對於任意形式為\(\phi(X) = \phi(x_1,x_2, \dots, x_n)\)的因子圖，變數消除演算法可以將這樣的因子圖轉換為因子化的貝葉斯網路，概率密度函式的形式為：

\[p(X) = p(x_1|S_1)p(x_2|S_2) \dots p(x_n) \]

稀疏高斯因子

\[\begin{aligned} \phi(l_1,l_2,x_1,x_2,x_3) &= \phi_1(x_1)\phi_2(x_2,x_1)\phi_3(x_3,x_2) \\ &\times \phi_4(l_1)\phi_5(l_2) \\ &\times \phi_6(x_1) \\ &\times \phi_7(x_1,l_1)\phi_8(x_2,l_1)\phi_9(x_3,l_2) \end{aligned} \]

\[\phi_i(X_i) \propto \operatorname{exp} \left\{ -\frac{1}{2} \left\| A_iX_i - b_i \right\|^2_2 \right\} \]

\(X_i\)是與因子\(\phi_i\)相關的變數，例如\(\phi_7\)，\(X_7 = \{ x_1;l_1\}\)，分號在這裡表示列方向級聯。

\[\phi_7(X_7) = \operatorname{exp} \left\{ -\frac{1}{2} \left\| A_{71}l_1 + A_{73}x_1 - b_7 \right\|^2_2 \right\} \]

令\(A_7 = [A_{71}|A_{73}]\)，則

\[\phi_7(X_7) = \operatorname{exp} \left\{ -\frac{1}{2} \left\| A_7X_7 - b_7 \right\|^2_2 \right\} \]

\[\begin{aligned} \psi(x_j,S_j) &= \prod_i \phi_i(X_i) \\ &= \operatorname{exp} \left\{ -\frac{1}{2} \sum_i \left\| A_{i}X_i - b_i \right\|^2_2 \right\} \\ &= \operatorname{exp} \left\{ -\frac{1}{2} \left\| \bar{A}_{j}[x_j;S_j] - \bar{b}_j \right\|^2_2 \right\} \end{aligned} \]

這裡以消除\(l_1\)\(為例，與其相鄰的因子有\)\phi_4,\phi_7,\phi_8$，因此

\[\begin{aligned} \psi(l_1,\{x_1;x_2\}) &= \operatorname{exp} \left\{ -\frac{1}{2} \left\| \bar{A}_{1}[l_1;x_1;x_2] - \bar{b}_1 \right\|^2_2 \right\} \end{aligned} \]

其中：

\[\bar{A}_{1} = \begin{bmatrix} A_{41} & & \\ A_{71} & A_{73} & \\ A_{81} & & A_{84} \end{bmatrix}, \bar{b}_1 = \begin{bmatrix} b_4 \\ b_7 \\ b_8 \end{bmatrix} \]

這一步只是將多個求和的因子整合到一個大矩陣中。

使用Partial QR消除變數

使用partial QR-factorization將\(\psi(x_j,S_j)\)中提取的增廣矩陣\([\bar{A}_{j}|\bar{b}_{j}]\)變換為：

\[[\bar{A}_{j}|\bar{b}_{j}] = Q\begin{bmatrix} R_j & T_j & d_j \\ &\tilde{A}_\tau & \tilde{b}_\tau \end{bmatrix} \]

其中\(Q\)為單位正交陣，\(R_j\)為上三角矩陣。

\[\psi(x_j,S_j) = \operatorname{exp} \left\{ -\frac{1}{2} \left\| \bar{A}_{j}[x_j;S_j] - \bar{b}_j \right\|^2_2 \right\} \]

\[\begin{aligned} \left\| \bar{A}_{j}[x_j;S_j] - \bar{b}_j \right\|^2_2 &= \left\| Q(\begin{bmatrix} R_j & T_j \\ &\tilde{A}_\tau \end{bmatrix}\begin{bmatrix}x_j \\ S_j \end{bmatrix} - \begin{bmatrix}d_j \\ \tilde{b}_\tau \end{bmatrix}) \right\|^2_2 \\ &= \left\| R_jx_j + T_jS_j - d_j \right\|^2_2 + \left\| \tilde{A}_\tau S_j - \tilde{b}_\tau \right\|^2_2 \end{aligned} \]

因此

\[\begin{aligned} \psi(x_j,S_j) &= \operatorname{exp} \left\{ -\frac{1}{2} \left\| R_jx_j + T_jS_j - d_j \right\|^2_2 \right\}\operatorname{exp} \left\{ -\frac{1}{2} \left\| \tilde{A}_\tau S_j - \tilde{b}_\tau \right\|^2_2 \right\} \\ &= p(x_j|S_j)p(S_j) \end{aligned} \]

比如消除變數\(l_1\)，此時

\[\psi(l_1,\{x_1, x_2\}) = \operatorname{exp} \left\{ -\frac{1}{2} \left\| R_1 l_1 + [T_{13}|T_{14}][x_1;x_2] - d_1 \right\|^2_2 \right\}\operatorname{exp} \left\{ -\frac{1}{2} \left\| [\tilde{A}_{13}|\tilde{A}_{14}] [x_1;x_2] - \tilde{b}_1 \right\|^2_2 \right\} \]

\[\left[{\begin{array}{c|c} \begin{matrix} & &A_{13} & & \\ & &A_{23} &A_{24} & \\ & & &A_{34} &A_{35} \\ A_{41} \\ &A_{52} \\ & &A_{63} \\ A_{71} & &A_{73} \\ A_{81} & & &A_{84} \\ &A_{92} & & &A_{95} \end{matrix} & \begin{matrix} b_1 \\ b_2 \\ b_3 \\ b_4 \\ b_5 \\ b_5 \\ b_6 \\b_7 \\ b_8 \\ b_9 \end{matrix} \end{array}} \right] \rightarrow \left[ {\begin{array}{c|cccc|c} R_1 & &T_{13} &T_{14} & &d_1\\ \hline & &A_{13} & & &b_1\\ & &A_{23} &A_{24} & &b_2\\ & & &A_{34} &A_{35} &b_3\\ &A_{52} & & & &b_5 \\ & &A_{63} & & &b_6 \\ &A_{92} & & &A_{95} &b_9 \\ \hline & &\tilde{A}_{13} &\tilde{A}_{14} & &\tilde{b}_1 \end{array}} \right] \]

後續消除後的結果：

a Bayes net is a directed acyclic graph (DAG), and that is exactly the “uppertriangular” property for matrices.
貝葉斯網路是一個有向無環圖，相關的矩陣具有可轉為上三角矩陣的特性。

轉換後的貝葉斯網路很明顯是個上三角矩陣，使用倒推法即可倒序求解問題。

Elimination Ordering

消除的順序對於效能有很大的影響。假設消除所有變數的開銷為：

\[f\left(\Phi_{1: n}\right) = \sum_{j=1}^{n} g\left(\Phi_{j: n}, x_{j}\right) \]

其中\(g\left(\Phi_{j: n}, x_{j}\right)\)為從剩下的圖結構\(\Phi_{j: n}\)中消除變數\(x_j\)的計算開銷。而每一步變數消除的開銷主要在partial QR factorization上面。對於一個\(m_k \times n_k\)的矩陣來說，QR分解需要\(4m_kn_k\)次浮點運算，那麼總開銷可以近似為：

\[g\left(\Phi_{j: n}, x_{j}\right) \approx \sum_{k=1}^{n_{j}} 4 m_{k} n_{k}=\sum_{k=1}^{n_{j}} 4\left(m_{j}-k\right)\left(n_{j}+s_{j}+1-k\right) \]

其中\(n_j\)為待消除變數\(x_j\)的維數，\(s_j\)為\(S_j\)中變數的個數，\(m_j\)為增廣矩陣\([\bar{A}_{j}|\bar{b}_{j}]\)的行數。對於一個\(m \times n\)稠密矩陣矩陣做QR分解所需算力為：

\(f\left(\Phi_{1: n}\right)=\sum_{k=1}^{n-1} 4(m-k)(n+1-k)=2(m-n / 3) n^{2}+O(m n)\)

這裡假設路標\(l\)為3維，位姿為\(x,y,\theta\)三自由度。則第一步消除(假設稠密矩陣)：4(39)(35+1) = 1728， 第二步消除：4(38)(34+1) = 1248。這裡的計算和書中的有出入。\(f\left(\Phi_{1: 5}\right)=1752+1304+1256+608+152=5072 \text { flops}\)
但實際上對於稀疏矩陣來說：\(f\left(\Phi_{1: 5}\right)=32+20+488+488+128=1156 \text { flops }\)。可以看到稀疏矩陣比稠密矩陣的所需的計算少了很多。

變數消除順序

變數消除順序對於計算量的影響很大，不同的消除順序會將因子圖轉為不同結構的貝葉斯網路，但是描述的是等價的MAP估計。

COLAMD and METIS are best overall performers.The Schur complement trick is an often-used technique in computer vision.

Updating a Matrix Factorization

\[\begin{aligned} \Delta^* &= \underset{\Delta}{\text{argmin}} \: \sum_i\parallel H_i\Delta_i - \{ z_i - h_i(X^0_i) \} \parallel ^{2}_{\Sigma_i} \\ &= \underset{\Delta}{\text{argmin}} \: \sum_i\parallel \underbrace{A_i}_{\Sigma_i^{-1/2}H_i}\Delta_i - \underbrace{b_i}_{\Sigma_i^{-1/2}(z_i - h_i(X^0_i))} \parallel ^{2}_{2} \\ &= \underset{\Delta}{\text{argmin}} \: \parallel A\Delta - b \parallel \end{aligned} \]

而

\[\|A \Delta-b\|_{2}^{2}=\left\|Q^{\top} A \Delta-Q^{\top} b\right\|_{2}^{2}=\|R \Delta-d\|_{2}^{2} + \|e\|_{2}^{2} \\ Q^{\top} b = \left[\begin{array}{l} d \\ e \end{array}\right] \]

當有新的測量值時，假設所帶來的影響為\(A' =\left[\begin{array}{l} A \\ a^\top \end{array}\right]，b' = \left[\begin{array}{l} b \\ \beta \end{array}\right]\)。構造新的正交矩陣\(Q'\)為

\[Q' = \left[\begin{array}{l} Q_{m \times m} & 0_{m \times 1} \\ 0_{1 \times m} & 1_{1 \times 1} \end{array}\right] \]

此時

\[\left\| A' \Delta - b' \right\|_{2}^{2} = \left\| Q'^{\top} A' \Delta - Q'^{\top} b' \right\|_{2}^{2} \\ \left[\begin{array}{l} Q^{\top}_{m \times m} & 0_{m \times 1} \\ 0_{1 \times m} & 1_{1 \times 1} \end{array}\right] \left[\begin{array}{l} A_{m \times n} \\ a^\top_{1 \times n} \end{array}\right] = \left[\begin{array}{l} R_{n \times n} \\ 0_{(m -n) \times n} \\ a^\top_{1 \times n} \end{array}\right] = R_a, \quad \left[\begin{array}{l} Q^{\top}_{m \times m} & 0_{m \times 1} \\ 0_{1 \times m} & 1_{1 \times 1} \end{array}\right] \left[\begin{array}{l} b_{m \times 1} \\ \beta_{1 \times 1} \end{array}\right] = \left[\begin{array}{l} d_{n \times 1} \\ e_{(m-n) \times 1} \\\beta_{1 \times 1} \end{array}\right] \]

實際上還有：

\[\left\| A' \Delta - b' \right\|_{2}^{2} = \left\| \left[\begin{array}{l} A \\ a^\top \end{array}\right]\Delta - \left[\begin{array}{l} b \\ \beta \end{array}\right] \right\|_{2}^{2} = \left\| A\Delta - b \right\|_{2}^{2} + \left\| a^{\top}\Delta - \beta \right\|_{2}^{2} \]

我們希望\(R_a\)滿足這樣的形式:

\[R_a = \left[\begin{array}{l} R' \\ 0 \end{array}\right] \]

使用旋轉的方式來更新矩陣分解在數值上更穩定也更精確，給定的旋轉為：

\[G = \left[\begin{array}{l} \text{cos}\phi & \text{sin}\phi \\ -\text{sin}\phi & \text{cos}\phi \end{array}\right] \]

挑選引數\(\phi\)可以使\(A\)中的第\((i,j)^{th}\)個元素為0。

上圖中最左邊的矩陣為\(G\)實際的形式，可以將R中的x位置元素變為0。通過一系列的旋轉操作將新新增的行變0，這樣以來：

\[G_{j_k} \dots G_{j_2}G_{j_1}R_a = \left[\begin{array}{l} R_{n \times n}' \\ 0_{(m - n + 1) \times n} \end{array}\right] \]

顯然這些\(G\)都是正交矩陣，因此

\[\begin{aligned} \|A' \Delta - b' \|_{2}^{2} &= \left\| Q'^{\top} A' \Delta - Q'^{\top} b' \right\|_{2}^{2} \\ &= \left\| G_{j_k} \dots G_{j_2}G_{j_1}[Q'^{\top} A' \Delta - Q'^{\top} b'] \right\|_{2}^{2} \\ &= \left\| \left[\begin{array}{l} R_{n \times n}' \\ 0_{(m - n + 1) \times n} \end{array}\right] \Delta -\left[\begin{array}{l} d_{n \times 1}' \\ e_{(m - n + 1) \times 1} \end{array}\right] \right\|_{2}^{2} \\ &= \|R_{n \times n}' \Delta - d' \|_{2}^{2} + \| e_{(m - n + 1) \times 1} \|_{2}^{2} \end{aligned} \]

The Bayes tree

clique: 一個無向圖中，滿足兩兩之間有邊連線的頂點的集合，被稱為該無向圖的團。
chord(弦): 環中的一條連線兩個非鄰頂點的邊；
chordal(弦圖): 任何長度超過3的環有一個chord；

在求解非線性問題的時候，矩陣分解的方法沒法很好的處理，因此我們引入新的圖模型Bayes tree。在推理方面，樹結構的圖效率很高。構建樹結構圖模型的步驟：

1. 使用變數消除將因子圖轉換為貝葉斯網路；
2. 在貝葉斯網路的cliques中找出一個樹結構；

為了檢測貝葉斯網路中的團結構(clique)，可能需要把貝葉斯網路重寫為貝葉斯樹。雖然不能明顯的看出貝葉斯網路中的團可以形成一個樹結構，但是由於弦的屬性，事實確實是這樣。羅列出無向樹中所有的團就是一個團樹（clique tree）。

貝葉斯樹是一棵有向樹，節點代表潛在的弦圖貝葉斯網路中的團\(C_k\)。定義每個節點的條件概率密度為\(p(F_k|S_k)\)，分隔因子\(S_k\)是團\(C_k\)與父團\(\prod_k\)的交集。

貝葉斯樹更新

增量推導只需要對貝葉斯樹做簡單的更新。相比於抽象的增量矩陣分解過程，增量更新貝葉斯樹可以提供一種更好的解釋和理解方式。當一個新的測量值帶來一個因子，比如該測量值與有兩個變數有關，將會引入一個新的二元因子\(f(x_j,x_{j'})\)，只有包含\(x_j,x_{j'}\)的團之間的邊和root節點受影響，這些團下面的子樹和其他不包含\(x_j,x_{j'}\)的子樹並不會受影響。因此，貝葉斯樹更新的過程就是，將樹中受影響的部分轉換回因子圖，然後將新測量相關的新因子新增進去。使用任何便捷的消除方法對這個臨時的因子圖做消除操作，形成新的貝葉斯樹，未受影響的子樹再新增上去。

貝葉斯樹的兩個重要屬性保證了只有樹的頂部部分會被影響。貝葉樹形成於弦圖貝葉斯網路的逆向消除順序，因此團中的變數是從他們的子團消除過程中收集到資訊，這就導致任何團中資訊只向其根節點方向傳播。其次，只有當與因子相關的第一個變數被消除時，該因子的資訊才進入消除狀態。

流型上的優化

增量旋轉

使用軸角\((\bar{\omega},\theta)\)來表示三維旋轉更加符合向量值增量的表示形式，\(\bar{\omega} \in S^2\)是球面上的單位向量，表示旋轉操作的旋轉軸，\(\theta\)是繞該向量旋轉的角度。\(\xi = \bar{\omega}\theta \in \mathbb{R}^3\)，對於一個很小的\(\theta\)有

\[R(\xi) \approx \left[ \begin{array}{} 1 & -\xi_{z} & \xi_{y} \\ \xi_{z} & 1 & -\xi_{x} \\ -\xi_{y} & \xi_{x} & 1 \end{array} \right] = I + \hat{\xi} \\ \hat{\xi} \triangleq \left[\begin{array}{ccc} 0 & -\xi_{z} & \xi_{y} \\ \xi_{z} & 0 & -\xi_{x} \\ -\xi_{y} & \xi_{x} & 0 \end{array}\right] \]

其中\(\hat{\xi}\)是\(\xi\)的斜對稱矩陣。

指數對映

相比於上面的近似，指數對映可以將三維增量\(\xi\)對映為精確的旋轉。

\[\exp \hat{\xi} \triangleq \sum_{k=0}^{\infty} \frac{1}{k !} \hat{\xi}^{k}=I+\hat{\xi}+\frac{\hat{\xi}^{2}}{2 !}+\frac{\hat{\xi}^{3}}{3 !}+\ldots \]

實際上指數對映對於任意大的向量\(\xi\)都有效，這就羅格里德斯公式：

\[\exp \hat{\xi} = I + \frac{\sin \theta}{\theta}\hat{\xi} + \frac{1 - \cos \theta}{\theta ^2}\hat{\xi}^2 \]

區域性座標

指數對映可以將一個區域性座標(Local Coordinates)向量\(\xi \in \mathbb{R}^3\)對映到待評估旋轉值\(R_0 \in SO(3)\)的附近。

\[R_{0} \oplus \xi \triangleq R_{0} \cdot \exp \hat{\xi} \]

指數化區域性座標\(\xi\)將會帶來以\(R_0\)為基礎的增量旋轉。

平面旋轉

對於平面旋轉來說，\(\xi \in \mathbb{R}\)，

\[\hat{\xi} \triangleq\left[\begin{array}{cc} 0 & -\xi \\ \xi & 0 \end{array}\right] \]

\[\exp \hat{\xi} \triangleq \sum_{k=0}^{\infty} \frac{1}{k !} \hat{\xi}^{k} = \left[\begin{array}{cc} 1-\xi^{2} / 2 \ldots & -\xi+\xi^{3} / 6 \ldots \\ \xi-\xi^{3} / 6 \ldots & 1-\xi^{2} / 2 \ldots \end{array}\right]=\left[\begin{array}{cc} \cos \xi & -\sin \xi \\ \sin \xi & \cos \xi \end{array}\right] \]

\[\begin{aligned} R_{0} \oplus \xi & \triangleq R_{0} \cdot \exp \hat{\xi} \\ &=\left[\begin{array}{cc} \cos \theta_{0} & -\sin \theta_{0} \\ \sin \theta_{0} & \cos \theta_{0} \end{array}\right]\left[\begin{array}{cc} \cos \xi & -\sin \xi \\ \sin \xi & \cos \xi \end{array}\right] \\ &=\left[\begin{array}{cc} \cos \left(\theta_{0}+\xi\right) & -\sin \left(\theta_{0}+\xi\right) \\ \sin \left(\theta_{0}+\xi\right) & \cos \left(\theta_{0}+\xi\right) \end{array}\right] \end{aligned} \]

區域性位置座標

假設在有限的時間\(\Delta \tau\)內角速度為\(\omega\)，平移速度為\(v\)，區域性位置座標\(\xi\)為：

\[\xi = \left[ \begin{array}{c} \omega \\ v \end{array} \right] \Delta \tau \]

hat操作符定義為：

\[\hat{ }:\left[\begin{array}{l} \omega \\ v \end{array}\right] \longmapsto \left[\begin{array}{ll} \hat{\omega} & v \\ 0 & 0 \end{array}\right] \]

\(SE(2)\)的hat操作：

\[\hat{} : \mathbb{R}^{3} \rightarrow \mathfrak{s e}(2): \xi \mapsto \left[\begin{array}{cc|c} 0 & -\omega_{z} & v_{x} \\ \omega_{z} & 0 & v_{y} \\ \hline 0 & 0 & 0 \end{array}\right] \Delta \tau \]

\(SE(3)\)的hat操作：

\[\hat{}: \mathbb{R}^{6} \rightarrow \mathfrak{s e}(3): \xi \mapsto\left[\begin{array}{ccc|c} 0 & -\omega_{z} & \omega_{y} & v_{x} \\ \omega_{z} & 0 & -\omega_{x} & v_{y} \\ -\omega_{y} & \omega_{x} & 0 & v_{z} \\ \hline 0 & 0 & 0 & 0 \end{array}\right] \Delta \tau \]

我們可以使用指數對映將一個區域性位置座標對映到初始評估位姿\(x_0\)附近：

\[x_0 \oplus \xi = x_0 \cdot \exp \hat{\xi} \]

矩陣李群

hat操作符將一個向量\(\xi\)對映到李代數\(\mathfrak{g}\)：

\[\hat{}:\mathbb{R}^n \rightarrow \mathfrak{g},\xi \mapsto \hat{\xi} \]

逆操作vee操作符：

\[{}^\vee: \mathfrak{g} \rightarrow \mathbb{R}^n, \hat{\xi} \mapsto \xi \]

指數對映可以將區域性座標\(\xi\)對映到任何初始評估點\(a \in G\)的附近：

\[a \oplus \xi \triangleq a \cdot \exp \hat{\xi} \]

General Manifolds and Retractions

不滿足群結構的流型\(\mathcal{M}\)上也可以做上述的對映操作：

retraction \(\mathcal{R}_a: \mathcal{M} \times \mathbb{R}^n \rightarrow \mathcal{M}\)

\[a \oplus \xi \triangleq \mathcal{R}_a(\xi), a \in \mathcal{M} \]

retraction定義在一般流型上，hat操作是定義在群上。