def transform_pose(self, pose):
pose_rot = quat2rotmat(pose[3:])
pose_trans = xyz2mat(pose[:3])
pose_cam = self.V.dot(pose_trans.T).dot(pose_rot).T
return np.concatenate(
[mat2xyz(pose_cam),
safemat2quat(pose_cam[:3, :3].T)])
def set_poses(self, poses):
self.poses_rot = [
np.ascontiguousarray(quat2rotmat(item[3:])) for item in poses
]
self.poses_trans = [
np.ascontiguousarray(xyz2mat(item[:3])) for item in poses
]
def set_allocentric_poses(self, poses):
self.poses_rot = []
self.poses_trans = []
for pose in poses:
x, y, z = pose[:3]
quat_input = pose[3:]
dx = np.arctan2(x, -z)
dy = np.arctan2(y, -z)
# print(dx, dy)
quat = euler2quat(-dy, -dx, 0, axes='sxyz')
quat = qmult(quat, quat_input)
self.poses_rot.append(np.ascontiguousarray(quat2rotmat(quat)))
self.poses_trans.append(np.ascontiguousarray(xyz2mat(pose[:3])))
