将KITTI velodyne投影到图像会产生一条窄条

Question

我正在尝试将KITTI velodyne投影到左侧的相机图像上。我在KITTI devkit中遵循了README，但是结果不对了-这些点被投影为图像顶部的窄带。该频段看起来似乎有些分布，所以我怀疑校准矩阵做错了什么。还是在PIL.ImageDraw.point中？

我使用的投影方程式是根据KITTI devkit文档提供的：

• x = P2 * R0_rect * Tr_velo_to_cam * y，其中
  • y是一个4xN矩阵，具有N格式的XYZL个点（L是发光的），
  • Tr_velo_to_cam是3x4 velodyne相机变换矩阵
  • R0_rect是3x3外部摄像机旋转矩阵
  • P2是3x3固有相机投影矩阵

下面是代码，其STDIO和生成的图像。

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)

生成的图像：

Answer 1

发现问题：注意pts2d的尺寸为(2, N)，这意味着它总共有N个点。但是，ImageDraw例程希望它是Nx2或1x2N行向量，具有交替的x和y值。尽管我无法使point例程与Nx2输入配合使用，但我将其置于for循环中（在转置了点之后），并且可以正常工作。

# ...
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))
# ...