三維空間剛體運(yùn)動(dòng)瘟栖，筆記內(nèi)容有向量?jī)?nèi)積葵擎，向量外積胎撇，歐氏變換甘桑，旋轉(zhuǎn)向量，歐拉角弄诲，旋轉(zhuǎn)矩陣寓涨，四元數(shù)以及它們的轉(zhuǎn)換關(guān)系盯串，代碼是Eigen庫(kù)的基本使用。

筆記1.jpg

筆記2.jpg

代碼如下：

#include <iostream>
#include <ctime>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry> 
using namespace std;
#define MATRIX_SIZE 50

int main(int argc, char** argv)
{
    ///Eigen 基本操作
    Eigen::Matrix<float,2,3> matrix_23 ;
    Eigen::Vector3d v_3d; //實(shí)質(zhì)是Eigen::Matrix<double,3,1>
    matrix_23 << 1,2,3,4,5,6; //賦值操作
    v_3d << 3,2,1;
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout<< result <<endl;
    
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Random();
    cout<< "matrix_33 is :" << matrix_33 <<endl;
    
    cout<< "matrix_33 transpose is :" << matrix_33.transpose() <<endl; //轉(zhuǎn)置
    cout<< "matrix_33 sum is :" << matrix_33.sum() <<endl; //各元素的和
    cout<< "matrix_33 trace is :"<< matrix_33.trace() <<endl; //矩陣的跡
    cout<< "matrix_33 inverse is :"<< matrix_33.inverse() <<endl; //逆
    cout<< "matrix_33 determinant is ::"<< matrix_33.determinant() <<endl; //行列式
    
    //特征值和特征向量
    
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33.transpose() * matrix_33);//實(shí)對(duì)稱矩陣
    cout<< "Eigen values = "<< eigen_solver.eigenvalues() << endl; //特征值
    cout<< "Eigen vectors = "<< eigen_solver.eigenvectors() << endl; //特征向量
    
    //解方程戒良，對(duì)比求逆和矩陣分解的速度
    
    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix < double ,MATRIX_SIZE,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
    //求逆
    clock_t time_stt = clock();
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
    cout<< " inverse time use is :"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" <<endl;
    
    //QR分解
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<< "Qr time use is :"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" <<endl;

    ///Eigen 中四元數(shù)体捏，歐拉角，旋轉(zhuǎn)矩陣糯崎，旋轉(zhuǎn)向量的轉(zhuǎn)換
    
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
    cout.precision(3);
    rotation_matrix = rotation_vector.toRotationMatrix(); //旋轉(zhuǎn)向量可以轉(zhuǎn)換為旋轉(zhuǎn)矩陣
    cout<<"rotation_matrix is :"<<rotation_matrix<<endl;
    
    ///AngleAxis 旋轉(zhuǎn)向量進(jìn)行坐標(biāo)變換
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotated:"<<v_rotated.transpose()<<endl;
    
    /// 旋轉(zhuǎn)矩陣進(jìn)行坐標(biāo)變換
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose();
    
    /// 歐拉角： 可以直接將旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角
    
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); //(2,1,0)表示ZYX 的旋轉(zhuǎn)順序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
    
    ///四元數(shù)  可以將旋轉(zhuǎn)向量轉(zhuǎn)換為四元數(shù)
    Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
    cout<<"Quaterniond = \n"<<q.coeffs()<<endl;//coeffs 表示順序是(x,y,z,w),w為實(shí)部几缭，前三者為虛部。
    
    /// 也可以將旋轉(zhuǎn)矩陣賦予給四元數(shù)
    q = Eigen::Quaterniond(rotation_matrix);
    cout<<"Quaterniond = \n"<<q.coeffs()<<endl;
    ///四元數(shù)旋轉(zhuǎn)一個(gè)矩陣
    v_rotated = q*v; //注意數(shù)學(xué)形式為 qvq^{-1}
    cout<<"(1,0,0) after rotated :"<<v_rotated.transpose()<<endl;
    
    ///使用歐氏變換
    
    Eigen::Isometry3d T_o = Eigen::Isometry3d::Identity();
    T_o.rotate (rotation_vector);
    T_o.pretranslate(Eigen::Vector3d(1,3,4));
    cout<< "Transform matrix = \n"<< T_o.matrix()<<endl;
    Eigen::Vector3d v_transformed = T_o*v;
    cout<<"v transformed :"<<v_transformed.transpose()<<endl;
    
    ///仿射變換
    Eigen::Affine3d T_a = Eigen::Affine3d::Identity();
    T_a.rotate (rotation_vector);
    T_a.prescale(0.5);
    T_a.pretranslate(Eigen::Vector3d(1,3,4));
    cout<< "Transform matrix = \n"<< T_a.matrix()<<endl;
    Eigen::Vector3d v_transformed1 = T_a*v;
    cout<<"v transformed :"<<v_transformed1.transpose()<<endl;
    ///射影變換 
    /*
    Eigen::Projective3d T_p;
    T_p.rotate (rotation_vector);
    T_p.prescale(0.5);
    T_p.pretranslate(Eigen::Vector3d(1,3,4));
    cout<< "Transform matrix = \n"<< T_p.matrix()<<endl;
    Eigen::Vector3d v_transformed2 = T_p*v;
    cout<<"v transformed :"<<v_transformed2.transpose()<<endl;
    */
    return 0;
   
}