SLAM笔记（八）-再谈四元数

在二维空间中，我们用复数表示某点坐标，此时可以用加法表达点的移动，用乘法（乘以一个复数）表示点绕原点的旋转。

在三维空间中，我们无法无法用三维的“超级复合数”来表示点的移动和旋转。这也是四元数发明者汉姆尔顿（爱尔兰数学家）曾苦恼的地方。后来他想：为什么要坚持3位数表达，不用四位数来表达三维空间的移动和旋转呢？经过一系列证明，现在我们已经知道可以用四元数来表示三维空间的旋转，同理还存在八元数、十六元数等；但越往上就越会失去一些特性，比如四元数失去了交换律。

1.四元数简介

四元数分为向量和旋转角度$\theta$。如果用三个数来表达，则不可避免会出现奇异情况（如欧拉角）；而旋转矩阵有9个分量，浪费空间；四元数刚好无奇异表达，又节约空间。

四元数分为实部和虚部：
$q=[q_0+q_{1}i+q_{2}j+q_{3}k]$ (2)
常写作：$[q_0,q_1,q_2,q_3]$
有时也将四元数用一个标量$q_0$，一个向量$[q_1,q_2,q_3]$ 来表示。

注: 高翔的博客中用实部在前、虚部在后的写法，而很多文章中采用虚部在前，实部在后的方式。但它们都对应于唯一的表达式（2），也就是这对四元数运算，如旋转公式$qp{q}^{-1}$无影响。

假设绕空间中某一向量$n=[n_x,n_y,n_z{]}^T$ 旋转$\theta$，则四元数$q=[q_0,q_1,q_2,q_3]$式子为：
$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}](1)$$
由上，我们也可以反过来通过任一四元数$q=[q_0,q_1,q_2,q_3]$来计算：夹角$\theta$和向量n。
从这儿也可以看出四元数与旋转向量的联系和区别：
旋转向量由三个量表示，
注：
(1)处为单位四元数，模为1，四元数$\theta$加上$2\pi$ 则正负相反，因此任一选择可以由q或它的共轭-q表示；如$\theta =0$，q = [1,0,0,0]或[-1,0,0,0]。

四元数可以不为单位四元数，非单位四元数与单位四元数的区别在于 $n_x,n_y,n_z$ 进行一定等比例缩放。

2.四元数运算（可先看3再回头看2）

i 加减法 对应元素相加减
ii 乘法$q_a,q_b$的乘法即$q_a$**每一个元素（带虚部）**与$q_b$每一个元素分别进行复数域的相乘（也就是有16次两两相乘）：
注意：
ii = jj = k*k = -1;
ij = k, ji = -k;
jk = i, kj = -i;
ki = j, ik = -j;

iii 求模
$||q||=sqrt(q_0^2+q_1^2+q_2^2+q_3^2)$
iv 共轭（可以视为表示同一个三维空间上的旋转）
$q^{\ast }=(q_0,-q_1,-q_2,-q_3)$
v. 求逆
$q^{-1}=\frac{q^{\ast }}{||q|{|}^2}$
通常用单位四元数来表示旋转，这样才能逆即为共轭：
$q^{-1}=q\ast =(q_0,-q_1,-q_2,-q_3)$
四元数的逆物理意义表示什么？
vi 点乘
即不考虑虚部i,j,k，只是对应元素相乘：
$$q_a\cdot q_b=q_{a0}{q}_{b0}+q_{a1}{q}_{b1}+q_{a2}{q}_{b2}+q_{a3}{q}_{b3}$$
注意：四元数不满足交换律，即$pq̸=qp$
vii. 求导
在时间t处的角速度为$\omega$，则四元数q(t)导数为：
$$\dot{q}(t)=\frac{1}{2}q\ast [0,\frac{1}{\omega }]$$

3.为什么要有四元数

我们知道用欧拉角或者方向角（roll， pitch，yaw）可以表示旋转，比如，
记录旋转轴顺序，
加上角度就是一个完整的欧拉角表示方式：$zxy->\theta \rho \varphi$

这在直观上也很好理解，为什么还要多此一举引入四元数？

