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

最新推荐文章于 2023-08-24 15:08:42 发布

RobotLife

最新推荐文章于 2023-08-24 15:08:42 发布

阅读量1.8k

点赞数

分类专栏：算法收集

本文链接：https://blog.csdn.net/RobotLife/article/details/105044367

版权

算法收集专栏收录该内容

10 篇文章 0 订阅

订阅专栏

///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 / tfScalar(2.0);
				euler_out.roll = delta;
				euler_out2.roll = delta;
			}
			else // gimbal locked up
			{
			  tfScalar delta = tfAtan2(-m_el[0].y(),-m_el[0].z());
				euler_out.pitch = -TFSIMD_PI / tfScalar(2.0);
				euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0);
				euler_out.roll = delta;
				euler_out2.roll = delta;
			}
		}
		else
		{
			euler_out.pitch = - tfAsin(m_el[2].x());
			euler_out2.pitch = TFSIMD_PI - euler_out.pitch;

			euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch), 
				m_el[2].z()/tfCos(euler_out.pitch));
			euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch), 
				m_el[2].z()/tfCos(euler_out2.pitch));

			euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch), 
				m_el[0].x()/tfCos(euler_out.pitch));
			euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch), 
				m_el[0].x()/tfCos(euler_out2.pitch));
		}

		if (solution_number == 1)
		{ 
			yaw = euler_out.yaw; 
			pitch = euler_out.pitch;
			roll = euler_out.roll;
		}
		else
		{ 
			yaw = euler_out2.yaw; 
			pitch = euler_out2.pitch;
			roll = euler_out2.roll;
		}
	}

代码来源：ros/LinearMath/Matrix3x3.h