• Create Account

# bitinf

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

### 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