Package org.photonvision.common.util.math

Class MathUtils

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

  • 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)

    • quantize

      public static int quantize(int value, int[] allowableSteps)
      Constrain a value to only take on certain values. Pick the next-highest allowed value in the array if in-between.
      Parameters:
      value - value to quantize
      allowableSteps - sorted array of the allowed values
      Returns:
      quantized value

    • 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 Transform3d convertOpenCVtoPhotonTransform(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 Transform3d convertApriltagtoOpenCV(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 Pose3d convertArucotoOpenCV(Transform3d pose)

    • rotationToOpencvRvec

      public static void rotationToOpencvRvec(Rotation3d rotation, org.opencv.core.Mat rvecOutput)

    • opencvRTtoPose3d

      public static 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