## Help on rotation using quaternions or YPR

lightyear66
Posts: 2
Joined: Fri Oct 28, 2011 2:58 pm
Location: Illinois, USA

### Re: Help on rotation using quaternions or YPR

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

% 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.

NaturalPoint - Brent
NaturalPoint Employee Posts: 59
Joined: Tue Nov 20, 2012 12:40 pm

### Re: Help on rotation using quaternions or YPR

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