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(edu.wpi.first.apriltag.AprilTagFieldLayout fieldTags, PhotonPoseEstimator.PoseStrategy strategy, PhotonCamera camera, 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.
      camera - PhotonCamera
      robotToCamera - Transform3d from the center of the robot to the camera mount position (ie, robot ➔ camera) in the Robot Coordinate System.

    • PhotonPoseEstimator

      public PhotonPoseEstimator(edu.wpi.first.apriltag.AprilTagFieldLayout fieldTags, PhotonPoseEstimator.PoseStrategy strategy, edu.wpi.first.math.geometry.Transform3d robotToCamera)

  • 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 TargetModel getTagModel()
      Get the TargetModel representing the tags being detected. This is used for on-rio multitag.

      By default, this is TargetModel.kAprilTag16h5.

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

    • 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<EstimatedRobotPose> update()
      Poll data from the configured cameras and update the estimated position of the robot. Returns empty if:
      • New data has not been received since the last call to update().
      • No targets were found from the camera
      • There is no camera set
      Returns:
      an EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate.

    • update

      public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult)
      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.
      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<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.N5,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.
      Parameters:
      cameraMatrix - Camera calibration data that can be used in the case of no assigned PhotonCamera.
      distCoeffs - Camera calibration data that can be used in the case of no assigned PhotonCamera
      Returns:
      an EstimatedRobotPose with an estimated pose, timestamp, and targets used to create the estimate.