点云配准中的数学基础二：空间变换不同表示方式之间的互相转换

空间变换不同表示方式之间的互相转换

免费点云配准视频教程，请前往b站搜索：TechFlowAI
欢迎关注微信公众号：TechFlowAI

引言

上一讲我们详细介绍了如何表示刚体运动的空间变换，实则是在讨论姿态的表示，因为位置的表示很简单，就是常说的平移向量。姿态的表示在上一讲中提高了四种方式，分别是：旋转矩阵、角轴、欧拉角和四元数。这四种表示方法各有优劣，大家在使用的时候需要自己斟酌。从目前我的经验来看，四元数在代码中用的比较多，有时为了验证算法，会将四元数转成欧拉角，让我们直观的想象出旋转量，最后通常会转成旋转矩阵在对移动智能体的位姿进行刻画。

由此看来，我们必须要知道这四种表示方法之前的转换关系，可以更加方便后续我们的各种操作。那么，接下来，我们就详细推导一下这四种姿态的表示是如何互相转换的。这里要说明的一点，可能推导有一定的复杂，但是在实际运用中非常简单，直接只用Eigen库即可。

这里，我要插一句，上一节感觉很多人对欧拉角内旋后的旋转矩阵是怎么得到的不太清楚，我在视频中也是一带而过。这里，我再解释一下是怎么得到的。其实我推荐的书籍《点云配准从入门到精通》已经讲的比较清楚了，内旋是绕自身坐标系旋转，可以理解为对坐标系在变换，是在发生列变换，因此右乘坐标向量。外旋是对坐标向量在变换，是在发生行变换，因此左乘坐标向量。 这里有一点绕，还是不明白，就去看我b站这讲的视频，b站搜索关注：TechFlowAI。文末有点云配准交流群​二维码，欢迎进群交流。

旋转矩阵与角轴

设旋转矩阵$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$，角轴使用一个单位向量$n$ 和一个角度值$\theta$ 来表示。如果已知轴角$\omega =\theta n$,式中$n=[n_x,n_y,n_z{]}^T$。根据罗德里格斯公式，则计算得到旋转矩阵：
$$R=\left[\begin{array}{ccc}{n}_x^2+(1-n_x^2)\cos \theta & n_x{n}_y(1-\cos \theta )-n_z\sin \theta & n_x{n}_z(1-\cos \theta )+n_y\sin \theta \\ n_x{n}_y(1-\cos \theta )+n_z\sin \theta & n_y^2+(1-n_y^2)\cos \theta & n_y{n}_z(1-\cos \theta )-n_x\sin \theta \\ n_x{n}_z(1-\cos \theta )-n_y\sin \theta & n_y{n}_z(1-\cos \theta )+n_x\sin \theta & n_z^2+(1-n_z^2)\cos \theta \end{array}\right]$$
如果对罗德里格斯公式还有印象，这个就是直接展开的结果，没有任何难度。但是有难度的是罗德里格斯公式是怎么得到的，那下面就来推导一下，其中涉及到了前面我们说的一些向量知识还有我让大家去《slam十四讲》学习的李群李代数相关知识。具体推导如下（为了防止符号歧义，推导的时候讲角轴表示为$\omega =\theta \mathbf{a}$）：

下面，我们在讨论如果将旋转矩阵转为角轴，推导如下：

根据罗德里格斯公式：
$$\mathbf{R}=\cos \theta \mathbf{I}+\left(1-\cos \theta \right)\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }.$$
两边同时取迹，可以得到：
$$\begin{array}{rl}tr\left(R\right)& =\cos \theta \mathrm{tr}\left(\mathbf{I}\right)+\left(1-\cos \theta \right)\mathrm{tr}\left(\mathbf{n}{\mathbf{n}}^{\mathrm{T}}\right)+\sin \theta \mathrm{tr}({\mathbf{n}}^{\wedge })\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta \end{array}$$
因此，有：
$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}.$$
对于转轴$n$,在旋转后保持不变，因此：
$Rn=n$
仔细一看，我们发现一个有趣的事实：转轴$n$是矩阵$R$的特征值1对应的特征向量，求解方程之后进行归一化就可以得到旋转轴。

旋转矩阵与欧拉角

这个就非常简单了，根据上一讲的知识我们知道，如果是ZYX顺序的内旋还是XYZ顺序的外旋，得到的旋转矩阵都是一样的，有：$R_1=R_2=Z\left(\gamma \right)Y(\beta )X(\alpha )$，并且上一节也分别给出了绕各轴旋转得到的旋转矩阵，因此直接代入即可：

