視覺slam十四講CH4 ---李群與李代數求導

視覺slam十四講 ---CH4 李群與李代數求導

李群與李代數相較於CH3是比較的抽象的數學知識，這個工具的提出目的是解決一些旋轉位姿描述的最佳化問題。本講最終的目的是解決如何描述對旋轉求導的問題。

1. 什麼是群

群是一種集合加上一種運算的代數結構

記集合為A，運算為\(\cdot\quad\)，則若此結構滿足以下幾條性質，構成一個群。

第三講中的旋轉矩陣R以及變換矩陣T加上乘法運算就構成一種群。

特殊正交群

\[SO(3) = \{R\in R^{n\times n}|RR^T = I, det(R) = 1\}\\ \]

特殊歐式群

\[SE(3) = \{ T = \begin{bmatrix} R & t \\ 0^T & 1\\ \end{bmatrix} \in R^{4\times 4} | R \in SO(3),t\in R^3 \} \]

對於SO(3)以及SE(3),它們對於乘法是封閉的，也就是說對於多個旋轉矩陣或者變換矩陣相乘，結果仍為旋轉矩陣或者變換矩陣。這一點在上一章描述多次旋轉時可以看作是多個旋轉矩陣相乘是相同的。

本章的章節名是李群與李代數。那麼什麼是李群呢？

李群是具有連續(光滑)性質的群。類似於二維函式的連續。數學上已經證明SO(3)以及SE(3)都是李群。

在涉及一些最佳化問題時，我們需要對於旋轉這一位姿變換進行求導操作來找出一些類似於"旋轉的變化率"的概念幫助我們去最佳化。(如梯度下降)。但是對於SO(3)以及SE(3),僅滿足乘法的封閉性，並不滿足加法的封閉性，也就是並不處於線性空間中。如下導數的定義，顯然想要求導且導數有意義的話，該代數結構應該線上性空間中才可以實現。

2. 李代數

對於R或者T來說，並不能直接對其求導，那麼能否找到一種對映關係將其對映到線性空間中呢？

給出如下推導,假設旋轉是隨時間變化的函式。

\[\forall R(t)\in SO(3),R(t)R(t)^T = I \\ 兩邊求導(抽象的求導)得\\ \dot{R(t)}R(t)^T + R(t)\dot{R(t)^T} = 0 \\ 即\\ \dot{R(t)}R(t)^T = -R(t)\dot{R(t)^T} \\ 由 (ab)^T = b^Ta^T \\ \dot{R(t)}R(t)^T = -(\dot{R(t)R(t)^T})^T \\ 對於一個反對稱矩陣，有 a = -a^T，顯然上式滿足，故\dot{R(t)}R(t)^T為反對稱矩陣。\\ 假設 \dot{R(t)}R(t)^T = \phi(t)^\wedge \]

需要注意的是，這裡的\(\phi(t)^{\wedge}\)中的^代表著一個三維向量\(\phi\)擴充為一個反對稱陣。比如

\[a = (1,2,3)^T \\ a^{\wedge} = \begin{bmatrix} 0&1&2 \\ -1&0&3 \\ -2&-3&0 \\ \end{bmatrix} \]

因此\(\phi(t)^{\wedge}\)代表我們假設R由\(\phi\)擴充而來。
接著推理有

\[兩邊右乘R，有 \dot{R(t)} = \phi(t)^{\wedge}R(t) \\ 假定t = 0時刻 R = I，且認為t = 0附近\phi(t)變化不大，記作\phi_0\quad可認為上式就是一個微分方程。\\ 解得 \ln(R(t)) = \phi_0^{\wedge} \\ 同理有 R(t) = exp(\phi_0^{\wedge}) \]

可以看出，經過一通推導，我們發現旋轉矩陣與一個東西有著對數形式上的對映。這裡的\(\phi_0\)就是SO(3)對應的李代數。
與李群一樣，李代數的構成也要滿足一定的性質。如下

可以知道對於\(\phi_0\)來說，滿足如上的定義

