Calculation of a quaternion for conversion between two 3D Cartesian coordinate systems

Question

I have two Cartesian coordinate systems with known unit vectors:

System A (x_A, y_A, z_A)

and

System B (x_B, y_B, z_B)

Both systems have the same origin (0,0,0). I am trying to compute a quaternion so that the vectors in system B can be expressed in system A.

I am familiar with the mathematical concept of quaternions. I have already completed the required math from here: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

One possible solution would be to calculate the Euler angles and use them for 3 quaternions. Multiplying them will lead to the final, so that I could convert my vectors:

v (A) = q * v (B) * q_conj

But this will turn on Gimbal Lock again, which was the reason that NOT using Euler angles in the beginning.

Any ideal how to solve this?

Answer 1

You can calculate the quaternion representing the best possible transformation from one coordinate system to another, using the method described in this article:

Paul J. Besle and Neil D. Mackay, “Method for Recording Three-Dimensional Figures,” Sensor Fusion IV: Paradigms of Control and Data Structure, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955

The document is not open access, but I can show you a Python implementation:

def get_quaternion(lst1,lst2,matchlist=None): if not matchlist: matchlist=range(len(lst1)) M=np.matrix([[0,0,0],[0,0,0],[0,0,0]]) for i,coord1 in enumerate(lst1): x=np.matrix(np.outer(coord1,lst2[matchlist[i]])) M=M+x N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2]) N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2]) N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2]) N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2]) N12=float(M[1][:,2]-M[2][:,1]) N13=float(M[2][:,0]-M[0][:,2]) N14=float(M[0][:,1]-M[1][:,0]) N21=float(N12) N23=float(M[0][:,1]+M[1][:,0]) N24=float(M[2][:,0]+M[0][:,2]) N31=float(N13) N32=float(N23) N34=float(M[1][:,2]+M[2][:,1]) N41=float(N14) N42=float(N24) N43=float(N34) N=np.matrix([[N11,N12,N13,N14],\ [N21,N22,N23,N24],\ [N31,N32,N33,N34],\ [N41,N42,N43,N44]]) values,vectors=np.linalg.eig(N) w=list(values) mw=max(w) quat= vectors[:,w.index(mw)] quat=np.array(quat).reshape(-1,).tolist() return quat 

This function returns the quaternion you were looking for. The arguments lst1 and lst2 are lists of numpy.arrays, where each array represents a three-dimensional vector. If both lists have a length of 3 (and contain orthogonal unit vectors), the quaternion must be an exact transformation. If you provide longer lists, you will get a quaternion that minimizes the difference between both sets of points. The optional matchlist argument is used to tell the function which point lst2 should be converted to which point in lst1. If no match list is provided, the function assumes that the first point in lst1 must match the first point in lst2, and so on ...

A similar function for 3-point sets in C ++ is as follows:

 #include <Eigen/Dense> #include <Eigen/Geometry> using namespace Eigen; /// Determine rotation quaternion from coordinate system 1 (vectors /// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2) Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1, Vector3d x2, Vector3d y2, Vector3d z2) { Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose(); Matrix4d N; N << M(0,0)+M(1,1)+M(2,2) ,M(1,2)-M(2,1) , M(2,0)-M(0,2) , M(0,1)-M(1,0), M(1,2)-M(2,1) ,M(0,0)-M(1,1)-M(2,2) , M(0,1)+M(1,0) , M(2,0)+M(0,2), M(2,0)-M(0,2) ,M(0,1)+M(1,0) ,-M(0,0)+M(1,1)-M(2,2) , M(1,2)+M(2,1), M(0,1)-M(1,0) ,M(2,0)+M(0,2) , M(1,2)+M(2,1) ,-M(0,0)-M(1,1)+M(2,2); EigenSolver<Matrix4d> N_es(N); Vector4d::Index maxIndex; N_es.eigenvalues().real().maxCoeff(&maxIndex); Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real(); Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3)); quat.normalize(); return quat; } 

Answer 2

What language do you use? If C ++, feel free to use my open source library:

http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/

In short, you will need to convert your vectors into quaternions, do your calculations, and then convert your quaternion into a transformation matrix.

Here's the code snippet:

