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