Eigen 欧拉角的说明，及四元数和旋转矩阵的变换
本文说明eulerAngles(0, 1, 2),和eulerAngles(2, 1, 0)的差异，并顺便将欧拉角、旋转矩阵、四元数一块的联系写了一下，也结合了一些有趣的博客内容。
1. 欧拉角旋转方向
不同的几何库对于旋转方向的正负号问题的定义不尽相同。这里主要验证下Eigen库旋转时，正负号判定的问题。如写简短测试程序：
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX());
Eigen::Vector3d input_point(0, 1, 0);
Eigen::Vector3d input_point_x(1, 0, 0);
Eigen::Vector3d output_point = R * input_point;
Eigen::Vector3d output_point_x = R * input_point_x;
std::cout << "output_point.x: " << output_point(0) << std::endl;
std::cout << "output_point.y: " << output_point(1) << std::endl;
std::cout << "output_point.z: " << output_point(2) << std::endl;
std::cout << "output_point_x.x: " << output_point_x(0) << std::endl;
std::cout << "output_point_x.y: " << output_point_x(1) << std::endl;
std::cout << "output_point_x.z: " << output_point_x(2) << std::endl;
if(output_point[2] > 0)
std::cout << "逆时针为正" << std::endl;
else
std::cout << "顺时针为正" << std::endl;output_point.x: 0
output_point.y: 0.707107
output_point.z: 0.707107
output_point_x.x: 1
output_point_x.y: 0
output_point_x.z: 0
逆时针为正
绕X轴旋转,形成的矩阵是逆时针为正的轴角.
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitX());
Eigen::Vector3d input_point(0, 1, 0);
Eigen::Vector3d input_point_x(1, 0, 0);
Eigen::Vector3d output_point = R * input_point;
R = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ());
Eigen::Vector3d output_point_x = R * input_point_x;
std::cout << "output_point.x: " << output_point(0) << std::endl;
std::cout << "output_point.y: " << output_point(1) << std::endl;
std::cout << "output_point.z: " << output_point(2) << std::endl;
std::cout << "output_point_x.x: " << output_point_x(0) << std::endl;
std::cout << "output_point_x.y: " << output_point_x(1) << std::endl;
std::cout << "output_point_x.z: " << output_point_x(2) << std::endl;
if(output_point[2] > 0)
std::cout << "逆时针为正" << std::endl;
else
std::cout << "顺时针为正" << std::endl;output_point.x: 0
output_point.y: 0.707107
output_point.z: 0.707107
output_point_x.x: 0.707107
output_point_x.y: 0.707107
output_point_x.z: 0
逆时针为正
2. eulerAngles函数参数说明
使用
Eigen::Matrix last_eulerAngle = base_change_rotation.eulerAngles(2,1,0);
Eigen::Matrix last_eulerAnglerpy = base_change_rotation.eulerAngles(0,1,2);
printf("roll = %f, pitch = %f, yaw = %f\n",\
last_eulerAngle[2] *180/M_PI , last_eulerAngle[1] *180/M_PI,last_eulerAngle[0]);
printf("roll = %f, pitch = %f, yaw = %f\n",\
last_eulerAngle[0] *180/M_PI , last_eulerAngle[1] *180/M_PI,last_eulerAngle[2]); roll = -0.002344, pitch = -0.005306, yaw = 0.000028
roll = 0.001619, pitch = -0.005306, yaw = -0.000041还有这样的:
roll = 180.000005, pitch = -180.000005, yaw =179.998781
roll = -0.000000, pitch = 0.000000, yaw =-0.001211
还能进行tf2的尝试：
//eulerAngles(2,1,0);
change roll = 179.973661, pitch = 179.993497, yaw = 179.982617
//eulerAngles(0,1,2);
rpy IMU change roll = 179.973661, pitch = 179.993497, yaw = 179.982617//tf2 getEulerYPR(Rz, Ry, Rx)
rpy IMU change roll = -0.026333, pitch = 0.006509, yaw =-0.017388####2
change roll = -0.018427, pitch = 0.002789, yaw = 0.011282
rpy change roll = 179.981568, pitch = 179.997227, yaw = -179.988718
tf2 rpy change roll = -0.018427, pitch = 0.002789, yaw =0.011282####3
change roll = 0.028579, pitch = -0.029884, yaw = 0.021761
rpy change roll = 0.028590, pitch = -0.029873, yaw = 0.021776
tf2 rpy change roll = 0.028579, pitch = -0.029884, yaw =0.021760###
IMU change roll = -179.953592, pitch = -179.971388, yaw = 179.984152
rpy IMU change roll = 0.046389, pitch = -0.028626, yaw = -0.015842
tf2 rpy IMU change roll = 0.046397, pitch = -0.028613, yaw =-0.015865#看下面，连续变化时eulerAngles(0,1,2)会出现接近180度时的正负号的变化，
eulerAngles(2,1,0)会出现 0.004586和179.979081变化
tf2的 tf2::Transform.getBasis().getEulerYPR(Rz, Ry, Rx)则更稳定
IMU change roll = 179.979062, pitch = 179.992545, yaw = 179.967832
rpy IMU change roll = 179.979081, pitch = 179.992525, yaw = 179.967832
tf2 rpy IMU change roll = -0.020938, pitch = 0.007450, yaw =-0.032174
lastest_NDT_time_ = 1662207100.606233, cloud_stamp = 1662207100.706121, time 99.887848
IMU change roll = -179.995420, pitch = -179.992389, yaw = 179.990194
rpy IMU change roll = 0.004586, pitch = -0.007603, yaw = -0.009804
tf2 rpy IMU change roll = 0.004588, pitch = -0.007602, yaw =-0.009807
可以看到，eulerAngles(0,1,2);和eulerAngles(2,1,0);都不太稳定，连续变化过程中，其获取的值会发生变化，选择使用tf2的欧拉角转换更佳。
测试一下
double x = M_PI / 6;
double y = M_PI / 4;
double z = M_PI / 3;
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ());
auto angle = R.eulerAngles(0, 1, 2) * 180 / M_PI;
auto angleypr = R.eulerAngles(2, 1, 0) * 180 / M_PI;
std::cout << "X = " << angle(0) << std::endl;
std::cout << "Y = " << angle(1) << std::endl;
std::cout << "Z = " << angle(2) << std::endl;
std::cout << "yaw = " << angleypr(0) << std::endl;
std::cout << "pitch = " << angleypr(1) << std::endl;
std::cout << "roll = " << angleypr(2) << std::endl;
if ((std::abs(angle(0) - 30) < 0.001) &&
(std::abs(angle(1) - 45) < 0.001) &&
(std::abs(angle(2) - 60) < 0.001))
{
std::cout << "xyz" << std::endl;
}输出结果:
X = 30
Y = 45
Z = 60
yaw = 69.1188
pitch = -7.28625
roll = 51.8766
xyz
可以看到,一个旋转矩阵R通过eulerAngles(0, 1, 2),和eulerAngles(2, 1, 0)是不一样的;
double x = M_PI / 6;
double y = M_PI / 4;
double z = M_PI / 3;
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ());
auto angle = R.eulerAngles(0, 1, 2) * 180 / M_PI;
auto angleypr = R.eulerAngles(2, 1, 0) * 180 / M_PI;
std::cout << "X = " << angle(0) << std::endl;
std::cout << "Y = " << angle(1) << std::endl;
std::cout << "Z = " << angle(2) << std::endl;
std::cout << "yaw = " << angleypr(0) << std::endl;
std::cout << "pitch = " << angleypr(1) << std::endl;
std::cout << "roll = " << angleypr(2) << std::endl;
if ((std::abs(angle(0) - 30) < 0.001) &&
(std::abs(angle(1) - 45) < 0.001) &&
(std::abs(angle(2) - 60) < 0.001))
{
std::cout << "xyz" << std::endl;
}
std::cout << "R = " << R <这里你会好奇,为什么pitch角也是不一样的?
因为欧拉角处理顺序表示不一样,其意义也不一样.
总结:
第一种：
R = rx* ry * rz;
R.eulerAngles(0, 1, 2) 的存储方式也是按照 [rx, ry,rz]存放的
这种坐标变换描述是，先绕着自身坐标系X轴旋转rx，再绕着自身Y轴旋转ry， 再绕着自身Z轴旋转rz，可以认为是X-Y-Z欧拉角。
R = yaw * pitch * roll;右乘的方式实现的
R.eulerAngles(2, 1, 0) 的存储方式也是按照 [yaw, pitch, roll]存放的.
这种坐标变换描述是，先绕着自身坐标系Z轴旋转yaw，再绕着自身Y轴旋转pitch， 再绕着自身X轴旋转roll，可以认为是Z-Y-X欧拉角。
欧拉角和RPY角
第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将B绕A的X轴旋转γ角，再绕A的Y轴旋转β角，最后绕A的Z轴旋转α角，完成旋转。整个过程，A不动，B动。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
由于是绕固定坐标系旋转，则旋转矩阵为：
R = Rz * Ry *Rx，乘法顺序：从右向左，左乘，先转的轴放在右边，后转的轴矩阵放在左边。
另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转α，然后绕旋转后的Y轴旋转β，最后绕旋转后的X轴旋转γ，就能旋转到当前姿态。
由于是绕动坐标系旋转，则旋转矩阵为：
R = Rz * Ry *Rx，即绕动轴坐标系旋转是右乘，先转的轴放在左边，后转的轴是放在右边
可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)和绕自身动坐标轴Z-Y-X旋转(α,β,γ)的最终结果一样，只是描述的方法有差别而已。
单位四元数与旋转矩阵：
旋转轴的单位方向向量为K,则有：
四元数转换为旋转矩阵：
对应的四元数为：
这样求解的角度就在[-PI,PI] .
注意欧拉角有24种，分成绕动轴和绕固定轴。
第一种是按照旋转的轴的顺序，绕动轴的表示一共12种。
三个轴只用两个的：Proper Euler angles (z−x−z, x−y−x, y−z−y, z−y−z, x−z−x, y−x−y）
三个轴全都用的：Tait-Bryan angles (x−y−z, y−z−x, z−x−y, x−z−y, z−y−x, y−x−z）
而上面推导的公式是按照Z-Y-X顺序进行的。
Axis-Angle与四元数的代码实现
绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。
初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)，可以看到前面绕标准轴的旋转就是轴角+对应方向单位向量。
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
旋转向量转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
旋转向量转四元数，通过拷贝构造和赋值构造函数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;quaternion=rotation_vector;
欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix=yawAngle*pitchAngle*rollAngle;
其他从旋转矩阵转欧拉角的程序：
#include
double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
//create ros msg
tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw);
static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr)
{
typedef typename Derived::Scalar Scalar_t;
Scalar_t y = ypr(0) / 180.0 * M_PI;
Scalar_t p = ypr(1) / 180.0 * M_PI;
Scalar_t r = ypr(2) / 180.0 * M_PI;
Eigen::Matrix Rz;
Rz << cos(y), -sin(y), 0,
sin(y), cos(y), 0,
0, 0, 1;
Eigen::Matrix Ry;
Ry << cos(p), 0., sin(p),
0., 1., 0.,
-sin(p), 0., cos(p);
Eigen::Matrix Rx;
Rx << 1., 0., 0.,
0., cos(r), -sin(r),
0., sin(r), cos(r);
return Rz * Ry * Rx;
}
旋转矩阵->欧拉角
static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
{
Eigen::Vector3d n = R.col(0);
Eigen::Vector3d o = R.col(1);
Eigen::Vector3d a = R.col(2);
Eigen::Vector3d ypr(3);
double y = atan2(n(1), n(0));
double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
ypr(0) = y;
ypr(1) = p;
ypr(2) = r;
return ypr / M_PI * 180.0;
}
旋转矩阵->四元数
Eigen::Quaterniond q = Eigen::Quaterniond(R);
q.normalize();
经典力学中使用zxz，量子力学使用的是zyz，航空航天使用zyx/zxy。所以在跨行业或者跨模块协作的时候，一定要问清楚对方是哪一种欧拉角。
不同的转序和不同的轴对应的万向节死锁的位置是不一样的，因此每个专业都想把万向节死锁的位置安排在自己最不常用的位置。但是，无论哪一种欧拉角都是避免不了万向节死锁的。
比如航空领域中，滚转俯仰偏航角有明确的物理意义。如果对导航的定义是是北(x)东(y)地(z)，那这种情况下，飞机本身的坐标系定义就必须是：前(x)右(y)下(z)，转序就是zyx(321)。如果对导航的定义是是东(x)北(y)天(z)， 那这种情况下，飞机本身的坐标系定义就必须是：右(x)前(y)上(z).转序就是zxy(312)。
欧拉角的优缺点：
优点：
1 简单，三个数就能表示出来
2 直观，用手就能比划出姿态
缺点：
1 万向节死锁
2 没办法进行旋转的叠加，必须借助旋转矩阵。这就是很少看见用欧拉角相加减原因。
3 插值误差大，因为是三个轴进行的旋转。但是四元数就十分适合插值。
4 欧拉角的种类太多，不同专业背景之间的沟通容易出问题。
综上，欧拉角在运动变换的时候很少用来直接参与计算，通常采用旋转矩阵(通过李群)或者四元数来进行表示。近年来，四元数的使用非常简便，广泛用于SLAM等领域，近年来的李群和李代数的应用作为类似于四元数的刚体运动操作方法让旋转的扰动更便于计算与应用。接下来会对李群和李代数进行部分说明。欧拉角的计算不好使，通常用于显示。