Python重实现Eigen库旋转矩阵转四元数结果不一致问题求解

核心错误点 #

• 四元数分量索引映射错误：你定义的q数组下标为0:qw, 1:qx, 2:qy, 3:qz，但else分支中直接将矩阵对角线下标i∈{0,1,2}作为q的下标赋值，导致分量完全错位，qz永远保持初始值0。
• 虚部分量赋值逻辑错位：else分支中，矩阵对角线索引i对应的是四元数的虚部分量：i=0对应qx、i=1对应qy、i=2对应qz，而非你代码中直接赋值给q[i]。

修正后的代码 #

import numpy as np

def rotationMatrixToQuaternion3(m):
    # q[0] = qw, q[1] = qx, q[2] = qy, q[3] = qz
    t = np.trace(m)
    q = np.asarray([0.0, 0.0, 0.0, 0.0], dtype=np.float64)

    if t > 0:
        t = np.sqrt(t + 1)
        q[0] = 0.5 * t
        t = 0.5 / t
        q[1] = (m[2, 1] - m[1, 2]) * t
        q[2] = (m[0, 2] - m[2, 0]) * t
        q[3] = (m[1, 0] - m[0, 1]) * t
    else:
        i = 0
        if m[1, 1] > m[0, 0]:
            i = 1
        if m[2, 2] > m[i, i]:
            i = 2
        j = (i + 1) % 3
        k = (j + 1) % 3

        t = np.sqrt(m[i, i] - m[j, j] - m[k, k] + 1)
        # 矩阵对角线索引i对应四元数虚部分量，下标需+1
        q[i+1] = 0.5 * t
        t = 0.5 / t
        q[0] = (m[k, j] - m[j, k]) * t
        q[j+1] = (m[j, i] + m[i, j]) * t
        q[k+1] = (m[k, i] + m[i, k]) * t

    return q

验证说明 #

修正后输出将与Eigen原生输出完全匹配，Eigen的四元数默认输出顺序为qw, qx, qy, qz，和你定义的返回顺序一致，可直接对比测试结果。

内容的提问来源于stack exchange，提问作者bi3ri