Hi,
I'm using TrackingTools (most recent version) to track a rigid body and stream its position and orientation using NatNet to my own custom software. I'm receiving the quaternions properly but I'm unable to get the Euler angles to match up with the ones shown in the realtime box of the TrackingTools software when I use the quaternion conversion code given here Quaternion Conversion. Nominally around roll, pitch and yaw angles of 0 degrees my software and the TrackingTools coincide but once I rotate about all axis the two sets of angles have larger errors. Sometimes one or two of the angles match but the third is way off. Also, if I take the body and make a large rotation about one axis it seems to match. I'm trying to figure out where the error is coming from, is it some rounding error in the TrackingTools software or in my software? Would it be possible to post the code that TrackingTools uses for the conversion (if it's different from the one I linked to)?
Thanks for your help, I really appreciate it.
Andrew
Tracking Tools Streaming Quaternions
Re: Tracking Tools Streaming Quaternions
For a given rigid body, it's orientation (which is a quaternion) is converted to an orientation matrix and then to yaw pitch at roll. For the quaternion to orientation matrix:
(where q is the quat and m is the 3x3 orientation matrix)
{
double sqw = q.w*q.w;
double sqx = q.x*q.x;
double sqy = q.y*q.y;
double sqz = q.z*q.z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
m[0] = ( sqx  sqy  sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
m[4] = (sqx + sqy  sqz + sqw)*invs ;
m[8] = (sqx  sqy + sqz + sqw)*invs ;
double tmp1 = q.x*q.y;
double tmp2 = q.z*q.w;
m[3] = 2.0 * (tmp1 + tmp2)*invs ;
m[1] = 2.0 * (tmp1  tmp2)*invs ;
tmp1 = q.x*q.z;
tmp2 = q.y*q.w;
m[6] = 2.0 * (tmp1  tmp2)*invs ;
m[2] = 2.0 * (tmp1 + tmp2)*invs ;
tmp1 = q.y*q.z;
tmp2 = q.x*q.w;
m[7] = 2.0 * (tmp1 + tmp2)*invs ;
m[5] = 2.0 * (tmp1  tmp2)*invs ;
}
then once the orientation matrix is in hand, the yaw, pitch roll is calculated from it: (m being a 3x3 orientation matrix)
{
yaw = atan2(m[6],m[0]);
pitch = asin(m[3]);
roll = atan2(m[5],m[4]);
}
(where q is the quat and m is the 3x3 orientation matrix)
{
double sqw = q.w*q.w;
double sqx = q.x*q.x;
double sqy = q.y*q.y;
double sqz = q.z*q.z;
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
m[0] = ( sqx  sqy  sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
m[4] = (sqx + sqy  sqz + sqw)*invs ;
m[8] = (sqx  sqy + sqz + sqw)*invs ;
double tmp1 = q.x*q.y;
double tmp2 = q.z*q.w;
m[3] = 2.0 * (tmp1 + tmp2)*invs ;
m[1] = 2.0 * (tmp1  tmp2)*invs ;
tmp1 = q.x*q.z;
tmp2 = q.y*q.w;
m[6] = 2.0 * (tmp1  tmp2)*invs ;
m[2] = 2.0 * (tmp1 + tmp2)*invs ;
tmp1 = q.y*q.z;
tmp2 = q.x*q.w;
m[7] = 2.0 * (tmp1 + tmp2)*invs ;
m[5] = 2.0 * (tmp1  tmp2)*invs ;
}
then once the orientation matrix is in hand, the yaw, pitch roll is calculated from it: (m being a 3x3 orientation matrix)
{
yaw = atan2(m[6],m[0]);
pitch = asin(m[3]);
roll = atan2(m[5],m[4]);
}
Re: Tracking Tools Streaming Quaternions
Thanks for your response Doug. Andrew (smith32) is out of town, so I've taken over trying to figure this out. I patched in the code you posted but that still didn't seem to produce output matching what the RealTime Info was displaying, unfortunately.
After experimenting a little I'm curious if the angles being displayed in the RealTime Info are traditional Euler angles, i.e. representing a rotation from inertial to body fixed coordinates via a yawpitchroll sequenced rotation. If we took our trackable and then rotated in that sequence, the angles in RealTime Info did not correspond with the individual rotations.
After hacking around and reverse engineering things a bit, we were able to come up with a transformation that was consistent with our understanding of Euler angles. We were able to convert to roll, pitch, yaw angles using the conversion we were familiar with (e.g. presented here: http://en.wikipedia.org/wiki/Conversion ... ler_angles), where q0 = qw, q1 = qx, q2 = qy, and q3 = qz. However, this transformation is slightly confusing because the yawpitchroll computed correspond to rotations about YXZ, as opposed to the conventional ZYX. This is commensurate with the frame displayed in TrackingTools, i.e. Y up and XZ in the plane (set using the ground plane tool).
So, I think we've found a workable solution but I'm curious if you have any ideas about the rollpitchyaw convention that TrackingTools is assuming in the RealTime Info and if that's different than what we're used to?
After experimenting a little I'm curious if the angles being displayed in the RealTime Info are traditional Euler angles, i.e. representing a rotation from inertial to body fixed coordinates via a yawpitchroll sequenced rotation. If we took our trackable and then rotated in that sequence, the angles in RealTime Info did not correspond with the individual rotations.
After hacking around and reverse engineering things a bit, we were able to come up with a transformation that was consistent with our understanding of Euler angles. We were able to convert to roll, pitch, yaw angles using the conversion we were familiar with (e.g. presented here: http://en.wikipedia.org/wiki/Conversion ... ler_angles), where q0 = qw, q1 = qx, q2 = qy, and q3 = qz. However, this transformation is slightly confusing because the yawpitchroll computed correspond to rotations about YXZ, as opposed to the conventional ZYX. This is commensurate with the frame displayed in TrackingTools, i.e. Y up and XZ in the plane (set using the ground plane tool).
So, I think we've found a workable solution but I'm curious if you have any ideas about the rollpitchyaw convention that TrackingTools is assuming in the RealTime Info and if that's different than what we're used to?
Re: Tracking Tools Streaming Quaternions
I have also had problems understanding the euler angles output from tracking tools, and my calculations often come up with a different solution, which I assume is due to a different rotation sequence that I am using (although I have tried many!). I have asked the question before, but thought it an apt time to try again.... what is the rotation sequence used in to calculate the euler angles in tracking tools?
I have routines which calculate euler angles in the convention i am used to, but currently I have to do two conversions to get to this 1) quaternions to direction cosine (orientation) matrix 2) orientation matrix to euler angles. Would it be possible for Tracking Tools software to output the orientation matrix?? This would make it particularly easy for animation purposes.
I have routines which calculate euler angles in the convention i am used to, but currently I have to do two conversions to get to this 1) quaternions to direction cosine (orientation) matrix 2) orientation matrix to euler angles. Would it be possible for Tracking Tools software to output the orientation matrix?? This would make it particularly easy for animation purposes.
Re: Tracking Tools Streaming Quaternions
We had a similar issue, it was caused by pitch/yaw returned from tracking tools opposite than what we expected in our quaternion implementation.
Our quat when converted to euler uses x as roll, y as pitch, z as yaw, while tracking tools uses x as roll, y as yaw and z as pitch.
so we solved this by basically swapping qy and qz:
MyQuaternion quat(RigidBodyData.qw,RigidBodyData.qx,RigidBodyData.qz,RigidBodyData.qy);
Depending on which rotation is positive or negative, you may have to do some negation.
Our quat when converted to euler uses x as roll, y as pitch, z as yaw, while tracking tools uses x as roll, y as yaw and z as pitch.
so we solved this by basically swapping qy and qz:
MyQuaternion quat(RigidBodyData.qw,RigidBodyData.qx,RigidBodyData.qz,RigidBodyData.qy);
Depending on which rotation is positive or negative, you may have to do some negation.
Re: Tracking Tools Streaming Quaternions
Thank you for the solution; switching qy and qz worked for us.
Naturalpoint techsupport  I appreciate your recent help in troubleshooting this issue. I thought you should know that the Simon Baker quaternion code you pointed us to had this exact problem, ie: I suspect they will have similar trouble if they are pointed to this resource without the switching qy and qz fix.
In the same spirit I would also recommend adding a brief addendum to the Tracking Tools API documentation that contains:
1) quaternion to matrix code (posted elsewhere on the forum)
2) the answer to the problem referenced above
3) a definite answer on the order that your lefthanded euler angles are applied in the tracking tools API
In our case; the fix required two days of stepping through our code until finding this post (also during a project deadline)
My colleagues each have a different "home brew" solution for lining up coordinate system given euler angles. It often takes new people hours of trial and error to fix related problems when calibrate settings change.
I know others would appreciate this information; I have read numerous questions related to euler angles and quaterions on your forum. More specifically, the people you would really be helping in this case are graduate students(professors anticipate a system like this to be easy to use; for all intents and purposes they are right; however there remains a critical lack of information in key areas)
Naturalpoint techsupport  I appreciate your recent help in troubleshooting this issue. I thought you should know that the Simon Baker quaternion code you pointed us to had this exact problem, ie: I suspect they will have similar trouble if they are pointed to this resource without the switching qy and qz fix.
In the same spirit I would also recommend adding a brief addendum to the Tracking Tools API documentation that contains:
1) quaternion to matrix code (posted elsewhere on the forum)
2) the answer to the problem referenced above
3) a definite answer on the order that your lefthanded euler angles are applied in the tracking tools API
In our case; the fix required two days of stepping through our code until finding this post (also during a project deadline)
My colleagues each have a different "home brew" solution for lining up coordinate system given euler angles. It often takes new people hours of trial and error to fix related problems when calibrate settings change.
I know others would appreciate this information; I have read numerous questions related to euler angles and quaterions on your forum. More specifically, the people you would really be helping in this case are graduate students(professors anticipate a system like this to be easy to use; for all intents and purposes they are right; however there remains a critical lack of information in key areas)

 Posts: 1896
 Joined: Tue Feb 01, 2011 8:41 am
 Location: Corvallis, OR
