Quaternion and Rotation Matrix

旋转矩阵和四元数

在写数学库的时候遇到了旋转矩阵和四元数之间的转换算法。

本身没什么难的，但是其中有些优化细节不得不提及一下，因为似乎所有的人都这么写，搞得我都看不懂，便抽出时间研究了一下这个。

关于四元数的前置只是，可以查看https://krasjet.github.io/quaternion/quaternion.pdf，这篇文章写的非常好且到位。

---

一个四元数 \(p = (q_x,q_y,q_z,q_w)\) 对\(\vec v\) 施加的旋转可以表示为\(p\vec v p^{-1}\)，此过程可以用矩阵表示成：

\[\vec v^\prime = M_L(p)M_R(p^{-1})\vec v = M_R(p^{-1})M_L(p)\vec v = M(p)\vec v \]

假设四元数\(p\)是一个单位四元数，即\(q_x^2 + q_y^2 + q_z^2 + q_w^2 = 1\)，那么：

\[\begin{align*} M(p) &=M_L(p)M_R(p^{-1}) \\ &= M_R(p^{-1})M_L(p) \\ &= \begin{bmatrix} 1-2(q_y^2 + q_z^2)&2(q_xq_y-q_wq_z)&2(q_xq_z+q_wq_y)&0\\ 2(q_xq_y+q_wq_z)&1-2(q_x^2+q_z^2)&2(q_yq_z-q_wq_x)&0\\ 2(q_xq_z-q_wq_y)&2(q_yq_z+q_wq_x)&1-2(q_x^2+q_y^2)&0\\ 0&0&0&1 \end{bmatrix} \end{align*} \]

如果给出一个3*3大小的旋转矩阵\(M_R\)，那么该矩阵对应的就是左上角3*3的部分。注意，旋转矩阵是正交阵并且每个列向量的模长为1。

通过上面的式子，我们能找到一种关系使得每一个旋转矩阵都可以转成一个四元数。

假设已知旋转矩阵\(M_R\)

\[M_R = \begin{bmatrix} m_{00}&m_{01}&m_{02}\\ m_{10}&m_{11}&m_{12}\\ m_{20}&m_{21}&m_{22} \end{bmatrix} \]

由\(M_R\)和\(M(p)\)的对应关系，有：

\[ m_{10}-m_{01} = 4q_wq_z,\\ m_{02}-m_{20} = 4q_wq_y,\\ m_{21}-m_{12} = 4q_wq_x,\\ tr(M_R) = m_{00}+m_{11}+m_{22} = 4q_w^2-1 \]

所以，我们有：

\[\begin{align*} q_x &= \frac{m_{21}-m_{12}}{4q_w},\\ q_y &= \frac{m_{02}-m_{20}}{4q_w},\\ q_z &= \frac{m_{10}-m_{01}}{4q_w},\\ q_w &= \frac{\sqrt{tr(M_R)+1}}{2} \end{align*} \]

到这里，数学上的问题就已经解决了。

不过在实际应用中，在求解\(q_x,q_y,q_z\)中，假如\(q_w\)的值很小，就会造成误差，为了避免误差，我们需要采用避免另外一种方式求解。

---

我们已经有了\(M(p)\)，便可以变幻出一些奇妙的东西。

\[M(p) + M(p)^T -diag(M(p)+M(p)^T)= \begin{bmatrix} 0&4q_xq_y&4q_xq_z&0\\ 4q_xq_y&0&4q_yq_z&0\\ 4q_xq_z&4q_yq_z&0&0\\ 0&0&0&0 \end{bmatrix} \]

假如在\(p = (q_x,q_y,q_z,q_w)\)中最大的元素是\(q_x\)，那么我们利用上述构造的矩阵，使用该最大元素去求解\(p\)中的其他元素，这样就尽量避免了误差。

致于\(q_w\)的求解，可以使用上述“由\(M_R\)和\(M(p)\)的对应关系”得到的上面三个公式之中的一个，不需要使用开方。

接下来的问题就是，如何找到\(p\)中最大的元素，以及如何得到该元素的具体值。

我们构造一个这样的值\(t\) ：

\[t = q_w^2 - q_x^2 -q_y^2 - q_z^2 \]

所以，有：

\[m_{00} = t + 2q_x^2,\\ m_{11} = t + 2q_y^2,\\ m_{22} = t + 2q_z^2,\\ tr(M_R) = t + 2q_w^2 \]

上述四个公式中的最大值就为对应元素的最大值。

最大值得求解可以使用下面这个公式：

\[\begin{align*} 4q_x^2 &= m_{00} - m_{11} - m_{22} + 1,\\ 4q_y^2 &= -m_{00} + m_{11} - m_{22} + 1,\\ 4q_z^2 &= -m_{00} - m_{11} + m_{22} + 1 \end{align*} \]

当我们得出最大元素值时，利用上述构造出来矩阵，比如我求得了\(q_x\)的值，那么：

\[q_y = \frac{m_{01}+m_{10}}{4q_x},\\ q_z = \frac{m_{02}+m_{20}}{4q_x},\\ q_w = \frac{m_{21}-m_{12}}{4q_x}\\ (其中m_{01}+m_{10}对应矩阵M(p)+M(p)^T中的[0][1]号元素，m_{02}+m_{20}对应[0][2]号元素) \]

---

总结一下，如果我们想要通过一个旋转矩阵求解一个单位四元数时，如果发现\(q_w\)的值最大，则可以使用最上面的方法，假如\(q_w\)不是最大值，则为了保证精确度，使用上面的方法。

最后贴出一个差不多的算法实现。

Quaternion RotationMatrixToQuaternion(const Matrix& rotation){
    Quaternion res;
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".
    float trace = rotation[0][0] + rotation[1][1] + rotation[2][2];
    float root;
    
    if(trace>0.f){
        root  = 	std::sqrt(trace + 1.f); //2w
        res.w = 0.5f * root;
        root  = 0.5f / root;             //1/(4w)
        res.x = (rotation[2][1] - rotation[1][2]) * root;
        res.y = (rotation[0][2] - rotation[2][0]) * root;
        res.z = (rotation[1][0] - rotation[0][1]) * root;
    }
    else{
        size_t s_iNext[3] = {1,2,0};
        size_t i          = 0;
        if(rotation[1][1] > rotation[0][0])
            i = 1;
        if(rotation[2][2] > rotation[i][i])
            i = 2;
        size_t j = s_iNext[i];
        size_t k = s_iNext[k];
        
        root              = std::sqrt(rotation[i][i]-rotation[j][j]-rotation[k][k]+1.f);
        float* apkQuat[3] = {&(res.x),&(res.y),&(res.z)};
        *apkQuat[i]       = 0.5f * root;
        root              = 0.5f / root;
        res.w             = (rotation[k][j] - rotation[j][k]) * root;
        *apkQuat[j]       = (rotation[j][i] + rotation[i][j]) * root;
        *apkQuat[k]       = (rotation[k][i] + rotation[i][k]) * root;
    }
    return res;
}