旋转矩阵、欧拉角、四元数及四元数插值

三维旋转表示

在三维世界中的旋转的表示的方法主要有：
旋转只是3个自由度未知量。

四元数

四元数原理是复平面的维度延伸。增加了三个虚轴。来表示三维空间刚体的旋转。一个四元数 q 拥有一个实部和三个虚部。

其中 i,j,k 为四元数的三个虚部。这三个虚部满足关系式：

也用一个标量和一个向量来表达四元数：向量v对应旋转轴坐标，s对应旋转角度的余弦量。

四元数之间可以进行乘法运算：

假设一个空间三维点 $p=[x,y,z]\in R3$，以 及一个由轴角 n,θ 指定的旋转。三维点 p 经过旋转之后变成为 $p\prime$。如果使用矩阵描述，那 么有 $p\prime =Rp$。如果用四元数描述旋转：
首先，把三维空间点用一个虚四元数来描述：

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。
假设某个旋转是绕单位向量$n=[nx,ny,nz{]}^T$进行了角度为 θ 的旋转，那么这个旋转的四元数形式为：

或

那么，旋转后的点$p\prime$即可表示为这样的乘积：

我们亦可从单位四元数中计算出对应旋转轴与夹角：

四元数时间导数（目前比较常用四元数来表示及计算旋转得方法）：
设初始旋转为$q=[s,v]$，若角速度为$\omega$，忽略角轴旋转，那么旋转的时间导数即为：

旋转矩阵

3维坐标系经过旋转，得到新的坐标系，新坐标系三轴在旧坐标系下的表示构成的3*3矩阵，就是能够表述旋转的旋转矩阵。
但是3*3矩阵用来表述3个自由度的旋转时，变量产生冗余，因此旋转矩阵及其还要满足一系列其他的性质。且表示也不够直观。
旋转矩阵介绍
首先，研究坐标系间的旋转描述，设某个单位正交基$(e^1,e^2,e^3)$经过一次旋转，变成了$(e{\prime }^1,e{\prime }^2,e{\prime }^3)$。那么，对于同一个向量a（注意该向量并没有 随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为$[a^1,a^2,a^3{]}^T$和$[a{\prime }^1,a{\prime }^2,a{\prime }^3{]}^T$。根据坐标的定义，有：

上面等式左右同时左乘 $[e^{T1}{e}^{T2}{e}^{T3}{]}^T$，得

其中，矩阵 R 描述了旋转本身。因此它又称为旋转矩阵。
由于旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转，因此RT可以描述旋转反变换。
旋转矩阵，是一个行列式为 1 的正交矩阵。

旋转矩阵导数
使用旋转矩阵R时，角速度为$\omega$，那么R相对于时间的导数可写作：

该式称为泊松公式，其中$\wedge$为反对称矩阵算子：

角轴

任何旋转都可以看成，围绕空间某个轴旋转一定的角度产生的，角轴对应旋转轴的方向，其模长可以用来关联旋转的角度。

• 欧拉角
  欧拉角它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。比较清晰直观，也较为常用，但是欧拉角会有万向锁的问题。

欧拉角

ZYX转角相当于把任意旋转分解成以下三个轴上的转角：
1. 绕物体的 Z 轴旋转，得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

此时，我们可以使用 $[r,p,y{]}^T$ 这样一个三维的向量描述任意旋转。
坐标系的定义不同，欧拉角变换矩阵会有所差异，以下是 东北天(右前上) 左手坐标系下的欧拉角变换矩阵。角的正负用右手规则确定。

若坐标系A绕其x轴转过角度δ后与坐标系B重合，则该矢量在两个坐标系中的投影可以通过一个初等旋转矩阵实现转换，即：

$$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\delta & sin\delta \\ 0& -sin\delta & cos\delta \end{array}\right]\left[\begin{array}{c}{X}_A\\ Y_A\\ Z_A\end{array}\right]$$

若坐标系A绕其y轴转过δ后与坐标系B重合，则有：

$$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{ccc}cos\delta & 0& -sin\delta \\ 0& 1& 0\\ sin\delta & 0& cos\delta \end{array}\right]\left[\begin{array}{c}{X}_A\\ Y_A\\ Z_A\end{array}\right]$$
若坐标系A绕其z轴转过δ后与坐标系B重合，则有：
$$\left[\begin{array}{c}{X}_B\\ Y_B\\ Z_B\end{array}\right]=\left[\begin{array}{ccc}cos\delta & sin\delta & 0\\ -sin\delta & cos\delta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_A\\ Y_A\\ Z_A\end{array}\right]$$
每次绕某坐标轴的旋转，均相当于左乘一个初等旋转矩阵。多次旋转相当于用相应的初等旋转矩阵连续左乘。旋转的正方向按右手法则确定(与坐标系之间角度正方向 定义相同)。
但是，欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）。当一个轴旋转90度与另一个轴重合，则两个旋转轴并为一个。如下图所示。

具体解释可参考https://www.zhihu.com/question/47736315

李群&李代数

以下内容来自十四讲：
在运动描述中，旋转矩阵及角轴可利用李群李代数的方式描述，从而解决旋转的微观描述，解决旋转的求导问题。
李群及李代数的转换关系如下：

exp实现过程
在将se(3)李代数向量转换为SE(3)变换矩阵时，可以使用指数映射（exponential map）来计算。

指数映射的计算公式如下：
$$\exp (\mathbf{X})=\mathbf{I}+\frac{\sin (\theta )}{\theta }\mathbf{A}+\frac{1-\cos (\theta )}{{\theta }^2}{\mathbf{A}}^2$$

其中：

($\mathbf{X}$) 是一个6维的se(3)李代数向量
($\mathbf{I}$) 是3x3单位矩阵
($\theta$) 是旋转角度，即($|\mathbf{w}|$)（其中($\mathbf{w}$)是旋转轴）
($\mathbf{A}$) 是一个3x3的反对称矩阵，表示旋转轴的叉乘形式

具体计算步骤如下：

