《视觉SLAM十四讲》公式推导（二）

CH3-5 四元数表示旋转

三维空间中任意点均可用一个纯虚四元数表示即$\mathbf{p}=[0,\mathbf{v}{]}^T$，经一个单位四元数$\mathbf{q}$的旋转后，得到${\mathbf{p}}^{\prime }$，则

$${\mathbf{p}}^{\prime }=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}\text{\hspace{1em}(3-5-1)}$$

最终${\mathbf{p}}^{\prime }$的虚部即为旋转后点的坐标。

首先证明几个定理。注意这里向量和矩阵的转换。

（1）首先证明 $\mathrm{Gra}\beta \mathrm{mann}$积定理，也就是四元数乘法规则。

两个四元数 $q_1=a+bi+cj+dk$ 、$q_2=e+fi+gj+hk$，则

$$\begin{array}{rl}{q}_1{q}_2& =(a+bi+cj+dk)(e+fi+gj+hk)\\ & =ae+afi+agj+ahk+bei+bf{i}^2+bgij+bhik+\\ & +cej+cfji+cg{j}^2+chjk+dek+dfki+dgkj+dh{k}^2\\ & =ae+afi+agj+ahk+bei-bf+bgk-bhj+cej-cfk-cg+chi+dek+dfj-dgi-dh\\ & =(ae-bf-cg-dh)+(af-dg+be+ch)i+(-bh+ce+df+ag)j+(ah+bg-cf+de)k\end{array}\text{\hspace{1em}(3-5-2)}$$

写成矩阵形式，即

q 1 q 2 = [ a − b − c − d b a − d c c d a − b d − c b a ] [ e f g h ] (3-5-3) q_1q_2=\left[\begin{array}{c} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{array}\right]\left[\begin{array}{c} e \\ f\\ g \\ h \end{array}\right] \tag{3-5-3} q1​q2​= ​abcd​−bad−c​−c−dab​−dc−ba​ ​ ​efgh​ ​(3-5-3)

令 $v=(b,c,d)$、$u=(f,g,h)$，则有

$$v\cdot u=bf+cg+dh\text{\hspace{1em}(3-5-4)}$$

v ⃗ × u ⃗ = ∣ i ⃗ j ⃗ k ⃗ b c d f g h ∣ = ( c h − d g ) i ⃗ + ( d f − b h ) j ⃗ + ( b g − c f ) k ⃗ (3-5-5) \begin{aligned} \vec{v}\times\vec{u}&=\left|\begin{array}{c} \vec{i} & \vec{j} & \vec{k} \\ b & c & d \\ f & g & h \end{array}\right| \\ &=(ch-dg)\vec{i}+(df-bh)\vec{j}+(bg-cf)\vec{k} \end{aligned} \tag{3-5-5} v ×u ​= ​i bf​j ​cg​k dh​ ​=(ch−dg)i +(df−bh)j ​+(bg−cf)k ​(3-5-5)

综合式（3-5-2）、式（3-5-4）、式（3-5-5），得

$$q_1{q}_2=[ae-v\cdot u,au+ev+v\times u]\text{\hspace{1em}(3-5-6)}$$

综上：$\mathrm{Gra}\beta \mathrm{mann}$积定理

对任意四元数 $q_1=[s,v]$，$q_2=[t,u]$，有

$$q_1{q}_2=[st-v\cdot u,su+tv+v\times u]\text{\hspace{1em}(3-5-7)}$$

对于纯四元数 $q_1=[0,v]$，$q_2=[0,u]$，有

$$q_1{q}_2=[-v\cdot u,v\times u]\text{\hspace{1em}(3-5-8)}$$

（2）证明：$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}$（$q^{\ast }$为$q$ 的共轭）.

$q$ 为单位四元数，则

$$q{q}^{-1}=1\text{\hspace{1em}(3-5-9)}$$

将上式左乘或右乘 $q\ast$，有

$$q^{\ast }q{q}^{-1}=q^{\ast },q{q}^{-1}{q}^{\ast }=q^{\ast }\text{\hspace{1em}(3-5-10)}$$

