c++四元式中算术表达式的翻译_头条 | 一文详解四元数、欧拉角、旋转矩阵、轴角如何相互转换...

点击上方蓝字，关注本公众号，获得更多资源

欧拉角与旋转矩阵

欧拉角---->旋转矩阵

D3D和OpenGL不同，用的坐标系是Y轴竖直向上的左手系，所以欧拉角的顺规是跟广大blog、OpenGL不一样的，那么博客上、甚至维基百科[2]上的各种基于右手系xyz顺规(分别对应roll, pitch,yaw)的看起来就不太能随随便便直接用了。

首先欧拉角旋转序列(Euler Angle Rotational Sequence)一共有12种顺规，6种绕三条轴的旋转(也叫Tait-Bryan Angle，XYZ,XZY,YXZ,YZX,ZXY,ZYX)，另外6种只绕两条轴的旋转(也叫Proper Euler Angle，XYX,YXY,XZX,ZXZ,YZY,ZYZ)。如果相邻两次旋转是绕同一条轴，例如XXY，那么其实可以坍缩成XY。那么只绕一条轴旋转就根本不够自由度就不需要说了。也就是说，一共有12种基础旋转的组合顺序，它们可以旋转出三维的所有旋转状态。所以一共是12种旋转顺规(可以表示所有旋转的集合)，DirectXMath库采用的是ZXY顺规，分别对应着Z-Roll，X-Pitch，Y-Yaw。

图：欧拉旋转与Yaw-Pitch-Roll的直观意义(网图魔改)

注意：那么下文我们都采用ZXY顺规来推导公式！采用列主向量(column major)！(但是注意DirectXMath API生成的矩阵其实是行主向量(row major)的)

参考一下维基百科的[3]Euler Angle、[4]Rotation Matrix

可以知道，欧拉角构造旋转矩阵就直接把三个Elemental Rotation Matrix乘在一起就好了(LaTeX扣得真累orz)： 其中： 上面的欧拉角--->矩阵的结果与维基百科Euler Angles[3]   给出的结果一致，那应该稳了：

图：其实可以不用自己推的，维基百科把12种顺规乘出来的矩阵都写出来了

旋转矩阵---->欧拉角 参考一篇NASA的关于姿态描述的技术报告[1]的Appendix-A6和[5]，我们可以用旋转矩阵元素的相乘、相除、反三角函数等操作去“凑”出欧拉角。[5]给出了从XYZ顺规提取欧拉角的方法、步骤、思路，[1]则给出了全部12种顺规的欧拉角提取公式，但是没有给一些细节注意事项。所以总结一下，根据[1]、[5]、[7]《Real Time Rendering 3rd Edition》4.2.2和自己的推导，从ZXY顺规旋转矩阵提取欧拉角的公式是([1]原文下标似乎有点小问题)：

• Y axis yaw angle:

• X axis pitch angle:

• Z axis roll angle:

. 注意到一点，注意到矩阵的每一个元素都是pitch angle   的函数…所以当   即   的时候，这时候其他的欧拉角提取表达式就凉凉了(分子分母都是0, arctan和atan2都没有意义了)….其实pitch angle   恰好就是Gimbal Lock的位置。在Gimbal Lock的时候，旋转矩阵会退化为： . 那么要进一步处理万向节死锁的corner case就需要分两种情况：

•  ，此时 

其中要给 或者  其中一个欧拉角赋值，另外一个就按等式计算出来。

•  ，此时 

同样的，要给 或者  其中一个欧拉角赋值，另外一个就按等式计算出来。 从旋转矩阵提取欧拉角的公式跟欧拉角顺规的选取有关，因为旋转矩阵的元素会略有不同，但是思路都是一样的，就是根据旋转矩阵的解析表达式+反三角函数凑出来。

四元数与旋转矩阵

