李群和李代數

問題的引入

當我們估計出相機姿態$[R,t]$了以後，估計的結果和實際的相機姿態肯定會有一些不一致性，因而我們需要對估計出來的結果進行優化。優化方法一般都採用迭代優化的方法，每次迭代都更新一個位姿的增量$\Delta$，使得目標函式最小。這個$\Delta$就是通過誤差函式對$\mathbf{T}$或者$\mathbf{R}$求微分得到的，也就是說我們需要對變換矩陣$\mathbf{T}$以及旋轉矩陣$\mathbf{R}$求導。

但是，旋轉矩陣並沒有良好定義的加法，即$R_1$和$R_2$是都是旋轉矩陣，但是$R_1+R_2$並不符合旋轉矩陣的定義：旋轉矩陣應該滿足$R{R}^T=I$，且$det(R)=1$，即行列式的值為1的正交矩陣，因此上邊的結論是顯而易見的。

所以，在實際操作中，我們無法通過如下的微調滿足我們對相機觀測模型的優化：

$$\begin{gathered} [R,t]\stackrel{\Delta R,\Delta t}{⟶}[R^{\prime },t^{\prime }] \\ {t}^{\prime }=t+\Delta t \\ {R}^{\prime }=R+\Delta R \end{gathered}$$

因而也會影響到觀測模型（關於旋轉矩陣的函式）對$\mathbf{R}$的求導，因為通常來說我們的導數定義為：

$$U(R)=\frac{dU}{dR}=\underset{\Delta R\to 0}{\lim }\frac{U(R+\Delta R)-U(R)}{\Delta R}$$

沒有良好定義的加法——沒有傳統意義上的導數定義。
因此，李群和李代數的作用就是間接對$\mathbf{R}$進行求導。

預備知識

矩陣指數

首先要引入矩陣指數的概念（Matrix Exponential）。矩陣指數可以由線性微分方程的解的形式匯出，比如有一階線性微分方程：

$$\stackrel{\cdot }{x}=ax(t)$$

其中，$x(t)\in \mathbb{R}$，$a\in \mathbb{R}$，並且初始分佈為$x(0)=x_0$，那麼解為：

$$x(t)=e^{at}{x}_0$$

其中指數函式可以展開為無窮級數：

$$e^{at}=1+at+\frac{(at{)}^2}{2!}+\frac{(at{)}^3}{3!}+\cdot \cdot \cdot$$

同理，如果我們對三維向量由一階線性微分方程：

$$\stackrel{\cdot }{x}(t)=Ax(t)$$

其中$x(t)\in {\mathbb{R}}^3$，$A\in {\mathbb{R}}^{3\times 3}$，且初始條件為$x(0)=x_0$，那麼解為：

$$x(t)=e^{At}{x}_0$$

其中矩陣指數函式可以展開為無窮級數的形式：

$${e}^{At}=I+At+\frac{(At{)}^2}{2!}+\frac{(At{)}^3}{3!}+\cdot \cdot \cdot$$

矩陣指數與矩陣旋轉的關係

如上圖，三維向量$p(0)$繞著單位旋轉軸$\hat{\omega }$ （$\hat{\omega }\in {\mathbb{R}}^3$，$||\hat{\omega }||=1$）旋轉$\theta$角度，得到三維向量 $p(\theta )$。該旋轉運動亦可以視作是三維向量$p(0)$以$1rad/s$的角速度繞著單位旋轉軸$\hat{\omega }$從時間$t=0$ 運動到時間 $t=\theta$得到三維向量$p(\theta )$。

如果用$p(t)$來表示旋轉路徑上向量端點處的位置，用$\stackrel{\cdot }{p}(t)$表示該點的瞬時速度值，則有：

$$\stackrel{\cdot }{p}=\hat{\omega }\times p$$

而向量叉乘可以寫為左端向量對應的反對稱矩陣與右端向量的乘積形式：

$$\stackrel{\cdot }{p}=[\hat{\omega }]p$$

其中，定義$[\cdot ]$為反對稱矩陣運算元，假如$\hat{\omega }=\left\{\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right\}$，那麼有：