不妨设 $q=[s,v]$，其中 $v=[q_1,q_2,q_3]$，则根据四元数乘法

$$\begin{array}{rl}{q}^{\ast }q& =[s,-v][s,v]\\ & =[s^2-{\mathbf{v}}^{\mathrm{T}}\mathbf{v},0]\\ & =(s^2-{\mathbf{v}}^{\mathrm{T}}\mathbf{v})+0\mathbf{i}+0\mathbf{j}+0\mathbf{k}\\ & =s^2+q_1^2+q_2^2+q_3^2\\ & =\Vert q{\Vert }^2\end{array}\text{\hspace{1em}(3-5-11)}$$

则可得

$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}\text{\hspace{1em}(3-5-12)}$$

证毕。

（3）下面讨论四元数和三维旋转矩阵之间的关系

        
                          

图中向量 $v$ 绕 单位旋转向量 $u$ 旋转 $\theta$ 度，我们分别来看 $v_{\parallel }$ 和 $v_{\perp }$ 的旋转。

1）首先看 $v$ 的旋转。图一中，将向量分解

$$v=v_{\parallel }+v_{\perp }$$

$$v^{\prime }={v^{\prime }}_{\parallel }+{v^{\prime }}_{\perp }\text{\hspace{1em}(3-5-13)}$$

图二中，

$${v^{\prime }}_{\perp }=v_{v^{\prime }}+v_{w^{\prime }}\text{\hspace{1em}(3-5-14)}$$

$v_w$ 是辅助向量，且 $\vec{v}w=\vec{u} \times \vec{v}{\perp} $。

由图二，可得

$$|v_{v^{\prime }}|=|{v^{\prime }}_{\perp }|\cos \theta =|v_{\perp }|\cos \theta$$

$$|v_{w^{\prime }}|=|{v^{\prime }}_{\perp }|\sin \theta =|v_w|\sin \theta \text{\hspace{1em}(3-5-15)}$$

推导，得

$$v_{v^{\prime }}=v_{\perp }\cos \theta$$

$$v_{w^{\prime }}=v_w\sin \theta \text{\hspace{1em}(3-5-16)}$$

又因为 $v_w=u\times v_{\perp }$，有

$$\begin{array}{rl}{v^{\prime }}_{\perp }& =v_{v^{\prime }}+v_{w^{\prime }}\\ & =v_{\perp }\cos \theta +v_w\sin \theta \\ & =v_{\perp }\cos \theta +\sin \theta (u\times v_{\perp })\end{array}\text{\hspace{1em}(3-5-17)}$$

令 $v_{\perp }=[0,v_{\perp }]$，$u=[0,u]$，根据纯四元数乘法法则，有

$$u{v}_{\perp }=[-u\cdot v_{\perp },u\times v_{\perp }]\text{\hspace{1em}(3-5-18)}$$

因为 $u$ 与 $v_{\perp }$ 垂直，则

$$u{v}_{\perp }=[0,u\times v_{\perp }]=u\times v_{\perp }\text{\hspace{1em}(3-5-19)}$$

则式（3-5-17）可写为四元数形式

$$\begin{array}{rl}{v}_{\perp }^{\prime }& =v_{\perp }\cos \theta +\sin \theta (u{v}_{\perp })\\ & =(\cos \theta +\sin \theta u)v_{\perp }\end{array}\text{\hspace{1em}(3-5-20)}$$

令 $q=\cos \theta +\sin \theta u$，将其视作本次旋转四元数，得到

$$v_{\perp }^{\prime }=q{v}_{\perp }\text{\hspace{1em}(3-5-21)}$$

2） $v$ 的平行分量并不会随旋转改变。下面直接推导 $v$ 的旋转。

$$v^{\prime }=v_{\parallel }+v_{\perp }^{\prime }=v_{\parallel }+q{v}_{\perp }\text{\hspace{1em}(3-5-22)}$$

在此之前，先证明几个引理：