首先，将se(3)李代数向量($\mathbf{X}$)分解为旋转部分和平移部分，即($\mathbf{X}=[\mathbf{w},\mathbf{v}]$)，其中($\mathbf{w}$)是旋转轴，($\mathbf{v}$)是平移向量。

计算旋转角度($\theta =|\mathbf{w}|$)。如果($\theta$)接近于0，则表示没有旋转，直接使用平移向量计算即可。

计算旋转轴的反对称矩阵($\mathbf{A}$)。具体公式为：

$$\mathbf{A}=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ -w_y& w_x& 0\end{array}\right]$$

根据上述公式计算指数映射($\exp (\mathbf{X})$)。如果(\theta)接近于0，则为平移变换，直接使用平移向量构建平移矩阵；否则，使用上述公式计算旋转矩阵和平移向量并组合成SE(3)变换矩阵。

需要注意的是，对于小角度旋转，可以使用一阶泰勒级数展开来近似计算指数映射，以提高计算效率。

导数及扰动

导数及扰动用来微观描述旋转或运动：存在两种解决办法：

1.对 R 对应的李代数加上小量，求相对于小量的变化率（导数模型）；
2.对 R 左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型）。

1. 导数模型：
   
   带雅可比矩阵，形式较复杂；
2. 扰动模型(左扰动)，常用
   SO3左扰动模型：
   
   SE3左扰动模型：
   

SO3(李群)&so3(李代数)vs旋转矩阵&角轴

旋转矩阵$R\in SO(3)$。
so3 对应角轴。

旋转矩阵R(t)具有如下性质：

通过整理：具体细节不展开，得：

其中，
∧ 符号，将向量转换成反对称矩阵。∨ 表示将反对称矩阵写成向量形式，如下：

整理计算得到旋转矩阵R(t)的导数：

定义三维李代数so(3) 的元素是 3 维向量或者 3 维反对称矩阵：

so(3)与 SO(3) 的关系如下：

SE(3)也有对应的李代数 se(3)：

由于$\varphi$是三维向量，我们可以定义它的模长和它的方向，分别记作$\theta$和 $a$，于是有 $\varphi =\theta a$。这里$a$是一个长度为 1 的方向向量。

se3和SE3的对应关系如下：
$$T=exp({\xi }^{\wedge })$$
可知李代数$\varphi$表述旋转矩阵R对应的旋转轴及旋转角度。$a$描述旋转轴，$\theta$描述旋转角度。$\varphi$可以说是角轴的表示了。

旋转表示之间的转换

左手坐标系，右手坐标系(如ENU)均存在应用场景，如Unity使用左手坐标系，而OpenGL使用右手坐标系。

• 右手坐标系

  • 伸出右手，让大拇指、食指和中指相互垂直。其中，大拇指的方向定义为(x)轴的正方向，食指的方向定义为(y)轴的正方向，中指的方向定义为(z)轴的正方向。
  • 遵循右手定则来确定坐标轴的旋转方向，即当右手的四指从(x)轴正方向以小于(180)度的角度转向(y)轴正方向时，大拇指所指的方向就是(z)轴的正方向，或者说，按照右手螺旋法则，以右手握住(z)轴，当右手的四指沿着从(x)轴到(y)轴的旋转方向时，大拇指的指向就是(z)轴的正方向。在右手坐标系中，从(x)轴正半轴到(y)轴正半轴的旋转方向为逆时针方向。
  • 右手坐标系中的旋转规定：逆时针旋转为正方向（符合右手螺旋法则）。

• 左手坐标系

  • 伸出左手，同样让大拇指、食指和中指相互垂直。此时，大拇指的方向为(x)轴的正方向，食指的方向为(y)轴的正方向，中指的方向为(z)轴的正方向。
  • 与右手坐标系相反，在左手坐标系中，从(x)轴正半轴到(y)轴正半轴的旋转方向为顺时针方向。即当左手的四指从(x)轴正方向以小于(180)度的角度转向(y)轴正方向时，大拇指所指的方向就是(z)轴的正方向，或者用左手螺旋法则，以左手握住(z)轴，当左手的四指沿着从(x)轴到(y)轴的旋转方向（顺时针）时，大拇指的指向就是(z)轴的正方向。
  • 左手坐标系中的旋转规定：顺时针旋转为正方向（符合左手螺旋法则）。比较少用。

通常默认使用右手坐标系，遵守右手法则，逆时针旋转为正。

通过交换两个轴，或者将其中一个轴反向，即可转化左右手坐标系。经过旋转后的坐标系的左右手性维持不变。

左手坐标系和右手坐标系旋转矩阵构建的规则是有差别的。
从欧拉角、四元数以及李代数构建旋转矩阵时，左手坐标系和右手坐标系得到的结果有所差别。

旋转矩阵与欧拉角转换推导

欧拉角到旋转矩阵

$R_{BW}体<-世界$

关于欧拉角旋转矩阵中sin的反号问题(上正下负和下正上负)， 以下规则左右手坐标系通用：
轴的排序（x->y->z）与旋转角度正方向一致，则上正下负。反之，下正上负。这是由于左右坐标系对旋转正负的定义相反，这里算的旋转矩阵都是旧坐标系转到新坐标系的坐标映射，RCur<-Last。

注：RLast<-Cur与上相反，而旋转矩阵通常的定义是从body坐标系到世界坐标系，从Cur->Last

$$R_z^{\text{右}}\left({\theta }_z\right)=\left[\begin{array}{ccc}cos{\theta }_z& sin{\theta }_z& 0\\ -sin{\theta }_z& cos{\theta }_z& 0\\ 0& 0& 1\end{array}\right],R_z^{\text{左}}\left({\theta }_z\right)=\left[\begin{array}{ccc}cos{\theta }_z& -sin{\theta }_z& 0\\ sin{\theta }_z& cos{\theta }_z& 0\\ 0& 0& 1\end{array}\right]$$

