Extrinsic Matrix (Homogeneous) creator

ExMat.py

import numpy as np

Z_ROT = lambda theta: np.array([[np.cos(theta), -np.sin(theta), 0],
                                [np.sin(theta), np.cos(theta), 0],
                                [0, 0, 1]])

def ConstructHomogeneousMatrices(phi_z, dx, dy, dz):

    Rot_Mat = Z_ROT(phi_z)
    Lidar_Camera_Base_Change = np.zeros(shape=(3, 3))

    Lidar_Camera_Base_Change[1, 0] = -1
    Lidar_Camera_Base_Change[2, 1] = -1
    Lidar_Camera_Base_Change[0, 2] = 1

    Rot_Mat = Rot_Mat.dot(Lidar_Camera_Base_Change)
    Disp_Vec = [[dx], [dy], [dz]]


    Homogeneous_Mat = np.concatenate([Rot_Mat, Disp_Vec], axis=1)
    Homogeneous_Mat = np.concatenate([Homogeneous_Mat, [[0, 0, 0, 1]]], axis=0)
    
    return Homogeneous_Mat

dx = 43.
dy = -37.5
dz = -33

phi_z = (-50.0*np.pi)/180

Homogeneous_Mat = ConstructHomogeneousMatrices(phi_z=phi_z, dx=dx, dy=dy, dz=dz)
print(Homogeneous_Mat)