Looking at questions asked in the forum I guess that I am not the only one with problems and unanswered questions here...
I would greatly appreciate some documentation or specifications on the rotation orders of YPR and quaternions.
Kahl Hellmer
Help on rotation using quaternions or YPR

 Posts: 10
 Joined: Fri Sep 14, 2007 7:33 am
 Location: Uppsala, Sweden
Re: Help on rotation using quaternions or YPR
Here's how you can create the orientation matrix from the Euler angles:
double sa = sin(pitch);
double ca = cos(pitch);
double sb = sin(roll);
double cb = cos(roll);
double sh = sin(yaw);
double ch = cos(yaw);
double matrix[3*3]=
{
ch*ca , ch*sa*cb + sh*sb, ch*sa*sb + sh*cb,
sa , ca*cb , ca*sb,
sh*ca, sh*sa*cb + ch*sb, sh*sa*sb + ch*cb
};
double sa = sin(pitch);
double ca = cos(pitch);
double sb = sin(roll);
double cb = cos(roll);
double sh = sin(yaw);
double ch = cos(yaw);
double matrix[3*3]=
{
ch*ca , ch*sa*cb + sh*sb, ch*sa*sb + sh*cb,
sa , ca*cb , ca*sb,
sh*ca, sh*sa*cb + ch*sb, sh*sa*sb + ch*cb
};
Re: Help on rotation using quaternions or YPR
angles are in radians there
Re: Help on rotation using quaternions or YPR
Thank you for you post Doug. Can you please let us know what rotation sequence this applies to? The calculations I would have used to go from yaw, pitch, roll to the direction cosine matrix (DCM) would have been 
A = a*pi/180; % a = azmiuth in degrees (yaw?)
E = e*pi/180; % e = elevation in degrees (pitch?)
R = r*pi/180; % r = roll in degrees (roll)
sa = sin(E);
ca = cos(E);
sb = sin(R);
sb = cos(R);
sh = sin(A);
ch = cos(A);
% Create the correct matrix coefficients
DCM = [(ch * ca) (sh * ca) (sa)
(ch * sa * sb  sh * cb) (sh * sa * sb + ch * cb) (ca * sb)
(ch * sa * cb + sh * sb) (sh * sa * cb  ch * sb) (ca * cb)];
A = a*pi/180; % a = azmiuth in degrees (yaw?)
E = e*pi/180; % e = elevation in degrees (pitch?)
R = r*pi/180; % r = roll in degrees (roll)
sa = sin(E);
ca = cos(E);
sb = sin(R);
sb = cos(R);
sh = sin(A);
ch = cos(A);
% Create the correct matrix coefficients
DCM = [(ch * ca) (sh * ca) (sa)
(ch * sa * sb  sh * cb) (sh * sa * sb + ch * cb) (ca * sb)
(ch * sa * cb + sh * sb) (sh * sa * cb  ch * sb) (ca * cb)];
Re: Help on rotation using quaternions or YPR
I should clarify that the convention above is a ZYX rotation sequence (rotate about the Z axis first, then about the Y' axis and then about the X' axis)

 Posts: 10
 Joined: Fri Sep 14, 2007 7:33 am
 Location: Uppsala, Sweden
