Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import pyquaternion
- def getQfromR(R):
- return pyquaternion.Quaternion(matrix=R)
- def quaternion_distance(r1, r2):
- r1 = r1.reshape((3, 3))
- r2 = r2.reshape((3, 3))
- q1 = getQfromR(r1)
- q2 = getQfromR(r2)
- dist = q1 * q2.inverse
- return dist.degrees
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement