SO3 与so3 & SE3与se3 & SIM3

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[

\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[

\right], G_y =\left[ \right] G_z =\left[ \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 ) = [ R ( t ) p ( t ) 0 1 ] T(t) =\left[

\right] T(t)=[R(t)0​p(t)1​]

同样，我们构造$\dot{T}=AT$的形式，从而使得$T=exp(A)$成立。
T ˙ ( t ) = [ R ˙ ( t ) p ˙ ( t ) 0 1 ] = [ ω ^ ( t ) R ( t ) p ˙ ( t ) 0 1 ] \dot T(t) =\left[

\right] =\left[ \right] T˙(t)=[R˙(t)0​p˙​(t)1​]=[ω^(t)R(t)0​p˙​(t)1​]

T ˙ ( t ) T − 1 ( t ) = [ ω ^ ( t ) − ω ^ ( t ) ⋅ p ( t ) + p ˙ ( t ) 0 0 ] = A 4 ∗ 4 \dot T(t) T^{-1} (t) =\left[

\right]=A_{4*4} T˙(t)T−1(t)=[ω^(t)0​−ω^(t)⋅p(t)+p˙​(t)0​]=A4∗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 = [ ξ ] = [ θ ^ ρ 0 0 ] A=[ξ]=\left[

\right] A=[ξ]=[θ^0​ρ0​]

同样求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 = [ δ ] = [ θ ^ + σ I 3 ∗ 3 ρ 0 3 T 0 ] A=[δ]=\left[

\right] A=[δ]=[θ^+σI3∗3​03T​​ρ0​]
e x p ⁡ ( A ) = e x p ⁡ ( [ δ ] ) = [ e x p ( σ ) e x p ⁡ ( θ ^ ) W ρ 0 1 ] exp⁡(A)=exp⁡([δ])=\left[

exp(σ)exp⁡(θ^)Wρ01" role="presentation" style="position: relative;">exp(σ)exp(θ^)0Wρ1$$\begin{array}{}exp(\sigma )exp(\hat{\theta })& W\rho \\ 0& 1\end{array}$$

\right] exp⁡(A)=exp⁡([δ])=[exp(σ)exp⁡(θ^)0​Wρ1​]

2. 第二种：
   https://ethaneade.com/latex2html/lie/node29.html
   $\delta$对应的A阵 A = [ δ ] = [ θ ^ ρ 0 − σ ] A=[δ]=\left[
   θ^ρ0−σ" role="presentation" style="position: relative;">θ^0ρ−σ$$\begin{array}{}\hat{\theta }& \rho \\ 0& -\sigma \end{array}$$
   \right] A=[δ]=[θ^0​ρ−σ​]

e x p ⁡ ( A ) = e x p ⁡ ( [ δ ] ) = [ e x p ⁡ ( θ ^ ) W ρ 0 e x p ⁡ ( − σ ) ) ] exp⁡(A)=exp⁡([δ])=\left[

\right] exp⁡(A)=exp⁡([δ])=[exp⁡(θ^)0​Wρexp⁡(−σ))​]
具体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 = [ ω ( t ) v ( t ) ] 6 ∗ 1 V_w=\left[

\right]_{6*1} Vw​=[ω(t)v(t)​]6∗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的向量构成的方阵。
令：

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

从而根据：

可得：