using solvePnP and triangulatePoints to get the object 3D points at 10 meter for obstacle coordinate estimation

edit

Hi,

I am a junior college student who is new to computer vision, right now me and my classmates are doing a self-driving electric car project. currently, we planning to use a stereo vision system for obstacle detection at least 10 meter.

Our idea is to use YOLO find the possible obstacle on the road( ex: pedestrian, car, bicycle...), and use the central point of bounding box as the object's pixel point on image, then use this image point find the 3D points of the object.

After looking for the data on internet, i found that I can find the projection matrix of two camera by solvePnP first, then find the 3D points of the object by using triangulatePoints with two camera's projection matrix and object's pixel point on two image taken by two camera.

Next, I want to know the error of 3D coordinate estimation along with the distance of the object. therefore, I did a experiment.

In the experiment,

1. I use microsoft lifeCam hd-3000 as right came and microsoft lifeCam VX-800 as left camera
2. two camera have different focal length
3. both image resolution is 640 X 480
4. the chessboard grid size is 3cm x 3cm
5. baseline is 60 cm

then what I did is

First, I calibrate two camera respectively, then I get the camera matrix and distortion coefficient of both camera, after that, I use getOptimalNewCameraMatrix function to get new camera Matrix and roi of two camera.

new_cameraMatrixLeft, roi_left = cv2.getOptimalNewCameraMatrix(cameraMatrixLeft, distCoeffsLeft, (width, height), 1, (width, height))
new_cameraMatrixRight, roi_right = cv2.getOptimalNewCameraMatrix(cameraMatrixRight, distCoeffsRight, (width, height), 1, (width, height))

Second, I put a chessboard in front of the camera 120 cm, here is the left camera image

here is the right camera image

and I set the up-right corner as world coordinate system origin, and z-axis is into image, use the chessboard corner as object.

the object's world coordinate is ( x, y, z )

Third, I undistort the image by using undistort function, the left image size after is 637 X 476, right image is 604 X 444 .

undistort_left_image = cv2.undistort(frame_left, cameraMatrixLeft, distCoeffsLeft, None, new_cameraMatrixLeft)
undistort_right_image = cv2.undistort(frame_right, cameraMatrixRight, distCoeffsRight, None, new_cameraMatrixRight)

then,I use opencv find chessboard corner and cornerSubPix function to find the two camera's undistort image points.

x, y, w, h = roi_left
undistort_left_image = undistort_left_image[y:y+h, x:x+w]
x, y, w, h = roi_right
undistort_right_image = undistort_right_image[y:y+h, x:x+w]

gray_l = cv2.cvtColor(undistort_left_image, cv2.COLOR_BGR2GRAY)
gray_r = cv2.cvtColor(undistort_right_image, cv2.COLOR_BGR2GRAY)
ret_l, corners_l = cv2.findChessboardCorners(gray_l, (column, row), None)
ret_r, corners_r = cv2.findChessboardCorners(gray_r, (column, row), None)

corners2_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)

imgpoints_l.append(corners2_l)
imgpoints_r.append(corners2_r)

imgpoints_l_undistort = imgpoints_l[0]
imgpoints_r_undistort = imgpoints_r[0]

Forth, I use solvePnP function to get the projection matrix of two camera with 63 object points, 63 left and right image points.

_, rotationLeft, translationLeft = cv2.solvePnP(objpoints[0], imgpoints_l_undistort, cameraMatrixLeft, distCoeffsLeft)
_, rotationRight ...