李群&李代数：SO3 与so3 & SE3与se3 & SIM3

• 旋转矩阵<->角轴对应于李群<->李代数
• $\dot{R}=\hat{\omega }R$中的$\omega$指的是角轴的速度，即单位旋转轴*角速率
  *Adjoint Map对应速度(线速度，角速度)之间的齐次映射

1 旋转*叉乘

1.1 旋转矩阵的导数

根据旋转矩阵的性质：$R{R}^T=I$，对两侧进行求导可得：
$$\dot{R}{R}^{\mathrm{T}}=-R{\dot{R}}^{\mathrm{T}}$$
从而可知，$\dot{R}{R}^T$为一个反对称对阵，则存在向量$\omega$，使得$R{\dot{R}}^T=\hat{\omega }$

$$\hat{\omega }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
即：$\dot{R}=\hat{\omega }R$

1.2 物理意义

设空间一个点$S(t)$，其绕空间某个轴旋转，设其在t时刻的旋转轴为$\omega (t)/\Vert \omega (t)\Vert$，其旋转角速度为$\Vert \omega \Vert$，则我们可知该点对应的线速率为：
$$\dot{S}(t)=\omega (t)\times S(t)=\hat{\omega }(t)\cdot S(t)$$
以此，我们便可理解旋转与轴角速率及叉乘之间的关系

1.3 实例

空间两个坐标系，W和B坐标系。W坐标系固定。
设B坐标系下有一点P与坐标系B固定，设其在B坐标系下坐标为：$S_B$(固定)，其在W坐标系下坐标为：$S_W(t)$，设：
$$S_W(t)=R(t)\ast S_B+p(t)$$
对其进行求导：
$${\dot{S}}_W(t)=\hat{\omega }(t)\cdot R(t)\cdot S_B+\dot{p}(t)=\hat{\omega }(t)\cdot (S_W(t)-p(t))+\dot{p}(t)$$$$=\hat{\omega }(t)\cdot S_W(t)-\hat{\omega }(t)\cdot p(t)+\dot{p}(t)$$
从上式可以看出，

$\omega$对应B坐标系在W坐标系下的旋转角速度，$\omega \times S_W(t)$即为由旋转引起的P点在W坐标系下的线速度。
$-\hat{\omega }(t)\dot{p}(t)+\hat{p}(t)$对应点P的线速度
注意：$\omega$为W坐标系下的向量表示。

可进一步写成：
$${\dot{S}}_W(t)=\hat{\omega }(t)\cdot S_W(t)+v(t)$$
本实例：B坐标系起始与W重合，t时刻B坐标系以$\omega (t)=[0,0,\omega (t)]$角速度进行旋转，即B坐标系绕z轴，以$\omega (t)$为角速度进行旋转。
设B坐标系的旋转角为$\theta (t)$，即$\dot{\theta }(t)=\omega (t)$

$\omega \stackrel{ˆ}{}$ 在本文中与$\hat{\omega }$同义

通过此实例，我们可以看出$\omega (t)$对应的旋转物理意义。

进一步我们了解到$\omega (t)$与则是角轴表示的角速度，从而了解角轴的叉乘几何特性。

1.4 角轴与反对称矩阵

每个轴的旋转均对应一个反对称矩阵，即将旋转同时分布在各个轴上，设如下一组：
$$G_x=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right],G_y=\left[\begin{array}{ccc}0& 0& 1\\ 0& 0& 0\\ -1& 0& 0\end{array}\right]G_z=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]$$

设角轴：$\theta =[{\theta }_x,{\theta }_y,{\theta }_z{]}^T$
$$R={\theta }_x{G}_x+{\theta }_y{G}_y+{\theta }_z{G}_z$$

Tips:
由此我们可以知道，角轴与欧拉角之间的小联系，当前两个旋转轴的旋转角度较小时，欧拉角$\approx$轴角。

2 SO3 与so3

由于旋转矩阵与角轴之间的关系，对应了李群与李代数之间的关系，设计旋转矩阵对应的李代数为其对应的角轴。

2.1 so3 2 SO3

在我们知道：$\dot{R}=\hat{\omega }R$，便可知R可表达成e指数的形式：
$$R(t)=exp([\int \omega (t)]\stackrel{ˆ}{})=exp(\hat{\theta }(t))$$