如果已知旋转矩阵R，以ZYX顺序的内旋计算出来的欧拉角为：
$$\{\begin{array}{l}\alpha =\arctan 2\left(r_{32},r_{33}\right)\\ \\ \beta =\arctan 2\left(-r_{31},\sqrt{r_{11}^2+r_{21}^2}\right)\\ \\ \gamma =\arctan 2\left(r_{21},r_{11}\right)\end{array}$$
这其实也没有什么技巧，就是通过观察上面矩阵的各个元素之间的关系，直接计算即可，具体见b站视频（b站搜索：TechFlowAI）。

旋转矩阵与四元数

下面，就进入这一讲最最最核心的内容，因为旋转矩阵与四元数的变化我们在实践中经常用到，但是不难，主要是要学会运用四元数聚乘法的矩阵形式。

$\text{设 }\mathbf{q}=[s,\mathbf{v}{]}^{\mathrm{T}}{\text{,那么,定义如下的符号}}^{+}{\text{ 和}}^{\oplus }\text{为}$：
$${\mathbf{q}}^{+}=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ & \\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right],{\mathbf{q}}^{\oplus }=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ & \\ \mathbf{v}& s\mathbf{I}-{\mathbf{v}}^{\wedge }\end{array}\right].$$

于是四元数的乘法可以写成矩阵的形式：
$$q_1^{+}{\mathbf{q}}_2=\left[\begin{array}{cc}{s}_1& -{\mathbf{v}}_1^{\mathrm{T}}\\ & \\ v_1& s_1\mathbf{I}+{\mathbf{v}}_1^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_2\\ \\ {\mathbf{v}}_2\end{array}\right]=\left[\begin{array}{c}-{\mathbf{v}}_1^{\mathrm{T}}{\mathbf{v}}_2+s_1{\mathbf{s}}_2\\ \\ s_1{\mathbf{v}}_2+s_2{\mathbf{v}}_1+{\mathbf{v}}_1^{\wedge }{\mathbf{v}}_2\end{array}\right]={\mathbf{q}}_1{\mathbf{q}}_2.$$

同理可得：
$${\mathbf{q}}_1{\mathbf{q}}_2={\mathbf{q}}_1^{+}{\mathbf{q}}_2={\mathbf{q}}_2^{\oplus }{\mathbf{q}}_1.$$

进而，可以再次考虑使用四元数对点进行旋转变换，通过上一节，我们有：
$$\begin{array}{rl}{p}^{\prime }& =qp{q}^{-1}=q^{+}{p}^{+}{q}^{-1}\\ & =q^{+}{q}^{-1^{\oplus }}p.\end{array}$$

代入上面两个符号对应的4*4矩阵，可得：
$${\mathbf{q}}^{+}{\left({\mathbf{q}}^{-1}\right)}^{\oplus }=\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ & \\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^{\mathrm{T}}\\ & \\ -\mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0^{\mathrm{T}}& \mathbf{v}{\mathbf{v}}^{\mathrm{T}}+{\mathbf{s}}^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2\end{array}\right].$$

