Computing Rotation Matrix about a rigid frame
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 ...