Eigen旋转矩阵和欧拉角相互转换

Eigen旋转矩阵和欧拉角相互转换

#include<iostream>
using namespace std;
#include<Eigen/Eigen>
#include<math.h>


Eigen::Matrix3d RPY2Rotation(float roll , float pitch , float yaw)		//roll , pitch , yaw
{
	///send EulerAngles and transform into RotationMatrix 

	Eigen::Matrix3d R, Rx, Ry, Rz;

	Rx << 1.0, 0.0, 0.0,
		0.0, cos(roll), -sin(roll),
		0.0, sin(roll), cos(roll);

	Ry << cos(pitch), 0.0, sin(pitch),
		0.0, 1.0, 0.0,
		-sin(pitch), 0.0, cos(pitch);

	Rz << cos(yaw), -sin(yaw), 0.0,
		sin(yaw), cos(yaw), 0.0,
		0.0, 0.0, 1.0;

	R = Rz * Ry * Rx;

	return R;

}

Eigen::Vector3d Rotation2RPY(Eigen::Matrix3d &aux_rotation)
{
	Eigen::Vector3d eulerAngle = aux_rotation.eulerAngles(2, 1, 0);		//row , pitch , yaw
	return eulerAngle;
}

int main()
{

	std::cout << "send virtual row , pitch ,yaw = 1,2,3 " << std::endl;

	Eigen::Matrix3d aux_R = RPY2Rotation(1, 2, 3);

	Eigen::Vector3d aux_eulerAngle = Rotation2RPY(aux_R);

	std::cout << "row : " << aux_eulerAngle.z() << std::endl;

	std::cout << "pitch : " << aux_eulerAngle.y() << std::endl;

	std::cout << "yaw : " << aux_eulerAngle.x() << std::endl;

}