Package org.photonvision.estimation

Class OpenCVHelp

java.lang.Object
org.photonvision.estimation.OpenCVHelp
public final class OpenCVHelp extends Object
A set of various utility functions for getting non-OpenCV data into an OpenCV-compatible format, along with other calculation functions.

  • Method Details

    • matrixToMat

      public static org.opencv.core.Mat matrixToMat(org.ejml.simple.SimpleMatrix matrix)
      Converts an EJML SimpleMatrix to OpenCV's Mat by copying the data.
      Parameters:
      matrix - The matrix of data.
      Returns:
      The Mat. Data is encoded as 64-bit floats.

    • matToMatrix

      public static Matrix<Num,Num> matToMatrix(org.opencv.core.Mat mat)
      Converts an OpenCV Mat to WPILib's Matrix by copying the data.
      Parameters:
      mat - The Mat of data. Data is assumed to be encoded as 64-bit floats.
      Returns:
      The Matrix.

    • translationToTvec

      public static org.opencv.core.MatOfPoint3f translationToTvec(Translation3d... translations)
      Creates a new MatOfPoint3f with these 3d translations. The OpenCV tvec is a vector with three elements representing {x, y, z} in the EDN coordinate system.
      Parameters:
      translations - The translations in the NWU coordinate system to convert into a MatOfPoint3f
      Returns:
      The OpenCV tvec.

    • tvecToTranslation

      public static Translation3d tvecToTranslation(org.opencv.core.Mat tvecInput)
      Returns a new 3d translation from this Mat. The OpenCV tvec is a vector with three elements representing {x, y, z} in the EDN coordinate system.
      Parameters:
      tvecInput - The tvec to create a Translation3d from
      Returns:
      The 3d translation in the NWU coordinate system.

    • rotationToRvec

      public static org.opencv.core.MatOfPoint3f rotationToRvec(Rotation3d rotation)
      Creates a new MatOfPoint3f with this 3d rotation. The OpenCV rvec Mat is a vector with three elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm, and axis = rvec / norm)
      Parameters:
      rotation - The rotation to convert into a MatOfPoint3f
      Returns:
      The OpenCV rvec

    • rvecToRotation

      public static Rotation3d rvecToRotation(org.opencv.core.Mat rvecInput)
      Returns a 3d rotation from this Mat. The OpenCV rvec Mat is a vector with three elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm, and axis = rvec / norm)
      Parameters:
      rvecInput - The rvec to create a Rotation3d from
      Returns:
      The 3d rotation

    • avgPoint

      public static org.opencv.core.Point avgPoint(org.opencv.core.Point[] points)
      Calculates the average point from an array of points.
      Parameters:
      points - The array of points
      Returns:
      The average of all the points

    • cornersToPoints

      public static org.opencv.core.Point[] cornersToPoints(List<TargetCorner> corners)
      Converts a TargetCorner list to a Point array.
      Parameters:
      corners - The TargetCorner list
      Returns:
      The Point array

    • cornersToPoints

      public static org.opencv.core.Point[] cornersToPoints(TargetCorner... corners)
      Converts a TargetCorner array to a Point array.
      Parameters:
      corners - The TargetCorner array
      Returns:
      The Point array

    • pointsToCorners

      public static List<TargetCorner> pointsToCorners(org.opencv.core.Point... points)
      Converts a Point array to a TargetCorner list.
      Parameters:
      points - The Point array
      Returns:
      The TargetCorner list

    • pointsToCorners

      public static List<TargetCorner> pointsToCorners(org.opencv.core.MatOfPoint2f matInput)
      Converts an OpenCV MatOfPoint2f to a TargetCorner list.
      Parameters:
      matInput - The Mat
      Returns:
      The TargetCorner list

    • reorderCircular

      public static <T> List<T> reorderCircular(List<T> elements, boolean backwards, int shiftStart)
      Reorders the list, optionally indexing backwards and wrapping around to the last element after the first, and shifting all indices in the direction of indexing.

      e.g.

      ({1,2,3}, false, 1) == {2,3,1}

      ({1,2,3}, true, 0) == {1,3,2}

      ({1,2,3}, true, 1) == {3,2,1}

      Type Parameters:
      T - Element type
      Parameters:
      elements - list elements
      backwards - If indexing should happen in reverse (0, size-1, size-2, ...)
      shiftStart - How much the initial index should be shifted (instead of starting at index 0, start at shiftStart, negated if backwards)
      Returns:
      Reordered list

    • distortPoints

      public static List<org.opencv.core.Point> distortPoints(List<org.opencv.core.Point> pointsList, org.opencv.core.Mat cameraMatrix, org.opencv.core.Mat distCoeffs)
      Distort a list of points in pixels using the OPENCV5/8 models. See image-rotation.md or https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html for the math here.
      Parameters:
      pointsList - the undistorted points
      cameraMatrix - standard OpenCV camera mat
      distCoeffs - standard OpenCV distortion coefficients. Must OPENCV5 or OPENCV8
      Returns:
      the list of distorted points

    • projectPoints

      public static org.opencv.core.Point[] projectPoints(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, RotTrlTransform3d camRt, List<Translation3d> objectTranslations)
      Project object points from the 3d world into the 2d camera image. The camera properties(intrinsics, distortion) determine the results of this projection.
      Parameters:
      cameraMatrix - the camera intrinsics matrix in standard OpenCV form
      distCoeffs - the camera distortion matrix in standard OpenCV form
      camRt - The change in basis from world coordinates to camera coordinates. See RotTrlTransform3d.makeRelativeTo(Pose3d).
      objectTranslations - The 3d points to be projected
      Returns:
      The 2d points in pixels which correspond to the camera's image of the 3d points

    • undistortPoints

      public static org.opencv.core.Point[] undistortPoints(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, org.opencv.core.Point[] points)
      Undistort 2d image points using a given camera's intrinsics and distortion.

      2d image points from projectPoints() will naturally be distorted, so this operation is important if the image points need to be directly used (e.g. 2d yaw/pitch).

      Parameters:
      cameraMatrix - The camera intrinsics matrix in standard OpenCV form
      distCoeffs - The camera distortion matrix in standard OpenCV form
      points - The distorted image points
      Returns:
      The undistorted image points

    • getBoundingRect

      public static org.opencv.core.Rect getBoundingRect(org.opencv.core.Point[] points)
      Gets the (upright) rectangle which bounds this contour.

      Note that rectangle size and position are stored with ints and do not have sub-pixel accuracy.

      Parameters:
      points - The points to be bounded
      Returns:
      Rectangle bounding the given points

    • getMinAreaRect

      public static org.opencv.core.RotatedRect getMinAreaRect(org.opencv.core.Point[] points)
      Gets the rotated rectangle with minimum area which bounds this contour.

      Note that rectangle size and position are stored with floats and have sub-pixel accuracy.

      Parameters:
      points - The points to be bounded
      Returns:
      Rotated rectangle bounding the given points

    • getConvexHull

      public static org.opencv.core.Point[] getConvexHull(org.opencv.core.Point[] points)
      Gets the convex hull contour (the outline) of a list of points.
      Parameters:
      points - The input contour
      Returns:
      The subset of points defining the convex hull. Note that these use ints and not floats.

    • solvePNP_SQUARE

      public static Optional<PnpResult> solvePNP_SQUARE(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, List<Translation3d> modelTrls, org.opencv.core.Point[] imagePoints)
      Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose relative to the target is determined by the supplied 3d points of the target's model and their associated 2d points imaged by the camera. The supplied model translations must be relative to the target's pose.

      For planar targets, there may be an alternate solution which is plausible given the 2d image points. This has an associated "ambiguity" which describes the ratio of reprojection error between the "best" and "alternate" solution.

      This method is intended for use with individual AprilTags, and will not work unless 4 points are provided.

      Parameters:
      cameraMatrix - The camera intrinsics matrix in standard OpenCV form
      distCoeffs - The camera distortion matrix in standard OpenCV form
      modelTrls - The translations of the object corners. These should have the object pose as their origin. These must come in a specific, pose-relative order (in NWU):
      • Point 0: [0, -squareLength / 2, squareLength / 2]
      • Point 1: [0, squareLength / 2, squareLength / 2]
      • Point 2: [0, squareLength / 2, -squareLength / 2]
      • Point 3: [0, -squareLength / 2, -squareLength / 2]
      imagePoints - The projection of these 3d object points into the 2d camera image. The order should match the given object point translations.
      Returns:
      The resulting transformation that maps the camera pose to the target pose and the ambiguity if an alternate solution is available.

    • solvePNP_SQPNP

      public static Optional<PnpResult> solvePNP_SQPNP(Matrix<N3,N3> cameraMatrix, Matrix<N8,N1> distCoeffs, List<Translation3d> objectTrls, org.opencv.core.Point[] imagePoints)
      Finds the transformation that maps the camera's pose to the origin of the supplied object. An "object" is simply a set of known 3d translations that correspond to the given 2d points. If, for example, the object translations are given relative to close-right corner of the blue alliance(the default origin), a camera-to-origin transformation is returned. If the translations are relative to a target's pose, a camera-to-target transformation is returned.

      There must be at least 3 points to use this method. This does not return an alternate solution-- if you are intending to use solvePNP on a single AprilTag, see solvePNP_SQUARE(edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N3>, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8, edu.wpi.first.math.numbers.N1>, java.util.List<edu.wpi.first.math.geometry.Translation3d>, org.opencv.core.Point[]) instead.

      Parameters:
      cameraMatrix - The camera intrinsics matrix in standard OpenCV form
      distCoeffs - The camera distortion matrix in standard OpenCV form
      objectTrls - The translations of the object corners, relative to the field.
      imagePoints - The projection of these 3d object points into the 2d camera image. The order should match the given object point translations.
      Returns:
      The resulting transformation that maps the camera pose to the target pose. If the 3d model points are supplied relative to the origin, this transformation brings the camera to the origin.