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

三维旋转表示

在三维世界中的旋转的表示的方法主要有：
旋转只是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$可以说是角轴的表示了。

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

欧拉角到旋转矩阵
在航天领域多用左手(东北天)坐标系，在slam中多用右手坐标系(西北天-左前上)。
下为右手坐标系(西北天) 下的欧拉角旋转矩阵。与左手坐标系下的欧拉角旋转矩阵的关系：角度反号。

关于欧拉角旋转矩阵中sin的反号问题(上正下负和下正上负)：
当右手定则(握住旋转轴，大拇指为旋转轴方向，与四肢弯曲方向一致，则旋转为正)，若此时，轴的排序与角度正方向一致，则上正下负。反之，下正上负。

绕x轴旋转ψ角的旋转矩阵：
$$R_x\left(\alpha \right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & -sin\alpha \\ 0& sin\alpha & cos\alpha \end{array}\right]$$
绕y轴旋转θ角的旋转矩阵：
$$R_y\left(\beta \right)=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]$$
绕z轴旋转Φ角的旋转矩阵：
$$R_z\left(\gamma \right)=\left[\begin{array}{ccc}cos\gamma & -sin\gamma & 0\\ sin\gamma & cos\gamma & 0\\ 0& 0& 1\end{array}\right]$$
即使围绕每个轴都旋转一样的角度，先围绕哪个轴旋转会造成不同的结果。
假设旋转的次序分别是X-Y-Z(RPY)轴,最终得到的旋转矩阵是：
$$R=R_z\left(\gamma \right)R_y\left(\beta \right)R_x\left(\alpha \right)$$
$$=\left[\begin{array}{ccc}cos\beta cos\gamma & sin\alpha sin\beta cos\gamma -cos\alpha sin\gamma & cos\alpha sin\beta cos\gamma +sin\alpha sin\gamma \\ cos\beta sin\gamma & sin\alpha sin\beta sin\gamma +cos\alpha cos\gamma & cos\alpha sin\beta sin\gamma -sin\alpha cos\gamma \\ -sin\beta & sin\alpha cos\beta & cos\alpha cos\beta \end{array}\right]$$
Tips: 关于如何快速通过旋转矩阵来观察出它的旋转顺序的方法：
观察单项(只有一个sin)在矩阵中的位置及其对应的旋转角，如上个矩阵中的$-\sin \beta$，则中间轴为Y轴，其位于矩阵的(3,1)位置，则说明顺序是Rz*Ry*Rx。因此如果是左乘，其旋转顺序为X-Y-Z。

旋转矩阵到欧拉角
设旋转矩阵如下：
$$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=\alpha =atan2\left(r_{32},r_{33}\right)$$
$${\theta }_y=\beta =atan2\left({-r}_{31},\sqrt{{r^2}_{32}+{r^2}_{33}}\right)或{\theta }_y=\beta =-asin\left(r_{31}\right)$$
$${\theta }_z=\gamma =atan2\left(r_{21},r_{11}\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]，若角速度为 ω，忽略角轴旋转，那么旋转的时间导数即为：

四元数插值

四元数处于四维空间，但是投影在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的旋转。

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

默认坐标系测试

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

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