碼上快樂

相關內容    簡體    繁體

使用Eigen實現四元數、歐拉角、旋轉矩陣、旋轉向量之間的轉換 Eigen::Affine3f和Eigen::Matrix4f的轉換 以及float 和 double類型轉換

本文轉載自   查看原文   2021-02-04 16:54   1334 

@

目錄

  • Vector3.normalized的特點是當前向量是不改變的並且返回一個新的規范化的向量；
  • Vector3.Normalize的特點是改變當前向量，也就是當前向量長度是1

前言：國內復制粘貼太嚴重了，都不驗證一下，歐拉角部分全網都是2,1,0的排序但經過自己驗證0,1,2才是正確的（如有錯誤請指正）。

一、旋轉向量

1.1 初始化旋轉向量

旋轉角為alpha(順時針)，旋轉軸為(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

1.2 旋轉向量轉旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.3 旋轉向量轉歐拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

1.4 旋轉向量轉四元數

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;
Quaterniond quaternion;

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

二、旋轉矩陣

2.1 初始化旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2.2 旋轉矩陣轉旋轉向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.3 旋轉矩陣轉歐拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

2.4 旋轉矩陣轉四元數

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

三、歐拉角

3.1 初始化歐拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle(roll,pitch,yaw);

3.2 歐拉角轉旋轉向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3.3 歐拉角轉旋轉矩陣

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.4 歐拉角轉四元數

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

四、四元數

4.1 初始化四元數

Eigen::Quaterniond quaternion(w,x,y,z);

4.2 四元數轉旋轉向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.3 四元數轉旋轉矩陣

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

4.4 四元數轉歐拉角(xyz，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);

五、Eigen::Affine3f和Eigen::Matrix4f的轉換

Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;

六、float 和 double類型轉換

Eigen::MatrixXd matrix_d；
Eigen::MatrixXf matrix_f；
matrix_f = matrix_d.cast<float>();

繞固定坐標系轉和繞當前坐標系旋轉

//
// Created by qian on 2021/3/7.
//
#include "iostream"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
int main(int argc, char **argv){
    Vector3d t0(1,2,3);// 初始向量
    Vector3d t(3,2,1); // 平移向量
    const float angle_x=30.0, angle_y=20.0, angle_z=10.0;
    // *************繞XYZ為當前坐標系旋轉軸************
    AngleAxisd rotation_X=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());
    //固定坐標系的 Y 軸先投影成 X 旋轉后的旋轉軸,再進行旋轉
    AngleAxisd rotation_Y=AngleAxisd(angle_y/180.0*M_PI,rotation_X.inverse()*Vector3d::UnitY());
    //固定坐標系的 Z 軸先投影成 XY 旋轉后的旋轉軸,再進行旋轉
    AngleAxisd rotation_Z=AngleAxisd(angle_z/180.0*M_PI,rotation_Y.inverse()*rotation_X.inverse()*Vector3d::UnitZ());
    // 旋轉向量
    AngleAxisd rotation_vector1;
    rotation_vector1=rotation_X*rotation_Y*rotation_Z;
    cout<<"旋轉角："<<rotation_vector1.angle()*180.0/M_PI<<", 旋轉軸"<<rotation_vector1.axis().transpose()<<endl;
    // 旋轉矩陣
    Matrix3d rotation_matrix1(rotation_vector1);
    cout<<"旋轉矩陣：\n"<<rotation_matrix1<<endl;
    // 四元數
    Quaterniond q1(rotation_matrix1);
    cout<<"四元數：\n"<<q1.coeffs()<<endl;
    // 齊次歐式變換
    Isometry3d T=Isometry3d::Identity();
    T.rotate(rotation_vector1);
    T.pretranslate(t);
    cout<<"齊次歐式變換：\n"<<T.matrix()<<endl;
    cout<<"旋轉平移變換后的向量"<<T*t0<<endl;

    // **********繞XYZ為世界固定坐標系旋轉軸**********
    AngleAxisd rotation_X2=AngleAxisd(angle_x/180.0*M_PI,Vector3d::UnitX());
    AngleAxisd rotation_Y2=AngleAxisd(angle_y/180.0*M_PI,Vector3d::UnitY());
    AngleAxisd rotation_Z2=AngleAxisd(angle_z/180.0*M_PI,Vector3d::UnitZ());
    // 旋轉向量
    AngleAxisd rotation_vector2;
    rotation_vector2=rotation_Z2*rotation_Y2*rotation_X2;
    cout<<"旋轉角："<<rotation_vector2.angle()*180.0/M_PI<<", 旋轉軸"<<rotation_vector2.axis().transpose()<<endl;
    // 旋轉矩陣
    Matrix3d rotation_matrix2(rotation_vector2);
    cout<<"旋轉矩陣：\n"<<rotation_matrix2<<endl;
    // 四元數
    Quaterniond q2(rotation_matrix2);
    cout<<"四元數：\n"<<q2.coeffs()<<endl;
    // 齊次歐式變換
    Isometry3d T2=Isometry3d::Identity();
    T2.rotate(rotation_vector2);
    T2.pretranslate(t);
    cout<<"齊次歐式變換：\n"<<T2.matrix()<<endl;

    cout<<"旋轉平移變換后的向量"<<q2*t0+t<<endl;
    cout<<rotation_matrix1*rotation_matrix2.transpose()<<endl;
    return 0;
}


免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。



猜您在找 旋轉矩陣、旋轉向量（軸角）、四元數、歐拉角之間相互轉換的代碼實現(利用Eigen實現) Eigen 旋轉矩陣,旋轉向量,四元數和歐拉角(初始化和相互轉換) eigen 中四元數、歐拉角、旋轉矩陣、旋轉向量 python 歐拉角，旋轉矩陣，四元數之間轉換 旋轉矩陣、旋轉向量、歐拉角、四元數的關系 opencv與eigen類型轉換 一文梳理旋轉矩陣，歐拉角，四元數，旋轉向量和齊次變換矩陣之間的聯系和用途 歐拉角，旋轉矩陣，旋轉向量 3D 旋轉中 旋轉矩陣 歐拉角 四元數的相互轉換 Eigen::Matrix與array數據轉換
 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM