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

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,

- I use
**microsoft lifeCam hd-3000**as right came and**microsoft lifeCam VX-800**as left camera - two camera have
**different focal length** - both image resolution is
**640 X 480** - the chessboard grid size is
**3cm x 3cm** - 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 ...
```