李群李代数基础

李群李代数基础

本文档推导了表示二维空间和三维空间中转换的李群常用公式。

李群 一个拓扑群，它也是一个光滑流形，还有一些其它的良好性质。与每个李群相关的是其李代数，它是下文中我们将要讨论的一个向量空间。重要的是，一个李群和它的李代数时密切相关的，允许李群和李代数之间的映射能进行有效的计算。

本文档没有对李群进行严格的介绍，也没有讨论李群的所有数学细节。它试图提供足够的信息，表示空间变换的李群可以在机器人和计算机视觉等领域中使用。

以下是本文档涉及的李群：

群	描述	维度	矩阵表示
SO(2)	二维旋转	1	二维旋转矩阵
SE(2)	二维刚体变换	3	同态3维向量上的线性变换
Sim(2)	二维相似变换（刚体运动+尺度）	4	同态3维向量上的线性变换
Aff(2)	二维仿射变换	6	二维仿射变换群
SO(3)	三维旋转	3	三维旋转矩阵
SE(3)	三维刚体变换	6	同态4维向量上的线性变换
Sim(3)	三维相似变换（刚体运动+尺度）	7	同态4维向量上的线性变换
SL(3)		8	单位行列（unit-determinant)式线性变换

对于每一个这样的群，本文档描述了它们的表示形式以及和李代数之间的转换关系，并推导了它们的指数映射、对数映射和伴随关系。

符号约定

$\wedge$: $alg$	将向量映射为李代数表示的矩阵 ，向量中的关于旋转的分量有时在第二个有时在第一个！
$\vee$: $al{g}^{-1}$	将李代数表示的矩阵映射为向量
exp	标量的指数映射
log	标量的对数映射
Exp	矩阵的指数映射，特别是将李代数表示的矩阵映射为李群表示的矩阵
Log	矩阵的对数映射，特别是将李群表示的矩阵映射为李代数表示的矩阵
$\mathfrak{g}$	李代数
$\mathbf{G}$	李群

1. 机器人和计算机视觉中为什么使用李群？

机器人技术和计算机视觉中的许多问题都设计到三维几何中的计算（operation）和估计。没有一个连贯和鲁棒的框架来表示和处理三维变换，这些任务既繁重又巨大。变换必须被结合，求逆，微分和内插。李群及其相关的机制解决了所有这些运算（operation）并且以有原则的方式完成，因此，一旦这样的直觉得到发展，它就能很好地被遵循。

2. 一般性质

2.1 矩阵李群

李群$G$既是一个光滑可微流形又是一个群。本文处理的李群均为实矩阵群；群元素可以表示为${\mathbb{R}}^{n\times n}$中的矩阵。群的乘法运算和逆运算等价于矩阵的乘法运算和逆运算。由于每个群可以由非奇异$n\times n$矩阵的一个特定子类表示，因此其自由度小于$n^2$。

2.2 李代数

每个李群都有一个相关的李代数，它是群在单位元附近的切空间。也就是说，李代数是一个向量空间，它是沿切线方向上的群变换在恒等元处进行微分得到的。切空间在所有群元素上都有相同的结构，每个群元素的切空间之间相互同构，因此我们可以仅仅研究单位元处的切空间。而切向量从一个切空间移动到另外一个切空间时会进行坐标变换。

考虑${\mathbb{R}}^{n\times n}$中表示的一个具有k自由度的李群$G$。李代数$\mathfrak{g}$是李群G在单位元附近的微分变换的空间-切空间。这个切空间是一个k维向量空间，它的基元素是$\left\{G_1,\cdots ,G_k\right\}:生成元$。李代数$\mathfrak{g}$的元素在中表示为矩阵，但是是在加法和标量乘法下，而不是在矩阵乘法下。李代数的基本元素称为生成元，所有切向量可以表示为生成元的线性组合。

2.3 non-commutativity

2.3.1 Adjoint representation

The Adjoint representation of G on ${\mathbb{R}}^n$ is defined as the operator $A{d}_G$:
$$A{d}_G(X)a=[X[a{]}_G^{\wedge }{X}^{-1}{]}_G^{\vee },\text{X∈G and a,b∈Rn}$$
The Adjoint representation of G on $\mathfrak{g}$ is defined as the linear operator $A{d}_G$, it captures properties related to commutation:
$${\exp }_G([A{d}_G(X)a{]}_G^{\wedge })X={\exp }_G(X[a{]}_G^{\wedge }{X}^{-1})X=X{\exp }_G([a{]}_G^{\wedge }),\text{X∈G and a,b∈Rn}$$
The Adjoint representation $A{d}_G\subset {\mathbb{R}}^{n\times n}$ of G on ${\mathbb{R}}^n$ is defined as
$${\exp }_G([a{]}_G^{\wedge })X=X{\exp }_G([A{d}_G(X^{-1})a{]}_G^{\wedge })$$

