Package com.irurueta.geometry

Class RotationUtils

java.lang.Object
com.irurueta.geometry.RotationUtils
public class RotationUtils extends Object
Utility methods related to rotations.

  • Field Details

    • N_ANGULAR_RATES

      public static final int N_ANGULAR_RATES
      Number of components of angular rates (angular speed).
      See Also:

    • SKEW_MATRIX_SIZE

      public static final int SKEW_MATRIX_SIZE
      Size of skew symmetric matrix omega.
      See Also:

  • Constructor Details

    • RotationUtils

      private RotationUtils()
      Constructor.

  • Method Details

    • w2omega

      public static void w2omega(double w1, double w2, double w3, com.irurueta.algebra.Matrix result)
      Skew symmetric matrix omega from angular rates vector w.
      Parameters:
      w1 - angular rate from x-axis.
      w2 - angular rate from y-axis.
      w3 - angular rate from z-axis.
      result - instance where result will be stored.
      Throws:
      IllegalArgumentException - if provided matrix is not 4x4.
      See Also:

    • w2omega

      public static void w2omega(double[] w, com.irurueta.algebra.Matrix result)
      Skew symmetric matrix omega from angular rates vector w.
      Parameters:
      w - array containing angular rates. Must have length 3.
      result - instance where result will be stored.
      Throws:
      IllegalArgumentException - if provided matrix is not 4x4 or array w does not have length 3.
      See Also:

    • w2omega

      public static com.irurueta.algebra.Matrix w2omega(double w1, double w2, double w3)
      Skew symmetric matrix omega from angular rates vector w.
      Parameters:
      w1 - angular rate from x-axis.
      w2 - angular rate from y-axis.
      w3 - angular rate from z-axis.
      Returns:
      a new skew symmetric matrix omega.
      See Also:

    • w2omega

      public static com.irurueta.algebra.Matrix w2omega(double[] w)
      Skew symmetric matrix omega from angular rates vector w.
      Parameters:
      w - array containing angular rates. Must have length 3.
      Returns:
      a new skew symmetric matrix omega.
      See Also:

    • quaternionToPiMatrix

      public static void quaternionToPiMatrix(Quaternion quaternion, com.irurueta.algebra.Matrix result)
      Converts provided quaternion into Pi matrix. Given a quaternion q = [a b c d]', and the angular rates vector w = [p q r]' and omega = w2omega(w) a skew symmetric matrix, then the PI matrix is the Jacobian respect to w, expressed as: PI = omega(w)*q.
      Parameters:
      quaternion - a quaternion.
      result - matrix where resulting pi matrix is stored.
      Throws:
      IllegalArgumentException - if provided result matrix is not 4x3.
      See Also:

    • quaternionToPiMatrix

      public static com.irurueta.algebra.Matrix quaternionToPiMatrix(Quaternion quaternion)
      Converts provided quaternion into Pi matrix. Given a quaternion q = [a b c d]', and the angular rates vector w = [p q r]' and omega = w2omega(w) a skew symmetric matrix, then the PI matrix is the Jacobian respect to w, expressed as: PI = omega(w)*q.
      Parameters:
      quaternion - a quaternion.
      Returns:
      pi matrix.
      See Also:

    • quaternionToConjugatedPiMatrix

      public static void quaternionToConjugatedPiMatrix(Quaternion quaternion, com.irurueta.algebra.Matrix result)
      Converts provided quaternion into conjugated Pi matrix. Given a quaternion q = [a b c d]', then the conjugated Pi matrix is the Pi matrix of the conjugated quaternion [a -b -c -d]'.
      Parameters:
      quaternion - a quaternion.
      result - matrix where resulting conjugated pi matrix is stored.
      Throws:
      IllegalArgumentException - if provided result matrix is not 4x3.
      See Also:

    • quaternionToConjugatedPiMatrix

      public static com.irurueta.algebra.Matrix quaternionToConjugatedPiMatrix(Quaternion quaternion)
      Converts provided quaternion into conjugated Pi matrix. Given a quaternion q = [a b c d]', then the conjugated Pi matrix is the Pi matrix of the conjugated quaternion [a -b -c -d]'.
      Parameters:
      quaternion - a quaternion.
      Returns:
      a matrix containing the conjugated pi matrix.
      See Also:

    • piMatrixToConjugatedPiMatrix

      public static void piMatrixToConjugatedPiMatrix(com.irurueta.algebra.Matrix pi, com.irurueta.algebra.Matrix result)
      Computes conjugated pi matrix from pi matrix.
      Parameters:
      pi - pi matrix used as input.
      result - instance where conjugated pi matrix is stored.
      Throws:
      IllegalArgumentException - if provided matrices are not 4x3.
      See Also:

    • piMatrixToConjugatedPiMatrix

      public static com.irurueta.algebra.Matrix piMatrixToConjugatedPiMatrix(com.irurueta.algebra.Matrix pi)
      Computes conjugated pi matrix from pi matrix.
      Parameters:
      pi - pi matrix used as input.
      Returns:
      conjugated pi matrix.
      Throws:
      IllegalArgumentException - if provided input matrix is not 4x3.
      See Also:

    • rotationMatrixTimesVector

      public static void rotationMatrixTimesVector(Quaternion q, double[] point, double[] result, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      result - array containing result of rotation.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Throws:
      IllegalArgumentException - if provided arrays of points or result don't have length 3, or if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • rotationMatrixTimesVector

      public static double[] rotationMatrixTimesVector(Quaternion q, double[] point, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Returns:
      array containing result of rotation as 3D inhomogeneous coordinates.
      Throws:
      IllegalArgumentException - if provided point array doesn't have length 3, or if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • rotationMatrixTimesVector

      public static void rotationMatrixTimesVector(Quaternion q, double[] point, double[] result)
      Rotates a point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      result - array containing result of rotation.
      Throws:
      IllegalArgumentException - if provided arrays of points or result don't have length 3.
      See Also:

    • rotationMatrixTimesVector

      public static double[] rotationMatrixTimesVector(Quaternion q, double[] point)
      Rotates a point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      Returns:
      array containing result of rotation as 3D inhomogeneous coordinates.
      Throws:
      IllegalArgumentException - if provided point array doesn't have length 3.
      See Also:

    • rotationMatrixTimesVector

      public static void rotationMatrixTimesVector(Quaternion q, Point3D point, Point3D result, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a 3D point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      result - result of rotation.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Throws:
      IllegalArgumentException - if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • rotationMatrixTimesVector

      public static Point3D rotationMatrixTimesVector(Quaternion q, Point3D point, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a 3D point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Returns:
      result of rotation.
      Throws:
      IllegalArgumentException - if jacobian of quaternions is not 3x4. (if provided), of if jacobian of point is not 3x3 (if provided).
      See Also:

    • rotationMatrixTimesVector

      public static void rotationMatrixTimesVector(Quaternion q, Point3D point, Point3D result)
      Rotates a 3D point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      result - result of rotation.
      See Also:

    • rotationMatrixTimesVector

      public static Point3D rotationMatrixTimesVector(Quaternion q, Point3D point)
      Rotates a 3D point by using the rotation matrix obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      Returns:
      result of rotation.
      See Also:

    • transposedRotationMatrixTimesVector

      public static void transposedRotationMatrixTimesVector(Quaternion q, double[] point, double[] result, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      result - array containing result of rotation.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Throws:
      IllegalArgumentException - if provided arrays of points or result don't have length 3, or if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • transposedRotationMatrixTimesVector

      public static double[] transposedRotationMatrixTimesVector(Quaternion q, double[] point, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Returns:
      result of rotation.
      Throws:
      IllegalArgumentException - if provided arrays of points doesn't have length 3, or if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • transposedRotationMatrixTimesVector

      public static void transposedRotationMatrixTimesVector(Quaternion q, double[] point, double[] result)
      Rotates a point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      result - result of rotation.
      Throws:
      IllegalArgumentException - if provided arrays of points doesn't have length 3.
      See Also:

    • transposedRotationMatrixTimesVector

      public static double[] transposedRotationMatrixTimesVector(Quaternion q, double[] point)
      Rotates a point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      Returns:
      array containing result of rotation as 3D inhomogeneous coordinates.
      Throws:
      IllegalArgumentException - if provided point array doesn't have length 3.
      See Also:

    • transposedRotationMatrixTimesVector

      public static void transposedRotationMatrixTimesVector(Quaternion q, Point3D point, Point3D result, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a 3D point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      result - result of rotation.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Throws:
      IllegalArgumentException - if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • transposedRotationMatrixTimesVector

      public static Point3D transposedRotationMatrixTimesVector(Quaternion q, Point3D point, com.irurueta.algebra.Matrix jacobianQ, com.irurueta.algebra.Matrix jacobianP)
      Rotates a 3D point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - array containing inhomogeneous 3D coordinates of a point to be rotated.
      jacobianQ - jacobian wrt of quaternion. Must be 3x4.
      jacobianP - jacobian wrt of point. Must be 3x3.
      Returns:
      result of rotation.
      Throws:
      IllegalArgumentException - if jacobian of quaternions is not 3x4 (if provided), or if jacobian of point is not 3x3 (if provided).
      See Also:

    • transposedRotationMatrixTimesVector

      public static void transposedRotationMatrixTimesVector(Quaternion q, Point3D point, Point3D result)
      Rotates a 3D point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      result - result of rotation.
      See Also:

    • transposedRotationMatrixTimesVector

      public static Point3D transposedRotationMatrixTimesVector(Quaternion q, Point3D point)
      Rotates a 3D point by the inverse rotation obtained from a quaternion.
      Parameters:
      q - quaternion containing rotation information.
      point - 3D point to be rotated.
      Returns:
      result of rotation.
      See Also:

    • cameraBodyToCameraSensorRotation

      public static void cameraBodyToCameraSensorRotation(MatrixRotation3D result)
      Rotation to convert camera body to camera sensor.
      Parameters:
      result - instance where resulting rotation will be stored.

    • cameraBodyToCameraSensorRotation

      public static MatrixRotation3D cameraBodyToCameraSensorRotation()
      Rotation to convert camera body to camera sensor.
      Returns:
      a new instance containing rotation.

    • quaternionToEulerGaussian

      public static void quaternionToEulerGaussian(Quaternion q, com.irurueta.algebra.Matrix quaternionCovariance, double[] angles, com.irurueta.algebra.Matrix anglesCovariance)
      Transform quaternion Gaussian to Euler Gaussian. The Gaussian quaternion is provided as N(q, Q) where q is the quaternion mean and Q is the quaternion covariance, and returns an Euler angles Gaussian as N(e,E) where e is the Euler angles mean and E is the Euler angles covariance.
      Parameters:
      q - mean quaternion to be transformed.
      quaternionCovariance - quaternion covariance to be transformed.
      angles - obtained mean Euler angles.
      anglesCovariance - obtained Euler covariance.
      Throws:
      IllegalArgumentException - if provided quaternion covariance is not 4x4
      See Also:

    • angularRatesToSkew

      public static void angularRatesToSkew(double[] angularRates, com.irurueta.algebra.Matrix result)
      Obtains the skew symmetric matrix from angular rates vector.
      Parameters:
      angularRates - a vector containing the 3 components (x,y,z) of angular rates (rad/s).
      result - a skew symmetric matrix.
      Throws:
      IllegalArgumentException - if provided array of angular rates does not have length 3 or if provided result matrix is not 4x4.

    • angularRatesToSkew

      public static com.irurueta.algebra.Matrix angularRatesToSkew(double[] angularRates)
      Obtains the skew symmetric matrix from angular rates vector.
      Parameters:
      angularRates - a vector containing the 3 components (x,y,z) of angular rates (rad/s).
      Returns:
      a new instance containing the skew symmetric matrix
      Throws:
      IllegalArgumentException - if provided array of angular rates does not have length 3.