Issue #98: Implement l2r calculation from extrinsics
...  ...  @@ 79,3 +79,24 @@ def create_matrix_from_string(parameter_string): 
]  
return create_matrix_from_list(param_list)  
def calculate_l2r_matrix(left_extrinsics: np.ndarray,  
right_extrinsics: np.ndarray) > np.ndarray:  


""" Return the left to right transformation matrix:  
l2r = L * R^1 """  
l2r = np.matmul(left_extrinsics, np.linalg.inv(right_extrinsics))  


return l2r  
def get_l2r_smartliver_format(l2r_matrix: np.ndarray) > np.ndarray:  
""" Convert 4x4 left to right matrix to smartliver l2r format:  
R1 R2 R3  
R4 R5 R6  
R7 R8 R9  
T1 T2 T3  
"""  
rot = l2r_matrix[:3, :3]  
translation = l2r_matrix[:3, 3]  
l2r_smartliver_format = np.vstack((rot, translation))  
return l2r_smartliver_format 