旋转的表示

计算机视觉

---

1. 表示形式

旋转的四种表示形式：

• 旋转矩阵（Rotation matrix）
• 轴角（Rotation vector/Axis angle）
• 四元数（Quaternion）
• 欧拉角（Euler angles）

1.1 旋转群 SO(3)

 Rotations are linear transformations that preserve vector length and relative vector orientation (i.e., handedness).

A rotation operator $r(\mathbf v)$ acting on vevtor $\mathbf v \in \mathbb R^3$:

• Rotation preserves the vector norm.
  $$\|r(\mathbf v)\| = \sqrt{r(\mathbf v)^Tr(\mathbf v)} = \sqrt{\mathbf v ^T \mathbf v}\|\mathbf v\|, \quad \forall \mathbf v \in \mathbb R^3 \tag{1.1a}$$
• Rotation preserves angles between vector.
  $$r(\mathbf v)^Tr(\mathbf w) = \mathbf v^T \mathbf w = \|\mathbf v\|\|\mathbf w\|\text{cos}\alpha, \quad \forall \mathbf v, \mathbf w \in \mathbb R^3 \tag{1.2b}$$
• Rotation preserves the relative orientations of vectors.
  $$\mathbf v \times \mathbf u = \mathbf w \iff r(\mathbf v) \times r(\mathbf u) = r(\mathbf w), \quad \forall \mathbf u, \mathbf v, \mathbf w \in \mathbb R^3 \tag{1.2c}$$

we can define the rotation group $SO(3)$ as,

$$SO(3) : \{r : \mathbb R^3 \rightarrow \mathbb R^3 | \forall \mathbf v,\mathbf w\in \mathbb R^3, \|r(\mathbf v)\| = \|\mathbf v\|, r(\mathbf v) \times r(\mathbf w) = r(\mathbf v\times \mathbf w) \} \tag{1.3}$$

1.2 旋转向量

如图，向量$\mathbf x$通过轴$\mathbf u$，旋转$\phi$得到向量$\mathbf x'$（右手定理）。记向量$\mathbf x$在$\mathbf u$上投影为$\mathbf x_\parallel$，垂直方向的投影为$\mathbf x_\perp$，有：

$$\mathbf x = \mathbf x_\parallel + \mathbf x_\perp \tag{1.2.1}$$

其中

$$\begin{align} \mathbf x_\parallel &= \mathbf u (\|\mathbf x\|\text{cos}\alpha) = \mathbf u\mathbf u^T \mathbf x \tag{1.2.2a}\\ \mathbf x_\perp &= \mathbf x-\mathbf x_\parallel = \mathbf x - \mathbf u\mathbf u^T\mathbf x \tag{1.2.2b} \end{align}$$

这里的$\mathbf x_\perp$表示了在平面上的旋转。建立基底$\{\mathbf e_1, \mathbf e_2\}$

$$\begin{align} \mathbf e_1 &= \mathbf x_\perp \tag{1.2.3a}\\ \mathbf e_2 &= \mathbf u\times \mathbf x_\perp = \mathbf u\times \mathbf x\tag{1.2.3b} \end{align}$$

令$\|\mathbf e_1\|=\|\mathbf e_2\|$，有$\mathbf x_\perp=\mathbf e_1 \cdot 1 + \mathbf e_2 \cdot 0$。在平面上旋转$\phi\text{rad}$，有

$$\mathbf x'_\perp=\mathbf e_1 \cdot \text{cos}\phi + \mathbf e_2 \cdot \text{sin}\phi \tag{1.2.4}$$

从而有

$$\mathbf x'_\perp=\mathbf x_\perp \text{cos}\phi + (\mathbf u\times \mathbf x) \text{sin}\phi \tag{1.2.5}$$

最终我们可以得到旋转后的向量，通过$\mathbf x'=\mathbf x'_\parallel+\mathbf x'_\perp$，得到向量旋转公式（vector rotation formula）