四元数---->旋转矩阵 众所周知的是，欧拉旋转是有万向节死锁(Gimbal Lock)的问题的。幸好我们有四元数(Quaternion)这种数学工具可以避免这个情况。一般来说，我们都会用单位四元数   来表示旋转，其中   。那么给定一个单位四元数，可以构造旋转矩阵(column major)[1][4][8][14][15]： 这个四元数构造的大概思路就是把四元数的旋转操作写成矩阵形式(注：给定一个用于旋转的单位四元数   和被旋转的三维向量   ，那么要直接用四元数旋转这个向量，则我们首先要构造一个纯四元数   ，设旋转后的向量为   ，旋转后的向量构造的纯四元数为   ，那么   )。因为是用四元数来构造矩阵的，所以这个矩阵构造公式就没有欧拉角顺规的说法了。 旋转矩阵---->四元数 那第一步肯定是判断3x3矩阵是一个正交矩阵啦(满足   )。那么如果这个矩阵已经是一个合法的旋转矩阵了，要从旋转矩阵里提取四元数，也是可以像提取欧拉角那样，用参数化过的矩阵的表达式凑出来。参考[8]《Real Time Rendering 3rd edition》Chapter4的思路，我们观察一下用四元数分量进行参数化的矩阵  ，然后经过一顿操作，我们发现： 于是我们再凑出个实分量  ，就可以把四元数四个分量都用矩阵元素表示出来了。于是我们又机智地发现了一个等式： 其中   是矩阵   的迹(trace)，也就是矩阵对角元素的和。因为这里用的是3x3矩阵，跟其他资料里面的表示有一点不同。所以我们可以把四元数的四个分量都用矩阵元素凑出来了： 有一点《Real Time Rendering》提到的，   绝对值比较小的时候，可能会出现数值不稳定的情况，那么想要数值稳定的话就得用一种不用除法的方式来凑，在这不展开了，可以看一下RTR。

欧拉角与四元数

欧拉角---->四元数 首先提一下四元数的乘积： 参考维基百科[2]的思路，欧拉角构造四元数，跟欧拉角构造旋转矩阵一样，就是把三个基础旋转Elemental Rotation组合在一起。 那么用于旋转的四元数   的表达式是： 这个我自己推导的结果跟[1]NASA Technical Report的Appendix A给出的结果对比过了， ([1]中四元数记号是   )，看起来没什么问题。 四元数---->欧拉角 本来我以为，从四元数提取欧拉角的思路可以跟旋转矩阵提取欧拉角类似，也是用四元数的元素运算和反三角函数凑出公式来。后来我发现这简直就是一个极其硬核的任务，展开之后每一项都是六次多项式，画面有一丢暴力且少儿不宜，直接强行凑的话画风大概是这样： 这个结果跟欧拉角参数化的旋转矩阵的   的表达式是吻合的。但这还只是最好凑的那一个，惹不起惹不起。所以舒服的思路还是四元数-->旋转矩阵-->欧拉角，想一步到位的话，把四元数分量参数化的旋转矩阵、欧拉角参数化的旋转矩阵结合在一起，参考下旋转矩阵转欧拉角的方法，替换下元素就完事了。这里就不把公式展开了，因为四元数直接转欧拉角 跟 旋转矩阵转欧拉角一样，依旧是要处理gimbal lock的corner case，还是那么麻烦，所以这里先鸽了

轴角

轴角---->四元数 轴-角(Axis-Angle)顾名思义就是绕某条单位轴旋转一定角度，从这个意义上看，它构造四元数是非常舒服的，毕竟直观的几何意义有一点点类似，绕单位轴   旋转   的四元数是： 轴角---->旋转矩阵 Axis Angle转Rotation Matrix可以从[9]罗德里格斯旋转公式Rodrigues Rotation Formula开始推导。 设   是我们要旋转的单位向量，旋转轴为   ，   绕   旋转角度   ，那么旋转后的向量为： 这个公式的推导思路是这样子的，我们先对向量   进行正交分解，分解成投影到旋转轴   的分量和垂直于   的分量： 其中：

图：魔性p图，假设k和v都在屏幕这个平面上吧