即我们设计使得$R$与角轴$\theta$对应。可知，由于三角函数的周期性，因此旋转矩阵到角轴是一对多的映射。由此可知，旋转矩阵对应李群SO3，而角轴表示对应李代数，其向量方向与旋转矩阵的切方向一致。
$$R=\exp \left(\hat{\theta }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\hat{\theta }}^{n_{\leftrightarrow }}$$ $$\begin{array}{rl}R& =\exp (\hat{\mathbf{\theta }})=I+\sum _{n=1}^{\infty }\frac{1}{n!}{\hat{\mathbf{\theta }}}^n=I+\sum _{n=0}^{\infty }\frac{1}{(2n+1)!}{\hat{\mathbf{\theta }}}^{2n+1}+\sum _{n=0}^{\infty }\frac{1}{(2n+2)!}{\hat{\mathbf{\theta }}}^{2n+2}\\ & =I+\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n\Vert \theta {\Vert }^{2n}}{(2n+1)!}\right)\hat{\theta }+\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n\Vert \theta {\Vert }^{2n}}{(2n+2)!}\right){\hat{\theta }}^2\\ & =I+\left(\frac{\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }\Vert }\right)\hat{\mathbf{\theta }}+\left(\frac{1-\cos \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}\right){\hat{\mathbf{\theta }}}^2\\ & =I_{3\ast 3}+\left(\frac{\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }\Vert }\right)\hat{\mathbf{\theta }}+\left(\frac{1-\cos \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}\right)\left(-\Vert \mathbf{\theta }{\Vert }^2{I}_{3\ast 3}+\mathbf{\theta }{\mathbf{\theta }}^{\mathbf{T}}\right)\\ & =cos\Vert \mathbf{\theta }\Vert {\mathbf{I}}_{3\ast 3}+\left(\frac{\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }\Vert }\right)\hat{\mathbf{\theta }}+\left(\frac{1-cos\Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}\right)\mathbf{\theta }{\mathbf{\theta }}^{\mathrm{T}}\end{array}$$

2.2 SO3 2 so3

$$\hat{\theta }=\log (R)=\sum _{n=1}^{\infty }\frac{(-1{)}^{n-1}}{n}(R-\mathbf{I}{)}^n$$

一对多的解算，其可以简化为旋转矩阵到角轴的计算：

3 SE3 与se3

设SE3的变换矩阵$T$
$$T(t)=\left[\begin{array}{cc}R(t)& p(t)\\ 0& 1\end{array}\right]$$

同样，我们构造$\dot{T}=AT$的形式，从而使得$T=exp(A)$成立。
$$\dot{T}(t)=\left[\begin{array}{cc}\dot{R}(t)& \dot{p}(t)\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\hat{\omega }(t)R(t)& \dot{p}(t)\\ 0& 1\end{array}\right]$$

$$\dot{T}(t)T^{-1}(t)=\left[\begin{array}{cc}\hat{\omega }(t)& -\hat{\omega }(t)\cdot p(t)+\dot{p}(t)\\ 0& 0\end{array}\right]=A_{4\ast 4}$$
对比1.3节的公式：
$${\dot{S}}_W(t)=\hat{\omega }(t)\cdot S_W(t)-\hat{\omega }(t)\cdot p(t)+\dot{p}(t)=\hat{\omega }(t)\cdot S_W(t)+v(t)$$

佐证:
$\omega$对应B坐标系在W坐标系下的旋转角速度。
$v(t)$对应W坐标系下线速度。

3.1 se3 2 SE3:

se3 对应的$A$阵为4*4的矩阵，加入偏移数据后，不再是一个反对称矩阵。我们设T阵对应的李代数为$\xi$，其对应的李代数矩阵[ξ]表示为：
$$A=[\xi ]=\left[\begin{array}{cc}\hat{\theta }& \rho \\ 0& 0\end{array}\right]$$

