Package org.photonvision

Class PhotonUtils

java.lang.Object
org.photonvision.PhotonUtils
public final class PhotonUtils extends Object

  • Method Details

    • calculateDistanceToTargetMeters

      public static double calculateDistanceToTargetMeters(double cameraHeightMeters, double targetHeightMeters, double cameraPitchRadians, double targetPitchRadians)
      Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates range to a target using the target's elevation. This method can produce more stable results than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and for there to exist a height differential between goal and camera. The larger this differential, the more accurate the distance estimate will be.

      Units can be converted using the Units class.

      Parameters:
      cameraHeightMeters - The physical height of the camera off the floor in meters.
      targetHeightMeters - The physical height of the target off the floor in meters. This should be the height of whatever is being targeted (i.e. if the targeting region is set to top, this should be the height of the top of the target).
      cameraPitchRadians - The pitch of the camera from the horizontal plane in radians. Positive values up.
      targetPitchRadians - The pitch of the target in the camera's lens in radians. Positive values up.
      Returns:
      The estimated distance to the target in meters.

    • estimateCameraToTargetTranslation

      public static Translation2d estimateCameraToTargetTranslation(double targetDistanceMeters, Rotation2d yaw)
      Estimate the Translation2d of the target relative to the camera.
      Parameters:
      targetDistanceMeters - The distance to the target in meters.
      yaw - The observed yaw of the target.
      Returns:
      The target's camera-relative translation.

    • estimateFieldToRobot

      public static Pose2d estimateFieldToRobot(double cameraHeightMeters, double targetHeightMeters, double cameraPitchRadians, double targetPitchRadians, Rotation2d targetYaw, Rotation2d gyroAngle, Pose2d fieldToTarget, Transform2d cameraToRobot)
      Estimate the position of the robot in the field.
      Parameters:
      cameraHeightMeters - The physical height of the camera off the floor in meters.
      targetHeightMeters - The physical height of the target off the floor in meters. This should be the height of whatever is being targeted (i.e. if the targeting region is set to top, this should be the height of the top of the target).
      cameraPitchRadians - The pitch of the camera from the horizontal plane in radians. Positive values up.
      targetPitchRadians - The pitch of the target in the camera's lens in radians. Positive values up.
      targetYaw - The observed yaw of the target. Note that this *must* be CCW-positive, and Photon returns CW-positive.
      gyroAngle - The current robot gyro angle, likely from odometry.
      fieldToTarget - A Pose2d representing the target position in the field coordinate system.
      cameraToRobot - The position of the robot relative to the camera. If the camera was mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be Transform2d(3 inches, 0 inches, 0 degrees).
      Returns:
      The position of the robot in the field.

    • estimateCameraToTarget

      public static Transform2d estimateCameraToTarget(Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle)
      Estimates a Transform2d that maps the camera position to the target position, using the robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system -- that is, it should read zero degrees when pointed towards the opposing alliance station, and increase as the robot rotates CCW.
      Parameters:
      cameraToTargetTranslation - A Translation2d that encodes the x/y position of the target relative to the camera.
      fieldToTarget - A Pose2d representing the target position in the field coordinate system.
      gyroAngle - The current robot gyro angle, likely from odometry.
      Returns:
      A Transform2d that takes us from the camera to the target.

    • estimateFieldToRobot

      public static Pose2d estimateFieldToRobot(Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot)
      Estimates the pose of the robot in the field coordinate system, given the position of the target relative to the camera, the target relative to the field, and the robot relative to the camera.
      Parameters:
      cameraToTarget - The position of the target relative to the camera.
      fieldToTarget - The position of the target in the field.
      cameraToRobot - The position of the robot relative to the camera. If the camera was mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be Transform2d(3 inches, 0 inches, 0 degrees).
      Returns:
      The position of the robot in the field.

    • estimateFieldToCamera

      public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget)
      Estimates the pose of the camera in the field coordinate system, given the position of the target relative to the camera, and the target relative to the field. This *only* tracks the position of the camera, not the position of the robot itself.
      Parameters:
      cameraToTarget - The position of the target relative to the camera.
      fieldToTarget - The position of the target in the field.
      Returns:
      The position of the camera in the field.

    • estimateFieldToRobotAprilTag

      public static Pose3d estimateFieldToRobotAprilTag(Transform3d cameraToTarget, Pose3d fieldRelativeTagPose, Transform3d cameraToRobot)
      Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial tag, the robot relative to the camera, and the target relative to the camera.
      Parameters:
      fieldRelativeTagPose - Pose3D the field relative pose of the target
      cameraToRobot - Transform3D of the robot relative to the camera. Origin of the robot is defined as the center.
      cameraToTarget - Transform3D of the target relative to the camera, returned by PhotonVision
      Returns:
      Transform3d Robot position relative to the field

    • getYawToPose

      public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose)
      Returns the yaw between your robot and a target.
      Parameters:
      robotPose - Current pose of the robot
      targetPose - Pose of the target on the field
      Returns:
      double Yaw to the target

    • getDistanceToPose

      public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose)
      Returns the distance between two poses
      Parameters:
      robotPose - Pose of the robot
      targetPose - Pose of the target
      Returns:
      distance to the pose