Camera Position In World Coordinate From Cv::solvePnP

Answer :

If with "world coordinates" you mean "object coordinates", you have to get the inverse transformation of the result given by the pnp algorithm.

There is a trick to invert transformation matrices that allows you to save the inversion operation, which is usually expensive, and that explains the code in Python. Given a transformation [R|t], we have that inv([R|t]) = [R'|-R'*t], where R' is the transpose of R. So, you can code (not tested):

cv::Mat rvec, tvec; solvePnP(..., rvec, tvec, ...); // rvec is 3x1, tvec is 3x1  cv::Mat R; cv::Rodrigues(rvec, R); // R is 3x3  R = R.t();  // rotation of inverse tvec = -R * tvec; // translation of inverse  cv::Mat T = cv::Mat::eye(4, 4, R.type()); // T is 4x4 T( cv::Range(0,3), cv::Range(0,3) ) = R * 1; // copies R into T T( cv::Range(0,3), cv::Range(3,4) ) = tvec * 1; // copies tvec into T  // T is a 4x4 matrix with the pose of the camera in the object frame 

Update: Later, to use T with OpenGL you have to keep in mind that the axes of the camera frame differ between OpenCV and OpenGL.

OpenCV uses the reference usually used in computer vision: X points to the right, Y down, Z to the front (as in this image). The frame of the camera in OpenGL is: X points to the right, Y up, Z to the back (as in the left hand side of this image). So, you need to apply a rotation around X axis of 180 degrees. The formula of this rotation matrix is in wikipedia.

// T is your 4x4 matrix in the OpenCV frame cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame 

These transformations are always confusing and I may be wrong at some step, so take this with a grain of salt.

Finally, take into account that matrices in OpenCV are stored in row-major order in memory, and OpenGL ones, in column-major order.

If you want to turn it into a standard 4x4 pose matrix specifying the position of your camera. Use rotM as the top left 3x3 square, tvec as the 3 elements on the right, and 0,0,0,1 as the bottom row

pose = [rotation   tvec(0)         matrix     tvec(1)         here       tvec(2)         0  , 0, 0,  1] 

then invert it (to get pose of camera instead of pose of world)