$$\mathbf x'= \mathbf x_\parallel + \mathbf x_\perp \text{cos}\phi + (\mathbf u\times \mathbf x) \text{sin}\phi \tag{1.2.6}$$

1.3 旋转矩阵

映射$r(\bullet)$可以使用矩阵$\mathbf R\in \mathbb R^{3\times3}$表示:

$$r(\mathbf v) = \mathbf R\mathbf v \tag{1.4}$$

旋转用矩阵形式表示，则特殊正交群表示为：

$$SO(3) = \{\mathbf R\in \mathbb R^{3\times3}|\mathbf R\mathbf R^T=\mathbf 1, \text{det}\mathbf R=1\} \tag{1.5}$$

与其对应的李代数表示为：

$${\frak so}(3) = \{ \Phi=\phi^\land\in\mathbb R^{3\times3}|\phi\in\mathbb R^3\}\\ \text{with Lie bracket}: [\Phi_1,\Phi_2] = \Phi_1\Phi_2 - \Phi_2\Phi_1 \tag{1.6}$$

$[\mathbf v]_\times$可以表示为$\mathbf v^\land$，这里有：

$$[\mathbf v]_\times = \mathbf v^\land = \begin{bmatrix}v_1\\v_2\\v_3\end{bmatrix}^\land = \begin{bmatrix}0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0\end{bmatrix}\in\mathbb R^{3\times3}, \mathbf v \in \mathbb R^3 \tag{1.7}$$
$\bullet^\vee$是$\bullet^\land$的逆运算。

四元数

四元数的乘法表示为：

$$\mathbf p \otimes \mathbf q = \begin{bmatrix}p_wq_w-\mathbf p_v^T\mathbf q_v\\ p_w\mathbf q_v + q_w\mathbf p_v + \mathbf p_v \times \mathbf q_v\end{bmatrix}$$

四元数作为另外一种表示形式，表示为：

$$r(\mathbf v) = \mathbf q \otimes \mathbf v \otimes \mathbf q^*$$

2. 转换关系

2.1 旋转矩阵与轴角

a. $[\mathbf v]_\times \overset{\text{exp}}{\Longrightarrow} \mathbf R$

旋转属于刚体运动，其在$SO(3)$上的轨迹$r(t)$可以认为是从初始位置$r(0)$开始对刚体的连续转动。由$\mathbf R^T\mathbf R = \mathbf I$，对时间求导，有：

$$\frac{d}{dt}(\mathbf R^T\mathbf R) = \dot{\mathbf R}^T\mathbf R + \mathbf R^T\dot{\mathbf R} = 0 \tag{2.1.1}$$

可以得到，

$$\mathbf R^T\dot{\mathbf R} = -(\mathbf R^T\dot{\mathbf R})^T \tag{2.1.2}$$

可见，$\mathbf R^T\dot{\mathbf R}$是一个$3\times3$的反对称阵（skew-symmetric），属于${\frak so}(3)$。根据式$(1.7)$我们有一对一的映射关系$\mathbf\omega\in\mathbb R^3 \leftrightarrow [\mathbf\omega]_\times\in{\frak so}(3)$，可以写作

$$\mathbf R^T\dot{\mathbf R} = [\mathbf\omega]_\times \tag{2.1.3}$$
从而我们可以得到微分方程(ODE):

$$\dot{\mathbf R} = \mathbf R[\mathbf\omega]_\times \tag{2.1.4}$$
在原点附近，$\mathbf R = \mathbf I$，等式化为$\dot{\mathbf R} = [\mathbf\omega]_\times$。我们可以解释李代数${\frak so}(3)$是$r(t)$在原点导数构成的空间，也就组成了$SO(3)$的切空间（tangent space），或者向量空间（vectocity space）。因此，$\mathbf\omega$也被称为瞬时角速度。

如果$\mathbf\omega$是恒定的，对上式做积分（对微分方程求解）可以得到：

