Eigen庫使用教程之旋轉矩陣,旋轉向量和四元數的初始化和相互轉換的實現
阿新 • • 發佈:2019-01-03
本系列文章為原創,轉載請註明出處。
作者:Dongdong Bai
郵箱: [email protected]
若您覺得本博文對您有幫助,請您為我點贊並關注我,以鼓勵我寫出更優秀的博文。謝謝!
Eigen: C++開源矩陣計算工具——Eigen的簡單用法
Eigen庫是一個開源的C++線性代數庫,它提供了快速的有關矩陣的線性代數運算,還包括解方程等功能。Eigen是一個用純標頭檔案搭建起來的庫,這意味這你只要能找到它的標頭檔案,就能使用它。Eigen標頭檔案的預設位置是“/usr/include/eigen3”.
由於Eigen庫相較於OpenCV中的Mat等庫而言更加高效,許多上層的軟體庫也使用Eigen進行矩陣運算,比如SLAM中常用的g2o,Sophus等。此外Eigen庫還被被用於Caffe,Tensorflow等許多深度學習相關的框架中。
- 剛體運動中的旋轉通常可以由旋轉矩陣,旋轉向量和四元數等多種方式表示(具體的轉換公式請參見這篇部落格),在Eigen庫中也有其對應的實現。本文主要介紹剛體運動時旋轉矩陣,旋轉向量和四元數的初始化以及相互轉換在Eigen中的實現方式。
Eigen庫中各種形式的表示如下:
旋轉矩陣(3X3):Eigen::Matrix3d
旋轉向量(3X1):Eigen::AngleAxisd
四元數(4X1):Eigen::Quaterniond
平移向量(3X1):Eigen::Vector3d
變換矩陣(4X4):Eigen::Isometry3d
以下是具體的實現程式碼eigen_geometry.cpp:
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
//下面三個變數作為下面演示的中間變數
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));
Matrix3d t_R = t_V.matrix();
Quaterniond t_Q(t_V);
//對旋轉向量(軸角)賦值的三大種方法
//1.使用旋轉的角度和旋轉軸向量(此向量為單位向量)來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以(0,0,1)為旋轉軸,旋轉45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
//2.使用旋轉矩陣轉旋轉向量的方式
//2.1 使用旋轉向量的fromRotationMatrix()函式來對旋轉向量賦值(注意此方法為旋轉向量獨有,四元數沒有)
AngleAxisd V2;
V2.fromRotationMatrix(t_R);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;
//2.2 直接使用旋轉矩陣來對旋轉向量賦值
AngleAxisd V3;
V3 = t_R;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;
//2.3 使用旋轉矩陣來對旋轉向量進行初始化
AngleAxisd V4(t_R);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;
//3. 使用四元數來對旋轉向量進行賦值
//3.1 直接使用四元數來對旋轉向量賦值
AngleAxisd V5;
V5 = t_Q;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;
//3.2 使用四元數來對旋轉向量進行初始化
AngleAxisd V6(t_Q);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;
//------------------------------------------------------
//對四元數賦值的三大種方法(注意Eigen庫中的四元數前三維是虛部,最後一維是實部)
//1.使用旋轉的角度和旋轉軸向量(此向量為單位向量)來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以(0,0,1)為旋轉軸,旋轉45度
//第一種輸出四元數的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
//第二種輸出四元數的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
//2. 使用旋轉矩陣轉四元數的方式
//2.1 直接使用旋轉矩陣來對旋轉向量賦值
Quaterniond Q2;
Q2 = t_R;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;
//2.2 使用旋轉矩陣來對四元數進行初始化
Quaterniond Q3(t_R);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;
//3. 使用旋轉向量對四元數來進行賦值
//3.1 直接使用旋轉向量對四元數來賦值
Quaterniond Q4;
Q4 = t_V;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;
//3.2 使用旋轉向量來對四元數進行初始化
Quaterniond Q5(t_V);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;
//----------------------------------------------------
//對旋轉矩陣賦值的三大種方法
//1.使用旋轉矩陣的函式來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
//2. 使用旋轉向量轉旋轉矩陣來對旋轉矩陣賦值
//2.1 使用旋轉向量的成員函式matrix()來對旋轉矩陣賦值
Matrix3d R2;
R2 = t_V.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;
//2.2 使用旋轉向量的成員函式toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R3;
R3 = t_V.toRotationMatrix();
cout << "Rotation_matrix3" << endl << R3 << endl;
//3. 使用四元數轉旋轉矩陣來對旋轉矩陣賦值
//3.1 使用四元數的成員函式matrix()來對旋轉矩陣賦值
Matrix3d R4;
R4 = t_Q.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;
//3.2 使用四元數的成員函式toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R5;
R5 = t_Q.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << endl;
return 0;
}
上述程式碼對應的CMakeLists.txt為:
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
project(useGeometry)
include_directories("/usr/include/eigen3")
add_executable(eigen_geometry eigen_geometry.cpp)
- 旋轉矩陣(R),旋轉向量(V)和四元數(Q)在Eigen中轉換關係的總結:
旋轉矩陣(R),旋轉向量(V)和四元數(Q)分別通過自身初始化自己的方式,也就是第一分部分程式碼對旋轉矩陣(R),旋轉向量(V)和四元數(Q)賦值的第一種方式。
- R通過自身初始化的方法:
//1.使用旋轉矩陣的函式來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
- V通過自身初始化的方法:
//1.使用旋轉的角度和旋轉軸向量(此向量為單位向量)來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以(0,0,1)為旋轉軸,旋轉45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
- Q通過自身初始化的方法:
//1.使用旋轉的角度和旋轉軸向量(此向量為單位向量)來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以(0,0,1)為旋轉軸,旋轉45度
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
本系列文章為原創,轉載請註明出處。
作者:Dongdong Bai
郵箱: [email protected]
若您覺得本博文對您有幫助,請您為我點贊並關注我,以鼓勵我寫出更優秀的博文。謝謝!