① 假设旋转四元数 $q=[\cos \theta ,\sin \theta u]$，其中 $u$ 为单位旋转向量。推导可得

$$\begin{array}{rl}{q}^2=qq& =[\cos \theta ,\sin \theta u][\cos \theta ,\sin \theta u]\\ & =[{\cos }^2\theta -\sin \theta u\cdot \sin \theta u,\cos \theta \sin \theta u+\cos \theta \sin \theta u+\sin u\times \sin \theta u]\\ & =[{\cos }^2\theta -{\sin }^2\theta u^2,2\cos \theta \sin \theta u+0]\\ & =[{\cos }^2\theta -{\sin }^2\theta ,2\cos \theta \sin \theta u]\\ & =[\cos (2\theta ),\sin (2\theta )u]\end{array}\text{\hspace{1em}(3-5-23)}$$

公式表明，绕 $u$ 连续旋转 $\theta$角两次，相当于直接旋转 $2\theta$ 度。

② 接下来将式（3-5-22）变形。根据上式，令 $q=p^2$ ，则 $p=[\cos (\frac{\theta }{2}),\sin (\frac{\theta }{2})u]$，则

$$\begin{array}{rl}{v}^{\prime }& =v_{\parallel }+v_{\perp }^{\prime }\\ & =v_{\parallel }+q{v}_{\perp }\\ & =(p{p}^{-1})v_{\parallel }+pp{v}_{\perp }\end{array}\text{\hspace{1em}(3-5-24)}$$

因为 $p$是单位四元数，可得

$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}=q^{\ast }\text{\hspace{1em}(3-5-25)}$$

代入上式

$$\begin{array}{rl}{v}^{\prime }& =(p{p}^{-1})v_{\parallel }+pp{v}_{\perp }\\ & =(p{p}^{\ast })v_{\parallel }+pp{v}_{\perp }\end{array}\text{\hspace{1em}(3-5-26)}$$

③ 证明一个引理：若 $v_{\parallel }=[0,v_{\parallel }]$，$q=[\alpha ,\beta u]$，$u$为单位旋转向量，且 $v_{\parallel }\parallel u$，则有

$$q{v}_{\parallel }=v_{\parallel }q\text{\hspace{1em}(3-5-27)}$$

下面进行证明：

$$\begin{array}{rl}左边=q{v}_{\parallel }& =[\alpha ,\beta u]\cdot [0,v_{\parallel }]\\ & =[-\beta u\cdot v_{\parallel },\alpha v_{\parallel }+0+\beta u\times v_{\parallel }]\\ & =[-\beta u\cdot v_{\parallel },\alpha v_{\parallel }]\end{array}$$

$$\begin{array}{rl}右边=v_{\parallel }q& =[0,v_{\parallel }]\cdot [\alpha ,\beta u]\\ & =[-\beta v_{\parallel }\cdot u,\alpha v_{\parallel }+\beta v_{\parallel }\times u]\\ & =[-\beta v_{\parallel }\cdot u,\alpha v_{\parallel }]\\ & =左边\end{array}$$

证毕。

④ 若 $v_{\perp }=[0,v_{\perp }]$，$q=[\alpha ,\beta u]$，$u$ 为单位旋转向量，且 $v_{\perp }\perp u$，则有

$$q{v}_{\perp }=v_{\perp }{q}^{\ast }\text{\hspace{1em}(3-5-28)}$$

证明：

$$\begin{array}{rl}左边=q{v}_{\perp }& =[\alpha ,\beta u]\cdot [0,v_{\perp }]\\ & =[-\beta u\cdot v_{\perp },\alpha v_{\perp }+0+\beta u\times v_{\perp }]\\ & =[-\beta u\cdot v_{\perp },\alpha v_{\perp }+\beta u\times v_{\perp }]\end{array}$$
$$\begin{array}{rlr}右边=v_{\perp }{q}^{\ast }& =[0,v_{\perp }]\cdot [\alpha ,-\beta u]& \\ & =[-\beta v_{\perp }\cdot u,\alpha v_{\perp }-\beta v_{\perp }\times u]& \\ & =[-\beta v_{\perp }\cdot u,\alpha v_{\perp }+\beta u\times v_{\perp }]& =左边\end{array}$$

