cv2 triangulatePoints always returns same Z value

Question

I am trying to get 3D points using cv2.triangulatePoints but it always returns almost same Z value. My output looks like this: As it seen, all points are in almost same Z value. There is no depth.

Here is my triangulation:

    def triangulate(self, proj_mat1, pts0, pts1):
        proj_mat0 = np.zeros((3,4))
        proj_mat0[:, :3] = np.eye(3)
        pts0, pts1 = self.normalize(pts0), self.normalize(pts1) 
        pts4d = cv2.triangulatePoints(proj_mat0, proj_mat1, pts0.T, pts1.T).T
        pts4d /= pts4d[:, 3:]
        out = np.delete(pts4d, 3, 1)
        print(out)
        return out

Here is my projection matrix calculation:

    def getP(self, rmat, tvec):
       P = np.concatenate([rmat, tvec.reshape(3, 1)], axis = 1)
       return P

Here is the part that I get rmat, tvec and call triangulation:

    E, mask = cv2.findEssentialMat(np.array(aa), np.array(bb), self.K)
    _, R, t, mask = cv2.recoverPose(E, np.array(aa), np.array(bb), self.K)
    proj_mat1 = self.getP(R, t)
    out = self.triangulate(proj_mat1, np.array(aa, dtype = np.float32), np.array(bb, dtype = np.float32))

My camera matrix:

array([[787.8113353 ,   0.        , 318.49905794],
       [  0.        , 786.9638204 , 245.98673477],
       [  0.        ,   0.        ,   1.        ]])

My projection matrix 1:

array([[1., 0., 0., 0.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.]])

Explanations:

• aa and bb are matched points from 2 frames.
• self.K is my camera matrix
• rotation and translation matrices are extracted from Essential matrix
• Essential matrix calculated from matched keypoints. It changes every frame.
• Projection matrix 2 changes every frame.

Output after changing first projection matrix (I switched from matplotlib to pangolin as 3D visualization tool):

Output after using P1 and P2 that I mentioned in comments:

Where is my mistake? Please let me know if any further information needed. I will update my question.

Answer 1

Unfortunately I don't have the possibility to double-check directly but my gut feeling is that the issues you are facing are essentially due to the choice of your first projection matrix

I did some research and I found this great paper with both theory and practice. Despite differing a little bit from your approach, there is a thing that is worth saying

If you check carefully, the first projection matrix is exactly the camera matrix with an additional last column equal to zero. In fact, the rotation matrix for the first camera reduces to the identity matrix and the corresponding translation vector is a null vector, so using this general formula:

P = KT

where P is the projection matrix, K the camera matrix and T the matrix obtained by the rotation matrix R flanked by the translation vector t according to:

T = [R|t]

then you will get:

Coming back to your case, first of all I would suggest to change your first projection matrix as just said

Also, I understand that you are planned to work with something different at every frame but if after the suggested change the things still don't match then in your shoes I'd start working with just 2 images [I think you implicitly did already to create the correspondence between aa and bb], calculating first the matrices with your algorithm and then checking with the ones obtained following the article above

In this way you would be able to understand/debug which matrices are creating you troubles

Answer 2

Thank you so much for all the effort @Antonino. My webcams were pretty bad. After changing every part of my code and making many trials I decided to change my webcams and bought good webcams. It worked :D Here is the result: