Package org.photonvision.common.util.math

Class MathUtils

java.lang.Object
org.photonvision.common.util.math.MathUtils
public class MathUtils extends Object

  • Method Summary

    Modifier and Type
    Method
    Description
    static edu.wpi.first.math.geometry.Transform3d
    convertApriltagtoOpenCV(edu.wpi.first.math.geometry.Transform3d pose)
    AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag instead of away from it and towards the camera.
    static edu.wpi.first.math.geometry.Pose3d
    convertArucotoOpenCV(edu.wpi.first.math.geometry.Transform3d pose)
     
    static edu.wpi.first.math.geometry.Transform3d
    convertOpenCVtoPhotonTransform(edu.wpi.first.math.geometry.Transform3d cameraToTarget3d)
    OpenCV uses the EDN coordinate system, but WPILib uses NWU.
    static double
    getPercentile(List<Double> list, double p)
    Get the value of the nTh percentile of a list
    static double
    lerp(double startValue, double endValue, double t)
    Linearly interpolates between two values.
    static double
    map(double value, double in_min, double in_max, double out_min, double out_max)
     
    static int
    map(int value, int inMin, int inMax, int outMin, int outMax)
     
    static long
    microsToNanos(long micros)
     
    static long
    millisToNanos(long millis)
     
    static long
    nanosToMicros(long nanos)
     
    static double
    nanosToMillis(double nanos)
     
    static double
    nanosToMillis(long nanos)
     
    static edu.wpi.first.math.geometry.Pose3d
    opencvRTtoPose3d(org.opencv.core.Mat rVec, org.opencv.core.Mat tVec)
    Convert an Opencv rvec+tvec pair to a Pose3d.
    static void
    rotationToOpencvRvec(edu.wpi.first.math.geometry.Rotation3d rotation, org.opencv.core.Mat rvecOutput)
     
    static double
    roundTo(double value, int to)
     
    static int
    safeDivide(int quotient, int divisor)
     
    static double
    toSlope(Number angle)
     
    static long
    wpiNanoTime()
     

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Method Details

    • toSlope

      public static double toSlope(Number angle)

    • safeDivide

      public static int safeDivide(int quotient, int divisor)

    • roundTo

      public static double roundTo(double value, int to)

    • nanosToMillis

      public static double nanosToMillis(long nanos)

    • nanosToMillis

      public static double nanosToMillis(double nanos)

    • millisToNanos

      public static long millisToNanos(long millis)

    • microsToNanos

      public static long microsToNanos(long micros)

    • nanosToMicros

      public static long nanosToMicros(long nanos)

    • map

      public static double map(double value, double in_min, double in_max, double out_min, double out_max)

    • map

      public static int map(int value, int inMin, int inMax, int outMin, int outMax)

    • wpiNanoTime

      public static long wpiNanoTime()

    • getPercentile

      public static double getPercentile(List<Double> list, double p)
      Get the value of the nTh percentile of a list
      Parameters:
      list - The list to evaluate
      p - The percentile, in [0,100]
      Returns:

    • lerp

      public static double lerp(double startValue, double endValue, double t)
      Linearly interpolates between two values.
      Parameters:
      startValue - The start value.
      endValue - The end value.
      t - The fraction for interpolation.
      Returns:
      The interpolated value.

    • convertOpenCVtoPhotonTransform

      public static edu.wpi.first.math.geometry.Transform3d convertOpenCVtoPhotonTransform(edu.wpi.first.math.geometry.Transform3d cameraToTarget3d)
      OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target transformation from EDN to NWU.

      Note: The detected target's rvec and tvec perform a rotation-translation transformation which converts points in the target's coordinate system to the camera's. This means applying the transformation to the target point (0,0,0) for example would give the target's center relative to the camera. Conveniently, if we make a translation-rotation transformation out of these components instead, we get the transformation from the camera to the target.

      Parameters:
      cameraToTarget3d - A camera-to-target Transform3d in EDN.
      Returns:
      A camera-to-target Transform3d in NWU.

    • convertApriltagtoOpenCV

      public static edu.wpi.first.math.geometry.Transform3d convertApriltagtoOpenCV(edu.wpi.first.math.geometry.Transform3d pose)
      AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag instead of away from it and towards the camera. This means we have to correct the transformation's rotation.
      Parameters:
      pose - The Transform3d with translation and rotation directly from the AprilTagPoseEstimate.

    • convertArucotoOpenCV

      public static edu.wpi.first.math.geometry.Pose3d convertArucotoOpenCV(edu.wpi.first.math.geometry.Transform3d pose)

    • rotationToOpencvRvec

      public static void rotationToOpencvRvec(edu.wpi.first.math.geometry.Rotation3d rotation, org.opencv.core.Mat rvecOutput)

    • opencvRTtoPose3d

      public static edu.wpi.first.math.geometry.Pose3d opencvRTtoPose3d(org.opencv.core.Mat rVec, org.opencv.core.Mat tVec)
      Convert an Opencv rvec+tvec pair to a Pose3d.
      Parameters:
      rVec - Axis-angle rotation vector, where norm(rVec) is the angle about a unit vector in the direction of rVec
      tVec - 3D translation
      Returns:
      Pose3d representing the same rigid transform