[Python] Projecting LiDAR points onto Image

I am trying to project my 3D lidar points onto an image using the KITTI dataset. This is my script to compute the projection for the lidar points

# -*- 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])


and this is the output when i project it onto my image



I think the X of my lidar points doesn't seem to be the issue but more of the Y and Z. I've tried using MATLAB's projectLidarPointsOnImage (https://www.mathworks.com/help/lidar/ref/projectlidarpointsonimage.html) but I didn't even receive an output. Anyone knows where the problem lies and how I can solve it? Thank you so much. (not sure if this information is useful but I'm running this on Ubuntu 18.04 and I used yolov8 for my object detection)

Continue reading...