我现在在尝试把KITTI数据集的3D激光雷达点投影到图像上，下面是我用来计算投影的脚本：

# -*- coding: utf-8 -*-

import numpy as np

# Define Matrices
K = np.array([
    [959.1977, 0.0, 694.4383],
    [0.0, 952.9324, 241.6793],
    [0.0, 0.0, 1.0]
])

P = np.array([
    [707.0912, 0.0, 601.8873, 46.88783],
    [0.0, 707.0912, 183.1104, 0.1178601],
    [0.0, 0.0, 1.0, 0.00620]
])

R = np.array([
    [0.9999019, 0.01307921, -0.005015634],
    [-0.01307809, 0.9999144, 0.0002561203],
    [0.005018555, -0.0001905003, 0.9999874]
])

T = np.array([[0.334], [0.062], [-0.075]])  # Translation vector
R_new = R.copy()
R_new[[0, 2], :] = R[[2, 0], :]  # Swap X and Z rows
R_new[:, [0, 2]] = R[:, [2, 0]]  # Swap X and Z columns
T = T[[2, 1, 0]]  # Swap X and Z in translation
T[1] = -T[1]  # Flip Y-axis translation

# Construct 4×4 Transformation Matrix
T_cam_lidar = np.eye(4)
#T_cam_lidar[:3, :3] = R  # Assign rotation
T_cam_lidar[:3, :3] = R_new
T_cam_lidar[:3, 3] = T.flatten()  # Assign translation

print("4×4 LiDAR-to-Camera Transformation Matrix:\n", T_cam_lidar)
print("Translation Vector T (after processing):\n", T.flatten())

# Function to Project LiDAR Points to Image
def project_lidar_to_image(lidar_points, P, T_cam_lidar):
    """ Project LiDAR points to image plane """
    # Convert LiDAR points to homogeneous coordinates (N, 4)
    lidar_hom = np.hstack((lidar_points, np.ones((lidar_points.shape[0], 1))))  # (N, 4)

    # Transform LiDAR points to the camera frame
    cam_points = np.dot(T_cam_lidar, lidar_hom.T).T  # (N, 4)
    cam_points = cam_points[:, :3]  # Drop homogeneous coordinate

    # 🚫 Remove points with negative depth (Z < 0)
    cam_points = cam_points[cam_points[:, 2] > 0]
    
    # Convert to homogeneous coordinates for projection
    cam_hom = np.hstack((cam_points, np.ones((cam_points.shape[0], 1))))  # (N, 4)
    img_points = np.dot(P, cam_hom.T).T  # (N, 3)

    # Normalize (divide by depth)
    valid_z = img_points[:, 2] > 1e-6  # Avoid division by near-zero Z values
    img_points = img_points[valid_z]  # Keep only valid depth points
    img_points[:, 0] /= img_points[:, 2]
    img_points[:, 1] /= img_points[:, 2]

    print("Projected X min/max:", np.min(img_points[:, 0]), np.max(img_points[:, 0]))
    print("Projected Y min/max:", np.min(img_points[:, 1]), np.max(img_points[:, 1]))


    return img_points[:, :2]  # Return (u, v) pixel coordinates

# Example LiDAR Points (Replace with actual data)
lidar_points = np.load("lidar_points.npy")  # LiDAR points
lidar_points = lidar_points[:, [0, 2, 1]]  # Swap X, Z, Y → X, Y, Z

# Project LiDAR Points to Image
img_points = project_lidar_to_image(lidar_points, P, T_cam_lidar)
np.save("projected_lidar.npy", img_points)

print("Projected 2D Image Points:\n", img_points[:10])  # Show first 5 points
print("First 5 LiDAR points (before projection):\n", lidar_points[:5])

运行这个脚本后，投影到图像上的结果是这样的：

我觉得问题可能出在LiDAR点的Y和Z轴上，X轴看起来没什么问题。我试过用MATLAB的projectLidarPointsOnImage函数，但连输出都没有。有没有人知道问题出在哪，或者我该怎么解决这个问题？非常感谢！（不知道这个信息有没有用：我是在Ubuntu 18.04上运行的，之前用YOLOv8做了目标检测）

备注：内容来源于stack exchange，提问作者dylan quek