从旋转矩阵到四元数

迷南。 2022-03-09 09:04 241阅读 0赞

在[文章：从四元数计算旋转矩阵][Link 1]的基础上，现在来考虑从旋转矩阵到四元数的计算。

### 从四元数(*w,x,y,z*)计算旋转矩阵 ###

![在这里插入图片描述][20190312202441950.png]

--------------------

### 从旋转矩阵计算四元数 ###

为了从旋转矩阵中求出相应的四元数，可以直接利用上述已经求得的矩阵。

#### 计算方法一： ####

计算对角线元素之和即可求的 ***w*** 值：  
![在这里插入图片描述][20190312202754139.png]  
同理可以计算 ***x,y,z*** 的值：  
![在这里插入图片描述][2019031220325524.png]  
![在这里插入图片描述][20190312203306210.png]  
![在这里插入图片描述][20190312203318543.png]  
**！！！重点提醒！！！**  
上述的计算方式是存在不完整性的，因为四元数所有分量的计算都是通过开方所得，所有值都是非负数，这与实际是不相符的。没有准确的依据来确定是选择正根还是负根。

#### 计算方式二： ####

计算相对于对角线堆成位置上的元素和与差：  
![在这里插入图片描述][20190312204758277.png]  
我们可以发现，一旦对角线元素和/差的平方根解的了4个值中的一个，就嫩用以下方法计算其他的三个：  
![在这里插入图片描述][20190312205536745.png]  
![在这里插入图片描述][20190312205549251.png]  
![在这里插入图片描述][20190312205556977.png]  
![在这里插入图片描述][20190312205603920.png]  
那么问题来了，应该选以上四种的哪一种呢？

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

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

#### 基于计算方式二的代码实现： ####

以下是C++版本的核心代码

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;
    }

**注意：以上所有的计算都是在左手坐标系中进行的，都是基于左手法则的。如果是存在坐标系等的变化结果可能与上述不同。**

[Link 1]: https://blog.csdn.net/loongkingwhat/article/details/88427828
[20190312202441950.png]: /images/20220309/b48920deab394b94abb7a8fef0297488.png
[20190312202754139.png]: /images/20220309/572f8f64020a41d0a09be559f6922df2.png
[2019031220325524.png]: /images/20220309/27d71cbb026a4ff495ae186f0e8eaf91.png
[20190312203306210.png]: /images/20220309/fcc3ee5473fe4ee292847e35b631b779.png
[20190312203318543.png]: /images/20220309/bd06d786b13d4a48a0fbc3485065620b.png
[20190312204758277.png]: /images/20220309/2d9ca9fa04484532a1f4f50db4dfe032.png
[20190312205536745.png]: /images/20220309/45c5d1879a7642d9b3b873f0d59aa97d.png
[20190312205549251.png]: /images/20220309/a72000c2a6ab49e196de637769a16c75.png
[20190312205556977.png]: /images/20220309/2340faefd476485a951244a93d95c36f.png
[20190312205603920.png]: /images/20220309/d0b9d4dde84843738c46e83b293cac96.png