李群李代数-(1)

作者: 光能蜗牛 | 来源:发表于2022-06-16 16:52 被阅读0次

李群李代数-(1)
李群李代数
2018-04-21:机器人领域常用C++库及工具介绍
李群与李代数+补充理解
Slam笔记-李群与李代数
李群李代数学习笔记
2019-11-26
李群和李代数（书，讲义和一些总结）
第四讲李群与李代数
李群李代数在SLAM系统中的使用

https://www.cnblogs.com/gaoxiang12/p/5137454.html
https://zhuanlan.zhihu.com/p/358455662
https://zhuanlan.zhihu.com/p/435445206
https://blog.csdn.net/youngpan1101/article/details/71087470

https://blog.csdn.net/qq_32998593/article/details/124684758?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2-124684758-blog-46628023.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2-124684758-blog-46628023.pc_relevant_default&utm_relevant_index=3

$SO(3)$

Special Orthogonal Group ,即特殊正交群，当我们研究的问题是三维空间，则认为是 $SO(3)$ ，而正交群对向量的作用一般都指代的是旋转，所以这样的群也叫旋转群
$SO(3)=\{R\in \mathbb{R}^{n\times n} | R\times R^T=I|det(R)=I\}$ （注： $R$ 表示旋转矩阵， $\mathbb{R}$ 表示实数域）

$SE(3)$

Special Euclidean Group,即特殊欧式群,这里变换矩阵T除了包含旋转R，还多了一个位移t，当研究的问题是三维空间问题，则认为是 $SE(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\}$

啥是群
中学的时候肯定都接触过集合这个概念，比如所有实数的集合 $\mathbb{R}$ ,或者我们可以自己定义一个集合，比如所有正方型的集合，不太严谨的说，其实就规定一些条件，所有满足这些条件的“东西”，就组成了一个集合。

，或者我们可以自己定义一个集合，比如所有正方型的集合，不太严谨的说，其实就规定一些条件，所有满足这些条件的“东西”，就组成了一个集合。

而群（Group），则是一种集合，加上一种运算组成的结构。假如我们把集合记作G，运算记作·，它要求集合和运算满足下面几个条件：

封闭性：对于所有 $G$ 中 $a, b$ ，运算 $a·b$ 的结果也在 $G$ 中
结合律：对于所有 $G$ 中的 $a, b$ 和 $c$ ，等式 $(a·b)·c = a· (b·c)$ 成立
幺元：存在 $G$ 中的一个元素 $e$ ，使得对于所有 $G$ 中的元素 $a$ ，总有等式 $e·a = a·e = a$ 成立
逆元：对于每个 $G$ 中的 $a$ ，存在 $G$ 中的一个元素 $b$ 使得总有 $a·b = b·a = e$ ，此处 $e$ 为单位元

看起来挺复杂，举个例子就很容易整明白了，整数（集合）和加法（运算），就可以构成一个群。

封闭性：对于任何两个整数 $a$ 和 $b$ ，它们的和 $a + b$ 也是整数
结合律：对于任何整数 $a, b$ 和 $c，(a + b) + c = a +（b + c）$
幺元：如果a是任何整数，那么 $0 + a = a + 0 = a$
逆元：对于任何整数a，存在另一个整数b使得 $a + b = b + a = 0$ ，整数 $b$ 叫做整数 $a$ 的逆元，记为 $-a$

啥是李群
简单的说，李群就是连续的群。上面的特殊正交群，特殊欧式群，都是对时间连续的群，都是李群的一种

啥是李代数
李代数是基于李群为基础的一种非结合代数

李代数的引出

我们知道对于李群中的任意一个 $R$ ( $R$ 是关于时间 $t$ 的矩阵 $R(t)$ ，这里简写为 $R$ )
有 $RR^T=I$
两边对 $t$ 求导
有 $R'R^T+R(R^T)'=0$
$R'R^T=-R(R^T)'$
$R'R^T=-(R^{'}R^T)^T$
所以 $R'R^T$ 是一个反对称矩阵
又我们知道反对称矩阵都可以由一个向量表达
具体原因如下
$a=\begin{bmatrix}a1&a2&a3\end{bmatrix}^T$
定义 $a^{\wedge} =\begin{bmatrix}0&-a3&a2\\a3&0&-a1\\-a2&a1&0\end{bmatrix}$

