Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
chen perach
2022-05-06 18:36:58 +03:00
committed by GitHub
parent b1abf455c1
commit ef7ed21a9d
5 changed files with 56 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
View File

@@ -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),

23
wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
View File

@@ -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);