def Process2DCSV(xyt, xyt_target):
m1 = np.identity(4)
m1[0:2, 3] = xyt[0:2]
m1[0:3, 0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, xyt[2]], degrees=True))
m2 = np.identity(4)
m2[0:2, 3] = xyt_target[0:2]
m2[0:3,
0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, xyt_target[2]],
degrees=True))
return pd.distance_RT(m1, m2)
def eulerChangeToWorldRot(current_Rmat, d_rot):
initial_rot = Rot.from_euler(np.array([0, np.pi, 0]), 'XYZ')
init_mat = Rot.as_dcm(initial_rot)
cameraCoordNewRot = Rot.from_euler('XYZ', d_rot).as_dcm()
# old orientation of cam frame in world coords
curCamFrameR = np.matmul(current_Rmat, init_mat)
# new orientation of cam frame in world coords
worldNewRotCamFrame = np.matmul(cameraCoordNewRot, curCamFrameR)
# rotate back to pre cam frame orientation
worldNewRotCamFrame = np.matmul(np.transpose(init_mat),
worldNewRotCamFrame)
return Rot.from_dcm(worldNewRotCamFrame).as_euler('XYZ')
def rotation_matrix(angle, rotation_axis):
""" Takes an angle (radians) and axis of rotation (3-D vector) and outputs a rotation matrix"""
normed_axis = rotation_axis / np.linalg.norm(rotation_axis)
rot_vec = R.from_rotvec(normed_axis * angle)
return R.as_dcm(rot_vec)
def Convert6dofPoseToMatrix(pose):
m = np.identity(4)
m[0:3, 3] = pose[0:3]
m[0:3, 0:3] = R.as_dcm(R.from_quat(pose[3:7]))
return m
print(np.transpose(m @ pt_zero))
for i in range(0, 3):
vec_zero = np.zeros([4, 1])
vec_zero[i] = 1
print(np.transpose(m @ vec_zero))
print('\n')
if __name__ == '__main__':
print('Checking Pose Distance Metric code\n')
m1 = np.identity(4)
d_ang = np.pi / 3
m1[0:3, 3] = [1, 1, 1]
m1[0:3, 0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, d_ang]))
m2 = np.identity(4)
m2[0:3, 3] = [2, 2, 2]
pd = PoseDistance()
dist_RT = pd.distance_RT(m1, m2)
dist_RT_seq = pd.distance_RT_seq(pd.get_sequence_RT(m1, m2, n=10))
print(
'Distance RT, should be {0:0.2f}, is {1:0.2f}, integrated {2:0.2f}\n'.
format(np.sqrt(3) + d_ang, dist_RT, dist_RT_seq))
mat_seq2 = pd.get_sequence_RT(m1, m2, 2)
print('M1:')
