Projection matrix calculation

Question

def euler_to_rotation_matrix(roll, pitch, yaw):

Convert Euler angles to rotation matrix

    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):

   

Camera intrinsic matrix (K)

    K_camera = np.array(camera_data[camera_name]["K"]).reshape(3, 3)



   

Camera extrinsic parameters

    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 extrinsic parameters

    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"]

    ])

Calculate velo_to_cam transformation matrix

    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)))

Calculate projection matrix

    projection_matrix = K_camera @ velo_to_cam



    return projection_matrix

Given camera and lidar data

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

        }

    }

}

Loop through each camera

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)

• I have camera intrinsic and camera, lidar extrinsic data. I want to calculate projection matrix P. But this is not fit in the image. Please help me to compute such. Help me for computing the matrix. The formula that I have used may be incorrect. So pls help me

Answer 1

For one thing, I would recommend using something like this for converting Euler angles to rotation matrix, just because writing it all out like that can be error-prone. (unless you’ve already tested that part and know that it works)

from scipy.spatial.transform import Rotation as R

def euler_to_rotation_matrix(roll, pitch, yaw):
    r = R.from_euler('xyz', [roll, pitch, yaw], degrees=True)
    return r.as_matrix()

Answer 2

The function euler_to_rotation_matrix you've defined takes roll, pitch, and yaw as inputs to create a rotation matrix. This is based on the common definition of Euler angles. However, the camera and lidar data provided seem to include a quaternion representation of rotation (with w, x, y, z as components) rather than Euler angles. So you need to convert the given quaternions to Euler angles

import numpy as np
from scipy.spatial.transform import Rotation as R
def quaternion_to_euler(w, x, y, z):
    """
    Converts a quaternion to Euler angles (roll, pitch, yaw)
    """
    r = R.from_quat([x, y, z, w])
    roll, pitch, yaw = r.as_euler('xyz', degrees=False)
    return roll, pitch, yaw

If your goal is to transform points from the Lidar coordinate system to the camera coordinate system and then project them onto the image plane using the camera intrinsics, you would need to do the following:

Convert the lidar points to the camera coordinate system using the extrinsic parameters (rotation and translation). Project the points onto the image plane using the camera intrinsic matrix.

def calculate_projection_matrix(camera_data, lidar_data, camera_name):
    # Camera intrinsic matrix (K)
    K_camera = np.array(camera_data[camera_name]["K"]).reshape(3, 3)

    # Camera extrinsic parameters (assume rotation is given in quaternion)
    camera_rotation_matrix = quaternion_to_rotation_matrix(
        camera_data[camera_name]["rotation"]["w"],
        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 extrinsic parameters (assume rotation is given in quaternion)
    lidar_rotation_matrix = quaternion_to_rotation_matrix(
        lidar_data["PQ"]["rotation"]["w"],
        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"]
    ])
    # Calculate camera_to_lidar transformation matrix (extrinsics)
    camera_to_lidar = np.eye(4)
    camera_to_lidar[:3, :3] = camera_rotation_matrix
    camera_to_lidar[:3, 3] = camera_translation

    lidar_to_camera = np.eye(4)
    lidar_to_camera[:3, :3] = lidar_rotation_matrix.T
    lidar_to_camera[:3, 3] = -lidar_rotation_matrix.T @ lidar_translation

    # Compute the complete transformation matrix from lidar to camera
    velo_to_cam = camera_to_lidar @ lidar_to_camera

    # Compute the projection matrix by multiplying intrinsic and extrinsic matrices
    projection_matrix = K_camera