《视觉slam十四讲》初学小白笔记（3）

Eigen中旋转向量、矩阵、四元数、欧拉角的表示方法

#include <iostream>

#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50
using namespace std;

int main(){
 /**************************************************旋转矩阵和向量**************************************************/
     
    //3d旋转矩阵
    Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();
    cout<<rotation_matrix<<endl<<endl;
    
    //旋转向量,绕Z轴旋转π/4
    Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
    cout.precision(3);									// rotation matrix =
    cout<<"rotation_matrix=\n"<<rotation_vector.matrix()<<endl; //将旋转向量转换成矩阵	//  0.707  -0.707      0
    rotation_matrix=rotation_vector.toRotationMatrix();		//可以直接赋值		//  0.707   0.707      0
    cout<<"rotation_matrix=\n"<<rotation_matrix<<endl;					//    0       0        1
    
    Eigen::Vector3d v(1,0,0);
    //使用旋转向量进行旋转
    Eigen::Vector3d v_rotated=rotation_vector*v;
    cout<<"(1,0,0)after rotation=\n"<<v_rotated.transpose()<<endl;    //(1,0,0)after rotation=0.707  0.707   0
    //使用旋转矩阵进行旋转
    v_rotated=rotation_matrix*v;
    cout<<"(1,0,0)after rotation=\n"<<v_rotated.transpose()<<endl;

/**************************************************欧拉角**********************************************************/
    
    Eigen::Vector3d euler_angles=rotation_matrix.eulerAngles(2,1,0);//按照ZYX顺序旋转         0.785=π/4
    cout<<"yaw_pitch_roll="<<euler_angles.transpose()<<endl;		 // yaw_pitch_roll = 0.785    -0     0
    
    //欧式变换矩阵
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();	//实际是一个4*4的矩阵		// Transform matrix = 
    T.rotate(rotation_vector);				//按照rotation_vector旋转		//  0.707 -0.707    0    1
    T.pretranslate(Eigen::Vector3d(1,3,4));		//平移量设为1,3,4			//  0.707  0.707    0    3
    cout<<"Transfrom matrix =\n"<<T.matrix()<<endl;					//      0      0    1    4
											//      0      0    0    1
    //用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed=T*v;		//相当于R*t+v
    cout<<"v transformed="<<v_transformed.transpose()<<endl;  //v transformed=   1.71    3.71    4

    
/***************************************************四元数**********************************************************/
    
    Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);   //将旋转向量赋值给四元数		//  0
    cout<<"quaterniond_vector=\n"<<q.coeffs()<<endl;   //coeffs()的前三位为虚部，最后一位为实部	//  0
												//  0.383
    q=Eigen::Quaterniond(rotation_matrix);   //将旋转矩阵赋值给四元数				//  0.924
    cout<<"quaterniond_matrix=\n"<<q.coeffs()<<endl;
  
    v_rotated=q*v;	//实际运算过程是qvq^(-1)
    cout<<"(1,0,0)after rotation="<<v_rotated.transpose()<<endl;	//(1,0,0)after rotation=0.707  0.707   0
}