2.3.2 adjoint representation

The adjoint representation of G on ${\mathbb{R}}^n$ is defined as the operator $a{d}_G$:
$$a{d}_G(a)b=[[a{]}_G^{\wedge }[b{]}_G^{\wedge }-[b{]}_G^{\wedge }[a{]}_G^{\wedge }{]}_G^{\vee }$$

3. SO(2)

3.1 描述及表示

SO(2)是二维平面上的旋转群。它的自由度为1：旋转角度。该群是可交换群。它的逆是它的转置：
$$\begin{array}{rl}X& \in SO(2)\subset {\mathbb{R}}^{2\times 2}\\ X^{-1}& =X^T\end{array}$$

3.2 李代数

李代数$\mathfrak{s}\mathfrak{o}(2)$由反对称元素生成，对应于可微的旋转生成元（连续旋转）：
$$\begin{array}{r}{G}_1=\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)\end{array}$$

李代数中的元素可以由生成元乘以一个标量得到：
$$\begin{array}{r}\theta \in \mathbb{R}\\ \theta G_1\in \mathfrak{s}\mathfrak{o}(2)\end{array}$$

3.3 指数映射和对数映射

从李代数$\mathfrak{s}\mathfrak{o}(2)$到李群$SO(2)$的指数映射是一个矩阵的指数映射，其结果是一个二维平面的旋转矩阵：
$$\begin{array}{rl}Exp(alg(\theta ))& =Exp\left(\begin{array}{cc}0& -\theta \\ \theta & 0\end{array}\right)\\ & =I+{\theta }_{\wedge }+\frac{1}{2!}{\theta }_{\wedge }^2+\frac{1}{3!}{\theta }_{\wedge }^3+\cdots \\ & =I+\left(\begin{array}{cc}0& -\theta \\ \theta & 0\end{array}\right)+\frac{1}{2!}\left(\begin{array}{cc}-{\theta }^2& 0\\ 0& -{\theta }^2\end{array}\right)+\frac{1}{3!}\left(\begin{array}{cc}0& {\theta }^3\\ -{\theta }^3& 0\end{array}\right)+\cdots \\ & =\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \cos \theta & \cos \theta \end{array}\right)\end{array}$$
对数映射可以很简单的通过$SO(2)$的元素通过反三角函数得到。

3.4 伴随表示

SO(2)的伴随表示是平凡的（由于平面上的旋转是可交换的，因此SO(2)的伴随表示是恒等元）：
$$\begin{array}{rlr}X=\left(\begin{array}{cc}a& -b\\ b& a\end{array}\right)& \in SO(2),a^2+b^2=1& \\ Ad{j}_X(alg(\theta ))& =X\cdot alg(\theta )\cdot X^{-1}& \text{定义2}\\ & =\left(\begin{array}{cc}0& -\theta \\ \theta & 0\end{array}\right)& \\ & =alg(\theta )& \\ ⟹Ad{j}_X& =I& \end{array}$$

如果按照定义3来计算，则$alg(Ad{j}_X(\theta ))=alg(\theta )\to Ad{j}_X=1$。

因为平面中旋转变换是可交换的，因此SO(2)的伴随作用是恒等函数（identity function）。

4. SE(2)

4.1 描述及表示

SE(2)是二维平面的刚体变换群，也是半直积$SO(2)⋉{\mathbb{R}}^2$。它有三个自由度：平移对应2个自由度，旋转对应1个自由度。它的子群包括$SO(2)$。
$$\begin{array}{rl}R& \in SO(2)\\ t& \in {\mathbb{R}}^2\\ X=\left(\begin{array}{cc}R& t\\ 0_{1\times 2}& 1\end{array}\right)& \in SE(2)\subset {\mathbb{R}}^{3\times 3}\\ X^{-1}& =\left(\begin{array}{cc}{R}^T& -R^{T}t\\ 0_{1\times 2}& 1\end{array}\right)\end{array}$$

4.2 李代数

李代数$\mathfrak{s}\mathfrak{e}(2)$有三个生成元
$$\begin{array}{r}{G}_1=\left(\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\end{array}\right),G_2=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),G_3=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)\end{array}$$

