Eigen中的几何模块（四元数、欧拉角、旋转矩阵、旋转向量）

1、Eigen的使用（C++）

在代码中包含头文件

#include <Eigen/Core>
#include <Eigen/Geometry> //提供了各种旋转和平移表示

Eigen是完全由头文件构成的库，因此，在CMakefile中添加“include_directories( "/usr/include/eigen3")”就可以使用Eigen

2、Eigen中常用的模块

Eigen中提供了丰富的几何变换,可以进行旋转向量,旋转矩阵,欧拉角,四元数之间的相互转换

2.1 旋转矩阵（3x3）Eigen::Matrix3d

// 初始化旋转矩阵
// 1 直接赋值
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,
                 x_10,x_11,x_12,
                 x_20,x_21,x_22;

// 2 旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);


Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

// 3 旋转矩阵转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

// 4 旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

2.2 旋转向量    Eigen::AngleAxisd

// 1 初始化旋转向量：旋转角为alpha(rad)，旋转轴为(x,y,z)--单位向量
// 表示绕旋转轴(x,y,z)逆时针旋转alpha

Eigen::AngleAxisd rotation_vector(alpha,Eigen::Vector3d(x,y,z))

// 2 旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

// 3 旋转向量转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

// 4 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;Quaterniond quaternion;

Eigen::Quaterniond quaternion;quaternion=rotation_vector;

2.3 欧拉角/平移向量（3x1）Eigen::Vector3d

// 1 初始化欧拉角(Z-Y-X，即RPY)（rad）

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

// 2 欧拉角转旋转向量
//绕x轴旋转roll角
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
//绕y轴选装pitch角
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
//绕z轴旋转yaw角
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ())); 

Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

// 3 欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ()));
//根据类型自动转换 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

// 4 欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ())); 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

2.4  四元数（4x1）Eigen::Quaterniond

// 1 初始化四元数
// 单位四元素才能表示任意旋转，即x^2 + y^2 + z^2 + w^2 = 1

Eigen::Quaterniond quaternion(w,x,y,z);

// 2 四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

// 3 四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

// 4 四元数转欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

2.5 欧式变换矩阵（4X4）Eigen::Isometry3d

	//欧式变换矩阵使用Eigen::Isometry
	Isometry3d T = Isometry3d::Identity();  //虽是3d,实际上是4*4的矩阵
	T.rotate(rotation_vector);		//按照rotation_vector进行旋转
	T.pretranslate(Vector3d(1,2,3));   	//把平移向量设成（1，2，3）
	cout<<"Transform matrix = \n"<<T.matrix()<<endl;
	//用变换矩阵进行坐标变换
	Vector3d v_transformed = T*v;   //相当于R*V+t
	cout<<"V transfromed = "<<v_transformed.transpose()<<endl;

	//对于仿射变换和射影变换，使用Eigen::Affine3d和Eigen::Projective3d即可

2.6 仿射矩阵 （4X4）Eigen::Affine3d

2.7 射影变换（4X4）Eigen::Projective3d