参考链接:
旋转变换(三)四元数_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