Changement de repere et quaternions

Question

Bonjour,
J'essaie désespérément d'effectuer un changement de repère entre deux capteurs. Chacun a 6 coordonnées absolues (x,y,z, Azimuth(rotation sur z), Elevation (rotation sur y), Roll (rotation sur x)). Mon objectif est d'exprimer les 6 coordonnées d'un capteur en fonction du 2e. Voici comment je procède:

On a:
Capteur 1 : x1,y1,z1,A1,E1,R1
Capteur 2 : x2,    ...,          R2

Mon programme: (C++)

P = (x2,y2,z2)
Pref =(x1,y1,z1)

O = quaternion obtenu à partir des angles d'Euler du capteur 2
Oref = idem pour le capteur 1

Où, pour des angles (A,E,R) le quaternion d'orientation O est ainsi obtenu (formule directement pompée d'internet)
O (a,b,c,d)
où
a = (cos(R/2) * cos(E/2) * cos(A/2) + sin(R/2) * sin(E/2) * sin(A/2)),
b = (sin(R/2) * cos(E/2) * cos(A/2) - cos(R/2) * sin(E/2) * sin(A/2)),
c = (cos(R/2) * sin(E/2) * cos(A/2) + sin(R/2) * cos(E/2) * sin(A/2)),
d = (cos(R/2) * cos(E/2) * sin(A/2) - sin(R/2) * sin(E/2) * cos(A/2))

Oref_inverse = inverse(Oref);
trans = P-Pref
Qtrans = (0,trans.x, trans.y, trans.z) //quaternion obtenu à partir du veteur trans.
Qtrans = Oref_inverse * Qtrans * Oref

//OBTENTION DE LA TRANSLATION
trans = (Qtrans.i, Qtrans.j, Qtrans.k) //vecteur des 3 composantes complexes de Qtrans

//OBTENTION DES ANGLES
O = Oref_inverse*O;
et on extrait les trois angles d'Euler à partir de O

Le pb est que les résultats sont faux: si je fixe les 2 capteurs sur un solide et que je bouge le solide, les coordonnées relatives devraient être fixes (les 2 cpateurs étant fixes l'un par rapport à l'autre), or elles ne le sont pas... ='(

SVP, je suis en train de péter les plombs, dites-moi ce qui ne va pas!!!

Merci bcp d'avance !

feadin91 · Answer

Wahou, c'est du lourd ça!

Tu n'as pas copié la fonction inverse(quaternion); 
Le problème vient peut-être de là? 

Tu as des warning lors de la compilation? (moi j'en ignore certains XD j'ai même pas honte)

ebousquie · Answer

ouais, merci... il semble que l'inversion soit bonne... d'ailleurs autant pour moi c'est pas inversion c'est complémentation (on prend le conjugué)

feadin91 · Answer

Tu peux la mettre ici aussi stp? (on sait jamais...) avec un commentaire bref sur son algo si possible

EDIT : 
trans = P-Pref 
Qtrans = (0,trans.x, trans.y, trans.z) //quaternion obtenu à partir du veteur trans. 
Qtrans = Oref_inverse * Qtrans * Oref 

//OBTENTION DE LA TRANSLATION 
trans = (Qtrans.i, Qtrans.j, Qtrans.k) //vecteur des 3 composantes complexes de Qtrans

Il n'y a pas une petite redondance? Si j'ai bien compris, tu calcule trans, tu "colle" ses paramètres dans Qtrans, puis tu "colle" les paramètres de Qtrans dans trans?

C'est normal?

ebousquie · Answer

//Conjugué

inline Quaternion& Quaternion::complement()
{
  a = a;
  b = -b;
  c = -c;
  d = -d;
  return *this;
}

// Inversion

inline Quaternion& Quaternion::invert()
{
  double norm = a*a+b*b+c*c+d*d;
  complement();
  *this /= norm;

  return *this;
}

// dans le main: 

Remplacer Oref_inverse par Oref_complement
puis calcul: Qtrans = Oref_complement * Qtrans * (Oref_complement)^-1

feadin91 · Answer

Et ça fonctionne avec tes modifications?

Sinon, bah je vois pas du tout (je suis un peu paumé :S)

ebousquie · Answer

La redondance c'est pour effectuer des calculs avec des quaternions et non avec des vecteurs, d'où la conversion "aller-retour" Vect -> Quat -> Vect

... et ça ne fonctionne pas. Déjà, si la démarche est la bonne, c'est cool. Mais je ne sais pas quoi corriger d'autre.

feadin91 · Answer

Ah d'accord... Et bien désolé, je ne vois pas du tout. Faut dire que je suis pas dans le contexte, j'ai du mal à saisir ^^

Tu n'as pas des collègues avec qui tu peux essayer de résoudre le problème? 

Moi je passe mon tour :D (désolé encore)

Changement de repere et quaternions

7 réponses

Votre réponse

Newsletters