假设 $\varPhi(t)$ 为关于 $t$ 的三维向量，
$\varPhi(t)=\begin{bmatrix}\varPhi_1&\varPhi_2&\varPhi_3\end{bmatrix}^T$

于是可以用 $\varPhi(t)^{\wedge}$ 来表达 $R'R^T$
$\varPhi(t)^{\wedge}=\begin{bmatrix}0&-\phi_3&\phi_2\\\phi_3&0&-\phi_1\\-\phi_2&\phi_1&0\end{bmatrix} =R'R^T$

即 $\varPhi(t)^{\wedge}=R(t)'R(t)^T$
于是 $R(t)'=\varPhi(t)^{\wedge}R(t)$ ，这表明如果要对 $R(t)$ 求导，只需要在 $R(t)$ 前面乘以一个 $\varPhi(t)^{\wedge}$ 即可
可见 $\varPhi$ 反应了 $R$ 的导数性质
简单的说
比如 $t_a$ 时刻
$R(t_a)'=\varPhi(t_a)^{\wedge}R(t_a)$
$t_b$ 时刻
$R(t_b)'=\varPhi(t_b)^{\wedge}R(t_b)$

李代数的定义

李代数是由一个集合 $V$ ,一个数域 $\mathbb{F}$ $($ $\mathbb{F}$ 可以是 $\mathbb{R}$ 实数域或者 $\mathbb{C}$ 复数域 $)$ 和一个二元运算 $[,]$ 组成，如果满足以下性质，则称 $g$ 是域 $\mathbb{F}$ 上的李代数 , $g = (V,\mathbb{F},[,])$

