Eigen库(3)-欧拉角与旋转矩阵互转

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>


void rpyMatrix() {

	//定义3x3变换矩阵，二维，一个角度（如果平移为0，其实就是3x3旋转矩阵，3维，有3个角度）
	Eigen::Matrix3d transformMatrix = Eigen::Matrix3d::Identity();
	//旋转45度
	float rota = M_PI / 4;
	transformMatrix(0, 0) = cos(rota);
	transformMatrix(0, 1) = -sin(rota);
	transformMatrix(1, 0) = sin(rota);
	transformMatrix(1, 1) = cos(rota);
	std::cout << transformMatrix << std::endl;

	Eigen::Vector3d eulerAngle = transformMatrix.eulerAngles(2, 1, 0);
	std::cout << eulerAngle * (180 / M_PI) << std::endl;  //45 -0 0


	//定义3x3旋转矩阵，3维，3个角度
	Eigen::Matrix3d rotation_matrix;
	//45 -0 0
	Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitX()));
	Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
	Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitZ()));
	rotation_matrix = yawAngle*pitchAngle*rollAngle;
	std::cout << rotation_matrix << std::endl;
	eulerAngle = rotation_matrix.eulerAngles(2, 1, 0);
	std::cout << eulerAngle << std::endl;

	//定义4x4变换矩阵，包含旋转矩阵
	Eigen::Matrix4d mat4d = Eigen::Matrix4d::Identity();
	mat4d << rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2), mat4d(0, 3),
		rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2), mat4d(1, 3),
		rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2), mat4d(2, 3),
		mat4d(3, 0), mat4d(3, 1), mat4d(3, 2), mat4d(3, 3);
	std::cout << mat4d;
	//eulerAngle = mat4d.eulerAngles(2, 1, 0);
	//std::cout << eulerAngle << std::endl;

}

int main()
{
	rpyMatrix();
	return 0;
}


//只有3x3矩阵才可以计算欧拉角，4x4变换矩阵不能直接用
//eulerAngle = rotation_matrix.eulerAngles(2, 1, 0);
//std::cout << eulerAngle << std::endl;