(rmat)
return np.array([x, y, z])
Converts a 3x3 rotation matrix to XYZ euler angles.
Converts a 3x3 rotation matrix to XYZ euler angles.
Converts a 3x3 rotation matrix to XYZ euler angles .
def _rmat_to_euler_xyz(rmat):
  """Converts a 3x3 rotation matrix to XYZ euler angles."""
  # | r00 r01 r02 |   |  cy*cz           -cy*sz            sy    |
  # | r10 r11 r12 | = |  cz*sx*sy+cx*sz   cx*cz-sx*sy*sz  -cy*sx |
  # | r20 r21 r22 |   | -cx*cz*sy+sx*sz   cz*sx+cx*sy*sz   cx*cy |
  if rmat[0, 2] > _POLE_LIMIT:
    logging.log_every_n_seconds(logging.WARNING, 'Angle at North Pole', 60)
    z = np.arctan2(rmat[1, 0], rmat[1, 1])
    y = np.pi/2
    x = 0.0
    return np.array([x, y, z])

  if rmat[0, 2] < -_POLE_LIMIT:
    logging.log_every_n_seconds(logging.WARNING, 'Angle at South Pole', 60)
    z = np.arctan2(rmat[1, 0], rmat[1, 1])
    y = -np.pi/2
    x = 0.0
    return np.array([x, y, z])

  z = -np.arctan2(rmat[0, 1], rmat[0, 0])
  y = np.arcsin(rmat[0, 2])
  x = -np.arctan2(rmat[1, 2], rmat[2, 2])

  # order of return is the order of input
  return np.array([x, y, z])
def _rmat_to_euler_xyz ( rmat ) : # | r00 r01 r02 |   |  cy*cz           -cy*sz            sy    | # | r10 r11 r12 | = |  cz*sx*sy+cx*sz   cx*cz-sx*sy*sz  -cy*sx | # | r20 r21 r22 |   | -cx*cz*sy+sx*sz   cz*sx+cx*sy*sz   cx*cy | if rmat [ 0 , 2 ] > _POLE_LIMIT : logging . log_every_n_seconds ( logging . WARNING , 'Angle at North Pole' , 60 ) z = np . arctan2 ( rmat [ 1 , 0 ] , rmat [ 1 , 1 ] ) y = np . pi / 2 x = 0.0 return np . array ( [ x , y , z ] ) if rmat [ 0 , 2 ] < - _POLE_LIMIT : logging . log_every_n_seconds ( logging . WARNING , 'Angle at South Pole' , 60 ) z = np . arctan2 ( rmat [ 1 , 0 ] , rmat [ 1 , 1 ] ) y = - np . pi / 2 x = 0.0 return np . array ( [ x , y , z ] ) z = - np . arctan2 ( rmat [ 0 , 1 ] , rmat [ 0 , 0 ] ) y = np . arcsin ( rmat [ 0 , 2 ] ) x = - np . arctan2 ( rmat [ 1 , 2 ] , rmat [ 2 , 2 ] ) # order of return is the order of input return np . array ( [ x , y , z ] )