以下为右手坐标系示例：
绕x轴旋转${\theta }_x$角的旋转矩阵：
$$R_x\left({\theta }_x\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos{\theta }_x& sin{\theta }_x\\ 0& -sin{\theta }_x& cos{\theta }_x\end{array}\right]$$
绕y轴旋转${\theta }_y$角的旋转矩阵：
$$R_y\left({\theta }_y\right)=\left[\begin{array}{ccc}cos{\theta }_y& 0& -sin{\theta }_y\\ 0& 1& 0\\ sin{\theta }_y& 0& cos{\theta }_y\end{array}\right]$$
绕z轴旋转${\theta }_z$角的旋转矩阵：
$$R_z\left({\theta }_z\right)=\left[\begin{array}{ccc}cos{\theta }_z& sin{\theta }_z& 0\\ -sin{\theta }_z& cos{\theta }_z& 0\\ 0& 0& 1\end{array}\right]$$

R的构造形式：Z-X-Y 左乘，先绕Y轴旋转
$$R=R_z\ast R_x\ast R_y$$
$$R(\mathbf{\theta })=\left[\begin{array}{ccc}\cos ({\theta }_z)\cos ({\theta }_y)+\sin ({\theta }_z)\sin ({\theta }_x)\sin ({\theta }_y)& \sin ({\theta }_z)\cos ({\theta }_x)& -\cos ({\theta }_z)\sin ({\theta }_y)+\sin ({\theta }_z)\sin ({\theta }_x)\cos ({\theta }_y)\\ -\sin ({\theta }_z)\cos ({\theta }_y)+\cos ({\theta }_z)\sin ({\theta }_x)\sin ({\theta }_y)& \cos ({\theta }_z)\cos ({\theta }_x)& \sin ({\theta }_z)\sin ({\theta }_y)+\cos ({\theta }_z)\sin ({\theta }_x)\cos ({\theta }_y)\\ \cos ({\theta }_x)\sin ({\theta }_y)& -\sin ({\theta }_x)& \cos ({\theta }_x)\cos ({\theta }_y)\end{array}\right]$$

角度验证：

• 当 $\theta =90^{\circ }$ 时，$\sin \theta =1$，检查旋转后坐标是否符合预期。

• 例如，右手坐标系中绕Z轴逆时针旋转 $90^{\circ }$，点 $(1,0,0)$ 的坐标在新坐标系下的表示为$(0,-1,0)$，即$R_z^{\text{右}}({\theta }_z)\cdot [1,0,0{]}^{\mathrm{T}}=[0,-1,0{]}^{\mathrm{T}}$符合矩阵计算结果。
  左手坐标系中绕Z轴顺时针旋转 $90^{\circ }$，点 $(1,0,0)$ 的坐标在新坐标系下的表示为$(0,1,0)$，即$R_z^{\text{左}}({\theta }_z)\cdot [1,0,0{]}^{\mathrm{T}}=[0,-1,0{]}^{\mathrm{T}}$符合矩阵计算结果。

• 右手坐标系：逆时针旋转时，$\sin \theta$ 在左下角为负，右上角为正。

• 左手坐标系：顺时针旋转时，$\sin \theta$ 在右上角为负，左下角为正。

• 关键口诀：

  • 右手系：“左下正，右上负” 上正下负。
  • 左手系：“右上正，左下负” 上负下正。

Tips: 关于如何快速通过旋转矩阵来观察出它的旋转顺序的方法：
观察单项(只有一个sin)在矩阵中的位置及其对应的旋转角，如上个矩阵中的$-\sin {\theta }_x$，则中间轴为X轴，其位于矩阵的(3,2)位置，则说明顺序是Rz*Rx*Ry。因此如果是左乘，其旋转顺序为Y->X->Z。

---

$R_{WB}世界<-体$

以下为将欧拉角映射为$R_{LC}$的旋转矩阵，为通用的旋转矩阵表示，即坐标系L(last)经过旋转后，成为坐标系C(current)。将点从坐标系{C}下转换到坐标系{L}下的旋转矩阵。与上文刚好相反，所以得到的旋转矩阵互为转至。
通常用$R_{WB}$对应的四元数来表示位姿（即表示位姿的旋转矩阵或者四元数，通常指的是从体坐标系到世界坐标系的旋转表示）。
以下介绍即$R_{WB}$

绕不同轴的旋转矩阵中 $\sin \theta$ 的符号
以绕 z轴 旋转为例：

• 右手坐标系（逆时针为正）：
  $$R_z^{\text{右}}(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$
  • $\sin \theta$ 出现在左下角（正号），$-\sin \theta$ 出现在右上角（负号）。

右手法则（逆时针为正）：

• 想象用右手握住旋转轴，大拇指指向轴的正方向。

• 四指弯曲方向为旋转正方向（逆时针）。

• 旋转矩阵中，$\sin \theta$ 的符号为：

  • 左下角：$+\sin \theta$
  • 右上角：$-\sin \theta$

• 左手坐标系（顺时针为正）：
  $$R_z^{\text{左}}(\theta )=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

  • $\sin \theta$ 出现在右上角（正号），$-\sin \theta$ 出现在左下角（负号）。

左手法则（顺时针为正）：

• 用左手握住旋转轴，大拇指指向轴的正方向。
• 四指弯曲方向为旋转正方向（顺时针）。
• 旋转矩阵中，$\sin \theta$ 的符号为：
  • 左下角：$-\sin \theta$
  • 右上角：$+\sin \theta$

常见旋转轴的符号规则$R_{WB}$

旋转轴	右手坐标系（逆时针）	左手坐标系（顺时针）
绕X轴	$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$	$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right]$
绕Y轴	$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$	$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right]$
绕Z轴	$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$	$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$

角度验证：

