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

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 2

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;
 }