Class PhotonPoseEstimator

java.lang.Object
org.photonvision.PhotonPoseEstimator
public class PhotonPoseEstimator extends Object
The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a given timestamp on the field to produce a single robot in field pose, using the strategy set below. Example usage can be found in our apriltagExample example project.

  • Constructor Details

    • PhotonPoseEstimator

      public PhotonPoseEstimator(org.wpilib.vision.apriltag.AprilTagFieldLayout fieldTags, org.wpilib.math.geometry.Transform3d robotToCamera)
      Create a new PhotonPoseEstimator.
      Parameters:
      fieldTags - A WPILib AprilTagFieldLayout linking AprilTag IDs to Pose3d objects with respect to the FIRST field using the Field Coordinate System. Note that setting the origin of this layout object will affect the results from this class.
      robotToCamera - Transform3d from the center of the robot to the camera mount position (ie, robot ➔ camera) in the Robot Coordinate System.

  • Method Details

    • getFieldTags

      public org.wpilib.vision.apriltag.AprilTagFieldLayout getFieldTags()
      Get the AprilTagFieldLayout being used by the PositionEstimator.

      Note: Setting the origin of this layout will affect the results from this class.

      Returns:
      the AprilTagFieldLayout

    • setFieldTags

      public void setFieldTags(org.wpilib.vision.apriltag.AprilTagFieldLayout fieldTags)
      Set the AprilTagFieldLayout being used by the PositionEstimator.

      Note: Setting the origin of this layout will affect the results from this class.

      Parameters:
      fieldTags - the AprilTagFieldLayout

    • getTagModel

      public TargetModel getTagModel()
      Get the TargetModel representing the tags being detected. This is used for on-rio multitag.

      By default, this is TargetModel.kAprilTag36h11.

    • setTagModel

      public void setTagModel(TargetModel tagModel)
      Set the TargetModel representing the tags being detected. This is used for on-rio multitag.
      Parameters:
      tagModel - E.g. TargetModel.kAprilTag16h5.

    • addHeadingData

      public void addHeadingData(double timestampSeconds, org.wpilib.math.geometry.Rotation3d heading)
      Add robot heading data to buffer. Must be called periodically for the PNP_DISTANCE_TRIG_SOLVE strategy.
      Parameters:
      timestampSeconds - Timestamp of the robot heading data.
      heading - Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

    • addHeadingData

      public void addHeadingData(double timestampSeconds, org.wpilib.math.geometry.Rotation2d heading)
      Add robot heading data to buffer. Must be called periodically for the PNP_DISTANCE_TRIG_SOLVE strategy.
      Parameters:
      timestampSeconds - Timestamp of the robot heading data.
      heading - Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

    • resetHeadingData

      public void resetHeadingData(double timestampSeconds, org.wpilib.math.geometry.Rotation3d heading)
      Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates from utilizing heading data provided prior to a pose or rotation reset.
      Parameters:
      timestampSeconds - Timestamp of the robot heading data.
      heading - Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

    • resetHeadingData

      public void resetHeadingData(double timestampSeconds, org.wpilib.math.geometry.Rotation2d heading)
      Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates from utilizing heading data provided prior to a pose or rotation reset.
      Parameters:
      timestampSeconds - Timestamp of the robot heading data.
      heading - Field-relative robot heading at given timestamp. Standard WPILIB field coordinates.

    • getRobotToCameraTransform

      public org.wpilib.math.geometry.Transform3d getRobotToCameraTransform()
      Returns:
      The current transform from the center of the robot to the camera mount position

    • setRobotToCameraTransform

      public void setRobotToCameraTransform(org.wpilib.math.geometry.Transform3d robotToCamera)
      Useful for pan and tilt mechanisms and such.
      Parameters:
      robotToCamera - The current transform from the center of the robot to the camera mount position

    • estimatePnpDistanceTrigSolvePose

      public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(PhotonPipelineResult cameraResult)
      Return the estimated position of the robot by using distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading data is up-to-date.

      Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)

      https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98

      Parameters:
      cameraResult - A pipeline result from the camera.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets or heading data.

    • estimateConstrainedSolvepnpPose

      public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(PhotonPipelineResult cameraResult, 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, org.wpilib.math.geometry.Pose3d seedPose, boolean headingFree, double headingScaleFactor)
      Return the estimated position of the robot by solving a constrained version of the Perspective-n-Point problem with the robot's drivebase flat on the floor. This computation takes place on the RoboRIO, and typically takes not more than 2ms. See ConstrainedSolvepnpJni for tuning handles this strategy exposes. Internally, the cost function is a sum-squared of pixel reprojection error + (optionally) heading error * heading scale factor. This strategy needs addHeadingData called every frame so heading data is up-to-date.
      Parameters:
      cameraResult - A pipeline result from the camera.
      cameraMatrix - Camera intrinsics from camera calibration data.
      distCoeffs - Distortion coefficients from camera calibration data.
      seedPose - An initial guess at robot pose, refined via optimization. Better guesses will converge faster. Can come from any pose source, but some battle-tested sources are estimateCoprocMultiTagPose(PhotonPipelineResult), or estimateLowestAmbiguityPose(PhotonPipelineResult) if MultiTag results aren't available.
      headingFree - If true, heading is completely free to vary. If false, heading excursions from the provided heading measurement will be penalized
      headingScaleFactor - If headingFree is false, this weights the cost of changing our robot heading estimate against the tag corner reprojection error cont.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets or heading data, or if the solver fails to solve the problem.

    • estimateCoprocMultiTagPose

      public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(PhotonPipelineResult cameraResult)
      Return the estimated position of the robot by using all visible tags to compute a single pose estimate on coprocessor. This option needs to be enabled on the PhotonVision web UI as well.
      Parameters:
      cameraResult - A pipeline result from the camera.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets, no multi-tag results, or multi-tag is disabled in the web UI.

    • estimateRioMultiTagPose

      public Optional<EstimatedRobotPose> estimateRioMultiTagPose(PhotonPipelineResult cameraResult, 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)
      Return the estimated position of the robot by using all visible tags to compute a single pose estimate on the RoboRIO. This can take a lot of time due to the RIO's weak computing power.
      Parameters:
      cameraResult - A pipeline result from the camera.
      cameraMatrix - Camera intrinsics from camera calibration data
      distCoeffs - Distortion coefficients from camera calibration data.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's less than 2 targets visible or SolvePnP fails.

    • estimateLowestAmbiguityPose

      public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(PhotonPipelineResult cameraResult)
      Return the estimated position of the robot with the lowest position ambiguity from a pipeline result.
      Parameters:
      cameraResult - A pipeline result from the camera.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets.

    • estimateClosestToCameraHeightPose

      public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(PhotonPipelineResult cameraResult)
      Return the estimated position of the robot using the target with the lowest delta height difference between the estimated and actual height of the camera.
      Parameters:
      cameraResult - A pipeline result from the camera.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets.

    • estimateClosestToReferencePose

      public Optional<EstimatedRobotPose> estimateClosestToReferencePose(PhotonPipelineResult cameraResult, org.wpilib.math.geometry.Pose3d referencePose)
      Return the estimated position of the robot using the target with the lowest delta in the vector magnitude between it and the reference pose.
      Parameters:
      cameraResult - A pipeline result from the camera.
      referencePose - reference pose to check vector magnitude difference against.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets.

    • estimateAverageBestTargetsPose

      public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(PhotonPipelineResult cameraResult)
      Return the average of the best target poses using ambiguity as weight.
      Parameters:
      cameraResult - A pipeline result from the camera.
      Returns:
      An EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate, or an empty optional if there's no targets.