四元数与旋转矩阵

在计算物体旋转时，如相机标定中的旋转矩阵R，通常都是以三维矩阵表示，三个自由度分别是绕三个坐标轴的旋转角度，但在Unity中，或者像colmap中,都是以四元数(Quaternion)来表示三维旋转的。实际上是复平面向量与实数域矩阵的一个转换关系，这里推导一下。

二维情况

一维数轴上的复数对应于一个二维实数空间，比如一个二维空间坐标为 $(x, y)$ 的复数表示为 $x + y i$ 。
给定两个复数 $z 1 = a + bi$ , $z 2 = c + d i$ , 其乘积可以表示为:
$$ z1z2 = (a+bi)(c+di) = (ac-bd)+(ad+bc)i $$ 对于向量 $z 2$ ,与 $z 1$ 的乘积可以表示为矩阵形式，即： $$ \begin{bmatrix} a & -b \\ b & a \\ \end{bmatrix} \cdot z2 $$ 如果将 $z 2$ 也看做一个变换表示成矩阵形式，则 $$ z1z2 = \begin{bmatrix} a & -b \\ b & a \\ \end{bmatrix} \cdot \begin{bmatrix} c & -d \\ d & a \\ \end{bmatrix} $$

此时满足交换律。

二维旋转

设向量模长为1，即 $2 a^{2} + b^{2} = 1$ ，则 $a = cos θ, b = s in θ$ , 则对一个向量 $v = x + y i$ ,其乘积为 $v z ˙ = (x cos θ - y s in θ) + (x s in θ + y cos θ) i$ ,

设向量 $v$ 的模为r，则$\vec{v}\cdot \vec{z} = r(cos\theta_1cos\theta_2 -sin\theta_1sin\theta_2)+r(cos\theta_1sin\theta_1 + sin\theta_1cos\theta_2)i = rcos(\theta_1 +\theta_2) +rsin(\theta_1 + \theta_2) i $ 可以看出，一个单位的二维向量，或一维复数，可以表示成一个旋转变换，即逆时针旋转 $θ$ 角。