封闭性
$\forall X,Y \in \mathbb{V} 有[X,Y] \in \mathbb{V}$
双线性
$\forall X,Y,Z\in \mathbb{V},a,b\in \mathbb{F},$
有 $[aX+bY,Z]=a[X,Z]+b[Y,Z]$
$[Z,aX+bY]=a[Z,X]+b[Z,Y]$
自反性
$\forall X\in\mathbb{V},[X,X]=0$
雅可比等价
$\forall X,Y,Z\in\mathbb{V},有[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

李代数 $so(3)$

由前面关于李代数的引出部分可知,

$so(3)$ 的元素是3维向量或者3维反对称矩阵，
$so(3)=\{\phi \in\mathbb{R^3},\varPhi=\phi^{\wedge}=\begin{bmatrix}0&-\phi_{13}&\phi_{12}\\\phi_{13}&0&-\phi_{11}\\-\phi_{12}&\phi_{11}&0\end{bmatrix} =R(t)'R(t)^T$

李括号
$[\phi_1,\phi_2]=(\varPhi_1\varPhi_2-\varPhi_2\varPhi_1)^{\vee}$ 这里的 $\vee$ 表示把反对称矩阵写成向量形式
这里要稍微解释一下上面的等式
其中
$\phi_1=\begin{bmatrix}\phi_{11}&\phi_{12}&\phi_{13}\end{bmatrix}$
$\phi_2=\begin{bmatrix}\phi_{21}&\phi_{22}&\phi_{23}\end{bmatrix}$
于是 $[\phi_1,\phi_2]=\phi_1\times \phi_2 =\begin{bmatrix} \phi_{12}\phi_{23}-\phi_{13}\phi_{22}\\ \phi_{13}\phi_{21}-\phi_{11}\phi_{23}\\ \phi_{11}\phi_{22}-\phi_{12}\phi_{21}\\ \end{bmatrix}$

其中
$\varPhi_1=\begin{bmatrix}0&-\phi_{13}&\phi_{12}\\\phi_{13}&0&-\phi_{11}\\-\phi_{12}&\phi_{11}&0\end{bmatrix}$

$\varPhi_2=\begin{bmatrix}0&-\phi_{23}&\phi_{22}\\\phi_{23}&0&-\phi_{21}\\-\phi_{22}&\phi_{21}&0\end{bmatrix}$

于是
$[\varPhi_1\varPhi_2-\varPhi_2\varPhi_1]^{V}$

$= \begin{bmatrix}0&-\phi_{13}&\phi_{12}\\\phi_{13}&0&-\phi_{11}\\-\phi_{12}&\phi_{11}&0\end{bmatrix}\begin{bmatrix}0&-\phi_{23}&\phi_{22}\\\phi_{23}&0&-\phi_{21}\\-\phi_{22}&\phi_{21}&0\end{bmatrix}-\begin{bmatrix}0&-\phi_{23}&\phi_{22}\\\phi_{23}&0&-\phi_{21}\\-\phi_{22}&\phi_{21}&0\end{bmatrix}\begin{bmatrix}0&-\phi_{13}&\phi_{12}\\\phi_{13}&0&-\phi_{11}\\-\phi_{12}&\phi_{11}&0\end{bmatrix}$

$=\begin{bmatrix} -\phi_{13}\phi_{23}-\phi_{12}\phi_{22}&\phi_{12}\phi_{21}&\phi_{13}\phi_{21}\\ \phi_{11}\phi_{22}&-\phi_{13}\phi_{23}-\phi_{11}\phi_{21}&\phi_{13}\phi_{22}\\ \phi_{11}\phi_{23}&\phi_{12}\phi_{23}&-\phi_{12}\phi_{22}-\phi_{11}\phi_{21} \end{bmatrix} -\begin{bmatrix} -\phi_{13}\phi_{23}-\phi_{12}\phi_{22}&\phi_{11}\phi_{22}&\phi_{11}\phi_{23}\\ \phi_{12}\phi_{21}&-\phi_{13}\phi_{23}-\phi_{11}\phi_{21}&\phi_{12}\phi_{23}\\ \phi_{13}\phi_{21}&\phi_{13}\phi_{22}&-\phi_{12}\phi_{22}-\phi_{11}\phi_{21} \end{bmatrix}$

$=\begin{bmatrix} 0&\phi_{12}\phi_{21}-\phi_{11}\phi_{22}&\phi_{13}\phi_{21}-\phi_{11}\phi_{23}\\ \phi_{11}\phi_{22}-\phi_{12}\phi_{21}&0&\phi_{13}\phi_{22}-\phi_{12}\phi_{23}\\ \phi_{11}\phi_{23}-\phi_{13}\phi_{21}&\phi_{12}\phi_{23}-\phi_{13}\phi_{22}&0 \end{bmatrix}^V$

$=\begin{bmatrix} \phi_{12}\phi_{23}-\phi_{13}\phi_{22}\\ \phi_{13}\phi_{21}-\phi_{11}\phi_{23}\\ \phi_{11}\phi_{22}-\phi_{12}\phi_{21} \end{bmatrix}$

验证李代数so(3)是否满足上述条件

封闭性证明
$\phi$ 是一个基本向量， $\phi^{\wedge}$ 是反对称矩阵， $\varPhi_1\varPhi_2-\varPhi_2\varPhi_1$ 的结果仍为反对称矩阵，也就是依然是属于集合 $V$ 中的元素
双线性证明
$[a\phi_1+b\phi_2,\phi_3]$
$=[(a\varPhi_1+b\varPhi_2)\varPhi_3-\varPhi_3(a\varPhi_1+b\varPhi_2)]^{V}$
$=[a\varPhi_1\varPhi_3+b\varPhi_2\varPhi_3-a\varPhi_3\varPhi_1+b\varPhi_3\varPhi_2]^{V}$
$=[a(\varPhi_1\varPhi_3-\varPhi_3\varPhi_1)+b(\varPhi_2\varPhi_3-\varPhi_3\varPhi_2)]^{V}$
$=a[(\varPhi_1\varPhi_3-\varPhi_3\varPhi_1)]^{V}+b[(\varPhi_2\varPhi_3-\varPhi_3\varPhi_2)]^{V}$
$=a[\phi_1,\phi_3]+b[\phi_2,\phi_3]$
同理可证
$[\phi_3,a\phi_1+b\phi_2]=a[\phi_3,\phi_1]+b[\phi_3,\phi_2]$
自反性证明
$[\phi_1,\phi_1]=[\varPhi_1\varPhi_1-\varPhi_1\varPhi_1]^{V}=\vec{0}$
雅可比等价证明
$\forall X,Y,Z\in\mathbb{V},有[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$
令 $X=\phi_1,Y=\phi_2,Z=\phi_3$

$[\phi_1,[\phi_2,\phi_3]]+[\phi_3,[\phi_1,\phi_2]]+[\phi_2,[\phi_3,\phi_1]]$

$=[\phi_1,[\varPhi_2\varPhi_3-\varPhi_3\varPhi_2]^{V}]+[\phi_3,[\varPhi_1\varPhi_2-\varPhi_2\varPhi_1]^{V}]+[\phi_2,[\varPhi_3\varPhi_1-\varPhi_1\varPhi_3]^{V}]$

$=[\varPhi_1(\varPhi_2\varPhi_3-\varPhi_3\varPhi_2)-(\varPhi_2\varPhi_3-\varPhi_3\varPhi_2)\varPhi_1]^{V}$
$+[\varPhi_3(\varPhi_1\varPhi_2-\varPhi_2\varPhi_1)-(\varPhi_1\varPhi_2-\varPhi_2\varPhi_1)\varPhi_3]^{V}$
$+[\varPhi_2(\varPhi_3\varPhi_1-\varPhi_1\varPhi_3)-(\varPhi_3\varPhi_1-\varPhi_1\varPhi_3)\varPhi_2]^{V}$

$=[\varPhi_1\varPhi_2\varPhi_3-\varPhi_1\varPhi_3\varPhi_2-\varPhi_2\varPhi_3\varPhi_1+\varPhi_3\varPhi_2\varPhi_1$
$+\varPhi_3\varPhi_1\varPhi_2-\varPhi_3\varPhi_2\varPhi_1-\varPhi_1\varPhi_2\varPhi_3+\varPhi_2\varPhi_1\varPhi_3$
$+\varPhi_2\varPhi_3\varPhi_1-\varPhi_2\varPhi_1\varPhi_3-\varPhi_3\varPhi_1\varPhi_2+\varPhi_1\varPhi_3\varPhi_2]^{V}$

$=\vec{0}$

李代数 $se(3)$

$se(3)$ 位于 $\mathbb{R}^6$ 空间中，前三维为平移，记作 $\rho$ ,后三维为旋转，记作 $\phi$
于是 $se(3)=\{\xi =\begin{bmatrix}\rho\\\phi\end{bmatrix}\in\mathbb{R}^6 ,\rho\in\mathbb{R}^3 ,\xi^{\wedge}=\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}\in \mathbb{R}^{4\times 4}\}$ 注意这里的 $\xi^{\wedge}$ 不在表示反对称矩阵，这里只是扩展了 $\wedge$ 符号的定义，用来将一个 $6$ 维 $\xi$ 转换成 $4\times 4$ 矩阵的一种形式
李括号
$[\xi_1,\xi_2]=(\xi_1^{\wedge}\xi_2^{\wedge}-\xi_2^{\wedge}\xi_1^{\wedge})^{\vee}$

同样这里对李代数 $se(3)$ 做一下证明

封闭性证明
$[\xi_1,\xi_2]=(\xi_1^{\wedge}\xi_2^{\wedge}-\xi_2^{\wedge}\xi_1^{\wedge})^{\vee}$
$=\Big(\begin{bmatrix}\phi_1^{\wedge}&\rho_1\\0^T&0\end{bmatrix}\begin{bmatrix}\phi_2^{\wedge}&\rho_2\\0^T&0\end{bmatrix}-\begin{bmatrix}\phi_2^{\wedge}&\rho_2\\0^T&0\end{bmatrix}\begin{bmatrix}\phi_1^{\wedge}&\rho_1\\0^T&0\end{bmatrix}\Big)^{\vee}$
$=\Big(\begin{bmatrix}\phi_1^{\wedge}\phi_2^{\wedge}&\phi_1^{\wedge}\rho_2\\0^T&0\end{bmatrix}-\begin{bmatrix}\phi_2^{\wedge}\phi_1^{\wedge}&\phi_2^{\wedge}\rho_1\\0^T&0\end{bmatrix}\Big)^{\vee}$
$=\Big(\begin{bmatrix}\phi_1^{\wedge}\phi_2^{\wedge}-\phi_2^{\wedge}\phi_1^{\wedge}&\phi_1^{\wedge}\rho_2-\phi_2^{\wedge}\rho_1\\0^T&0\end{bmatrix}\Big)^{\vee}$
可见得到的结果的四个元素，左上角依然是反对称矩阵，右上角是仍然是一个平移向量，因此封闭性成立
双线性证明
$[a\xi_1+b\xi_2,\xi_3]$
$=\Big((a\xi_1+b\xi_2)^{\wedge}\xi_3^{\wedge}-\xi_3^{\wedge}(a\xi_1+b\xi_2)^{\wedge}\Big)^{\vee}$
$=\Big((a\xi_1^{\wedge}+b\xi_2^{\wedge})\xi_3^{\wedge}-\xi_3^{\wedge}(a\xi_1^{\wedge}+b\xi_2^{\wedge})\Big)^{\vee}$
$=\Big((a\xi_1^{\wedge}\xi_3^{\wedge}+b\xi_2^{\wedge}\xi_3^{\wedge})-(a\xi_3^{\wedge}\xi_1^{\wedge}+b\xi_3^{\wedge}\xi_2^{\wedge})\Big)^{\vee}$
$=\Big(a(\xi_1^{\wedge}\xi_3^{\wedge}-\xi_3^{\wedge}\xi_1^{\wedge})+b(\xi_2^{\wedge}\xi_3^{\wedge}-\xi_3^{\wedge}\xi_2^{\wedge})\Big)^{\vee}$
$=a[\xi_1,\xi_3]+b[\xi_2,\xi_3]$
同理会有
$[\xi_3,a\xi_1+b\xi_2]=a[\xi_3,\xi_1]+b[\xi_3,\xi_2]$
自反性证明
$[\xi_1,\xi_1]=(\xi_1^{\wedge}\xi_1^{\wedge} -\xi_1^{\wedge}\xi_1^{\wedge})^\vee=0^\vee$
雅各比等价证明
同上面的 $so(3)$ 雅各比等价证明符号换一下即可

so(3)映射到SO(3)
首先我们知道对于初等函数
$e^x=\sum_{n=0}^{\infty}\frac{1}{n!}(x)^n$

推广到矩阵上就是
$e^{A}=\sum_{n=0}^{\infty}\frac{1}{n!}(A)^n$
而对于反对称矩阵 $\phi^{\wedge}$
$e^{\phi^{\wedge}}=\sum_{n=0}^{\infty}\frac{1}{n!}(\phi^{\wedge})^n$
我们知道对于矩阵的无穷次幂一般没法计算。但对于这个特殊的反对称矩阵，我们可以进行一些推导，因为 $\phi$ 是一个三维向量，于是我们可以定义他的模长和方向，分别记为 $\theta$ 和 $a$ ,其中 $||a||=1$ 是,于是我们有 $\phi=\theta a$
由于 $a^{\wedge}$ 是反对称矩阵
反对称矩阵的介绍可以点击这里
即反对称矩阵具有如下性质
$a^{\wedge}a^{\wedge}=aa^T-I$ 其中 $a$ 为单位向量
$a^{\wedge}a^{\wedge}a^{\wedge}=-a^{\wedge}$ 其中 $a$ 为单位向量
于是有
$e^{\phi^{\wedge}}=e^{\theta a^{\wedge}}=\sum_{n=0}^{\infty}\frac{1}{n!}(\theta a^{\wedge})^n$
$=I+\theta a^{\wedge}+\frac{1}{2!}\theta^2a^{\wedge}a^{\wedge}+\frac{1}{3!}\theta^3(a^{\wedge})^3+\frac{1}{4!}\theta^4(a^{\wedge})^4+...$
$=aa^T-a^{\wedge}a^{\wedge}+\theta a^{\wedge}+\frac{1}{2!}\theta^2a^{\wedge}a^{\wedge}+\frac{1}{3!}\theta^3(a^{\wedge})^3+\frac{1}{4!}\theta^4(a^{\wedge})^4+\frac{1}{5!}\theta^5(a^{\wedge})^5+...$
$=aa^T-(a^{\wedge})^2+\theta a^{\wedge}+\frac{1}{2!}\theta^2(a^{\wedge})^2-\frac{1}{3!}\theta^3a^{\wedge}-\frac{1}{4!}\theta^4(a^{\wedge})^2+\frac{1}{5!}\theta^5a^{\wedge}+...$
$=aa^T+(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5-\frac{1}{7!}\theta^7+...) a^{\wedge}-(1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4-\frac{1}{6!}\theta^6+...)(a^{\wedge})^2$
$=a^{\wedge}a^{\wedge}+I+sin\theta a^{\wedge}-cos\theta(a^{\wedge})^2$
$=(1-cos\theta )a^{\wedge}a^{\wedge}+I+sin\theta a^{\wedge}$
$=(1-cos\theta )(aa^T-I)+I+sin\theta a^{\wedge}$
$=cos\theta I+(1-cos\theta )aa^T+sin\theta a^{\wedge}$
这和罗德里格斯旋转公式一致
也就是说，也就是说，任意的一个李代数so(3)中的旋转向量的指数映射刚好对应了一个位于SO(3)中的旋转矩阵
反之，我们也可以用对数映射，将SO(3)中的旋转矩阵对应到李代数so(3)的旋转向量中
具体的公式如下：
$R=e^{\phi^{\wedge}}=cos\theta I+(1-cos\theta )aa^T+sin\theta a^{\wedge}$
两边同时求迹
$tr(R)=cos\theta tr(I)+(1-cos\theta )tr(aa^T)+sin\theta\cdot tr(a^{\wedge})$
$=3cos\theta+(1-cos\theta )$
$=(1-2cos\theta )$
于是 $\theta =arccos(\frac{tr(R)-1}{2})$
至于 $a$ ,我们知道 $Ra=a$ ,所以只要求解 $R$ 的特征值为 $1$ 的特征向量就是对应的 $a$
自此, $\theta$ 和对应的 $a$ 都有了，那么就得到了 $\phi=\theta a$
另外对数映射
$\phi=ln(R)^{\vee}=\Big(\sum_{n=0}^{\infty}\frac{(-1)^n}{n+1}(R-I)^{n+1}\Big)^{\vee}$
------------ps：关于 $lnx$ 的泰勒展开先留个坑

se(3)指数映射到SE(3)
$se(3)$ 的元素
$\xi^{\wedge}=\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}$
首先
$(\xi^{\wedge})^2=\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}=\begin{bmatrix}(\phi^{\wedge})^2&\phi^{\wedge}\rho\\0^T&0\end{bmatrix}$
$(\xi^{\wedge})^3=\begin{bmatrix}(\phi^{\wedge})^2&\phi^{\wedge}\rho\\0^T&0\end{bmatrix}\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}=\begin{bmatrix}(\phi^{\wedge})^3&(\phi^{\wedge})^2\rho\\0^T&0\end{bmatrix}$
$\vdots$
$\vdots$
$(\xi^{\wedge})^n=\begin{bmatrix}(\phi^{\wedge})^n&(\phi^{\wedge})^{n-1}\rho\\0^T&0\end{bmatrix}$

