def write_joint_rotation_matrices_to_bvh(bvh_filename, hip_pose_seq,
r_matrix_seq):
#print (quaternion_data.shape)
out_seq = []
seq_len = hip_pose_seq.shape[0]
for i in range(seq_len):
hip_pose = hip_pose_seq[i] #3
out_frame = [hip_pose[0], hip_pose[1], hip_pose[2]]
hip_x, hip_y, hip_z = euler.mat2euler(r_matrix_seq[i, 0],
'sxyz') #hip euler rotation
out_frame += [
hip_z * 180 / np.pi, hip_y * 180 / np.pi, hip_x * 180 / np.pi
] #notice in cmu bvh files, the channel for hip rotation is z y x
for joint in skeleton:
if (("hip" not in joint)
and (len(skeleton[joint]["channels"]) == 3)):
index = joint_index[joint]
y, x, z = euler.mat2euler(r_matrix_seq[i, index], 'syxz')
out_frame += [
z * 180 / np.pi, x * 180 / np.pi, y * 180 / np.pi
] #notice in cmu bvh files, the channel for joint rotation is z x y
out_seq += [out_frame]
##out_seq now should be seq_len*(3+3*joint_num)
out_seq = np.array(out_seq)
out_seq = np.round(out_seq, 6)
#out_seq2=np.ones(out_seq.shape)
#out_seq2[:,3:out_seq2.shape[1]]=out_seq[:,3:out_seq2.shape[1]].copy()
#print ("bvh data shape")
#print (out_seq.shape)
write_frames(standard_bvh_file, bvh_filename, out_seq)
def _recursive_apply_frame(self, joint, frame_pose, index_offset, p, r,
M_parent, p_parent):
if joint.position_animated():
offset_position, index_offset = self._extract_position(
joint, frame_pose, index_offset)
else:
offset_position = np.zeros(3)
if len(joint.channels) == 0:
joint_index = list(self.joints.values()).index(joint)
p[joint_index] = p_parent + M_parent.dot(joint.offset)
r[joint_index] = mat2euler(M_parent)
return index_offset
if joint.rotation_animated():
M_rotation, index_offset = self._extract_rotation(
frame_pose, index_offset, joint)
else:
M_rotation = np.eye(3)
M = M_parent.dot(M_rotation)
position = p_parent + M_parent.dot(joint.offset) + offset_position
rotation = np.rad2deg(mat2euler(M))
joint_index = list(self.joints.values()).index(joint)
p[joint_index] = position
r[joint_index] = rotation
for c in joint.children:
index_offset = self._recursive_apply_frame(c, frame_pose,
index_offset, p, r, M,
position)
return index_offset
def filter_predict(X, lidar_data, particle_num, index, rotation_matrix,
O_last):
position_mu = 0
position_sigma = 0.01
angle_mu = 0
angle_sigma = 1 / 180 * np.pi
x = lidar_data[index]['pose'][0][0]
y = lidar_data[index]['pose'][0][1]
theta = lidar_data[index]['pose'][0][2]
wOl = np.array([[np.cos(theta), -np.sin(theta), 0, x],
[np.sin(theta), np.cos(theta), 0, y], [0, 0, 1, 0.48],
[0, 0, 0, 1]])
O = wOl.dot(np.linalg.inv(rotation_matrix))
x_diff = O[0][3] - O_last[0][3]
y_diff = O[1][3] - O_last[1][3]
theta_diff = mat2euler(O[0:3][0:3])[2] - mat2euler(O_last[0:3][0:3])[2]
O_diff = np.tile([x_diff, y_diff, theta_diff], (particle_num, 1))
w = np.random.normal(position_mu, position_sigma, (particle_num, 2))
angle_random = np.random.normal(angle_mu, angle_sigma, (particle_num, 1))
X[:, 0:2] = X[:, 0:2] + O_diff[:, 0:2] + w[:, 0:2]
X[:, 2] = X[:, 2] + O_diff[:, 2] + angle_random[:, 0]
return O, theta_diff
def get_one_frame_data(joints, motion):
"""
Parse one frame data to dataframe
"""
joints_nm = list(joints.keys())
all_joints = []
all_joints_nm = []
for joint_nm in joints_nm:
joint = joints[joint_nm]
# parameters from self
coord = joint.coordinate.ravel().tolist()
coord_nm = [joint.name + '_coord_' + str(i) for i in range(3)]
norm_coord = joint.norm_coordinate.ravel().tolist()
norm_coord_nm = [
joint.name + '_norm_coord_' + str(i) for i in range(3)
]
ax, ay, az = mat2euler(joint.matrix)
norm_ax, norm_ay, norm_az = mat2euler(joint.norm_matrix)
l = joint.length
# add to storage
all_joints.extend(coord)
all_joints_nm.extend(coord_nm)
all_joints.extend(norm_coord)
all_joints_nm.extend(norm_coord_nm)
all_joints.extend([ax, ay, az, norm_ax, norm_ay, norm_az, l])
nm_rest = [
'angle_0', 'angle_1', 'angle_2', 'norm_angle_0', 'norm_angle_1',
'norm_angle_2', 'length'
]
all_joints_nm.extend([str(joint.name) + "_" + i for i in nm_rest])
# parameters from motion (local)
all_pars = []
all_nms = []
for bone, pars in motion.items():
all_pars.extend(pars)
all_nms.extend([bone + "_motion_" + str(i) for i in range(len(pars))])
all_joints.extend(all_pars)
all_joints_nm.extend(all_nms)
df = pd.DataFrame(all_joints).T
df.columns = all_joints_nm
return df
def invert_direction(self):
if np.allclose(self.Rapplied, np.eye(3)):
self.tilt_degrees(x_angle=180, y_angle=180, store=False)
else:
euler1 = mat2euler(self.Rapplied)
euler2 = mat2euler(self.Rapplied.transpose())
self.tilt_radiants(euler2[0], euler2[1], euler2[2])
self.tilt_degrees(x_angle=180, y_angle=180, store=False)
self.tilt_radiants(euler1[0], euler1[1], euler1[2])
self.is_inverted = not self.is_inverted
def spin_degrees(self, angle):
if np.allclose(self.Rapplied, np.eye(3)):
self.tilt_degrees(y_angle=angle, store=False)
else:
euler1 = mat2euler(self.Rapplied)
euler2 = mat2euler(self.Rapplied.transpose())
self.tilt_radiants(euler2[0], euler2[1], euler2[2])
self.tilt_degrees(y_angle=angle, store=False)
self.tilt_radiants(euler1[0], euler1[1], euler1[2])
self.spinned += angle
def xyz_to_rotations_debug(skel, position, root_name):
all_rotations = {}
all_rotation_matrices = {}
children_dict = get_child_dict(skel)
while len(children_dict.keys()) - 1 > len(all_rotation_matrices.keys()):
for bone in children_dict.keys():
if bone == None:
continue
parent = skel[bone]['parent']
if bone in all_rotation_matrices.keys():
continue
if parent not in all_rotation_matrices.keys() and parent != None:
continue
upper = parent
parent_rot = np.identity(3)
while upper != None:
upper_rot = all_rotation_matrices[upper]
parent_rot = np.dot(upper_rot, parent_rot)
upper = skel[upper]['parent']
children = children_dict[bone]
children_xyz = np.zeros([len(children), 3])
children_orig = np.zeros([len(children), 3])
for i in range(len(children)):
children_xyz[i, :] = np.array(position[children[i]]) - np.array(
position[bone]) # translation from cur joint to child i
children_orig[i, :] = np.array(skel[children[i]]['offsets'])
# normalize the translation according to the bvh file bone length (linalg.norm returns bone length)
children_xyz[i, :] = children_xyz[i, :] * np.linalg.norm(
children_orig[i, :]) / np.linalg.norm(children_xyz[i, :])
assert np.allclose(np.linalg.norm(
children_xyz[i, :]), np.linalg.norm(children_orig[
i, :])) # new bone length ~= bvh bone length
parent_space_children_xyz = np.dot(children_xyz, parent_rot)
rotation = kabsch(parent_space_children_xyz, children_orig)
# The following conditional phrase works only sometimes. I guess it is related to the order of rotations,
# i.e., z first, y next and x last (='sxyz'), or z first, x next and y last (=syxz).
# when representing in blender, it is using 'sxyz'.
# so I changed it to use 'sxyz' always.
if True: # bone == root_name:
all_rotations[bone] = np.array(
euler.mat2euler(rotation, 'sxyz')) * (180.0 / math.pi)
else:
angles = np.array(euler.mat2euler(rotation,
'syxz')) * (180.0 / math.pi)
all_rotations[bone] = np.array(
[angles[1], angles[0], angles[2]])
all_rotation_matrices[bone] = rotation
return (all_rotation_matrices, all_rotations)
def pos_to_rotation_mat(skelton, position, non_end_bones):
all_rotation = {}
all_rotation_matrices = {}
while len(non_end_bones) - 1 > len(all_rotation_matrices.keys()):
for bone_name in ['Root'] + non_end_bones:
parent = skelton[bone_name]['parent']
if bone_name in all_rotation_matrices.keys():
continue
if (not parent in all_rotation_matrices.keys()) and (parent
is not None):
continue
# Calcurate rotarion matrix from root
parent_rot = np.identity(3)
while parent is not None:
parent_rot = np.dot(all_rotation_matrices[parent], parent_rot)
parent = skelton[parent]['parent']
children = skelton[bone_name]['children']
children_positions = np.zeros((len(children), 3))
children_offsets = np.zeros((len(children), 3))
for i in range(len(children)):
children_positions[i, :] = np.array(
position[children[i]]) - np.array(position[bone_name])
children_offsets[i, :] = np.array(
skelton[children[i]]['offsets'])
children_positions[
i, :] = children_positions[i, :] * np.linalg.norm(
children_offsets[i, :]) / np.linalg.norm(
children_positions[i, :] + 1e-32)
assert np.allclose(np.linalg.norm(children_positions[i, :]),
np.linalg.norm(children_offsets[i, :]))
parent_space_children_positoins = np.dot(children_positions,
parent_rot)
rotation_mat = kabsch(parent_space_children_positoins,
children_offsets)
if bone_name == 'Root':
all_rotation[bone_name] = np.array(
euler.mat2euler(rotation_mat, 'sxyz')) * (
180.0 / math.pi
) #rotation_mat_to_euler(rotation_mat) * (180.0 / math.pi)
else:
angles = np.array(euler.mat2euler(rotation_mat, 'sxyz')) * (
180.0 / math.pi
) #rotation_mat_to_euler(rotation_mat) * (180.0 / math.pi)
all_rotation[bone_name] = [angles[1], angles[0], angles[2]]
all_rotation_matrices[bone_name] = rotation_mat
return all_rotation_matrices, all_rotation
def pose_to_tf(self, camera_ext, stamp):
inv_R = camera_ext[:3, :3].T
inv_t = -np.matmul(inv_R, camera_ext[:3, 3])
if self.noisy:
# Translation noise
t_noise = np.random.normal(loc=0.0,
scale=self.translation_noise,
size=(3))
inv_t += t_noise
# Rotation noise
euler_angles = np.array(mat2euler(inv_R))
euler_angles += np.random.normal(loc=0.0,
scale=self.rotation_noise,
size=(3))
inv_R = euler2mat(*euler_angles)
# 4x4 homogeneous rotation
inv_R = np.concatenate([inv_R, np.zeros((3, 1))], axis=1)
inv_R = np.concatenate([inv_R, np.array([[0, 0, 0, 1]])], axis=0)
pose_msg = TransformStamped()
pose_msg.header.frame_id = self.world_frame
pose_msg.child_frame_id = self.camera_frame
pose_msg.header.stamp = stamp
quat = quaternion_from_matrix(inv_R)
pose_msg.transform.rotation.x = quat[0]
pose_msg.transform.rotation.y = quat[1]
pose_msg.transform.rotation.z = quat[2]
pose_msg.transform.rotation.w = quat[3]
pose_msg.transform.translation.x = inv_t[0]
pose_msg.transform.translation.y = inv_t[1]
pose_msg.transform.translation.z = inv_t[2]
return pose_msg
def print_poses(poses):
print 'translations'
print poses[:, :3]
print 'euler'
for i in xrange(poses.shape[0]):
a = txe.mat2euler(txq.quat2mat(poses[i, 3:]))
print[np.rad2deg(aa) for aa in a]
def rectify_16(R):
az, el, ti = mat2euler(R, 'szyz')
el = np.rad2deg(el)
if el >= -80 or el <= -101:
return rectify_symmetric_rotation(R, gen_axis_group(180, 2))
else:
return rectify_symmetric_rotation(R, gen_axis_group(90, 2))
def rectify_15(R):
az, el, ti = mat2euler(R, 'szyz')
el = np.rad2deg(el)
if el >= -80:
return R
else:
return rectify_z_axis_symmetric_rotation(R)
def test_mat2euler():
# Test mat2euler function
angles = (4 * math.pi) * (np.random.random(3) - 0.5)
for axes in euler._AXES2TUPLE.keys():
R0 = euler2mat(axes=axes, *angles)
R1 = euler2mat(axes=axes, *mat2euler(R0, axes))
assert_almost_equal(R0, R1)
def visualize_only_trajectory(pose, path_name="Unknown", show_ori=False):
fig, ax = plt.subplots(figsize=(7, 7))
n_pose = pose.shape[2]
ax.plot(pose[0, 3, :], pose[1, 3, :], 'r-', linewidth=2, label=path_name)
ax.scatter(pose[0, 3, 0], pose[1, 3, 0], marker='s', label="start")
ax.scatter(pose[0, 3, -1], pose[1, 3, -1], marker='o', label="end")
if show_ori:
select_ori_index = list(range(0, n_pose, max(int(n_pose / 50), 1)))
yaw_list = []
for i in select_ori_index:
_, _, yaw = mat2euler(pose[:3, :3, i])
yaw_list.append(yaw)
dx = np.cos(yaw_list)
dy = np.sin(yaw_list)
dx, dy = [dx, dy] / np.sqrt(dx**2 + dy**2)
ax.quiver(pose[0,3,select_ori_index],pose[1,3,select_ori_index],dx,dy,\
color="b",units="xy",width=1)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.axis('equal')
ax.grid(False)
ax.legend()
plt.show(block=True)
return fig, ax
def print_poses(poses):
print('translations')
print(poses[:, :3])
print('euler')
for i in range(poses.shape[0]):
a = txe.mat2euler(txq.quat2mat(poses[i, 3:]))
print([np.rad2deg(aa) for aa in a])
def test_euler_axes():
# Test there and back with all axis specs
aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)]
axis_perms = list(permutations('xyz', 3)) + aba_perms
for (a, b, c), mat in zip(euler_tuples, euler_mats):
for rs, axes in product('rs', axis_perms):
ax_spec = rs + ''.join(axes)
conventioned = [EulerFuncs(ax_spec)]
if ax_spec in euler.__dict__:
conventioned.append(euler.__dict__[ax_spec])
mat = euler2mat(a, b, c, ax_spec)
a1, b1, c1 = mat2euler(mat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
quat = euler2quat(a, b, c, ax_spec)
a1, b1, c1 = quat2euler(quat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
ax, angle = euler2axangle(a, b, c, ax_spec)
a1, b1, c1 = axangle2euler(ax, angle, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
for obj in conventioned:
mat = obj.euler2mat(a, b, c)
a1, b1, c1 = obj.mat2euler(mat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
quat = obj.euler2quat(a, b, c)
a1, b1, c1 = obj.quat2euler(quat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
ax, angle = obj.euler2axangle(a, b, c)
a1, b1, c1 = obj.axangle2euler(ax, angle)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
def test_mat2euler():
# Test mat2euler function
angles = (4 * math.pi) * (np.random.random(3) - 0.5)
for axes in euler._AXES2TUPLE.keys():
R0 = euler2mat(axes=axes, *angles)
R1 = euler2mat(axes=axes, *mat2euler(R0, axes))
assert_almost_equal(R0, R1)
def read_label(filename, location):
label_file = open(filename, 'r')
labels = label_file.read()
label_file.flush()
label_file.close()
labels = labels.split('\n')
_location = map(float, labels[4].split(' ')[0:3])
location.extend(_location)
labels = map(lambda x: map(float, x[0:3]), map(lambda x: x.split(' '), labels[0:3]))
# print labels
rot_matrix = np.array(labels)
eu = euler.mat2euler(rot_matrix, 'sxyz')
pitch_max = 3
yaw_max = 3
yaw = int((eu[0] + math.pi/4) / (math.pi / (2 * yaw_max)))
pitch = int((eu[1] + math.pi/2) / (math.pi / pitch_max))
yaw = crop_number(yaw, 0, yaw_max - 1)
pitch = crop_number(pitch, 0, pitch_max - 1)
# for i in range(3):
# if location[i] < locationmin[i]:
# locationmin[i] = location[i]
# if location[i] > locationmax[i]:
# locationmax[i] = location[i]
label = pitch
label *= yaw_max
label += yaw
f_lab = open(filename[0:-8] + "label.txt", "w")
f_lab.write(str(eu) + '\n' + str(label))
f_lab.close()
# print labels
return [label]
def peaks_from_best_vector_match(single_match_result, library, rank=0):
"""Takes a VectorMatchingResults object and return the associated peaks,
to be used in combination with map().
Parameters
----------
single_match_result : ndarray
An entry in a VectorMatchingResults
library : DiffractionLibrary
Diffraction library containing the phases and rotations
rank : int
Get peaks from nth best orientation (default: 0, best vector match)
Returns
-------
peaks : ndarray
Coordinates of peaks in the matching results object in calibrated units.
"""
best_fit = get_nth_best_solution(single_match_result, "vector", rank=rank)
phase_index = best_fit.phase_index
rotation_orientation = mat2euler(best_fit.rotation_matrix)
# Don't change the original
structure = library.structures[phase_index]
sim = library.diffraction_generator.calculate_ed_data(
structure,
reciprocal_radius=library.reciprocal_radius,
rotation=rotation_orientation,
with_direct_beam=False,
)
# Cut z
return sim.coordinates[:, :2]
def DeadReckoning(xt, o, o_next, b_T_l):
r = np.array([0, 0, 0, 1])
R_body = euler2mat(0, 0, xt[2])
h0 = np.reshape(np.array([xt[0], xt[1], .93]), [3, 1])
w_T_b = np.block([[R_body, h0], [r]])
R_odom = euler2mat(0, 0, o[2])
o_xy = np.reshape(np.array([o[0], o[1], 0]), [3, 1])
w_T_l = np.block([[R_odom, o_xy], [r]])
R_odom2 = euler2mat(0, 0, o_next[2])
o_xy2 = np.reshape(np.array([o_next[0], o_next[1], 0]), [3, 1])
w_T_l2 = np.block([[R_odom2, o_xy2], [r]])
A = np.matmul(b_T_l, inv(w_T_l))
B = np.matmul(A, w_T_l2)
C = np.matmul(B, inv(b_T_l))
w_T_b = np.dot(w_T_b, C)
x_next = np.zeros((3, 1))
x_next[0] = w_T_b[0, 3]
x_next[1] = w_T_b[1, 3]
x_next[2] = mat2euler(w_T_b[:3, :3])[2]
return x_next
def rot_euler(self):
"""Returns the (rx, ry, rz) Euler rotations."""
if self._rot_euler is not None:
return self._rot_euler
if self._rot_mat is not None:
self._rot_euler = mat2euler(self.rot, axes='rxyz')
return self._rot_euler
def integrate(imu_data, train=True):
dt = [(imu_data['ts'][0, i + 1] - imu_data['ts'][0, i])
for i in range(imu_data['ts'].shape[1] - 1)]
angular_velocities = imu_data['vals'][3:, :]
quaternion_estimates = [Quaternion(1, [0, 0, 0])]
for t in range(angular_velocities.shape[1] - 1):
w = angular_velocities[:, t][[1, 2, 0]]
#quaternion_estimates.append(quaternion_estimates[-1].multiply(Quaternion(0, (0.5*w*dt[t])).exp().unit()))
quaternion_estimates.append((quaternion_estimates[t].multiply(
Quaternion(0, 0.5 * w * dt[t]).exp())).unit())
euler_estimates = []
for quaternion_estimate in quaternion_estimates:
euler_estimates.append(e3d.quat2euler(quaternion_estimate.to_numpy()))
vicon_data = read_dataset('trainset', 'vicon', '1')
vicon_euler = []
for i in range(vicon_data['rots'].shape[2]):
vicon_euler.append(e3d.mat2euler(vicon_data['rots'][:, :, i]))
if train:
for i in range(3):
pl.plot(imu_data['ts'][0], np.array(euler_estimates)[:, i])
pl.plot(vicon_data['ts'][0], np.array(vicon_euler)[:, i])
#pl.plot(vicon_data['ts'][0], np.vstack((np.array(euler_estimates)[:len(vicon_euler), i], np.array(vicon_euler)[:, i])).T)
pl.show()
else:
for i in range(3):
pl.plot(imu_data['ts'][0], np.array(euler_estimates)[:, i])
#pl.plot(vicon_data['ts'][0],np.array(vicon_euler)[:, i])
#pl.plot(vicon_data['ts'][0], np.vstack((np.array(euler_estimates)[:len(vicon_euler), i], np.array(vicon_euler)[:, i])).T)
pl.show()
def test_euler_axes():
# Test there and back with all axis specs
aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)]
axis_perms = list(permutations('xyz', 3)) + aba_perms
for (a, b, c), mat in zip(euler_tuples, euler_mats):
for rs, axes in product('rs', axis_perms):
ax_spec = rs + ''.join(axes)
conventioned = [EulerFuncs(ax_spec)]
if ax_spec in euler.__dict__:
conventioned.append(euler.__dict__[ax_spec])
mat = euler2mat(a, b, c, ax_spec)
a1, b1, c1 = mat2euler(mat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
quat = euler2quat(a, b, c, ax_spec)
a1, b1, c1 = quat2euler(quat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
ax, angle = euler2axangle(a, b, c, ax_spec)
a1, b1, c1 = axangle2euler(ax, angle, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
for obj in conventioned:
mat = obj.euler2mat(a, b, c)
a1, b1, c1 = obj.mat2euler(mat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
quat = obj.euler2quat(a, b, c)
a1, b1, c1 = obj.quat2euler(quat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
ax, angle = obj.euler2axangle(a, b, c)
a1, b1, c1 = obj.axangle2euler(ax, angle)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
def visualize_trajectory_2d(pose_old,pose,M,path_name="Unknown",show_ori=False):
'''
function to visualize the trajectory in 2D
Input:
pose: 4*4*N matrix representing the camera pose,
where N is the number of pose, and each
4*4 matrix is in SE(3)
'''
fig,ax = plt.subplots(figsize=(10,10))
n_pose = pose.shape[2]
ax.plot(pose[0,3,:],pose[1,3,:],'r-',label=path_name)
ax.plot(pose_old[0,3,:],pose_old[1,3,:],'g-',label="Dead_reckoning")
ax.scatter(M[0,:],M[1,:],marker='*',label="landmarks")
ax.scatter(pose[0,3,0],pose[1,3,0],marker='s',label="start")
ax.scatter(pose[0,3,-1],pose[1,3,-1],marker='o',label="end")
if show_ori:
select_ori_index = list(range(0,n_pose,int(n_pose/50)))
yaw_list = []
for i in select_ori_index:
_,_,yaw = mat2euler(pose[:3,:3,i])
yaw_list.append(yaw)
dx = np.cos(yaw_list)
dy = np.sin(yaw_list)
dx,dy = [dx,dy]/np.sqrt(dx**2+dy**2)
ax.quiver(pose[0,3,select_ori_index],pose[1,3,select_ori_index],dx,dy,\
color="b",units="xy",width=1)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.axis('equal')
ax.grid(False)
ax.legend()
plt.show(block=True)
return fig, ax
def prerotate(self, part: ET.Element):
"""Prerotate the part to match the orientation"""
angles = parse_numstr(part.get('rotation'))
angles = [n * np.pi/180 for n in angles]
M = euler2mat(*[angles[n] for n in (2, 0, 1)], 'szxy')
angles = mat2euler(np.matmul(M, self.prerotation_matrix), 'szxy')
angles = [angles[n] * 180/np.pi for n in (1, 2, 0)]
part.set('rotation', create_numstr(angles))
def objectOrientation(self):
tvec, r = self.pose()
eulerAngles = mat2euler(cv2.Rodrigues(r)[0], axes='rzxy')
tilt = eulerAngles[1]
rot = eulerAngles[0]
dist = tvec[2, 0] # only take depth component np.linalg.norm(tvec)
return dist, tilt, rot
def calculate_vicon_angle(vicd):
vicon_angle = []
vicon_time = []
for i in range(1, len(vicd['rots'][0, 0])):
x_deg, y_deg, z_deg = mat2euler(vicd['rots'][:, :, i])
vicon_angle.append([x_deg, y_deg, z_deg])
vicon_time.append(vicd['ts'][0][i])
return np.array(vicon_angle), vicon_time
def LocalizationUpdate(mu, alpha, z, m, b_T_l, grid, Polar):
res = grid[1] - grid[0]
xr = np.arange(-4 * res, 5 * res, res)
yr = xr
thr = np.arange(-4, 5, 1) * np.pi / 180
corr = np.zeros((len(xr), len(yr), len(thr)))
cmax = np.zeros((len(mu.T), 1))
mu_upd = np.zeros((3, len(mu.T)))
r = np.array([0, 0, 0, 1])
m_th = np.zeros(m.shape).astype(np.uint8)
thresh = m > 0.
m_th[thresh] = 1
thresh = m < 0.
m_th[thresh] = 0
for i, x in enumerate(mu.T):
R_body = euler2mat(0, 0, x[2])
h0 = np.reshape(np.array([x[0], x[1], .93]), [3, 1])
w_T_b = np.block([[R_body, h0], [r]])
Cart = polar2cart(Polar, z)
row = np.ones([1, 1081])
Cart = np.vstack((Cart, row))
Cart = Cart[:, removescan(z)]
z_w = np.dot(np.dot(w_T_b, b_T_l), Cart)
z_w = z_w[:, removeGround(z_w)]
for j, th in enumerate(thr):
R_th = euler2mat(0, 0, th)
h_th = np.reshape(np.array([0, 0, 0]), [3, 1])
THr = np.block([[R_th, h_th], [r]])
z_th = np.matmul(THr, z_w)
corr[:, :, j] = mapCorrelation(m_th, grid, grid, z_th[:3, :], xr,
yr)
cmax[i, 0] = np.max(corr)
cmax_ind = np.unravel_index(np.argmax(corr),
(len(xr), len(yr), len(thr)))
C = np.array((xr[cmax_ind[0]], yr[cmax_ind[1]], thr[cmax_ind[2]]))
R_C = euler2mat(0, 0, C[2])
h_C = np.reshape(np.array([C[0], C[1], 0]), [3, 1])
Cr = np.block([[R_C, h_C], [r]])
w_T_b = np.matmul(Cr, w_T_b)
mu_upd[0, i] = w_T_b[0, 3]
mu_upd[1, i] = w_T_b[1, 3]
mu_upd[2, i] = mat2euler(w_T_b[:3, :3])[2]
p = softmax(cmax)
alpha = alpha * p.T / np.sum(alpha * p.T)
return mu_upd, alpha
def test_euler2mat():
# Test mat creation from random angles and round trip
ai, aj, ak = (4 * math.pi) * (np.random.random(3) - 0.5)
for ax_specs in euler._AXES2TUPLE.items():
for ax_spec in ax_specs:
R = euler2mat(ai, aj, ak, ax_spec)
bi, bj, bk = mat2euler(R, ax_spec)
R2 = euler2mat(bi, bj, bk, ax_spec)
assert_almost_equal(R, R2)
def rectify_17(R):
az, el, ti = mat2euler(R, 'szyz')
el = np.rad2deg(el)
if el >= -70:
return R
elif el <= -117:
return rectify_symmetric_rotation(R, gen_axis_group(180, 2))
else:
return rectify_z_axis_symmetric_rotation(R)
def test_euler2mat():
# Test mat creation from random angles and round trip
ai, aj, ak = (4 * math.pi) * (np.random.random(3) - 0.5)
for ax_specs in euler._AXES2TUPLE.items():
for ax_spec in ax_specs:
R = euler2mat(ai, aj, ak, ax_spec)
bi, bj, bk = mat2euler(R, ax_spec)
R2 = euler2mat(bi, bj, bk, ax_spec)
assert_almost_equal(R, R2)
def rectify_30(R):
az, el, ti = mat2euler(R, 'szyz')
el = np.rad2deg(el)
# -80 81-159 161-
if el >= -80: return rectify_symmetric_rotation(R, gen_axis_group(180, 2))
elif el >= -160:
return rectify_symmetric_rotation(R, gen_axis_group(90, 2))
else:
return rectify_z_axis_symmetric_rotation(R)
def get_pos_euler(
self,
device: VrDevice,
raw: bool = False,
) -> Tuple[np.ndarray, np.ndarray]:
"""Returns the translation and euler rotation of the given device."""
pos, rot = self.get_pos_rot(device, raw)
ai, aj, ak = mat2euler(rot, axes='rxyz')
return pos, np.array([ai, aj, ak], dtype=np.float32)
def write_bvh(skeleton_original, skeleton_animation, f):
rest = skeleton_original
successors = rest.get_joint_id2children()
joints = rest.joints
joints_visit_order = []
#recursion because we have to put closing bracket }
#for each joint -> it is easier to do with recursion.
#overwise has to add visited array
def record_joint_bvh(jid, shift):
joint = joints[jid]
is_root = jid == 0
has_children = len(successors[jid]) > 0
is_end_site = not has_children
tab = "\t" * shift
tab2 = "\t" * (shift + 1)
if not is_end_site:
joints_visit_order.append(jid)
if is_root:
header = "%sROOT %s\n" % (tab, joint.name)
elif has_children:
header = "%sJOINT %s\n" % (tab, joint.name)
else:
header = "%sEnd Site\n" % tab
if is_root:
offset = [0.0, 0.0, 0.0]
else:
offset = joint.transform.translation
if is_root:
channels = "CHANNELS 6 Xposition Yposition Zposition Zrotation Xrotation Yrotation"
elif has_children:
channels = "CHANNELS 3 Zrotation Xrotation Yrotation"
else:
channels = None
f.write(header)
f.write("%s{\n" % tab)
f.write("%sOFFSET\t %s\n" % (tab2, "\t ".join(map(str, offset))))
if channels is not None:
f.write("%s%s\n" % (tab2, channels))
for cid in successors[jid]:
record_joint_bvh(cid, shift + 1)
f.write("%s}\n" % tab)
#starting from root
root_id = 0
shift = 0
f.write("HIERARCHY\n")
record_joint_bvh(root_id, shift)
f.write("MOTION\n")
f.write("Frames: %d\n" % len(skeleton_animation))
f.write("Frame Time: 0.033333\n")
from math import degrees
for skeleton in skeleton_animation:
joints = skeleton.joints
for jid in joints_visit_order:
joint = joints[jid]
#bvh angles order
angles_rad = mat2euler(joint.transform.rotation, axes='rzxy')
#bvh stores angles in degrees
angles = map(degrees, angles_rad)
if joint.is_root():
f.write("\t ".join(map(str, joint.transform.translation)))
f.write("\t ")
f.write("\t ".join(map(str, angles)))
f.write("\t ")
f.write("\n")
def rvec2euler(rvec):
return mat2euler(cv2.Rodrigues(rvec)[0], axes='rzxy')