$$\mathbf R(t) = \mathbf R(0)e^{[\mathbf\omega]_\times t} = \mathbf R(0)e^{[\mathbf\omega t]_\times} \tag{2.1.5}$$

这里的$R(t)$，$R(0)$都为旋转矩阵，$e^{[\mathbf\omega t]_\times}$也为旋转矩阵。定义$\mathbf v = \omega \Delta t$为在$\Delta t$期间的旋转向量，有：

$$\mathbf R=e^{[\mathbf v]_\times} \tag{2.1.5}$$

这就是从${\frak}so(3)$到$SO(3)$的指数映射：

$$\color{blue}{\text{exp}}:{\frak so}(3)\rightarrow SO(3); \quad [\mathbf v]_\times \mapsto \text{exp}([\mathbf v]_\times)=e^{[\mathbf v]_\times} \tag{2.1.6}$$

b. $\mathbf v \overset{\text{Exp}}{\Longrightarrow} \mathbf R$

定义从$\mathbb R^3 \mapsto SO(3)$的映射为$\text{Exp}$，有

$$\color{blue}{\text{Exp}}: \mathbb R^3\rightarrow SO(3); \quad \mathbf v \mapsto \text{Exp}(\mathbf v)=e^{[\mathbf v]_\times} \tag{2.1.7}$$

与$\text{exp}$的关系为

$$\text{Exp}(\mathbf v) \triangleq\text{exp}([\mathbf v]_\times) \tag{2.1.8}$$
记$\mathbf v = \omega\Delta t=\phi \mathbf u$，称为旋转向量（rotation vector）或者轴角向量（angle-axis vector）， $\mathbf u$为旋转轴，$\phi$为旋转角度。

c. $\mathbf R \overset{\text{log}}\Longrightarrow [\mathbf v]_\times$

与指数映射相反，给出对数映射：

$$\color{blue}{\text{log}}:SO(3)\rightarrow {\frak so}(3); \quad \mathbf R \mapsto \text{log}(\mathbf R)=[\mathbf u\phi]_\times \tag{2.1.9}$$

d. $\mathbf R \overset{\text{Log}}\Longrightarrow \mathbf v$

对于旋转向量$\mathbf v = \mathbf u\phi\in \mathbb R^3$，从旋转矩阵的映射记为：

$$\color{blue}{\text{Log}} : SO(3)\rightarrow\mathbb R^3; \quad \mathbf R\mapsto \text{Log}(\mathbf R) = \mathbf u\phi \tag{2.1.10}$$

和对数映射$\text{log}$之间的关系为:

$$\text{Log}(\mathbf R) \triangleq (\text{log}(\mathbf R))^\vee \tag{2.1.11}$$

e. 转换公式

旋转向量$\mathbf v = \phi \mathbf u$通过指数映射，$\mathbf R=e^{[\mathbf v]_\times}$做泰勒展开：

$$\mathbf R = e^{\phi[\mathbf u]_\times} = \mathbf I + \phi[\mathbf u]_\times+\frac{1}{2}\phi^2[\mathbf u]_\times^2+\frac{1}{3}\phi^3[\mathbf u]_\times^3+\frac{1}{4}\phi^4[\mathbf u]_\times^4+\cdots \tag{2.1.12}$$

对于单位向量$\mathbf u$，叉乘矩阵$[\mathbf u]_\times$满足:

$$\begin{align} [\mathbf u]_\times^2 &= \mathbf u\mathbf u^T-\mathbf I \tag{2.1.13}\\ [\mathbf u]_\times^3 &= -[\mathbf u]_\times \tag{2.1.14} \end{align}$$

使用上述两个公式，可以得到：

$$[\mathbf u]_\times^4 = -[\mathbf u]_\times^2, \quad [\mathbf u]_\times^5 = -[\mathbf u]_\times, \quad [\mathbf u]_\times^6 = [\mathbf u]_\times^2, \quad [\mathbf u]_\times^7 = -[\mathbf u]_\times \cdots \tag{2.1.15}$$

