四元数转欧拉角常用函数

阿狸OKay

于 2024-04-23 11:05:43 发布

阅读量133

点赞数 1

文章标签：算法

本文链接：https://blog.csdn.net/weixin_50084948/article/details/138116769

版权

文章介绍了如何在C++中使用eigen库进行四元数到欧拉角（quat_to_rpy函数）和欧拉角到旋转矩阵（euler_to_rotation函数）的转换，涉及具体的数学计算和矩阵操作。

摘要由CSDN通过智能技术生成

使用了eigen库的格式

四元数转欧拉角

void quat_to_rpy(Quaterniond q, Matrix<double, 3, 1> &rpy)
{
    // from my MATLAB implementation
    // edge case!
    double as = t_min2(2. * (q.w() * q.y() - q.x() * q.z()), .99999);
    rpy(0) = atan2(2.f * (q.w() * q.x() + q.y() * q.z()),
                   1. - 2.f * (sq2(q.x()) + sq2(q.y())));
    rpy(1) = asin(as);
    rpy(2) = atan2(2.f * (q.w() * q.z() + q.x() * q.y()),
                   1. - 2.f * (sq2(q.y()) + sq2(q.z())));
}

欧拉角转四元数

Matrix<fpt, 3, 3> euler_to_rotation(fpt roll, fpt pitch, fpt yaw)
{
    fpt r = roll;
    fpt p = pitch;
    fpt y = yaw;
    // Calculate rotation matrix
    Matrix<fpt, 3, 3>  Rb;//Rx, Ry, Rz,
    // Rx << 1, 0, 0,
    //     0, cos(r), -sin(r),
    //     0, sin(r), cos(r);
    // Ry << cos(p), 0, sin(p),
    //     0, 1, 0,
    //     -sin(p), 0, cos(p);

    // Rb << cos(y), -sin(y), 0,
    //         sin(y), cos(y), 0,
    //         0, 0, 1;

    Rb << cos(y) * cos(p), -sin(y), 0,
        sin(y) * cos(p), cos(y), 0,
        -sin(p), 0, 1;

    

    // Rb <<  sin(p)*sin(y) , cos(y), 0,
    //     sin(p) * cos(y), -sin(y), 0,
    //     cos(p), 0, 1;
    // Rb <<  1 , 0 , -sin(p) ,
    //     0 ,  cos(r),sin(r) * cos(p),
    //     0, -sin(r), cos(p) * cos(r);

    Matrix<fpt, 3, 3> R =Rb.transpose();// Rb.inverse();
    return R;
}