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