通过对比$\text{sin}\phi$和$\text{cos}\phi$可以得到从旋转向量到旋转矩阵的封闭形式的表达式，也称为罗德里格斯公式（Rodrigues rotation formula）:

$$\color{green}{\mathbf R = \mathbf I + \text{sin}\phi[\mathbf u]_\times + (1-\text{cos}\phi)[\mathbf u]_\times^2 \tag{2.1.16a}}$$

记作$\mathbf R=\mathbf R\{\mathbf v\}\triangleq\text{Exp}(\mathbf v)$。同时有另外的形式表示为

$$\mathbf R = \mathbf I\text{cos}\phi + [\mathbf u]_\times\text{sin}\phi + \mathbf u\mathbf u^T(1-\text{cos}\phi) \tag{2.1.16b}$$

对$\mathbf R$求迹可得

$$\begin{split} \text{trace}(\mathbf R) &= \text{trace}(\mathbf I) + \text{sin}\phi\cdot\text{trace}([\mathbf u]_\times) + (1-\text{cos}\phi)\cdot\text{trace}(\mathbf u\mathbf u^T-\mathbf I)\\ &= 3 + 0 + (1-\text{cos}\phi)(\mathbf u^T\mathbf u-3)\\ &= 1 + 2\text{cos}\phi \end{split} \tag{2.1.17}$$

因此有：

$$\color{green}{\begin{align} \phi &= \text{arcos}\begin{pmatrix}\frac{\text{trace}(\mathbf R)-1}{2}\end{pmatrix} \tag{2.1.18a}\\ \mathbf u &= \frac{(\mathbf R-\mathbf R^T)^\vee}{2\text{sin}\phi} \tag{2.1.18b} \end{align}}$$

f. 旋转操作

为了说明旋转矩阵通过对数映射得到的向量就是旋转向量，这里给出证明。

把一个向量$\mathbf x$绕着单位旋转轴$\mathbf u$旋转一定角度$\phi$，可以表示为如下的线性乘法：

$$\mathbf x' = \mathbf R\mathbf x\tag{2.1.19}$$

这里的$\mathbf R = \text{Exp}(\mathbf u\phi)$.

$$\begin{split} \mathbf x' &= \mathbf R\mathbf x\\ &= (\mathbf I + \text{sin}\phi[\mathbf u]_\times + (1-\text{cos}\phi)[\mathbf u]_\times^2)\cdot\mathbf x\\ &= \mathbf x + \text{sin}\phi[\mathbf u]_\times\mathbf x + (1-\text{cos}\phi)[\mathbf u]_\times^2\mathbf x\\ &= \mathbf x + \text{sin}\phi(\mathbf u \times\mathbf x) + (1-\text{cos}\phi)(\mathbf u\mathbf u^T-\mathbf I)\mathbf x\\ &= \mathbf x_\parallel + \mathbf x_\perp + (\mathbf u \times\mathbf x)\text{sin}\phi + (\text{cos}\phi-1)\mathbf x_\perp\\ &= \mathbf x_\parallel + \mathbf x_\perp\text{cos}\phi + (\mathbf u \times\mathbf x)\text{sin}\phi \end{split}\tag{2.1.20}$$
可见结果就是向量旋转公式$(1.2.6)$

2.2 四元数与轴角

a. $\mathbf V\overset{\text{exp}}\Longrightarrow\mathbf q$

考虑单位四元数$\mathbf q\in S^3$，有$\mathbf q^*\otimes \mathbf q = 1$，对时间求导，有：

$$\frac{d(\mathbf q^*\otimes \mathbf q)}{dt} = \dot{\mathbf q}^*\otimes \mathbf q+\mathbf q^*\otimes \dot{\mathbf q} = 0 \tag{2.2.1}$$

有：

$$\mathbf q^*\otimes \dot{\mathbf q} = -(\dot{\mathbf q}^*\otimes \mathbf q)= - (\mathbf q^*\otimes \dot{\mathbf q})^* \tag{2.2.2}$$

