旋转矩阵与四元数转换关系

基础概念

四元数与3D旋转

https://blog.csdn.net/reasonyuanrobot/article/details/110938473

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

https://www.cnblogs.com/flyinggod/p/8144100.html

 

从旋转矩阵到四元数

参考：https://blog.csdn.net/loongkingwhat/article/details/88428310

左手坐标系中

 
计算对角线元素之和即可求的 w 值：

同理可以计算 x,y,z 的值：

上述的计算方式是存在不完整性的，因为四元数所有分量的计算都是通过开方所得，所有值都是非负数，这与实际是不相符的。没有准确的依据来确定是选择正根还是负根。

计算相对于对角线堆成位置上的元素和与差：

我们可以发现，一旦对角线元素和/差的平方根解的了4个值中的一个，就嫩用以下方法计算其他的三个：

那么问题来了，应该选以上四种的哪一种呢？

似乎最简单的策略就是总是先计算同一个分量，比如 w，然后再计算 x,y,z 。这样的方式可能存在如下错误：如果 w=0，除法就没有意义(分母不能为0)；如有w非常小，将会出现数值不稳定。有学者建议首先判断 w,x,y,z 中哪一个最大，就用对角线元素计算钙元素，然后再通过计算方式二中公式计算其他三个分量。

NOTE： 你可以能有一个疑惑，为什么方法二中可以不用考虑开方取正根还是负根的问题。此处用到了负四元数的性质，那就是 q(w,x,y,z) 和 -q(-w,-x,-y,-z) 代表相同的方位。所以只要保证他们的符号是相对一致即可，同时负号并不影响结果。

 

右手坐标系中

可以发现，由同一四元数q求得的左右手系下的转转矩阵，互为转置；

由旋转矩阵求四元数的推导过程，与左手坐标系中类似；

 

参考C++代码：

//先判断R是否为旋转矩阵，R*(RT)=I

float m11,m12,m13;
float m21,m22,m23;
float m31,m32,m33;

float w,x,y,z;


//探测四元数中最大的项 
float fourWSquaredMinusl = m11+m22+m33;
float fourXSquaredMinusl = m11-m22-m33;
float fourYSquaredMinusl = m22-m11-m33;
float fourZSquaredMinusl = m33-m11-m22;

int biggestIndex = 0;
float fourBiggestSqureMinus1 = fourWSquaredMinusl;
if(fourXSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourXSquaredMinusl;
	biggestIndex =1;
} 
if(fourYSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourYSquaredMinusl;
	biggestIndex =2;
} 
if(fourZSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourZSquaredMinusl;
	biggestIndex =3;
} 

//计算平方根和除法 
float biggestVal = sqrt(fourBiggestSqureMinus1+1.0f)*0.5f;
float mult = 0.25f/biggestVal;

/************   计算四元数的值  ****************/
//************ 左手系
switch(biggestIndex){
	case 0:
		w=biggestVal;
		x=(m23-m32)*mult;
		y=(m31-m13)*mult;
		z=(m12-m21)*mult;
		break;
	case 1:
		x = biggestVal;
		w =(m23-m32)*mult;
		y =(m12+m21)*mult;
		z =(m31+m13)*mult;
		break;
	case 2:
		y =biggestVal;
		w =(m31-m13)*mult;
		x =(m12+m21)*mult;
		z =(m23+m32)*mult;
		break;
	case 3:
		z =biggestVal;
		w =(m12-m21)*mult;
		x =(m31+m13)*mult;
		y =(m23+m32)*mult;
		break;
} 

//************，右手系
//由同一q求得的左右手系下的转转矩阵，互为转置；
switch(biggestIndex){
	case 0:
		w=biggestVal;
		x=-1*(m23-m32)*mult;
		y=-1*(m31-m13)*mult;
		z=-1*(m12-m21)*mult;
		break;
	case 1:
		x = biggestVal;
		w =-1*(m23-m32)*mult;
		y =(m12+m21)*mult;
		z =(m31+m13)*mult;
		break;
	case 2:
		y =biggestVal;
		w =-1*(m31-m13)*mult;
		x =(m12+m21)*mult;
		z =(m23+m32)*mult;
		break;
	case 3:
		z =biggestVal;
		w =-1*(m12-m21)*mult;
		x =(m31+m13)*mult;
		y =(m23+m32)*mult;
		break;
}