Inconsistent Camera-LiDAR Calibration

[tylertian123]

I'm trying to project LiDAR points from os1 into cam0 using the provided projection_matrix in calib_os1_to_cam0.yaml for each sequence. This works well in most cases, but results in a noticeable offset in some scenarios, for example the ones below:

• Sequence 16 index 1000:
  
• Sequence 16 index 7610:
  

These are far from the only examples, and I have not gone through all sequences, but it seems like sequence 16 in particular has lots of these issues, while e.g. sequence 0 and 3 are better. It's also inconsistent even within the same sequence, for example sequence 16 index 7600 looks perfectly fine:

I have also tried combining the extrinsic matrix and the intrinsics found in calib_cam0_intrinsics.yaml instead of directly using the given projection matrix but that seemed to be even worse. I'm wondering if this is a known issue, whether there are any ways to mitigate it (is improved calibration data available?), or am I just simply using the incorrect calibration matrices?

Here is the code I used to create these visualizations:

#!/usr/bin/python3

import cv2
import numpy as np
import yaml
import sys
from pathlib import Path
from matplotlib import pyplot as plt

def project_pointcloud_to_image(img, pc, ext_lidar_to_cam, proj_lidar_to_cam):
    """
    Projects LiDAR points to image pixels.
    Returns: (N, 3) array of [u, v, depth]
    """
    # 1. Convert to homogeneous coordinates (x, y, z, 1)
    pc_homo = np.hstack((pc[:, :3], np.ones((pc.shape[0], 1), dtype=pc.dtype)))
    
    # 2. Project to image plane (u, v, w)
    # Transpose pc so it is (4, N) for matrix multiplication
    img_pts = proj_lidar_to_cam @ pc_homo.T
    img_pts /= img_pts[2] # Normalize by dividing by w (depth)

    # 3. Transform to camera frame to check depth (z > 0)
    pc_cam_frame = ext_lidar_to_cam @ pc_homo.T

    # 4. Filter: Point must be in front of camera AND within image bounds
    # Note: img_pts is now (3, N) -> [u, v, 1]
    admissible_idx = (pc_cam_frame[2] >= 0) & \
                     (img_pts[0] >= 0) & (img_pts[0] < img.shape[1]) & \
                     (img_pts[1] >= 0) & (img_pts[1] < img.shape[0])
    
    # Extract valid u, v coordinates and corresponding z depth
    valid_uv = img_pts[:2].T[admissible_idx]
    valid_z = pc_cam_frame.T[admissible_idx][:, 2]
    
    return np.hstack([valid_uv, valid_z.reshape(-1, 1)])

def load_calib(calib_path, field):
    with open(calib_path, "r") as f:
        data = yaml.safe_load(f)
    
    # Combined
    mat_data = data[field]
    if "data" in mat_data:
        mat = np.array(mat_data["data"]).reshape(mat_data["rows"], mat_data["cols"])
    else: # Separate R and T
        R = np.array(mat_data["R"]["data"]).reshape(mat_data["R"]["rows"], mat_data["R"]["cols"])
        t = np.array(mat_data["T"])
        mat = np.eye(4)
        mat[:3, :3] = R
        mat[:3, 3] = t

    return mat

if __name__ == "__main__":
    # Usage: <dataset_path> <sequence> <img_index>
    root = Path(sys.argv[1])
    seq = sys.argv[2]
    index = sys.argv[3]
    
    calib_file = root / "calibrations" / seq / "calib_os1_to_cam0.yaml"
    E = load_calib(calib_file, "extrinsic_matrix")
    P = load_calib(calib_file, "projection_matrix")
    calib_intrin = root / "calibrations" / seq / "calib_cam0_intrinsics.yaml"
    K = load_calib(calib_intrin, "projection_matrix")[:3, :3]

    rgb_path = root / "2d_rect" / "cam0" / seq / f"2d_rect_cam0_{seq}_{index}.png"
    pc_path = root / "3d_comp" / "os1" / seq / f"3d_comp_os1_{seq}_{index}.bin"
    assert rgb_path.exists()
    assert pc_path.exists()

    print(f"Visualizing: {rgb_path}, {pc_path}")

    img = cv2.imread(str(rgb_path))
    # Load binary float32 pointcloud (x, y, z, intensity)
    pc = np.fromfile(pc_path, dtype=np.float32).reshape((-1, 4))

    depth_pts = project_pointcloud_to_image(img, pc, E, P)

    plt.figure(figsize=(12, 8))
    plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
    plt.scatter(depth_pts[:, 0], depth_pts[:, 1], c=depth_pts[:, 2], cmap='jet', s=1)
    plt.colorbar(label='Depth (m)')
    plt.axis('off')
    plt.show()

    # Use separate intrinsics and extrinsics
    # K2 = np.eye(4)
    # K2[:3, :3] = K
    # depth_pts = project_pointcloud_to_image(img, pc, E, K2 @ E)
    # plt.figure(figsize=(12, 8))
    # plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB))
    # plt.scatter(depth_pts[:, 0], depth_pts[:, 1], c=depth_pts[:, 2], cmap='jet', s=1)
    # plt.colorbar(label='Depth (m)')
    # plt.axis('off')
    # plt.show()