可以得到$\mathbf q^*\otimes \dot{\mathbf q}$是纯四元数。我们可以记作：

$$\mathbf q^*\otimes \dot{\mathbf q} = \mathbf\Omega = \begin{bmatrix}0\\\mathbf\Omega\end{bmatrix} \in \mathbb H_p \tag{2.2.3}$$

把式子两边左乘$\mathbf q$可以得到：

$$\dot{\mathbf q} = \mathbf q\otimes \mathbf\Omega \tag{2.2.4}$$

如果$\Omega$恒定，解一阶齐次线性微分方程可得：

$$\mathbf q(t) = \mathbf q(0) \otimes e^{\mathbf\Omega t} \tag{2.2.5}$$

这里的$\mathbf q(t)$、$\mathbf q(0)$都是单位（unit）四元数，$e^{\mathbf\Omega t}$同样也是单位四元数。定义$\mathbf V = \mathbf\Omega\Delta t$，有

$$\mathbf q = e^{\mathbf V} \tag{2.2.6}$$

这里给出的是从纯四元数空间到表示运动的单位四元数的映射关系

$$\color{blue}{\text{exp}} : \mathbb H_p \mapsto S^3; \quad \mathbf V \mapsto \text{exp}(\mathbf V) = e^{\mathbf V} \tag{2.2.7}$$

b. $\mathbf v\overset{\text{Exp}}\Longrightarrow\mathbf q$

上述表示了$\mathbf \Omega\in\mathbb H_p$到四元数的映射，但是纯四元数空间$\mathbb H_p$是单位球$S^3$的切空间（或李代数），并不是直接的速度空间（velocity space）。实际上是一个“半速空间”，我们有$\omega = 2\Omega\in\mathbb R^3$。有$\mathbf V = \theta\mathbf u = \phi \mathbf u/2$。我们可以直观的考虑，用四元数表示对一个三维向量的旋转为$\mathbf x'=\mathbf q \otimes \mathbf x \otimes \mathbf q^*$，向量$\mathbf x$经历了“两次”在四元数的作用，换句话说，四元数$\mathbf q$只对我们在$\mathbf x$上的旋转进行了“一半”的编码。具体会在下文说明。

为了表示直接从旋转向量$\mathbf v = \phi\mathbf u\in \mathbb R^3$到四元数的映射关系。这里体现half-angle effect， 定义映射如下：

$$\color{blue}{\text{Exp}}: \mathbf R^3 \mapsto S^3; \quad \mathbf v\mapsto\text{Exp}(\mathbf v)=e^{\mathbf v/2} \tag{2.2.8}$$

与$\text{exp}$的关系如下：

$$\text{Exp}(\mathbf v) \triangleq \text{exp}(\mathbf v/2) \tag{2.2.9}$$

同时根据角速度$\mathbf \omega = 2\Omega\in \mathbb R^3$，式子$(2.2.4)$和$(2.2.6)$变为

$$\begin{align} \dot{\mathbf q} &= \frac{1}{2}\mathbf q\otimes \mathbf\omega \tag{2.2.10}\\ \mathbf q &= e^{\mathbf v/2} \tag{2.2.11} \end{align}$$

c. $\mathbf q\overset{\text{log}}\Longrightarrow\mathbf V$

与指数映射对应，这里给出对数映射：

$$\color{blue}{\text{log}}: S^3 \mapsto \mathbb H_p; \quad \mathbf q\mapsto\text{log}(\mathbf q)=\mathbf u\theta \tag{2.2.12}$$

d. $\mathbf q\overset{\text{Log}}\Longrightarrow\mathbf v$

同时给出$\text{Log}$映射：

$$\color{blue}{\text{Log}}: S^3 \mapsto \mathbb R^3; \quad \mathbf q\mapsto\text{Log}(\mathbf q)=\mathbf u\phi \tag{2.2.13}$$