$$[\omega ]=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$

那麼上式就有如下解：

$$p(t)=e^{[\hat{\omega }]t}p(0)$$

因為角速度為$1rad/s$，所以這裡角度$\theta$和時間$t$是可以互換的：

$$p(t)=e^{[\hat{\omega }]\theta }p(0)$$

由於反對稱矩陣有滿足${[\hat{\omega }]}^3=-[\hat{\omega }]$，將矩陣指數函式展開有：

$$\begin{gathered} e^{[\hat{\omega }]\theta }=I+[\hat{\omega }]\theta +[\hat{\omega }{]}^2\frac{{\theta }^2}{2!}+[\hat{\omega }{]}^3\frac{{\theta }^3}{3!}+\cdot \cdot \cdot \\ =I+(\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdot \cdot \cdot )[\hat{\omega }]+（\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\frac{{\theta }^6}{6!}-\cdot \cdot \cdot ）[\hat{\omega }{]}^2 \\ =I+\sin \theta [\hat{\omega }]+(a-\cos \theta )[\hat{\omega }{]}^2 \end{gathered}$$

因此，三維旋轉矩陣$R$就可以用矩陣指數$e^{[\hat{\omega }]\theta }$的形式來表示。

李群和李代數

群

群是一種集合加上一種代數運算。這個集合和運算應該滿足“封結么逆”四個性質，把集合記為 $\mathbf{A}$ ，運算記為$\cdot$, 群就可以記為 $\mathbf{G}=(\mathbf{A},\cdot )$。

• 封閉性：$\forall a_1,a_2\in \mathbf{A},a_1\cdot a_2\in \mathbf{A}$
• 結合律：$\forall a_1,a_2,a_3\in \mathbf{A},(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
• 么元：$\exists a_0\in \mathbf{A},s.t.\forall a\in \mathbf{A},a_0\cdot a=a\cdot a_0$
• 逆元：$\forall a\in \mathbf{A},\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

可以驗證，三維旋轉矩陣對矩陣乘法構成了特殊正交群$SO(3)$——直觀意義上接連兩次旋轉動作認為旋轉。

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3) = \{R \in \mathbb{R}^{3 \times 3} | RR^T = I, det(R) = 1\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}

變換矩陣構成了特殊歐式群$SE(3)$，它也對乘法封閉。

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 × 3 } SE(3) = \{ T = \begin{bmatrix} R & t \\ {0^T} & 1 \\ \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), t \in \mathbb{R}^{3\times 3}\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3×3}

但是二者對加法都不封閉，即兩個變換矩陣相加後並不是旋轉矩陣。

李群

李群（Lie group）是具有群結構的光滑微分流形，其群作用與微分結構相容，從抽象上來說，李群就是具有光滑或連續性質的群。對應到我們的問題上，由於三維機器人在空間中移動必然是連續地運動，不可能發生“瞬移”的情形，因而$SO(3)$和$SE(3)$都是李群。

下面由旋轉矩陣$R$引出李代數。因為R是隨時間連續變化的——相機可看成是連續運動，我們將相機姿態看作是時間$t$的函式，那麼自然就有$R(t)R(t{)}^T=I$成立。上式的左右兩側同時對t求導，我們可得

$$\stackrel{\cdot }{R}(t)R(t{)}^T+R(t)\stackrel{\cdot }{R}(t{)}^T=0$$

整理後可得

$$\stackrel{\cdot }{R}(t)R(t{)}^T=-{(R(t)\stackrel{\cdot }{R}(t{)}^T)}^T$$

如果我們把等式左邊看成一個整體，那麼我們會發現上式左邊是一個反對陳矩陣。

對於任意一個反對稱矩陣，我們都由一個向量做反對稱變換得到，比如：

a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a = { a 1 , a 2 , a 3 } . a^{\wedge} = A = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}, \quad A^{\vee} = a = \{a_1, a_2, a_3\}. a∧=A=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​,A∨=a={a1​,a2​,a3​}.

