Camera pose estimation (OpenCV PnP)

I solved this a while ago, apologies for the year delay.

In the python OpenCV 2.1 I was using, and the newer version 3.0.0-dev, I have verified that to get the pose of the camera in the global frame you must:

_, rVec, tVec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
Rt = cv2.Rodrigues(rvec)
R = Rt.transpose()
pos = -R * tVec

Now pos is the position of the camera expressed in the global frame (the same frame the objectPoints are expressed in).
R is an attitude matrix DCM which is a good form to store the attitude in.
If you require Euler angles then you can convert the DCM to Euler angles given an XYZ rotation sequence using:

roll = atan2(-R[2][1], R[2][2])
pitch = asin(R[2][0])
yaw = atan2(-R[1][0], R[0][0])

Leave a Comment