很显然两个指数映射之间的关系为：

$$\text{Log}(\mathbf q) \triangleq 2\text{log}(\mathbf q) \tag{2.2.14}$$

e. 转换公式

先考虑从旋转向量$\mathbf v=\phi\mathbf u$到四元数的转换。我们可以通过对欧拉公式进行推导得到：

$$\color{green}{\mathbf q \triangleq \text{Exp}(\phi\mathbf u) = e^{\phi\mathbf u/2} = \text{sin}\frac{\phi}{2} + \mathbf u\text{sin}\frac{\phi}{2} = \begin{bmatrix}\text{cos}(\phi/2)\\\mathbf u\text{sin}(\phi/2)\end{bmatrix}} \tag{2.2.15}$$
这也就是指数映射$\mathbf q = \mathbf q\{\mathbf v\}\triangleq\text{Exp}(\mathbf v)$

由于$\mathbf q = (q_w, \mathbf q_v)^T$，我们可以从得到：

$$\begin{align} \phi &= 2\text{arctan}(\|\mathbf q_v\|, q_w) \tag{2.2.16a}\\ \mathbf u &= \mathbf q_v/\| \mathbf q_v\| \tag{2.2.16b} \end{align}$$

对于小角度旋转，对$\text{arctan}()$做一阶泰勒展开近似，有：

$$\text{Log}(\mathbf q)=\phi\mathbf u\approx 2\frac{\mathbf q_v}{\mathbf q_w}(1-\frac{\|\mathbf q_v\|^2}{2q_2^2}) \tag{2.2.17}$$

f. 旋转操作

把一个向量$\mathbf x$围绕轴$\mathbf u$选旋转一定角度$\phi$可以使用双四元数乘法来实现：

$$\mathbf x'=\mathbf q \otimes \mathbf x \otimes \mathbf q^* \tag{2.2.18}$$
这里的$\mathbf q=\text{Exp}(\mathbf u\phi)$，我们把向量$\mathbf x$写作纯四元数的形式
$$\mathbf x=xi+yj+zk=\begin{bmatrix}0\\\mathbf x\end{bmatrix}\in \mathbb H_p \tag{2.2.19}$$
四元数乘法为：
$$\mathbf p \otimes \mathbf q = \begin{bmatrix}p_wq_w-\mathbf p_v^T\mathbf q_v\\ p_w\mathbf q_v + q_w\mathbf p_v + \mathbf p_v \times \mathbf q_v\end{bmatrix}$$

根据四元数的乘法和公式$(2.2.15)$，我们有

$$\begin{array} \mathbf x' &= \mathbf q \otimes \mathbf x \otimes \mathbf q^* \\ &= (\text{sin}\frac{\phi}{2} + \mathbf u\text{sin}\frac{\phi}{2})\otimes(0+\mathbf x)\otimes(\text{sin}\frac{\phi}{2} + \mathbf u\text{sin}\frac{\phi}{2})\\ &= \mathbf x \text{cos}^2\frac{\phi}{2} + (\mathbf u\otimes\mathbf x-\mathbf x\otimes\mathbf u)\text{sin}\frac{\phi}{2}\text{cos}\frac{\phi}{2} - \mathbf u\otimes\mathbf x\otimes\mathbf u\text{sin}^2\frac{\phi}{2}\\ &= \mathbf x \text{cos}^2\frac{\phi}{2} + 2(\mathbf u\times\mathbf x)\text{sin}\frac{\phi}{2}\text{cos}\frac{\phi}{2} - (\mathbf x(\mathbf u^T\mathbf u)-2\mathbf u(\mathbf u^T\mathbf x))\text{sin}^2\frac{\phi}{2}\\ &= \mathbf x (\text{cos}^2\frac{\phi}{2}-\text{sin}^2\frac{\phi}{2}) + (\mathbf u\times\mathbf x)(2\text{sin}\frac{\phi}{2}\text{cos}\frac{\phi}{2}) + \mathbf u(\mathbf u^T\mathbf x)(2\text{sin}^2\frac{\phi}{2})\\ &= \mathbf x \text{cos}\phi + (\mathbf u\times\mathbf x)\text{sin}\phi + \mathbf u(\mathbf u^T\mathbf x)(1-\text{cos}\phi)\\ &= (\mathbf x - \mathbf u\mathbf u^T\mathbf x)\text{cos}\phi + (\mathbf u\times\mathbf x)\text{sin}\phi + \mathbf u\mathbf u^T\mathbf x\\ &= \mathbf x_\perp\text{cos}\phi + (\mathbf u \times\mathbf x)\text{sin}\phi + \mathbf x_\parallel \end{array}\tag{2.2.20}$$

