I am using Tracking Tool 2.3.3 and I find that the yaw-pitch-roll and quaternion are inconsistent.

According to this post: https://forums.naturalpoint.com/viewtopic.php?f=56&t=7834&hilit=orientation, the last response on the first page by NaturalPoint-Brent at 4:23pm 2013 Wed Mar27, yaw is about y axis, pitch about z axis, and roll about x axis. The rotation matrix is computed by

R = Rx*Ry*Rz.

but when I compare the rotation matrix computed using quaternions, call it Rq, I find that Rq != R. The computation of rotation matrix from quaternion is from page 5 of paper https://www.atmos.colostate.edu/ECE555/reading/article_8.pdf, or page 496 of book "Principle of Robot Motion, Theory, Algorithms, and Implementation" by Howie Choset, Kevin Lynch et al.

According to this post: https://forums.naturalpoint.com/viewtopic.php?f=56&t=8381, second response by beckdo at 4:01 2010 Tue Jul 06. The yaw-pitch-roll is computed by the quaternions. I believe the formula is right, because it is consistent with page 26 of this paper: https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf. But using this formula, the resulting yaw-pitch-roll is different from what is exported by TrackingTool.

I used a single frame of the exported csv file. and I used Matlab to obtain the above result.

Below is the code:

- Code: Select all
`qx = -0.42941201;`

qy = 0.10853183;

qz = -0.37245581;

qw = 0.81553835;

% Angle theta is in degrees

% yaw is about y axis

yaw = 35.40805435;

% pitch is about z axis

pitch = 30.9502964;

% roll is about x axis

roll = -65.63815308;

%% Euler angles

theta_y = yaw * pi / 180

theta_z = pitch * pi / 180

theta_x = roll * pi / 180

Ry = [ cos(theta_y) 0 sin(theta_y);

0 1 0 ;

-sin(theta_y) 0 cos(theta_y)];

Rz = [ cos(theta_z) -sin(theta_z) 0 ;

sin(theta_z) cos(theta_z) 0 ;

0 0 1 ];

Rx = [ 1 0 1 ;

0 cos(theta_x) -sin(theta_x);

0 sin(theta_x) cos(theta_x)];

Rtheta = Rx*Rz*Ry

Rtheta_2 = Rx*Ry*Rz

Rtheta_3 = Ry*Rz*Rx

Rtheta_4 = Ry*Rx*Rz

Rtheta_5 = Rz*Ry*Rx

Rtheta_6 = Rz*Rx*Ry

%% Quaternions

q0 = qw;

q1 = qx;

q2 = qy;

q3 = qz;

Rq11 = 2*(q0^2 + q1^2) - 1;

Rq12 = 2*(q1*q2 - q0*q3);

Rq13 = 2*(q1*q3 + q0*q2);

Rq21 = 2*(q1*q2 + q0*q3);

Rq22 = 2*(q0^2 + q2^2) - 1;

Rq23 = 2*(q2*q3 - q0*q1);

Rq31 = 2*(q1*q3 - q0*q2);

Rq32 = 2*(q2*q3 + q0*q1);

Rq33 = 2*(q0^2 + q3^2) - 1;

Rq = [Rq11 Rq12 Rq13;

Rq21 Rq22 Rq23;

Rq31 Rq32 Rq33]

%% quaternion to Euler-angle

q2y = atan2(-Rq13, Rq11)

q2z = asin(Rq12)

q2x = atan2(-Rq32, Rq22)

The resulting R=Rx*Ry*Rz is

- Code: Select all
`Rtheta =`

0.1196 -0.5143 1.3119

-0.3549 0.3538 0.8654

-0.6208 -0.7813 0.0648

and Rq from quaternion is

- Code: Select all
`Rq =`

0.6990 0.5143 0.4969

-0.7007 0.3538 0.6196

0.1429 -0.7813 0.6077

As can be seen Rtheta != Rq. Moreover, no sequence of rotation results in the same rotation matrix as Rq:

- Code: Select all
`Rtheta_2 =`

0.2021 -0.1212 1.3944

-0.2405 0.6252 0.7425

-0.6735 -0.6583 0.3362

Rtheta_3 =

0.6990 -0.7007 0.5561

0.5143 0.3538 1.2955

-0.4969 -0.6196 0.1108

Rtheta_4 =

0.4275 -0.8718 1.0540

0.2121 0.3538 0.9110

-0.8787 -0.3388 -0.2432

Rtheta_5 =

0.6990 -0.6648 0.4355

0.4192 0.0823 1.3233

-0.5794 -0.7425 -0.2432

Rtheta_6 =

0.4735 -0.2121 0.8140

-0.3315 0.3538 1.3539

-0.2390 -0.9110 0.3362

The yaw-pitch-roll in radian are computed as

- Code: Select all
`theta_y =`

0.6180

theta_z =

0.5402

theta_x =

-1.1456

but the converted yaw-pitch-roll from quaternion returns:

- Code: Select all
`q2y =`

-0.6180

q2z =

0.5402

q2x =

1.1456

I think the above suggests either quaternion or yaw-pitch-roll exported by TrackingTool is incorrect. Could Natural Point enlighten me how to handle this please? I would really appreciate it!

I attach the Matlab code.

Thank you!