Eigen/Geometry模块学习笔记

本文深入讲解了Eigen库的Geometry模块,涵盖了旋转向量、欧拉角、欧式变换和四元数的使用方法及相互转换。通过实例演示了如何在3D空间中进行旋转和平移操作,适用于机器人、计算机视觉等领域。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

Eigen/Geometry模块

1. 旋转向量:Eigen::AngleAxis

  1. 初始化
AngleAxisd rotation_vector(alpha, Vector3d(x, y, z));  //alpha:旋转角度,(x, y, z):旋转轴
  1. 转换为旋转矩阵
Matrix3d rotation_matrix;
rotation_matrix = rotation_vector.matrix();

rotation_matrix = rotation_vector.toRotationMatrix();
  1. 转换为欧拉角(需要先转换为旋转矩阵)
euler_angles = rotation_vector.matrix().eulerAngles(2, 1, 0);
  1. 转换为四元数
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. 欧拉角与旋转矩阵

举例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

  1. 定义
Isometry3d T = Isometry3d::Identity();  //T实际为4*4矩阵
T.rotate(rotation_vector);  //定义欧式变换的旋转矩阵
T.pretranslate(Vector3d(x, y, z));  //定义欧式变换的平移向量
  1. 转换为4*4变换矩阵
Matrix4d transform_matrix = T.matrix();
  1. 用欧式变换进行坐标变换
Vector3d v = (1, 2, 3);
Vector3d v_transformed = T * v;  //相当于R*v+t

4. 四元数:Eigen::Quaternion

  1. 初始化
    直接初始化:
Quaterniond q(w, x, y, z);

注意:初始化时实部在前,虚部在后,但在内部存储顺序为[x, y, z, w]

使用旋转向量和旋转矩阵初始化:

Quaterniond q1(rotation_vector);
Quaterniond q2(rotation_matrix);
  1. 打印四元数
cout << q.coeffs() << endl;

结果为[x, y, z, w]

  1. 使用四元数旋转向量
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,坐标点PPPF2F_2F2下的坐标PF2=R∗PF1P_{F_2}=R*P_{F_1}PF2=RPF1

测试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)转换为欧拉角时,如果

  1. 期望旋转轴顺序为Z -> Y -> X,则语法应为R.eulerAngles(0, 1, 2);,且结果中欧拉角索引[0], [1], [2]分别对应X, Y, Z轴的欧拉角;
  2. 期望旋转轴顺序为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
******************************************

结论:
旋转变换的逆变换,旋转轴顺序、旋转角度都要取反

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值