后续慢慢更新
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