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

  • Constructor Details

  • 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.