李代数中的元素可以表示成生成元的线性组合：
$$\begin{array}{r}(u_1,u_2,\theta {)}^T\in {\mathbb{R}}^3\\ u_1{G}_1+u_2{G}_2+\theta G_3=\left(\begin{array}{ccc}0& -\theta & u_1\\ \theta & 0& u_2\\ 0& 0& 0\end{array}\right)\in \mathfrak{s}\mathfrak{e}(2)\end{array}$$

4.3 指数映射和对数映射

从李代数$\mathfrak{s}\mathfrak{e}(2)$到李群SE(2)的指数映射有一个闭合形式：
$$\begin{array}{rl}v& =\left(\begin{array}{c}\theta \\ x\\ y\end{array}\right)\in {\mathbb{R}}^3\\ R& =\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \cos \theta & \cos \theta \end{array}\right)\\ V& =\left(\begin{array}{cc}\frac{\sin \theta }{\theta }& -\frac{1-\cos \theta }{\theta }\\ \frac{1-\cos \theta }{\theta }& \frac{\sin \theta }{\theta }\end{array}\right)\\ Exp(alg(v))=Exp\left(\begin{array}{ccc}0& -\theta & x\\ \theta & 0& y\\ 0& 0& 0\end{array}\right)& =\left(\begin{array}{cc}R& V\cdot \left(\begin{array}{c}x\\ y\end{array}\right)\\ 0_{1\times 2}& 1\end{array}\right)\end{array}$$
V是R的左雅克比矩阵。当$\theta$是小量的时候，V的每个元素可以通过泰勒级数展开求解。

4.4 伴随表示

$$\begin{array}{rl}X& =\left(\begin{array}{cc}R& t\\ 0_{1\times 2}& 1\end{array}\right)=\left(\begin{array}{cc}R& \left(\begin{array}{c}{t}_1\\ t_2\end{array}\right)\\ 0_{1\times 2}& 1\end{array}\right)\in SE(2)\\ Ad{j}_X& =\left(\begin{array}{cc}\left(\begin{array}{c}{t}_2\\ -t_1\end{array}\right)& R\\ 1& 0_{1\times 2}\end{array}\right)\end{array}$$

如果$v=\left(\begin{array}{c}x\\ y\\ \theta \end{array}\right)\in {\mathbb{R}}^3$，则$Ad{j}_X=\left(\begin{array}{cc}R& \left(\begin{array}{c}{t}_2\\ -t_1\end{array}\right)\\ 0_{1\times 2}& 1\end{array}\right)$。

5. Sim(2)

5.1 描述及表示

Sim(2)是二维平面上保持方向相似变换的群，也是半直积$SE(2)⋊{\mathbb{R}}^{\ast }$。它有4个自由度：平移有2个自由度，旋转有1个自由度，尺度对应1个自由度。它的子群包括：$SE(2)$和$R^{\ast }$。
$$\begin{array}{rl}R& \in SO(2)\\ t& \in {\mathbb{R}}^2\\ X=\left(\begin{array}{cc}R& t\\ 0_{1\times 2}& s^{-1}\end{array}\right)& \in Sim(2)\subset {\mathbb{R}}^{3\times 3}\\ X^{-1}& =\left(\begin{array}{cc}{R}^T& -s{R}^{T}t\\ 0_{1\times 2}& s\end{array}\right)\end{array}$$

5.2 李代数

李代数$\mathfrak{s}\mathfrak{i}\mathfrak{m}(2)$有4个生成元：
$$\begin{array}{r}{G}_1=\left(\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\end{array}\right),G_2=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),G_3=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)，G_4=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& -1\end{array}\right)\end{array}$$

5.3 指数映射和对数映射

从李代数$\mathfrak{s}\mathfrak{i}\mathfrak{m}(2)$到李群$Sim(2)$的指数映射得到一个闭合形式结果：
$$\begin{array}{rl}v& =\left(\begin{array}{c}x\\ y\\ \theta \\ \lambda \end{array}\right)\in {\mathbb{R}}^4\\ & \\ R& =\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \cos \theta & \cos \theta \end{array}\right)\\ A& =\frac{\sin \theta }{\theta }\\ B& =\frac{1-\cos \theta }{\theta }\\ C& =\frac{\theta -\sin \theta }{{\theta }^3}\\ \alpha & =\frac{{\lambda }^2}{{\lambda }^2+{\theta }^2}\\ s& =e^{\lambda }\\ X& =\alpha (\frac{1-s^{-1}}{\lambda })+(1-\alpha )(A-\lambda B)\\ Y& =\alpha (\frac{s^{-1}-1+\lambda }{{\lambda }^2})+(1-\alpha )(B-\lambda C)\\ V& =\left(\begin{array}{cc}X& -\theta Y\\ \theta Y& X\end{array}\right)\\ Exp(alg(v))=Exp\left(\begin{array}{ccc}0& -\theta & x\\ \theta & 0& y\\ 0& 0& -\lambda \end{array}\right)& =\left(\begin{array}{cc}R& V\cdot \left(\begin{array}{c}x\\ y\end{array}\right)\\ 0_{1\times 2}& s^{-1}\end{array}\right)\end{array}$$
当$\theta$和$\lambda$比较小的时候，V中的元素可以通过泰勒级数展开计算。

