旋转矩阵和四元数之间互相转换代码

旋转矩阵和四元数之间互相转换代码

python3代码：

import numpy as np

import math

import random

import os



def rotation2quaternion(M):

    tr = np.trace(M)

    m = M.reshape(-1)

    if tr > 0:

        s = np.sqrt(tr + 1.0) * 2

        w = 0.25 * s

        x = (m[7] - m[5]) / s

        y = (m[2] - m[6]) / s

        z = (m[3] - m[1]) / s

    elif m[0] > m[4] and m[0] > m[8]:

        s = np.sqrt(1.0 + m[0] - m[4] - m[8]) * 2

        w = (m[7] - m[5]) / s

        x = 0.25 * s

        y = (m[1] + m[3]) / s

        z = (m[2] + m[6]) / s

    elif m[4] > m[8]:

        s = np.sqrt(1.0 + m[4] - m[0] - m[8]) * 2

        w = (m[2] - m[6]) / s

        x = (m[1] + m[3]) / s

        y = 0.25 * s

        z = (m[5] + m[7]) / s

    else:

        s = np.sqrt(1.0 + m[8] - m[0] - m[4]) * 2

        w = (m[3] - m[1]) / s

        x = (m[2] + m[6]) / s

        y = (m[5] + m[7]) / s

        z = 0.25 * s

    Q = np.array([w, x, y, z]).reshape(-1)

    return Q



def quaternion2rotation(quat):

    assert (len(quat) == 4)

    # normalize first

    quat = quat / np.linalg.norm(quat)

    a, b, c, d = quat



    a2 = a * a

    b2 = b * b

    c2 = c * c

    d2 = d * d

    ab = a * b

    ac = a * c

    ad = a * d

    bc = b * c

    bd = b * d

    cd = c * d



    # s = a2 + b2 + c2 + d2



    m0 = a2 + b2 - c2 - d2

    m1 = 2 * (bc - ad)

    m2 = 2 * (bd + ac)

    m3 = 2 * (bc + ad)

    m4 = a2 - b2 + c2 - d2

    m5 = 2 * (cd - ab)

    m6 = 2 * (bd - ac)

    m7 = 2 * (cd + ab)

    m8 = a2 - b2 - c2 + d2



    return np.array([m0, m1, m2, m3, m4, m5, m6, m7, m8]).reshape(3, 3)