Calculating quaternion for transformation between 2 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 share the same origin (0,0,0). I'm trying to calculate a quaternion, so that vectors in system B can be expressed in system A.

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

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

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

But this would incorporate Gimbal Lock again, which was the reason NOT to use Euler angles in the beginning.

Any idead how to solve this?

Answer 1

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

Answer 2

You can compute what you want using only quaternion algebra.

Given two unit vectors v₁ and v₂ you can directly embed them into quaternion algebra and get the corresponding pure quaternions q₁ and q₂. The rotation quaternion Q that align the two vectors such that:

Q q₁ Q^* = q₂

is given by:

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

The above product is the quaternion product.

Answer 3

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

Paul J. Besl and Neil D. McKay "Method for registration of 3-D shapes", Sensor Fusion IV: Control Paradigms and Data Structures, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955

The paper is not open access but I can show you the 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 that you were looking for. The arguments lst1 and lst2 are lists of numpy.arrays where every array represents a 3D vector. If both lists are of length 3 (and contain orthogonal unit vectors), the quaternion should be the exact transformation. If you provide longer lists, you get the quaternion that is minimizing the difference between both point sets. The optional matchlist argument is used to tell the function which point of lst2 should be transformed to which point in lst1. If no matchlist is provided, the function assumes that the first point in lst1 should match the first point in lst2 and so forth...

A similar function for sets of 3 Points in C++ is the following:

#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 4

I just ran into this same problem. I was on the track to a solution, but I got stuck.

So, you'll need TWO vectors which are known in both coordinate systems. In my case, I have 2 orthonormal vectors in the coordinate system of a device (gravity and magnetic field), and I want to find the quaternion to rotate from device coordinates to global orientation (where North is positive Y, and "up" is positive Z). So, in my case, I've measured the vectors in the device coordinate space, and I'm defining the vectors themselves to form the orthonormal basis for the global system.

With that said, consider the axis-angle interpretation of quaternions, there is some vector V about which the device's coordinates can be rotated by some angle to match the global coordinates. I'll call my (negative) gravity vector G, and magnetic field M (both are normalized).

V, G and M all describe points on the unit sphere. So do Z_dev and Y_dev (the Z and Y bases for my device's coordinate system). The goal is to find a rotation which maps G onto Z_dev and M onto Y_dev. For V to rotate G onto 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 equations:

|V - G| = |V - Z_dev|

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

But, the same situation is true of Y_dev, M and V:

|V - M| = |V - Y_dev|

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

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

Then you just have to find the angle to rotate by, which you should be able to do using the vectors going from V to your corresponding bases (G and Z_dev for me).

Ultimately, I got gummed up towards the end of the algebra in solving for V.. but either way, I think everything you need is here -- maybe you'll have better luck than I did.

Answer 5

What language are you using? If c++, feel free to use my open source library:

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

The short of it is, you'll need to convert your vectors to quaternions, do your calculations, and then convert your quaternion to a transformation matrix.

Here's a 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 the matrix from quaternion:

Matrix nTrans::matFromQuat( cQuat q ) {
    Matrix t;
    q = q.normalized();
    t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
    t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);         
    t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);   
    t.M[0][3] = 0;
    t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);        
    t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) ); 
    t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);         
    t.M[1][3] = 0;
    t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);       
    t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);        
    t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
    t.M[2][3] = 0;
    t.M[3][0] = 0;                  
    t.M[3][1] = 0;                  
    t.M[3][2] = 0;              
    t.M[3][3] = 1;
    return t;
 }

Answer 6

You need to express the orientation of B, with respect to A as a quaternion Q. Then any vector in B can be transformed to a vector in A e.g. by using a rotation matrix R derived from Q. vectorInA = R*vectorInB.

There is a demo script for doing this (including a 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