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