Commit b743bddf authored by Thomas Dowrick's avatar Thomas Dowrick

Issue #98: Implement l2r calculation from extrinsics

parent 980b7c46
......@@ -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:
Please register or sign in to reply
""" Return the left to right transformation matrix:
l2r = L * R^-1 """
l2r = np.matmul(left_extrinsics, np.linalg.inv(right_extrinsics))
Please register or sign in to reply
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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment