基于eigen实现轴角，旋转向量，欧拉角，四元数，旋转矩阵相互转换

小资12

已于 2023-08-15 13:26:58 修改

阅读量515

点赞数

文章标签：矩阵线性代数

于 2023-07-19 09:10:46 首次发布

本文链接：https://blog.csdn.net/weixin_44928759/article/details/131800532

版权

已在机器人上测试，没问题

本人使用qt编写，需要添加以下语句，Eigen库文件请自行去官网下载

pro文件添加：
#eigen函数库
INCLUDEPATH += $$PWD/eigen-3.4.0

头文件添加：
#include "Eigen/Dense"
typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

这里特别解释一下轴角和旋转向量的关系，转自四元数，旋转矩阵，轴角，欧拉角的相互转换（原理+Eigen代码实现）_eigen四元数转欧拉角_每天都在努力学习SLAM的小黑的博客-CSDN博客

轴角和旋转向量本质上是一个东西, 轴角用四个元素表达旋转, 其中的三个元素用来描述旋转轴, 另外一个元素描述旋转的角度,如下所示：

$r = [x,y,z,\theta ]$

其中单位向量 n=[x,y,z]对应的是旋转轴 θ 对应的是旋转角度。旋转向量与轴角相同, 只是旋转向量用三个元素来描述旋转, 它把 θ 角乘到了旋转轴上, 如下:

$r_{v} = [x*\theta ,y*\theta ,z*\theta]$

有的人们一般习惯把旋转向量也叫做轴角；

实现代码：

void AxisToRpy(double *pose)//旋转向量转欧拉角,传入位姿指针，弧度radian
{
    Eigen::Vector3d vec(pose[3], pose[4], pose[5]);
    Eigen::AngleAxisd rot_vec(vec.norm(), vec.normalized());//旋转向量转轴角

    Eigen::Matrix3d M = rot_vec.toRotationMatrix();//轴角转旋转矩阵

    Eigen::Vector3d R_ZYX = M.eulerAngles(2,1,0);//旋转矩阵转欧拉角Z-Y-X，注意顺序
    //重新赋值
    pose[3] = R_ZYX[2];
    pose[4] = R_ZYX[1];
    pose[5] = R_ZYX[0];

    std::cout << pose[3] << std::endl;
    std::cout << pose[4] << std::endl;
    std::cout << pose[5] << std::endl;
}

void RpyToAxis(double *pose)//欧拉角转轴角，传入位姿指针，弧度radian
{
    //欧拉角转四元数
    Eigen::AngleAxisd yawAngle(pose[5], Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pose[4], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(pose[3], Eigen::Vector3d::UnitX());
    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;
    //四元数转旋转向量
    Eigen::AngleAxisd v=Eigen::AngleAxisd(q);
    pose[3] = v.axis()[0]*v.angle();
    pose[4] = v.axis()[1]*v.angle();
    pose[5] = v.axis()[2]*v.angle();

    std::cout << pose[3] << std::endl;
    std::cout << pose[4] << std::endl;
    std::cout << pose[5] << std::endl;
}

Eigen::MatrixXd getT_fromPose(double pose[6])//位姿转齐次矩阵，欧拉角弧度radian
{
    Eigen::MatrixXd Rx(3, 3);
    Eigen::MatrixXd Ry(3, 3);
    Eigen::MatrixXd Rz(3, 3);

    Rx << 1, 0, 0, 0, cos(pose[3]), -sin(pose[3]), 0, sin(pose[3]), cos(pose[3]);
    Ry << cos(pose[4]), 0, sin(pose[4]), 0, 1, 0, -sin(pose[4]), 0, cos(pose[4]);
    Rz << cos(pose[5]), - sin(pose[5]), 0, sin(pose[5]), cos(pose[5]), 0, 0, 0, 1;

    Eigen::MatrixXd R(3, 3);
    R = Rz * Ry * Rx;//注意顺序

    Eigen::MatrixXd P(3, 1);
    P << pose[0], pose[1], pose[2];

    Eigen::MatrixXd T(4,4);
    T << R, P, Eigen::MatrixXd::Zero(1, 3), Eigen::MatrixXd::Identity(1,1);

    return T;
}

std::vector<double> getPose_fromT(Eigen::MatrixXd T)//齐次矩阵转位姿，欧拉角弧度radian
{
    double x = T(0, 3);
    double y = T(1, 3);
    double z = T(2, 3);
    double rx = atan2(T(2, 1), T(2, 2));
    double ry = asin(-T(2, 0));
    double rz = atan2(T(1, 0), T(0, 0));

    std::vector<double> pose;
    pose.push_back(x);
    pose.push_back(y);
    pose.push_back(z);
    pose.push_back(rx);
    pose.push_back(ry);
    pose.push_back(rz);

    return pose;
}

double* RpyToQuaternion(double *pose)//欧拉角位姿转四元数位姿,传入位姿指针，返回四元数位姿
{
    double quaternion[7] = {0};

    Eigen::AngleAxisd yawAngle(pose[5], Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pose[4], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(pose[3], Eigen::Vector3d::UnitX());
    Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle;

    quaternion[0] = pose[0];
    quaternion[1] = pose[1];
    quaternion[2] = pose[2];
    quaternion[3] = q.x();
    quaternion[4] = q.y();
    quaternion[5] = q.z();
    quaternion[6] = q.w();

    std::cout  << "," << q.x() << "," << q.y() << "," << q.z() << q.w() << std::endl;

    return quaternion;
}

double* QuaternionToRpy(double *quaternion)//四元数位姿转欧拉角位姿,传入位姿指针，返回四元数位姿
{
    double pose[6] = {0};

    Eigen::Quaternion<double> q;
    q.x() = quaternion[3];
    q.y() = quaternion[4];
    q.z() = quaternion[5];
    q.w() = quaternion[6];
    Eigen::Vector3d eulerAngle=q.matrix().eulerAngles(2,1,0);

    //重新赋值
    pose[0] = quaternion[0];
    pose[1] = quaternion[1];
    pose[2] = quaternion[2];
    pose[3] = eulerAngle[2];
    pose[4] = eulerAngle[1];
    pose[5] = eulerAngle[0];

    std::cout << pose[3] << std::endl;
    std::cout << pose[4] << std::endl;
    std::cout << pose[5] << std::endl;

    return pose;
}

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d& theta)
{
    Eigen::Matrix3d R_x;
    R_x <<
        1, 0, 0,
    0, cos(theta[0]), -sin(theta[0]),
        0, sin(theta[0]), cos(theta[0]);

    Eigen::Matrix3d R_y;
    R_y <<
        cos(theta[1]), 0, sin(theta[1]),
        0, 1, 0,
        -sin(theta[1]), 0, cos(theta[1]);

    Eigen::Matrix3d R_z;
    R_z <<
        cos(theta[2]), -sin(theta[2]), 0,
        sin(theta[2]), cos(theta[2]), 0,
        0, 0, 1;
    Eigen::Matrix3d R = R_z * R_y * R_x;
    return R;
}
bool isRotationMatrix(Eigen::Matrix3d R)
{
    double err = 1e-6;
    Eigen::Matrix3d shouldIdentity;
    shouldIdentity = R * R.transpose();
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    return (shouldIdentity - I).norm() < err;
}
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d& R)
{
    assert(isRotationMatrix(R));
    double sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular) {
        x = atan2(R(2, 1), R(2, 2));
        y = atan2(-R(2, 0), sy);
        z = atan2(R(1, 0), R(0, 0));
    }
    else
    {
        x = atan2(-R(1, 2), R(1, 1));
        y = atan2(-R(2, 0), sy);
        z = 0;
    }
    return { x,y,z };
}