同样求e指数：
$$\begin{array}{rl}\exp (A)& =\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n=\left[\sum _{n=0}^{\infty }\frac{1}{n!}(\hat{\mathbf{\theta }}{)}^n\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\hat{\mathbf{\theta }}{)}^n\rho \right]\\ & =\left[\begin{array}{cc}\exp (\hat{\mathbf{\theta }})& J_L(\mathbf{\theta })\rho \\ 0& 1\end{array}\right]\\ J_L(\mathbf{\theta })& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\hat{\mathbf{\theta }}{)}^n=I_{3\times 3}+\left(\frac{1-\cos \Vert \theta \Vert }{\Vert \theta {\Vert }^2}\right)\hat{\mathbf{\theta }}+\left(\frac{\Vert \mathbf{\theta }\Vert -\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^3}\right){\hat{\mathbf{\theta }}}^2\end{array}$$

note:
$$R(\mathbf{\theta })=I_{3\times 3}+{\mathbf{\theta }}^{\wedge }{J}_L(\mathbf{\theta })$$

3.2 SE3 2 se3

其中：$J_L^{-1}$可由以下公式得到：
$$J_L^{-1}=I_{3\times 3}-\frac{1}{2}\hat{\mathbf{\theta }}+\frac{1-\Vert \theta \Vert \cos \Vert \theta /2\Vert }{2\sin \Vert \theta /2\Vert \ast \Vert \theta {\Vert }^2}\ast {\hat{\mathbf{\theta }}}^2$$

此外，注意：不成立

一个常用性质：

https://natanaso.github.io/ece276a2019/ref/ECE276A_12_SE3.pdf
https://vnav.mit.edu/material/04-05-LieGroups-notes.pdf

4 SIM3 与sim3

SIM3与sim3之间的exp指数映射方式有多种：

1. 第一种：
   https://qiita.com/shinsumicco/items/a2a00a3942caf2c88ecb
   [Trajectory Alignment and Evaluation in SLAM: Horn’s Method vs Alignment on the Manifold]

设一个sim3，$\delta$，其对应的$A$阵
$$A=[\delta ]=\left[\begin{array}{cc}\hat{\theta }+\sigma I_{3\ast 3}& \rho \\ 0_3^T& 0\end{array}\right]$$
$$exp(A)=exp([\delta ])=\left[\begin{array}{cc}exp(\sigma )exp(\hat{\theta })& W\rho \\ 0& 1\end{array}\right]$$

2. 第二种：
   https://ethaneade.com/latex2html/lie/node29.html
   $\delta$对应的A阵 $A=[\delta ]=\left[\begin{array}{cc}\hat{\theta }& \rho \\ 0& -\sigma \end{array}\right]$

$$exp(A)=exp([\delta ])=\left[\begin{array}{cc}exp(\hat{\theta })& W\rho \\ 0& exp(-\sigma ))\end{array}\right]$$
具体W不给出，请与链接中查看

5 Adjoint repression & Map

https://dellaert.github.io/20S-8803MM/Readings/3D-Adjoints-note.pdf

Adjoint repression可以用于速度层面的直接线性映射。

根据上文：
$${\dot{S}}_w(t)=\hat{\omega }(t)\cdot S_W(t)+v(t)$$
写成齐次形式:

其中，
$$V_w={\left[\begin{array}{c}\omega (t)\\ v(t)\end{array}\right]}_{6\ast 1}$$
根据$S_W(t)=R(t)\cdot S_B+p(t)$计算${\dot{P}}_B$与$P_B$之间的映射
齐次：

代入${\dot{P}}_W=V_W{P}_W$得：
$$T_B^W{\dot{P}}_B=V_W{T}_B^W{P}_B⟹{\dot{P}}_B=(T_B^W{)}^{-1}{\hat{V}}_W{T}_B^W{P}_B$$
从而可知：
$${\hat{V}}_B=(T_B^W{)}^{-1}{\hat{V}}_W{T}_B^W=T_W^B{\hat{V}}_W(T_W^B{)}^{-1}$$
以上便是Adjoint Map，可见Adjoint Map是将速度之间的映射
我们可以从${\dot{V}}_B$与${\dot{V}}_W$之间的关系中，找到$V_W$与$V_B$之间的映射。$V_W$与$V_B$为6维向量，而${\hat{V}}_B$与${\hat{V}}_W$为4*4的向量构成的方阵。
令：

根据叉乘的反交换律和

可得：

从而根据：

可得：

