yaw-pitch-roll and quaternion inconsistent

[puffin]

Hi.

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/viewtop ... rientation, 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/ ... icle_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/viewtop ... =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/kapte ... titude.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!

[steven.andrews]

Hello puffin,

Thank you for using OptiTrack and thank you for reaching out to us regarding your questions.

The software you are using, TrackingTools, is the early version of our software and is quite old. It has errors that have since been corrected, and you may be experiencing something related to this. Would it be possible for you to upgrade to the latest working version of Motive?

Overall, it would be a good idea to upgrade to the current version of our software, Motive 1.8 Final. This would require the purchase of a license renewal, but would provide you with the benefits of the latest software including improved usability and data quality, as well as fixes for any problems that existed in the early software. You would also have access to all new releases within the year of your license renewal, so you would continue to benefit from the ongoing improvements being made to the software.

Best regards,
Steven
--
Steven Andrews
OptiTrack | Customer Support Engineer

[puffin]

Hi Steven,

Thank you for your suggestion. I will consider that.

Still, I would like to know if I can trust either the quaternion or the yaw-pitch-roll, or neither. Do you have the answer to that?

Thanks!

[puffin]

Update:

Take away: The quaternion is correct.

Minor finding: The yaw-pitch-roll is incorrect if yaw is about y axis, pitch is about z-axis, and roll is about x-axis, and the rotation is obtained by R = Rx*Rz*Ry.

I verified the correctness of the quaternion by writing a simple animation in Matlab.

[steven.andrews]

Hi puffin,

I'm glad to hear our suggestion of creating a fresh Rigid Body and comparing the 0 rotations to your calculated rotations worked out. Thank you very much for sharing the results.

Cheers,
Steven
--
Steven Andrews
OptiTrack | Customer Support Engineer