Zed camera triangulatePoints via cv2

[stephanschulz]

Hello I hope it is ok to contact you this way about one of your articles: https://temugeb.github.io/opencv/python/2021/02/02/stereo-camera-calibration-and-triangulation.html

I am trying to triangulate the 3d xyz from two sets of 2d points. But my 3d xyz values do not seem to make sense when using the non-SDK version of Stereolabs code. I am using cv2.triangulatePoints not DLT like so:

        point_4d_hom = cv2.triangulatePoints(camera_matrix_left, camera_matrix_right, left_point, right_point)
        good_pts_mask = np.where(point_4d_hom[3]!= 0)[0]
        point_4d = point_4d_hom / point_4d_hom[3] 

Here my whole code which is mainly based on zed_opencv_native.py example https://gist.github.com/stephanschulz/158fb66c8f7516e0f95bc10a846bdb3f#file-zed3-py-L202

I am probably skipping a few steps but am unsure which. In your article you are doing the while calibration from scratch, while I am using the calibration file the Stereolabs provides for my specific camera.

Here is a short video in which I show how via the mouse I select the point pairs. The x,yz text print out shows how z most of the time is around -65

Any advice would be greatly appreciate. Thank you.

[TemugeB]

Hi,

I don't know if you've figured out your problem but try this method for getting the 3d point:

#make sure variables are numpy arrays
def DLT(P1, P2, point1, point2):

    A = [point1[1]*P1[2,:] - P1[1,:],
         P1[0,:] - point1[0]*P1[2,:],
         point2[1]*P2[2,:] - P2[1,:],
         P2[0,:] - point2[0]*P2[2,:]
        ]
    A = np.array(A).reshape((4,4))
    #print('A: ')
    #print(A)

    B = A.transpose() @ A
    from scipy import linalg
    U, s, Vh = linalg.svd(B, full_matrices = False)

    #print('Triangulated point: ')
    #print(Vh[3,0:3]/Vh[3,3])
    return Vh[3,0:3]/Vh[3,3]

P0 and P1 are your camera projection matrices. point1 and point2 are 2D pixel coordinates in each camera frame. So point1 = [px_x, px_y]. This should give you the 3d triangulated point if your projection matrices are correct. If you need example of how this code is used, check my repository here. The code I show is in utils.py.

The other thing you can do to check if your projection matrix is correct is to draw the world coordinate axes in the camera frame. Try something like this. This will draw the world coordinate axes in each camera frame.

#define axes points in homogeneous coorindates
axes = np.array([[0,0,0,1], [1,0,0,1], [0,1,0,1], [0,0,1,1]])
#project to camera view
projected_axes = (P @ axes.T).T
#Remove the homogeneous coordinate information
projected_axes = projected_axes[:,:2]/projected_axes[:,2:3]
#convert to integers because image space is integer coorindates
projected_axes = projected_axes.astype('int32')
#define some colors for your axes
cols = [(0,0,255), (0,255,0), (255,0,0)] #the axes are drawn with ['red', 'green', 'blue'] colors

#draw the axes on the camera image
    for _ax, _col in zip(projected_axes[1:], cols):
        _o = projected_axes[0] #origin point
        cv.line(image, (_o[0], _o[1]), (_ax[0], _ax[1]), _col, 1)

If you draw on both camera frames, you can visually check if the world space coordinate origin matches in both camera views. This will not work however if the world space origin is defined outside the view of the cameras.

Hopefully this helps.

[stephanschulz]

Thank you for your response.

I followed your suggestion and tried DLT https://gist.github.com/stephanschulz/d2c20c8ebb731d7362250b11730b1359#file-zed4-py-L252

It seems to return the same values as what cv2.triangulatePoints(camera_matrix_left, camera_matrix_right, left_point, right_point) returns.

I am using the Zed and i am using the supplied calibration file as can be downloaded specific to each serial number. This means i do not have to do the checker board calibration you mentioned since the calibration file is used to generate the cameraMatrix_left and cameraMatrix_right during the init_calibration. Right?

Am i right to assume when you say camera projection matrices that is the same as my mentioned cameraMatrix_left ?

I tried to draw the coordinate axes in the camera frame. I replaced your P with cameraMatrix_left and it now draws a line from top left to center of left camera image. Not sure if that's what it was meant to do?

I am also wondering if i might be passing the wrong pixel pair points to DLT() and triangulatePoints. My right point is inside the right camera image and so offset by at least the by what ever one camera's width is. Are the functions maybe assuming the images have the same origin and the X difference between two pixel pairs should not include the camera width?

Thank you for your advice.

[TemugeB]

Hi, If DLT and the previous method agree, then the problem is with the points or the projection matrices you are giving to the function. I quickly looked at the stereolabs calibration info and it says that they provide the location of right camera with respect to the left camera. So there is no global origin defined. The code snippet I gave you shows the coordinate axes at the world origin, see the image below. You can't see this because you don't have the world origin defined in front of the camera. But this is not a big problem. Your options are:

1. Use calibration pattern to define the world origin.
2. Simply use the left camera view position as the world origin. I went over this some time ago here. Try to read my answer there. You need to calculate the projection matrix like this:

   RT = np.concatenate([R, T], axis = 1)
   P = cmtx @ RT

   Where cmtx is the camera intrinsic matrix, R the rotation matrix and T the translation vector. If you don't want to use calibration pattern, then you can set R = np.eye(3) and T = np.zeros((3, 1)) for left camera and use the values provided on the stereolabs website for right camera R and T. This simply overlaps world origin with the left camera coodrindate system.

By the way, the 3D values that are printed on your screenshot look perfectly reasonable. Z = -68mm, so about 7cm from the camera, which seem reasonable. Can't tell the scale of the stuff in the background but it could be -68cm, which again is reasonable. If you're using projection matrix provided by stereolab, then likely they did exactly what I described above, so the 3D points should come out like that. Try to draw you triangulated points in 3D and see if they make sense.

Nice plants by the way. Hope this helps.

[stephanschulz]

Thank you again. I will try your R = np.eye(3) and T = np.zeros((3, 1)) suggestion to better evaluate the accuracy provided by strereolabs' calibration data.

By moving the right image "on top" of the left image's coordinates by doing right_point = (x-2208,left_point[1]) https://gist.github.com/stephanschulz/d2c20c8ebb731d7362250b11730b1359#file-zed4-py-L174 the resulting Z values do look more like mm values. But they are still not very accurate. For example, the code gives me 2m value for a 2.4m away object, or 4.9m for a 5.3m away object. I guess this type of 3d localization will never be very accurate judging by this post https://github.com/stereolabs/zed-examples/issues/44

FYI, the above -65 value was more a 1.4m away object.

Nice plants by the way. Hope this helps.

You should see our living room :)