diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java new file mode 100644 index 0000000000..6829f20a0b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java @@ -0,0 +1,138 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; + +public final class ComputerVisionUtil { + private ComputerVisionUtil() { + throw new AssertionError("utility class"); + } + + /** + * 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 {@link edu.wpi.first.math.util.Units} class. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param 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). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @return The estimated distance to the target in meters. + */ + public static double calculateDistanceToTarget( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians) { + return (targetHeightMeters - cameraHeightMeters) + / Math.tan(cameraPitchRadians + targetPitchRadians); + } + + /** + * Estimate the position of the robot in the field. + * + * @param cameraHeightMeters The physical height of the camera off the floor in meters. + * @param 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). + * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians. + * Positive values up. + * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive + * values up. + * @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and + * Photon returns CW-positive. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param 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). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + double cameraHeightMeters, + double targetHeightMeters, + double cameraPitchRadians, + double targetPitchRadians, + Rotation2d targetYaw, + Rotation2d gyroAngle, + Pose2d fieldToTarget, + Transform2d cameraToRobot) { + return estimateFieldToRobot( + estimateCameraToTarget( + new Translation2d( + calculateDistanceToTarget( + cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + 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. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param 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). + * @return The position of the robot in the field. + */ + public static Pose2d estimateFieldToRobot( + Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) { + return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot); + } + + /** + * Estimates a {@link 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. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target + * relative to the camera. + * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @return A Transform2d that takes us from the camera to the target. + */ + public static Transform2d estimateCameraToTarget( + Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) { + // Map our camera at the origin out to our target, in the robot reference + // frame. Gyro angle is needed because there's a circle of possible camera + // poses for which the camera has the same yaw from camera to target. + return new Transform2d( + cameraToTargetTranslation, gyroAngle.unaryMinus().minus(fieldToTarget.getRotation())); + } + + /** + * 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. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ + public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) { + var targetToCamera = cameraToTarget.inverse(); + return fieldToTarget.transformBy(targetToCamera); + } +} diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp new file mode 100644 index 0000000000..7279e19970 --- /dev/null +++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/ComputerVisionUtil.h" + +#include "units/math.h" + +namespace frc { + +units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, + units::meter_t targetHeight, + units::radian_t cameraPitch, + units::radian_t targetPitch) { + return (targetHeight - cameraHeight) / + units::math::tan(cameraPitch + targetPitch); +} + +frc::Pose2d EstimateFieldToRobot( + units::meter_t cameraHeight, units::meter_t targetHeight, + units::radian_t cameraPitch, units::radian_t targetPitch, + const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, + const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) { + return EstimateFieldToRobot( + EstimateCameraToTarget(frc::Translation2d{CalculateDistanceToTarget( + cameraHeight, targetHeight, + cameraPitch, targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); +} + +frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget, + const frc::Transform2d& cameraToRobot) { + return EstimateFieldToCamera(cameraToTarget, fieldToTarget) + .TransformBy(cameraToRobot); +} + +frc::Transform2d EstimateCameraToTarget( + const frc::Translation2d& cameraToTargetTranslation, + const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) { + // Map our camera at the origin out to our target, in the robot reference + // frame. Gyro angle is needed because there's a circle of possible camera + // poses for which the camera has the same yaw from camera to target. + return frc::Transform2d{cameraToTargetTranslation, + -gyroAngle - fieldToTarget.Rotation()}; +} + +frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget) { + auto targetToCamera = cameraToTarget.Inverse(); + return fieldToTarget.TransformBy(targetToCamera); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h new file mode 100644 index 0000000000..da73c06d47 --- /dev/null +++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "frc/geometry/Translation2d.h" +#include "units/angle.h" +#include "units/length.h" + +namespace frc { + +/** + * 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. + * + * @param cameraHeight The height of the camera off the floor. + * @param targetHeight The height of the target off the floor. + * @param cameraPitch The pitch of the camera from the horizontal plane. + * Positive values up. + * @param targetPitch The pitch of the target in the camera's lens. Positive + * values up. + * @return The estimated distance to the target. + */ +WPILIB_DLLEXPORT +units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, + units::meter_t targetHeight, + units::radian_t cameraPitch, + units::radian_t targetPitch); + +/** + * Estimate the position of the robot in the field. + * + * @param cameraHeight The physical height of the camera off the floor. + * @param targetHeight The physical height of the target off the floor. 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). + * @param cameraPitch The pitch of the camera from the horizontal plane. + * Positive values up. + * @param targetPitch The pitch of the target in the camera's lens. Positive + * values up. + * @param targetYaw The observed yaw of the target. Note that this *must* be + * CCW-positive, and Photon returns CW-positive. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @param fieldToTarget A Pose2d representing the target position in the field + * coordinate system. + * @param 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 + * frc::Transform2d(3_in, 0_in, 0_deg). + * @return The position of the robot in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToRobot( + units::meter_t cameraHeight, units::meter_t targetHeight, + units::radian_t cameraPitch, units::radian_t targetPitch, + const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, + const frc::Pose2d& fieldToTarget, const frc::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. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @param 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 + * frc::Transform2d(3_in, 0_in, 0_deg). + * @return The position of the robot in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget, + const frc::Transform2d& cameraToRobot); + +/** + * 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. + * + * @param cameraToTargetTranslation A Translation2d that encodes the x/y + * position of the target relative to the + * camera. + * @param fieldToTarget A Pose2d representing the target position in the field + * coordinate system. + * @param gyroAngle The current robot gyro angle, likely from odometry. + * @return A Transform2d that takes us from the camera to the target. + */ +WPILIB_DLLEXPORT +frc::Transform2d EstimateCameraToTarget( + const frc::Translation2d& cameraToTargetTranslation, + const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle); + +/** + * 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. + * + * @param cameraToTarget The position of the target relative to the camera. + * @param fieldToTarget The position of the target in the field. + * @return The position of the camera in the field. + */ +WPILIB_DLLEXPORT +frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, + const frc::Pose2d& fieldToTarget); + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java new file mode 100644 index 0000000000..2e3c92a398 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +class ComputerVisionUtilTest { + @Test + void testCalculateDistanceToTarget() { + var camHeight = 1; + var targetHeight = 3; + var camPitch = Units.degreesToRadians(0); + var targetPitch = Units.degreesToRadians(30); + + var dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + + Assertions.assertEquals(3.464, dist, 0.01); + + camHeight = 1; + targetHeight = 2; + camPitch = Units.degreesToRadians(20); + targetPitch = Units.degreesToRadians(-10); + + dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + Assertions.assertEquals(5.671, dist, 0.01); + + camHeight = 3; + targetHeight = 1; + camPitch = Units.degreesToRadians(0); + targetPitch = Units.degreesToRadians(-30); + + dist = + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch); + + Assertions.assertEquals(3.464, dist, 0.01); + } + + @Test + void testEstimateFieldToRobot() { + var camHeight = 1; + var targetHeight = 3; + var camPitch = 0; + var targetPitch = Units.degreesToRadians(30); + var targetYaw = new Rotation2d(); + var gyroAngle = new Rotation2d(); + var fieldToTarget = new Pose2d(); + var cameraToRobot = new Transform2d(); + + var fieldToRobot = + ComputerVisionUtil.estimateFieldToRobot( + ComputerVisionUtil.estimateCameraToTarget( + new Translation2d( + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + + Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getY(), 0.1); + Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1); + + gyroAngle = Rotation2d.fromDegrees(-30); + + fieldToRobot = + ComputerVisionUtil.estimateFieldToRobot( + ComputerVisionUtil.estimateCameraToTarget( + new Translation2d( + ComputerVisionUtil.calculateDistanceToTarget( + camHeight, targetHeight, camPitch, targetPitch), + targetYaw), + fieldToTarget, + gyroAngle), + fieldToTarget, + cameraToRobot); + + Assertions.assertEquals(-3.0, fieldToRobot.getX(), 0.1); + Assertions.assertEquals(1.732, fieldToRobot.getY(), 0.1); + Assertions.assertEquals(-30.0, fieldToRobot.getRotation().getDegrees(), 0.1); + } +} diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp new file mode 100644 index 0000000000..6b84ce438b --- /dev/null +++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp @@ -0,0 +1,81 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/ComputerVisionUtil.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Transform2d.h" +#include "gtest/gtest.h" +#include "units/angle.h" +#include "units/length.h" + +TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) { + auto camHeight = 1_m; + auto targetHeight = 3_m; + auto camPitch = 0_deg; + auto targetPitch = 30_deg; + + auto dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + + EXPECT_NEAR(3.464, dist.value(), 0.01); + + camHeight = 1_m; + targetHeight = 2_m; + camPitch = 20_deg; + targetPitch = -10_deg; + + dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + EXPECT_NEAR(5.671, dist.value(), 0.01); + + camHeight = 3_m; + targetHeight = 1_m; + camPitch = 0_deg; + targetPitch = -30_deg; + + dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch); + + EXPECT_NEAR(3.464, dist.value(), 0.01); +} + +TEST(ComputerVisionUtilTest, EstimateFieldToRobot) { + auto camHeight = 1_m; + auto targetHeight = 3_m; + auto camPitch = 0_deg; + auto targetPitch = 30_deg; + frc::Rotation2d targetYaw; + frc::Rotation2d gyroAngle; + frc::Pose2d fieldToTarget; + frc::Transform2d cameraToRobot; + + auto fieldToRobot = frc::EstimateFieldToRobot( + frc::EstimateCameraToTarget( + frc::Translation2d{ + frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); + + EXPECT_NEAR(-3.464, fieldToRobot.X().value(), 0.1); + EXPECT_NEAR(0, fieldToRobot.Y().value(), 0.1); + EXPECT_NEAR(0, fieldToRobot.Rotation().Degrees().value(), 0.1); + + gyroAngle = -30_deg; + + fieldToRobot = frc::EstimateFieldToRobot( + frc::EstimateCameraToTarget( + frc::Translation2d{ + frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, + targetPitch), + targetYaw}, + fieldToTarget, gyroAngle), + fieldToTarget, cameraToRobot); + + EXPECT_NEAR(-3.0, fieldToRobot.X().value(), 0.1); + EXPECT_NEAR(1.732, fieldToRobot.Y().value(), 0.1); + EXPECT_NEAR(-30.0, fieldToRobot.Rotation().Degrees().value(), 0.1); +}