则 $se(3)$ 上的指数映射为
$e^{\xi^{\wedge}}=\sum_{n=0}^{\infty}\frac{1}{n!}(\xi^{\wedge})^n$

$=\sum_{n=0}^{\infty}\frac{1}{n!}\Big(\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}\Big)^n$

$=I+\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}+\frac{1}{2!}\Big(\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}\Big)^2+\frac{1}{3!}\Big(\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}\Big)^3+...$

$=I+\begin{bmatrix}\phi^{\wedge}&\rho\\0^T&0\end{bmatrix}+\frac{1}{2!}\begin{bmatrix}(\phi^{\wedge})^2&\phi^{\wedge}\rho\\0^T&0\end{bmatrix}+\frac{1}{3!}\begin{bmatrix}(\phi^{\wedge})^3&(\phi^{\wedge})^2\rho\\0^T&0\end{bmatrix}+...$

$=I+ \begin{bmatrix} \phi^{\wedge}+\frac{1}{2!}(\phi^{\wedge})^2+\frac{1}{3!}(\phi^{\wedge})^3+... &\Big(I+\frac{1}{2!}\phi^{\wedge}+\frac{1}{3!}(\phi^{\wedge})^2+...\Big)\rho \\0^T&0 \end{bmatrix}$

$=\begin{bmatrix} I+\phi^{\wedge}+\frac{1}{2!}(\phi^{\wedge})^2+\frac{1}{3!}(\phi^{\wedge})^3+... &\Big(I+\frac{1}{2!}\phi^{\wedge}+\frac{1}{3!}(\phi^{\wedge})^2+...\Big)\rho \\0^T&1 \end{bmatrix}$

