Obliczanie kwaternion transformacji pomiędzy 2 3D kartezjańskim układzie współrzędnych

Question

że dwa kartezjańskie układy współrzędnych ze znanymi wektor jednostkowy:

System A (x_A, y_A, z_A)

i

System B (x_B , y_B, z_B)

Oba systemy mają to samo pochodzenie (0,0,0). Próbuję obliczyć kwaternion, aby wektory w systemie B mogły być wyrażone w systemie A.

Jestem zaznajomiony z matematyczną koncepcją kwaternionów. Mam już zaimplementowaną wymaganą matematykę stąd: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

Jednym z możliwych rozwiązań może być obliczenie kątów Eulera i wykorzystanie ich na 3 kwartały. ich pomnożenie doprowadzi do ostatecznego jednym, tak że mogę przekształcić moje wektory:

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

Ale to byłoby włączyć ponownie przegubowego Lock, co było powodem NIE używać na początku kątów Eulera.

Wszelkie idead, jak rozwiązać ten problem?

Answer 1

Musisz wyrazić orientację B, w odniesieniu do A jako quaternion Q. Następnie dowolny wektor w B może zostać przekształcony do wektora w A, np. za pomocą macierzy rotacji R uzyskanej z Q. vectorInA = R * vectorInB.

Istnieje skrypt demo dla tej operacji (w tym ładnym wizualizacji) w bibliotece Matlab/Octave dostępne na tej stronie: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations

Answer 2

Jakiego języka używasz? Jeśli C++, zachęcamy do korzystania z mojego Open Library źródło:

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

Krótki z to jest, trzeba konwertować wektory do kwaterniony, robić swoje obliczenia, a następnie przekształcić kwaternion do transformacji matryca.

Oto fragment kodu:

Kwaterniony z wektorem:

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

A dla macierzy z kwaterniony:

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

można obliczyć kwaternion reprezentującej najlepszą możliwą przemianę z jednego układ współrzędnych do innego metodą opisaną w tym dokumencie:

Paul J. Besl i Neil D. McKay "Metoda rejestracji kształtów 3D", Sensor Fusion IV: Paradygmaty sterowania i struktury danych, 586 (30 kwietnia 1992 r.); http://dx.doi.org/10.1117/12.57955

Papier nie jest otwarty dostęp, ale mogę pokazać realizację 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 

ta funkcja zwraca kwaternion że szukaliśmy. Argumenty lst1 i lst2 są listami numpy.tablice, w których każda tablica reprezentuje wektor 3D. Jeżeli obie listy mają długość 3 (i zawierają ortogonalne wektory jednostkowe), kwaternionem powinna być dokładna transformacja. Jeśli podajesz dłuższe listy, otrzymujesz kwaternion, który minimalizuje różnicę między obydwoma zestawami punktów. Opcjonalny argument listy meczowej służy do informowania funkcji, który punkt lst2 powinien zostać przekształcony, do którego punktu w lst1. Jeśli nie matchlist jest funkcja zakłada, że ​​pierwszy punkt w lst1 powinien pasować pierwszy punkt w lst2 i tak dalej ...

Podobna funkcja do zestawów 3 punkty w C++ jest następujący:

#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

Po prostu wpadłem na ten sam problem. Byłem na dobrej drodze do rozwiązania, ale utknąłem.

Potrzebne są DWIE wektory, które są znane w obu układach współrzędnych. W moim przypadku mam 2 ortonormalne wektory w układzie współrzędnych urządzenia (pole grawitacyjne i magnetyczne), i chcę znaleźć kwaternion do obrócenia od współrzędnych urządzenia do globalnej orientacji (gdzie północ jest dodatnia Y, a "w górę" jest dodatni Z). Tak więc, w moim przypadku, zmierzyłem wektory w przestrzeni współrzędnych urządzenia, i jestem definiująca same wektory w celu utworzenia podstawy ortonormalnej dla systemu globalnego.

Mając to na uwadze, weź pod uwagę interpretację kąta osi kwaternionów, jest tam pewien wektor V, wokół którego można obrócić współrzędne urządzenia o pewien kąt, aby dopasować współrzędne globalne. Nazwę mój (ujemny) wektor grawitacji G, a pole magnetyczne M (oba są znormalizowane).

V, G i M wszystkie opisują punkty na jednostce sfera. Podobnie jak Z_dev i Y_dev (podstawy Z i Y układu współrzędnych mojego urządzenia). Celem jest znalezienie rotacji, która mapuje G na Z_dev i M na Y_dev. Aby V obróciło G na Z_dev, odległość między punktami zdefiniowanymi przez G i V musi być taka sama, jak odległość między punktami zdefiniowanymi przez V i Z_dev. W równaniach:

| V - G | = | V - Z_dev |

Rozwiązanie tego równania tworzy płaszczyznę (wszystkie punkty są w równej odległości od G i Z_dev). Ale V jest ograniczony do długości jednostki, co oznacza, że ​​rozwiązaniem jest pierścień skupiony na początku - wciąż nieskończona liczba punktów.

Ale taka sama sytuacja jest w przypadku Y_dev, M i V:

| V - M | = | V - Y_dev |

Rozwiązaniem tego problemu jest również pierścień skupiony na pochodzeniu. Pierścienie te mają dwa punkty przecięcia, gdzie jeden jest ujemny drugiego. Albo jest prawidłową osią obrotu (kąt obrotu będzie w każdym przypadku ujemny).

Wykorzystując dwa równania powyższe oraz fakt, że każdy z tych wektorów jest długość urządzenie powinno być w stanie rozwiązać za V.

Wtedy po prostu trzeba znaleźć kąt do obracania przez, które powinny być w stanie zrobić używając wektorów idących od V do odpowiednich baz (dla mnie G i Z_dev).

Ostatecznie, pod koniec algebry zaplątałem się w rozwiązywanie V .. ale tak czy inaczej, myślę, że wszystko, czego potrzebujesz, jest tutaj - może będziesz miał więcej szczęścia niż ja.