Reference:

《视觉SLAM十四讲》
http://www.cs.toronto.edu/~gwtaylor/publications/nips2006mhmublv
https://github.com/asheshjain399/RNNexp/tree/srnn/structural_rnn/CRFProblems/H3.6m/mhmublv/Motion

基础概念

外积（外积的方向垂直于两个向量）

$\mathbf{a}\times\mathbf{b}=\left\| \begin{matrix} \mathbf{i}& \mathbf{j}& \mathbf{k} \\ a_1 & a_2& a_3\\ b_1 & b_2& b_3 \end{matrix} \right\|=\left[ \begin{matrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{matrix} \right]=\left[ \begin{matrix} 0 & -a_3& a_2\\ a_3 & 0& -a_1\\ -a_2 & a_1& 0 \end{matrix} \right]\mathbf{b}$
即有
$\mathbf{a}\times\mathbf{b}=\left[ \begin{matrix} 0 & -a_3& a_2\\ a_3 & 0& -a_1\\ -a_2 & a_1& 0 \end{matrix} \right]\mathbf{b}= \mathbf{a}^{\wedge}\mathbf{b}$
这里的 $\wedge$ 为反对称符号，提供了一个向量到矩阵（反对称矩阵，Skew-symmetric）的对应关系。

1、旋转矩阵

坐标 $[a_1, a_2, a_3]^T$ 旋转成 $[a_1', a_2', a_3']^T$
于是有
$\left[ \begin{matrix} e_1, e_2, e_3 \end{matrix} \right] \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] = \left[ \begin{matrix} e_1', e_2', e_3' \end{matrix} \right] \left[ \begin{matrix} a_1' \\ a_2' \\ a_3' \end{matrix} \right]$
进一步，有
$\left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \end{matrix} \right] = \left[ \begin{matrix} e_1^{T}e_1'&e_1^{T}e_2'&e_1^{T}e_3'\\ e_2^{T}e_1'&e_2^{T}e_2'&e_2^{T}e_3'\\ e_3^{T}e_1'&e_3^{T}e_2'&e_3^{T}e_3' \end{matrix} \right] \left[ \begin{matrix} a_1' \\ a_2' \\ a_3' \end{matrix} \right] \triangleq \mathbf{R}\mathbf{a}'$
这里的 $\mathbf{R}$ 就称为旋转矩阵（且为正交阵）。

特殊正交群（SOG, Special Orthogonal Group）

$SO(n)=\{\mathbf{R}\in\mathbb{R}^{n\times n}|\mathbf{R}\mathbf{R}^T=\mathbf{I}, det(\mathbf{R})=1\}$

