旋转矩阵、欧拉角、四元数理论及其转换关系

• 转载链接：https://blog.csdn.net/lql0716/article/details/72597719

旋转矩阵求四元数

• 情况1：
• 情况2：
  如果$q_0$趋近于0，$tr(R)$趋近于-1,则求解四元数的过程为：
  • 2.1 如果 m a x { r 11 , r 22 , r 33 } = r 11 max \left\{ r_{11},r_{22},r_{33} \right\} =r_{11} max{r11​,r22​,r33​}=r11​
    
  • 2.2 如果 m a x { r 11 , r 22 , r 33 } = r 22 max \left\{ r_{11},r_{22},r_{33} \right\} =r_{22} max{r11​,r22​,r33​}=r22​
    
  • 2.3 如果 m a x { r 11 , r 22 , r 33 } = r 33 max \left\{ r_{11},r_{22},r_{33} \right\} =r_{33} max{r11​,r22​,r33​}=r33​
    
• C++实现代码

void R2quaternion(double R[3][3], double q[4]) {

	//四元数的顺序：qw、qx、qy、qz
	double tr_R, qw_4;

	tr_R = R[0][0] + R[1][1] + R[2][2];
	if (tr_R > -1 && fabs(tr_R + 1) > 1e-5) {

		q[0] = sqrt(tr_R + 1) / 2.0;
		qw_4 = 4 * q[0];
		q[1] = (R[2][1] - R[1][2]) / qw_4;
		q[2] = (R[0][2] - R[2][0]) / qw_4;
		q[3] = (R[1][0] - R[0][1]) / qw_4;
	}
	else {
		
		if (R[0][0] > R[1][1] && R[0][0] > R[2][2]) {
			double t = sqrt(1 + R[0][0] - R[1][1] - R[2][2]);
			q[0] = (R[2][1] - R[1][2]) / t;
			q[1] = t / 4.0;
			q[2] = (R[0][2] + R[2][0]) / t;
			q[3] = (R[0][1] + R[1][0]) / t;
		}
		else if (R[1][1] > R[0][0] && R[1][1] > R[2][2]) {
			double t = sqrt(1 - R[0][0] + R[1][1] - R[2][2]);
			q[0] = (R[0][2] - R[2][0]) / t;
			q[1] = (R[0][1] + R[1][0])  / t;
			q[2] = t / 4.0;
			q[3] = (R[2][1] + R[1][2]) / t;
		}
		else {
			double t = sqrt(1 - R[0][0] - R[1][1] + R[2][2]);
			q[0] = (R[1][0] - R[0][1]) / t;
			q[1] = (R[0][2] + R[2][0]) / t;
			q[2] = (R[1][2] - R[2][1]) / t;									
			q[3] = t / 4.0;
		}	
	}
}