Re: Tracking Tools Streaming Quaternions
Thanks for the suggestion. I'm currently undertaking a massive documentation project, and a writeup on quaternions would make an excellent addition to these.
Re: Tracking Tools Streaming Quaternions
I was trying to generate the Rotation matrix and RPY extract from Rotation matrix. Non of this works and @dadams post was really helpful. It uses a different convention. Usually we consider x as roll, y as pitch, z as yaw, while tracking tools uses x as roll, y as yaw and z as pitch.
Also the "Tracking Tools" > "Realtime info" outputs the correct data, but "NatNet Managed Client Sample" Program outputs a wrong Roll, Pitch, Yaw.
So to avoid confusion and to make it simple i will drop the correct way of creating the conventional Rotation matrix and extract RPY in the conventional method.
[color:#3366FF]for[/color]([color:#3366FF]int[/color] i=0; i nRigidBodies; i++){
trackables.pos[0] = data>RigidBodies.z*1000;
trackables.pos[1] = data>RigidBodies.x*1000;
trackables.pos[2] = data>RigidBodies.y*1000;
[color:#3366FF]double[/color] sqx = data>RigidBodies.qx*data>RigidBodies.qx;
[color:#3366FF]double[/color] sqy = data>RigidBodies.qy*data>RigidBodies.qy;
[color:#3366FF]double[/color] sqz = data>RigidBodies[i].qz*data>RigidBodies[i].qz;
[color:#3366FF]double[/color] sqw = data>RigidBodies[i].qw*data>RigidBodies[i].qw;
[color:#33CC00]// invs (inverse square length) is only required if quaternion is not already normalised[/color]
[color:#3366FF]double[/color] invs = 1 / (sqx + sqy + sqz + sqw);
trackables[i].rotMatrix[0] = ( sqx  sqy  sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
trackables[i].rotMatrix[4] = (sqx  sqy + sqz + sqw)*invs ;
trackables[i].rotMatrix[8] = (sqx + sqy  sqz + sqw)*invs ;
[color:#3366FF]double[/color] tmp1 = data>RigidBodies[i].qx*data>RigidBodies[i].qz;
[color:#3366FF]double[/color] tmp2 = data>RigidBodies[i].qy*data>RigidBodies[i].qw;
trackables[i].rotMatrix[3] = 2.0 * (tmp1 + tmp2)*invs ;
trackables[i].rotMatrix[1] = 2.0 * (tmp1  tmp2)*invs ;
tmp1 = data>RigidBodies[i].qx*data>RigidBodies[i].qy;
tmp2 = data>RigidBodies[i].qz*data>RigidBodies[i].qw;
trackables[i].rotMatrix[6] = 2.0 * (tmp1  tmp2)*invs ;
trackables[i].rotMatrix[2] = 2.0 * (tmp1 + tmp2)*invs ;
tmp1 = data>RigidBodies[i].qz*data>RigidBodies[i].qy;
tmp2 = data>RigidBodies[i].qx*data>RigidBodies[i].qw;
trackables[i].rotMatrix[7] = 2.0 * (tmp1 + tmp2)*invs ;
trackables[i].rotMatrix[5] = 2.0 * (tmp1  tmp2)*invs ;
trackables[i].euler[0] = atan2(trackables[i].rotMatrix[7], trackables[i].rotMatrix[8]); [color:#33CC00]// x[/color]
trackables[i].euler[1] = asin(trackables[i].rotMatrix[6]); [color:#33CC00]// y [/color]
trackables[i].euler[2] = atan2(trackables[i].rotMatrix[3], trackables[i].rotMatrix[0]); [color:#33CC00]// z[/color]
}
[color:#33CC00]// Rigid Bodies data print[/color]
printf([color:#CC0000]"\t\tx\ty\tz\trx\try\trz\n"[/color]);
for(int i=0; i nRigidBodies; i++){
printf([color:#CC0000]"RBody [ID=%d] \t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n"[/color],
data>RigidBodies[i].ID,
trackables[i].pos[0],
trackables[i].pos[1],
trackables[i].pos[2],
trackables[i].euler[0]*RADTODEG,
trackables[i].euler[1]*RADTODEG,
trackables[i].euler[2]*RADTODEG);
}
Also the "Tracking Tools" > "Realtime info" outputs the correct data, but "NatNet Managed Client Sample" Program outputs a wrong Roll, Pitch, Yaw.
So to avoid confusion and to make it simple i will drop the correct way of creating the conventional Rotation matrix and extract RPY in the conventional method.
[color:#3366FF]for[/color]([color:#3366FF]int[/color] i=0; i nRigidBodies; i++){
trackables.pos[0] = data>RigidBodies.z*1000;
trackables.pos[1] = data>RigidBodies.x*1000;
trackables.pos[2] = data>RigidBodies.y*1000;
[color:#3366FF]double[/color] sqx = data>RigidBodies.qx*data>RigidBodies.qx;
[color:#3366FF]double[/color] sqy = data>RigidBodies.qy*data>RigidBodies.qy;
[color:#3366FF]double[/color] sqz = data>RigidBodies[i].qz*data>RigidBodies[i].qz;
[color:#3366FF]double[/color] sqw = data>RigidBodies[i].qw*data>RigidBodies[i].qw;
[color:#33CC00]// invs (inverse square length) is only required if quaternion is not already normalised[/color]
[color:#3366FF]double[/color] invs = 1 / (sqx + sqy + sqz + sqw);
trackables[i].rotMatrix[0] = ( sqx  sqy  sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs
trackables[i].rotMatrix[4] = (sqx  sqy + sqz + sqw)*invs ;
trackables[i].rotMatrix[8] = (sqx + sqy  sqz + sqw)*invs ;
[color:#3366FF]double[/color] tmp1 = data>RigidBodies[i].qx*data>RigidBodies[i].qz;
[color:#3366FF]double[/color] tmp2 = data>RigidBodies[i].qy*data>RigidBodies[i].qw;
trackables[i].rotMatrix[3] = 2.0 * (tmp1 + tmp2)*invs ;
trackables[i].rotMatrix[1] = 2.0 * (tmp1  tmp2)*invs ;
tmp1 = data>RigidBodies[i].qx*data>RigidBodies[i].qy;
tmp2 = data>RigidBodies[i].qz*data>RigidBodies[i].qw;
trackables[i].rotMatrix[6] = 2.0 * (tmp1  tmp2)*invs ;
trackables[i].rotMatrix[2] = 2.0 * (tmp1 + tmp2)*invs ;
tmp1 = data>RigidBodies[i].qz*data>RigidBodies[i].qy;
tmp2 = data>RigidBodies[i].qx*data>RigidBodies[i].qw;
trackables[i].rotMatrix[7] = 2.0 * (tmp1 + tmp2)*invs ;
trackables[i].rotMatrix[5] = 2.0 * (tmp1  tmp2)*invs ;
trackables[i].euler[0] = atan2(trackables[i].rotMatrix[7], trackables[i].rotMatrix[8]); [color:#33CC00]// x[/color]
trackables[i].euler[1] = asin(trackables[i].rotMatrix[6]); [color:#33CC00]// y [/color]
trackables[i].euler[2] = atan2(trackables[i].rotMatrix[3], trackables[i].rotMatrix[0]); [color:#33CC00]// z[/color]
}
[color:#33CC00]// Rigid Bodies data print[/color]
printf([color:#CC0000]"\t\tx\ty\tz\trx\try\trz\n"[/color]);
for(int i=0; i nRigidBodies; i++){
printf([color:#CC0000]"RBody [ID=%d] \t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n"[/color],
data>RigidBodies[i].ID,
trackables[i].pos[0],
trackables[i].pos[1],
trackables[i].pos[2],
trackables[i].euler[0]*RADTODEG,
trackables[i].euler[1]*RADTODEG,
trackables[i].euler[2]*RADTODEG);
}

 Posts: 1896
 Joined: Tue Feb 01, 2011 8:41 am
 Location: Corvallis, OR
Re: Tracking Tools Streaming Quaternions
The key thing to note about Euler angles is that you have to make some assumptions. If you have different assumptions the output will appear 'wrong' to you. The assumptions boil down to this: Lefthanded or righthanded coordinate system, absolute or relative angles, and what axis corresponds to what Euler angle.
Re: Tracking Tools Streaming Quaternions
Following up on this ï¿½ just a couple quick questions about the quaternion output:
1) righthanded, presumably?
2) qw in radians?
Thanks
1) righthanded, presumably?
2) qw in radians?
Thanks