Thanks for the answer! Very helpuful. Sorry I took a long time to post how I solved my problem. I ended up using some of your equations and an additional one.
- From the camera intrinsic parameters I obtained: fx, fy, cx, and cy.
- From previous measurements I have the [object points] and their correspondent [image points] (used in solvePnP). I obtained the [image points] with 'mouse callback' function.
- 'Rodrigues' function gets me the Rotation Matrix with the 'rvec' from 'solvePnP'.
- 'HoughCircles' gives me the [u,v] of the hole center I'm identifying.
- Converted the 2D image coordinate of the hole in the normalized camera frame. [x', y', 1] (above equation)
- I made the scale factor 's' the distance from the surface to the camera sensor.
- Multiply 's' by the normalize camera sensor. (equation above).
- Finally, I use this equation to transfom the 3D coordinate in camera frame to my world frame.
'R^T' is the transposed rotation matrix and 't' is tvec.
Results where pretty accurate. I've changed some things since then and I don't remember the correct tolerance but it was better than I expected. I think a different lens might help to, mine has a fish eye distortion and even the 'undistort' function doesn't fix it completely. The edges are no good.
Thanks again.
