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轴旋转π/4Eigen::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      0rotation_matrix=rotation_vector.toRotationMatrix();		//可以直接赋值		//  0.707   0.707      0cout<<"rotation_matrix=\n"<<rotation_matrix<<endl;					//    0       0        1Eigen::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=π/4cout<<"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    1T.pretranslate(Eigen::Vector3d(1,3,4));		//平移量设为1,3,4			//  0.707  0.707    0    3cout<<"Transfrom matrix =\n"<<T.matrix()<<endl;					//      0      0    1    4//      0      0    0    1//用变换矩阵进行坐标变换Eigen::Vector3d v_transformed=T*v;		//相当于R*t+vcout<<"v transformed="<<v_transformed.transpose()<<endl;  //v transformed=   1.71    3.71    4/***************************************************四元数**********************************************************/Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);   //将旋转向量赋值给四元数		//  0cout<<"quaterniond_vector=\n"<<q.coeffs()<<endl;   //coeffs()的前三位为虚部，最后一位为实部	//  0//  0.383q=Eigen::Quaterniond(rotation_matrix);   //将旋转矩阵赋值给四元数				//  0.924cout<<"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
}