Calculating 3D position of point captured by 2 cameras OpenCV

Question

0.00/5 (No votes)

See more:

Code from "Learn OpenCV" provides all matrix information needed to calculate 3D position of point captured by both cameras. I was planing to use cvUndistortPoints on both points to calculate disparity and then feed one point coordinates plus disparity to cvPerspectiveTransform to obtain 3D position.

I'm bumping in a problem while trying cvUndistortPoints, despite all parameters being ok (or I hope so) points returned are NaN or QNaN.

I'm creating matrix of points(well one point only as this is all I'm interested in) like that:

C++

typedef struct elem_ {
        float f1;
        float f2;
} elem;
CvMat myMat = cvMat(1,1,CV_32FC2);
CV_MAT_ELEM(myMat,elem, 0, 0).f1 = 100.0f;//this is x position
CV_MAT_ELEM(myMat,elem, 0, 0).f2 = 120.0f;//this is y position

So I hope no error here.
then

C++

cvUndistortPoints(myMat, myMat, &_M1, &_D1, &_R1, &_M1);
float x = CV_MAT_ELEM(myMat,elem, 0, 0).f1; //this returns NaN

All matrixes are from original example that works fine I just made them member variables so I could access them in my method. They ware calculated as in here:
http://www.codeproject.com/Questions/75461/OpenCV-how-to-use-remapping-parameters-disparity-t.aspx[^]

I did ask questions on OpenCV forum but I had no replies ever.
Any ideas?

Posted 24-May-10 22:02pm

mateusz.matyaszek

Updated 24-May-10 22:11pm

Moak

v2

Add a Solution

5 solutions

Add a Solution

Add your solution here

Treat my content as plain text, not as HTML

Preview 0

…

Existing Members

Sign in to your account

...or Join us

Download, Vote, Comment, Publish.

Your Email
Password
Forgot your password?

Your Email
This email is in use. Do you need your password?
Optional Password

I have read and agree to the Terms of Service and Privacy Policy
Please subscribe me to the CodeProject newsletters

When answering a question please:

Read the question carefully.
Understand that English isn't everyone's first language so be lenient of bad spelling and grammar.
If a question is poorly phrased then either ask for clarification, ignore it, or edit the question and fix the problem. Insults are not welcome.
Don't tell someone to read the manual. Chances are they have and don't get it. Provide an answer or move on to the next question.

Let's work to help developers, not make them feel stupid.

This content, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

mateusz.matyaszek · Answer 1 · 2010-05-27T01:03:00

Solution 1

Ok I found solution for this problem had to clone all matrixes used here as somehow they are getting changed in method im taking them from despite that I could not find where.

Posted 27-May-10 1:03am

mateusz.matyaszek

hasanm321 · Answer 2 · 2010-09-25T21:51:00

Solution 2

Dear,
I am stuck after this point,
I have a two float point f1,f2.
I need to save them and feed them to cvUndistortPoints and find the (xyd)from left and right frame.
Can you please give me a hint what you did after you had the same problem?

Thanks in advance

Posted 25-Sep-10 21:51pm

hasanm321

Comments

mateusz.matyaszek 26-Sep-10 5:06am

Well if you could give more details I could give better answer. Im guessing thats what you need:
//sets data
typedef struct elem_ {
float f1;
float f2;
} elem;

elem data1[1] = { 1.0f };
elem data2[1] = { 1.0f };
CvMat _rightPoint = cvMat(1, 1, CV_32FC2, data1 );
CvMat _leftPoint = cvMat(1, 1, CV_32FC2, data2 );

CV_MAT_ELEM(_rightPoint,elem, 0, 0).f1 = rightPoint.x;
CV_MAT_ELEM(_rightPoint,elem, 0, 0).f2 = rightPoint.y;

CV_MAT_ELEM(_leftPoint,elem, 0, 0).f1 = leftPoint.x;
CV_MAT_ELEM(_leftPoint,elem, 0, 0).f2 = leftPoint.y;