大家可能会想到因为欧拉角表示导致的万向锁。
万向锁只是方向角在实际物理实现上会出现的一个问题，理论上
要规避万向节死锁，选择合适的旋转顺序就行了。

但问题是：决定顺序也要花费判定成本。

用方向角表示旋转，虽然直观，但没有考虑到歧义，以及计算和存储的需求。

所谓歧义，即我们一般是先知道欧拉角再去计算唯一的旋转（旋转向量、旋转矩阵、四元数等能唯一表示三维旋转的方式），但给定一个三维旋转，我们却有至少两种欧拉角表示方式。

消除歧义：
欧拉角的选取顺序有(x,y x,x,z,x)等
6种，可见选取顺序是a,b,a这样的顺序，也就是绕a轴旋转某角度后，绕新生成的b轴旋转一个角度，最后绕两次旋转以后的a轴再旋转一个角度

为了消除歧义，我们让旋转一次到位，即绕某一旋转轴旋n转$\theta$，这即是旋转向量：$n\theta$。

但如果要将旋转构建成一个群，则三维是满足不了群的幺元、封闭性、結合律、有逆元的性质的，所以我们尝试用四维空间来表示三维旋转，即四元数。

本质上，四元数表示的三维旋转是四维空间的一个子集：当以(0,x,y,z）形式表达三维点时，即可参数化四元数。

优化：存储和计算
用欧拉角来计算旋转，如果用常规的李群表示，大致如下：

由以上可以看出：存储上和存储上，至少需要存储六组数据（三个角的cos,sin值)。
而四元数的旋转直接相互运算，包括插值（球面插值），都是非常快的，详如下节。

4.物理上的四元数运算

i.三维点p经过某旋转(以四元数表达)后的三维点位置

三维空间的点可以用虚四元数表示:p = [0,x,y,z];

将点p绕着向量n(四元数旋转轴)旋转角度$\theta$的，旋转后的点/向量为：
$$p^{\prime }=qp{q}^{-1}$$
可以证明得到的p’的维度中第一位是0.

现场落地测试下：对点p = [1,1,0]绕z轴进行90度顺时针旋转($q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]=[\sqrt{2}/2,0,0,\sqrt{2}/2]$)
转换后的四元数结果为？

答案为：[0, -i, j, 0].

可以认为四元数表征的是四维空间。如果要描述旋转，则必须是单位四元数，或者说单位四元数所代表的球面能正好描述三维旋转。

如果只是左乘一个单位四元数，右边什么都不乘，那么我们得到的是四维旋转的一个子集，这个子集并不能保证结果限制在三维超平面上。如果只右乘，不左乘也是一样一样的。

将某三维点绕四元数p旋转，直接用两次四元数乘积，消耗比较大，$p^{\prime }=qp{q}^{-1}$的实现一般采用更快的方法：

void rotate_vector_by_quaternion(const Vector3& v, const Quaternion& q, Vector3& vprime)
{
    // Extract the vector part of the quaternion
    Vector3 u(q.x, q.y, q.z);

    // Extract the scalar part of the quaternion
    float s = q.w;

    // Do the math
    vprime = 2.0f * dot(u, v) * u
          + (s*s - dot(u, u)) * v
          + 2.0f * s * cross(u, v);
}

ii.四元数之间的旋转

a.四元数表征旋转，当我们需要比较两个旋转谁的旋转幅度大时，比较$\theta$即可，或在归一化成单位四元数后，比较第$q_0$的大小即可。

b.先经过p1旋转，再经过p2旋转，则总共旋转（类似球面最短路径）：
$$p_{total}=p_1{p}_2$$
注意此处是四元数的乘法，而非四元素的点积。任意两个四元数总是可以组合成一个新的旋转。
比如先绕着x轴旋转60度，再绕着(1,1,1)旋转30度，可以合二为一，用绕着某轴旋转某度来表示。
c. 计算从旋转状态p1到达旋转状态p2的变换$p_{trans}$（也是一个四元数）：
$$p_{trans}=p_1^{-1}\ast p_2$$
四元数的逆表示一个反向的旋转，任意两个四元数总是可以组合成一个新的旋转（四元数的表达中不存在万向锁）。