所以二维的旋转矩阵可以很直观的求得:
$$ \begin{bmatrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \\ \end{bmatrix} $$

极坐标形式

其实将复数表示成极坐标形式，欧拉公式将三角函数和复平面关联起来，于是可以很直接的将$ e^{i\theta} = cos\theta + isin\theta $带入，角度旋转即$ $ e^{i\theta_1} * e^{i\theta_2} = e^{i(\theta_1 + \theta_2)}$$

三维情况

先看向量的旋转: 将向量 $v$ 绕旋转轴 $u$ 旋转 $θ$ 角。
将 $v$ 分解成两个正交向量的和，分别是平行于 $u$ 和垂直于 $u$ 的向量，记为 $v_∣∣$ 和 $v_⊥$ ,平行的向量旋转不变，因此只需要考虑垂直向量即可。
对于 $v_∣∣$ ，其实就是在 $u$ 上的正交投影，因此有 $v_∣∣ = \frac{u \cdot v}{∣∣ u ∣ ∣ ^{2}} u$ ,设 $u$ 为单位向量，则可以表示为 $v_∣∣ = (u \cdot v) u$ 。所以，$ $v_⊥ = v - v_∣∣ = v - (u \cdot v) u$ $ 因为 $v_⊥$ 和 $u$ 垂直，所以旋转可以转化成二维平面的旋转，构造一个向量 $w = u \times v$ ,如图所示，

所以旋转后的向量为$ $v_⊥^{'} = v_⊥ cos θ + w s in θ = v_⊥ cos θ + u \times v_⊥ s in θ$ $ 最后旋转后的向量为 $$ \begin{aligned} \vec{v}^\prime & = \vec{v_{||}} + \vec{v_\bot}^\prime \\ & = \vec{v_{||}} + \vec{v_\bot}cos\theta + \vec{u} \times \vec{v_\bot} sin\theta \\ & = (\vec{u}\cdot \vec{v})\vec{u} + \vec{v_\bot}cos\theta + \vec{u} \times \vec{v_\bot} sin\theta \\ & = (\vec{u}\cdot \vec{v})\vec{u} + (\vec{v} - (\vec{u}\cdot \vec{v})\vec{u} )cos\theta + \vec{u} \times (\vec{v} - (\vec{u}\cdot \vec{v})\vec{u}) sin\theta \\ & = \vec{v}cos\theta +(1-cos\theta)(\vec{u}\cdot \vec{v})\vec{u} +(\vec{u}\times \vec{v}))sin\theta \end{aligned}$$

四元数

四元数可以看做一个四元向量，或是有三个虚部的复数，如 $q = a + bi + c j + d k$ ,也可以写成矩阵形式， $$\vec{q} = \begin{bmatrix} a \\ b \\ c \\ d \\ \end{bmatrix} $$

如三维坐标轴的顺序，复数相乘有， $$ij =k \\ jk =i \\ ki =j$$ 令 $q 1 = a + bi + c j + d k$ , $q 2 = e + f i + g j + hk$ 则左乘 $q 1$ 可以为 $$ \begin{aligned} q1q2 & = ae + a f i + agj + ahk + \\ & bei − b f + bgk − bhj + \\ & cej − c f k − cg + chi + \\ & dek + d f j − dgi − dh \\ & = ( ae − b f − cg − dh )+ \\ & ( be + a f − dg + ch ) i \\ & ( ce + d f + ag − bh ) j \\ & ( de − c f + bg + ah ) k \end{aligned} $$

矩阵形式可以写成， $$q1q2 = \begin{bmatrix} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{bmatrix} \begin{bmatrix} e \\ f \\ g \\ h \\ \end{bmatrix} $$ 四元数向量不满足交换律，右乘会有一些区别。

Graßmann积

将四元数的虚部表示成一个向量，即$ q1 = [a,\vec{v}] $，$ q2 = [e, \vec{u}] $，其中，$ \vec{v} = bi+cj+dk $，$ \vec{u} = fi+gj+hk$。
则左乘 $q 1$ 可以化简成 $ $q 1 q 2 = [a e - v \cdot u, a u + e v + v \times u]$ $ 这个结果也被称为 Graßmann 积。这样，当a和e为零时，两者乘积可以写成， $$ q1q2 = [− \vec{v} \cdot \vec{u}, \vec{v} \times \vec{u} ]$$ 同纯虚数的说法，这时q1和q2叫纯四元数。

共轭性质

与二元虚数类似，四元数的共轭也是将虚部方向取反，即 $q^{\*} = a - bi - c j - d k$ ,则

q q^{\*} = [s, v] \cdot [s, - v] = [s^{2} - v \cdot (- v), s (- v) + s v + v \times (- v)] = [s^{2} + v \cdot v, 0]

实部平方与虚部平方和，即该向量的模的平方,最后虚部为零，所以

q q^{\*} = [s^{2} + v \cdot v, 0] = s^{2} + ∣ v ∣^{2} = a^{2} + b^{2} + c^{2} + d^{2}

由于q与其共轭的积最后是个标量，为其模长，所以该乘法是满足交换律的。即 $q q^{\*} = q^{\*} q = ∣ q ∣^{2}$ 。这样， $$ q^*q = |q|^2 \\ \frac{q^*}{|q|^2}q =1 $$ 则可以发现 $q^{- 1} = \frac{q ^{\*}}{∣ q ∣ ^{2}}$ 满足 $q^{- 1} q = q q^{- 1} = 1$ ,即为该四元数的逆。

而单位四元数的逆即为其共轭四元数。

三维旋转

旋转轴 $u$ 不妨设为单位向量，与之前的旋转类似， $$ \vec{v'} = \vec{v'_{||}} +\vec{v'_\bot} = \vec{v_{||}} +\vec{v'_\bot} $$ 之前计算过正交与旋转轴的向量旋转得到的结果，$ $v^{'}_⊥ = v_⊥ cos θ + (u \times v_⊥) s in θ$ $ 设u,v都是纯四元数，即 $u = [0, u]$ , $v = [0, v]$ ,两个纯四元数的Graßmann积为$$uv_\bot = [− \vec{v_\bot} \cdot \vec{u}, \vec{v_\bot} \times \vec{u} ] = [ 0, \vec{v_\bot} \times \vec{u} ] = \vec{v_\bot} \times \vec{u} $$ 也是一个纯四元数。
所以， $$ \begin{aligned} v'_\bot & = v_\bot cos\theta + (u v_\bot)sin\theta \\ & = (cos\theta + usin\theta)v_\bot \end{aligned} $$ 令四元数 $q = (cos θ + u s in θ) v_⊥$ ，则$ v'_\bot = qv_\bot$ 所以对于垂直于旋转轴的向量，旋转 $θ$ 角度之后的向量可以用四元数的乘法来获得，用向量表示为 $q = [cos θ, u s in θ]$ 由于 $u$ 是单位向量，所以$$||q|| = cos^2\theta + ||\vec{u}||^2 sin^2\theta =1 $$ 同样的表示方式， $q q v_⊥ = q (q v_⊥)$ 几何上表示旋转两次，因此有$qqv_\bot = (cos2\theta + usin2\theta)v_\bot $ 所以最后旋转之后的四元数， $$\begin{aligned} v' & = v'_{||} + v'_\bot \\ & = v_{||} + qv_\bot \\ & = pp^{-1}v_{||} + ppv_\bot \\ & = pp^*v_{||} + ppv_\bot \end{aligned} $$ 其中， $p = [cos (\frac{θ}{2}), u s in (\frac{θ}{2})]$ ,是旋转半角的单位向量，因此 $q = p^{2}$ 。

交换性质：由之前的Graßmann积，上式中，将q写成向量形式， $q = [α, β u]$ $$ \begin{aligned} qv_{||} & = [\alpha, \beta \vec{u}] \cdot [0,\vec{v}_{||}] \\ & = [-\beta \vec{u} \cdot \vec{v}_{||} , \alpha \vec{v}_{||} + \beta \vec{u} \times \vec{v}_{||}] \\ & = [-\beta \vec{u} \cdot \vec{v}_{||} , \alpha \vec{v}_{||}] \\ \end{aligned} $$ 右乘， $$ \begin{aligned} v_{||}q & = [0,\vec{v}_{||}] \cdot [\alpha, \beta \vec{u}] \cdot \\ & = [-\beta \vec{u} \cdot \vec{v}_{||} , \alpha \vec{v} +\vec{u} \times \vec{v}_{||}] \\ & = [-\beta \vec{u} \cdot \vec{v}_{||} , \alpha \vec{v} ] \\ & = qv_{||} \end{aligned} $$

再看垂直部分， $$ \begin{aligned} qv_\bot & = [\alpha, \beta \vec{u}] \cdot [0,\vec{v}_{}] \\ & = [-\beta \vec{u} \cdot \vec{v}_\bot , \alpha \vec{v}_\bot + \beta \vec{u} \times \vec{v}_\bot] \\ & = [0, \alpha \vec{v}_\bot + \beta \vec{u} \times \vec{v}_\bot] \\ \end{aligned} $$

右乘共轭， $$ \begin{aligned} v_\bot q^* & = [0,\vec{v}_\bot] \cdot [\alpha, -\beta \vec{u}] \cdot \\ & = [-\beta \vec{u} \cdot \vec{v}_\bot , \alpha \vec{v}_\bot + \beta \vec{u} \times \vec{v}_\bot] \\ & = [0, \alpha \vec{v}_\bot + \beta \vec{u} \times \vec{v}_\bot] \\ & = qv_\bot \end{aligned} $$

所以旋转之后的结果可以化简为 $$\begin{aligned} v' & = pp^*v_{||} + ppv_\bot \\ & = pv_{||}p^* + pv_\bot p^* \\ & = p(v_{||} + v_\bot) p^* \\ & = pvp^* \end{aligned} $$

实际上，从计算过程可以看出，对于平行分量，乘 $p p^{\*}$ ，实际上是没有变化，对于垂直分量，乘 $pp$ ,旋转了 $\frac{θ}{2} + \frac{θ}{2} = θ$ 角度。因此可以用旋转半角的四元数乘法来表示绕单位向量 $u$ 的旋转。

矩阵形式

单位向量 $p = [cos (\frac{θ}{2}), u s in (\frac{θ}{2})]$ ,以通用四元数方式表示为 $p = a + bi + c j + d k$ 其中 $a = cos (\frac{θ}{2}), b = u_x s in (\frac{θ}{2}), c = u_y s in (\frac{θ}{2}), b = u_z s in (\frac{θ}{2})$ 写成矩阵形式，之前说了四元数的矩阵形式左乘和右乘有点区别，左乘矩阵为 $$L= \begin{bmatrix} a & -b & -c & -d \\ b & a & -d & c \\ c & d & a & -b \\ d & -c & b & a \end{bmatrix} $$

右乘的矩阵等同于左乘矩阵 $$R = \begin{bmatrix} a & -b & -c & -d \\ b & a & -d & -c \\ c & -d & a & b \\ d & c & -b & a \end{bmatrix} $$

所以有，

q v q^{\*} = L (q) R (q^{\*}) v = a b c d - b a d - c - c - d a b - d c - b a a - b - c - d b a d - c c - d a b d c - b a v = 1000 0 1 - 2 c^{2} - 2 d^{2} 2 b c + 2 a d 2 b d - 2 a c 0 2 b c - 2 a d 1 - 2 b^{2} - 2 d^{2} 2 ab + 2 c d 0 2 a c + 2 b d 2 c d - 2 ab 1 - 2 b^{2} - 2 c^{2} v

矩阵最外圈不会有任何影响，所以可以得出向量 $v$ 绕单位向量旋转轴 $u$ 旋转的三维矩阵变换，即

v^{'} = 1 - 2 c^{2} - 2 d^{2} 2 b c + 2 a d 2 b d - 2 a c 2 b c - 2 a d 1 - 2 b^{2} - 2 d^{2} 2 ab + 2 c d 2 a c + 2 b d 2 c d - 2 ab 1 - 2 b^{2} - 2 c^{2} v

其中 $a = cos (\frac{θ}{2}), b = u_x s in (\frac{θ}{2}), c = u_y s in (\frac{θ}{2}), b = u_z s in (\frac{θ}{2})$ 。