Ytukyg

I am trying to project the KITTI velodyne onto the left camera images. I followed the README in the KITTI devkit, but the result is off -- the points are projected as a narrow band on the top of the image. The band looks like it has some distribution, so I am suspecting I am doing something wrong with the calibration matrices. Or maybe in the PIL.ImageDraw.point?

The projection equation that I am using is per the KITTI devkit documentation:

• 
  x = P2 * R0_rect * Tr_velo_to_cam * y, where
  
  
  
  
  • 
    y is a 4xN matrix with N points in XYZL format (L is the luminescence),
  
  
  • 
    Tr_velo_to_cam is the 3x4 velodyne to camera transformation matrix
  
  
  • 
    R0_rect is the 3x3 extrinsic camera rotation matrix
  
  
  • 
    P2 is the 3x3 intrinsic camera projection matrix
  
  
  
  

Below is the code, STDIO of it, and the produced image.

test.py:

import numpy as np

import os

from PIL import Image, ImageDraw



DATASET_PATH = "<DATASET PATH HERE>"



vld_path = os.path.join(DATASET_PATH, "velodyne/{:06d}.bin")

img_path = os.path.join(DATASET_PATH, "image_2/{:06d}.png")

clb_path = os.path.join(DATASET_PATH, "calib/{:06d}.txt")



frame_num = 58



# Load files

img = Image.open(img_path.format(frame_num))

clb = {}

with open(clb_path.format(frame_num), 'r') as clb_f:

  for line in clb_f:

    calib_line = line.split(':')

    if len(calib_line) < 2:

      continue

    key = calib_line[0]

    value = np.array(list(map(float, calib_line[1].split())))

    value = value.reshape((3, -1))

    clb[key] = value

vld = np.fromfile(vld_path.format(frame_num), dtype=np.float32)

vld = vld.reshape((-1, 4)).T



print("img.shape:", np.shape(img))

print("P2.shape:", clb['P2'].shape)

print("R0_rect.shape:", clb['R0_rect'].shape)

print("Tr_velo_to_cam.shape:", clb['Tr_velo_to_cam'].shape)

print("vld.shape:", vld.shape)



# Reshape calibration files

P2 = clb['P2']

R0 = np.eye(4)

R0[:-1, :-1] = clb['R0_rect']

Tr = np.eye(4)

Tr[:-1, :] = clb['Tr_velo_to_cam']



# Prepare 3d points

pts3d = vld[:, vld[-1, :] > 0].copy()

pts3d[-1, :] = 1



# Project 3d points

pts3d_cam = R0 @ Tr @ pts3d

mask = pts3d_cam[2, :] >= 0  # Z >= 0

pts2d_cam = P2 @ pts3d_cam[:, mask]

pts2d = (pts2d_cam / pts2d_cam[2, :])[:-1, :]



print("pts2d.shape:", pts2d.shape)



# Draw the points

img_draw = ImageDraw.Draw(img)

img_draw.point(pts2d, fill=(255, 0, 0))

img.show()

STDOUT:

$> python ./test.py 

img.shape: (370, 1224, 3)

P2.shape: (3, 4)

R0_rect.shape: (3, 3)

Tr_velo_to_cam.shape: (3, 4)

vld.shape: (4, 115052)

pts2d.shape: (2, 53119)

Produced image:

Found the problem: Notice that the dimensions of the pts2d are (2, N), which means it has N points in total. However, the ImageDraw routine expects it to be either Nx2 or 1x2N row vector with alternating x and y values. Although I couldn't get the point routine to work with the Nx2 input, I put it in the for loop (after transposing the points), and it worked.

# ...

pts2d = (pts2d_cam / pts2d_cam[2, :])[:-1, :].T



print("pts2d.shape:", pts2d.shape)



# Draw the points

img_draw = ImageDraw.Draw(img)

for point in pts2d:

  img_draw.point(point, fill=(255, 0, 0))

# ...