Triangulation with openCV

asked 2017-05-03 02:38:41 -0600

Eiyam gravatar image

I'm working on 3-D tracking code, and i decided to use openCV functions to implement the algorithm.

1- Intrinsic Parameters:

I have two cameras and i computed the intrinsic parameters of each one using chessboard to get "_camMatrix"

2- Extrinsic Parameters:

I decided to use "solvePnP" to calculate the extrinsic parameters, So i defined 6 3D points in "_pts3d" vector: [(-1, 1, 0),(0, 1, 0),(1, 1, 0),(1,-1, 0),(0,-1, 0),(-1,-1, 0)] and for 6 times i get a new frame and push the 2D point to "_pts2d" vector, and finally calculate the "_rvec" and "_tvec" of the camera.

 cv::solvePnP(_pts3d, _pts2d, _camMatrix, _disCoeff, _rvec, _tvec, false,  cv::SOLVEPNP_ITERATIVE);

3- Build the Projection Matrix:

 cv::Mat rotM(3, 3, CV_64F);
 Rodrigues(_rvec, rotM);

 // push tvec to transposed Mat
 cv::Mat rotMT = rotM.t();
 rotMT.push_back(_tvec.reshape(1, 1));

 // transpose back, and multiply
 cv::Mat _projMat = _camMatrix * rotMT.t();

At this point i have two projection matrices for each camera P1 and P2, Now it's the time for triangulation: for infinity i get a new 2D point from each camera "pointsMat1" and "pointsMat1" then triangulate them into "pnt3D":

  cv::triangulatePoints(P1, P2, pointsMat1, pointsMat2, pnt3D);

then calculate X,Y,Z :

  X = pnt3D.at<double>(0,0) / pnt3D.at<double>(3,0);
  Y = pnt3D.at<double>(1,0) / pnt3D.at<double>(3,0);
  Z = pnt3D.at<double>(2,0) / pnt3D.at<double>(3,0);

Finally, i'm sending those reading to my graphics engine in openGL but there is a big error in the X,Y,Z, is there anything wrong in my calculations ?

edit retag flag offensive close merge delete

Comments

Why don't you use stereocalibrate ?

All points are in plane z=0 ([(-1, 1, 0),(0, 1, 0),(1, 1, 0),(1,-1, 0),(0,-1, 0),(-1,-1, 0)] )?

LBerger gravatar imageLBerger ( 2017-05-03 03:28:20 -0600 )edit

do you mean i need to define my six points in different z planes ?

Eiyam gravatar imageEiyam ( 2017-05-03 11:41:11 -0600 )edit

Yes you must do it. Otherwise system cannot be solved

LBerger gravatar imageLBerger ( 2017-05-03 14:03:02 -0600 )edit

Thank you, but i'm wondered how in this video https://www.youtube.com/watch?v=UVzJl..., he could to have very good 3d tracking when he used 6 points at z=1. Note: he used the same two functions solvePNP and triangulatePoints, but i don't have the full code of him.

Eiyam gravatar imageEiyam ( 2017-05-03 14:25:12 -0600 )edit

I haven't got time to investigate more but I think tracker size is known and it is not in your equation at the moment. when you write z=(-1,1,0) it is tracker center. You have to measure radius and you can find distance (if intrinsic parameters are known)

LBerger gravatar imageLBerger ( 2017-05-03 15:22:36 -0600 )edit