从而引出伴随表示，使伴随表示也符合李群的性质。SE3为4*4的矩阵，而伴随矩阵为6*6的李群矩阵，
设伴随李代数：

令：
$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\theta }\\ \mathbf{\rho }\end{array}\right],T(\mathbf{\xi })=\left[\begin{array}{cc}R& \mathbf{p}\\ 0& 1\end{array}\right]$$

对应A阵：
$$A=ad(\mathbf{\xi })={\mathbf{\xi }}^{⋏}={\left[\begin{array}{c}\mathbf{\theta }\\ \mathbf{\rho }\end{array}\right]}^{⋏}=\left[\begin{array}{cc}{\mathbf{\theta }}^{\wedge }& 0\\ {\mathbf{\rho }}^{\wedge }& {\mathbf{\theta }}^{\wedge }\end{array}\right]\in {\mathbb{R}}^{6\times 6},\mathbf{\rho },\mathbf{\theta }\in {\mathbb{R}}^3.$$
$$Ad(T)=A{d}_{\mathrm{T}}=\left[\begin{array}{cc}R& 0\\ {\mathbf{p}}^{\wedge }R& R\end{array}\right]=\exp (ad(\mathbf{\xi }))$$

以上参考《State estimation for robotics》，为与上文保持一致，本文的伴随ad矩阵采取这样的结构，与书中有出入。

Sophus软件中李代数sim3结构排序是：先trans，后角度，最后scale，而本文中的说明均是角度在前，trans在后，因此与书中的伴随矩阵表示有出入。

证明：$T{T}_0{T}^{\mathrm{T}}=\exp (a{d}_T\cdot {\mathbf{\xi }}_0)$
$$\begin{array}{rl}T{T}_0{T}_0^{-1}& =\left[\begin{array}{cc}R& p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0^T& -R^{T}p\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}R{R}_0& R{P}_0+p\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0^T& -R^{T}p\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}R{R}_0{R}^T& -R{R}_0{R}^{T}p+R{p}_0+p\\ 0& 1\end{array}\right]\end{array}$$

$$\begin{array}{rl}\mathrm{a}{\mathrm{d}}_{\mathrm{T}}& :\left[\begin{array}{cc}R& p_{\times }R\\ 0& R\end{array}\right]{\mathbf{\xi }}_0:\left[\begin{array}{c}{\mathbf{\theta }}_0\\ {\mathbf{\rho }}_0\end{array}\right]\\ \mathrm{a}{\mathrm{d}}_{\mathrm{T}}& =\left[\begin{array}{cc}R& p_{\times }R\\ 0& 12\end{array}\right]\left[\begin{array}{c}{\mathbf{\theta }}_0\\ {\mathbf{\rho }}_0\end{array}\right]=\left[\begin{array}{c}R{\theta }_0\\ p_{\times }R{\theta }_0+R{\rho }_0\end{array}\right]\\ \exp \left(\mathrm{a}{\mathrm{d}}_{\mathrm{T}}{\mathbf{\xi }}_0\right)& =\left[\begin{array}{cc}R{R}_0{R}^T& J_{R{\mathbf{\theta }}_0}(p_{\times }R{\theta }_0+R{\rho }_0)\\ 0& 1\end{array}\right]\end{array}$$

即要证明：$-R{R}_0{R}^{T}p+R{p}_0+p=J_{R{\mathbf{\theta }}_0}(p_{\times }R{\theta }_0+R{\rho }_0)$
首先：
$$\begin{array}{rl}{J}_{R\theta }=\frac{1}{(n+1)!}\sum {\left(\widehat{R\theta }\right)}^n& =\frac{1}{(n+1)!}\sum {\left(R\hat{\theta }{\hat{R}}^T\right)}^n=\frac{1}{(n+1)!}R\left(\sum {\hat{\theta }}^n\right)\cdot R^T\\ J_{\theta }=\frac{1}{(n+1)!}\sum {\hat{\theta }}^n& \\ \therefore J_{R\theta }=R\cdot J_0\cdot R^T& \end{array}$$

以及：
$$R=I_{3\times 3}+J\hat{\mathbf{\theta }}$$$$p=J\rho$$$$R\hat{\theta }{R}^T=R\hat{\theta }$$

