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)
Code: Select all
Rtheta =
0.1196 -0.5143 1.3119
-0.3549 0.3538 0.8654
-0.6208 -0.7813 0.0648
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
Code: Select all
theta_y =
0.6180
theta_z =
0.5402
theta_x =
-1.1456
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!