• 当 $\theta =90^{\circ }$ 时，$\sin \theta =1$，检查旋转后坐标是否符合预期。
• 例如，右手坐标系中绕Z轴旋转 $90^{\circ }$，点 $(1,0,0)$ 的坐标在新坐标系下的表示为$(0,-1,0)$，即$R_z^{\text{右}}(\theta )\cdot [0,-1,0{]}^{\mathrm{T}}=[1,0,0{]}^{\mathrm{T}}$符合矩阵计算结果。
  左手坐标系中绕Z轴顺时针旋转 $90^{\circ }$，点 $(1,0,0)$ 的坐标在新坐标系下的表示为$(0,1,0)$，即$R_z^{\text{左}}({\theta }_z)\cdot [0,-1,0{]}^{\mathrm{T}}=[1,0,0{]}^{\mathrm{T}}$符合矩阵计算结果
• 右手坐标系：逆时针旋转时，$\sin \theta$ 在左下角为正，右上角为负。
• 左手坐标系：顺时针旋转时，$\sin \theta$ 在右上角为正，左下角为负。
• 关键口诀：
  • 右手系：“左下正，右上负” 上负下正。
  • 左手系：“右上正，左下负” 上正下负。

---

旋转矩阵到欧拉角
设旋转矩阵如下：
$$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]$$
转换成欧拉角矩阵：
$${\theta }_x=atan2\left({-r}_{32},\sqrt{{r^2}_{31}+{r^2}_{33}}\right)或{\theta }_y=\beta =-asin\left(r_{32}\right)$$
$${\theta }_y=atan2\left(r_{31},r_{33}\right)$$

$${\theta }_z=\gamma =atan2\left(r_{22},r_{12}\right)$$

绕3轴分别转$\pm \pi$则坐标系回到初始状态

旋转矩阵表示绕3轴分别转$\pm \pi$则坐标系：

四元数到欧拉角相互转换

四元数到欧拉角的转换关系公式如下：
设$\psi ，\theta ，\varphi$分别为绕Z轴、Y轴、X轴的旋转角度，对应Yaw、Pitch、Roll。
欧拉角到四元数的转换公式：

四元数到欧拉角的转换公式：

由于$arctan$的值域为$[-\pi /2,\pi /2]$，如下图所示。

并没有覆盖所有角度，因此采用atan2来替代arctan计算欧拉角。atan2的值域为$[-\pi ,\pi ]$，atan2的计算公式如下：
因此四元数到欧拉角时，会有$-\pi ,\pi$之间的跳变。

得最终的四元数到欧拉角的计算公式如下：

欧拉角与四元数转换实现见https://blog.csdn.net/weixin_41469272/article/details/105510219

• 四元数
  四元数原理是复平面的维度延伸。增加了三个虚轴。来表示三维空间刚体的旋转。即1个实轴，3个虚轴，来表示旋转。
  四元数的描述：
  https://www.bilibili.com/video/BV1Lt411U7og?from=search&seid=16908357282964192364

常用一个标量和一个向量来表达四元数：向量v对应旋转轴坐标，s对应旋转角度的余弦量。

首先，把三维空间点用一个虚四元数来描述：

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。
假设某个旋转是绕单位向量n = [nx,ny,nz]T进行了角度为 θ 的旋转，那么这个旋转的四元数形式为：

(1)

即：

那么，旋转后的点p′即可表示为这样的乘积：

反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：

四元数时间导数（目前比较常用四元数来表示及计算旋转得方法）：
设初始旋转为 q =[s,v]，若角速度为 ω，忽略角轴旋转，那么旋转的时间导数即为：

四元数转旋转矩阵

四元数到旋转矩阵根据坐标系手性的不同，得到的旋转矩阵也不相同。针对左手坐标系和右手坐标系。关键区别在于坐标系旋向性（叉乘方向或轴方向符号的调整）。

右手坐标系（右手法则）

四元数定义为 $q=w+x\mathbf{i}+y\mathbf{j}+z\mathbf{k}$，其旋转矩阵公式为：
$$R_{\text{RH}}=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2zw& 2xz+2yw\\ 2xy+2zw& 1-2x^2-2z^2& 2yz-2xw\\ 2xz-2yw& 2yz+2xw& 1-2x^2-2y^2\end{array}\right]$$

左手坐标系（左手法则）

调整四元数的 Z轴分量符号（$z\to -z$），公式为：
$$R_{\text{LH}}=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2(-z)w& 2x(-z)+2yw\\ 2xy+2(-z)w& 1-2x^2-2z^2& 2y(-z)-2xw\\ 2x(-z)-2yw& 2y(-z)+2xw& 1-2x^2-2y^2\end{array}\right]$$
化简后：
$$R_{\text{LH}}=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy+2zw& -2xz+2yw\\ 2xy-2zw& 1-2x^2-2z^2& -2yz-2xw\\ -2xz-2yw& -2yz+2xw& 1-2x^2-2y^2\end{array}\right]$$

---

测试代码

#include <Eigen/Dense> // 使用Eigen库简化矩阵运算

Eigen::Matrix3d quaternionToRotationMatrixRH(const Eigen::Quaterniond& q) {
    // 右手坐标系：旋转方向遵循右手法则
    Eigen::Matrix3d R;
    double w = q.w(), x = q.x(), y = q.y(), z = q.z();
    R << 1 - 2*y*y - 2*z*z,   2*x*y - 2*z*w,     2*x*z + 2*y*w,
          2*x*y + 2*z*w,     1 - 2*x*x - 2*z*z,   2*y*z - 2*x*w,
          2*x*z - 2*y*w,     2*y*z + 2*x*w,     1 - 2*x*x - 2*y*y;
    return R;
}

Eigen::Matrix3d quaternionToRotationMatrixLH(const Eigen::Quaterniond& q) {
    // 左手坐标系：调整叉乘方向（通常反转Z轴符号）
    Eigen::Matrix3d R;
    double w = q.w(), x = q.x(), y = q.y(), z = -q.z(); // Z轴反向
    R << 1 - 2*y*y - 2*z*z,   2*x*y - 2*z*w,     2*x*z + 2*y*w,
          2*x*y + 2*z*w,     1 - 2*x*x - 2*z*z,   2*y*z - 2*x*w,
          2*x*z - 2*y*w,     2*y*z + 2*x*w,     1 - 2*x*x - 2*y*y;
    return R;
}