$=\begin{bmatrix} e^{\phi^{\wedge}} &J\rho \\0^T&1 \end{bmatrix}$

其中 $\phi=\theta a$ ， $a$ 为单位向量
其中 $J=I+\frac{1}{2!}\phi^{\wedge}+\frac{1}{3!}(\phi^{\wedge})^2+...$
对 $J$ 进行化简
$J=I+\frac{1}{2!}\theta a^{\wedge}+\frac{1}{3!}\theta^2(a^{\wedge})^2+...$
$=I+\frac{1}{\theta}\Big(\frac{1}{2!}\theta^2 a^{\wedge}+\frac{1}{3!}\theta^3(a^{\wedge})^2+...\Big)$
$=I+\frac{1}{\theta}\Big(\frac{1}{2!}\theta^2 a^{\wedge}+\frac{1}{3!}\theta^3(a^{\wedge})^2+\frac{1}{4!}\theta^4(a^{\wedge})^3+\frac{1}{5!}\theta^5(a^{\wedge})^4+\frac{1}{6!}\theta^6(a^{\wedge})^5+...\Big)$
$=I+\frac{1}{\theta}\Big(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...\Big) a^{\wedge}+\frac{1}{\theta}\Big(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+...\Big)(a^{\wedge})^2$
$=I+\frac{1}{\theta}\Big(1-(1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4+...)\Big) a^{\wedge}+\frac{1}{\theta}\Big(\theta-(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5+...)\Big)(a^{\wedge})^2$