证毕。

3）结合上面两个引理和式（3-5-25），对式（3-5-26）推导（写成四元数形式）

$$\begin{array}{rl}{v}^{\prime }& =(p{p}^{\ast })v_{\parallel }+pp{v}_{\perp }\\ & =p(p^{\ast }{v}_{\parallel })+p(p{v}_{\perp })\\ & =p{v}_{\parallel }{p}^{\ast }+p{v}_{\perp }{p}^{\ast }\\ & =p(v_{\parallel }+v_{\perp })p^{\ast }\\ & =pv{p}^{\ast }\\ & =pv{p}^{-1}\end{array}\text{\hspace{1em}(3-5-26)}$$

CH3-6 四元数、旋转向量、旋转矩阵、欧拉角之间的转换

1、四元数和旋转向量的互相转换

由式（3-5-26），

$$v^{\prime }=p{p}^{\ast }{v}_{\parallel }+pp{v}_{\perp }\text{\hspace{1em}(3-6-1)}$$

可以看出，$v_{\parallel }$ 经过`$p$ 和 $p^{\ast }$ 两次旋转，抵消了；而 $v_{\perp }$ 经过两次旋转 $\frac{\theta }{2}$，变成了 $v_{\perp }^{\prime }$，这也说明了为什么 $p=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}u]$。

假设已知单位旋转四元数`$p=[a,b]$，根据上面的对应关系，可以提取出旋转角度和旋转向量：

$$\{\begin{array}{c}\frac{\theta }{2}=\arccos a\\ u=\frac{b}{\sin \frac{\theta }{2}}=\frac{b}{\sin (\arccos a)}\end{array}\text{\hspace{1em}(3-6-2)}$$

反之，知道旋转向量也可求出四元数。

2、旋转矩阵和四元数相互转换

（1）前面我们已经推导过，左乘四元数 $q_1=a+bi+cj+dk$，相当于左乘矩阵

L ( q ) = [ a − b − c − d b a − d c c d a − b d − c b a ] (3-6-3) \boldsymbol{L}(q)=\left[\begin{array}{c} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{array}\right] \tag{3-6-3} L(q)= ​abcd​−bad−c​−c−dab​−dc−ba​ ​(3-6-3)

类似的，右乘矩阵为

R ( q ) = [ a − b − c − d b a d − c c − d a b d c − b a ] (3-6-4) \boldsymbol{R}(q)=\left[\begin{array}{c} a & -b & -c & -d \\ b & a & d & -c \\ c & -d & a & b \\ d & c & -b & a \end{array}\right] \tag{3-6-4} R(q)= ​abcd​−ba−dc​−cda−b​−d−cba​ ​(3-6-4)

利用这两个式子，将 $v^{\prime }=qv{q}^{\ast }$ （式3-5-26）写成矩阵乘法形式。已知 $p=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}u]$，令 $u=[u_x,u_y,u_z]$，则

$$q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}{u}_{x}i+\sin \frac{\theta }{2}{u}_{y}j+\sin \frac{\theta }{2}{u}_{z}k\text{\hspace{1em}(3-6-5)}$$

根据对应关系

