def setterPatternRotation(data, value, collection_key):
assert len(value) == 3, "value must be a list with length 3."
matrix = utilities.rodriguesToMatrix(value)
hmatrix = np.identity(4).astype(np.float)
hmatrix[0:3, 0:3] = matrix
quat = transformations.quaternion_from_matrix(hmatrix)
data['patterns']['collections'][collection_key]['quat'] = quat
def setterSensorRotation(data, value, sensor_key):
assert len(value) == 3, "value must be a list with length 3."
matrix = utilities.rodriguesToMatrix(value)
hmatrix = np.identity(4)
hmatrix[0:3, 0:3] = matrix
quat = transformations.quaternion_from_matrix(hmatrix)
calibration_parent = data['sensors'][sensor_key]['calibration_parent']
calibration_child = data['sensors'][sensor_key]['calibration_child']
transform_key = calibration_parent + '-' + calibration_child
for _collection_key in data['collections']:
data['collections'][_collection_key]['transforms'][transform_key][
'quat'] = quat
def computeHomographyMat(collection, rvecs, tvecs, K_s, D_s, K_t, D_t):
# ----------------------------------------------------------------------
# ----- Find pose of the source and target cameras w.r.t the pattern
# ----------------------------------------------------------------------
ss_T_chess_h = np.zeros((4, 4), np.float32)
ss_T_chess_h[3, 3] = 1
ss_T_chess_h[0:3, 3] = tvecs[:, 0]
ss_T_chess_h[0:3, 0:3] = opt_utilities.rodriguesToMatrix(rvecs)
target_frame = train_dataset['calibration_config']['sensors'][
target_sensor]['link']
source_frame = train_dataset['calibration_config']['sensors'][
source_sensor]['link']
selected_collection_key = train_dataset['collections'].keys()[0]
st_T_ss = atom_core.atom.getTransform(
target_frame, source_frame,
train_dataset['collections'][selected_collection_key]['transforms'])
ss_T_st = atom_core.atom.getTransform(
source_frame, target_frame,
train_dataset['collections'][selected_collection_key]['transforms'])
st_T_chess_h = np.dot(inv(ss_T_st), ss_T_chess_h)
ss_T_chess = np.zeros((3, 3), np.float32)
st_T_chess = np.zeros((3, 3), np.float32)
for c in range(0, 2):
for l in range(0, 3):
ss_T_chess[l, c] = ss_T_chess_h[l, c]
st_T_chess[l, c] = st_T_chess_h[l, c]
ss_T_chess[:, 2] = ss_T_chess_h[0:3, 3]
st_T_chess[:, 2] = st_T_chess_h[0:3, 3]
# ---------------------------------------------------------------------
# ----- Compute homography matrix
# ---------------------------------------------------------------------
A = np.dot(K_t, st_T_chess)
B = np.dot(A, inv(ss_T_chess))
C = np.dot(B, inv(K_s))
H = C
return H, np.dot(st_T_ss, ss_T_chess_h)
def setterTransform(dataset, values, transform_key, collection_name=None):
# The pose must be returned as a list of translation vector and rotation, i.e. [tx, ty, tz, r1, r2, r3] where r1,
# r2,r3 are angles of the Rodrigues rotation vector.
# Convert from [tx, ty, tz, r1, r2, r3] format to trans and quat format
trans, rod = values[0:3], values[3:]
matrix = utilities.rodriguesToMatrix(rod)
h_matrix = np.identity(4)
h_matrix[0:3, 0:3] = matrix
quat = transformations.quaternion_from_matrix(h_matrix)
if collection_name is None: # if collection_name is None, set all collections with the same value
for collection_key in dataset['collections']:
dataset['collections'][collection_key]['transforms'][
transform_key]['trans'] = trans # set the translation
dataset['collections'][collection_key]['transforms'][
transform_key]['quat'] = quat # set the quaternion
else:
dataset['collections'][collection_name]['transforms'][transform_key][
'trans'] = trans # set the translation
dataset['collections'][collection_name]['transforms'][transform_key][
'quat'] = quat # set the quaternion