Camera position in world coordinate from cv::solvePnP

by

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.

More Related Contents:

  1. OpenCV C++/Obj-C: Detecting a sheet of paper / Square Detection
  2. Get 3D coordinates from 2D image pixel if extrinsic and intrinsic parameters are known
  3. connected components in OpenCV
  4. Convert RGB to Black & White in OpenCV
  5. Converting data from glReadPixels() to OpenCV::Mat
  6. Setting ROI with mouse from a rectangle on a video
  7. scale and rotation Template matching
  8. Writing robust (color and size invariant) circle detection with OpenCV (based on Hough transform or other features)
  9. OpenCV: Find all non-zero coordinates of a binary Mat image
  10. How can I determine distance from an object in a video?
  11. How to calculate the angle from rotation matrix
  12. OpenCV tracking using optical flow
  13. Executing cv::warpPerspective for a fake deskewing on a set of cv::Point
  14. Calculating normals in a triangle mesh
  15. OpenCV VideoCapture lag due to the capture buffer
  16. Is there a formula to determine overall color given BGR values? (OpenCV and C++)
  17. Copy an cv::Mat inside a ROI of another one
  18. Compiling a static executable with CMake
  19. Converting cv::Mat to IplImage*
  20. Fast color quantization in OpenCV
  21. How to get a color palette from an image using OpenCV [closed]
  22. Inverse fourier transformation in OpenCV
  23. Easy framework for OpenGL Shaders in C/C++
  24. load image with openCV Mat c++
  25. OpenCV warpperspective
  26. OpenCV SURF function is not implemented
  27. how to remove straight lines or non-curvical lines in a canny image
  28. Why “cout” works weird for “unsigned char”?
  29. OpenCV cv::Mat to std::ifstream for base64 encoding
  30. how to use gpu::Stream in OpenCV?

Leave a Comment