• 右手系直接使用标准公式。
• 左手系代码中 z = -q.z() 对应公式中的 $z\to -z$。

李代数（旋转向量）转旋转矩阵

同样，李代数到旋转矩阵根据坐标系手性的不同，得到的旋转矩阵也不相同。针对左手坐标系和右手坐标系。关键区别在于坐标系旋向性（叉乘方向或轴方向符号的调整）。

右手坐标系（罗德里格斯公式）

旋转向量定义为 $\mathbf{\varphi }=\theta \mathbf{u}$，其中 $\mathbf{u}$ 为单位旋转轴，$\theta$ 为旋转角度。旋转矩阵为：
$$R_{\text{RH}}=e^{\theta [\mathbf{u}{]}_{\times }}=I+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2$$
其中叉乘矩阵为：
$$[\mathbf{u}{]}_{\times }=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right]$$

左手坐标系（调整叉乘方向）

在左手系中，叉乘方向反转（( \mathbf{a} \times \mathbf{b} \to -\mathbf{a} \times \mathbf{b} )），因此叉乘矩阵符号取反：
$$[\mathbf{u}{]}_{\times }^{\text{LH}}=\left[\begin{array}{ccc}0& u_z& -u_y\\ -u_z& 0& u_x\\ u_y& -u_x& 0\end{array}\right]$$
旋转矩阵公式调整为：
$$R_{\text{LH}}=I+\sin \theta [\mathbf{u}{]}_{\times }^{\text{LH}}+(1-\cos \theta ){\left([\mathbf{u}{]}_{\times }^{\text{LH}}\right)}^2$$

测试代码

#include <cmath>
Eigen::Matrix3d so3ToRotationMatrixRH(const Eigen::Vector3d& so3) {
    // 右手坐标系：旋转方向遵循右手法则
    double theta = so3.norm();
    if (theta < 1e-6) return Eigen::Matrix3d::Identity();
    Eigen::Vector3d axis = so3 / theta;
    Eigen::Matrix3d K;
    K << 0, -axis.z(), axis.y(),
         axis.z(), 0, -axis.x(),
         -axis.y(), axis.x(), 0;
    Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + sin(theta) * K + (1 - cos(theta)) * K * K;
    return R;
}

Eigen::Matrix3d so3ToRotationMatrixLH(const Eigen::Vector3d& so3) {
    // 左手坐标系：反转旋转轴或角度符号
    Eigen::Vector3d so3_adj = -so3; // 或反转旋转轴
    double theta = so3_adj.norm();
    if (theta < 1e-6) return Eigen::Matrix3d::Identity();
    Eigen::Vector3d axis = so3_adj / theta;
    Eigen::Matrix3d K;
    K << 0, axis.z(), -axis.y(),    // 调整叉乘符号
         -axis.z(), 0, axis.x(),
         axis.y(), -axis.x(), 0;
    Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + sin(theta) * K + (1 - cos(theta)) * K * K;
    return R;
}

---

关键区别总结

坐标系	四元数调整	李代数调整
右手坐标系	无调整	标准叉乘矩阵 $[\mathbf{u}{]}_{\times }$
左手坐标系	四元数 $z\to -z$	叉乘矩阵符号反转 $[\mathbf{u}{]}_{\times }\to -[\mathbf{u}{]}_{\times }$

• 右手系使用标准叉乘矩阵 ([\mathbf{u}]_\times)。
• 左手系代码中的 K << 0, axis.z(), -axis.y(), ... 对应调整后的叉乘矩阵 $[\mathbf{u}{]}_{\times }^{\text{LH}}$。

---

其他

四元数插值

四元数处于四维空间，但是投影在3维空间是个球体，对应着单位旋转轴构成的球体。
即$||n|{|}_2=1$的球体。对式（1）的 θ 加上 2π，得到一个相同的旋转，但此时对应的四元数变成了 −q。
因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取 θ 为 0，则得到一个没有任何旋转的实四元数：（摘自十四讲）

普通线性插值

设空间存在两个向量$p,q$，使用普通线性插值，来填充$p$到$q$之间旋转。
取$t\in [0,1]$，设中间插值$r=p+(q-p)t$。

假如t取值为1/4、2/4、3/4即将P0P1弦长均分为4等份，可以看出其对应的弧长并不相等。靠近中间位置的弧长较长，而靠近两段处的弧长较短，这就意味着当t匀速变化时，代表姿态矢量的角速度变化并不均匀。

球面线性插值(slerp)

• 以下内容主要摘自https://www.cnblogs.com/21207-iHome/p/6952004.html
  即按角度及弧面均匀插值。球面插值是四元数的一种线性插值运算，主要用于在两个表示旋转的四元数之间平滑插值。
  设$p$与$q$之间的夹角$\theta$，要将角度均匀划分，因此，插值向量$r$与$p$之间的关系是$t\theta$，$q$和$r$之间的夹角为$(1-t)\theta$：

求解$\mathbf{r}=a(t)\mathbf{p}+b(t)\mathbf{q}$，找到合适的a(t)和b(t)。满足$r$与$p$之间的关系是$t\theta$，$q$和$r$之间的夹角为$(1-t)\theta$。
将上面的公式两边点乘p可得$$\begin{gathered} \mathbf{p}\cdot \mathbf{r}=a(t)\mathbf{p}\cdot \mathbf{p}+b(t)\mathbf{p}\cdot \mathbf{q} \\ \cos t\theta =a(t)+b(t)\cos \theta \end{gathered}$$

同样地，对公式两边点乘q可得$$\cos [(1-t)\theta ]=a(t)\cos \theta +b(t)$$

两个方程可以解出两个未知量a(t)和b(t)：$$\begin{gathered} a(t)=\frac{\cos t\theta -\cos [(1-t)\theta ]\cos \theta }{1-{\cos }^2\theta } \\ b(t)=\frac{\cos [(1-t)\theta ]-\cos t\theta \cos \theta }{1-{\cos }^2\theta } \end{gathered}$$

