四元数，旋转矩阵，欧拉角互转（python）

本文链接：https://blog.csdn.net/weixin_45024226/article/details/125114042

该博客介绍了如何在Python中进行旋转矩阵与四元数之间的转换，包括旋转矩阵到欧拉角的转换和欧拉角到旋转矩阵的转换。此外，还展示了如何将旋转矩阵转换为四元数以及四元数回转到旋转矩阵的过程。代码示例详细解释了每个函数的使用方法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

运行代码之前需要安装pyquaternion和scipy。

pip install pyquaternion

pip install scipy

代码之前放下面，main函数有使用的方式

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import numpy as np
import math
#旋转矩阵转四元数需要pyquaternion包
from pyquaternion import Quaternion
#四元数转旋转矩阵需要scipy
from scipy.spatial.transform import Rotation as R

def isRotationMatrix(R) :
  Rt = np.transpose(R)
  shouldBeIdentity = np.dot(Rt, R)
  I = np.identity(3, dtype = R.dtype)
  n = np.linalg.norm(I - shouldBeIdentity)
  return n < 1e-6
#rotationMatrixToEulerAngles 用于旋转矩阵转欧拉角
def rotationMatrixToEulerAngles(R) :
 
  assert(isRotationMatrix(R))
   
  sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
   
  singular = sy < 1e-6
 
  if not singular :
    x = math.atan2(R[2,1] , R[2,2])
    y = math.atan2(-R[2,0], sy)
    z = math.atan2(R[1,0], R[0,0])
  else :
    x = math.atan2(-R[1,2], R[1,1])
    y = math.atan2(-R[2,0], sy)
    z = 0
 
  return np.array([x, y, z])
#eulerAnglesToRotationMatrix欧拉角转旋转矩阵
def eulerAnglesToRotationMatrix(theta) :
  R_x = np.array([[1,     0,         0          ],
          [0,     math.cos(theta[0]), -math.sin(theta[0]) ],
          [0,     math.sin(theta[0]), math.cos(theta[0]) ]
          ])
  R_y = np.array([[math.cos(theta[1]),  0,   math.sin(theta[1]) ],
          [0,           1,   0          ],
          [-math.sin(theta[1]),  0,   math.cos(theta[1]) ]
          ])
         
  R_z = np.array([[math.cos(theta[2]),  -math.sin(theta[2]),  0],
          [math.sin(theta[2]),  math.cos(theta[2]),   0],
          [0,           0,           1]
          ])
  R = np.dot(R_z, np.dot( R_y, R_x ))
  return R
#旋转矩阵转四元数
def rotateToQuaternion(rotateMatrix):
    q = Quaternion(matrix=rotateMatrix)
    return q

if __name__ == '__main__' :
  #初始化旋转矩阵
  rotationMat = np.array([[ -0.90748313, -0.30075654, -0.29329146],
                 [-0.05041803, -0.61514386, 0.78680115],
                 [ -0.41705203, 0.72879595, 0.54306912]])


  #旋转矩阵转欧拉角
  EulerAngles = rotationMatrixToEulerAngles(rotationMat)
  print("\nOutput Euler angles :\n{0}".format(EulerAngles))


  #欧拉角转旋转矩阵
  rotationMat_1 = eulerAnglesToRotationMatrix(EulerAngles)
  print ("\nR1 :\n{0}".format(rotationMat_1))

  #旋转矩阵转四元数
  Quaternion = rotateToQuaternion(rotationMat)
  print("四元数x为: ", Quaternion.x, "\n四元数y为: ", Quaternion.y, "\n四元数z为: ", Quaternion.z, "\n四元数w为: ", Quaternion.w)


  #四元数转旋转矩阵
  Rq = [Quaternion.x.astype(float), Quaternion.y.astype(float), Quaternion.z.astype(float), Quaternion.w.astype(float)]
  Rm = R.from_quat(Rq)
  rotation_matrix = Rm.as_matrix()
  print('rotation:\n', rotation_matrix)