void Widget::on_pushButton_2_clicked()
{
    // 初始化欧拉角{ -38.0669[°], -17.7130[°], -122.7096[°], "ZYX"}
//        Eigen::Vector3d euler_angle(deg_to_rad(-38.0669), deg_to_rad(-17.7130), deg_to_rad(-122.7096));
        Eigen::Vector3d euler_angle(deg_to_rad(-123.977), deg_to_rad(-22.915), deg_to_rad(35.4893));

        // 使用eigen库，欧拉角转旋转矩阵
        Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
        rotation_matrix1 =
            Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
        std::cout << "\nrotation matrix1 =\n" << rotation_matrix1 << std::endl << std::endl;

        // 使用自定义函数，欧拉角转旋转矩阵
        rotation_matrix2 = eulerAnglesToRotationMatrix(euler_angle);
        std::cout << "rotation matrix2 = \n" << rotation_matrix2 << std::endl << std::endl;

        // 使用eigen将旋转矩阵转换为欧拉角,欧拉角的实际范围是:roll:[−π,π];pitch:        
        //[−π/2,π/2];yaw:[−π,π],
        //Eigen库中欧拉角范围是：0:[0:π];1:[−π:π];2:[−π:π],所以eigen得到的结果不一样，
        //Eigen中确保第一个角度的转换,不建议使用eigen
        Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2, 1, 0); //zyx顺序
        std::cout << "roll_2 pitch_2 yaw_2 = " << rad_to_deg(eulerAngle1[0]) << " " << rad_to_deg(eulerAngle1[1])
            << " " << rad_to_deg(eulerAngle1[2]) << std::endl << std::endl;

        // 使用自定义函数将旋转矩阵转换为欧拉角
        Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1);
        std::cout << "roll_2 pitch_2 yaw_2 = " << rad_to_deg(eulerAngle2[0]) << " " << rad_to_deg(eulerAngle2[1])
            << " " << rad_to_deg(eulerAngle2[2]) << std::endl << std::endl;

}

调用实例：

背景： current_tcp_pose是机器人某个固定姿态工具坐标的位姿，tcp_pose是机器人当前位置的位姿，目的是求tcp_pose相对于current_tcp_pose的位姿

    double tcp_pose[6] = {0};
    double current_tcp_pose[6] = {0.640174, -0.320434, 0.355579, -0.524125, 0.533343, 1.391761};//轴角位姿

    Eigen::MatrixXd T;
    Eigen::MatrixXd T1;
    Eigen::MatrixXd T2;
    std::vector<double> pose;

    AxisToRpy(current_tcp_pose);//转换成欧拉角位姿
    T = getT_fromPose(current_tcp_pose);//转换成矩阵

        getTcpPos(tcp_pose);//获取当前位姿（轴角）
        AxisToRpy(tcp_pose);//转换成欧拉角位姿
        T1 = getT_fromPose(tcp_pose);//转换成矩阵

        T2 = T.inverse() * T1;//矩阵乘法
        pose = getPose_fromT(T2);矩阵转换成位姿（欧拉角）

小资12

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
基于eigen实现轴角，旋转向量，欧拉角，四元数，旋转矩阵相互转换

/旋转矩阵转欧拉角，注意顺序。void AxisToRpy(double *pose)//旋转向量转欧拉角,传入位姿指针。void RpyToAxis(double *pose)//欧拉角转轴角，传入位姿指针。//四元数转旋转向量。
复制链接

扫一扫