Tracking Tools Streaming Quaternions

smith32
Posts: 11
Joined: Wed Mar 24, 2010 2:13 pm

Tracking Tools Streaming Quaternions

Post by smith32 » Thu Jul 01, 2010 5:34 pm

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 real-time 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

beckdo
NaturalPoint Employee
NaturalPoint Employee
Posts: 519
Joined: Tue Jan 02, 2007 2:02 pm

Re: Tracking Tools Streaming Quaternions

Post by beckdo » Tue Jul 06, 2010 4:01 pm

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

sprussell
Posts: 2
Joined: Wed Jul 07, 2010 5:22 pm

Re: Tracking Tools Streaming Quaternions

Post by sprussell » Wed Jul 07, 2010 7:28 pm

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 Real-Time Info was displaying, unfortunately.

After experimenting a little I'm curious if the angles being displayed in the Real-Time Info are traditional Euler angles, i.e. representing a rotation from inertial to body fixed coordinates via a yaw-pitch-roll sequenced rotation. If we took our trackable and then rotated in that sequence, the angles in Real-Time 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 yaw-pitch-roll computed correspond to rotations about Y-X-Z, as opposed to the conventional Z-Y-X. This is commensurate with the frame displayed in TrackingTools, i.e. Y up and X-Z 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 roll-pitch-yaw convention that TrackingTools is assuming in the Real-Time Info and if that's different than what we're used to?

litch09
Posts: 32
Joined: Tue Mar 10, 2009 6:28 pm

Re: Tracking Tools Streaming Quaternions

Post by litch09 » Thu Jul 08, 2010 4:15 pm

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.

dadams
Posts: 2
Joined: Fri Jul 10, 2009 1:49 pm

Re: Tracking Tools Streaming Quaternions

Post by dadams » Thu Aug 12, 2010 7:38 am

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.

bidwej
Posts: 1
Joined: Tue Mar 16, 2010 9:12 am

Re: Tracking Tools Streaming Quaternions

Post by bidwej » Sat Aug 27, 2011 3:10 pm

Thank you for the solution; switching qy and qz worked for us.

Naturalpoint tech-support - 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 left-handed 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 - Mike
Posts: 1896
Joined: Tue Feb 01, 2011 8:41 am
Location: Corvallis, OR

Re: Tracking Tools Streaming Quaternions

Post by NaturalPoint - Mike » Mon Aug 29, 2011 2:04 pm

Thanks for the suggestion. I'm currently undertaking a massive documentation project, and a writeup on quaternions would make an excellent addition to these.

chariths
Posts: 1
Joined: Wed Dec 14, 2011 1:28 am
Location: Tokyo, Japan

Re: Tracking Tools Streaming Quaternions

Post by chariths » Wed Dec 14, 2011 1:45 am

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" -> "Real-time 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);
}

NaturalPoint - Mike
Posts: 1896
Joined: Tue Feb 01, 2011 8:41 am
Location: Corvallis, OR

Re: Tracking Tools Streaming Quaternions

Post by NaturalPoint - Mike » Mon Dec 19, 2011 3:15 pm

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: Left-handed or right-handed coordinate system, absolute or relative angles, and what axis corresponds to what Euler angle.

gsolman
Posts: 1
Joined: Thu Oct 04, 2012 12:15 pm

Re: Tracking Tools Streaming Quaternions

Post by gsolman » Thu Oct 04, 2012 12:22 pm

Following up on this � just a couple quick questions about the quaternion output:

1) right-handed, presumably?

2) qw in radians?

Thanks

Post Reply