SLAM学习笔记3

三维空间刚体运动，笔记内容有向量内积，向量外积，欧氏变换，旋转向量，欧拉角，旋转矩阵，四元数以及它们的转换关系，代码是Eigen库的基本使用。

笔记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; //实质是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; //转置
    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);//实对称矩阵
    cout<< "Eigen values = "<< eigen_solver.eigenvalues() << endl; //特征值
    cout<< "Eigen vectors = "<< eigen_solver.eigenvectors() << endl; //特征向量
    
    //解方程，对比求逆和矩阵分解的速度
    
    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 中四元数，欧拉角，旋转矩阵，旋转向量的转换
    
    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(); //旋转向量可以转换为旋转矩阵
    cout<<"rotation_matrix is :"<<rotation_matrix<<endl;
    
    ///AngleAxis 旋转向量进行坐标变换
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotated:"<<v_rotated.transpose()<<endl;
    
    /// 旋转矩阵进行坐标变换
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose();
    
    /// 欧拉角： 可以直接将旋转矩阵转换为欧拉角
    
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); //(2,1,0)表示ZYX 的旋转顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
    
    ///四元数  可以将旋转向量转换为四元数
    Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
    cout<<"Quaterniond = \n"<<q.coeffs()<<endl;//coeffs 表示顺序是(x,y,z,w),w为实部，前三者为虚部。
    
    /// 也可以将旋转矩阵赋予给四元数
    q = Eigen::Quaterniond(rotation_matrix);
    cout<<"Quaterniond = \n"<<q.coeffs()<<endl;
    ///四元数旋转一个矩阵
    v_rotated = q*v; //注意数学形式为 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;
   
}