Estimation of camera position in world coordinates using OpenCV SolvePnP