基础概念
四元数与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;
}