根据旋转矩阵R求解欧拉角yaw,pitch,roll源代码

原创

于 2020-03-23 11:19:05 发布 · 1.9k 阅读

0 ·

CC 4.0 BY-SA版权

本文详细介绍了如何将3x3矩阵转换为绕YXZ轴的欧拉角表示，包括处理奇异点和求解两个可能的角度解决方案。通过判断矩阵元素，避免了万向节锁定问题，并提供了两个可能的欧拉角解决方案。

///Data storage for the matrix, each vector is a row of the matrix
	Vector3 m_el[3];
/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
	* @param yaw Yaw around Z axis
	* @param pitch Pitch around Y axis
	* @param roll around X axis */	
	void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
	{
		struct Euler
		{
			tfScalar yaw;
			tfScalar pitch;
			tfScalar roll;
		};

		Euler euler_out;
		Euler euler_out2; //second solution
		//get the pointer to the raw data

		// Check that pitch is not at a singularity
  		// Check that pitch is not at a singularity
		if (tfFabs(m_el[2].x()) >= 1)
		{
			euler_out.yaw = 0;
			euler_out2.yaw = 0;
	
			// From difference of angles formula
			if (m_el[2].x() < 0)  //gimbal locked down
			{
			  tfScalar delta = tfAtan2(m_el[0].y(),m_el[0].z());
				euler_out.pitch = TFSIMD_PI / tfScalar(2.0);
				euler_out2.pitch = TFSIMD_PI / tfS