因而：
$$\begin{gathered} -R{R}_0{R}^{T}p+R{p}_0+p=-R(1+J_{{\theta }_0}{\hat{\mathbf{\theta }}}_0)R^{T}p+R{p}_0+p \\ =-p-R{J}_{{\theta }_0}{\hat{\mathbf{\theta }}}_0{R}^{T}p+R{J}_{{\theta }_0}{\rho }_0+p \\ =-R{J}_{{\theta }_0}{\hat{\mathbf{\theta }}}_0{R}^{T}p+R{J}_{{\theta }_0}{\rho }_0 \\ =-R{J}_{{\theta }_0}{\hat{\mathbf{\theta }}}_{0}p+R{J}_{{\theta }_0}{\rho }_0 \end{gathered}$$

李括号(Lie Bracket)和伴随(adjoint representation)

设a，b为两个李代数元素， A和B为对应的李群表示：
$A=\hat{a},B=\hat{b}$
$$\begin{array}{rl}& \exp (A+B)=\exp (\hat{a}+\hat{b})=\exp (A)\exp (B)\exp (-\frac{1}{2}[A,B])\end{array}$$
[]表示李括号
$$[a,b]=[\hat{a}\hat{b}-\hat{b}\hat{a}{]}^{\vee }$$$$[A,B]=AB-BA$$

note：李群$\exp (A+B)=\exp (A)\exp (B)$性质不一定成立。

BCH展开公式：
e A e B = exp ⁡ { A + B + 1 2 ! [ A , B ] + 1 3 ! ( 1 2 [ A , [ A , B ] ] + 1 2 [ [ A , B ] , B ] ) + ⋯   } , \begin{aligned}e^{A}e^{B}=\exp\{A+B+\frac{1}{2!}[A,B]+\frac{1}{3!}(\frac{1}{2}[A,[A,B]]+\frac{1}{2}[[A,B],B])+\cdots\},\end{aligned} eAeB=exp{A+B+2!1​[A,B]+3!1​(21​[A,[A,B]]+21​[[A,B],B])+⋯},​
即：
$$ln(e^A{e}^B)=A+B+\frac{1}{2!}[A,B]+\frac{1}{3!}(\frac{1}{2}[A,[A,B]]+\frac{1}{2}[[A,B],B])+\cdots$$

$$\begin{array}{r}{e}^{A}B{e}^{-A}=B+[A,B]+\frac{1}{2!}[A,[A,B]]+\dots ,\end{array}$$

对于李代数中的元素 ( a ) 和 ( b )，如果它们之间的对易子（李括号）为 ($[a,b]$)，那么这个对易子可以被表示为 ( $a{d}_a(b)$ )，表示 ( b ) 在 ( a ) 的伴随表示下的像，即
$$a{d}_{a}b=[a,b]=[AB-BA{]}^{\vee }$$

设$$\mathbf{a}=\left[\begin{array}{c}{\mathbf{\theta }}_a\\ {\mathbf{\rho }}_a\end{array}\right],A=\left[\begin{array}{cc}{\mathbf{\theta }}_a^{\wedge }& {\mathbf{\rho }}_a^{\wedge }\\ 0& 0\end{array}\right]$$$$\mathbf{b}=\left[\begin{array}{c}{\mathbf{\theta }}_b\\ {\mathbf{\rho }}_b\end{array}\right],B=\left[\begin{array}{cc}{\mathbf{\theta }}_b^{\wedge }& {\mathbf{\rho }}_b^{\wedge }\\ 0& 0\end{array}\right]$$
$$a{d}_a=\left[\begin{array}{cc}{\mathbf{\theta }}_a^{\wedge }& 0\\ {\mathbf{\rho }}_a^{\wedge }& {\mathbf{\theta }}_a^{\wedge }\end{array}\right]$$

Tips:
$${\mathbf{\theta }}_a^{\wedge }{\mathbf{\theta }}_b^{\wedge }-{\mathbf{\theta }}_b^{\wedge }{\mathbf{\theta }}_a^{\wedge }={\mathbf{\theta }}_a^{\wedge }{\mathbf{\theta }}_b=-{\mathbf{\theta }}_b^{\wedge }{\mathbf{\theta }}_a$$

参考文献：https://zhuanlan.zhihu.com/p/647480442