$$\{\begin{array}{c}a=\cos \frac{\theta }{2}\\ b=\sin \frac{\theta }{2}{u}_x\\ c=\sin \frac{\theta }{2}{u}_y\\ d=\sin \frac{\theta }{2}{u}_z\end{array}\text{\hspace{1em}(3-6-6)}$$

将四元数乘法写为矩阵乘法形式

v ′ = q v q ∗ = L ( q ) R ( q ∗ ) v = [ 1 0 0 0 0 1 − 2 c 2 − 2 d 2 2 b c − 2 a d 2 a c + 2 b d 0 2 b c + 2 a d 1 − 2 b 2 − 2 d 2 2 c d − 2 a b 0 2 b d − 2 a c 2 a b + 2 c d 1 − 2 b 2 − 2 c 2 ] [ 0 v x v y v z ] (3-6-7) \begin{aligned} v'=qvq^*=& \boldsymbol{L}(q)\boldsymbol{R}(q^*)\boldsymbol{v} \\ =& \left[\begin{array}{c} 1 & 0 & 0 & 0 \\ 0 & 1-2c^2-2d^2 & 2bc-2ad & 2ac+2bd \\ 0 & 2bc+2ad & 1-2b^2-2d^2 & 2cd-2ab \\ 0 & 2bd-2ac & 2ab+2cd & 1-2b^2-2c^2 \end{array}\right]\left[\begin{array}{c} 0 \\ v_x\\ v_y\\ v_z \end{array}\right] \end{aligned} \tag{3-6-7} v′=qvq∗==​L(q)R(q∗)v ​1000​01−2c2−2d22bc+2ad2bd−2ac​02bc−2ad1−2b2−2d22ab+2cd​02ac+2bd2cd−2ab1−2b2−2c2​ ​ ​0vx​vy​vz​​ ​​(3-6-7)

矩阵外围不会对变换产生影响，整理为

v ′ ⃗ = [ 1 − 2 c 2 − 2 d 2 2 b c − 2 a d 2 a c + 2 b d 2 b c + 2 a d 1 − 2 b 2 − 2 d 2 2 c d − 2 a b 2 b d − 2 a c 2 a b + 2 c d 1 − 2 b 2 − 2 c 2 ] v ⃗ (3-6-7) \vec{v'}=\left[\begin{array}{c} 1-2c^2-2d^2 & 2bc-2ad & 2ac+2bd \\ 2bc+2ad & 1-2b^2-2d^2 & 2cd-2ab \\ 2bd-2ac & 2ab+2cd & 1-2b^2-2c^2 \end{array}\right] \vec{v} \tag{3-6-7} v′ = ​1−2c2−2d22bc+2ad2bd−2ac​2bc−2ad1−2b2−2d22ab+2cd​2ac+2bd2cd−2ab1−2b2−2c2​ ​v (3-6-7)
至此，我们由旋转向量得到了单位四元数，再得到了旋转矩阵。

（2）再由旋转矩阵恢复四元数，注意单位四元数（$a^2+b^2+c^2+d^2=1$）

$$\begin{array}{rl}{q}_0& =\frac{\sqrt{tr(\mathbf{R})+1}}{2}\\ & =\frac{\sqrt{(3-4b^2-4c^2-4d^2)+1}}{2}=\frac{\sqrt{4-4b^2-4c^2-4d^2}}{2}\\ & =\frac{\sqrt{4(1-b^2-c^2-d^2)}}{2}=\frac{\sqrt{4a^2}}{2}\\ & =a\end{array}\text{\hspace{1em}(3-6-8)}$$

其余三项分别为

$$q_1=\frac{m_{32}-m_{23}}{4q_0}=\frac{4ab}{4a}=b$$

$$q_2=\frac{m_{13}-m_{31}}{4q_0}=\frac{4ac}{4a}=c$$

$$q_1=\frac{m_{21}-m_{12}}{4q_0}=\frac{4ad}{4a}=d\text{\hspace{1em}(3-6-9)}$$

证毕。

3、旋转矩阵和相互转换

（1）欧拉角转旋转矩阵

假设绕 XYZ 三个轴的旋转角度分别为 $\alpha$ $\beta$ $\gamma$，则三次旋转的旋转矩阵分别为

R x ( α ) = [ 1 0 0 0 cos ⁡ α − sin ⁡ α 0 sin ⁡ α cos ⁡ α ] R y ( β ) = [ cos ⁡ β 0 sin ⁡ β 0 1 0 − sin ⁡ β 0 cos ⁡ β ] R z ( γ ) = [ cos ⁡ γ − sin ⁡ γ 0 sin ⁡ γ cos ⁡ γ 0 0 1 ] \begin{aligned} & R_x(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{array}\right] \\ & R_y(\beta)=\left[\begin{array}{ccc} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{array}\right] \\ & R_z(\gamma)=\left[\begin{array}{ccc} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & \\ 0 & 0 & 1 \end{array}\right] \end{aligned} ​Rx​(α)= ​100​0cosαsinα​0−sinαcosα​ ​Ry​(β)= ​cosβ0−sinβ​010​sinβ0cosβ​ ​Rz​(γ)= ​cosγsinγ0​−sinγcosγ0​01​ ​​

可得旋转矩阵为

R = R z ( γ ) ∗ R y ( β ) ∗ R x ( α ) = [ cos ⁡ β cos ⁡ γ sin ⁡ α sin ⁡ β cos ⁡ γ − cos ⁡ α sin ⁡ γ cos ⁡ α sin ⁡ β cos ⁡ γ + sin ⁡ α sin ⁡ γ cos ⁡ β sin ⁡ γ sin ⁡ α sin ⁡ β sin ⁡ γ + cos ⁡ α cos ⁡ γ cos ⁡ α sin ⁡ β sin ⁡ γ − sin ⁡ α cos ⁡ γ − sin ⁡ β sin ⁡ α cos ⁡ β cos ⁡ α cos ⁡ β ] R=R_z(\gamma) * R_y(\beta) * R_x(\alpha) =\left[\begin{array}{ccc} \cos \beta \cos \gamma & \sin \alpha \sin \beta \cos \gamma-\cos \alpha \sin \gamma & \cos \alpha \sin \beta \cos \gamma+\sin \alpha \sin \gamma \\ \cos \beta \sin \gamma & \sin \alpha \sin \beta \sin \gamma+\cos \alpha \cos \gamma & \cos \alpha \sin \beta \sin \gamma-\sin \alpha \cos \gamma \\ -\sin \beta & \sin \alpha \cos \beta & \cos \alpha \cos \beta \end{array}\right] R=Rz​(γ)∗Ry​(β)∗Rx​(α)= ​cosβcosγcosβsinγ−sinβ​sinαsinβcosγ−cosαsinγsinαsinβsinγ+cosαcosγsinαcosβ​cosαsinβcosγ+sinαsinγcosαsinβsinγ−sinαcosγcosαcosβ​ ​

（2）旋转矩阵转欧拉角

有旋转矩阵

R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right] R= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​

欧拉角为

$$\begin{array}{c}{\theta }_x=\mathrm{atan}2\left(r_{32},r_{33}\right)\\ {\theta }_y=\mathrm{atan}2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ {\theta }_z=\mathrm{atan}2\left(r_{21},r_{11}\right)\end{array}$$

CH3-7 四元数复合

向量 $v$ 分别绕不同轴、 不同角度旋转两次

$$\begin{gathered} v^{\prime }=q_{1}v{q}_1^{\ast } \\ {v}^{\prime \prime }=q_2{v}^{\prime }{q}_2^{\ast }\text{\hspace{1em}(3-7-1)} \end{gathered}$$

合起来，写为

$$v^{\prime \prime }=q_2{q}_{1}v{q}_1^{\ast }{q}_2^{\ast }\text{\hspace{1em}(3-7-2)}$$

先证明一个引理：

对任意四元数 $q_1=[s,v]$、$q_2=[t,u]$，有

$$q_1^{\ast }{q}_2^{\ast }=(q_2{q}_1{)}^{\ast }\text{\hspace{1em}(3-7-3)}$$

证明：

$$\begin{array}{rl}左边& =q_1^{\ast }{q}_2^{\ast }\\ & =[s,-v][t,-u]\\ & =[st-u\cdot v,-su-tv+v\times u]\end{array}$$

$$\begin{array}{rl}右边& =(q_2{q}_1{)}^{\ast }\\ & =([t,u][s,v]{)}^{\ast }\\ & =([st-u\cdot v,su+tv+u\times v]{)}^{\ast }\\ & =[st-u\cdot v\\ & \end{array}$$