考虑带平移的旋转：
$\mathbf{a'}=\mathbf{T}\mathbf{a}+\mathbf{t}$
引入齐次坐标，则有
$\left[ \begin{matrix} \mathbf{a'} \\ 1 \end{matrix} \right] = \left[ \begin{matrix} \mathbf{R} & \mathbf{t}\\ \mathbf{0^T} &1 \end{matrix} \right] \left[ \begin{matrix} \mathbf{a} \\ 1 \end{matrix} \right] \triangleq \mathbf{T}\left[ \begin{matrix} \mathbf{a} \\ 1 \end{matrix} \right]$
这里 $\mathbf{T}$ 为变换矩阵，也是正交阵。

特殊欧式群（SEG, Special Euclidean Group）

$SE(3)=\left\{\mathbf{T}=\left[ \begin{matrix} \mathbf{R} & \mathbf{t}\\ \mathbf{0^T} &1 \end{matrix} \right]\in\mathbb{R}^{4\times 4}|\mathcal{R}\in SO(3), \mathbf{t}\in\mathbb{R}^3\right\}$

2、旋转向量与欧拉角

任何旋转可以由一个旋转轴 $\mathbf{n}$ 和一个旋转角 $\mathbf{\theta}$ 来刻画。
于是考虑用一个向量与旋转轴一致，而长度等于旋转角，这个向量就是旋转向量，只需要一个三维向量表示即可，该向量也是后面会提到的李代数。

罗德里格斯公式（Rodrigues's Formula）

旋转向量 $\rightarrow$ 旋转矩阵
$\mathbf{R}=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{n}\mathbf{n}^T+\sin{\theta}\mathbf{n}^{\wedge}$
其中 $\wedge$ 为反对称符号。
$\begin{align} \mathbf{tr}(\mathbf{R})&=\cos\theta\mathbf{tr}(\mathbf{I})+(1-\cos\theta)\mathbf{tr}(\mathbf{n}\mathbf{n}^T)+\sin{\theta}\mathbf{tr}(\mathbf{n}^{\wedge})\\ &=3\cos\theta+(1-\cos\theta)\\ &=1+2\cos\theta \end{align}$
p.s. 其中用到了反对称矩阵Trace为0。

于是有，
$\theta = \arccos{\left(\frac{\mathbf{tr(R)}-1}{2}\right)}$

因为 $\mathbf{n}$ 是旋转轴，则
$\mathbf{R}\mathbf{n}=\mathbf{n}$
即转轴 $\mathbf{n}$ 是旋转矩阵 $\mathbf{R}$ 特征值为1对应的特征向量。

比起旋转向量，欧拉角（三个轴的旋转参数）会更加直观，不过会遇到万向锁的问题。且不适合插值和迭代。

3、四元数

旋转矩阵用9个量描述3个自由度，但是具有冗余性；欧拉角和旋转向量虽然紧凑，但是具有奇异性。

复数的乘法表示了复平面上的旋转。
四元数 $\mathbf{q}$ 是一种扩展的复数。紧凑没有奇异性。
$\mathbf{q}=q_0+q_1i+q_2j+q_3k$
且满足
$\begin{aligned} & i^2 =j^2=k^2=-1\\ &ij=k, ji=-k\\ &jk=i,kj=-i\\ &ki=j, ik=-j \end{aligned}$

简记为： $\mathbf{q}=[\mathbf{s}, \mathbf{v}]$

进一步，用单位四元数（可以表示三维空间任意旋转）。
假设某个旋转是绕单位向量 $\mathbf{n}=[n_x, n_y, n_z]$ 进行了 $\theta$ 的旋转，则这个旋转的四元数可以表示为：
$\mathbf{q}=[\cos(\frac{\theta}{2}), n_x \sin(\frac{\theta}{2}), n_y \sin(\frac{\theta}{2}), n_z \sin(\frac{\theta}{2})]$

反之，也可以从单位四元数求对应的旋转轴和夹角：
$\begin{aligned} &\theta=2\arccos q_0\\ &[n_x, n_y, n_z]^T=[q_1, q_2, q_3]^T/sin{\frac{\theta}{2}} \end{aligned}$
注意到 $\theta$ 每加 $2\pi$ ， $\mathbf{q}$ 就变成 $-\mathbf{q}$ 。即每个旋转可以由两个互为相反数的四元数表示。

P.S. 四元数的逆为
$\mathbf{q}^{-1}=\mathbf{q}^{\star}/\|\mathbf{q}\|^2$
其中， $\mathbf{q}^{\star}$ 为四元数共轭，虚部取相反数。 $\mathbf{q}^{-1}\mathbf{q}=\mathbf{q}\mathbf{q}^{-1}=\mathbf{1}$

四元数表示旋转

设一个三维点 $\mathbf{p}=[x, y, z]\in\mathbb{R}^3$ 以及一个由轴角 $\mathbf{n}, \theta$ 指定的旋转，令得到的 $\mathbf{p}'=\mathbf{R}\mathbf{p}$ 。

将三维空间点用一个虚四元数表示
$\mathbf{p}=[0, x, y, z]=[0, \mathbf{v}]$
则将这个点按 $\mathbf{n}$ 旋转，有对应这个旋转的四元数：
$\mathbf{q}=[\cos(\frac{\theta}{2}), n_x \sin(\frac{\theta}{2}), n_y \sin(\frac{\theta}{2}), n_z \sin(\frac{\theta}{2})]$
可以验证，旋转后得到
$\mathbf{p}'=\mathbf{q}\mathbf{p}\mathbf{q}^{-1}$
旋转后的 $\mathbf{p}'$ 也为纯虚四元数，三个虚部分量即为旋转后的3D点。

四元数&旋转矩阵

$\mathbf{q}=q_0+q_1i+q_2j+q_3k$ 对应的旋转矩阵 $\mathbf{R}$ ： $\mathbf{R}=\left[\begin{matrix} 1-2q_2^2-2q_3^2& 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2\\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1\\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^22q_2^2 \end{matrix}\right]$

假设矩阵 $\mathbf{R}={m_{ij}}, i,j\in[1, 2, 3]$ ，则四元数为
$q_0\! = \!\frac{\sqrt{\mathbf{tr}(\mathbf(R))\!+\!1}}{2},q_1\!=\!\frac{m_{32}\!-\!m_{23}}{4q_0},q_2\!=\!\frac{m_{13}\!-\!m_{31}}{4q_0},\ \ q_3\!=\!\frac{m_{21}\!-\!m_{12}}{4q_0}$

小结

特殊正交群（SOG, Special Orthogonal Group）
$SO(n)=\{\mathbf{R}\in\mathbb{R}^{n\times n}|\mathbf{R}\mathbf{R}^T=\mathbf{I}, det(\mathbf{R})=1\}$
特殊欧式群（SEG, Special Euclidean Group）
$SE(3)=\left\{\mathbf{T}=\left[ \begin{matrix} \mathbf{R} & \mathbf{t}\\ \mathbf{0^T} &1 \end{matrix} \right]\in\mathbb{R}^{4\times 4}|\mathcal{R}\in SO(3), \mathbf{t}\in\mathbb{R}^3\right\}$

李代数

考虑旋转矩阵 $\mathbf{R}$ ，且满足
$\mathbf{R}\mathbf{R}^T=\mathbf{I}$
进一步假设 $\mathbf{R}$ 为随时间的旋转 $\mathbf{R}(t)$ .
$\mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I}$
关于时间求导：
$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=0$
即
$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-\left(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T\right)^T$
可以看出 $\dot{\mathbf{R}}(t)\mathbf{R}(t)^T$ 是一个反对称矩阵。于是可以找到反对称的三维向量 $\phi(t)$ 满足
$\dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^{\wedge}$
右乘 $\mathbf{R}(t)$ ，有
$\dot{\mathbf{R}}(t)=\phi(t)^{\wedge}\mathbf{R}(t)=\left[ \begin{matrix} 0 & -\phi_3 & \phi_2\\ \phi_3 & 0 & -\phi_1\\ -\phi_2 & \phi_1 & 0 \end{matrix}\right]$
令 $t_0=0$ ，且 $\mathbf{R}(0)=\mathbf{I}$ ，做泰勒展开即有
$\mathbf{R}(t)\approx\mathbf{R}(t_0)+\dot{\mathbf{R}}(t)(t-t_0)=\mathbf{I}+\phi(t)^{\wedge}(t)$
由其可知 $\phi$ 反应了 $\mathbf{R}$ 的导数性质，故称它在SO(3)原点附近的正切空间上。
即有 $\dot{\mathbf{R}}(t)=\phi_0^{\wedge}\mathbf{R}(t)$ ，且初始条件为 $\mathbf{R}(0)=\mathbf{I}$ , 即可解微分方程得：
$\mathbf{R}(t)=exp(\phi_0^{\wedge}t)$

结论：给定某个时刻的 $\mathbf{R}$ ，我们就能求得一个 $\phi$ ，它描述了 $\mathbf{R}$ 在局部的导数关系。
这里的 $\phi$ 实际上是一种李代数 $\mathcal{so}(3)$ 。

$\mathcal{so}(3)$ 的元素是三维向量或者三维反对称矩阵：
$\mathcal{so}(3)=\{\phi\in\mathbb{R}^3, \mathbf{\Phi}=\phi^\in\mathbb{R}^{3\times3}\}$ 它与SO(3)的关系由指数映射给定： $\mathbf{R}=exp(\phi^{\wedge})$

$\mathcal{se}(3)=\left\{\mathbf{\xi}=\left[\mathbf{ \begin{matrix} \mathbf{\rho} \\ \mathbf{\phi} \end{matrix}}\right]\in\mathbb{R}^6, \mathbf{\rho}\in\mathbb{R}^3, \mathbf{\phi}\in\mathcal{so}^3, \mathbf{\xi}^{\wedge} = \left[\begin{matrix} \mathbf{\phi}^{\wedge}&\mathbf{\rho} \\ \mathbf{0}^T & 0 \end{matrix}\right]\in\mathbb{R}^{4\times4}\right\}$

推导

$exp(\mathbf{\phi}^{\wedge})=\sum_{n=0}^\infty{\frac{1}{n!}(\mathbf{\phi}^{\wedge})^n}$ 这里的 $\phi$ 是三维向量，令它的模长和方向为 $\theta$ 、 $\mathbf{a}$ 。令 $\mathbf{a}$ 为长度为1的方向向量，则有 $\phi=\theta\mathbf{a}$
通过推导可得 $exp(\mathbf{\phi}^{\wedge})=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{a}\mathbf{a}^T+\sin{\theta}\mathbf{a}^{\wedge}$

Lie group & Lie algebra

Lie group & Lie algebra

基础概念

外积（外积的方向垂直于两个向量）

1、旋转矩阵

特殊正交群（SOG, Special Orthogonal Group）

特殊欧式群（SEG, Special Euclidean Group）

2、旋转向量与欧拉角

罗德里格斯公式（Rodrigues's Formula）

3、四元数

四元数表示旋转

四元数&旋转矩阵

小结

李代数

推导

Lie group & Lie algebra

基础概念

外积 （外积的方向垂直于两个向量）

1、旋转矩阵

特殊正交群（SOG, Special Orthogonal Group）

特殊欧式群（SEG, Special Euclidean Group）

2、旋转向量与欧拉角

罗德里格斯公式（Rodrigues's Formula）

3、四元数

四元数表示旋转

四元数&旋转矩阵

小结

李代数

推导

外积（外积的方向垂直于两个向量）