显然这就是向量旋转公式。注：这里的$\mathbf u\otimes\mathbf x\otimes\mathbf u$可以化为

$$\require{cancel} \begin{array} && \mathbf u \otimes \mathbf x \otimes \mathbf u\\ &= (-\mathbf u^T \mathbf x + \mathbf u \times \mathbf x) \otimes \mathbf u\\ &= -(\mathbf u^T \mathbf x)\mathbf u + (\mathbf u \times \mathbf x) \otimes \mathbf u\\ &= -(\mathbf u^T \mathbf x)\mathbf u - \cancel{(\mathbf u \times \mathbf x)^T \mathbf u} + (\mathbf u \times \mathbf x) \times\mathbf u\\ &= -(\mathbf u^T \mathbf x)\mathbf u - \mathbf u \times (\mathbf u \times \mathbf x) \\ &= -(\mathbf u^T \mathbf x)\mathbf u - (\mathbf u \mathbf u^T - \mathbf I) \mathbf x \\ &= \mathbf x - 2\mathbf u(\mathbf u^T \mathbf x) \end{array} \tag{2.2.21}$$

在$(2.2.20)$第三个等式中连续叉乘的化简使用了叉乘的反交换率和公式$[\mathbf u]_\times^2=\mathbf u\mathbf u^T - \mathbf I$。此外也可以使用如下公式

$$\mathbf a \times (\mathbf b \times \mathbf c) = (\mathbf c \times \mathbf b) \times \mathbf a = (\mathbf a^T\mathbf c)\mathbf b - (\mathbf a^T\mathbf b)\mathbf c \tag{2.2.22}$$

公式$(2.2.19)$证明了四元数也可以表示旋转，旋转向量通过指数映射$\mathbf q=\text{Exp}(\mathbf u\phi)$可以得到四元数。接下来将对四元数的“半角效应”进行说明。

对于旋转轴$\mathbf v = \mathbf u \phi$，通过指数映射有四元数：

$$\mathbf q = \begin{bmatrix}q_w\\\mathbf q_v\end{bmatrix} = \begin{bmatrix}\text{cos}(\phi/2)\\\mathbf u\text{sin}(\phi/2)\end{bmatrix} \tag{2.2.23}$$
考虑四元数$\mathbf q$与单位（identity）四元数$\mathbf q_1=[1,0,0,0]^T$之间的夹角：
$$\text{cos}\theta = \mathbf q_1^T\mathbf q=\mathbf q(1) = \mathbf q_w \tag{2.2.24}$$
因此我们有$q_w=\text{cos}(\phi/2)=\text{cos}\theta$，可见四元数在4D空间中与单位（identity）四元数$\mathbf q_1$的夹角是四元数对3D空间向量旋转的旋转角度的一半。
$$\theta = \phi/2 \tag{2.2.25}$$

2.3 四元数与旋转矩阵

在给定旋转向量$\mathbf v = \mathbf u\phi$，我们有旋转矩阵和四元数的指数映射$\mathbf q = \text{Exp}(\mathbf u \phi)$，$\mathbf R = \text{Exp}(\mathbf u \phi)$，他们对向量$\mathbf v$做旋转操作的结果都是使其绕着同一旋转轴$\mathbf u$旋转同一角度$\phi$。

