def mat2quat(mat):
mt = threeMat * m
v1, v2, v3 = mat.v1, mat.v2, mat.v3
rotMat = [[v1.x, v1.y, v1.z], [v2.x, v2.y, v2.z], [v3.x, v3.y, v3.z]]
euler = eulerangles.mat2euler(rotMat)
quat = eulerangles.euler2quat(euler.z, euler.y, euler.x)
return [quat[1], quat[2], quat[3], quat[0]]
def compute_pose_by_transform(transform):
""" Compute object pose by transformation matrix. """
transform = np.matrix(transform).reshape(4, 4, order='F')
scale_matrix = np.matrix(np.identity(4))
for i in range(3):
scale_matrix[i, i] = np.linalg.norm(transform[:, i])
transform *= scale_matrix.I
coordinate_transform = np.matrix([[0, 0, 1], [1, 0, 0], [0, 1, 0]],
dtype=np.float)
transform[0:3, 0:3] = coordinate_transform * transform[
0:3, 0:3] * coordinate_transform.T
transform[0:3, 3] = coordinate_transform * transform[0:3, 3]
z_angle, y_angle, x_angle = mat2euler(transform[0:3, 0:3])
pose = [
transform[0, 3], transform[1, 3], transform[2, 3], x_angle, y_angle,
z_angle
]
return pose
def rotated_to_local(T_w_c):
# Input is 7 DoF absolute poses (3 trans, 4 quat), output is 6 DoF relative poses
poses_local = []
T_w_c = numpy.insert(T_w_c, 0, 1, axis=1) # add dummy timestamp
# print(T_w_c)
for i in range(1, len(T_w_c)):
# print(T_w_c[i])
# print(T_w_c[i][0,1:4])
T_w_c_im1 = transform44(T_w_c[i - 1])
T_w_c_i = transform44(T_w_c[i])
T_c_im1_c_i = numpy.dot(numpy.linalg.pinv(T_w_c_im1), T_w_c_i)
# 3D: x, y, z, roll, pitch, yaw
eular_c_im1_c_i = mat2euler(T_c_im1_c_i[0:3, 0:3])
poses_local.append([SCALER * T_c_im1_c_i[0, 3], SCALER * T_c_im1_c_i[1, 3], SCALER * T_c_im1_c_i[2, 3],
SCALER * eular_c_im1_c_i[2] * RADIUS_2_DEGREE, SCALER * eular_c_im1_c_i[1] * RADIUS_2_DEGREE,
SCALER * eular_c_im1_c_i[0] * RADIUS_2_DEGREE])
poses_local = numpy.array(poses_local)
return poses_local
def read_obj_poses(filepath):
with open(filepath) as f:
lines = f.readlines()
if len(lines) == 0:
return None
joints_sequence = np.zeros((len(lines), num_joints * 3))
i = -1
obj_poses = np.zeros((len(lines), 6))
for line in lines:
i += 1
line_split = line.split(' ')
frame_num = line_split[0]
obj_mtx = np.array([float(x) for x in line_split[1:-1]]).reshape(
(4, 4)).T
obj_transl = obj_mtx[0:3, 3]
obj_rot_mtx = obj_mtx[0:3, 0:3]
obj_euler_angles = np.array(mat2euler(obj_rot_mtx))
obj_poses[i, 0:3] = obj_transl
obj_poses[i, 3:] = obj_euler_angles
return obj_poses
poses[i].split(',')[3], poses[i].split(',')[7],
poses[i].split(',')[11]
]
temp_rot = [[
float(poses[i].split(',')[0]),
float(poses[i].split(',')[1]),
float(poses[i].split(',')[2])
],
[
float(poses[i].split(',')[4]),
float(poses[i].split(',')[5]),
float(poses[i].split(',')[6])
],
[
float(poses[i].split(',')[8]),
float(poses[i].split(',')[9]),
float(poses[i].split(',')[10])
]]
euler_rot = mat2euler(temp_rot) # format: z, y, x
quat_rot = euler2quat(euler_rot[0], euler_rot[1],
euler_rot[2]) # format: w, x, y z
# required format: timestamp tx ty tz qx qy qz qw
if int(args.is_gt) == 0:
output_line = str(timestamps[i]) + ' ' + str(temp_trans[0]) + ' ' + str(temp_trans[1]) + ' ' + str(temp_trans[2]) \
+ ' ' + str(quat_rot[1]) + ' ' + str(quat_rot[2]) + ' ' + str(quat_rot[3]) + ' ' + str(quat_rot[0]) + '\n'
if int(args.is_gt) == 1:
output_line = str(timestamps[i+1].split(',')[0]) + ' ' + str(temp_trans[0]) + ' ' + str(temp_trans[1]) + ' ' + str(temp_trans[2]) \
+ ' ' + str(quat_rot[1]) + ' ' + str(quat_rot[2]) + ' ' + str(quat_rot[3]) + ' ' + str(quat_rot[0]) + '\n'
fw.write(output_line)
print('File is successfully converted to: ', args.save_path)