i'm trying to use to calibration information provided with the dataset, but I cannot get good correspondence between coordinates in the world space and the pixel space. I use the provided intrincis matrix, distortion coffecients and for camera rotation I use the pitch of 10 degrees (nothing else is provided with the data).
I am using the below code which should draw horizontal lines in what should be the distance of 5 meters, 10 meters, etc, but as you can see the lines distances are way off - the first line is 5 meters away, but in the image is very close to the car.
What is even more worrying is that these drawn lines should converge to the vanishing point (the left edge of the lines should correspond to the world coordinates of [X=0, Y=0, Z=Inf]
), but this point in the pixel space seems to be higher and lot more to the left that where it actually is. Am I missing something here?
import cv2
import numpy as np
import matplotlib.pyplot as plt
def world_points_to_pixel_space(keypoints_3d, proj_mat):
keypoints_3d_hom = np.hstack([keypoints_3d, np.ones((keypoints_3d.shape[0], 1), dtype=np.float32)])
keypoints_hom = np.dot(proj_mat, keypoints_3d_hom.T).T
keypoints = np.vstack([keypoints_hom[:, 0] / keypoints_hom[:, 2], keypoints_hom[:, 1] / keypoints_hom[:, 2]]).T
return keypoints
#Load calibration details
calib = {"cam_height_mm": 1270, "cam_pitch_deg": -10, "dim": [1920, 1080], "K": [[1004.8374471951423, 0.0, 960.1025514993675], [0.0, 1004.3912782107128, 573.5538287373604], [0.0, 0.0, 1.0]], "D": [[-0.02748054291929438], [-0.007055051080370751], [-0.039625194298025156], [0.019310795479533783]]}
mtx = np.array(calib['K'])
cam_height = np.array(calib['cam_height_mm']) / 1000.
distCoeffs = np.array(calib['D'])
h, w = (1080, 1920)
pitch = calib['cam_pitch_deg'] * math.pi / 180.
#Load image
image_path = '$PIE_PATH/images/set01/video_0002/04951.png'
img_data = cv2.imread(image_path)
img_data = cv2.cvtColor(img_data, cv2.COLOR_BGR2RGB)
#Undistort image
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, distCoeffs, (w, h), 0, (w, h), centerPrincipalPoint=True)
img_data = cv2.undistort(img_data, mtx, distCoeffs, None, newcameramtx)
# Get projection matrix as K * R
K = np.hstack([newcameramtx, np.zeros((3, 1))])
rot = np.array([[1, 0, 0, 0], [0, np.cos(pitch), -np.sin(pitch), 0], [0, np.sin(pitch), np.cos(pitch), 0], [0, 0, 0, 1]])
proj_mat = np.matmul(K, rot)
# Draw lines on the road which are 2 meters wide, with left edge in the center of the camera (X = 0m)
line_width_in_meters = 2
for distance in range(5, 200, 5):
points_3d = np.asarray([[0, cam_height, distance], [line_width_in_meters, cam_height, distance]], dtype='float32')
points_pixel = world_points_to_pixel_space(points_3d, proj_mat).astype(np.int32)
cv2.line(img_data, tuple(points_pixel[0, :]), tuple(points_pixel[1, :]), (255, 0, 0), lineType=cv2.LINE_AA)
if distance <= 15:
cv2.putText(img_data, '{}m'.format(distance), tuple(points_pixel[1,:]), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0))
plt.imshow(img_data)
plt.show(block=True)