使用三角函数公式可以将其化简为：$$\begin{gathered} a(t)=\frac{\sin [(1-t)\theta ]}{\sin \theta } \\ b(t)=\frac{\sin t\theta }{\sin \theta } \end{gathered}$$

于是四元数的球面线性插值公式为：$$Slerp(\mathbf{p},\mathbf{q},t)=\frac{\sin [(1-t)\theta ]\mathbf{p}+\sin t\theta \mathbf{q}}{\sin \theta }$$

需考虑两种情况：

1. 如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。

   四元数的三个虚变量对应旋转轴，因此两个相反向量对应的是同一旋转轴，因此两个相反的四元数表示的旋转是一样的。

2. 当$p$和$q$的夹角$\theta$差非常小时会导致$sin\theta \to 0$，这时除法可能会出现问题。为了避免这样的问题，当θ非常小时可以使用简单的线性插值代替（$\theta \to 0$时，$sin\theta \approx \theta$，因此方程退化为线性方程：$slerp(p,q,t)=(1-t)p+tq)$

球面插值实现代码，基于c++ eigen

#include <math.h>

Eigen::Quaterniond Quaternion_S_lerp(Eigen::Quaterniond &start_q, Eigen::Quaterniond &end_q, double t)
{
        Eigen::Quaterniond lerp_q;

        double cos_angle = start_q.x() * end_q.x()
                         + start_q.y() * end_q.y()
                         + start_q.z() * end_q.z()
                         + start_q.w() * end_q.w();

        // If the dot product is negative, the quaternions have opposite handed-ness and slerp won't take
        // the shorter path. Fix by reversing one quaternion.
        if (cos_angle < 0) {
                end_q.x() = -end_q.x();
                end_q.y() = -end_q.y();
                end_q.z() = -end_q.z();
                end_q.w() = -end_q.w();
                cos_angle = -cos_angle;
        }

        double ratio_A, ratio_B;
         If the inputs are too close for comfort, linearly interpolate
        if (cos_angle > 0.99995f) {
                ratio_A = 1.0f - t;
                ratio_B = t;
        }
        else {
                double sin_angle = sqrt( 1.0f - cos_angle * cos_angle);
                double angle = atan2(sin_angle, cos_angle);
                ratio_A = sin((1.0f - t) * angle)  / sin_angle;
                ratio_B = sin(t * angle) / sin_angle;
        }

        lerp_q.x() = ratio_A * start_q.x() + ratio_B * end_q.x();
        lerp_q.y() = ratio_A * start_q.y() + ratio_B * end_q.y();
        lerp_q.z() = ratio_A * start_q.z() + ratio_B * end_q.z();
        lerp_q.w() = ratio_A * start_q.w() + ratio_B * end_q.w();

        return lerp_q.normalized();
}

Eigen::Quaterniond q_inte = Quaternion_S_lerp(q_last, q_curr, 1/hz_inc_c*inte_i);

Eigen::Quaterniond::Identity().slerp(t, q_last_curr) 能够实现四元数球面插值。
$t\in [0,1]$为插值点。q_last_curr为两帧之间的旋转四元数，即在两帧之间的旋转线性插入旋转四元数。

不同上述代码不同的是，这个在两帧之间的变换插值，即假设前帧为参考帧，而后帧相对前帧参考系进行了q_last_curr的旋转。

而实现代码是得到了前后两帧相对于原始参考系的四元数，在两个四元数之间进行插值。

四元数与旋转矩阵的意义

• 四元数表示姿态，描述了体坐标系相对于本地固定坐标系（原点坐标系）的旋转。
• 旋转矩阵的物理意义为：将体坐标系中的向量转换到**本地固定坐标系（原点坐标系）**中。

在ros 发布odometry消息时，则对应的$q,t$构成从体到世界坐标系的变换矩阵。

若旋转矩阵为 $R$，则对任意体坐标系中的向量 ${\mathbf{v}}_{\text{body}}$，其世界坐标系中的表示为：
$${\mathbf{v}}_{\text{world}}=R\cdot {\mathbf{v}}_{\text{body}}$$

默认坐标系测试

注意：
除欧拉角外，旋转其余的表示方法，如旋转矩阵，四元数，轴角(李代数)等都没有次序问题，因此从欧拉角到其他表示之间相互转换需要指定旋转顺序。而其他表示**(旋转矩阵，四元数，轴角(李代数))之间互相转换不需要指定旋转次序**。

Eigen

Eigen支持指定旋转顺序，但是其有固定默认的坐标关系，Eigen坐标系各轴关系如下：

上图仅是表示轴之间的关系，而非Eigen自身的定义如x轴一定指向右方，下同

tf

tf不支持旋转顺序定义，其默认旋转顺序为Z->Y->X(先转Z，累乘顺序是RxRyRz，最后的才是第一旋转轴)，其各轴关系如下：

测试代码如下：

#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

