Computing x,y coordinate (3D) from image point

Question

I have a task to locate an object in 3D coordinate system. Since I have to get almost exact X and Y coordinate, I decided to track one color marker with known Z coordinate that will be placed on the top of the moving object, like the orange ball in this picture:

First, I have done the camera calibration to get intrinsic parameters and after that I used cv::solvePnP to get rotation and translation vector like in this following code:

std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> objectPoints;
//img points are green dots in the picture
imagePoints.push_back(cv::Point2f(271.,109.));
imagePoints.push_back(cv::Point2f(65.,208.));
imagePoints.push_back(cv::Point2f(334.,459.));
imagePoints.push_back(cv::Point2f(600.,225.));

//object points are measured in millimeters because calibration is done in mm also
objectPoints.push_back(cv::Point3f(0., 0., 0.));
objectPoints.push_back(cv::Point3f(-511.,2181.,0.));
objectPoints.push_back(cv::Point3f(-3574.,2354.,0.));
objectPoints.push_back(cv::Point3f(-3400.,0.,0.));

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

cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Rodrigues(rvec,rotationMatrix);

After having all matrices, this equation that can help me with transforming image point to wolrd coordinates:

where M is cameraMatrix, R - rotationMatrix, t - tvec, and s is an unknown. Zconst represents the height where the orange ball is, in this example it is 285 mm. So, first I need to solve previous equation, to get "s", and after I can find out X and Y coordinate by selecting image point:

Solving this I can find out variable "s", using the last row in matrices, because Zconst is known, so here is the following code for that:

cv::Mat uvPoint = (cv::Mat_<double>(3,1) << 363, 222, 1); // u = 363, v = 222, got this point using mouse callback

cv::Mat leftSideMat  = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
cv::Mat rightSideMat = rotationMatrix.inv() * tvec;

double s = (285 + rightSideMat.at<double>(2,0))/leftSideMat.at<double>(2,0)); 
//285 represents the height Zconst

std::cout << "P = " << rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec) << std::endl;

After this, I got result: P = [-2629.5, 1272.6, 285.]

and when I compare it to measuring, which is: Preal = [-2629.6, 1269.5, 285.]

the error is very small which is very good, but when I move this box to the edges of this room, errors are maybe 20-40mm and I would like to improve that. Can anyone help me with that, do you have any suggestions?

Estrada

Have you applied undistortion to your images with the intrinsic parameters ? Especially at the edges of an image the distortion can be quite high with any COTS lens which could be at least one reason why you're getting this error. – Blondellblondelle

@Blondellblondelle Yes, I did, first I load the cameraMatrix and distCoeffs from xml files and then I apply this cv::undistort(inputImage,undistorted,cameraMatrix,distCoeffs); After with mouse callback I select the orange ball and store the values in uvPoint. – Estrada

I think with a COTS camera and a room with a span of a few meters an error of 2-4 cm is what you'll have to live with. It's actually quite good for a system like that. To deal with real 3d you'll have to use a multi-view camera system anyways, and due to disparity error objects further away from your camera will have a higher error. So the answer here is: use multiple measurements of whatever kind. – Blondellblondelle

Wonder useful example. Thanks! – Quirinal

Incredibly useful example, helped me solve my projection issues – Goldin

+1 for this incredibly useful example. @Estrada -- you, sir, are a gentleman and a scholar. I've been able to extend this approach to solve the projection of an image point onto a real-world 3D sphere with great success on the first try. Thank you. I can't upvote this enough. – Offload

The question is an Incredible answer in the core, Thank you very much – Nurserymaid

Big thanks to everyone for such nice comments, I am glad to see that even after such a long time this example helps the community. – Estrada

This example ended my 12-hour quest to figure out how to do a reverse projection. I wish I saw this result at the beginning of the 12 hours :D – Vennieveno

Thank you @Estrada for clear and important contribution. I have two quick questions. 1) Why are you not using axis coordinates that correspond to screen pixels, top left corner, X right, Y down? 2) In general sense, how could a single camera to accurate obtain all three X, Y, Z? – Cristencristi

Answer 1

Given your configuration, errors of 20-40mm at the edges are average. It looks like you've done everything well.

Without modifying camera/system configuration, doing better will be hard. You can try to redo camera calibration and hope for better results, but this will not improve them alot (and you may eventually get worse results, so don't erase actual instrinsic parameters)

As said by count0, if you need more precision you should go for multiple measurements.

Pulitzer

Ok, I wasn't sure if am I doing it correctly, thanks for advice. – Estrada

Answer 2

Do you get the green dots (imagePoints) from the distorted or undistorted image? Because the function solvePnP already undistort the imagePoints (unless you don't pass the distortion coefficients, or pass them as null). You may be undistorting those imagePoints twice if you are getting them from the undistorted image, and this would end up causing an increased error in the corners.

https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/solvepnp.cpp

Raguelragweed

This does not provide an answer to the question. To critique or request clarification from an author, leave a comment below their post - you can always comment on your own posts, and once you have sufficient reputation you will be able to comment on any post. – Ewens

Hi Rick. I edited the answer. I hope it is better now. Thanks for the advice. – Raguelragweed

Answer 3

I think maybe the distortion of the image is the cause of the error. So maybe using a very large pattern (from a projector above the center), then building "a look up table" might be a solution.

Fenestella

As it’s currently written, your answer is unclear. Please edit to add additional details that will help others understand how this addresses the question asked. You can find more information on how to write good answers in the help center. – Pikestaff