Matrix 转四元数已验证

最新推荐文章于 2025-03-31 17:29:31 发布

原创最新推荐文章于 2025-03-31 17:29:31 发布 · 473 阅读

0 ·

CC 4.0 BY-SA版权

算法专栏收录该内容

9 篇文章

订阅专栏

本文提供了两种从旋转矩阵转换为四元数的方法。通过判断矩阵的不同元素最大值来选择合适的公式进行计算，确保了转换过程的准确性和稳定性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

方法一：

float qw,qx,qy,qz;

float tr = pose[0] + pose[5] + pose[10];
            
            if (tr > 0) {
                float S = sqrt(tr+1.0) * 2; // S=4*qw
                qw = 0.25 * S;
                qx = (pose[9] - pose[6]) / S;
                qy = (pose[2] - pose[8]) / S;
                qz = (pose[4] - pose[1]) / S;
            } else if ((pose[0] > pose[5])&(pose[0] > pose[10])) {
                float S = sqrt(1.0 + pose[0] - pose[5] - pose[10]) * 2; // S=4*qx
                qw = (pose[9] - pose[6]) / S;
                qx = 0.25 * S;
                qy = (pose[1] + pose[4]) / S;
                qz = (pose[2] + pose[8]) / S;
            } else if (pose[5] > pose[10]) {
                float S = sqrt(1.0 + pose[5] - pose[0] - pose[10]) * 2; // S=4*qy
                qw = (pose[2] - pose[8]) / S;
                qx = (pose[1] + pose[4]) / S;
                qy = 0.25 * S;
                qz = (pose[6] + pose[9]) / S;
            } else { 
                float S = sqrt(1.0 + pose[10] - pose[0] - pose[5]) * 2; // S=4*qz
                qw = (pose[4] - pose[1]) / S;
                qx = (pose[2] + pose[8]) / S;
                qy = (pose[6] + pose[9]) / S;
                qz = 0.25 * S;
            }

方法二：

    if (m[0] > m[5] && m[0] > m[10])
    {
        float v84 = sqrt(m[0] + 1.0 - m[5] - m[10]) * 2;
        q[0] = v84 * 0.25f;
        q[1] = (m[1] + m[4]) / v84;
        q[2] = (m[2] + m[8]) / v84;
        q[3] = (m[6] - m[9]) / v84;
    }
    else if(m[5] > m[10])
    {
        float v55 = sqrt(m[5] + 1.0 - m[0] - m[10]) * 2;
        q[0] = (m[1] + m[4]) / v55;
        q[1] = v55 * 0.25f;
        q[2] = (m[6] + m[9]) / v55;
        q[3] = (m[8] - m[2]) / v55;
    }
    else
    {
        float v25 = sqrt(m[10] + 1.0 - m[0] - m[5]) * 2;
        q[0] = (m[8] + m[2]) / v25;
        q[1] = (m[6] + m[9]) / v25;
        q[2] = v25 * 0.25f;
        q[3] = (m[1] - m[4]) / v25;
    }