From ef7ed21a9d17142063baec84cfbe57cd1f811bf2 Mon Sep 17 00:00:00 2001 From: chen perach <45401724+chenPerach@users.noreply.github.com> Date: Fri, 6 May 2022 18:36:58 +0300 Subject: [PATCH] [wpimath] Improve accuracy of ComputerVisionUtil.calculateDistanceToTarget() (#4215) --- .../wpi/first/math/ComputerVisionUtil.java | 12 +++++++--- .../main/native/cpp/ComputerVisionUtil.cpp | 17 +++++++------ .../native/include/frc/ComputerVisionUtil.h | 4 +++- .../first/math/ComputerVisionUtilTest.java | 24 ++++++++++++++----- .../native/cpp/ComputerVisionUtilTest.cpp | 23 ++++++++++++------ 5 files changed, 56 insertions(+), 24 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java index 6829f20a0b..e1fe7ee89c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java +++ b/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), diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp index 7279e19970..079b11b9c2 100644 --- a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp +++ b/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); } diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h index da73c06d47..45b3860d5b 100644 --- a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h +++ b/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. diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java index 2e3c92a398..ea3f8d58eb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java +++ b/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), diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp index 6b84ce438b..c10baef785 100644 --- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp +++ b/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);