# Triangulation with openCV

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 ?

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)] )?

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

Yes you must do it. Otherwise system cannot be solved

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.

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)