5.4 伴随表示

$$\begin{array}{rl}X& =\left(\begin{array}{cc}R& t\\ 0_{1\times 2}& s^{-1}\end{array}\right)=\left(\begin{array}{cc}R& \left(\begin{array}{c}{t}_1\\ t_2\end{array}\right)\\ 0_{1\times 2}& s^{-1}\end{array}\right)\in Sim(2)\\ Ad{j}_X& =\left(\begin{array}{cc}sR& s\left(\begin{array}{cc}{t}_2& -t_1\\ -t_1& -t_2\end{array}\right)\\ 0_{2\times 2}& I_{2\times 2}\end{array}\right)\end{array}$$

6. $Aff(2)$

6.1 描述及表示、

$Aff(2)$是二维平面上的一个仿射变换群。它有6个自由度：平移对应2个自由度，旋转对应1个自由度，缩放（尺度，scale）对应1个自由度，拉伸（stretch）对应1个自由度，剪切（shear）对应1个自由度。它的子群包括$Sim(2)$。
$$\begin{array}{rl}X=\left(\begin{array}{cc}A& t\\ 0_{1\times 2}& 1\end{array}\right)& \in Aff(2)\subset {\mathbb{R}}^{3\times 3}\\ X^{-1}& =\left(\begin{array}{cc}{A}^T& -A^{T}t\\ 0_{1\times 2}& 1\end{array}\right)\end{array}$$

6.2 李代数

李代数$\mathfrak{a}\mathfrak{f}\mathfrak{f}(2)$有6个生成元：
$$\begin{array}{r}{G}_1=\left(\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\end{array}\right),G_2=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& 0& 0\end{array}\right),G_3=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)，\\ G_4=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right),G_5=\left(\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& 0\end{array}\right),G_6=\left(\begin{array}{ccc}0& 1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)\end{array}$$

6.3 指数映射和对数映射

从李代数$\mathfrak{a}\mathfrak{f}\mathfrak{f}(2)$到李群$Aff(2)$的指数映射没有闭合形式。它可以用任何一般的矩阵指数例程计算。对数映射也是一样的。

6.4 伴随表示

记
$$\begin{array}{rl}X=\left(\begin{array}{cc}A& t\\ 0_{1\times 2}& 1\end{array}\right)=\left(\begin{array}{ccc}a& b& x\\ c& d& y\\ 0& 0& 1\end{array}\right)& \in Aff(2)\subset {\mathbb{R}}^{3\times 3}\\ E& =\left(\begin{array}{cc}A& t\\ 0_{1\times 2}& 1\end{array}\right)\end{array}$$
则
$$\begin{array}{rl}Ad{j}_X& =E^T\left(\begin{array}{cc}A& -T\cdot C\\ 0_{2\times 4}& C\end{array}\right)E\end{array}$$
明确写出乘积就是：
$$\begin{array}{rl}Ad{j}_X& =\left(\begin{array}{cccccc}a& b& fy(a^2+b^2)-fx(ac+bd)& -x& fy(2ab)-fx(ad+bc)& fx(ac-bd)-fy(a^2-b^2)\\ c& d& fy(ac+bd)-fx(c^2+d^2)& -y& fy(ad+bc)-fx(2cd)& fx(c^2-d^2)-fy(ac-bd)\\ 0& 0& \frac{f}{2}(s^2+b^2+c^2+d^2)& 0& f(ab+cd)& \frac{f}{2}(-a^2+b^2-c^2+d^2)\\ 0& 0& 0& 1& 0& 0\\ 0& 0& f(ac+bd)& 0& f(ad+bc)& f(bd-ac)\\ 0& 0& \frac{f}{2}(-a^2-b^2+c^2+d^2)& 0& f(cd-ab)& \frac{f}{2}(a^2-b^2-c^2+d^2)\end{array}\right)\end{array}$$