为了计算从旋转状态p1到达旋转状态p2所需的最小角度$\theta$，只需计算$p{1}^{-1}\ast p2$的实部，实部等于$cos(\theta /2)$。因此在Unity中计算两个四元数lhs与rhs之间的旋转最小角度可为：

float Quaternion::Angle(const Quaternion &lhs, const Quaternion &rhs)  
{  
    float cos_theta = Dot(lhs, rhs);  
  
    // if B is on opposite hemisphere from A, use -B instead  
    if (cos_theta < 0.f)  
    {  
        cos_theta = -cos_theta;  
    }  
    float theta = acos(cos_theta);  
    return 2 * Mathf::Rad2Deg * theta;  
}  

**d.**两个四元数的余弦角度，无具体物理含义，既不能表征谁的旋转幅度大，也不能表征各自旋转轴的关系，但在球面插值时会有用(参见部分4)。

3.四元数、旋转矩阵、旋转向量、欧拉角的相互转换

3.1.四元数与旋转向量

旋转向量的表示:$\theta [n_x,n_y,n_z$，其中n为单位矩阵:
在上文已经知道旋转向量到四元数$[q_0,q_1,q_2,q_3]$的转换法，则：
$\theta =2\cdot acos(q_0)$
$[n_x,n_y,n_z]=[q_1,q_2,q_3]/sin(\frac{\theta }{2})$

3.2.四元数与旋转矩阵

有了四元数，我们可以算出n与$\theta$，然后根据罗格斯变换再算出选择矩阵R，也可以直接算R（此处为单位四元数）：

如果四元数非单位四元数，则：

由于q和−q表示同一个旋转，对同一个旋转矩阵R，对应4种q。存在其他三种与上式类似的计算方式。实际编程中，当q0接近0时，其余三个分量会非常大，导致解不稳定，此时会考虑使用剩下的三种种方式之一计算。具体推导可参见该链接

3.3 从四元数到欧拉角

相互转实现可参考：
用C++实现一个Quaternion类

4.四元数插值

在两个四元数的插值操作称为slerp，即球面线性插值（Spherical Linear Interpolation）。slerp运算非常有用，可以避免欧拉角插值的所有问题（如万向锁）。
设开始与结束的四元数为q0，q1，插值变量设为t，t在[0, 1]之间变化 。则slerp函数定义为: $$slerp(q0,q1,t)=q0(q{0}^{-1}q1{)}^t$$。其代码实现为：

	Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float t)  
	{  
		float cos_theta = Dot(a, b);  
		// if B is on opposite hemisphere from A, use -B instead  
		float sign;  
		if (cos_theta < 0.f){  
			cos_theta = -cos_theta;  
			sign = -1.f;  
		}  
		else sign = 1.f;  
	  
		float c1, c2;  
		if (cos_theta > 1.f - Mathf::EPSILON)  {  // if q2 is (within precision limits) the same as q1,  
			// just linear interpolate between A and B.  
	  
			c2 = t;  
			c1 = 1.f - t;  
		}  
		else  
		{  
			// faster than table-based :  
			//const float theta = myacos(cos_theta);  
			float theta = acos(cos_theta);  
			float sin_theta = sin(theta);  
			float t_theta = t*theta;  
			float inv_sin_theta = 1.f / sin_theta;  
			c2 = sin(t_theta) * inv_sin_theta;  
			c1 = sin(theta - t_theta) * inv_sin_theta;  
		}  
	  
		c2 *= sign; // or c1 *= sign  
					// just affects the overrall sign of the output  
					// interpolate  
		return Quaternion(a.x * c1 + b.x * c2, a.y * c1 + b.y * c2, a.z * c1 + b.z * c2, a.w * c1 + b.w * c2);  
	}

参考小结：
【Numberphile数字狂】神奇四元数 @柚子
Understanding Quaternions 中文翻译《理解四元数》
知乎：如何形象地理解四元数？
用C++实现一个Quaternion类