guivi

Projecting point to world coordinate

—

by

guivi
in

This post is a copy of an original answer from the OpenCV forum, I copied here so I can have fast access from my webpage and for future archiving just in case. Original post can be found at: 

https://answers.opencv.org/question/62779/image-coordinate-to-world-coordinate-opencv/

You need to obtain the tvec, rvec and rotationMatrix. The first two you get them through the solvePnP() function and the latter through the Rodrigues() function. Once you have this information in addition to the camera calibration coefficients you can transform the pixel point to world coordinates point. The code should be something like that (it is taken from an old project of mine):

cv::Mat intrinsics, distCoeffs;
cv::Mat rotationMatrix, rvec, tvec;
rvec.create(1,3,cv::DataType<double>::type);
tvec.create(1,3,cv::DataType<double>::type);
rotationMatrix.create(3,3,cv::DataType<double>::type);

// WorldPlane are the 3D coordinate of the pattern used to solvePnP
// ImagePlane points are from pattern detection e.g: cv::findChessboardCorners
cv::solvePnP(worldPlane, imagePlane, intrinsics, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec,rotationMatrix);

cv::Mat uvPoint = cv::Mat::ones(3,1,cv::DataType<double>::type); //u,v,1

// image point to project
uvPoint.at<double>(0,0) = 3.; //got this point using mouse callback
uvPoint.at<double>(1,0) = 134.;

cv::Mat tempMat, tempMat2;
double s, zConst = 0;
tempMat = rotationMatrix.inv() * intrinsics.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = zConst + tempMat2.at<double>(2,0);
s /= tempMat.at<double>(2,0);
cv::Mat wcPoint = rotationMatrix.inv() * (s * intrinsics.inv() * uvPoint - tvec);

cv::Point3f realPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0)); // point in world coordinates

link

Comments as per forum post

So you basically invert the pinhole camera equation and find out s such that the ray intersects with the z = 0 plane?

————————————————————————————————–

Thanks for your complete answer. Is it possible that instead of using solvepnp I fill rvec and tvec? Based on IMU data?

I have the rotation [Roll, Pitch, Yaw] and transition from the Z=0 plane which is [X, Y, Z].

If it is possible could you briefly tell me how can i fill rvec and tvec?

——————————————————————————————————-

@above something like that. @Top I do not know/remember that, the project that I worked with it was 2 years ago, and since then there was not need to work with it again so some of the information/knowledge faded out. But try to search with the keywords that I gave you at the comment in your opening question. I still remember that there was a lot of material, with examples as well, to read about it when I was searching at that point.

—————————————————————————————————————–

What is these worldPlaneimagePlane two parameters for? And how to determine?

———————————————————————————————————–

Hello.

I did this, and also this related article on StackOverflow: https://stackoverflow.com/questions/1…

with that same image (its the image described there), and I can’t get the positions (realPoints) right!

And of course, when I proyected them back, they are well off.

Also, in :

s = zConst + tempMat2.at<double>(2,0);

s /= tempMat.at<double>(2,0);

The “2” is because is z-constant? its the z-pos in the matrix? being z the vertical axis, zeroed at floor?