def getterSensorRotation(data, sensor_key, collection_key): calibration_parent = data['sensors'][sensor_key]['calibration_parent'] calibration_child = data['sensors'][sensor_key]['calibration_child'] transform_key = calibration_parent + '-' + calibration_child # We use collection selected_collection and assume they are all the same quat = data['collections'][collection_key]['transforms'][transform_key][ 'quat'] hmatrix = transformations.quaternion_matrix(quat) matrix = hmatrix[0:3, 0:3] return utilities.matrixToRodrigues(matrix)
def getterTransform(dataset, transform_key, collection_name): # 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. trans = dataset['collections'][collection_name]['transforms'][ transform_key]['trans'] quat = dataset['collections'][collection_name]['transforms'][ transform_key]['quat'] # Convert from (trans, quat) to [tx, ty, tz, r1, r2, r3] format h_matrix = transformations.quaternion_matrix( quat) # quaternion to homogeneous matrix matrix = h_matrix[0:3, 0:3] # non-homogeneous matrix 3x3 rod = utilities.matrixToRodrigues(matrix).tolist() # matrix to Rodrigues return trans + rod
ret, rvecs, tvecs = cv2.solvePnP(objp.T[:3, :].T[idxs_s], np.array(corners_s, dtype=np.float32), K_s, D_s) pattern_pose_source = opt_utilities.traslationRodriguesToTransform( tvecs, rvecs) bTp = atom_core.atom.getTransform( common_frame, source_frame, train_dataset['collections'] [selected_collection_key]['transforms']) pattern_pose_source = np.dot(bTp, pattern_pose_source) delta = np.dot(np.linalg.inv(pattern_pose_source), pattern_pose_target) deltaT = delta[0:3, 3] deltaR = opt_utilities.matrixToRodrigues(delta[0:3, 0:3]) terr.append(deltaT * deltaT) rerr.append(deltaR * deltaR) # == # Get homography matrix if not ret: print("ERROR: Pattern wasn't found on collection " + str(collection_key)) continue else: H, T = computeHomographyMat(collection, rvecs, tvecs, K_s, D_s, K_t, D_t) # Project corners of source image into target image
def getterPatternRotation(data, collection_key): quat = data['patterns']['collections'][collection_key]['quat'] hmatrix = transformations.quaternion_matrix(quat) matrix = hmatrix[0:3, 0:3] return utilities.matrixToRodrigues(matrix)