如果將上式左邊記為$\varphi (t{)}^{\wedge }=\stackrel{\cdot }{R}(t)R(t{)}^T\in {\mathbb{R}}^3$，我們對這個式子兩邊右乘$R(t)$並注意到$R(t)$的正交矩陣性質，就可以得到：

$$\stackrel{\cdot }{R}(t)={\varphi }^{\wedge }(t)R(t)$$

就好像對$R(t)$求導後其實是左乘了一個$\varphi (t)$，而上邊這個等式就類似一個一階微分方程，$\varphi$反應了一節導數的性質，它位於正切空間上(tangent space)。我們認為初始值$R(0)=I$，那麼就可以解得：

$$R(t)=e^{{\varphi }^{\wedge }t}$$

由於這個式子只在$t$附近有效。由此可知，對任意時刻$t$，旋轉矩陣$R$和一個向量$\varphi$對應，二者滿足矩陣指數關係，這個$\varphi$就是$SO（3）$上的李代數$so（3）$。因而，為了求$R$，我們只需要求出$exp(\varphi t)$即可，這個關係即為指數對映。現在的問題是$so(3)$是如何定義的，指數對映又如何求出呢？

李代數

每個李群都有與之對應的李代數，李代數描述了李群的區域性性質。在這個區域性正切空間裡，一些我們可以找到一些比較好的性質，比如良好定義的加法，從而我們有了良好定義的導數——將李群的導數轉化為李代數上的導數，求出來後再轉化回去，這就是我們處理李群導數相關內容的基本思路，同時也是研究的重心。

李代數由一個集合$V$, 一個數域$F $和一個二元運算$[\cdot]$ 組成。如果它們滿足以下幾條性質，則稱$(V,F,[\cdot ])$ 為一個李代數，記作$g$，其中二元運算$[\cdot ]$成為離括號，表達了兩個元素的差異。

• 封閉性： $\forall \mathbf{X},\mathbf{Y}\in V,[\mathbf{X},\mathbf{Y}]\in V$
• 雙線性： $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in V,a,b\in F$ , 有 $[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$
• 自反性： $\forall \mathbf{X}\in V,[\mathbf{X},\mathbf{X}]=0$
• 雅可比等價：$\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in V,[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$

舉例三位向量${\mathbb{R}}^3$上定義的叉乘就是一個離括號，其中$g=({\mathbb{R}}^3,\mathbb{R},\times )$就構成了李代數。

李代數$so(3)$

之前提到的$SO(3)$對應的向量$\varphi$就是一種李代數，他定義在${\mathbb{R}}^3$的向量上。它的反對稱矩陣記作：$\mathbf{\Phi }={\varphi }^{\wedge }$，那麼SO(3)中的向量對應的李括號為：

$$[{\varphi }_1,{\varphi }_2]=({\mathbf{\Phi }}_1{\mathbf{\Phi }}_2-{\mathbf{\Phi }}_2{\mathbf{\Phi }}_1{)}^{\vee }$$

經過驗證可以得到上邊的式子等價於兩向量作外積。

$$[{\varphi }_1,{\varphi }_2]={\varphi }_1\times {\varphi }_2$$

李代數$se(3)$

與$so(3)$不同的是，$se(3)$定義在${\mathbb{R}}^6$空間中。

$$se(3)=\left\{\begin{array}{c}\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in R^6,\rho \in R^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in R^{4\times 4}\end{array}\right\}$$

其中$se(3)$每個元素都稱為$\xi$，它是一個六維向量：前三維為平移，記作$\rho$，後三位即為三維旋轉向量$\varphi$，實質上就是$so(3)$中的元素。另外在$se(3)$ 中， $\wedge$ 符號的含義被擴充了：這裡它將一個六維向量轉換為四維矩陣，但這裡不再表示反對稱矩陣，其實這裡$\xi$即為變換矩陣的轉置。同樣，李代數$se(3)$的李括號被定義為：

$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$

上式經驗證後發現，李括號將兩個變換矩陣變成了了一個無旋的變換矩陣：

$$[{\xi }_1,{\xi }_2]=\left\{\begin{array}{c}{\Phi }_1^{\wedge }{\rho }_2-{\Phi }_2^{\wedge }{\rho }_1\\ 0_{3\times 3}\end{array}\right\}$$

即旋轉部分被抹去，只剩下平移部分，而平移部分是由原來兩個元素中的旋轉和平移部分組合得到的。

指數對映和對數對映

李群到李代數之間是一個指數對映（反之是對數對映），通過藉助這個指數對映我們可以將大量在李群上不好解決的問題轉嫁到李代數上解決，之後再對映回去間接得到李群上的答案。

$SO(3)$上的指數對映

我們利用關係李群和李代數之間的關係：

$$\stackrel{\cdot }{R}(t)={\varphi }^{\wedge }(t)R(t)$$

可知求出起初指數對映，就能得到李群上的導數。求指數對映我們需要用到泰勒展開式。任意矩陣的指數對映都可以寫成一個泰勒展開式，這個展開式只有在收斂的情況下才有解，結果仍然是一個矩陣。所以對於$so(3)$中的元素$\varphi$，其指數對映可以寫成

$$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$

我們定義$\varphi =\theta \mathbf{a}$，其中$\theta$和$a$分別是$\varphi$的模長和單位向量。我們可以驗證：

$$\begin{gathered} {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T-\mathbf{I} \\ {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge } \end{gathered}$$

以上兩個式子提供了我們對泰勒展開式中高次項化簡的方法。 那麼我們對上邊泰勒展開式化簡可以得到：

$$\begin{gathered} \exp {\varphi }^{\wedge }=\exp (\theta \mathbf{a})=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n \\ =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^T \end{gathered}$$

這和之前的羅德里格斯公式有著相同的形式，標明$so(3)$實際上就是由旋轉向量組成的空間，而指數對映其實就是羅德里格斯公式。那麼與之對應的對數對映，給定旋轉矩陣，我們就可以求李代數，將$SO(3)$對映到$so(3)$上：

$$\varphi =\ln (R{)}^{\vee }=(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}{)}^{\vee }$$

但是實際上沒必要這樣求，因為旋轉向量已經介紹了矩陣到向量的轉換關係：

$$\begin{gathered} \theta =\arccos \frac{tr(\mathbf{R})-1}{2} \\ \mathbf{R}n=n \end{gathered}$$

這裡需要注意的是，指數對映是一個滿射，即每個$SO(3)$中的元素都可以找到一個$so(3)$裡的元素與之對應；但是泛指不成立，因為多旋轉一圈的旋轉向量對應的李代數是相同的。但是如果把旋轉角固定在$[0,2\pi ]$之間，那麼李群和李代數的元素就是一一對應的。

$SE(3)$上的指數對映

下面是推導結果：

$$\exp ({\xi }^{\wedge })=\left[\begin{array}{cc}\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& \sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\rho \\ 0^T& 1\end{array}\right]=\mathbf{T}$$

其中$exp({\xi }^{\wedge })$的左上角是SO(3)的元素，即旋轉部分。矩陣$\mathbf{J}$則為：

$$\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$

這個公式與落地裡格斯公式類似，$\xi$中的平一部分經過$\rho$指數變換後，發生了一次以$\mathbf{J}$為係數的線性變換。

相反的，$SE(3)$到$se(3)$也有相應的對數對映，但是我們一般不用，我們一般利用左上角的旋轉矩陣計算出旋轉向量，再用右上角的平移向量得到$\rho$。

李代數求導與擾動模型

李代數與李群的計算對應關係

我們由於李群沒有加法討論出來了李代數，我們想要利用李代數的加法定義李群的導數，再利用指數對映和對數對映完成變換關係。但是還有一個基本問題，就是在李代數上做加法，是否就等價於在李群上做乘法呢？

$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp({({\varphi }_1+{\varphi }_2)}^{\wedge })$$

上式在標量的情況下顯然是成立的，但是現在指數上式矩陣，結果很遺憾是不成立的。我們由BCH(Baker-Campbell-Hausdorff)公式給出：

$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{12}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\dots$$

其中$[\cdot ]$為李括號，可以看到，兩個矩陣指數的乘積結果的指數項，是兩個矩陣之和再加上一些由李括號組成的餘項。

我們在求導數的時候，其中一項肯定是小量，可以看成相機連續兩幀之間的位姿變化，利用這個小量，我們就可以對上邊的式子進行優化：

$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+\varphi 2,當{\varphi }_1為小量\\ {\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1,當{\varphi }_2為小量\end{array}$$

上邊的優化結果分別對應著${\varphi }_1$和${\varphi }_2$為小量時的近似狀態，我們分別稱為左乘模型和右乘模型。之所以要區分做成還是右乘模型，是因為其中的${\mathbf{J}}_l$和${\mathbf{J}}_r$是不一樣的。對於左乘BCH近似模型，雅可比矩陣${\mathbf{J}}_l$就是：

$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$

它的逆為：

$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

而對於右乘模型，右乘雅可比矩陣就是將左乘雅可比矩陣的自變數取負號。

$${\mathbf{J}}_r(\varphi )={\mathbf{J}}_l(\varphi )$$

現在來小結一下，假如有一個旋轉矩陣$\mathbf{R}$，對應的李代數為$\varphi$，現在對它有一個微小的擾動，記作$\mathbf{\Delta }\mathbf{R}$，對應的李代數為$\Delta \varphi$。那麼在李群上，新的旋轉矩陣即為$\delta \mathbf{R}\cdot \mathbf{R}$，而在李代數上，新的李代數為${\mathbf{J}}_l^{-1}\Delta \varphi +\varphi$，即：

$$\exp ({\varphi }^{\wedge })\exp (\Delta {\varphi }^{\wedge })=\exp ((\varphi +{\mathbf{J}}_l(\varphi {)}^{-1}\Delta \varphi {)}^{\wedge })$$

反之，如果我們在李代數上做加法，那麼可以近似為李群上帶左/右雅可比矩陣的乘法：

$$exp((\varphi +\Delta \varphi {)}^{\wedge })=exp(({\mathbf{J}}_{\mathbf{l}}\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp({\varphi }^{\wedge })exp(({\mathbf{J}}_{\mathbf{r}}\Delta \varphi {)}^{\wedge })$$

至此，我們可以討論一下怎麼求沒有良好定義加法的李群上的導數：

1. 利用李代數表示位子，然後根據李代數的加法對李代數進行求導。（導數模型）
2. 對李群來左乘或右乘微小擾動，並對這個擾動的李代數求導。（擾動模型）

求導模型

假設空間點$\mathbf{p}$進行了一次旋轉$\mathbf{R}$，得到了新的點$\mathbf{R}\mathbf{p}$，要計算旋轉後的點相對於旋轉的導數，可以不嚴謹得寫為：

$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{R}}$$

設$\mathbf{R}$對應的李代數為$\varphi$，就可以寫成：

$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{R}}=\frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }$$

按照導數的定義並利用一階泰勒展開以及BCH公式，就可以得到：

$$\frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l$$

這個公式時直接對旋轉矩陣$\mathbf{R}$進行求導，最後結果含有左雅可比矩陣${\mathbf{J}}_{\mathbf{l}}$。當然，我們自然希望避免計算雅可比矩陣，所以我們一般選用下邊的擾動模型。

擾動模型

假設某空間點$\mathbf{p}$ 經過了一次變換$\mathbf{T}$，對應的李代數為$\xi$，得到$\mathbf{T}\mathbf{p}$。給 $\mathbf{T}$左乘一個微小擾動$\mathbf{\Delta }\mathbf{T}$，擾動項的李代數為$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$。則有：

$$\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \xi }=\underset{\delta \xi \to 0}{\lim }\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}_{3\times 3}^{\wedge }\\ 0_{1\times 3}^T& 0_{1\times 3}^T\end{array}\right]=(\mathbf{T}\mathbf{p}{)}^{⨀}$$

最後結果被定義為了運算子號$⨀$，把一個齊次座標下的空間點變換成一個4*6 的矩陣。