在SLAM中經常會用到李群李代數與四元數來表示旋轉變換，這些數學公式往往需要推導來推導去，分分鍾搞到頭都大了。但在SLAM中往往用到其中那么幾個固定的性質，所以是沒有必要對這些數學基礎作過多深入的研究，只需要記住其中一些常用的公式及性質即可。因此，本人在這里對這些數學基礎作一個簡單的總結，以便日后在工程中使用。
旋轉的表示方式
SLAM中，往往會使用三種方式來表達空間中的旋轉變換，分別為：旋轉向量、旋轉矩陣、四元數。
旋轉向量
旋轉向量可以非常直觀地表示空間中的一個旋轉變換：假設空間中有一單位向量\(\mathbf{n}\)，以\(\mathbf{n}\)為轉軸且旋轉方向與\(\mathbf{n}\)相同，旋轉的角度為\(\theta\)。可以將上述旋轉變換記為“角軸”的組合形式：\(<\theta,\mathbf{n}>\)，也可以組合成變通向量的形式：\(\mathbf{\phi}=\theta \mathbf{n}\).
旋轉矩陣
旋轉矩陣\(\mathbf{R}_{AB}\)可以表示一個物體由姿態\(A\)旋轉變換到姿態\(B\)的過程，若將\(A\)與\(B\)視為一個坐標系，則\(\mathbf{R}_{BA}\) 可以表示坐標系\(A\)在坐標系\(B\)中的姿態。設\(W\)為世界坐標系，物體由姿態\(\mathbf{R}_{WA}\)轉換到姿態\(\mathbf{R}_{WB}\)可以用\(\mathbf{R}_{AB}\)來表示，即：\(\mathbf{R}_{WB} = \mathbf{R}_{WA} \mathbf{R}_{AB}\).
如果坐標關系非常明確，則可以直接將下標去掉，直接記為\(\mathbf{R}\). 其中\(\mathbf{R}\)為單位正交陣，其具有如下性質：
- \(\mathbf{R}\)的行（列）均為單位向量且兩兩正交；
- \(\mathrm{det}(\mathbf{R})=1\)；
- \(\mathbf{R}^{-1} = \mathbf{R}^\mathrm{T}\)
四元數
四元數可以定義為:
\[\mathbf{q} = w+xi+yj+zk \]
其中\(i,j,k\) 分別為四元數的三個虛部，這三個虛部滿足如下性質：
\[\left\{\begin{matrix} i^2 + j^2 + k^2 = 1\\ ij=k, ji=-k \\ jk=i, kj=-i \\ ki=j, ik=-j \end{matrix}\right. \]
四元數還可以表示為
\[\mathbf{q} = [w,x,y,z]^\mathrm{T} \]
或
\[\mathbf{q} = [s,\mathbf{v}], \mathbf{v} = [x,y,z]^\mathrm{T} \in \mathbb{R}^3 \]
我們可以使用單位四元數來表示空間任意的旋轉變換，其中符合條件\(w^2+x^2+y^2+z^2=1\)的四元數為單位四元數。
三種方式之間轉換
上述三種數學工具都可以表示空間中的旋轉，他們其實是等價的，可以進行相互轉換。
旋轉向量 \(\leftrightarrow\) 旋轉矩陣
設旋轉向量為\(\mathbf{\phi}=\theta \mathbf{n}\)，與之等價的旋轉矩陣為\(\mathbf{R}\)，兩者之間的轉換可以使用羅德里格斯公式來實現，即
\[\mathbf{R} = \cos \theta \mathbf{I} + (1 - \cos \theta) \mathbf{n} \mathbf{n}^\mathrm{T} + \sin \theta \mathbf{n}^{\wedge} \]
其中\(\mathbf{n}^{\wedge}\)為由\(\mathbf{n}\)轉化而成的反對稱矩陣，即
\[\mathbf{n}^{\wedge} = \begin{bmatrix} 0 & -n_3 & n_2 \\ n_3 & 0 & -n_1 \\ -n_2 & n_1 & 0 \end{bmatrix} \]
\(\mathbf{n}\)的反對稱矩陣也可以用\([\mathbf{n}]_{\times}\)來表示。
反過來，由\(\mathbf{R}\)轉化為\(\mathbf{n}\)的方法為：
\[\left \{ \begin{matrix} \theta = \arccos(\frac{\mathrm{tr}(\mathbf{R})-1}{2}) \\ \mathbf{R} \mathbf{n} = \mathbf{n} \end{matrix} \right. \]
其中\(\mathbf{R} \mathbf{n} = \mathbf{n}\)表示矩陣\(\mathbf{R}\)的為1的特征值對應的單位特征向量。
旋轉向量 \(\leftrightarrow\) 四元數
設旋轉向量為\(\mathbf{\phi}=\theta \mathbf{n}\)，與之等價的四元數為\(\mathbf{q}\)，則有：
\[\mathbf{q} = \cos \frac{\theta}{2} + \mathbf{n} \sin \frac{\theta}{2} = [\cos \frac{\theta}{2}, \mathbf{n} \sin \frac{\theta}{2}]^\mathrm{T} \]
這種轉換與虛數中的歐拉公式十分相似，即\(\mathrm{e}^{j \theta}=\cos \theta + j \sin \theta\)，此處可以進行類比記憶。
旋轉矩陣 \(\leftrightarrow\) 四元數
設四元數為\(\mathbf{q}=q_0+ q_1i+ q_2j + q_3k\)，與之等價的旋轉矩陣為\(\mathbf{R}\)，他們之間的變換有：
\[\mathbf{R} = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2 q_1 q_2 - 2 q_0 q_3 & 2 q_1 q_3 + 2 q_0 q_2 \\ 2 q_1 q_2 + 2 q_0 q_3 & 1 - 2 q_1^2 - 2 q_3^2 & 2 q_2 q_3 - 2 q_0 q_1 \\ 2 q_1 q_3 - 2 q_0 q_2 & 2 q_2 q_3 + 2 q_0 q_1 & 1 - 2 q_1^2 - 2 q_2^2 \end{bmatrix} \]
\[\begin{matrix} q_0=\frac{\sqrt{\mathrm{tr}(\mathbf{R})+1}}{2} & q_1 = \frac{m_{23}-m_{32}}{4 q_0} & q_2 = \frac{m_{31}-m_{13}}{4 q_0} & q_3 = \frac{m_{12} - m_{21}}{4 q_0} \end{matrix} \]
李群李代數的性質
上面講到的旋轉矩陣\(\mathbf{R}\)可以構成一個李群SO(3)，即特殊正交群，而旋轉向量\(\mathbf{\phi}=\theta \mathbf{a}\)可以構成一個李代數\(\mathfrak{so}(3)\)。由於引入發李代數，所以對旋轉矩陣的求導就變得非常方便。在SLAM中對旋轉矩陣求導或對物體的姿態作優化時會大量用到李代數與李群之間的轉換。
指數與對數映射
李群與李代數之間的轉換可以使用指數與對數來實現.
指數映射：
\[\mathbf{R} = \exp(\mathbf{\phi}^{\wedge}) = \exp(\theta \mathbf{a}^{\wedge})= \cos \theta \mathbf{I} + (1-\cos \theta) \mathbf{a} \mathbf{a} ^{\mathrm{T}} + \sin \theta \mathbf{a}^{\wedge} \]
上面這條公式是不是貌似十分相識？？？沒錯，它就是上面曾經曰過的羅德里格斯公式。
對數映射：
\[\mathbf{\phi} = \ln(\mathbf{R})^{\vee} = \left ( \sum_{n=0}^{\infty} \frac{(-1)^n}{n+1} (\mathbf{R} - \mathbf{I})^{n+1} \right)^{\vee} \]
這條公式看起來有點復雜，不過沒有關系，反正也用得不太多，因為我們通常不會用上式來計算對數映射，而是用旋轉矩陣的跡來計算，請見旋轉向量 \(\leftrightarrow\) 旋轉矩陣一節中有介紹。
BCH公式
指數函數\(\exp(x)\)(其中x是標量)具有\(\exp(x_1) \exp(x_2)=\exp(x_1+x_2)\)的性質，但對於李代數的指數映射是否具有如\(\exp(\mathbf{\phi}_1^{\wedge})\exp(\mathbf{\phi}_1^{\wedge}) = \exp((\mathbf{\phi}_1+\mathbf{\phi}_2)^{\wedge})\)的性質呢？很不幸，這個性質是不存在的，但可以使用BCH來近似。
BCH公式如下：
\[\ln(\exp(\mathbf{\phi}_1^{\wedge})\mathbf{\phi}_2^{\wedge}))^{\vee} \approx \left \{ \begin{matrix} \mathbf{J}_l(\mathbf{\phi}_2)^{-1} \mathbf{\phi}_1 + \mathbf{\phi}_2 & \mathbf{\phi}_1 \in \sigma \\ \mathbf{\phi}_1 + \mathbf{J}_r(\mathbf{\phi}_1)^{-1} \mathbf{\phi}_2 & \mathbf{\phi}_2 \in \sigma \end{matrix} \right. \]
其中\(\sigma\)表示一個微波量的集合。上式中，第一個式子為左乘近似，第二個式子為右乘近似。現在此對其進行解釋如下。
對於第一個式子，假設旋轉矩陣為\(\mathbf{R}_2\)（其對應的李代數為\(\mathbf{\phi}_2\)），對\(\mathbf{R}_2\)左乘一個微小的旋轉矩陣\(\mathbf{R}_1\)（其對應的李代數為\(\mathbf{\phi}_1\)），則\(\mathbf{R}_1\mathbf{R}_2\)所對應的李代數近似為\(\mathbf{J}_l(\mathbf{\phi}_2)^{-1} \mathbf{\phi}_1 + \mathbf{\phi}_2\)，其中\(\mathbf{J}_l(\mathbf{\phi}_2)^{-1} \mathbf{\phi}_1\)為左乘了微小旋轉矩陣\(\mathbf{R}_1\)所導致的李代數增加的部分。
對於第二個式子，假設旋轉矩陣為\(\mathbf{R}_1\)（其對應的李代數為\(\mathbf{\phi}_1\)），對\(\mathbf{R}_1\)右乘一個微小的旋轉矩陣\(\mathbf{R}_2\)（其對應的李代數為\(\mathbf{\phi}_2\)），則\(\mathbf{R}_1\mathbf{R}_2\)所對應的李代數近似為\(\mathbf{J}_r(\mathbf{\phi}_1)^{-1} \mathbf{\phi}_2 + \mathbf{\phi}_1\)，其中\(\mathbf{J}_r(\mathbf{\phi}_1)^{-1} \mathbf{\phi}_2\)為左乘了微小旋轉矩陣\(\mathbf{R}_2\)所導致的李代數增加的部分。
BCH公式還可以寫成如下的形式，其中\(\Delta \mathbf{\phi}\)為增加的微小旋轉：
\[\begin{matrix} \exp(\Delta \mathbf{\phi^{\wedge}}) \exp(\mathbf{\phi^{\wedge}})=\exp((\mathbf{J}_l^{-1}(\mathbf{\phi}) \Delta \mathbf{\phi} + \mathbf{\phi})^{\wedge}) \\ \exp(\mathbf{\phi^{\wedge}}) \exp(\Delta \mathbf{\phi^{\wedge}})=\exp((\mathbf{\phi} + \mathbf{J}_r^{-1}(\mathbf{\phi}) \Delta \mathbf{\phi})^{\wedge}) \\ \exp((\mathbf{\phi} + \Delta \mathbf{\phi})^{\wedge}) = \exp((\mathbf{J}_l \Delta \mathbf{\phi})^{\wedge}) \exp(\mathbf{\phi}^{\wedge}) = \exp(\mathbf{\phi}^{\wedge}) \exp((\mathbf{J}_r \Delta \mathbf{\phi})^{\wedge}) \end{matrix} \]
李代數求導
假設我們對一個空間點\(\mathbf{p}\)進行了旋轉，得到了\(\mathbf{R} \mathbf{p}\)，現在我們要計算旋轉之后的點關於旋轉矩陣的導數，即：
\[\frac{\partial (\mathbf{R} \mathbf{p})}{\partial \mathbf{R}^{\mathrm{T}}} \]
由於\(\mathbf{R} \in \mathrm{SO}(3)\)是不在在加法的，所以為了方便求導，我們可以使用李代數來進行求導，即將上式轉化為正式：
\[\frac{\partial (\mathbf{R} \mathbf{p})}{\partial \mathbf{R}^{\mathrm{T}}} \Leftrightarrow \frac{\partial(\exp(\mathbf{\phi}^{\wedge}) \mathbf{p})}{\partial \mathbf{\phi}} = -(\mathbf{R} \mathbf{p})^{\wedge} \mathbf{J}_l \]
上式的結果是用左乘BCH來近似得到的，由於\(\mathbf{J}_l\)的存在，增加計算的復雜度，所以可以使用擾動模型（左乘）來進行簡化。左擾動模型的結果為：
\[\frac{\partial (\mathbf{R} \mathbf{p})}{\partial \mathbf{R}^{\mathrm{T}}} \Leftrightarrow \frac{\partial (\mathbf{R} \mathbf{p})}{\partial \phi} = -(\mathbf{R} \mathbf{p})^{\wedge} \]
上述公式只需直接記憶並使用即可，如果想知道如何推導的，可見參考文獻[1]中的p74~p75.
四元數的性質
這里主要是對四無數的運算及若干性質作一簡單的總結。具體的四元數的教程請閱讀參考文獻[2]。
四無數加法
\[\mathbf{p} + \mathbf{q} = [p_w, \mathbf{p}_v]^{\mathrm{T}} + [q_w, \mathbf{q}_v]^{\mathrm{T}} = [p_w + q_w, \mathbf{p}_v + \mathbf{q}_v]^{\mathrm{T}} \]
四無數乘法
四元數的加法用符號\(\otimes\)表示。其運算法則如下：
\[\mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_w q_w - \mathbf{p}_v^\mathrm{T} \mathbf{q}_v \\ p_w \mathbf{q}_v + q_w \mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v \end{bmatrix} \]
四元數的乘法不滿足交換律，即：
\[\mathbf{p} \otimes \mathbf{q} \neq \mathbf{q} \otimes \mathbf{p} \]
但四元數乘法仍然滿足結合律，即：
\[(\mathbf{p} \otimes \mathbf{q}) \otimes \mathbf{r} = \mathbf{p} \otimes (\mathbf{q} \otimes \mathbf{r}) \]
故四元數加法也滿足分配律，即：
\[\mathbf{p} \otimes (\mathbf{q} + \mathbf{r}) = \mathbf{p} \otimes \mathbf{q} + \mathbf{p} \otimes \mathbf{r} \]
或者是：
\[(\mathbf{p} + \mathbf{q}) \otimes \mathbf{r} = \mathbf{p} \otimes \mathbf{r} + \mathbf{q} \otimes \mathbf{r} \]
兩個四元數的乘法還可以用矩陣的形式的表示，即：
\[\mathbf{q}_1 \otimes \mathbf{q}_2=[\mathbf{q}]_L \mathbf{q}_2 =[\mathbf{q}]_R \mathbf{q}_1 \]
而其中的\([\mathbf{q}]_L\) 與 \([\mathbf{q}]_R\)分別如下：
\[[\mathbf{q}]_L = \begin{bmatrix} q_w & -q_x & -q_y & -q_x \\ q_x & q_w & -q_z & q_y \\ q_y & q_z & q_w & -q_x \\ q_z & -q_y & q_x & q_w \end{bmatrix} = q_w \mathbf{I} + \begin{bmatrix} 0 & -\mathbf{q}_v^\mathbf{T} \\ \mathbf{q}_v & [\mathbf{q}_v]_{\times} \end{bmatrix} \]
\[[\mathbf{q}]_R = \begin{bmatrix} q_w & -q_x & -q_y & -q_x \\ q_x & q_w & q_z & -q_y \\ q_y & -q_z & q_w & q_x \\ q_z & q_y & -q_x & q_w \end{bmatrix} = q_w \mathbf{I} + \begin{bmatrix} 0 & -\mathbf{q}_v^\mathbf{T} \\ \mathbf{q}_v & -[\mathbf{q}_v]_{\times} \end{bmatrix} \]
上面這兩條公式在證明或推導公式過程中十分有用，可以重點關注。
參考文獻
[1] 《視覺SLAM十四講——從理論到實踐》高翔， 張濤
[2] 《Quaternion kinematics for the error-state Kalman filter》 Joan Sola.
