Utility Class: Common Calculations

A PhotonUtils class with helpful common calculations is included within PhotonLib to aid teams. This class contains two methods, calculateDistanceToTargetMeters()/CalculateDistanceToTarget() and estimateTargetTranslation2d()/EstimateTargetTranslation() (Java and C++ respectively).

Calculating Distance to Target

If your camera is at a fixed height on your robot and the height of the target is fixed, you can calculate the distance to the target based on your camera’s pitch and the pitch to the target.

Java

C++

    // Constants such as camera and target height stored. Change per robot and goal!
    final double CAMERA_HEIGHT_METERS = Units.inchesToMeters(24);
    final double TARGET_HEIGHT_METERS = Units.feetToMeters(5);

    // Angle between horizontal and the camera.
    final double CAMERA_PITCH_RADIANS = Units.degreesToRadians(0);

        if (xboxController.getAButton()) {
            // Vision-alignment mode
            // Query the latest result from PhotonVision
            var result = camera.getLatestResult();

            if (result.hasTargets()) {
                // First calculate range
                double range =
                        PhotonUtils.calculateDistanceToTargetMeters(
                                CAMERA_HEIGHT_METERS,
                                TARGET_HEIGHT_METERS,
                                CAMERA_PITCH_RADIANS,
                                Units.degreesToRadians(result.getBestTarget().getPitch()));

  // Constants such as camera and target height stored. Change per robot and
  // goal!
  const units::meter_t CAMERA_HEIGHT = 24_in;
  const units::meter_t TARGET_HEIGHT = 5_ft;

  // Angle between horizontal and the camera.
  const units::radian_t CAMERA_PITCH = 0_deg;

      units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
          CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
          units::degree_t{result.GetBestTarget().GetPitch()});

Note

The C++ version of PhotonLib uses the Units library. For more information, see here.

Estimating Camera Translation to Target

You can get a translation to the target based on the distance to the target (calculated above) and angle to the target (yaw).

Java

C++

// Calculate a translation from the camera to the target.
Translation2d translation = PhotonUtils.estimateCameraToTargetTranslation(
  distanceMeters, Rotation2d.fromDegrees(-target.getYaw()));

// Calculate a translation from the camera to the target.
frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslationn(
  distance, frc::Rotation2d(units::degree_t(-target.GetYaw())));

Note

We are negating the yaw from the camera from CV (computer vision) conventions to standard mathematical conventions. In standard mathematical conventions, as you turn counter-clockwise, angles become more positive.

Estimating Field Relative Pose

You can get your robot’s Pose2D on the field using various camera data, target yaw, gyro angle, target pose, and camera position. This method estimates the target’s relative position using estimateCameraToTargetTranslation (which uses pitch and yaw to estimate range and heading), and the robot’s gyro to estimate the rotation of the target.

Java

C++

// Calculate robot's field relative pose
Pose2D robotPose = PhotonUtils.estimateFieldToRobot(
  kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, Rotation2d.fromDegrees(-target.getYaw()), gyro.getRotation2d(), targetPose, cameraToRobot);

// Calculate robot's field relative pose
frc::Pose2D robotPose = photonlib::EstimateFieldToRobot(
  kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot);