Computing Rotation Matrix about a rigid frame

edit

Hi guys,

This has left me confused a bit . I have a robot in the form of a manikin head which has four markers placed on it. The robot is situated on a table as shown below

I am tracking the position of the face with vicon markers (using the vicon bridge package) placed at the forehead, left/right cheeks and chin respectively.

I would like to compute the orientation of the face with respect to a rigid frame to be defined on the table (with ROS' right-handed coordinate frame).

My Questions: (i) How do I define a rigid frame on the table? Do I use something like the tf::Transform class? I saw tutorials similar to the following

            tf::Transform transform;
            transform.setOrigin(tf::Vector3(0.0,0.0,0.2));
            transform.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));

that is assuming the centroid of the face is 20cm above the table. I am not too sure if I should be using the tf transform class for this purpose.

(ii)I would like to attach a coordinate frame to the head in real-time so that I can find the orientation of the face with respect to the rigid frame on the table.

I would like to hear your thoughts on what is the best option to take.

PS: I know the vicon_bridge package gives the tf transform of the subject with respect to the camera frame but I found the default computed quaternion to suffer a drift that defeats the purpose of my experiment i.e. achieving sub-millimeter and sub-degree accuracy while controlling the motion of the head. The cartesian coordinates of the markers is accurate to less than 1mm so I am inclined to think something is wrong in the way the calculation is done in the vicon_sdk software. I feel it is best I compute the transforms myself given the position of the markers.

EDIT 1

For example, here is the dump from the screen based on returned orientation as computed by the vicon_bridge package. Note that the head is really static in the scene and there should not be a lot of drift

 -3.851588 ,    -89.742720,     2.474993
 |  roll    |   pitch   |   yaw
 -6.287211 ,    -89.741393,     4.915040
|   roll    |   pitch   |   yaw
 -5.358562 ,    -89.741111,     3.981298
|   roll    |   pitch   |   yaw
 9.701361 ,     -89.705700,     -11.091304
|   roll    |   pitch   |   yaw
 11.665852 ,    -89.703736,     -13.057809
|   roll    |   pitch   |   yaw
 10.510902 ,    -89.706893,     -11.903041
|   roll    |   pitch   |   yaw
 -13.265921 ,   -89.758196,     11.884158
|   roll    |   pitch   |   yaw
  9.701361 ,    -89.705700,     -11.091304
|   roll    |   pitch   |   yaw
 11.466865 ,    -89.708170,     -12.858929
|   roll    |   pitch   |   yaw
 -7.994014 ,    -89.756785,     6.613398
|   roll    |   pitch   |   yaw
 -11.421505 ,   -89.760937,     10.039571
|   roll    |   pitch   |   yaw
 -13.407172 ,   -89.757765,     12.023137
|   roll    |   pitch   |   yaw
 10.069726 ,    -89.706260,     -11.460297
|   roll    |   pitch   |   yaw
 9.701361 ,     -89.705700,     -11.091304
|   roll    |   pitch   |   yaw
 9.701361 ,     -89.705700,     -11.091304
|   roll    |   pitch   |   yaw
 9.701361 ,     -89.705700 ...