Page 2 of 2

Re: Help on rotation using quaternions or YPR

Posted: Wed Apr 03, 2013 11:13 am
by lightyear66
I wanted to post a more specific solution to the OptiTrack environment. I am using Matlab, but an implementation in C/C++ should be easily derived from the below results.

If using the API output, I understand that the convention is for a left-handed coordinate system. OptiTrack has C++ code (markers.cpp) equivalent to the LHS Matlab code below.

*******************************************
function [transform_matrix_local_to_global, transform_matrix_inverse_global_to_local] = Transform_CoordinatesOptitrack(roll, pitch, yaw, x, y, z)
% Find the rotation matrices to transform back an forth from the orientation of the global camera coordinate system to the local trackable coordinate system
% roll, pitch, and yaw are input in degrees
% The Optitrack system reports the rotation angles in the local coordinate system of the tracked object
% Assumes Left-handed coordinate system inputs
% convert units from degrees to radians for use in Matlab trignometry functions
degtorad = -1*pi/180; % there are pi (3.141592653589793) radians for every 180 degrees
roll = roll*degtorad;
pitch = pitch*degtorad;
yaw = yaw*degtorad;

% find the rotation portion of the transformation matrix from local to global coordinates
Rx = [1 0 0 0; ...
0 cos(roll) -sin(roll) 0; ...
0 sin(roll) cos(roll) 0; ...
0 0 0 1]; % roll [row1; row2; row3; row4]
Rz = [cos(pitch) -sin(pitch) 0 0; ...
sin(pitch) cos(pitch) 0 0; ....
0 0 1 0; ...
0 0 0 1]; % pitch [row1; row2; row3; row4]
Ry = [cos(yaw) 0 sin(yaw) 0; ...
0 1 0 0;...
-sin(yaw) 0 cos(yaw) 0 ; ...
0 0 0 1]; % yaw [row1; row2; row3; row4]
rot_matrix = Rx * Rz * Ry; % total rotation matrix from local to global coordinates
% find the translation portion of the transformation matrix from local to global coordinates
translation = [x;y;z]; % coordinates of the trackable target centroid at current time
translate = zeros(4,4);
%translate = [0 0 0 x; ...
% 0 0 0 y; ...
% 0 0 0 z; ...
% 0 0 0 0];
translate(1:3,4) = translation; % build into 4x4 matrix for combination with the rotation portion of the transformation
%total transformation from local to global coordinates
transform_matrix_local_to_global = rot_matrix + translate;

% now find the transformation matrix from global to local coordinates
% obtain transform_matrix inverse: http://stackoverflow.com/questions/2624 ... -transform
rot_matrix_inverse = (rot_matrix).'; % note the inverse is the same as the transpose for rotation matrices! http://www.dept.aoe.vt.edu/~durham/AOE5214/Ch03.pdf
translation_inverse = -rot_matrix_inverse(1:3,1:3)*translation;
transform_matrix_inverse_global_to_local = rot_matrix_inverse;
transform_matrix_inverse_global_to_local(1:3,4) = translation_inverse.';
*******************************************

You have the option to use a right-handed coordinate system and I think this is the default in the .CSV output files that can be exported from the tracking tools. in this case the following modifications are required to the above code:

z = -1*z % invert the z-axis
roll = -1*roll*degtorad; % negate for LHS to RHS
yaw = -1*yaw*degtorad; % negate for LHS to RHS
LHStoRHS = [1 0 0 0; ...
0 1 0 0; ...
0 0 -1 0; ...
0 0 0 1];
rot_matrix_RHS = LHStoRHS * rot_matrix * LHStoRHS; % reference http://www.geometrictools.com/Documenta ... Handed.pdf
transform_matrix_local_to_global = rot_matrix_RHS + translate;
rot_matrix_inverse = (rot_matrix_RHS).'; % note the inverse is the same as the transpose for rotation matrices! http://www.dept.aoe.vt.edu/~durham/AOE5214/Ch03.pdf

************************************************************

After the two transformation matrices are found one can convert from local-to-global or global-to-local coordinates by using

global = transform_matrix_local_to_global * local

local = transform_matrix_inverse_global_to_local * global

where local is the column vector coordinates in the trackable local reference frame [x';y';z';1] or where global is corresponding column vector coordinates in the camera (global) reference frame [x;y;z;1]. Both of these column vectors are padded with a 1 in the 4th dimension.

Re: Help on rotation using quaternions or YPR

Posted: Wed Apr 03, 2013 2:28 pm
by NaturalPoint - Brent
lightyear66,
Thank you for posting the results of your work and suggestions to other users! It is much appreciated in the community and will save a lot of people a lot of work.

Brent Mason