位姿转换及估计等代码汇总（持续更新）

1 四元数转旋转矩阵（原理版）。

def quat2rot(qx, qy, qz, qw):
    # 求四元数的模长
    qmod = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
    # 四元数归一化
    qx = qx / qmod
    qy = qy / qmod
    qz = qz / qmod
    qw = qw / qmod

    # 计算旋转矩阵
    rot = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]], np.float)
    rot[0, 0] = 1 - 2 * (qy * qy + qz * qz)
    rot[0, 1] = 2 * (qx * qy - qw * qz)
    rot[0, 2] = 2 * (qx * qz + qw * qy)
    rot[1, 0] = 2 * (qx * qy + qw * qz)
    rot[1, 1] = 1 - 2 * (qx * qx + qz * qz)
    rot[1, 2] = 2 * (qy * qz - qw * qx)
    rot[2, 0] = 2 * (qx * qz - qw * qy)
    rot[2, 1] = 2 * (qy * qz + qw * qx)
    rot[2, 2] = 1 - 2 * (qx * qx + qy * qy)
    # 返回旋转矩阵(与Matlab quat2dcm函数保持一致)
    return rot.transpose()
if __name__ == '__main__':
 	# Poses: qx,qy,qz,qw,x,y,z
    pose_ref1 = [-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614]
    ref1_ros = quat2rot(pose_ref1[0], pose_ref1[1], pose_ref1[2], pose_ref1[3])

2四元数转旋转矩阵（scipy库简单版）。

from scipy.spatial.transform import Rotation as R
  


if __name__ == '__main__':
 	# Poses: qx,qy,qz,qw,x,y,z
    pose_ref1 = [-0.757610, -0.348629, -0.497711, 0.238261, 4.460675, -1.680515, 0.579614]
	# 四元数到旋转矩阵
	r = R.from_quat(pose_ref1[0:4])
	Rm = r.as_matrix()