激光雷达到相机投影矩阵计算错误排查与修正求助

手里有相机内参、相机与激光雷达的外参数据，需要计算激光雷达到相机的投影矩阵P，但当前计算结果无法匹配图像，怀疑公式或代码实现存在错误，附上完整代码求排查修正。

import numpy as np

def euler_to_rotation_matrix(roll, pitch, yaw):
    # 将欧拉角转换为旋转矩阵
    sy = np.sin(yaw)
    cy = np.cos(yaw)
    sp = np.sin(pitch)
    cp = np.cos(pitch)
    sr = np.sin(roll)
    cr = np.cos(roll)

    rotation_matrix = np.array([
        [cy * cp, -sy * cr + cy * sp * sr, sy * sr + cy * sp * cr],
        [sy * cp, cy * cr + sy * sp * sr, -cy * sr + sy * sp * cr],
        [-sp, cp * sr, cp * cr]
    ])

    return rotation_matrix

def calculate_projection_matrix(camera_data, lidar_data, camera_name):
    # 相机内参矩阵（K）
    K_camera = np.array(camera_data[camera_name]["K"]).reshape(3, 3)

    # 相机外参
    camera_rotation = euler_to_rotation_matrix(
        camera_data[camera_name]["rotation"]["x"],
        camera_data[camera_name]["rotation"]["y"],
        camera_data[camera_name]["rotation"]["z"]
    )
    camera_translation = np.array([
        camera_data[camera_name]["translation"]["x"],
        camera_data[camera_name]["translation"]["y"],
        camera_data[camera_name]["translation"]["z"]
    ])

    # 激光雷达外参
    lidar_rotation = euler_to_rotation_matrix(
        lidar_data["PQ"]["rotation"]["x"],
        lidar_data["PQ"]["rotation"]["y"],
        lidar_data["PQ"]["rotation"]["z"]
    )
    lidar_translation = np.array([
        lidar_data["PQ"]["translation"]["x"],
        lidar_data["PQ"]["translation"]["y"],
        lidar_data["PQ"]["translation"]["z"]
    ])

    # 计算激光雷达到相机的变换矩阵
    R_combined = camera_rotation @ lidar_rotation.T
    t_combined = camera_translation - R_combined @ lidar_translation
    velo_to_cam = np.hstack((R_combined, t_combined.reshape(3, 1)))

    # 计算投影矩阵
    projection_matrix = K_camera @ velo_to_cam

    return projection_matrix

# 给定相机与激光雷达数据
camera_data = {
    "Cam1": {
        "rotation": {
            "w": 0.45,
            "x": -0.542,
            "y": 0.00548,
            "z": 0.0023
        },
        "translation": {
            "x": 0.01,
            "y": 0.16,
            "z": -0.97
        },
        "K": [
            412.14536,
            0.0,
            52.2143,
            0.0,
            235.452,
            525.5249,
            0.0,
            0.0,
            1.0
        ]
    }
}

lidar_data = {
    "PQ": {
        "rotation": {
            "w": 3.0,
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        },
        "translation": {
            "x": 0.5,
            "y": 0.0,
            "z": 1.0
        }
    }
}

# 遍历每个相机
for camera_name in camera_data.keys():
    projection_matrix = calculate_projection_matrix(camera_data, lidar_data, camera_name)
    print(f"Projection Matrix for {camera_name}:\n", projection_matrix)

1. 核心错误：混淆四元数与欧拉角 #

代码中传入euler_to_rotation_matrix的参数是相机和雷达外参里的x/y/z，但给定的外参数据里包含w字段，这是四元数（而非欧拉角）。直接把四元数的x/y/z当成欧拉角计算旋转矩阵，会导致旋转完全错误，这是投影不匹配的主要原因。

2. 无效的四元数数据 #

激光雷达外参中的rotation.w = 3.0，四元数的模必须为1，这个数据明显无效，需要先归一化或修正输入。

3. 内参矩阵的潜在问题 #

