001/*
002 * MIT License
003 *
004 * Copyright (c) PhotonVision
005 *
006 * Permission is hereby granted, free of charge, to any person obtaining a copy
007 * of this software and associated documentation files (the "Software"), to deal
008 * in the Software without restriction, including without limitation the rights
009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
010 * copies of the Software, and to permit persons to whom the Software is
011 * furnished to do so, subject to the following conditions:
012 *
013 * The above copyright notice and this permission notice shall be included in all
014 * copies or substantial portions of the Software.
015 *
016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
022 * SOFTWARE.
023 */
024
025package org.photonvision;
026
027import edu.wpi.first.math.geometry.*;
028
029public final class PhotonUtils {
030    private PhotonUtils() {
031        // Utility class
032    }
033
034    /**
035     * Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates
036     * range to a target using the target's elevation. This method can produce more stable results
037     * than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method
038     * requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and
039     * for there to exist a height differential between goal and camera. The larger this differential,
040     * the more accurate the distance estimate will be.
041     *
042     * <p>Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
043     *
044     * @param cameraHeightMeters The physical height of the camera off the floor in meters.
045     * @param targetHeightMeters The physical height of the target off the floor in meters. This
046     *     should be the height of whatever is being targeted (i.e. if the targeting region is set to
047     *     top, this should be the height of the top of the target).
048     * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
049     *     Positive values up.
050     * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
051     *     values up.
052     * @return The estimated distance to the target in meters.
053     */
054    public static double calculateDistanceToTargetMeters(
055            double cameraHeightMeters,
056            double targetHeightMeters,
057            double cameraPitchRadians,
058            double targetPitchRadians) {
059        return (targetHeightMeters - cameraHeightMeters)
060                / Math.tan(cameraPitchRadians + targetPitchRadians);
061    }
062
063    /**
064     * Estimate the {@link Translation2d} of the target relative to the camera.
065     *
066     * @param targetDistanceMeters The distance to the target in meters.
067     * @param yaw The observed yaw of the target.
068     * @return The target's camera-relative translation.
069     */
070    public static Translation2d estimateCameraToTargetTranslation(
071            double targetDistanceMeters, Rotation2d yaw) {
072        return new Translation2d(
073                yaw.getCos() * targetDistanceMeters, yaw.getSin() * targetDistanceMeters);
074    }
075
076    /**
077     * Estimate the position of the robot in the field.
078     *
079     * @param cameraHeightMeters The physical height of the camera off the floor in meters.
080     * @param targetHeightMeters The physical height of the target off the floor in meters. This
081     *     should be the height of whatever is being targeted (i.e. if the targeting region is set to
082     *     top, this should be the height of the top of the target).
083     * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
084     *     Positive values up.
085     * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
086     *     values up.
087     * @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and
088     *     Photon returns CW-positive.
089     * @param gyroAngle The current robot gyro angle, likely from odometry.
090     * @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
091     * @param cameraToRobot The position of the robot relative to the camera. If the camera was
092     *     mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
093     *     Transform2d(3 inches, 0 inches, 0 degrees).
094     * @return The position of the robot in the field.
095     */
096    public static Pose2d estimateFieldToRobot(
097            double cameraHeightMeters,
098            double targetHeightMeters,
099            double cameraPitchRadians,
100            double targetPitchRadians,
101            Rotation2d targetYaw,
102            Rotation2d gyroAngle,
103            Pose2d fieldToTarget,
104            Transform2d cameraToRobot) {
105        return PhotonUtils.estimateFieldToRobot(
106                PhotonUtils.estimateCameraToTarget(
107                        PhotonUtils.estimateCameraToTargetTranslation(
108                                PhotonUtils.calculateDistanceToTargetMeters(
109                                        cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians),
110                                targetYaw),
111                        fieldToTarget,
112                        gyroAngle),
113                fieldToTarget,
114                cameraToRobot);
115    }
116
117    /**
118     * Estimates a {@link Transform2d} that maps the camera position to the target position, using the
119     * robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system
120     * -- that is, it should read zero degrees when pointed towards the opposing alliance station, and
121     * increase as the robot rotates CCW.
122     *
123     * @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target
124     *     relative to the camera.
125     * @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
126     * @param gyroAngle The current robot gyro angle, likely from odometry.
127     * @return A Transform2d that takes us from the camera to the target.
128     */
129    public static Transform2d estimateCameraToTarget(
130            Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) {
131        // This pose maps our camera at the origin out to our target, in the robot
132        // reference frame
133        // The translation part of this Transform2d is from the above step, and the
134        // rotation uses our robot's
135        // gyro.
136        return new Transform2d(
137                cameraToTargetTranslation, gyroAngle.times(-1).minus(fieldToTarget.getRotation()));
138    }
139
140    /**
141     * Estimates the pose of the robot in the field coordinate system, given the position of the
142     * target relative to the camera, the target relative to the field, and the robot relative to the
143     * camera.
144     *
145     * @param cameraToTarget The position of the target relative to the camera.
146     * @param fieldToTarget The position of the target in the field.
147     * @param cameraToRobot The position of the robot relative to the camera. If the camera was
148     *     mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
149     *     Transform2d(3 inches, 0 inches, 0 degrees).
150     * @return The position of the robot in the field.
151     */
152    public static Pose2d estimateFieldToRobot(
153            Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) {
154        return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot);
155    }
156
157    /**
158     * Estimates the pose of the camera in the field coordinate system, given the position of the
159     * target relative to the camera, and the target relative to the field. This *only* tracks the
160     * position of the camera, not the position of the robot itself.
161     *
162     * @param cameraToTarget The position of the target relative to the camera.
163     * @param fieldToTarget The position of the target in the field.
164     * @return The position of the camera in the field.
165     */
166    public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) {
167        var targetToCamera = cameraToTarget.inverse();
168        return fieldToTarget.transformBy(targetToCamera);
169    }
170
171    /**
172     * Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial
173     * tag, the robot relative to the camera, and the target relative to the camera.
174     *
175     * @param cameraToRobot Transform3D of the robot relative to the camera. Origin of the robot is
176     *     defined as the center.
177     * @param fieldRelativeTagPose Pose3D the field relative pose of the target
178     * @param cameraToTarget Transform3D of the target relative to the camera, returned by
179     *     PhotonVision
180     * @return Transform3d Robot position relative to the field
181     */
182    public static Pose3d estimateFieldToRobotAprilTag(
183            Transform3d cameraToTarget, Pose3d fieldRelativeTagPose, Transform3d cameraToRobot) {
184        return fieldRelativeTagPose.plus(cameraToTarget.inverse()).plus(cameraToRobot);
185    }
186
187    /**
188     * Returns the yaw between your robot and a target.
189     *
190     * @param robotPose Current pose of the robot
191     * @param targetPose Pose of the target on the field
192     * @return double Yaw to the target
193     */
194    public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
195        Translation2d relativeTrl = targetPose.relativeTo(robotPose).getTranslation();
196        return new Rotation2d(relativeTrl.getX(), relativeTrl.getY());
197    }
198
199    /**
200     * Returns the distance between two poses
201     *
202     * @param robotPose Pose of the robot
203     * @param targetPose Pose of the target
204     * @return distance to the pose
205     */
206    public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
207        return robotPose.getTranslation().getDistance(targetPose.getTranslation());
208    }
209}