Search code examples
opencvtriangulationstereo-3d

Opencv - Triangulation origin with stereo system

I m using a stereo system and so i m trying to get world coordinates of some points. I can do it with specific calibration for each camera and then i calculate rotation matrix and translation vector. And finally i triangulate but i m not sure of the origin of the world coordinates.

As you can see on my figure, values correspond to depth value but they shoud be close of 400 as it is flat. So i suppose that the origin is the left camera that s why it variates...

enter image description here

A piece of my code with my projective arrays and triangulate function :

#C1 and C2 are the cameras matrix (left and rig)
#R_0 and T_0 are the transformation between cameras
#Coord1 and Coord2 are the correspondant coordinates of left and right respectively
P1 = np.dot(C1,np.hstack((np.identity(3),np.zeros((3,1))))) 

P2 =np.dot(C2,np.hstack(((R_0),T_0)))

for i in range(Coord1.shape[0])
    z = cv2.triangulatePoints(P1, P2, Coord1[i,],Coord2[i,])

My cameras present an angle, the Z axis direction (direction of the depth) is not normal to my surface. And i want the depth from the baseline direction. So i have to rotate my points?

enter image description here

Solution

  • In the following code, points4DNorm will contains the 3D points in world coordinates. I didn't use rectify at all, I just used a few 2d/3d point pairs and then solvePnPRansac on these pairs. 

    // rotMat1, rotMat2,tvec1 and tvec2 are retrieved from solvePnPRansac and Rodrigues
        Mat points4D;
        rotMat1.copyTo(myCam1.ProjectionMat(Rect(0, 0, 3, 3)));
        tvec1.copyTo(myCam1.ProjectionMat(Rect(3, 0, 1, 3)));

        rotMat2.copyTo(myCam2.ProjectionMat(Rect(0, 0, 3, 3)));
        tvec2.copyTo(myCam2.ProjectionMat(Rect(3, 0, 1, 3)));

        myCam1.ProjectionMat = myCam1.NewCameraMat* myCam1.ProjectionMat;
        myCam2.ProjectionMat = myCam2.NewCameraMat* myCam2.ProjectionMat;
        triangulatePoints(myCam1.ProjectionMat, myCam2.ProjectionMat, balls12d, balls22d, points4D);
        Mat points4DNorm;
      for (int k = 0; k < points4D.cols;k++)
      {
        points4D.at<float>(0, k) = (points4D.at<float>(0, k) / points4D.at<float>(3, k))/304.8;
        points4D.at<float>(1, k) = (points4D.at<float>(1, k) / points4D.at<float>(3, k)) / 304.8;
        points4D.at<float>(2, k) = (points4D.at<float>(2, k) / points4D.at<float>(3, k)) / 304.8;
        points4D.at<float>(3, k) = (points4D.at<float>(3, k) / points4D.at<float>(3, k)) / 304.8;
        std::cout << std::setprecision(9) << points4D.at<float>(0, k) << "," << points4D.at<float>(1, k) << "," << points4D.at<float>(2, k) << std::endl;
       }