Eigen库学习 Eigen几何模块

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace std;

int main()
{
//旋转矩阵直接使用Matrix3d或Matrix3f,d为双精度的数据结构，f为单精度的数据结构
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); //初始化为一个单位矩阵

//旋转向量使用AngleAxis
Eigen::AngleAxisd rotation_vector (3.14/4,Eigen::Vector3d(0,0,1));//角+轴：沿Z轴旋转45度
cout .precision(3); //设置有效数字为3

//旋转矩阵和旋转向量的转换
cout<<"rotation matrix=\n"<<rotation_vector.matrix()<<endl; //旋转向量-》旋转矩阵
rotation_matrix = rotation_vector.toRotationMatrix();//

Eigen::Vector3d v (1,0,0);
Eigen::Vector3d v_rotated = rotation_vector*v;
cout<<"(1,0,0)after rotation"<<v_rotated.transpose()<<endl;
v_rotated = rotation_matrix*v;
cout<<"(1,0,0)after rotation"<<v_rotated.transpose()<<endl;

//欧拉角：可将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);
cout<<"yaw pitch roll ="<<euler_angles.transpose()<<endl;

//欧式变换矩阵使用 Eigen::Isometry
Eigen ::Isometry3d T =Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"Transform matrix = \n"<<T.matrix()<<endl;

//用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v;   //相当于R*v+t
cout<<"v tranformed ="<<v_transformed.transpose()<<endl;

//可直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout <<"quaternion = \n"<<q.coeffs()<<endl; //注意coeffs的顺序是（x,y,z,w）,w 为实部，前三者为虚部

//也可以把旋转矩阵赋值给四元数
q=Eigen::Quaterniond(rotation_matrix);
cout<<"quaternion = \n"<<q.coeffs()<<endl;

//使用四元数旋转一个向量，使用重载的乘法即可
v_rotated =q*v;
cout<<"(1,0,0)after rotation ="<<v_rotated.transpose()<<endl;
getchar();
}