【Eigen】旋转矩阵与四元数互转--简单明了！

最新推荐文章于 2024-12-02 20:50:38 发布

向日葵xyz

最新推荐文章于 2024-12-02 20:50:38 发布

阅读量171

点赞数 1

文章标签：矩阵线性代数

本文链接：https://blog.youkuaiyun.com/hgaohr1021/article/details/143942213

版权

请添加图片描述

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>

Eigen::Quaternionf rotationMatrixToQuaternion(const Eigen::Matrix3f& R) {
    Eigen::Quaternionf q(R); // 使用Eigen库的直接转换功能
    q.normalize();  // 确保结果四元数是标准化的
    return q;
}


Eigen::Matrix3f quaternionToRotationMatrix(const Eigen::Quaternionf& q) {
    Eigen::Matrix3f R = q.toRotationMatrix();
    return R;
}

int main() {
    Eigen::Matrix3f R;
    R << 0.225449, -0.929, -0.2932,
        0.96947, 0.24372, -0.0268296,
        0.0963978, -0.278249, 0.9556594;

    Eigen::Quaternionf quaternion = rotationMatrixToQuaternion(R);


    std::cout << " 将旋转矩阵--转---四元数 " << std::endl;
    std::cout << "Quaternion:" << std::endl;
    std::cout << "w: " << quaternion.w() << std::endl;
    std::cout << "x: " << quaternion.x() << std::endl;
    std::cout << "y: " << quaternion.y() << std::endl;
    std::cout << "z: " << quaternion.z() << std::endl;




    std::cout << " 将四元数转--换为---旋转矩阵 " << std::endl;

    // 将四元数转换为旋转矩阵
    Eigen::Matrix3f rotationMatrix = quaternionToRotationMatrix(quaternion);

    std::cout << "Rotation Matrix:\n" << rotationMatrix << std::endl;



    return 0;
}