SO3 与so3 & SE3与se3 & SIM3

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

1 旋转*叉乘

1.1 旋转矩阵的导数

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

ω ^ = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \hat \omega =\left[ \begin{array}{ccc} 0&-ω_z&ω_y\\ ω_z&0&-ω_x\\ -ω_y&ω_x&0 \end{array} \right] ω^= ​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​ ​
即：$\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 = [ 0 0 0 0 0 − 1 0 1 0 ] , G y = [ 0 0 1 0 0 0 − 1 0 0 ] G z = [ 0 − 1 0 1 0 0 0 0 0 ] 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] Gx​= ​000​001​0−10​ ​,Gy​= ​00−1​000​100​ ​Gz​= ​010​−100​000​ ​

设角轴：$\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，而角轴表示对应李代数，其向量方向与旋转矩阵的切方向一致。

2.2 SO3 2 so3

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

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指数：

3.2 SE3 2 se3

此外，注意：不成立

一个常用性质：

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 Map

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

根据上文：
$${\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的向量构成的方阵。
令：

根据叉乘的反交换律和
可得：

从而根据：

可得：