6dof rigid-body based tracking

I am looking for any library that could help on computing the trigonometry necessary to estimate the position and orientation of a given marker-based rigid body.

This is a very well known technique (used in Vicon and NaturalPoint for multiple camera calibration), so I’m wondering if any of you have already been in this waters and what have you used.

The computer vision process to extract markers locations in pixel space is quite obvious, so I’m not interested on that part. I’m looking for libraries or documentation explaining the trigonometry side of the problem.

Here there is a paper with nice pictures that will give you an idea of what I’m looking for:

Thank you for any pointers.