Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make ComputerVisionUtil use 3D geometry classes (#4528)

Browse Source 
Closes #4189.
This commit is contained in:
Tyler Veness
2022-10-26 22:20:08 -07:00
committed by GitHub
parent cfb84a6083
commit 8bc3b04f5b
5 changed files with 159 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,10 +4,11 @@
package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
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.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
@@ -15,69 +16,68 @@ import org.junit.jupiter.api.Test;
class ComputerVisionUtilTest {
@Test
void testCalculateDistanceToTarget() {
var camHeight = 1;
var cameraHeight = 1;
var targetHeight = 3;
var camPitch = Units.degreesToRadians(0);
var cameraPitch = Units.degreesToRadians(0);
var targetPitch = Units.degreesToRadians(30);
var targetYaw = Units.degreesToRadians(0);
var dist =
var distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
Assertions.assertEquals(3.464, dist, 0.01);
camHeight = 1;
cameraHeight = 1;
targetHeight = 2;
camPitch = Units.degreesToRadians(20);
cameraPitch = Units.degreesToRadians(20);
targetPitch = Units.degreesToRadians(-10);
dist =
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(5.671, dist, 0.01);
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(5.671, distanceAlongGround, 0.01);
camHeight = 3;
cameraHeight = 3;
targetHeight = 1;
camPitch = Units.degreesToRadians(0);
cameraPitch = Units.degreesToRadians(0);
targetPitch = Units.degreesToRadians(-30);
dist =
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
Assertions.assertEquals(3.464, dist, 0.01);
camHeight = 1;
cameraHeight = 1;
targetHeight = 3;
camPitch = Units.degreesToRadians(0);
cameraPitch = Units.degreesToRadians(0);
targetPitch = Units.degreesToRadians(30);
targetYaw = Units.degreesToRadians(30);
dist =
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw);
Assertions.assertEquals(4, dist, 0.01);
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(4, distanceAlongGround, 0.01);
}
@Test
void testEstimateFieldToRobot() {
var camHeight = 1;
var cameraHeight = 1;
var targetHeight = 3;
var camPitch = 0;
var cameraPitch = 0;
var targetPitch = Units.degreesToRadians(30);
var targetYaw = new Rotation2d();
var gyroAngle = new Rotation2d();
var fieldToTarget = new Pose2d();
var cameraToRobot = new Transform2d();
var fieldToTarget = new Pose3d();
var cameraToRobot = new Transform3d();
var distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw.getRadians());
var range = Math.hypot(distanceAlongGround, targetHeight - cameraHeight);
var fieldToRobot =
ComputerVisionUtil.estimateFieldToRobot(
ComputerVisionUtil.estimateCameraToTarget(
new Translation2d(
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
targetYaw),
new Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
fieldToTarget,
gyroAngle),
fieldToTarget,
@@ -85,17 +85,19 @@ class ComputerVisionUtilTest {
Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1);
Assertions.assertEquals(0, fieldToRobot.getY(), 0.1);
Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1);
Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1);
Assertions.assertEquals(0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1);
gyroAngle = Rotation2d.fromDegrees(-30);
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw.getRadians());
range = Math.hypot(distanceAlongGround, targetHeight - cameraHeight);
fieldToRobot =
ComputerVisionUtil.estimateFieldToRobot(
ComputerVisionUtil.estimateCameraToTarget(
new Translation2d(
ComputerVisionUtil.calculateDistanceToTarget(
camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()),
targetYaw),
new Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
fieldToTarget,
gyroAngle),
fieldToTarget,
@@ -103,6 +105,7 @@ class ComputerVisionUtilTest {
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);
Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1);
Assertions.assertEquals(-30.0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1);
}
}

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

@@ -11,80 +11,87 @@
#include "units/length.h"
TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
auto camHeight = 1_m;
auto cameraHeight = 1_m;
auto targetHeight = 3_m;
auto camPitch = 0_deg;
auto cameraPitch = 0_deg;
auto targetPitch = 30_deg;
auto targetYaw = 0_deg;
auto dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw);
EXPECT_NEAR(3.464, dist.value(), 0.01);
auto distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
camHeight = 1_m;
cameraHeight = 1_m;
targetHeight = 2_m;
camPitch = 20_deg;
cameraPitch = 20_deg;
targetPitch = -10_deg;
dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw);
EXPECT_NEAR(5.671, dist.value(), 0.01);
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(5.671, distanceAlongGround.value(), 0.01);
camHeight = 3_m;
cameraHeight = 3_m;
targetHeight = 1_m;
camPitch = 0_deg;
cameraPitch = 0_deg;
targetPitch = -30_deg;
dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw);
EXPECT_NEAR(3.464, dist.value(), 0.01);
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
camHeight = 1_m;
cameraHeight = 1_m;
targetHeight = 3_m;
camPitch = 0_deg;
cameraPitch = 0_deg;
targetPitch = 30_deg;
targetYaw = 30_deg;
dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw);
EXPECT_NEAR(4, dist.value(), 0.01);
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(4, distanceAlongGround.value(), 0.01);
}
TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
auto camHeight = 1_m;
auto cameraHeight = 1_m;
auto targetHeight = 3_m;
auto camPitch = 0_deg;
auto cameraPitch = 0_deg;
auto targetPitch = 30_deg;
frc::Rotation2d targetYaw;
frc::Rotation2d gyroAngle;
frc::Pose2d fieldToTarget;
frc::Transform2d cameraToRobot;
frc::Pose3d fieldToTarget;
frc::Transform3d cameraToRobot;
auto distanceAlongGround =
frc::CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
targetPitch, targetYaw.Radians());
auto range =
units::math::hypot(distanceAlongGround, targetHeight - cameraHeight);
auto fieldToRobot = frc::EstimateFieldToRobot(
frc::EstimateCameraToTarget(
frc::Translation2d{
frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw.Radians()),
targetYaw},
frc::Translation3d{
range, frc::Rotation3d{0_rad, targetPitch, targetYaw.Radians()}},
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);
EXPECT_NEAR(2.0, fieldToRobot.Z().value(), 0.1);
EXPECT_NEAR(0, fieldToRobot.Rotation().Z().value(), 0.1);
gyroAngle = -30_deg;
distanceAlongGround =
frc::CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
targetPitch, targetYaw.Radians());
range = units::math::hypot(distanceAlongGround, targetHeight - cameraHeight);
fieldToRobot = frc::EstimateFieldToRobot(
frc::EstimateCameraToTarget(
frc::Translation2d{
frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch,
targetPitch, targetYaw.Radians()),
targetYaw},
frc::Translation3d{
range, frc::Rotation3d{0_rad, targetPitch, targetYaw.Radians()}},
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);
EXPECT_NEAR(2.0, fieldToRobot.Z().value(), 0.1);
EXPECT_NEAR(-30.0, units::degree_t{fieldToRobot.Rotation().Z()}.value(), 0.1);
}