Package org.photonvision

Class PhotonUtils

java.lang.Object

org.photonvision.PhotonUtils

public final class PhotonUtils
extends Object

Method Summary

Modifier and Type	Method	Description
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.
static edu.wpi.first.math.geometry.Transform2d	estimateCameraToTarget(edu.wpi.first.math.geometry.Translation2d cameraToTargetTranslation, edu.wpi.first.math.geometry.Pose2d fieldToTarget, edu.wpi.first.math.geometry.Rotation2d gyroAngle)	Estimates a Transform2d that maps the camera position to the target position, using the robot's gyro.
static edu.wpi.first.math.geometry.Translation2d	estimateCameraToTargetTranslation(double targetDistanceMeters, edu.wpi.first.math.geometry.Rotation2d yaw)	Estimate the Translation2d of the target relative to the camera.
static edu.wpi.first.math.geometry.Pose2d	estimateFieldToCamera(edu.wpi.first.math.geometry.Transform2d cameraToTarget, edu.wpi.first.math.geometry.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.
static edu.wpi.first.math.geometry.Pose2d	estimateFieldToRobot(double cameraHeightMeters, double targetHeightMeters, double cameraPitchRadians, double targetPitchRadians, edu.wpi.first.math.geometry.Rotation2d targetYaw, edu.wpi.first.math.geometry.Rotation2d gyroAngle, edu.wpi.first.math.geometry.Pose2d fieldToTarget, edu.wpi.first.math.geometry.Transform2d cameraToRobot)	Estimate the position of the robot in the field.
static edu.wpi.first.math.geometry.Pose2d	estimateFieldToRobot(edu.wpi.first.math.geometry.Transform2d cameraToTarget, edu.wpi.first.math.geometry.Pose2d fieldToTarget, edu.wpi.first.math.geometry.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.
static edu.wpi.first.math.geometry.Pose3d	estimateFieldToRobotAprilTag(edu.wpi.first.math.geometry.Transform3d cameraToTarget, edu.wpi.first.math.geometry.Pose3d fieldRelativeTagPose, edu.wpi.first.math.geometry.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.
static double	getDistanceToPose(edu.wpi.first.math.geometry.Pose2d robotPose, edu.wpi.first.math.geometry.Pose2d targetPose)	Returns the distance between two poses
static edu.wpi.first.math.geometry.Rotation2d	getYawToPose(edu.wpi.first.math.geometry.Pose2d robotPose, edu.wpi.first.math.geometry.Pose2d targetPose)	Returns the yaw between your robot and a target.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

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 edu.wpi.first.math.geometry.Translation2d estimateCameraToTargetTranslation(double targetDistanceMeters,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Pose2d estimateFieldToRobot(double cameraHeightMeters,
 double targetHeightMeters,
 double cameraPitchRadians,
 double targetPitchRadians,
 edu.wpi.first.math.geometry.Rotation2d targetYaw,
 edu.wpi.first.math.geometry.Rotation2d gyroAngle,
 edu.wpi.first.math.geometry.Pose2d fieldToTarget,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Transform2d estimateCameraToTarget(edu.wpi.first.math.geometry.Translation2d cameraToTargetTranslation,
 edu.wpi.first.math.geometry.Pose2d fieldToTarget,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Pose2d estimateFieldToRobot(edu.wpi.first.math.geometry.Transform2d cameraToTarget,
 edu.wpi.first.math.geometry.Pose2d fieldToTarget,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Pose2d estimateFieldToCamera(edu.wpi.first.math.geometry.Transform2d cameraToTarget,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Pose3d estimateFieldToRobotAprilTag(edu.wpi.first.math.geometry.Transform3d cameraToTarget,
 edu.wpi.first.math.geometry.Pose3d fieldRelativeTagPose,
 edu.wpi.first.math.geometry.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 edu.wpi.first.math.geometry.Rotation2d getYawToPose(edu.wpi.first.math.geometry.Pose2d robotPose,
 edu.wpi.first.math.geometry.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(edu.wpi.first.math.geometry.Pose2d robotPose,
 edu.wpi.first.math.geometry.Pose2d targetPose)

Returns the distance between two poses

Parameters:

robotPose - Pose of the robot

targetPose - Pose of the target

Returns:

distance to the pose