eigen——旋转向量、旋转矩阵、四元数、欧拉角的初始化及相互转化

最新推荐文章于 2023-03-23 20:30:58 发布

wangdx_robot

最新推荐文章于 2023-03-23 20:30:58 发布

阅读量1k

点赞数 1

分类专栏： SLAM 文章标签： eigen

本文链接：https://blog.csdn.net/qq_40081208/article/details/102937581

版权

SLAM 专栏收录该内容

6 篇文章

订阅专栏

//
// Created by iqr on 2019/11/6.
//

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
#define sqr_2 sqrt(2)

int main(int argc, char **argv)
{
    double theta = M_PI / 4;
    Eigen::AngleAxisd rotation_vec(theta, Eigen::Vector3d(0, 0, 1));     // 旋转向量
    Eigen::Matrix3d rotation_mat;
    rotation_mat<< sqr_2/2, sqr_2/(-2), 0,
                   sqr_2/2, sqr_2/2,    0,
                   0,       0,          1;    // 旋转矩阵
    Eigen::Quaterniond rotation_quat(cos(theta/2), 0, 0, sin(theta/2));  // 四元数
    Eigen::Vector3d rotation_eulerAngle(theta, 0, 0);    // 欧拉角

    Eigen::AngleAxisd vec;  // 旋转向量
    Eigen::Matrix3d mat;    // 旋转矩阵
    Eigen::Quaterniond quat;// 四元数
    Eigen::Vector3d euler;  // 欧拉角
    // 旋转向量转其他
    printf(">>> 旋转向量转其他形式：\n");
    mat = rotation_vec.matrix();
    cout << "旋转矩阵=\n" << mat << endl;
    quat = rotation_vec;
    cout << "四元数: (x, y, z, w)=(" << quat.x() << "," << quat.y() << "," << quat.z() << "," << quat.w() << ")" << endl;
    euler = rotation_vec.matrix().eulerAngles(2,1,0);
    cout << "欧拉角: (roll, pitch, yaw)=" << euler << endl << endl;

    // 旋转矩阵转其他
    printf(">>> 旋转矩阵转其他形式：\n");
    vec = rotation_mat;
    cout << "旋转向量: axis=" << vec.axis() << "\nangle=" << vec.angle() << endl;
    quat = rotation_mat;
    cout << "四元数: (x, y, z, w)=(" << quat.x() << "," << quat.y() << "," << quat.z() << "," << quat.w() << ")" << endl;
    euler = rotation_mat.eulerAngles(2,1,0);
    cout << "欧拉角: (roll, pitch, yaw)=" << euler << endl << endl;

    // 四元数转其他
    printf(">>> 四元数转其他形式：\n");
    vec = rotation_quat;
    cout << "旋转向量: axis=" << vec.axis() << "\nangle=" << vec.angle() << endl;
    mat = rotation_quat.toRotationMatrix();
    cout << "旋转矩阵=\n" << mat << endl;
    euler = mat.eulerAngles(2,1,0);
    cout << "欧拉角: (yaw, pitch, roll)=" << euler << endl << endl;

    // 欧拉角转其他
    printf(">>> 欧拉角转其他形式：\n");
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rotation_eulerAngle(2),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rotation_eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rotation_eulerAngle(0),Eigen::Vector3d::UnitZ()));
    vec = yawAngle * pitchAngle * rollAngle;
    cout << "旋转向量: axis=" << vec.axis() << "\nangle=" << vec.angle() << endl;
    mat = yawAngle * pitchAngle * rollAngle;
    cout << "旋转矩阵=\n" << mat << endl;
    quat = yawAngle * pitchAngle * rollAngle;
    cout << "四元数: (x, y, z, w)=(" << quat.x() << "," << quat.y() << "," << quat.z() << "," << quat.w() << ")" << endl << endl;

    // 欧式变换矩阵
    printf(">>> 旋转向量+平移向量=欧式变换矩阵");
    Eigen::Vector3d translation(1, 2, 3);
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();    // 虽然称为3d，实质上是4＊4的矩阵 齐次坐标
    T.rotate (rotation_mat);                                // 按照rotation_vec进行旋转
    T.pretranslate (translation);     // 把平移向量设成(1,2,3)
    cout << "欧式变换矩阵=\n" << T.matrix() << endl << endl;

    // 欧式变换矩阵分解
    printf(">>> 欧式变换矩阵分解为：旋转向量+平移向量");
    mat = T.rotation();
    translation = T.translation();
    cout << "旋转矩阵=\n" << mat << endl;
    cout << "平移矩阵=\n" << translation << endl << endl;
}