Ros::tf中的欧拉角和四元数转换

代码如下，这两个转换坑了我好多次，这次给好好记录下来我写的代码，主要是用ros才可以满足我的需求，用Eigen不能满足我的需求，并且结果还转不回去，用tf实现了我的需求，已经验证过结论，下面代码取用tf。
注意：是部分代码片段，需要的同学可以自己拿去剪辑

#include <math.h>
#include<ros/ros.h>
#include<tf/tf.h>
#define RAD2DEG(x) ((x)*180./M_PI)  //弧度转角度 
#define DEG2RAD(x) ((x)*M_PI/180.)  //角度转弧度

//注意：是部分代码片段，需要的同学可以自己拿去剪辑
// tf 欧拉角转四元数
tf::Quaternion tfInsQuaternion;;/*定义四元数*/
tfInsQuaternion.setRPY(DEG2RAD(curInsRpy[0]), DEG2RAD(curInsRpy[1]), DEG2RAD( 360 - curInsRpy[2]));
tfInsQuaternion = tfInsQuaternion.normalize();
// tf 四元数转到欧拉角
double roll1, pitch1, yaw1;
tf::Matrix3x3(tfInsQuaternion).getRPY(roll1, pitch1, yaw1);
Eigen::Vector3d Euler_ZYX1 = Eigen::Vector3d(yaw1,pitch1,roll1) * 180 / M_PI;
std::cout << "INS yaw1(z) pitch1(y) roll1(x) = " << Euler_ZYX1.transpose() << std::endl;        
double roll2, pitch2, yaw2;
tf::Matrix3x3(tfInsQuaternion).getEulerYPR(yaw2,pitch2,roll2);
Eigen::Vector3d Euler_ZYX2 = Eigen::Vector3d(yaw2,pitch2,roll2) * 180 / M_PI;
std::cout << "INS yaw2(z) pitch2(y) roll2(x) = " << Euler_ZYX2.transpose() << std::endl;

// Eigen 欧拉角转四元数
// Eigen::AngleAxisd rollAngle(DEG2RAD(curInsRpy[0]), Eigen::Vector3d::UnitX());
// Eigen::AngleAxisd pitchAngle(DEG2RAD(curInsRpy[1]), Eigen::Vector3d::UnitY());
// Eigen::AngleAxisd yawAngle(DEG2RAD( 360 - curInsRpy[2]), Eigen::Vector3d::UnitZ()); //
// Eigen::Quaterniond tfInsQuaternion = yawAngle * pitchAngle * rollAngle;

// Eigen 四元数转欧拉角(Z-Y-X，即RPY)
// Eigen::Vector3d Euler_ZYX = Eigen::EulerAngles<double, Eigen::EulerSystemZYX>(tfInsQuaternion.normalized().toRotationMatrix()).angles();
// std::cout << "INS yaw(z) pitch(y) roll(x) = " << Euler_ZYX.transpose() * 180. / M_PI << std::endl;
// Eigen::Vector3d gpsEulerAngle = tfInsQuaternion.matrix().eulerAngles(2,1,0);
// std::cout << "INS yaw(z) pitch(y) roll(x) = " << gpsEulerAngle.transpose() * 180. / M_PI << std::endl;

验证的结果：

INS: 256.920 0.490 -2.510 
INS yaw1(z) pitch1(y) roll1(x) = -103.080    0.490   -2.500
INS yaw2(z) pitch2(y) roll2(x) = -103.080    0.490   -2.500

上面的航向角我再解释一下，航向角的定义是绕z轴旋转，这里不需要区分x或者y轴为定义的起始轴，因为二者都一样，逆时针方向为正方向，可以记录为0 - 2π，或者0 - π、0 -（-π）。二者均可以在上面代码使用。
对于后者0 - π、0 -（-π），转到0 - 2π°。分两种情况，如果是0 -（-π），直接加上2π；如果是0 - π，直接使用即可。