Rotation matrix to quaternion equivalence
I am tracking 3 points (3D, xyz) on a rigid body, which I use to define a local coordinate system. I am using this local coordinate system's orientation (in the global frame of reference) to set the view orientation in a VR program. To do this, and avoid gimbal-lock with Euler angles, I am attempting to use a quaternion to set the view orientation.
I create a rotation matrix from the 3 points, then I use this method described on Wikipedia to extract the supposed equivalent quaternion. I then simply set the view orientation to be the calculated quaternion.
However, what I observe is that there is mainly only 1 degree of freedom (pitch) when I should also be able to simultaneously see changes in the yaw and roll. I have extracted the Euler angles from the rotation matrix, and it works well except at gimbal-lock. So I am certain the rotation matrix is usable, though it is improper in my case.
My question is why does the supposed equivalent quaternion seem to only change the 'pitch' degree of freedom?
I am aware that a quaternion is a rotation about 1 axis, however I thought if it was derived from the rotation matrix, the end result would be the same as with setting Euler angles?
Here is my code in python:
import viz
import numpy as np
vec1 = np.array([-0.96803,-0.25022,0.01751],dtype=float)
vec3 = np.array([-0.024815,0.96553,0.07863],dtype=float)
vec4 = np.array([-0.03655,0.07178,-0.99675],dtype=float)
#normalize to unit length
vec1 = vec1 / np.linalg.norm(vec1)
vec3 = vec3 / np.linalg.norm(vec3)
vec4 = vec4 / np.linalg.norm(vec4)
M1 = np.zeros((3,3),dtype=float) #rotation matrix
#rotation matrix setup
M1[:,0] = vec1
M1[:,1] = vec3
M1[:,2] = vec4
#get the real part of the quaternion first
r = np.math.sqrt(float(1)+M1[0,0]+M1[1,1]+M1[2,2])*0.5
i = (M1[2,1]-M1[1,2])/(4*r)
j = (M1[0,2]-M1[2,0])/(4*r)
k = (M1[1,0]-M1[0,1])/(4*r)
viz.MainView.setQuat(i,j,k,r)
Any help or ideas would be great!
The key problem here is that you applied an algorithm that pertains only to proper 3x3 rotation matrices to a matrix that is not orthogonal and is very close to an improper rotation matrix. It is the latter that is the key source of your problems.
Your matrix M1 is
array([[-0.9994477 , -0.02887993, 0.0164005 ],
[-0.02958325, 0.99862763, -0.04323132],
[ 0.01513678, 0.0436899 , 0.99893047]])
You'll get nonsense when you improperly apply that algorithm for extracting a quaternion from a proper rotation matrix to this improper matrix. In particular, because M[2,1]
is approximately equal to -M[1,2]
, M[0,2]
is approximately equal to M[2,0]
, and M[1,0]
is approximately equal to M[0,1]
, you'll get what appears to be almost pure roll.
Note well: The same applies to algorithms for extracting Euler angles from a matrix. These algorithms all assume the matrix is a proper rotation matrix. When you improperly apply them to an improper rotation matrix you will get nonsense.
链接地址: http://www.djcxy.com/p/81784.html上一篇: 旋转角度来自Quaternion
下一篇: 旋转矩阵到四元数等价