投影矩阵计算

Question

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

    ])

计算 velo_to_cam 变换矩阵

    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)

• 我有相机内部数据和相机、激光雷达外部数据。我想计算投影矩阵P。 但这与图像不符。请帮我计算一下。帮我计算矩阵。我使用的公式可能不正确。所以请帮助我

Answer 1

一方面，我建议使用类似的方法将欧拉角转换为旋转矩阵，因为这样写出来可能很容易出错。 （除非您已经测试过该部分并知道它有效）

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