7. SO(3)

7.1 描述及表示

三维旋转群的每个元素$SO(3)$可以由行列式等于1的三维旋转矩阵表示。它的自由度为3：每个旋转轴对应1个自由度。群元素的组合和逆对应于矩阵的乘法和逆运算。因为旋转矩阵是正交的，所以它的逆等于它的转置。
$$\begin{array}{rlr}R& \in SO(3)\subset {\mathbb{R}}^{3\times 3}& \\ R^{-1}& =R^T& \text{invSO3}\\ \det (R)& =1& \end{array}$$

7.2 李代数

李代数$\mathfrak{s}\mathfrak{o}(3)$是所有$3\times 3$反对称矩阵的集合，它由各轴的微分旋转产生：
$$\begin{array}{r}{G}_1=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right),G_2=\left(\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right),G_3=\left(\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right)\end{array}$$
李代数$\mathfrak{s}\mathfrak{o}(3)$的所有元素可以写成生成元的线性组合：
$$\begin{array}{rl}w& \in {\mathbb{R}}^3\\ w_1{G}_1+w_2{G}_2+w_3{G}_3& \in \mathfrak{s}\mathfrak{o}(3)\end{array}$$
映射$alg:{\mathbb{R}}^3\to \mathfrak{s}\mathfrak{o}(3)$将三维向量映射为反对称矩阵：
$$\begin{array}{rlr}w& =\left(\begin{array}{c}a\\ b\\ c\end{array}\right)& \\ alg(w)& =w_{\wedge }& \text{HatSO3}\\ & =\left(\begin{array}{ccc}0& -c& b\\ b& 0& -a\\ -b& a& 0\end{array}\right)& \end{array}$$

dualHatSO3: w_hat * x = x_dual_hat * w x_dual_hat=$-HatSO3(x)$

李代数到向量的映射VecSO3为:$w=(w_{\wedge }(3,2);w_{\wedge }(1,3);w_{\wedge }(2,1))$

normalizeSO3: $[u,s,v]=svd(R);Rnorm=v\ast v’$

PhiSO3:
$$\begin{gathered} Phiw=I_{3\times 3}+\frac{1}{2||w|{|}^2}(4-||w||\sin (||w||)-4\cos (||w||))\ast a{d}_{SO3}(w) \\ +\frac{1}{2||w|{|}^3}(4||w||-5\sin (||w||)+||w||\cos (||w||))\ast a{d}_{SO3}(w{)}^2 \\ +\frac{1}{2||w|{|}^4}(2-||w||\sin (||w||)-2\cos (||w||))\ast a{d}_{SO3}(w{)}^3 \\ +\frac{1}{2||w|{|}^5}(2||w||-3\sin (||w||)+||w||\cos (||w||))\ast a{d}_{SO3}(w{)}^4 \end{gathered}$$

7.3 指数映射和对数映射

从李代数到李群的指数映射有闭合形式，该闭合形式也称为罗德里格斯公式。切向量w可以解释为旋转的轴角表示：它的指数是围绕轴$\frac{w}{||w||}$旋转$||w||$弧度：
$$\begin{array}{rl}w& \in {\mathbb{R}}^3\\ \theta & =\sqrt{w^{T}w}=||w||\\ w_{\wedge }^3& =-{\theta }^2{w}_{\wedge }\\ Exp(alg(w))& =Exp(w_{\wedge })\\ & =I+w_{\wedge }+\frac{1}{2!}{w}_{\wedge }^2+\frac{1}{3!}{w}_{\wedge }^3+\cdots \\ & =I+(\frac{\sin \theta }{\theta })w_{\wedge }+(\frac{1-\cos \theta }{{\theta }^2})w_{\wedge }^2\end{array}$$
当$\theta$较小时，R的系数可以由泰勒级数计算。

从李群SO(3)到李代数$\mathfrak{s}\mathfrak{o}(3)$的对数映射可以计算如下：给定一个旋转矩阵$R\in SO(3)$，对数映射可以先确定$\cos \theta =\frac{1}{2}(tr(R)-1)$，然后计算$w$，w可以从$Log(R)$中的非对角线元素中取下来合成该向量。
$$\begin{array}{rl}R& \in SO(3)\\ \theta & =\arccos (\frac{tr(R)-1}{2})\\ Log(R)& =\frac{\theta }{2\sin \theta }\cdot (R-R^T)\end{array}$$
当$\theta$较小时，$\frac{\theta }{2\sin \theta }$的系数可以由泰勒级数展开计算。