Quaternion from vector:

 cQuat nTrans::quatFromVec( Vec vec ) { float angle = vec.v[3]; float s_angle = sin( angle / 2); float c_angle = cos( angle / 2); return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle, vec.v[2]*s_angle )).normalized(); } 

And for a matrix from a quaternion:

 Matrix nTrans::matFromQuat( cQuat q ) { Matrix t; q = q.normalized(); tM[0][0] = ( 1 - (2*qy*qy + 2*qz*qz) ); tM[0][1] = ( 2*qx*qy + 2*qw*qz); tM[0][2] = ( 2*qx*qz - 2*qw*qy); tM[0][3] = 0; tM[1][0] = ( 2*qx*qy - 2*qw*qz); tM[1][1] = ( 1 - (2*qx*qx + 2*qz*qz) ); tM[1][2] = ( 2*qy*qz + 2*qw*qx); tM[1][3] = 0; tM[2][0] = ( 2*qx*qz + 2*qw*qy); tM[2][1] = ( 2*qy*qz - 2*qw*qx); tM[2][2] = ( 1 - (2*qx*qx + 2*qy*qy) ); tM[2][3] = 0; tM[3][0] = 0; tM[3][1] = 0; tM[3][2] = 0; tM[3][3] = 1; return t; } 

Answer 3

I ran into this problem. I was on my way to a solution, but I was stuck.

So, you will need two vectors that are known in both coordinate systems. In my case, I have 2 orthonormal vectors in the device coordinate system (gravity and magnetic field), and I want the quaternion to rotate from the device coordinates to the global orientation (where North is positive Y and "up" is positive Z). So, in my case, I measured the vectors in the coordinate space of the device, and I define the vectors themselves to form an orthonormal basis for the global system.

Based on the foregoing, let us consider the autumn-angular interpretation of quaternions, there is some vector V along which the coordinates of the device can be rotated by some angle to correspond to global coordinates. I will name my (negative) gravitational vector G and magnetic field M (both normalized).

V, G and M describe points on the unit sphere. So, Z_dev and Y_dev (Z and Y bases for my device coordinate system). The goal is to find a rotation that maps G to Z_dev and M to Y_dev. In order for V to rotate G by Z_dev, the distance between the points defined by G and V must be the same as the distance between the points defined by V and Z_dev. In the equations:

| V - G | = | V - Z_dev |

The solution of this equation forms a plane (all points equidistant to G and Z_dev). But V is limited to a unit length, which means that the solution is a ring centered at the origin — still an infinite number of points.

But, the same situation for Y_dev, M and V:

| V - M | = | V - Y_dev |

A solution to this is also a ring centered on the origin. These rings have two intersection points, where one is negative. Or the actual axis of rotation (the rotation angle in one case will be negative).

Using the above equations and the fact that each of these vectors is a unit length, you should be able to solve for V.

Then you just need to find the angle of rotation that you can do using vectors coming from V to the corresponding bases (G and Z_dev for me).

Ultimately, I got gummed to the end of the algebra in the solution for V .. but anyway, I think that all you need is here - maybe you are more fortunate than me.

Answer 4

You need to express the orientation of B with respect to A as a quaternion of Q. Then any vector from B can be converted to a vector in A, for example. using the rotation matrix R obtained from Q. vectorInA = R * vectorInB.

To do this, there is a demo script (including nice visualization) in the Matlab / Octave library available on this site: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations

Answer 5

You can calculate what you want using only quaternion algebra.

Having two unit vectors v ₁ and v ₂ , you can directly embed them in the quaternion algebra and get the corresponding pure quaternions q ₁ and q ₂ . A quaternion of rotation Q, which aligns two vectors so that:

Q q ₁ Q ^* = q ₂

provided by:

Q = q ₁ (q ₁ + q ₂ ) / (|| q ₁ + q ₂ ||)

The above product is a quaternion product.

Answer 6

Define 3x3 A and B matrices as you specified them, so columns A are x_A, x_B and x_C, and columns B are defined in the same way. Then the transformation of T with the coordinate system A to B is a solution TA = B, therefore T = BA ^ {-1}. From the rotation matrix of the T transform, you can calculate the quaternion using standard methods.