讲到这里，我们就可以解决上一节最后遗留的一个问题：为什么对某个点进行四元数变换之后直接取虚部就是变换后点的坐标？ 根据上式就可以直接看出来变换后的四元数仍然是一个虚四元数。此外，根据上式，可以得到从四元数到旋转矩阵的变换关系为：
$$R=v{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2.$$

对上式展开，可得到：

如果已知旋转矩阵R，则四元数为：

角轴与四元数

已知轴角，绕单位向量 $n={\left[\begin{array}{c}{n}_x,n_y,n_z\end{array}\right]}^{\mathrm{T}}$旋转角度 $\theta$, 则求得四元数：
$$p=\left[\cos \left(\frac{1}{2}\theta \right),\sin \left(\frac{1}{2}\theta \right)n_x,\sin \left(\frac{1}{2}\theta \right)n_y,\sin \left(\frac{1}{2}\theta \right)n_z\right]$$

这里，需要解释一下，因为我看网上很多文章感觉就是直接给公式，也没有说明原因为什么是这样。最后，还是去读了一下论文《Quaternion kinematics for the error-state Kalman filter》找到了一个合理的解释，这篇论文我在上一讲也提到了，非常经典，建议大家都去读一读。这里直接粘贴原文的解释，其实很简单，不明白的话就去b站看我这讲视频。

回顾四元数到旋转矩阵的变换：
$$R=v{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+{\left({\mathbf{v}}^{\wedge }\right)}^2.$$

两边同时取迹，可得：
$$\begin{array}{rl}\text{tr(R)}& =\mathrm{tr}(\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+3s^2+2s\cdot 0)+\mathrm{tr}(({\mathbf{v}}^{\wedge }{)}^2\\ & =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ & =(1-s^2)+3s^2-2(1-s^2)\\ & =4s^2-1.\end{array}$$
根据旋转矩阵与角轴的关系：
$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}.$$
可以得到：
$$\begin{array}{rl}\theta & =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}\\ & =\arccos (2s^2-1).\end{array}$$
即：
$$\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1,$$
最终，可得到：
$$\theta =2arccos(s)$$
对于旋转轴，q的虚部构成的向量在旋转时是不动的，即构成了旋转轴（直接将q的虚部代替点p即可，如果不懂就去看b站视频解析）。只需要除以其模长（因为单位四元数才能表示旋转）。由此，四元数到旋转向量的转换公式如下：
$$\{\begin{array}{rl}\theta & =2arccos(q_0)\\ & \\ {\left[n_x,n_y,n_z\right]}^{\mathrm{T}}& ={\left[q_1,q_2,q_3\right]}^{\mathrm{T}}/\sin \frac{\theta }{2}\end{array}$$

角轴与欧拉角

轴角与欧拉角采用间接转换的方式，从轴角转到欧拉角需要先将轴角转换为旋转矩阵再转换为欧拉角，而从欧拉角转到轴角则需要先将欧拉角每个元素转换为单一的轴角再相乘之后转换为最终的轴角。这里的意思是指：欧拉角是绕三个旋转轴旋转得到，但是角轴只有一个旋转轴，因此需要将欧拉角一一转换为单一的轴角，最后相乘。

欧拉角与四元数

将$ZYX$ 内旋方式旋转的欧拉角 (或者说是绕固定轴 X-Y-Z 依次旋转 $\alpha$ 、$\beta$ 和 γ) 转换为四元数(四元数的变换叠加，这里使用了角轴到四元数的变换)：
$$q=[\begin{array}{c}\cos \frac{\gamma }{2}\\ 0\\ 0\\ \sin \frac{\gamma }{2}\end{array}]\left[\begin{array}{c}\cos \frac{\beta }{2}\\ 0\\ \sin \frac{\beta }{2}\\ 0\end{array}\right]\left[\begin{array}{c}\cos \frac{\alpha }{2}\\ \sin \frac{\alpha }{2}\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}\cos \frac{\alpha }{2}\cos \frac{\beta }{2}\cos \frac{\gamma }{2}+\sin \frac{\alpha }{2}\sin \frac{\beta }{2}\sin \frac{\gamma }{2}\\ \sin \frac{\alpha }{2}\cos \frac{\beta }{2}\cos \frac{\gamma }{2}-\cos \frac{\alpha }{2}\sin \frac{\beta }{2}\sin \frac{\gamma }{2}\\ \cos \frac{\alpha }{2}\sin \frac{\beta }{2}\cos \frac{\gamma }{2}+\sin \frac{\alpha }{2}\cos \frac{\beta }{2}\sin \frac{\gamma }{2}\\ \cos \frac{\alpha }{2}\cos \frac{\beta }{2}\sin \frac{\gamma }{2}-\sin \frac{\alpha }{2}\sin \frac{\beta }{2}\cos \frac{\gamma }{2}\end{array}\right]$$

根据上式可以计算逆解，即四元数到欧拉角的转换为：
$$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}\arctan \frac{2(q_0{q}_1+q_2{q}_3)}{1-2(q_1^2+q_2^2)}\\ \\ \arcsin (2(q_0{q}_2-q_3{q}_1))\\ \\ \arctan \frac{2(q_0{q}_3+q_1{q}_2)}{1-2(q_2^2+q_3^2)}\end{array}\right]$$

由于 arctan 和 arcsin 的取值范围在$\left(\frac{-\pi }{2},\frac{\pi }{2}\right)$，而绕某个轴旋转时最大为 360°, 因此使用atan2函数代替arctan 函数(atan2的取值范围是$\left(-\pi ,\pi \right]$)：
$$[\begin{array}{c}\alpha \\ \\ \beta \\ \\ \gamma \end{array}]=\left[\begin{array}{c}\mathrm{atan}2(2(q_0{q}_1+q_2{q}_3),1-2(q_1^2+q_2^2))\\ \\ \mathrm{asin}(2(q_0{q}_2-q_3{q}_1))\\ \\ \mathrm{atan}2(2(q_0{q}_3+q_1{q}_2),1-2(q_2^2+q_3^2))\end{array}\right]$$

总结

这一节全面的介绍了不同空间变换关系之间的互相转换，应该是全网最详细的了。通过这节内容我们需要知道的是各个变换关系之间如何转换，以及用到了哪些方法。当然，也不需要我们全部记住，只需要知道有这一回事，如果以后遇见，可以在回过头看看。最后，附上点云配准交流群二维码，欢迎进群讨论交流。

参考资料

[1] 高翔等，视觉slam十四讲[M].

[2] 郭浩，点云配准从入门到精通[M].

[3] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.