7.4 伴随表示

对于三维空间中的旋转群$SO(3)$以及李代数$w_{\wedge }$，考虑伴随表示的计算公式
$$\begin{array}{rl}R& \in SO(3)\\ alg(w)& \in \mathfrak{s}\mathfrak{o}(3)\\ Exp[alg(Ad{j}_R\cdot w)]& =R\cdot Exp[alg(w)]\cdot R^{-1}\\ alg(Ad{j}_R\cdot w)& =R\cdot alg(w)\cdot R^{-1}\end{array}$$
由于叉积的性质，SO(3)的伴随表示实际上与旋转矩阵表示相同：
$$\begin{array}{rlr}R& \in SO(3)& \\ a,b,c& \in {\mathbb{R}}^3& \\ a=b\times c& ⟺(Ra)=(Rb)\times (Rc)& \\ & ⟺a=R^{-1}(Rb)\times (Rc)& \\ & ⟺R^{-1}(Rb)\times R=b\times & \\ & ⟹R{b}_{\wedge }{R}^T=(Rb{)}_{\wedge }& \\ alg(b)& \in \mathfrak{s}\mathfrak{o}(3)& \\ Exp[alg(Ad{j}_R(b))]& =R\cdot Exp[alg(b)]\cdot R^{-1}& \text{定义3}\\ alg(Ad{j}_R(b))& =R\cdot alg(b)\cdot R^{-1}=R\cdot (\sum _{i=1}^3{b}_i{G}_i)\cdot R^{-1}=alg(Rb)& \\ Ad{j}_R(b)& =Rb& \\ Ad{j}_R& =R& \end{array}$$

ad表示：$ad=a{d}_{S0(3)}(w)=HatSO3(w)$

Ad表示：$Ad=A{d}_{SO(3)}(R)=R$

7.5 雅克比

7.5.1 李群$SO(3)$作用于空间${\mathbb{R}}^3$的微分

考虑李群中的一个元素$R\in SO(3)$，同时$x\in {\mathbb{R}}^3$。通过旋转矩阵R来旋转向量x，结果为向量左乘旋转矩阵：
$$y=f(R,x)=Rx$$
对向量的微分很简单，因为f在x中是线性的：
$$\frac{\partial y}{\partial x}=R$$
对旋转参数的微分是通过隐式左乘旋转切向量的指数并在零附近扰动表达式进行微分实现的。这等价于生成元左边乘以y得到的：
$$\begin{array}{rl}\frac{\partial y}{\partial R}& =\frac{\partial }{\partial w}{|}_{w=0}(exp(w)\cdot R)\cdot x\\ & =\frac{\partial }{\partial w}{|}_{w=0}exp(w)\cdot (R\cdot x)\\ & =\frac{\partial }{\partial w}{|}_{w=0}exp(w)\cdot y\\ & =(G_{1}y|G_{2}y|G_{3}y)\\ & =-y_{\wedge }\end{array}$$

7.5.2 通过群中的一个参数对群-值函数微分

考虑一个群G和群上的一个映射$f:G\to G$。由于定义域和值域都不是向量空间，通过切空间扰动在输入变量端引入参数，在结果端也可以引入参数，然后我们通过微分记号可以得到从输入扰动到输出扰动的一个映射关系：
$$\begin{array}{rl}\exp (ϵ)\cdot f(g)& =f(\exp (\delta )\cdot g)\\ \frac{\partial f}{\partial g}& \equiv \frac{\partial ϵ}{\partial \delta }{|}_{\delta =0}\end{array}$$
求解$ϵ$并且微分得到输入扰动$\delta$对输出扰动$ϵ$微分的显示表达式：
$$\begin{array}{rl}ϵ& =\log (f(\exp (\delta )\cdot g)\cdot f(g{)}^{-1})\\ \frac{\partial f}{\partial g}& \equiv \frac{\partial \log (f(\exp (\delta )\cdot g)\cdot f(g{)}^{-1})}{\partial \delta }{|}_{\delta =0}\end{array}$$
上式产生了一个从参数的左切空间扰动到结果的左切空间扰动的映射。正如所料，如果将这个微分简写应用于恒等函数$f(g)=g$，就得到了单位矩阵。

