Question

is there a function of OpenCV such that with:

• coordinates of a marker in the image plane
• extrinsic parameters
• intrinsic parameters
• z coordinate (distance between the marker and the cam, because I use sensor kinect)

provides the corresponding world coordinates of the marker?

Any help is much appreciated. Thanks!

Answer 1

To find the world coordinates of the marker you need its coordinates relative to the camera. If you know the camera pose P relative to the origin and the marker pose M relative to the camera, to get the marker pose relative to the origin you simply multiply them together

final = [P]*[M]

It sounds like you are just struggling with finding M. All you need to do is multiply your position by the inverse of the camera matrix and then by your Z coordinate.

Z*cam_mat.inv()*[x_image,y_image,1] = [x_world,y_world,z_world]

M = [1,0,0,x_world,
     0,1,0,y_world,
     0,0,1,z_world,
     0,0,0,1]