Package org.photonvision

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.

  • Field Details

    • poseCacheTimestampSeconds

      protected double poseCacheTimestampSeconds

  • Constructor Details

    • PhotonPoseEstimator

      public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, PhotonPoseEstimator.PoseStrategy strategy, 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 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(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.

    • getPrimaryStrategy

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

    • setPrimaryStrategy

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

    • setMultiTagFallbackStrategy

      public void setMultiTagFallbackStrategy(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 Pose3d getReferencePose()
      Return the reference position that is being used by the estimator.
      Returns:
      the referencePose

    • setReferencePose

      public void setReferencePose(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(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(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(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, 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, 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.

    • getRobotToCameraTransform

      public Transform3d getRobotToCameraTransform()
      Returns:
      The current transform from the center of the robot to the camera mount position

    • setRobotToCameraTransform

      public void setRobotToCameraTransform(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<EstimatedRobotPose> update(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<EstimatedRobotPose> update(PhotonPipelineResult cameraResult, Optional<Matrix<N3,N3>> cameraMatrix, Optional<Matrix<N8,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<EstimatedRobotPose> update(PhotonPipelineResult cameraResult, Optional<Matrix<N3,N3>> cameraMatrix, Optional<Matrix<N8,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.