对于此过程的一个重要示例应用，考虑群$G=SO(3)$中的元素与第二个因子$R_0$相乘：
$$\begin{array}{r}{R}_2=f(R_0)\equiv R_1\cdot R_0\end{array}$$
首先，切空间so(3)中的输入扰动和输出扰动都是明确的：
$$\begin{array}{r}\exp (ϵ)\cdot R_2=R_1\cdot \exp (w)\cdot R_0\end{array}$$
输入扰动w对$ϵ$的微分是在$w=0$附近进行的。伴随映射将切向量移动到表达式的左边。表达式的其余部分抵消，结果很简单。
$$\begin{array}{rl}\frac{\partial R_2}{\partial R_0}& \equiv \frac{\partial \log ((R_1\cdot \exp (w)\cdot R_0)\cdot (R_1\cdot R_0{)}^{-1})}{\partial w}{|}_{w=0}\\ & =\frac{\partial }{\partial w}{|}_{w=0}\log (\exp (Ad{j}_{R_1}\cdot w\cdot R_1\cdot R_0)\cdot (R_1\cdot R_0{)}^{-1})\\ & =\frac{\partial }{\partial w}{|}_{w=0}[\log (\exp (Ad{j}_{R_1}\cdot w))]\\ & =\frac{\partial }{\partial w}{|}_{w=0}[Ad{j}_{R_1}\cdot w]=Ad{j}_{R_1}=R_1\end{array}$$

7.6 SO(3)上的高斯分布

7.6.1 采样

我们可以对三维旋转的高斯分布进行规定：用SO(3)中的元素表示均值，用so(3)中的一个切向量的二次形式表示协方差。更精确的说，考虑高斯分布的均值$R\in SO(3)$，协方差为$\Sigma \in {\mathbb{R}}^{3\times 3}$。我们可以通过在切空间中对零均值分布采样，然后用它左乘均值，从而从分布中采样得到一个旋转S：
$$\begin{gathered} ϵ\in N(0,\Sigma ) \\ S=\exp (ϵ)\cdot R \end{gathered}$$

7.6.2 不确定旋转的合成

给定了旋转上的两个高斯分布，利用伴随变换可以构造出两个不确定变换。记其中一个零均值方程对为$(R_0,{\Sigma }_0)$，另外一个记为$R_1,{\Sigma }_1$。那么先经过$R_0$变换，再经过$R_1$旋转得到的旋转的分布可以记为：
$$(R_1,{\Sigma }_1)\circ (R_0,{\Sigma }_0)=(R_1\cdot R_0,{\Sigma }_1+R_1\cdot {\Sigma }_0\cdot R_1^T)$$

7.6.3 旋转估计的贝叶斯组合

这两个高斯分布的信息矩阵$(R_c,{\Sigma }_c)$可以通过贝叶斯方法组合得到：首先找出切空间中两个向量之间的偏差，然后对两个向量的估计通过信息矩阵进行加权。信息矩阵（也即协方差矩阵的逆）可以像往常一样计算：
$$\begin{array}{rlr}\Sigma & =({\Sigma }_0^{-1}+{\Sigma }_1^{-1}{)}^{-1}={\Sigma }_0-{\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}{\Sigma }_0& \text{协方差更新}\\ v& =R_1\ominus R_0=Log(R_1\cdot R_0^{-1})& \text{新息}\\ R_c& =Exp({\Sigma }_c\cdot {\Sigma }_1^{-1}\cdot v)\cdot R_0& \text{均值更新}\end{array}$$

7.7 SO(3)上的EKF

两个不确定旋转的组合可以看作EKF的动力学更新过程，其中$(R_0,{\Sigma }_0)$看作是状态的先验信息，$(R_1,{\Sigma }_1)$是系统的动力学模型。

旋转估计的贝叶斯组合可以看作是EKF的量测过程中的均值更新（假设量测更新雅克比矩阵是单位矩阵，量测矩阵为单位矩阵）和协方差更新。而切向量v是EKF量测更新过程中的新息。

如果量测矩阵为单位矩阵，则EKF的卡尔曼滤波增益K计算如下：
$$K={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}$$
所以卡尔曼更新可以写成如下标准形式：
$$\begin{array}{rlr}\Sigma & =(I-k){\Sigma }_0& \text{协方差更新}\\ R_c& =R_o\oplus (K\cdot v)=Exp(K\cdot v)\cdot R_0& \text{均值更新}\end{array}$$
j将上述方程记为标准EKF框架，给出的状态协方差为${\Sigma }_0$，量测噪声为${\Sigma }_1$。

如果雅克比矩阵是一个非平凡的测量，则需要对所给的方程做一个简单的修改。

8. SE(3)

8.1 描述及表示