其李括號的定義的運算型別為

對於SE(3)上的李群，直接給出其李代數對應形式

其李括號的定義為

這裡的\(\wedge\)操作與so(3)的定義不同，不再表示由三維向量擴充反對稱陣，而是表示將\(\xi\)這個六維向量擴充為一個四維矩陣。不難看出\(\xi\)的前三維由平移向量得出，後三維就是se(3)。

數學上已經證明，李代數處於線性空間中，可以進行求導操作

3. 對數對映與指數對映

上面我們給出了李代數與李群的對映關係，即

\[\ln(R(t)) = \phi_0^{\wedge}\\ 即 \ln{R(t)}^{\vee} = \phi_0 \\\ R(t) = exp(\phi_0^\wedge) \]

但是這只是抽象的公式，並沒有告訴我們如何將其中的一些具體元素對應轉換。但是透過這個我們可以知道對於已知李群求李代數是一種對數對映，已知李代數求李群是一種指數對映。

\[對於exp(\phi^{\wedge})，進行泰勒展開 \\ 因為e^x = \sum_{n=0}^{\infty} \frac{x^n}{n!} = 1 + x + \frac{x^2}{2!} + \frac{x^3}{3!} + \frac{x^4}{4!} + \ldots \\ 又對於一個三維向量可以寫成模長乘單位方向向量的模式，不妨假設\phi = \theta\vec{a},\vec{a}為單位向量 \\ \]

具體形式與化簡如下圖

化簡應用的性質如下

最終我們得出這樣的一個式子

不難看出，這個公式與上一講旋轉矩陣轉角軸(旋轉向量)的羅德里格斯公式如出一轍。這也揭示了so(3)的物理意義就是旋轉向量。

因此由so(3)的李代數指數對映求其對應的李群SO(3)的過程本質上就是旋轉向量向旋轉矩陣的轉換。

對於已知旋轉矩陣求其so(3)上的李代數的過程，我們有對數對映形式\(\ln{R(t)} = \phi\),盡然可以使用與求指數對映形式類似的泰勒展開來推導，這樣比較麻煩。

但是我們已經知道了so(3)上的李代數實際上就是角軸，因此使用第三章中旋轉向量轉角軸的公式即可。

\[\text{角度}\\ \theta = arcos(\frac{tr(R)-1}{2})\\ \text{軸向量}\\ R\vec{n} = \vec{n}，求解R特徵值為1時的特徵向量 \]

關於方向向量的求解，因為使用角軸描述旋轉時，旋轉前後的單位方向向量是不變的(因為是繞著這個軸轉的嘛)。因此對軸乘以旋轉矩陣進行旋轉操作後的結果仍是其本身，依據這個列出的特徵方程。

至於SE(3)與se(3)之間的轉換，由下圖總結給出

最後總結以下李群與李代數的對映關係以及變換

4. 李群與李代數的近似

既然李代數處於線性空間，而且李群與李代數可以透過指數對映以及對數對映進行轉換，那麼我們是否可以直接使用李代數表述李群的求導操作呢？

一個樸素的思想

\[對於R_1R_2 = exp(\phi_1^\wedge)exp(\phi_2^\wedge) \\ 在實數域中有exp(x_1)exp(x_2) = exp(x_1 + x_2) \\ 猜想：是否有 R_1R_2 = exp(\phi_1^\wedge)exp(\phi_2^\wedge) = exp(\phi_1^\wedge + \phi_2^\wedge) = exp((\phi_1 + \phi_2)^\wedge) \]

但是根據BCH近似公式

可以知道對於兩個矩陣，\(exp(A)exp(B) \neq exp(A+B)而是等於exp(A+B+餘項)\)。

不過根據如下公式可以將上式進行線性近似

其中\(J_l\)為左乘雅可比矩陣，\(J_r\)為右乘雅可比矩陣。具體表示方法如下

