Class VisionEstimation

java.lang.Object
org.photonvision.estimation.VisionEstimation
public class VisionEstimation extends Object

  • Method Summary

    Modifier and Type
    Method
    Description
    static Optional<PnpResult>
    estimateCamPosePNP(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N3> cameraMatrix, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N8, org.wpilib.math.numbers.N1> distCoeffs, List<PhotonTrackedTarget> visTags, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout, TargetModel tagModel)
    Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.
    static Optional<PnpResult>
    estimateRobotPoseConstrainedSolvepnp(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N3> cameraMatrix, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N8, org.wpilib.math.numbers.N1> distCoeffs, List<PhotonTrackedTarget> visTags, org.wpilib.math.geometry.Transform3d robot2camera, org.wpilib.math.geometry.Pose3d robotPoseSeed, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout, TargetModel tagModel, boolean headingFree, org.wpilib.math.geometry.Rotation2d gyroθ, double gyroErrorScaleFac)
    Performs constrained solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.
    static List<org.wpilib.vision.apriltag.AprilTag>
    getVisibleLayoutTags(List<PhotonTrackedTarget> visTags, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout)
    Get the list of visible AprilTags which are in the tag layout using the visible tag IDs.

    Methods inherited from class Object

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

  • Method Details

    • getVisibleLayoutTags

      public static List<org.wpilib.vision.apriltag.AprilTag> getVisibleLayoutTags(List<PhotonTrackedTarget> visTags, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout)
      Get the list of visible AprilTags which are in the tag layout using the visible tag IDs.
      Parameters:
      visTags - The list of targets to search for visible tags.
      tagLayout - The tag layout to search

    • estimateCamPosePNP

      public static Optional<PnpResult> estimateCamPosePNP(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N3> cameraMatrix, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N8, org.wpilib.math.numbers.N1> distCoeffs, List<PhotonTrackedTarget> visTags, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout, TargetModel tagModel)
      Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation. If only one tag is visible, the result may have an alternate solution.

      Note: The returned transformation is from the field origin to the camera pose!

      With only one tag: OpenCVHelp.solvePNP_SQUARE(Matrix, Matrix, List, Point[])

      With multiple tags: OpenCVHelp.solvePNP_SQPNP(Matrix, Matrix, List, Point[])

      Parameters:
      cameraMatrix - The camera intrinsics matrix in standard opencv form
      distCoeffs - The camera distortion matrix in standard opencv form
      visTags - The visible tags reported by PV. Non-tag targets are automatically excluded.
      tagLayout - The known tag layout on the field
      tagModel - The model describing the tag's geometry
      Returns:
      The transformation that maps the field origin to the camera pose. Ensure the PnpResult are present before utilizing them.

    • estimateRobotPoseConstrainedSolvepnp

      public static Optional<PnpResult> estimateRobotPoseConstrainedSolvepnp(org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N3, org.wpilib.math.numbers.N3> cameraMatrix, org.wpilib.math.linalg.Matrix<org.wpilib.math.numbers.N8, org.wpilib.math.numbers.N1> distCoeffs, List<PhotonTrackedTarget> visTags, org.wpilib.math.geometry.Transform3d robot2camera, org.wpilib.math.geometry.Pose3d robotPoseSeed, org.wpilib.vision.apriltag.AprilTagFieldLayout tagLayout, TargetModel tagModel, boolean headingFree, org.wpilib.math.geometry.Rotation2d gyroθ, double gyroErrorScaleFac)
      Performs constrained solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the field-to-camera transformation.

      Note: The returned transformation is from the field origin to the robot drivebase

      Parameters:
      cameraMatrix - The camera intrinsics matrix in standard opencv form
      distCoeffs - The camera distortion matrix in standard opencv form
      visTags - The visible tags reported by PV. Non-tag targets are automatically excluded.
      robot2camera - The Transform3d from the robot odometry frame to the camera optical frame
      robotPoseSeed - An initial guess at robot pose, refined via optimization. Better guesses will converge faster.
      tagLayout - The known tag layout on the field
      tagModel - The physical size of the AprilTags
      headingFree - If heading is completely free, or if our measured gyroθ is taken into account
      gyroθ - If headingFree is false, the best estimate at robot yaw. Excursions from this are penalized in our cost function.
      gyroErrorScaleFac - A relative weight for gyro heading excursions against tag corner reprojection error.
      Returns:
      The transformation that maps the field origin to the camera pose. Ensure the PnpResult are present before utilizing them.