给定的相机内参K数组是[fx, 0, cx, 0, fy, cy, 0, 0, 1]，reshape为3x3后是正确的内参格式，但需要确认cx（52.2143）和cy（525.5249）的数值是否符合相机实际的主点位置，若主点坐标错误也会导致投影偏移。

import numpy as np

def quaternion_to_rotation_matrix(w, x, y, z):
    # 四元数归一化（处理输入无效的情况）
    norm = np.sqrt(w**2 + x**2 + y**2 + z**2)
    w, x, y, z = w/norm, x/norm, y/norm, z/norm

    # 四元数转旋转矩阵（右手坐标系）
    rotation_matrix = np.array([
        [1 - 2*y**2 - 2*z**2, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
        [2*x*y + 2*z*w, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*x*w],
        [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x**2 - 2*y**2]
    ])
    return rotation_matrix

def calculate_projection_matrix(camera_data, lidar_data, camera_name):
    # 相机内参矩阵（K）
    K_camera = np.array(camera_data[camera_name]["K"]).reshape(3, 3)

    # 相机外参：四元数转旋转矩阵
    cam_rot = camera_data[camera_name]["rotation"]
    camera_rotation = quaternion_to_rotation_matrix(
        cam_rot["w"], cam_rot["x"], cam_rot["y"], cam_rot["z"]
    )
    camera_translation = np.array([
        camera_data[camera_name]["translation"]["x"],
        camera_data[camera_name]["translation"]["y"],
        camera_data[camera_name]["translation"]["z"]
    ])

    # 激光雷达外参：四元数转旋转矩阵
    lidar_rot = lidar_data["PQ"]["rotation"]
    lidar_rotation = quaternion_to_rotation_matrix(
        lidar_rot["w"], lidar_rot["x"], lidar_rot["y"], lidar_rot["z"]
    )
    lidar_translation = np.array([
        lidar_data["PQ"]["translation"]["x"],
        lidar_data["PQ"]["translation"]["y"],
        lidar_data["PQ"]["translation"]["z"]
    ])

    # 计算激光雷达到相机的变换矩阵 T_cam_lidar = T_cam_world * T_world_lidar
    # 其中 T_world_lidar = T_lidar_world.inv()，旋转矩阵的逆是转置，平移是 -R.T @ t
    R_cam_lidar = camera_rotation @ lidar_rotation.T
    t_cam_lidar = camera_translation - R_cam_lidar @ lidar_translation
    velo_to_cam = np.hstack((R_cam_lidar, t_cam_lidar.reshape(3, 1)))

    # 计算投影矩阵 P = K * [R | t]
    projection_matrix = K_camera @ velo_to_cam

    return projection_matrix

# 修正后的测试数据（雷达四元数改为有效单位四元数）
camera_data = {
    "Cam1": {
        "rotation": {
            "w": 0.45,
            "x": -0.542,
            "y": 0.00548,
            "z": 0.0023
        },
        "translation": {
            "x": 0.01,
            "y": 0.16,
            "z": -0.97
        },
        "K": [
            412.14536,
            0.0,
            52.2143,
            0.0,
            235.452,
            525.5249,
            0.0,
            0.0,
            1.0
        ]
    }
}

lidar_data = {
    "PQ": {
        "rotation": {
            "w": 1.0,  # 修正为单位四元数
            "x": 0.0,
            "y": 0.0,
            "z": 0.0
        },
        "translation": {
            "x": 0.5,
            "y": 0.0,
            "z": 1.0
        }
    }
}

# 测试运行
for camera_name in camera_data.keys():
    projection_matrix = calculate_projection_matrix(camera_data, lidar_data, camera_name)
    print(f"Projection Matrix for {camera_name}:\n", projection_matrix)

• 若外参的定义是相机相对于雷达的变换而非相对于世界坐标系，变换矩阵的计算逻辑需要调整，需根据实际外参定义确认。
• 投影时需注意点云的坐标系：激光雷达点通常是齐次坐标[x,y,z,1]，投影计算为uv = P @ [x,y,z,1].T，之后需要除以齐次分量得到像素坐标(u/w, v/w)。

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