python工具(自用)

后续慢慢更新

1.  pip install时 添加清华镜像源

pip3 install 安装包的名字 -i https://pypi.tuna.tsinghua.edu.cn/simple/

 

2. python 四元数转旋转矩阵  quatanion 2 rotation matrix

def qua_to_rot(qua):
    qw, qx, qy, qz = qua[0], qua[1], qua[2], qua[3]
    r11 = 1 - 2 * np.square(qy) - 2 * np.square(qz)
    r12 = 2 * qx * qy - 2 * qz * qw
    r13 = 2 * qx * qz + 2 * qy * qw

    r21 = 2 * qx * qy + 2 * qz * qw
    r22 = 1 - 2 * np.square(qx) - 2 * np.square(qz)
    r23 = 2 * qy * qz - 2 * qx * qw

    r31 = 2 * qx * qz - 2 * qy * qw
    r32 = 2 * qy * qz + 2 * qx * qw
    r33 = 1 - 2 * np.square(qx) - 2 * np.square(qy)

    rot_new = np.array([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]])
    return rot_new