Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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);
}
}