def process_poses(all_poses, pose_format='full-mat', normalize_poses=True):
# pose_format value here is the default(current) representation
_, _, poses_mean, poses_std = calc_poses_params(all_poses,
pose_format='full-mat')
# print('poses_mean = {}'.format(poses_mean))
# print('poses_std = {}'.format(poses_std))
# Default pose format is full-mat
new_poses = all_poses
if pose_format == 'quat':
# Convert to quaternions -> tuples created for quaternions. Define a rotation in term q0 about a vector. Avoids wrap around infinity.
new_poses = np.zeros((len(all_poses), 7))
for i in range(len(all_poses)):
p = all_poses[i]
R = p[:3, :3]
t = p[:3, 3]
q = txq.mat2quat(R)
# Constrain rotations to one hemisphere
q *= np.sign(q[0])
new_poses[i, :3] = t
new_poses[i, 3:] = q
all_poses = new_poses
if normalize_poses:
# print('Poses Normalized! pose_format = {}'.format(pose_format))
if pose_format == 'quat':
all_poses[:, :
3] -= poses_mean # Subtrating the mean from translations
all_poses[:, :3] = np.divide(
all_poses[:, :3], poses_std, where=poses_std != 0
) # Dividing translation with std deviation! Hence normalized values!
else: # 'full-mat'
all_poses[:, :3, 3] -= poses_mean
all_poses[:, :3, 3] = np.divide(all_poses[:, :3, 3],
poses_std,
where=poses_std != 0)
# print('all_poses samples = {}'.format(all_poses[:10]))
return all_poses, poses_mean, poses_std
def process_poses(all_poses, pose_format='full-mat', normalize_poses=True):
# pose_format value here is the default(current) representation
_, _, poses_mean, poses_std = calc_poses_params(all_poses,
pose_format='full-mat')
# print('poses_mean = {}'.format(poses_mean))
# print('poses_std = {}'.format(poses_std))
# Default pose format is full-mat
new_poses = all_poses
if pose_format == 'quat':
# Convert to quaternions
new_poses = np.zeros((len(all_poses), 7))
for i in range(len(all_poses)):
p = all_poses[i]
R = p[:3, :3]
t = p[:3, 3]
q = txq.mat2quat(R)
# Constrain rotations to one hemisphere
q *= np.sign(q[0])
new_poses[i, :3] = t
new_poses[i, 3:] = q
all_poses = new_poses
if normalize_poses:
# print('Poses Normalized! pose_format = {}'.format(pose_format))
if pose_format == 'quat':
all_poses[:, :3] -= poses_mean
all_poses[:, :3] = np.divide(all_poses[:, :3],
poses_std,
where=poses_std != 0)
else: # 'full-mat'
all_poses[:, :3, 3] -= poses_mean
all_poses[:, :3, 3] = np.divide(all_poses[:, :3, 3],
poses_std,
where=poses_std != 0)
# print('all_poses samples = {}'.format(all_poses[:10]))
return all_poses, poses_mean, poses_std
def get_poses_params(self, all_records=False):
"""Returns min, max, mean and std values the poses translations"""
poses = self.d_poses if all_records else self.d_poses_rec
return calc_poses_params(poses, pose_format=self.pose_format)