76

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: undistored

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:

transform_equation

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: equation2

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?

kwmen
  • 63
  • 2
  • 5
Banana
  • 1,276
  • 2
  • 16
  • 19
  • 1
    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. – count0 Sep 06 '12 at 12:17
  • @count0 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`. – Banana Sep 06 '12 at 12:35
  • 5
    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. – count0 Sep 06 '12 at 13:03
  • 3
    Wonder useful example. Thanks! – Vyacheslav Dec 01 '16 at 17:30
  • 3
    Incredibly useful example, helped me solve my projection issues – Reece Sheppard Aug 11 '17 at 06:15
  • 1
    +1 for this incredibly useful example. @Banana -- 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. – Basil Jul 25 '20 at 23:25
  • 1
    The question is an Incredible answer in the core, Thank you very much – Saeed Masoomi Nov 02 '20 at 12:14
  • 1
    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. – Banana Nov 03 '20 at 13:11
  • 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 – Michael Currie May 07 '22 at 21:02
  • Thank you @Banana 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? – Stan Bashtavenko Feb 27 '23 at 14:55

2 Answers2

14

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.

Pascal Lécuyot
  • 526
  • 4
  • 10
7

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

Mariana Mascarenhas de Carvalh
  • 106
  • 1
  • 4
  • 1
    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](http://stackoverflow.com/help/whats-reputation) you will be able to [comment on any post](http://stackoverflow.com/help/privileges/comment). – Rick Smith Oct 02 '15 at 15:49
  • Hi Rick. I edited the answer. I hope it is better now. Thanks for the advice. – Mariana Mascarenhas de Carvalh Oct 12 '15 at 17:36