Jump to content

  • Log In with Google      Sign In   
  • Create Account


bitinf

Member Since 06 Apr 2013
Offline Last Active Apr 08 2013 03:54 PM

Topics I've Started

conversion quaternion <--->euler angles goes wrong

06 April 2013 - 01:28 PM

 

I'm trying to implement the conversion between quaternion and Euler angles. I've referred to some exsiting methods. But the results have "sign(±)" problems. That is, I first convert a quaternion q to Euler angles (pan,tilt,roll) and then computer the quaternion again from the acquired Euler angles. But the result sometimes is -q, and sometimes it's q. Below is my code. In my code, I use a coodinate system with z pointing up. Pan is rotation with respect to y-axis, tilt x-axis, roll z-axis. I apply roll first, then tilt, and last pan. Could anyone tell me what is wrong? Thanks a lot!

 

//from quaterion to pan,tilt,roll

void q2ptr(quaternion q, double &pan, double &tilt, double &roll)

{

  double sqw = q.w * q.w;

  double sqx = q.x * q.x;

  double sqy = q.y * q.y;

  double sqz = q.z * q.z;

  double n2 = sqx + sqy + sqz + sqw;

  double eps = 1.e-7;

  double test = q.x * q.y + q.z * q.w;

  if (test > (0.5-eps) * n2)

     {

       // Singularity at north pole

       roll = 2 * atan2(q.x, q.w); // Yaw

       tilt = M_PI/2; // Pitch pan = 0;

      }

   else if (test < -(0.5-eps) * n2)

   {

      // Singularity at south pole

      roll = -2*atan2(q.x, q.w); // Yaw

      tilt = -M_PI/2; // Pitch pan = 0; // Roll

    }

    else

    {

      roll = atan2(2*(q.w*q.y - q.x*q.z), sqx - sqy - sqz + sqw);

      pan = atan2(2*(q.w*q.x - q.y*q.z), -sqx + sqy - sqz + sqw);

      tilt = asin(2*test/n2);

     }

 }

 

 //from pan,tilt,roll to quaterion void

 ptr2q(double pan, double tilt, double roll, quaternion &q)

 {

   double c1, c2, c3, s1, s2, s3; c1 = cos(roll/2);

   c2 = cos(tilt/2);  c3 = cos(pan/2); s1 = sin(roll/2); s2 = sin(tilt/2); s3 = sin(pan/2);

   q.w = c1*c2*c3 - s1*s2*s3;

   q.x = s1*s2*c3 + c1*c2*s3;

   q.y = s1*c2*c3 + c1*s2*s3;

   q.z = c1*s2*c3 - s1*c2*s3;

   }


PARTNERS