python - 在 Python 中重新实现 Eigen 旋转矩阵到四元数的转换

Question

为了一致性，我想在 python 中重新实现从旋转矩阵到 Cpp Eigen 库的四元数的转换。

Cpp 实现可以在这里找到，下面你可以找到我的 python 实现：

def rotationMatrixToQuaternion3(m):
    #q0 = qw
    t = np.matrix.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)
        q[i] = 0.5 * t
        t = 0.5 / t
        q[0] = (m[k,j] - m[j,k]) * t
        q[j] = (m[j,i] + m[i,j]) * t
        q[k] = (m[k,i] + m[i,k]) * t

    return q

以下是一些示例结果：

rotation matrix: 
[[-0.00998882  0.01194957 -0.99987871]
 [ 0.49223613 -0.87032691 -0.01531875]
 [-0.8704044  -0.49232944  0.00281153]]
python implemnation - qw, qx, qy, qz:
[-0.68145553 -0.18496647  0.68613542  0.        ]
eigen:
-0.686135  -0.174997 -0.681456 0.184966



rotation matrix: 
[[ 0.01541426  0.02293597 -0.9996181 ]
 [ 0.49081359 -0.87117607 -0.01242048]
 [-0.87112825 -0.49043469 -0.02468582]]
python implemnation - qw, qx, qy, qz:
[-0.17288173  0.18580601 -0.67658628  0.        ]
eigen:
-0.686135  -0.174997 -0.681456 0.184966



rotation matrix: 
[[ 0.03744363 -0.01068005 -0.99924167]
 [ 0.48694091 -0.87299945  0.02757743]
 [-0.87263195 -0.48760425 -0.02748771]]
python implemnation - qw, qx, qy, qz:
[-0.18503815  0.17105894 -0.67232212  0.        ]
eigen:
-0.672322  -0.185038 -0.696048 0.171059

帮助将不胜感激！谢谢，约翰内斯

标签： pythoneigenrosquaternionsrotational-matrices