于是绕   旋转向量   其实就是把上面正交投影后的向量分别旋转之后再加起来。那么很明显的，投影到旋转轴上的部分   都跟旋转轴共享了，那么自然旋转之后的结果就没有变化了，于是我们只需要旋转和旋转轴垂直的部分   。那么这个 旋转后的表达式就是： 然后我们不按wikipedia里面坑爹的、不考虑下文的变形，自己推一波： 这里我们把旋转后向量的表达式变形得只剩下叉积(cross product)，去掉点积(dot product)了，这样子我们才可以把这个绕轴旋转的表达式写成矩阵形式。怎么写呢？首先叉积可以写成矩阵形式： 于是罗德里格斯旋转公式的变换就可以写成矩阵形式： 展开之后就是：

参考文献

[1]Henderson, D.M.. Euler angles, quaternions, and transformation matrices for space shuttle analysis[C]//NASA, Jun 09, 1977. [2]  https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles%23Euler_Angles_to_Quaternion_Conversion [3] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Euler_angles [4]  https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Rotation_matrix [5] Slabaugh G G. Computing Euler angles from a rotation matrix[J]. 1999. [6] Mike Day, Converting a Rotation Matrix to a Quaternion.  https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf [7] Tomas K.M. , Eric H., Naty H.. Real Time Rendering 3rd Edition , p68-p69, 2008. [8] Tomas K.M. , Eric H., Naty H.. Real Time Rendering 3rd Edition , p76-p77, 2008. [9]  https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Rodrigues%2527_rotation_formula [10]  https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Cross_product%23Conversion_to_matrix_multiplication [11] https://link.zhihu.com/?target=http%3A//mathworld.wolfram.com/RodriguesRotationFormula.html [12]  https://link.zhihu.com/?target=https%3A//zh.wikipedia.org/wiki/%25E5%259B%259B%25E5%2585%2583%25E6%2595%25B8 [13]  https://link.zhihu.com/?target=https%3A//blog.csdn.net/silangquan/article/details/39008903 [14] Quaternion and Rotations,  https://link.zhihu.com/?target=http%3A//run.usc.edu/cs520-s12/quaternions/quaternions-cs520.pdf [15] https://link.zhihu.com/?target=https%3A//en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

本文转自知乎：鸡哥

原文网址：https://zhuanlan.zhihu.com/p/45404840

未经同意请勿二次转载

- END -

关注本公众号，获得更多的“自动驾驶”算法原创干货，帮你快速入行 关注本公众号，点击“入群”，加入深度学习技术群，热心大佬帮你答疑解惑

自动驾驶历史文章阅读

资料下载：

2019自动驾驶资料大放送

7天，搞定机器学习基础知识

人工智能相关资料

头条 | 高定位精度的交通标志识别----开源了

头条 | COVID-19 CT数据库下载

激光雷达(Lidar)相关： 主要涉及lidar的目标检测方法分析、融合感知方法分析、如何从点云做end-to-end目标轨迹预测、模型加速与工程化 头条 | 自动驾驶多传感器融合技术浅析 一文览尽“基于激光雷达点云(lidar)的目标检测方法” 头条 | 自动驾驶环境感知技术要点浅析 头条 | 深度学习模型inference优化之编译优化 头条 | Fast and Furious，速度与激情？ 图像相关： 涉及了目标检测与分割、网络结构设计与模型加速。从基础的知识点出发，帮你一步一步理解方法，加深印象。 轻量(高效)目标检测网络结构设计 mask rcnn 与 PointRend 一文读懂RPN和ROI Align CNN中的感受野 目标检测中的Anchor分析 行业信息： 梳理行业大事件，帮你了解行业的发展状况。 主要车企和造车新势力自动驾驶传感器配置方案 福特开源1.8TB自动驾驶数据，到底开源数据哪家强？

这个冬天不太冷：小马5亿美元，驭势获博士领投，waymo22.5亿美元

以上相关方法，后续会对照代码再次回忆，关注公众号，敬请留意。