Eigen/Geometry模块
1. 旋转向量:Eigen::AngleAxis
- 初始化
AngleAxisd rotation_vector(alpha, Vector3d(x, y, z)); //alpha:旋转角度,(x, y, z):旋转轴
- 转换为旋转矩阵
Matrix3d rotation_matrix;
rotation_matrix = rotation_vector.matrix();
或
rotation_matrix = rotation_vector.toRotationMatrix();
- 转换为欧拉角(需要先转换为旋转矩阵)
euler_angles = rotation_vector.matrix().eulerAngles(2, 1, 0);
- 转换为四元数
Quaternion q = Quaterniond(rotation_vector);
2. 欧拉角:Eigen::eulerAngles(a0, a1, a2)
用法:a0, a1, a2从{0, 1, 2}中取值,0代表X轴,1代表Y轴,2代表Z轴,旋转顺序为从后到前(下一次旋转使用的旋转轴是上一次旋转之后的)。
得到的Euler Angle角度值范围:[0, pi] x [-pi, pi] x [-pi, pi]
- 欧拉角与旋转矩阵
举例1:
Vector3f euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
等价于
rotation_matrix = AngleAxisd(euler_angles[0], Vector3f::UnitZ())
* AngleAxisd(euler_angles[1], Vector3f::UnitY())
* AngleAxisd(euler_angles[2], Vector3f::UnitX());
均表示先绕X轴旋转euler_angles[2] rad,再绕Y轴旋转euler_angles[1] rad,再绕Z轴旋转euler_angles[0] rad。
举例2:
Vector3f euler_angles = rotation_matrix.eulerAngles(0, 1, 2);
等价于
rotation_matrix = AngleAxisd(euler_angles[0], Vector3f::UnitX())
* AngleAxisd(euler_angles[1], Vector3f::UnitY())
* AngleAxisd(euler_angles[2], Vector3f::UnitZ());
均表示先绕Z轴旋转euler_angles[2] rad,再绕Y轴旋转euler_angles[1] rad,再绕X轴旋转euler_angles[0] rad。
坐标系:X轴朝前, Y轴朝左, Z轴朝上
*注意:XYZ轴并不一定总是对应roll, pitch, yaw,要根据实际情况判断。roll表示横滚,pitch表示俯仰,yaw表示偏航。
3. 欧式变换:Eigen::Isometry
- 定义
Isometry3d T = Isometry3d::Identity(); //T实际为4*4矩阵
T.rotate(rotation_vector); //定义欧式变换的旋转矩阵
T.pretranslate(Vector3d(x, y, z)); //定义欧式变换的平移向量
- 转换为4*4变换矩阵
Matrix4d transform_matrix = T.matrix();
- 用欧式变换进行坐标变换
Vector3d v = (1, 2, 3);
Vector3d v_transformed = T * v; //相当于R*v+t
4. 四元数:Eigen::Quaternion
- 初始化
直接初始化:
Quaterniond q(w, x, y, z);
注意:初始化时实部在前,虚部在后,但在内部存储顺序为[x, y, z, w]
使用旋转向量和旋转矩阵初始化:
Quaterniond q1(rotation_vector);
Quaterniond q2(rotation_matrix);
- 打印四元数
cout << q.coeffs() << endl;
结果为[x, y, z, w]
- 使用四元数旋转向量
Vector3d v_rotated = q * v;
注意:内部是q*v*q^-1
借助ros tf功能包生成四元数
#include <tf/tf.h>
tf::createQuaternionFromRPY(euler_angles[2], euler_angles[1], euler_angles[0]);
注意,虽然代码中三个欧拉角的顺序是roll, pitch, yaw,但旋转时还是先绕Z轴旋转yaw,再绕Y轴旋转pitch,再绕X轴旋转roll,且下一次旋转使用的旋转轴是上一次旋转之后的
测试
测试时使用的Eigen版本:3.3.7
查看Eigen版本:/usr/include/eigen3/Eigen/src/Core/util/Macros.h
测试1
测试代码:
cout << "******************************************" << endl;
Eigen::Matrix3d R1;
R1 = Eigen::AngleAxisd(30*torad, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(45*torad, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(60*torad, Eigen::Vector3d::UnitZ());
cout << "R1:\n" << R1 << endl;
Eigen::Matrix3d R_x;
Eigen::Matrix3d R_y;
Eigen::Matrix3d R_z;
R_x << 1, 0, 0,
0, cos(30*torad), -sin(30*torad),
0, sin(30*torad), cos(30*torad);
R_y << cos(45*torad), 0, sin(45*torad),
0, 1, 0,
-sin(45*torad), 0, cos(45*torad);
R_z << cos(60*torad), -sin(60*torad), 0,
sin(60*torad), cos(60*torad), 0,
0, 0, 1;
cout << "R_x * R_y * R_z:\n" << R_x * R_y * R_z << endl;
cout << "******************************************" << endl;
运行结果:
******************************************
R1:
0.353553 -0.612372 0.707107
0.926777 0.126826 -0.353553
0.126826 0.78033 0.612372
R_x * R_y * R_z:
0.353553 -0.612372 0.707107
0.926777 0.126826 -0.353553
0.126826 0.78033 0.612372
******************************************
结论:
利用yaw, pitch, roll生成旋转矩阵的代码
注意,此处的旋转矩阵公式的使用方法:当坐标系F1F_1F1绕旋转轴顺时针旋转θ\thetaθ(从旋转轴的正方向一侧看,θ>0\theta>0θ>0)到坐标系F2F_2F2,坐标点PPP在F2F_2F2下的坐标PF2=R∗PF1P_{F_2}=R*P_{F_1}PF2=R∗PF1
测试2
测试代码:
cout << "******************************************" << endl;
Eigen::Matrix3d R1;
R1 = Eigen::AngleAxisd(30*torad, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(45*torad, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(60*torad, Eigen::Vector3d::UnitZ());
cout << "R1:\n" << R1 << endl;
Eigen::Vector3d euler_angle_1 = R1.eulerAngles(0, 1, 2);
Eigen::Vector3d euler_angle_2 = R1.eulerAngles(2, 1, 0);
cout << "euler_angle_1:\n" << euler_angle_1 << endl;
cout << "euler_angle_1(degree):\n" << euler_angle_1/torad << endl;
cout << "euler_angle_2:\n" << euler_angle_2 << endl;
cout << "euler_angle_2(degree):\n" << euler_angle_2/torad << endl;
Eigen::Matrix3d R_calculate1;
R_calculate1 = Eigen::AngleAxisd(euler_angle_1[0], Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(euler_angle_1[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle_1[2], Eigen::Vector3d::UnitZ());
cout << "R_calculate1:\n" << R_calculate1 << endl;
Eigen::Matrix3d R2;
R2 = Eigen::AngleAxisd(30*torad, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(45*torad, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(60*torad, Eigen::Vector3d::UnitX());
cout << "R2:\n" << R2 << endl;
euler_angle_1 = R2.eulerAngles(0, 1, 2);
euler_angle_2 = R2.eulerAngles(2, 1, 0);
cout << "euler_angle_1:\n" << euler_angle_1 << endl;
cout << "euler_angle_1(degree):\n" << euler_angle_1/torad << endl;
cout << "euler_angle_2:\n" << euler_angle_2 << endl;
cout << "euler_angle_2(degree):\n" << euler_angle_2/torad << endl;
Eigen::Matrix3d R_calculate2;
R_calculate2 = Eigen::AngleAxisd(euler_angle_2[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle_2[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle_2[2], Eigen::Vector3d::UnitX());
cout << "R_calculate2:\n" << R_calculate2 << endl;
cout << "******************************************" << endl;
运行结果:
******************************************
R1:
0.353553 -0.612372 0.707107
0.926777 0.126826 -0.353553
0.126826 0.78033 0.612372
euler_angle_1:
0.523599
0.785398
1.0472
euler_angle_1(degree):
30
45
60
euler_angle_2:
1.20635
-0.127169
0.905417
euler_angle_2(degree):
69.1188
-7.28625
51.8766
R_calculate1:
0.353553 -0.612372 0.707107
0.926777 0.126826 -0.353553
0.126826 0.78033 0.612372
R2:
0.612372 0.28033 0.739199
0.353553 0.739199 -0.573223
-0.707107 0.612372 0.353553
euler_angle_1:
1.01813
0.83188
-0.429303
euler_angle_1(degree):
58.3345
47.6632
-24.5972
euler_angle_2:
0.523599
0.785398
1.0472
euler_angle_2(degree):
30
45
60
R_calculate2:
0.612372 0.28033 0.739199
0.353553 0.739199 -0.573223
-0.707107 0.612372 0.353553
******************************************
结论:
在将旋转矩阵(Eigen::Matrix3d
)转换为欧拉角时,如果
- 期望旋转轴顺序为Z -> Y -> X,则语法应为
R.eulerAngles(0, 1, 2);
,且结果中欧拉角索引[0], [1], [2]
分别对应X, Y, Z轴的欧拉角; - 期望旋转轴顺序为X -> Y -> Z,则语法应为
R.eulerAngles(2, 1, 0);
,且结果中欧拉角索引[0], [1], [2]
分别对应Z, Y, X轴的欧拉角;
测试3
测试代码:
cout << "******************************************" << endl;
Eigen::Matrix3d R1;
R1 = Eigen::AngleAxisd(30*torad, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(45*torad, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(60*torad, Eigen::Vector3d::UnitZ());
cout << "R1:\n" << R1 << endl;
cout << "R1.transpose():\n" << R1.transpose() << endl;
Eigen::Matrix3d R1_transpose_calculate;
R1_transpose_calculate = Eigen::AngleAxisd(-60*torad, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(-45*torad, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(-30*torad, Eigen::Vector3d::UnitX());
cout << "R1_transpose_calculate:\n" << R1_transpose_calculate << endl;
cout << "******************************************" << endl;
运行结果:
******************************************
R1:
0.353553 -0.612372 0.707107
0.926777 0.126826 -0.353553
0.126826 0.78033 0.612372
R1.transpose():
0.353553 0.926777 0.126826
-0.612372 0.126826 0.78033
0.707107 -0.353553 0.612372
R1_transpose_calculate:
0.353553 0.926777 0.126826
-0.612372 0.126826 0.78033
0.707107 -0.353553 0.612372
******************************************
结论:
旋转变换的逆变换,旋转轴顺序、旋转角度都要取反