Eigen：旋转矩阵与欧拉角互换

matrix3d->AngleAxisd->matrix3d->matrix4d

友情提示：别将matrix3d变成matrix4d，原因见我之前写的Eigen库常见问题

//测试空间变换
void check() {
	Eigen::Matrix3d transformMatrix = Eigen::Matrix3d::Identity();
	//旋转45度，并延x轴平移0.25米
	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);


	cout << transformMatrix << endl;
	Eigen::Vector3d eulerAngle = transformMatrix.eulerAngles(2, 1, 0);
	cout << eulerAngle * (180 / M_PI) << endl;
	
	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()));

	Eigen::Matrix3d rotation_matrix;
	rotation_matrix = yawAngle*pitchAngle*rollAngle;
	cout<< rotation_matrix<<endl;
	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);
	cout << mat4d ;
}