Calcolo quaternione per la trasformazione tra 2 cartesiano 3D sistemi di coordinate

Question

ho due sistemi di coordinate cartesiane con vettori unitari noti:

Sistema A (x_A, y_A, Z_A)

e

sistema B (x_B , y_B, z_B)

Entrambi i sistemi condividono la stessa origine (0,0,0). Sto cercando di calcolare un quaternione, in modo che i vettori nel sistema B possano essere espressi nel sistema A.

Ho familiarità con il concetto matematico di quaternioni. Ho già implementato la matematica richiesta da qui: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

Una possibile soluzione potrebbe essere quella di calcolare gli angoli di Eulero e utilizzarli per 3 quaternioni. moltiplicandoli porterebbe ad una finale, in modo da poter trasformare il mio vettori:

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

ma questo sarebbe incorporare ancora giunto cardanico Lock, che era il motivo NON usare gli angoli di Eulero all'inizio.

Qualunque idea di come risolvere questo?

Answer 1

È necessario esprimere l'orientamento di B, rispetto ad A come quaternione Q. Quindi qualsiasi vettore in B può essere trasformato in un vettore in A ad es. usando una matrice di rotazione R derivata da Q. vectorInA = R * vectorInB.

C'è uno script demo per fare questo (tra cui una bella visualizzazione) nella libreria Matlab/Octave disponibili su questo sito: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations

Answer 2

Che lingua stai usando? Se C++, sentitevi liberi di usare la mia libreria open source:

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

Il corto di esso è, è necessario convertire i vettori a quaternioni, fare i vostri calcoli, e poi convertire il quaternione ad una trasformazione matrice.

Ecco un frammento di codice:

Quaternion dal vettore:

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

E per la matrice da quaternione:

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 3

È possibile calcolare il quaternione che rappresenta la migliore trasformazione possibile da una sistema di coordinate a un altro con il metodo descritto in questo documento:

Paul J. Besl e Neil D. McKay "Metodo per la registrazione di forme 3D", Sensor Fusion IV: Paradigmi di controllo e strutture dati, 586 (30 aprile 1992); http://dx.doi.org/10.1117/12.57955

La carta non è l'accesso aperto, ma posso mostrarvi l'implementazione di Python:

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 

Questa funzione restituisce il quaternione che stavate cercando. Gli argomenti lst1 e lst2 sono elenchi di numpy.array in cui ogni matrice rappresenta un vettore 3D. Se entrambe le liste sono di lunghezza 3 (e contengono vettori di unità ortogonali), il quaternione dovrebbe essere la trasformazione esatta. Se fornisci elenchi più lunghi, ottieni il quaternione che riduce al minimo la differenza tra entrambi i gruppi di punti. L'argomento della lista di corrispondenze opzionale viene utilizzato per indicare alla funzione quale punto di lst2 deve essere trasformato in quale punto in lst1. Se non viene fornito matchlist, la funzione presuppone che il primo punto in LST1 dovrebbe corrispondere al primo punto LST2 ecc ...

Una funzione analoga per gruppi di 3 punti in C++ è il seguente:

#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

Mi sono imbattuto in questo stesso problema. Ero sulla strada per una soluzione, ma mi sono bloccato.

Quindi, avrete bisogno di due vettori noti in entrambi i sistemi di coordinate. Nel mio caso, ho 2 vettori ortonormali nel sistema di coordinate di un dispositivo (gravità e campo magnetico), e voglio trovare il quaternione per ruotare dalle coordinate del dispositivo all'orientamento globale (dove Nord è positivo Y, e "su" è Z positivo). Quindi, nel mio caso, ho misurato i vettori nello spazio delle coordinate del dispositivo, e io sono che definisce gli stessi vettori per formare la base ortonormale per il sistema globale.

Con ciò detto, considerare l'interpretazione dell'angolo-asse dei quaternioni, esiste un vettore V su cui le coordinate del dispositivo possono essere ruotate di un angolo per far corrispondere le coordinate globali. Chiamerò il mio vettore di gravità (negativo) G e il campo magnetico M (entrambi sono normalizzati).

V, G e M descrivono tutti i punti sulla sfera dell'unità. Così fa Z_dev e Y_dev (le basi Z e Y per il sistema di coordinate del mio dispositivo). L'obiettivo è trovare una rotazione che mappa G su Z_dev e M su Y_dev. Affinché V ruoti G su Z_dev, la distanza tra i punti definiti da G e V deve essere uguale alla distanza tra i punti definiti da V e Z_dev. In equazioni:

| V - G | = | V - Z_dev |

La soluzione a questa equazione forma un piano (tutti i punti equidistanti da G e Z_dev). Ma, V è vincolato ad essere una lunghezza unitaria, il che significa che la soluzione è un anello centrato sull'origine - ancora un numero infinito di punti.

Ma, la stessa situazione vale per Y_dev, M e V:

| V - M | = | V - Y_dev |

La soluzione a questo è anche un anello centrato sull'origine. Questi anelli hanno due punti di intersezione, dove uno è il negativo dell'altro. O è un asse di rotazione valido (l'angolo di rotazione sarà solo negativo in un caso).

Usando i due equazioni di cui sopra, e il fatto che ciascuno di questi vettori è lunghezza unitaria si dovrebbe essere in grado di risolvere per V.

Poi basta per trovare l'angolo di rotazione da che si dovrebbe essere in grado di fare usando i vettori che vanno da V alle basi corrispondenti (G e Z_dev per me).

Alla fine, mi sono fatto carico di gomme verso la fine dell'algebra per risolvere V .. ma in entrambi i casi, penso che tutto ciò che serve sia qui - forse avrò più fortuna di me.