KITTI数据集点云匹配图像RGB特征的实现方法问询

问题描述 #

我正在使用KITTI数据集，为3D目标检测模型进行早期阶段的点云与图像融合。当前点云每个点的特征向量为[x,y,z,r]，其中x、y、z是坐标，r是强度。我需要为每个点添加对应图像的R、G、B数据，扩展为[x,y,z,r,R,G,B]特征向量。我了解需利用标定文件将点云投影到图像平面并获取RGB数据，但不清楚具体实现方式，也不知道如何过滤出图像视野范围内的点云。我找到了一段相关代码：

name = ''
img = ''
binary =''
with open(f'./testing/calib/{name}.txt','r') as f:
    calib = f.readlines()
# P2 (3 x 4) for left eye
P2 = np.array([float(x) for x in calib[2].strip('\n').split(' ')[1:]]).reshape(3, 4)
R0_rect = np.array([float(x) for x in calib[4].strip('\n').split(' ')[1:]]).reshape(3, 3)
# Add a 1 in bottom-right, reshape to 4 x 4
R0_rect = np.insert(R0_rect, 3, values=[0, 0, 0], axis=0)
R0_rect = np.insert(R0_rect, 3, values=[0, 0, 0, 1], axis=1)
Tr_velo_to_cam = np.array([float(x) for x in calib[5].strip('\n').split(' ')[1:]]).reshape(3, 4)
Tr_velo_to_cam = np.insert(Tr_velo_to_cam, 3, values=[0, 0, 0, 1], axis=0)

# read raw data from binary
scan = np.fromfile(binary, dtype=np.float32).reshape((-1, 4))
points = scan[:, 0:3]  # lidar xyz (front, left, up)
# TODO: use fov filter?
velo = np.insert(points, 3, 1, axis=1).T
velo = np.delete(velo, np.where(velo[0, :] < 0), axis=1)
cam = P2.dot(R0_rect.dot(Tr_velo_to_cam.dot(velo)))
cam = np.delete(cam, np.where(cam[2, :] < 0), axis=1)
# get u,v,z
cam[:2] /= cam[2, :]
# do projection staff
plt.figure(figsize=(12, 5), dpi=96, tight_layout=True)
png = mpimg.imread(img)
IMG_H, IMG_W, _ = png.shape
# restrict canvas in range
plt.axis([0, IMG_W, IMG_H, 0])
plt.imshow(png)
# filter point out of canvas
u, v, z = cam
u_out = np.logical_or(u < 0, u > IMG_W)
v_out = np.logical_or(v < 0, v > IMG_H)
outlier = np.logical_or(u_out, v_out)
cam = np.delete(cam, np.where(outlier), axis=1)
# generate color map from depth
u, v, z = cam
plt.scatter([u], [v], c=[z], cmap='rainbow_r', alpha=0.5, s=2)
plt.title(name)
plt.savefig(f'{name}.png', bbox_inches='tight')
plt.show()

现寻求能正确实现该点云RGB特征扩展功能的函数（Python或其他编程语言）。

实现方案：Python函数 #

以下函数完整实现点云投影、有效点过滤、RGB特征扩展的功能，严格遵循KITTI数据集的标定规则：

import numpy as np
import cv2

def extend_point_cloud_with_rgb(velo_path, img_path, calib_path):
    # 解析KITTI标定文件，获取投影所需矩阵
    with open(calib_path, 'r') as f:
        calib_lines = f.readlines()
    
    # 左相机投影矩阵P2 (3x4)
    P2 = np.array([float(x) for x in calib_lines[2].strip().split()[1:]]).reshape(3, 4)
    # 相机外参校正矩阵R0_rect，扩展为4x4齐次矩阵
    R0_rect = np.array([float(x) for x in calib_lines[4].strip().split()[1:]]).reshape(3, 3)
    R0_rect = np.hstack((R0_rect, np.zeros((3, 1))))
    R0_rect = np.vstack((R0_rect, np.array([0, 0, 0, 1])))
    # 激光雷达到相机的转换矩阵Tr_velo_to_cam，扩展为4x4齐次矩阵
    Tr_velo_to_cam = np.array([float(x) for x in calib_lines[5].strip().split()[1:]]).reshape(3, 4)
    Tr_velo_to_cam = np.vstack((Tr_velo_to_cam, np.array([0, 0, 0, 1])))

    # 读取原始点云数据，格式为[x,y,z,r]
    point_cloud = np.fromfile(velo_path, dtype=np.float32).reshape(-1, 4)
    points_velo = point_cloud[:, :3]  # 提取三维坐标
    points_intensity = point_cloud[:, 3:4]  # 提取强度特征

    # 将点云转换为齐次坐标，进行坐标系转换
    points_velo_hom = np.hstack((points_velo, np.ones((points_velo.shape[0], 1)))).T
    # 激光雷达坐标 -> 校正后相机坐标 -> 图像平面坐标
    points_cam_hom = P2 @ R0_rect @ Tr_velo_to_cam @ points_velo_hom
    # 齐次坐标转像素坐标（除以深度值z）
    points_cam = points_cam_hom[:3, :] / points_cam_hom[2, :]
    u, v, z = points_cam  # u为像素横坐标，v为像素纵坐标，z为相机坐标系下的深度

    # 读取图像并获取尺寸
    img = cv2.imread(img_path)
    img_h, img_w = img.shape[:2]

    # 过滤有效点：只保留能投影到图像内的点
    # 条件1：激光雷达前方的点（x>0，KITTI激光雷达x轴朝前）
    mask_front = points_velo[:, 0] > 0
    # 条件2：投影后深度为正（在相机前方）
    mask_depth = z > 0
    # 条件3：像素坐标在图像边界内
    mask_u = (u >= 0) & (u < img_w)
    mask_v = (v >= 0) & (v < img_h)
    # 合并所有过滤条件
    valid_mask = mask_front & mask_depth & mask_u & mask_v

    # 提取有效点对应的RGB值（OpenCV默认BGR，转为RGB）
    u_valid = u[valid_mask].astype(np.int32)
    v_valid = v[valid_mask].astype(np.int32)
    rgb_valid = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)[v_valid, u_valid]

    # 拼接扩展后的特征向量：[x,y,z,r,R,G,B]
    valid_points = point_cloud[valid_mask]
    extended_points = np.hstack((valid_points, rgb_valid))

    return extended_points

函数调用示例 #

# 替换为你的KITTI文件路径
velo_file = "./testing/velodyne/000000.bin"
img_file = "./testing/image_2/000000.png"
calib_file = "./testing/calib/000000.txt"

# 执行扩展操作
extended_pc = extend_point_cloud_with_rgb(velo_file, img_file, calib_file)

# 输出结果信息
print(f"原始点云总数: {np.fromfile(velo_file, dtype=np.float32).reshape(-1,4).shape[0]}")
print(f"扩展后有效点数量: {extended_pc.shape[0]}")
print(f"扩展后特征维度: {extended_pc.shape[1]}")  # 输出7，对应x,y,z,r,R,G,B

关键说明 #

• 坐标系规则：KITTI激光雷达坐标系x轴朝前，y轴朝左，z轴朝上；相机图像坐标系左上角为原点，u轴向右，v轴向下
• 矩阵运算顺序：严格遵循KITTI标定文件定义的转换流程，确保投影坐标准确
• 过滤逻辑：多重过滤条件避免无效点和索引越界问题，只保留有对应图像RGB值的点
• 图像格式处理：将OpenCV读取的BGR格式转为RGB，与点云特征的RGB顺序匹配

内容的提问来源于stack exchange，提问作者Den Mor