也就是說當兩個量中有一個為小量時可以使用BCH近似來實現李群乘法轉化為李代數加法。但是還是不夠直觀，給出如下推導。注意雅可比矩陣視為常係數。

\[exp((J_l(\phi_2)\phi_1)^{\wedge})exp(\phi_2^{\wedge}) = exp(J_l(\phi_2)(\phi_1)^{\wedge})exp(\phi_2^{\wedge}) \\ 根據以上BCH近似將J_l(\phi_2)(\phi_1)^{\wedge}看作小量，有 \\ exp(J_l(\phi_2)(\phi_1)^{\wedge})exp(\phi_2^{\wedge}) = exp(J_l^{-1}(\phi_2)J_l(\phi_2)\phi_1^\wedge+\phi_2^\wedge) = exp(\phi_1^\wedge + \phi_2^\wedge) = exp((\phi_1 + \phi_2)^\wedge) \]

即最後得出了這樣一個等式,且將\(J_l(\phi_2)\)簡寫為\(J_l\)

\[exp((\phi_1+\phi_2)^\wedge) = exp(J_l\phi_1^\wedge)exp(\phi_2^\wedge) \]

依據這個等式，我們就可以將李代數的加法與李群的乘法近似且簡潔的聯絡在一起了。下面貼出SE(3)與se(3)的關係，推導類似。

5. 李代數求導

以上我們就已經具備了實現李代數求導的準備。

求導的方法有兩種

• 李代數求導
• 擾動模型求導
  對於旋轉矩陣求導的應用場景如下

\[對於一個三維向量\vec{p},對其進行旋轉操作R，求旋轉後的向量或者點相對於旋轉的變化率 \\ 不妨記為\frac{\partial{Rp}}{\partial{R}} \]

李代數求導

李代數求導就是將上式中的旋轉矩陣李群直接用對應的李代數來進行替換，然後在李代數中套用導數定義來實現

• 第二步使用了上面推導的\(exp((\phi_1+\phi_2)^\wedge) = exp(J_l\phi_1^\wedge)exp(\phi_2^\wedge)\)
• 第三步近似，因為\(J_l\delta\phi^\wedge\)是一個小量，因此由泰勒展開或者說等價無窮小，\(e^x = 1+x\)有\(J_l\delta\phi^\wedge = (I + (J_l)\delta\phi^\wedge)\)
• 第四步展開括號或者提公因式可以得出
• 第五步由反對稱陣與向量叉乘的關係\(\vec{a}\times\vec{b} = \vec{a}^\wedge\vec{b} = -\vec{b}\times\vec{a} = -\vec{b}^\wedge\vec{a}\) 即 \(\vec{a}^\wedge\vec{b} = -\vec{b}^\wedge\vec{a}\)
• 第六步上下約掉\(\delta\phi\)

擾動模型求導

擾動模型求導的思路是在原來的旋轉矩陣上左乘一個小的擾動旋轉。然後對這個擾動求導。
不嚴謹的表示為

\[\lim_{\Delta R\rightarrow0}\frac{\Delta RRp-Rp}{\Delta R} \]

要做的就是將上述中的李群換成對應的李代數。設\(\Delta R\leftrightarrow \delta\varphi\),推導如下圖。

推導所使用的化簡方法與李代數求導並無二異。
可以看出，使用擾動求導的方法得出的結果更加簡潔，沒有最後的左乘雅可比矩陣。

但是使用兩種方法求出的結果是有差異的。

• 李代數求導的物件是對旋轉矩陣本身，令導數等於0求出的旋轉矩陣即為最優的旋轉。
• 擾動模型求導的物件是\(\Delta R\),令導數等於0求出的為最優的\(\Delta R\),最終的\(R = \Delta RR\)

最後貼出SE(3)求導的推導。

需要注意的是\((Tp)^\odot\)中的\(\odot\)符號代表著SE(3)求導。

最後想要支援李代數李群相關運算，可以使用Sophus庫來實現，Sophus庫是Eigen的一個關於李代數李群的一個擴充套件庫。