//undistort points
cvUndistortPoints(&_leftPoint, &_leftPoint, m_M1, m_D1, m_R1, m_M1);
cvUndistortPoints(&_rightPoint, &_rightPoint, m_M2, m_D2, m_R2 ,m_M2);

//calculate disparity
float disparity = CV_MAT_ELEM(_rightPoint,elem, 0, 0).f1 - CV_MAT_ELEM(_leftPoint,elem, 0, 0).f1;

//prapares point with (x,y,d) coordinates
typedef struct elem3_ {
float f1;
float f2;
float f3;
} elem3;

elem3 data3[3] = { 1.0f };
CvMat _3point = cvMat(1, 1, CV_32FC3, data3 );
CV_MAT_ELEM(_3point,elem3, 0, 0).f1 = rightPoint.x;
CV_MAT_ELEM(_3point,elem3, 0, 0).f2 = rightPoint.y;
CV_MAT_ELEM(_3point,elem3, 0, 0).f3 = disparity;

//gets parameters for reprojection matrix Q
float Fx_= cvmGet( m_M1, 0, 0 ); // focal length in x direction of the rectified image in pixels
float Fy_= cvmGet( m_M2, 1, 1 ); // focal length in y direction of the rectified image in pixels
float Tx_= cvmGet( m_T, 0, 0 ); // Translation in x direction from the left camera to the right camera
float Clx_= cvmGet( m_M1, 0, 2 ); // x coordinate of the optical center of the left camera
float Crx_= cvmGet( m_M2, 0, 2 ); // x coordinate of the optical center of the right camera
float Cy_= cvmGet( m_M1, 1, 2 ); // y coordinate of the optical center of both left and right cameras

float Q[]=
{
1, 0, 0, -Clx_,
0, 1, 0, -Cy_,
0, 0, 0, (Fx_+Fy_)/2,
0, 0, -1/Tx_, (Crx_-Clx_)/Tx_
};

CvMat m_Q= cvMat( 4, 4, CV_32F, Q );

//calculates 3D position of marker
cvPerspectiveTransform(&_3point,&_3point, &m_Q);

mismõttes 15-Jan-11 15:54pm

Hey, do you happen to use calibration in your project? I've run out of ideas how to get my stuff running and as you yourself have experienced the opencv list is the worst ever.

Member 11703426 19-May-15 15:02pm

The results are in cm?And wich is the reference system?

hasanm321 · Answer 3 · 2010-09-27T09:55:00

Solution 3

Thank you very much.You are a life saver.Its exactly what i needed.

But In
cvUndistortPoints(&_leftPoint, &_leftPoint, m_M1, m_D1, m_R1, m_M1);

Why you use "m_M1" twice?

Should it not be the P1 in the end?

Posted 27-Sep-10 9:55am

hasanm321

Updated 27-Sep-10 13:23pm

v2

Comments

mateusz.matyaszek 28-Sep-10 3:07am

Ha I think I have read this somewhere in OpenCV book but how valid that is I don't know.
My solution was working but I had problem with distance measure not groving linear way eg. at 10cm it was showing 10 at 20cm 30 at 40cm 90 etc. Maybe thats the reason.
What is P1 anyway?

mateusz.matyaszek 28-Sep-10 3:18am

Ok P is "new camera matrix" - which doesn't explain anything. I guess thats why I have used camera matrix M. If you find explanation whats the difference please post it here.

hasanm321 · Answer 4 · 2010-09-28T00:34:00

Solution 4

i got p1 and p2 from calibration using Bouguet method.
Without new camera matrix the result is like yours not linear.
But as soon as i use p1 the result is much reliable.
For 100cm im getting around 110 to 122cm
for 200cm im geeting 204 around to 221cm

I am surprised by my result.

Posted 28-Sep-10 0:34am

hasanm321

cccarmen · Answer 5 · 2010-11-14T13:19:00

Hi hasanm321,

I performed the perspective transformation to 3D coordinates (X,Y,Z). The height,Y and distance,Z coordinates are fairly accurate. But the X coordinate is not what I expect. For a point that is about 1 meter to the right of the Left camera, I get X coordinate of -0.04meter. Did you have this problem? What is the X coordinate relative to?
Thanks for your help