double deg2rad = M_PI/180;
int main()
{
  double rx = 0;
  double ry = 0;
  double rz = 0;

  cout << "Enter the roll: ";
  cin >> rx; 
  cout << "Enter the pitch: ";
  cin >> ry; 
  cout << "Enter the yaw: ";
  cin >> rz; 

  cout << "catch the rpy value: " << rx << " " << ry << " " << rz << endl;
  
  rx *= deg2rad;
  ry *= deg2rad;
  rz *= deg2rad;

  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  //WUN Left up forward
  Eigen::Matrix3d NR; 
  NR <<
    crz*cry+srz*srx*sry, srz*crx, -crz*sry+srz*srx*cry,
    -srz*cry+crz*srx*sry,   crz*crx, srz*sry+crz*srx*cry,
    crx*sry,              -srx,   crx*cry;

  cout << "R generate by matrix: " << endl << NR << endl;

  Eigen::Vector3d Nr = NR.eulerAngles(2,0,1);
  cout << "euler: rx, ry, rz: " << -Nr[1]/deg2rad  << " " << -Nr[2]/deg2rad << " "   << -Nr[0]/deg2rad<< endl;

  cout << "That means the eigen use frame is East(right), Down, North(behind)" << endl;

#if 1
  Eigen::AngleAxisd rxA(rx, Eigen::Vector3d::UnitX());
  cout << "rxA: " << endl << rxA.matrix() << endl;
  Eigen::AngleAxisd ryA(ry, Eigen::Vector3d::UnitY());
  cout << "ryA: " << endl << ryA.matrix() << endl;
  Eigen::AngleAxisd rzA(rz, Eigen::Vector3d::UnitZ());
  cout << "rzA: " << endl << rzA.matrix() << endl;

  NR = (rxA * ryA * rzA).matrix();
  cout << "R generate by angle axisd: " << endl << NR << endl;

  /*Eigen::Vector3d */Nr = NR.eulerAngles(0,1,2);
  cout << "euler: rx, ry, rz: " << Nr[0]/deg2rad  << " " << Nr[1]/deg2rad << " "   << Nr[2]/deg2rad<< endl;
#endif

  cout << "Then test the default rotation order" << endl;

  Eigen::Quaterniond q = Eigen::Quaterniond(NR);
  cout << "q: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl;

  cout << "q to rotation matrix:" << endl << q.toRotationMatrix() << endl;

  cout << "Rotation trans from q if equals angleaxisd X-Y-Z." << endl
       << "That means the default rotation order is X-Y-Z." << endl;
  return 0;
}
//g++ eigen_frame_test.cpp `pkg-config eigen3 --libs --cflags`                                                                                

#include <iostream>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tf/transform_datatypes.h>

using namespace std;