$=I+\frac{1}{\theta}\Big(1-cos\theta\Big) a^{\wedge}+\frac{1}{\theta}\Big(\theta-sin\theta\Big)(a^{\wedge})^2$
同样，我们有 $a^{\wedge}a^{\wedge}=aa^T-I$
于是 $=I+\frac{1}{\theta}\Big(1-cos\theta\Big) a^{\wedge}+\frac{1}{\theta}\Big(\theta-sin\theta\Big)(aa^T-I)$
$=I+\frac{1}{\theta}\Big(1-cos\theta\Big) a^{\wedge}+\Big(1-\frac{sin\theta}{\theta}\Big)(aa^T-I)$
$=\frac{sin\theta}{\theta}I+\Big(1-\frac{sin\theta}{\theta}\Big)aa^T+\Big(\frac{1-cos\theta}{\theta}\Big) a^{\wedge}$

里
$T=e^{\xi^{\wedge}}=\begin{bmatrix} e^{\phi^{\wedge}} &J\rho \\0^T&1 \end{bmatrix}$

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

这叫做 $se(3)$ 到 $SE(3)$ 的指数映射
同样的，我们来尝试得到 $SE(3)$ 到 $se(3)$ 的对数映射
尝试对等式两边取迹
$tr(T)=tr(\begin{bmatrix} e^{\phi^{\wedge}} &J\rho \\0^T&1 \end{bmatrix})$
$=tr(e^{\phi^{\wedge}})+1$
$=tr(R)+1$
$=tr(cos\theta I+(1-cos\theta )aa^T+sin\theta a^{\wedge})+1$
$=3cos\theta+(1-cos\theta )+1$
$=2cos\theta+2$
于是
$\theta=arccos\frac{tr(T)-2}{2}=arccos\frac{tr(R)-1}{2}$
同样 $Ra=a$ 求出 $a$
于是 $\phi=\theta a$ 以及 $\rho=tJ^{-1}$