Re: Help on rotation using quaternions or YPR
Does anyone know how I can project a point, using the quaternions of the rigid body (trackable)? The point is statically offset relative to the rigid body.
I.e., a rigid body is defined, and from the rigid body a point is projected with the coordinates x=1, y=0 and z=0. When the rigid body moves 90 degrees counter clockwise in the horizontal plane the coordinates should be x=0, y=0 and z=1.
Thanks,
Kahl
I.e., a rigid body is defined, and from the rigid body a point is projected with the coordinates x=1, y=0 and z=0. When the rigid body moves 90 degrees counter clockwise in the horizontal plane the coordinates should be x=0, y=0 and z=1.
Thanks,
Kahl
Re: Help on rotation using quaternions or YPR
This is how I would do it 
I think that is typically easiest to use the Direction Cosine Matrix (DCM) for rotation operations. So first you need to convert quaternions to DCM 
DCM = [(12*qy^2  2*qz^2) (2*qx*qy(i)  2*qz*qw) (2*qx*qz + 2*qy*qw)
(2*qx*qy + 2*qz*qw) (1  2*qx^2  2*qz^2) (2*qy*qz  2*qx*qw)
(2*qx*qz  2*qy*qw) (2*qy*qz + 2*qx*qw) (1  2*qx^2  2*qy^2)];
Then you can rotate your point [Xp Yp Zp] ï¿½
[Xr; Yr; Zr] = transpose (DCM) * [Xp; Yp; Zp]
 Xr, Yr, Zr are coordinates of the rotated point (rotated about the origin)
And then translate your point by adding [Xb ;Yb; Zb] to [Xr; Yr; Zr] (position of rigid body).
I hope this makes sense.
A question for the Natural Point team. Would it be possible to get the DCM out of Tracking Tools as an alternative to Quaternions or Euler angles?
I think that is typically easiest to use the Direction Cosine Matrix (DCM) for rotation operations. So first you need to convert quaternions to DCM 
DCM = [(12*qy^2  2*qz^2) (2*qx*qy(i)  2*qz*qw) (2*qx*qz + 2*qy*qw)
(2*qx*qy + 2*qz*qw) (1  2*qx^2  2*qz^2) (2*qy*qz  2*qx*qw)
(2*qx*qz  2*qy*qw) (2*qy*qz + 2*qx*qw) (1  2*qx^2  2*qy^2)];
Then you can rotate your point [Xp Yp Zp] ï¿½
[Xr; Yr; Zr] = transpose (DCM) * [Xp; Yp; Zp]
 Xr, Yr, Zr are coordinates of the rotated point (rotated about the origin)
And then translate your point by adding [Xb ;Yb; Zb] to [Xr; Yr; Zr] (position of rigid body).
I hope this makes sense.
A question for the Natural Point team. Would it be possible to get the DCM out of Tracking Tools as an alternative to Quaternions or Euler angles?

 Posts: 10
 Joined: Fri Sep 14, 2007 7:33 am
 Location: Uppsala, Sweden
Re: Help on rotation using quaternions or YPR
Thanks alot, it makes perfect sense and works just fine.
If you're ever in Stockholm, I'll buy you a beer =)
Kahl
If you're ever in Stockholm, I'll buy you a beer =)
Kahl

 Posts: 2
 Joined: Fri Oct 28, 2011 2:58 pm
 Location: Illinois, USA
Re: Help on rotation using quaternions or YPR
In order to transform from one coordinate system to another using the Euler yaw pitch and roll angles or the quaternions, there are several key assumptions that must be known. Since this is such a common practice that might be done with the data from the tracking tools, it would have been expected that this information would clearly have been presented all in one place in the user's manual  maybe even with an example.
Is the system a right handed or left handed coordinate system? By default the Optitrack system is right handed and the handedness is reported in the CSV file that can be exported from a timeline.
To use Euler angles: What axis is the roll about? What axis is the yaw about? What axis is the pitch about? I have seen it posted in a couple of locations in the forums that roll is about the X axis, yaw is about the Y axis and pitch is about the Z axis.
What is the rotation sequence? I have seen it posted in a couple of places that the rotation sequence associated with the Optitrack convention is X(Roll) followed by Z(pitch) followed by Y(yaw).
Finally, you must know the direction that the coordinate transformation goes from and to. For instance does it take you from the camera frame of reference to the frame of reference of the trackable centroid or does it take you from the trackable object frame of reference to the frame of reference of the camera? I have not definitively been able to find this information in a clear post.
But, if we are provided with the correct direction cosine matrix, then maybe we do not need to know all the above details  so I was pleased to find what I thought might be this formulation posted by Doug with very little explanation of its basis or how to apply it. I have tried to use Doug's equation above to blindly transform the position of a marker on the rigid body of a defined trackable into the frame of reference of the trackable by using:
M*[xp; yp; zp]  [xtargetcentroid; ytargetcentroid; ztargetcentroid]
where M is Doug's orientation matrix, [xp; yp; zp] is the position of a marker on the trackable (in the camera frame of reference) and [xtargetcentroid; ytargetcentroid; ztargetcentroid] is the position of a trackable (in the camera frame of reference). As I move or rotate my rigid body trackable around, I would expect the above equation to give me a constant position for the marker in the target frame of reference. It does not. I have tried a few variations, including taking the matrix transform of Doug's matrix prior to applying it in this equation and using a few different direct cosine matrices that might be derived if I somehow misunderstood the order of the rotations associated with the Euler angles. I also tried using the quaternions to derive the direction cosine matrix and use this in the equation, but none of these seem to work. Either my understanding from the forums is incorrect or somehow I have applied this information improperly. The above poster, Kahl, seems to have found satisfaction  I think from using equations from another poster that provide a DCM matrix that I think assumes a different rotation sequence than Doug. Is Doug's post incorrect?
I really expect that having a complete explanation, with an example, all in a single posting (and added to any new User's Guide maybe even with a drawing) would be really helpful to many people trying to use the rich information available with the system. Optitrack is providing data that they know many people want and then the information required to actually use the data is missing or scattered through the forums, with the briefest (and sometimes ambiguous/unclear) explanations. I do not want to seem too frustrated, but I think this topic needs some simple attention and not just a reference to some math website.
Is the system a right handed or left handed coordinate system? By default the Optitrack system is right handed and the handedness is reported in the CSV file that can be exported from a timeline.
To use Euler angles: What axis is the roll about? What axis is the yaw about? What axis is the pitch about? I have seen it posted in a couple of locations in the forums that roll is about the X axis, yaw is about the Y axis and pitch is about the Z axis.
What is the rotation sequence? I have seen it posted in a couple of places that the rotation sequence associated with the Optitrack convention is X(Roll) followed by Z(pitch) followed by Y(yaw).
Finally, you must know the direction that the coordinate transformation goes from and to. For instance does it take you from the camera frame of reference to the frame of reference of the trackable centroid or does it take you from the trackable object frame of reference to the frame of reference of the camera? I have not definitively been able to find this information in a clear post.
But, if we are provided with the correct direction cosine matrix, then maybe we do not need to know all the above details  so I was pleased to find what I thought might be this formulation posted by Doug with very little explanation of its basis or how to apply it. I have tried to use Doug's equation above to blindly transform the position of a marker on the rigid body of a defined trackable into the frame of reference of the trackable by using:
M*[xp; yp; zp]  [xtargetcentroid; ytargetcentroid; ztargetcentroid]
where M is Doug's orientation matrix, [xp; yp; zp] is the position of a marker on the trackable (in the camera frame of reference) and [xtargetcentroid; ytargetcentroid; ztargetcentroid] is the position of a trackable (in the camera frame of reference). As I move or rotate my rigid body trackable around, I would expect the above equation to give me a constant position for the marker in the target frame of reference. It does not. I have tried a few variations, including taking the matrix transform of Doug's matrix prior to applying it in this equation and using a few different direct cosine matrices that might be derived if I somehow misunderstood the order of the rotations associated with the Euler angles. I also tried using the quaternions to derive the direction cosine matrix and use this in the equation, but none of these seem to work. Either my understanding from the forums is incorrect or somehow I have applied this information improperly. The above poster, Kahl, seems to have found satisfaction  I think from using equations from another poster that provide a DCM matrix that I think assumes a different rotation sequence than Doug. Is Doug's post incorrect?
I really expect that having a complete explanation, with an example, all in a single posting (and added to any new User's Guide maybe even with a drawing) would be really helpful to many people trying to use the rich information available with the system. Optitrack is providing data that they know many people want and then the information required to actually use the data is missing or scattered through the forums, with the briefest (and sometimes ambiguous/unclear) explanations. I do not want to seem too frustrated, but I think this topic needs some simple attention and not just a reference to some math website.

 NaturalPoint Employee
 Posts: 59
 Joined: Tue Nov 20, 2012 12:40 pm
Re: Help on rotation using quaternions or YPR
Here is a condensed form of what needs to happen to compose a world transform matrix:
Roll is about the positive X axis, Yaw is about the positive Y axis, and Pitch is about the positive Z axis.
To compose the world transform matrix, you first compose the rotation matrix.
Given a world transform matrix of the form:
M =
[ [ ] Tx ]
[ [ R ] Ty ]
[ [ ] Tz ]
[ 0 0 0 1 ]
where Tx, Tz, Tz are the worldspace position of the origin, and R is a 3x3 rotation matrix composed as:
R = [ Rx (Roll) ] * [ Rz (Pitch) ] * [ Ry (Yaw) ]
where Rx, Ry, and Rz are 3x3 rotation matrices composed according to (always use radians for angles):
Once the world transform matrix is composed, it can be validated by looking at each column in the matrix. The first three rows of Column 1 are the (normalized) XYZ direction vector of the worldspace X axis, column 2 holds the Y axis, and column 3 is the Z axis. Column 4, as noted previously, is the location of the worldspace origin.
To convert a point from world coordinates (coordinates reported by the API for a 3D point anywhere in space), you need a matrix that converts from world space to local space. We have a localtoworld matrix (where the local coordinates are defined as the coordinate system of the rigid body used to compose the transform matrix), so we need to invert that matrix to get a worldtolocal transformation matrix.
Inversion of a general 4x4 matrix can be slightly complex and may result in singularities, however we are dealing with a special transform matrix that only contains rotations and a translation. Because of that, we can take advantage of the method shown here to easily invert the matrix:
http://stackoverflow.com/questions/2624 ... transform
Once the world matrix is converted, multiplying it by the coordinates of a worldspace point will yield a point in the local space of the rigid body. Any number of points can be multiplied by this inverted matrix to transform them from world (API) coordinates to local (rigid body) coordinates.
Roll is about the positive X axis, Yaw is about the positive Y axis, and Pitch is about the positive Z axis.
To compose the world transform matrix, you first compose the rotation matrix.
Given a world transform matrix of the form:
M =
[ [ ] Tx ]
[ [ R ] Ty ]
[ [ ] Tz ]
[ 0 0 0 1 ]
where Tx, Tz, Tz are the worldspace position of the origin, and R is a 3x3 rotation matrix composed as:
R = [ Rx (Roll) ] * [ Rz (Pitch) ] * [ Ry (Yaw) ]
where Rx, Ry, and Rz are 3x3 rotation matrices composed according to (always use radians for angles):
Once the world transform matrix is composed, it can be validated by looking at each column in the matrix. The first three rows of Column 1 are the (normalized) XYZ direction vector of the worldspace X axis, column 2 holds the Y axis, and column 3 is the Z axis. Column 4, as noted previously, is the location of the worldspace origin.
To convert a point from world coordinates (coordinates reported by the API for a 3D point anywhere in space), you need a matrix that converts from world space to local space. We have a localtoworld matrix (where the local coordinates are defined as the coordinate system of the rigid body used to compose the transform matrix), so we need to invert that matrix to get a worldtolocal transformation matrix.
Inversion of a general 4x4 matrix can be slightly complex and may result in singularities, however we are dealing with a special transform matrix that only contains rotations and a translation. Because of that, we can take advantage of the method shown here to easily invert the matrix:
http://stackoverflow.com/questions/2624 ... transform
Once the world matrix is converted, multiplying it by the coordinates of a worldspace point will yield a point in the local space of the rigid body. Any number of points can be multiplied by this inverted matrix to transform them from world (API) coordinates to local (rigid body) coordinates.