也就对：

$$\forall \mathbf x,\mathbf x \in \mathbb R^3, \mathbf q = \text{Exp}(\mathbf v), \mathbf R = \text{Exp}(\mathbf v) \tag{2.3.1}$$
有：
$$\mathbf q \otimes \mathbf x \otimes \mathbf q^* = \mathbf R \mathbf v \tag{2.3.2}$$
两边都是对$\mathbf x$做线性操作，我们可以得到四元数到旋转矩阵的变换公式：
$$\color{green}{\mathbf R = \begin{bmatrix} q_w^2+q_x^2-q_y^2-q_z^2 & 2(q_xq_y-q_wq_z) & 2(q_xq_z+q_wq_y)\\ 2(q_xq_y+q_wq_z) &q_w^2-q_x^2+q_y^2-q_z^2 & 2(q_yq_z-q_wq_x)\\ 2(q_xq_z-q_wq_y) & 2(q_yq_z+q_wq_x) & q_w^2-q_x^2-q_y^2+q_z^2 \end{bmatrix}\tag{2.3.3}}$$
记作$\mathbf R = \mathbf R\{\mathbf q\}$。通过四元数的左乘矩阵和右乘矩阵，我们有：
$$\mathbf q \otimes \mathbf x \otimes \mathbf q^* = [\mathbf q^*]_R[\mathbf q]_L \begin{bmatrix}0\\\mathbf x\end{bmatrix}= \begin{bmatrix}0\\\mathbf R\mathbf x\end{bmatrix} \tag{2.3.4}$$
通过上式可以解得：
$$\color{green}{\mathbf R = (q_w^2-\mathbf q_v^T\mathbf q_v)\mathbf I + 2\mathbf q_v^T\mathbf q_v + 2q_w[\mathbf q_c]_\times \tag{2.3.5}}$$
旋转矩阵和四元数之间有如下性质：
$$\begin{align} \mathbf R{[1,0,0,0]^T} &= \mathbf I \tag{2.3.6}\\ \mathbf R\{-\mathbf q\} &= \mathbf R\{\mathbf q\} \tag{2.3.7}\\ \mathbf R\{\mathbf q^*\} &= \mathbf R\{\mathbf q\}^T \tag{2.3.8}\\ \mathbf R\{\mathbf q_1 \otimes \mathbf q_2\} &= \mathbf R\{\mathbf q_1\}\mathbf R\{\mathbf q_2\} \tag{2.3.9}\\ \mathbf R\{\mathbf q^t\} &= \mathbf R\{\mathbf q\}^t \tag{2.3.10} \end{align}$$

---

备注：
1. 对于一阶线性微分方程：

$$\frac{dy}{dx}+P(x)y=Q(x)$$
假设$P(x)$，$Q(x)$都连续。令$Q(x)=0$有
$$\frac{dy}{dx}+P(x)y=0$$
为一阶齐次线性方程，其通解为：
$$y=Ce^{-\int P(x)dx}$$

1. 泰勒展开
   $$\begin{split} e^x &= 1 + x + \frac{1}{2!}x^2 + \cdots + \frac{1}{n!}x^n + \cdots\\ \text{sin}(x) &＝x-\frac{1}{3!}x^3+\frac{1}{5!}x^5+\cdots+(-1)^{k-1}\frac{x^{2k-1}}{(2k-1)!}x^{2k-1} + \cdots\\ \text{cos}(x) &＝1-\frac{1}{2!}x^2+\frac{1}{4!}x^4+\cdots+(-1)^{k}\frac{x^{2k}}{(2k)!}x^{2k}+\cdots\\ \text{arctanx}(x) &＝x-\frac{1}{3}x^3+\frac{1}{5}x^5-\frac{1}{7}x^7+\frac{1}{9}x^9 + \cdots \end{split}$$

参考：
Quaternion kinematics for the error-state Kalman filter