Search code examples
pythonopencvcomputer-visioncamera-calibrationtriangulation

cv2 triangulatePoints always returns same Z value

I am trying to get 3D points using cv2.triangulatePoints but it always returns almost same Z value. My output looks like this: output image and output 3d points 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

Output after using P1 and P2 that I mentioned in comments: enter image description here

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

Solution

  • 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: enter image description here