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)