double deg2rad = M_PI/180;
int main()
{
  double rx = 0;
  double ry = 0;
  double rz = 0;

  cout << "Enter the roll: ";
  cin >> rx; 
  cout << "Enter the pitch: ";
  cin >> ry; 
  cout << "Enter the yaw: ";
  cin >> rz; 

  cout << "catch the rpy value: " << rx << " " << ry << " " << rz << endl;
  
  rx *= deg2rad;
  ry *= deg2rad;
  rz *= deg2rad;

  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  //ENU:2,0,1  WUN:1,0,2
  Eigen::Matrix3d R;

  cout << "***************WUN test*************" << endl;
  cout << "Rotation around x axis:" << endl;
  R <<
     1, 0, 0,
     0, crx, srx,
     0, -srx, crx;
  cout << R << endl;
  cout << "Rotation around y axis:" << endl;
  R <<
     cry, 0, -sry,
     0, 1, 0,
     sry, 0, cry;
  cout << R << endl;
  cout << "Rotation around z axis:" << endl;
  R <<
     crz, srz, 0,
     -srz, crz, 0,
     0, 0, 1;
  cout << R << endl;

  R <<
    crz*cry+srz*srx*sry, srz*crx, -crz*sry+srz*srx*cry,
    -srz*cry+crz*srx*sry,   crz*crx, srz*sry+crz*srx*cry,
    crx*sry,              -srx,   crx*cry;

  cout << "R generate by matrix: " << endl << R << endl;

  cout << "***************Eigen test*************" << endl;

  cout << "Test the relationship of axes:" << endl;
  cout << "Rotation around x axis:" << endl;
  Eigen::AngleAxisd rxAT(rx, Eigen::Vector3d::UnitX());
  cout << rxAT.matrix() << endl;
  cout << "Rotation around y axis:" << endl;
  Eigen::AngleAxisd ryAT(ry, Eigen::Vector3d::UnitY());
  cout << ryAT.matrix() << endl;
  cout << "Rotation around z axis:" << endl;
  Eigen::AngleAxisd rzAT(rz, Eigen::Vector3d::UnitZ());
  cout << rzAT.matrix() << endl;

  cout << endl << "Test Euler to rotation***" << endl;
  //Eigen::Vector3d Nr = R.eulerAngles(0,1,2);
  //cout << "euler: rx, ry, rz: " << Nr[0]/deg2rad  << " " << Nr[1]/deg2rad << " "   << Nr[2]/deg2rad<< endl;
  Eigen::Vector3d Nr = R.eulerAngles(2,0,1);
  cout << "euler: rx, ry, rz get from eigen rotation: "
       << -Nr[1]/deg2rad  << " " << -Nr[2]/deg2rad << " "   << -Nr[0]/deg2rad<< endl;

  Eigen::AngleAxisd rxA(-rx, Eigen::Vector3d::UnitX());
  Eigen::AngleAxisd ryA(-ry, Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd rzA(-rz, Eigen::Vector3d::UnitZ());
  R = (rzA * rxA * ryA).matrix();
  cout << "R generate by z x y order: negitive" << endl << R << endl;

  cout << "Then test the default rotation order" << endl;

  Eigen::Quaterniond q_e = Eigen::Quaterniond(R);
  cout << "q_e: " << q_e.w() << " " << q_e.x() << " " << q_e.y() << " " << q_e.z() << endl;

  cout << "q_e to rotation matrix:" << endl << q_e.toRotationMatrix() << endl << endl;

  cout << "***************tf test*************" << endl;
  tf::Quaternion q_t;
  tf::Matrix3x3 R_tf;
  cout << "Test the relationship of axes:" << endl;
  q_t.setRPY(rx, 0, 0);
  R_tf.setRotation(q_t);
  cout << "Rotation around x axis:" << endl
       << setw(10) << R_tf[0][0] << setw(10) << R_tf[1][0] << setw(10) << R_tf[2][0] << endl
       << setw(10) << R_tf[0][1] << setw(10) << R_tf[1][1] << setw(10) << R_tf[2][1] << endl
       << setw(10) << R_tf[0][2] << setw(10) << R_tf[1][2] << setw(10) << R_tf[2][2] << endl;

  q_t.setRPY(0, ry, 0);
  R_tf.setRotation(q_t);
  cout << "Rotation around y axis:" << endl
       << setw(10) << R_tf[0][0] << setw(10) << R_tf[1][0] << setw(10) << R_tf[2][0] << endl
       << setw(10) << R_tf[0][1] << setw(10) << R_tf[1][1] << setw(10) << R_tf[2][1] << endl
       << setw(10) << R_tf[0][2] << setw(10) << R_tf[1][2] << setw(10) << R_tf[2][2] << endl;

  q_t.setRPY(0, 0, rz);
  R_tf.setRotation(q_t);
  cout << "Rotation around z axis:" << endl
       << setw(10) << R_tf[0][0] << setw(10) << R_tf[1][0] << setw(10) << R_tf[2][0] << endl
       << setw(10) << R_tf[0][1] << setw(10) << R_tf[1][1] << setw(10) << R_tf[2][1] << endl
       << setw(10) << R_tf[0][2] << setw(10) << R_tf[1][2] << setw(10) << R_tf[2][2] << endl;

  cout << endl << "Test Euler quaternion and rotation***" << endl;

  //tfScalar yaw,pitch,roll;
  //Doesn't support specify the rotation order, just assign values to function
  //and this will not change the order of rotation, which different from eigen
  q_t.setRPY(rz, rx, ry);
  q_t = {q_t[1], q_t[2], q_t[0], q_t[3]};

  //q_t.setRPY(rx, ry, rz);
  //q_t = {q_t[0], q_t[1], q_t[2], q_t[3]};
  //cout<<"quaternion:"<<q_t[3]<<","<<q_t[0]<<","<<q_t[1]<<","<<q_t[2]<<endl;

  R_tf.setRotation(q_t);
  cout << "R generate by tf quaternion: " << endl
       << setw(10) << R_tf[0][0] << setw(10) << R_tf[1][0] << setw(10) << R_tf[2][0] << endl
       << setw(10) << R_tf[0][1] << setw(10) << R_tf[1][1] << setw(10) << R_tf[2][1] << endl
       << setw(10) << R_tf[0][2] << setw(10) << R_tf[1][2] << setw(10) << R_tf[2][2] << endl;

  double rxe = 0, rye = 0, rze = 0;
  R_tf.getEulerYPR(rze, rxe, rye);
  //This will be see that same rotation matrix, and same axis correlation,
  // but different rotation order
  cout << "euler: rx, ry, rz: from same rotation: " << rx/deg2rad  << " " << ry/deg2rad << " "   << rz/deg2rad<< endl;

  q_t.setRPY(rx, ry, rz);
  R_tf.setRotation(q_t);
  cout << "R generate by tf quaternion with the x-y-z order: " << endl
       << setw(10) << R_tf[0][0] << setw(10) << R_tf[1][0] << setw(10) << R_tf[2][0] << endl
       << setw(10) << R_tf[0][1] << setw(10) << R_tf[1][1] << setw(10) << R_tf[2][1] << endl
       << setw(10) << R_tf[0][2] << setw(10) << R_tf[1][2] << setw(10) << R_tf[2][2] << endl;

  cout << "euler: rx, ry, rz: " << rx/deg2rad  << " " << ry/deg2rad << " "   << rz/deg2rad<< endl;
  return 0;
}

编译命令

g++ tf_euler.cpp `pkg-config eigen3 --libs --cflags` `pkg-config --libs --cflags tf_conversions`

运行结果：

Enter the roll: 30
Enter the pitch: 45
Enter the yaw: 60
catch the rpy value: 30 45 60
***************WUN test*************
Rotation around x axis:
       1        0        0
       0 0.866025      0.5
       0     -0.5 0.866025
Rotation around y axis:
 0.707107         0 -0.707107
        0         1         0
 0.707107         0  0.707107
Rotation around z axis:
      0.5  0.866025         0
-0.866025       0.5         0
        0         0         1
R generate by matrix: 
   0.65974       0.75 -0.0473672
 -0.435596   0.433013   0.789149
  0.612372       -0.5   0.612372
***************Eigen test*************
Test the relationship of axes:
Rotation around x axis:
       1        0        0
       0 0.866025     -0.5
       0      0.5 0.866025
Rotation around y axis:
 0.707107         0  0.707107
        0         1         0
-0.707107         0  0.707107
Rotation around z axis:
      0.5 -0.866025         0
 0.866025       0.5         0
        0         0         1

Test Euler to rotation***
euler: rx, ry, rz get from eigen rotation: 150 -135 -120
R generate by z x y order: negitive
   0.65974       0.75 -0.0473672
 -0.435596   0.433013   0.789149
  0.612372       -0.5   0.612372
Then test the default rotation order
q_e: 0.822363 -0.391904 -0.200562 -0.360423
q_e to rotation matrix:
   0.65974       0.75 -0.0473672
 -0.435596   0.433013   0.789149
  0.612372       -0.5   0.612372

***************tf test*************
Test the relationship of axes:
Rotation around x axis:
         1         0         0
         0  0.866025       0.5
         0      -0.5  0.866025
Rotation around y axis:
  0.707107         0 -0.707107
         0         1         0
  0.707107         0  0.707107
Rotation around z axis:
       0.5  0.866025         0
 -0.866025       0.5         0
         0         0         1

Test Euler quaternion and rotation***
R generate by tf quaternion: 
   0.65974      0.75-0.0473672
 -0.435596  0.433013  0.789149
  0.612372      -0.5  0.612372
euler: rx, ry, rz: from same rotation: 30 45 60
R generate by tf quaternion with the x-y-z order: 
  0.353553  0.612372 -0.707107
 -0.573223  0.739199  0.353553
  0.739199   0.28033  0.612372
euler: rx, ry, rz: 30 45 60

参考链接

https://www.cnblogs.com/calence/p/7479867.html
https://www.cnblogs.com/21207-iHome/p/6952004.html
学习笔记—四元数与欧拉角之间的转换-http://www.cppblog.com/heath/archive/2009/12/13/103127.html