Exemple #1
0
 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)])
Exemple #2
0
 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
     ]
Exemple #3
0
 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])))