SE(3)是三维空间中的刚体变换群，同时也是半直积：$SO(3)⋉{\mathbb{R}}^3$。它有6个自由度：三维空间平移对应3个自由度，三维空间旋转对应3个自由度。它的子群包括：$SE(2)$和$SO(3)$。
$$\begin{array}{rlr}R& \in SO(3)& \\ t& \in {\mathbb{R}}^3& \\ X=\left(\begin{array}{cc}R& t\\ 0_{1\times 3}& 1\end{array}\right)& \in SE(3)\subset {\mathbb{R}}^{4\times 4}& \text{HatSE3}\\ X_1,X_2\in SE(3)& & \\ X_1\cdot X_2=\left(\begin{array}{cc}{R}_1& t_1\\ 0_{1\times 3}& 1\end{array}\right)\left(\begin{array}{cc}{R}_2& t_2\\ 0_{1\times 3}& 1\end{array}\right)& =\left(\begin{array}{cc}{R}_1{R}_2& R_1{t}_2+t_1\\ 0_{1\times 3}& 1\end{array}\right)& \\ X^{-1}& =\left(\begin{array}{cc}{R}^T& -R^{T}t\\ 0_{1\times 3}& 1\end{array}\right)& \text{invSE3}\end{array}$$

8.2 李代数

李代数$\mathfrak{s}\mathfrak{e}(3)$有6个生成元：
$$\begin{array}{r}{G}_1=\left(\begin{array}{cccc}0& 0& 0& 1\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right),G_2=\left(\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right),G_3=\left(\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right)，\\ G_4=\left(\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ 0& 0& 0& 0\end{array}\right),G_5=\left(\begin{array}{cccc}0& 0& -1& 0\\ 0& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right),G_6=\left(\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right)\end{array}$$
因此映射$alg:{\mathbb{R}}^3\to \mathfrak{s}\mathfrak{e}(3)$：
$$\begin{array}{rl}u,w& \in {\mathbb{R}}^3\\ u_1{G}_1+u_2{G}_2+u_3{G}_3+w_1{G}_4+w_2{G}_5+w_3{G}_6& \in \mathfrak{s}\mathfrak{e}(3)\\ alg\left(\begin{array}{c}u\\ w\end{array}\right)& =\left(\begin{array}{cc}{w}_{\wedge }& u\\ 0_{1\times 3}& 0\end{array}\right)\end{array}$$

李代数映射为向量$VecSE3$为: $m(4:6)=m_{hat}(1:3,4);m(1:3)=VecSO3(m_{hat}(1:3,1:3));$

8.3 指数映射和对数映射

李代数$\mathfrak{s}\mathfrak{e}(3)$到李群SE(3)的指数映射有一个闭合形式：
$$\begin{array}{rlr}w,u& \in {\mathbb{R}}^3& \\ \theta & =\sqrt{w^{T}w}=||w||& \\ w_{\wedge }^3& =-{\theta }^2{w}_{\wedge }& \\ Exp(alg\left(\begin{array}{c}u\\ w\end{array}\right))& =Exp\left(\begin{array}{cc}{w}_{\wedge }& u\\ 0_{1\times 3}& 0\end{array}\right)& \\ & =I+\left(\begin{array}{cc}{w}_{\wedge }& u\\ 0_{1\times 3}& 0\end{array}\right)+\frac{1}{2!}\left(\begin{array}{cc}{w}_{\wedge }^2& w_{\wedge }u\\ 0_{1\times 3}& 0\end{array}\right)+\frac{1}{3!}\left(\begin{array}{cc}{w}_{\wedge }^3& w_{\wedge }^{2}u\\ 0_{1\times 3}& 0\end{array}\right)+\cdots & \\ & =\left(\begin{array}{cc}Exp(w_{\wedge })& V\cdot u\\ 0_{1\times 3}& 1\end{array}\right)& \text{expSE3}\\ V& =I+\frac{1}{2!}{w}_{\wedge }+\frac{1}{3!}{w}_{\wedge }^2+\cdots & \\ & =I+(\frac{1}{2!}-\frac{{\theta }^2}{4!}+\frac{{\theta }^4}{6!}+\cdots )w_{\wedge }+(\frac{1}{3!}-\frac{{\theta }^2}{5!}+\frac{{\theta }^4}{7!}+\cdots )w_{\wedge }^2& \\ & =I+(\frac{1-\cos \theta }{{\theta }^2})w_{\wedge }+(\frac{\theta -\sin \theta }{{\theta }^3})w_{\wedge }^2& \end{array}$$
当 θ \theta