[wpimath] Improve accuracy of ComputerVisionUtil.calculateDistanceToTarget() (#4215)

wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java

@@ -32,15 +32,17 @@ public final class ComputerVisionUtil {
    *     Positive values up.
    * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
    *     values up.
+   * @param targetYawRadians The yaw of the target in the camera's lens in radians.
    * @return The estimated distance to the target in meters.
    */
   public static double calculateDistanceToTarget(
       double cameraHeightMeters,
       double targetHeightMeters,
       double cameraPitchRadians,
-      double targetPitchRadians) {
+      double targetPitchRadians,
+      double targetYawRadians) {
     return (targetHeightMeters - cameraHeightMeters)
-        / Math.tan(cameraPitchRadians + targetPitchRadians);
+        / (Math.tan(cameraPitchRadians + targetPitchRadians) * Math.cos(targetYawRadians));
   }
 
   /**
@@ -76,7 +78,11 @@ public final class ComputerVisionUtil {
         estimateCameraToTarget(
             new Translation2d(
                 calculateDistanceToTarget(
-                    cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians),
+                    cameraHeightMeters,
+                    targetHeightMeters,
+                    cameraPitchRadians,
+                    targetPitchRadians,
+                    targetYaw.getRadians()),
                 targetYaw),
             fieldToTarget,
             gyroAngle),

wpimath/src/main/native/cpp/ComputerVisionUtil.cpp

@@ -11,9 +11,11 @@ namespace frc {
 units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
                                          units::meter_t targetHeight,
                                          units::radian_t cameraPitch,
-                                         units::radian_t targetPitch) {
+                                         units::radian_t targetPitch,
+                                         units::radian_t targetYaw) {
   return (targetHeight - cameraHeight) /
-         units::math::tan(cameraPitch + targetPitch);
+         (units::math::tan(cameraPitch + targetPitch) *
+          units::math::cos(targetYaw));
 }
 
 frc::Pose2d EstimateFieldToRobot(
@@ -22,11 +24,12 @@ frc::Pose2d EstimateFieldToRobot(
     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),
+      EstimateCameraToTarget(
+          frc::Translation2d{
+              CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
+                                        targetPitch, targetYaw.Radians()),
+              targetYaw},
+          fieldToTarget, gyroAngle),
       fieldToTarget, cameraToRobot);
 }
 

wpimath/src/main/native/include/frc/ComputerVisionUtil.h

@@ -28,13 +28,15 @@ namespace frc {
  *                    Positive values up.
  * @param targetPitch The pitch of the target in the camera's lens. Positive
  *                    values up.
+ * @param targetYaw The yaw of the target in the camera's lens.
  * @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);
+                                         units::radian_t targetPitch,
+                                         units::radian_t targetYaw);
 
 /**
  * Estimate the position of the robot in the field.

wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java

@@ -19,10 +19,10 @@ class ComputerVisionUtilTest {
     var targetHeight = 3;
     var camPitch = Units.degreesToRadians(0);
     var targetPitch = Units.degreesToRadians(30);
-
+    var targetYaw = Units.degreesToRadians(0);
     var dist =
         ComputerVisionUtil.calculateDistanceToTarget(
-            camHeight, targetHeight, camPitch, targetPitch);
+            camHeight, targetHeight, camPitch, targetPitch, targetYaw);
 
     Assertions.assertEquals(3.464, dist, 0.01);
 
@@ -33,7 +33,7 @@ class ComputerVisionUtilTest {
 
     dist =
         ComputerVisionUtil.calculateDistanceToTarget(
-            camHeight, targetHeight, camPitch, targetPitch);
+            camHeight, targetHeight, camPitch, targetPitch, targetYaw);
     Assertions.assertEquals(5.671, dist, 0.01);
 
     camHeight = 3;
@@ -43,9 +43,21 @@ class ComputerVisionUtilTest {
 
     dist =
         ComputerVisionUtil.calculateDistanceToTarget(
-            camHeight, targetHeight, camPitch, targetPitch);
+            camHeight, targetHeight, camPitch, targetPitch, targetYaw);
 
     Assertions.assertEquals(3.464, dist, 0.01);
+
+    camHeight = 1;
+    targetHeight = 3;
+    camPitch = Units.degreesToRadians(0);
+    targetPitch = Units.degreesToRadians(30);
+    targetYaw = Units.degreesToRadians(30);
+
+    dist =
+        ComputerVisionUtil.calculateDistanceToTarget(
+            camHeight, targetHeight, camPitch, targetPitch, targetYaw);
+
+    Assertions.assertEquals(4, dist, 0.01);
   }
 
   @Test
@@ -64,7 +76,7 @@ class ComputerVisionUtilTest {
             ComputerVisionUtil.estimateCameraToTarget(
                 new Translation2d(
                     ComputerVisionUtil.calculateDistanceToTarget(
-                        camHeight, targetHeight, camPitch, targetPitch),
+                        camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
                     targetYaw),
                 fieldToTarget,
                 gyroAngle),
@@ -82,7 +94,7 @@ class ComputerVisionUtilTest {
             ComputerVisionUtil.estimateCameraToTarget(
                 new Translation2d(
                     ComputerVisionUtil.calculateDistanceToTarget(
-                        camHeight, targetHeight, camPitch, targetPitch),
+                        camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
                     targetYaw),
                 fieldToTarget,
                 gyroAngle),

wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp

@@ -15,10 +15,10 @@ TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
   auto targetHeight = 3_m;
   auto camPitch = 0_deg;
   auto targetPitch = 30_deg;
+  auto targetYaw = 0_deg;
 
   auto dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
-                                             targetPitch);
-
+                                             targetPitch, targetYaw);
   EXPECT_NEAR(3.464, dist.value(), 0.01);
 
   camHeight = 1_m;
@@ -27,7 +27,7 @@ TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
   targetPitch = -10_deg;
 
   dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
-                                        targetPitch);
+                                        targetPitch, targetYaw);
   EXPECT_NEAR(5.671, dist.value(), 0.01);
 
   camHeight = 3_m;
@@ -36,9 +36,18 @@ TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
   targetPitch = -30_deg;
 
   dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
-                                        targetPitch);
-
+                                        targetPitch, targetYaw);
   EXPECT_NEAR(3.464, dist.value(), 0.01);
+
+  camHeight = 1_m;
+  targetHeight = 3_m;
+  camPitch = 0_deg;
+  targetPitch = 30_deg;
+  targetYaw = 30_deg;
+
+  dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
+                                        targetPitch, targetYaw);
+  EXPECT_NEAR(4, dist.value(), 0.01);
 }
 
 TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
@@ -55,7 +64,7 @@ TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
       frc::EstimateCameraToTarget(
           frc::Translation2d{
               frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
-                                             targetPitch),
+                                             targetPitch, targetYaw.Radians()),
               targetYaw},
           fieldToTarget, gyroAngle),
       fieldToTarget, cameraToRobot);
@@ -70,7 +79,7 @@ TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
       frc::EstimateCameraToTarget(
           frc::Translation2d{
               frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
-                                             targetPitch),
+                                             targetPitch, targetYaw.Radians()),
               targetYaw},
           fieldToTarget, gyroAngle),
       fieldToTarget, cameraToRobot);