Package org.team1126.robot.util

Class PhotonPoseEstimator

java.lang.Object
org.team1126.robot.util.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.

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static final record 
    PhotonPoseEstimator.ConstrainedSolvepnpParams
    Tuning handles we have over the CONSTRAINED_SOLVEPNP.

  • Field Summary

    Fields
    Modifier and Type
    Field
    Description
    protected double
    poseCacheTimestampSeconds
     

  • Constructor Summary

    Constructors
    Constructor
    Description
    PhotonPoseEstimator(edu.wpi.first.apriltag.AprilTagFieldLayout fieldTags, org.photonvision.PhotonPoseEstimator.PoseStrategy strategy, edu.wpi.first.math.geometry.Transform3d robotToCamera)
    Create a new PhotonPoseEstimator.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addHeadingData(double timestampSeconds, edu.wpi.first.math.geometry.Rotation2d heading)
    Add robot heading data to buffer.
    void
    addHeadingData(double timestampSeconds, edu.wpi.first.math.geometry.Rotation3d heading)
    Add robot heading data to buffer.
    void
    clearHeadingData()
    Clears all heading data in the buffer.
    edu.wpi.first.apriltag.AprilTagFieldLayout
    getFieldTags()
    Get the AprilTagFieldLayout being used by the PositionEstimator.
    org.photonvision.PhotonPoseEstimator.PoseStrategy
    getPrimaryStrategy()
    Get the Position Estimation Strategy being used by the Position Estimator.
    edu.wpi.first.math.geometry.Pose3d
    getReferencePose()
    Return the reference position that is being used by the estimator.
    edu.wpi.first.math.geometry.Transform3d
    getRobotToCameraTransform()
     
    org.photonvision.estimation.TargetModel
    getTagModel()
    Get the TargetModel representing the tags being detected.
    void
    setFieldTags(edu.wpi.first.apriltag.AprilTagFieldLayout fieldTags)
    Set the AprilTagFieldLayout being used by the PositionEstimator.
    void
    setLastPose(edu.wpi.first.math.geometry.Pose2d lastPose)
    Update the stored last pose.
    void
    setLastPose(edu.wpi.first.math.geometry.Pose3d lastPose)
    Update the stored last pose.
    void
    setMultiTagFallbackStrategy(org.photonvision.PhotonPoseEstimator.PoseStrategy strategy)
    Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen.
    void
    setPrimaryStrategy(org.photonvision.PhotonPoseEstimator.PoseStrategy strategy)
    Set the Position Estimation Strategy used by the Position Estimator.
    void
    setReferencePose(edu.wpi.first.math.geometry.Pose2d referencePose)
    Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
    void
    setReferencePose(edu.wpi.first.math.geometry.Pose3d referencePose)
    Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
    void
    setRobotToCameraTransform(edu.wpi.first.math.geometry.Transform3d robotToCamera)
    Useful for pan and tilt mechanisms and such.
    void
    setTagModel(org.photonvision.estimation.TargetModel tagModel)
    Set the TargetModel representing the tags being detected.
    Optional<org.photonvision.EstimatedRobotPose>
    update(org.photonvision.targeting.PhotonPipelineResult cameraResult)
    Updates the estimated position of the robot, assuming no camera calibration is required for the selected strategy.
    Optional<org.photonvision.EstimatedRobotPose>
    update(org.photonvision.targeting.PhotonPipelineResult cameraResult, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3>> cameraMatrix, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8,edu.wpi.first.math.numbers.N1>> distCoeffs)
    Updates the estimated position of the robot.
    Optional<org.photonvision.EstimatedRobotPose>
    update(org.photonvision.targeting.PhotonPipelineResult cameraResult, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3>> cameraMatrix, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8,edu.wpi.first.math.numbers.N1>> distCoeffs, Optional<PhotonPoseEstimator.ConstrainedSolvepnpParams> constrainedPnpParams)
    Updates the estimated position of the robot.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • poseCacheTimestampSeconds

      protected double poseCacheTimestampSeconds

  • Constructor Details

    • PhotonPoseEstimator

      public PhotonPoseEstimator(edu.wpi.first.apriltag.AprilTagFieldLayout fieldTags, org.photonvision.PhotonPoseEstimator.PoseStrategy strategy, edu.wpi.first.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.
      strategy - The strategy it should use to determine the best pose.
      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 edu.wpi.first.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(edu.wpi.first.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 org.photonvision.estimation.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(org.photonvision.estimation.TargetModel tagModel)
      Set the TargetModel representing the tags being detected. This is used for on-rio multitag.
      Parameters:
      tagModel - E.g. TargetModel.kAprilTag16h5.

    • getPrimaryStrategy

      public org.photonvision.PhotonPoseEstimator.PoseStrategy getPrimaryStrategy()
      Get the Position Estimation Strategy being used by the Position Estimator.
      Returns:
      the strategy

    • setPrimaryStrategy

      public void setPrimaryStrategy(org.photonvision.PhotonPoseEstimator.PoseStrategy strategy)
      Set the Position Estimation Strategy used by the Position Estimator.
      Parameters:
      strategy - the strategy to set

    • setMultiTagFallbackStrategy

      public void setMultiTagFallbackStrategy(org.photonvision.PhotonPoseEstimator.PoseStrategy strategy)
      Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must NOT be MULTI_TAG_PNP
      Parameters:
      strategy - the strategy to set

    • getReferencePose

      public edu.wpi.first.math.geometry.Pose3d getReferencePose()
      Return the reference position that is being used by the estimator.
      Returns:
      the referencePose

    • setReferencePose

      public void setReferencePose(edu.wpi.first.math.geometry.Pose3d referencePose)
      Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
      Parameters:
      referencePose - the referencePose to set

    • setReferencePose

      public void setReferencePose(edu.wpi.first.math.geometry.Pose2d referencePose)
      Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE strategy.
      Parameters:
      referencePose - the referencePose to set

    • setLastPose

      public void setLastPose(edu.wpi.first.math.geometry.Pose3d lastPose)
      Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy.
      Parameters:
      lastPose - the lastPose to set

    • setLastPose

      public void setLastPose(edu.wpi.first.math.geometry.Pose2d lastPose)
      Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy.
      Parameters:
      lastPose - the lastPose to set

    • addHeadingData

      public void addHeadingData(double timestampSeconds, edu.wpi.first.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, edu.wpi.first.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.

    • clearHeadingData

      public void clearHeadingData()
      Clears all heading data in the buffer.

    • getRobotToCameraTransform

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

    • setRobotToCameraTransform

      public void setRobotToCameraTransform(edu.wpi.first.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

    • update

      public Optional<org.photonvision.EstimatedRobotPose> update(org.photonvision.targeting.PhotonPipelineResult cameraResult)
      Updates the estimated position of the robot, assuming no camera calibration is required for the selected strategy. Returns empty if:
      • The timestamp of the provided pipeline result is the same as in the previous call to update().
      • No targets were found in the pipeline results.
      Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not provided in this overload.
      Parameters:
      cameraResult - The latest pipeline result from the camera
      Returns:
      an EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate.

    • update

      public Optional<org.photonvision.EstimatedRobotPose> update(org.photonvision.targeting.PhotonPipelineResult cameraResult, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3>> cameraMatrix, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8,edu.wpi.first.math.numbers.N1>> distCoeffs)
      Updates the estimated position of the robot. Returns empty if:
      • The timestamp of the provided pipeline result is the same as in the previous call to update().
      • No targets were found in the pipeline results.
      • The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the other function overload).
      Parameters:
      cameraMatrix - Camera calibration data for multi-tag-on-rio strategy - can be empty otherwise
      distCoeffs - Camera calibration data for multi-tag-on-rio strategy - can be empty otherwise
      Returns:
      an EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate.

    • update

      public Optional<org.photonvision.EstimatedRobotPose> update(org.photonvision.targeting.PhotonPipelineResult cameraResult, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N3>> cameraMatrix, Optional<edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N8,edu.wpi.first.math.numbers.N1>> distCoeffs, Optional<PhotonPoseEstimator.ConstrainedSolvepnpParams> constrainedPnpParams)
      Updates the estimated position of the robot. Returns empty if:
      • The timestamp of the provided pipeline result is the same as in the previous call to update().
      • No targets were found in the pipeline results.
      • The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty.
      Parameters:
      cameraMatrix - Camera calibration data for multi-tag-on-rio strategy - can be empty otherwise
      distCoeffs - Camera calibration data for multi-tag-on-rio strategy - can be empty otherwise
      constrainedPnpParams - Constrained SolvePNP params, if needed.
      Returns:
      an EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate.