参考链接:
旋转变换(三)四元数_Frank的专栏-CSDN博客_旋转四元数
scipy.spatial.transform.Rotation — SciPy v1.7.1 Manual
https://krasjet.github.io/quaternion/quaternion.pdf
目标:
已知一个向量 fromVector,要旋转成为一个新向量 toVector。求旋转过程中的四元数。
#!/usr/bin/env python3 # -*- coding: utf-8 -*- import cv2 import numpy as np import math import random from scipy.spatial.transform import Rotation as R model = 'zyx' class QuaternionHandler: def __init__(self): pass def main(self): pass # 已知 起始向量 最终向量,获取旋转的四元数 # 要注意的要点: # 1. 算中心轴的方向:toVector 叉乘 fromVector # 2. 绕轴转的方向,以逆时针为正方向 # 3. 四元数的组成 [Ux*sin(theta/2), Uy*sin(theta/2), Uz*sin(theta/2), cos(theta/2)] # 4. [Ux, Uy, Uz]必须是单位向量 def getQuaternion(self, fromVector, toVector): fromVector = np.array(fromVector) fromVector_e = fromVector / np.linalg.norm(fromVector) toVector = np.array(toVector) toVector_e = toVector / np.linalg.norm(toVector) cross = np.cross(toVector_e, fromVector_e) cross_e = cross / np.linalg.norm(cross) dot = np.dot(fromVector_e, toVector_e) angle = math.acos(dot) if angle == 0 or angle == math.pi: print("两个向量处于一条直线") return False else: return [cross_e[0]*math.sin(angle/2), cross_e[1]*math.sin(angle/2), cross_e[2]*math.sin(angle/2), math.cos(angle/2)] # 已知四元数,求欧拉角 def getEuler(self, quaternion): rotationClass = self.getRotationClass(quaternion) # euler = rotationClass.as_euler(model, degrees=False) euler = rotationClass.as_euler(model, degrees=True) return euler # 已知四元数,求旋转矩阵 def getRotationClass(self, quaternion): rotation = R.from_quat(quaternion) return rotation def getRotation(self, quaternion): rotationClass = self.getRotationClass(quaternion) return rotationClass.as_matrix() if __name__ == '__main__': q = QuaternionHandler() fromV = np.array([1,0,0]) toV = np.array([1,0,1]) print('toV') print(np.linalg.norm(toV)) Rq1 = q.getQuaternion(fromV, toV) print(Rq1) print(np.linalg.norm(np.array(Rq1))) # Rq1 = [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299] rC = q.getRotationClass(Rq1) r = q.getRotation(Rq1) print(r) print('test') print(np.matmul(np.array(fromV), r)) print(rC.apply(fromV)) print('apply(fromV)') e = q.getEuler(Rq1) print(e)
如代码,使用
scipy.spatial.transform。其中的注意事项,已经写在代码中。
要注意的要点: 1. 算中心轴的方向:toVector 叉乘 fromVector 2. 绕轴转的方向,以逆时针为正方向 3. 四元数的组成 [Ux*sin(theta/2), Uy*sin(theta/2), Uz*sin(theta/2), cos(theta/2)] 4. [Ux, Uy, Uz]必须是单位向量,从而四元数的模也是1