下面是关于 $SO(3),so(3)$ 以及 $SE(3),se(3)$ 的指数和对数映射的图表，可以有一个完整的认识，

image.png

李群李代数-(1)
https://www.cnblogs.com/gaoxiang12/p/5137454.html[https:/...
李群李代数
李群在SLAM中，我们经常会遇到李群李代数的概念。我只知道对于3维空间的旋转矩阵 \bf R 其是个特殊的正交群...
2018-04-21:机器人领域常用C++库及工具介绍
线性代数库: Eigen3李群李代数库: Sophus 通用的非线性优化库: Ceres g2o机器人专用的非线性...
李群与李代数+补充理解
1. 浅显易懂了解李群和李代 2. 补充转载自https://mp.weixin.qq.com/s/sVjy9k...
Slam笔记-李群与李代数
4.1.1 什么是群？旋转矩阵R对加法是不封闭的，意思是，对于任意旋转矩阵、，按照矩阵加法对它俩做加法运算，结...
李群李代数学习笔记
2019-11-26
第八章群是紧致的单纯李群，它的李代数是。方块权图方法可以计算单纯李群不可约表示状态基的权和生成元在这组状态基里的...
李群和李代数（书，讲义和一些总结）
李群和李代数（一）数学是一直喜欢的，不过大多数时候是浅尝辄止，一本 Nakahara 的 “Geometry, ...
第四讲李群与李代数
这部分主要是用李群与李代数来表达位姿，简化和规范化位姿的数学上的表达，为后面进行更复杂计算做准备。一、定义： 1...
李群李代数在SLAM系统中的使用
这篇文章针对有一定SLAM基础的同学或者对李群李代数的应用感兴趣的数学专业同学,已经很小众了，但对于真正干这行并想...