Eigen四元數和歐拉角相互轉化的有關函數詳細定義

視覺SLAM——第三章

Eigen幾何模塊Geometry使用 四元素 歐式變換矩陣

github鏈接 點擊打開鏈接

* 本程序演示了 Eigen 幾何模塊的使用方法

* 旋轉向量 Eigen::AngleAxisd 角度 軸 Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 軸旋轉 45 度

* 旋轉矩陣 Eigen::Matrix3d rotation_vector.toRotationMatrix(); //旋轉向量轉換到旋轉矩陣

* Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();// 直接轉

* 歐拉角 Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 );// ( 2,1,0 )表示ZYX順序，即roll pitch yaw順序 旋轉矩陣到 歐拉角轉換到歐拉角

* 四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 旋轉向量 定義四元素

* q = Eigen::Quaterniond ( rotation_matrix );//旋轉矩陣定義四元素

* 歐式變換矩陣 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣 旋轉 R+ 平移T

* T.rotate ( rotation_vector ); // 按照rotation_vector進行旋轉

* 也可 Eigen::Isometry3d T(q)　 // 一步 按四元素表示的旋轉 旋轉 轉換矩陣

* T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量設成(1,3,4)

*  輸出  cout<< T.matrix() <<endl;

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 幾何模塊
#include <Eigen/Geometry>

/****************************

• 本程序演示了 Eigen 幾何模塊的使用方法
  ****************************/

int main ( int argc, char** argv )
{
//注意一下類型名的最後一個字符爲d表示雙精度類型,換成f表示單精度類型，兩種類型不能混用，必須顯示轉換
// Eigen/Geometry 模塊提供了各種旋轉和平移的表示
// 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3f
/旋轉向量/
// 旋轉向量使用 AngleAxis, 它底層不直接是Matrix，但運算可以當作矩陣（因爲重載了運算符）
// 乘以該向量，表示進行一個座標變換
//任意旋轉可用一個旋轉軸和一個旋轉角度來表示。
//旋轉向量，旋轉向量的方向與旋轉軸一致，長度爲旋轉角度。
/*********************************/
/旋轉向量　沿 Z 軸旋轉 45 度 角度　軸 /
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 軸旋轉 45 度
cout .precision(3);
cout<<“rotation matrix =\n”<<rotation_vector.matrix() <<endl; //用matrix()轉換成矩陣
// 也可以直接賦值
/************************/
/旋轉矩陣/
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
rotation_matrix = rotation_vector.toRotationMatrix();//轉成旋轉矩陣　由羅德里格公式進行轉換
// 用 AngleAxis 可以進行座標變換
Eigen::Vector3d v ( 1,0,0 );
/旋轉向量進行座標變換/
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = “<<v_rotated.transpose()<<endl;
// 或者用旋轉矩陣
/*旋轉矩陣進行座標變換/
v_rotated = rotation_matrix * v;
cout<<”(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

/**歐拉角表示的旋轉**/
// 歐拉角: 可以將旋轉矩陣直接轉換成歐拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX順序，即roll pitch yaw順序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

/歐式變換矩陣表示旋轉/
// 歐氏變換矩陣使用 Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標
T.rotate ( rotation_vector ); // 按照rotation_vector進行旋轉
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量設成(1,3,4)
cout << “Transform matrix = \n” << T.matrix() <<endl;
// 用變換矩陣進行座標變換
Eigen::Vector3d v_transformed = Tv; // 相當於R*v+t
cout<<"(1,0,0) after Isometry3d tranformed = "<<v_transformed.transpose()<<endl;

// 對於仿射和射影變換，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

/*******四元數表示的旋轉***********/
// 可以直接把AngleAxis賦值給四元數，反之亦然 Quaterniond 表示雙精度　四元素　Quaternionf　表示單精度四元素
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 表示沿Z 軸旋轉 45 度　的四元素變換 
cout<<"quaternion from AngleAxis rotation_vector = \n"<<q.coeffs() <<endl;   // 請注意coeffs的順序是(x,y,z,w),w爲實部，前三者爲虛部
// 也可以把旋轉矩陣賦給它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion from rotation_matrix = \n"<<q.coeffs() <<endl;
// 使用四元數旋轉一個向量，使用重載的乘法即可
/*注意程序表達形式和實際運算的不一樣*/
v_rotated = q*v; // 注意數學上是q*v*q^{-1}  而程序爲了簡化表示　直接使用　q*v代替
cout<<"(1,0,0) after Quaterniond rotation = "<<v_rotated.transpose()<<endl;

/*編程題目
小蘿蔔１號位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]’　　　世界座標系到相機變換
小蘿蔔２號位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]’
小蘿蔔１號看到位於自身座標系下p=[0.5,0,0.2]’
求該向量在小蘿蔔２號下的座標
*/
Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz
//q1 << 0.35,0.2,0.3,0.1;
Eigen::Matrix<double, 3, 1> t1;//float類型
t1 << 0.3,0.1,0.1;
Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2);
//q2 << -0.5,0.4,-0.1,0.2;
Eigen::Matrix<double, 3, 1> t2;//float類型
t2 << -0.1,0.5,0.3;
Eigen::Matrix<double, 3, 1> p1;//float類型
p1 << 0.5,0,0.2;

cout<<“q1= \n”<< q1.coeffs() <<endl;
cout<<“t1= \n”<< t1 <<endl;
cout<<“q2= \n”<< q2.coeffs() <<endl;
cout<<“t2= \n”<< t2 <<endl;

/*
q1.setIdentity();
cout<<“q1 after setIdentity \n”<<q1.coeffs() <<endl;
q2.setIdentity();
cout<<“q2 after setIdentity \n”<<q2.coeffs() <<endl;
*/
//規範化　　歸一化 除以模長
q1=q1.normalized();
cout<<“q1 after normalized\n”<<q1.coeffs() <<endl;
q2=q2.normalized();
cout<<“q2 after normalized \n”<<q2.coeffs() <<endl;

Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
q1rotation_matrix=q1.toRotationMatrix();
Eigen::Isometry3d Tc1w=Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標

Tc1w.rotate (q1rotation_matrix ); // 按照q1rotation_matrix進行旋轉
Tc1w.pretranslate ( t1); // 把平移向量設成t1

//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆變換　　成　c1到world
Eigen::Matrix<double, 3, 1> pw=Tc1w.inverse()*p1; //將c1座標系下的點p1變換到world座標系下

Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
q2rotation_matrix=q2.toRotationMatrix();
Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標

Tc2w.rotate (q2rotation_matrix ); // 按照q1rotation_matrix進行旋轉
Tc2w.pretranslate ( t2); // 把平移向量設成t1

Eigen::Matrix<double, 3, 1> p2=Tc2w*pw; //將world座標系下的點pw變換到c2座標系下
cout<<“the loc of p1 in c1 = \n”<< p1<<endl;
cout<<“the loc of p1 in world = \n”<< pw<<endl;
cout<<“the loc of p1 in c2 = \n”<< p2<<endl;

return 0;
}