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

[wpimath] Remove broken and obsoleted ComputerVisionUtil functions (#4775)

Browse Source 
The ComputerVisionUtil class was added before AprilTag support was
announced. Now that it has, the functions for estimating a pose from
range and yaw are no longer needed; it's just better to get the pose
directly from the AprilTag.

The coordinate system on some function arguments was confusing or didn't
match the NWU convention the rest of the library uses. It's easier to
remove the functions now and add them back after they're fixed since the
fixes aren't trivial.

The range function was removed because it uses pitch and yaw in the
camera's spherical coordinate system, which is obsoleted by AprilTags.
AprilTags give you a 6DOF pose directly, so range can be obtained via
Pose2d.getTranslation().getDistance().

Fixes #4757.
This commit is contained in:
Tyler Veness
2022-12-06 21:11:27 -08:00
committed by GitHub
parent 2d0faecf4f
commit f18fd41ac3
5 changed files with 66 additions and 466 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,108 +4,30 @@
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
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;
class ComputerVisionUtilTest {
@Test
void testCalculateDistanceToTarget() {
var cameraHeight = 1;
var targetHeight = 3;
var cameraPitch = Units.degreesToRadians(0);
var targetPitch = Units.degreesToRadians(30);
var targetYaw = Units.degreesToRadians(0);
void testObjectToRobotPose() {
var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
var cameraToObject =
new Transform3d(
new Translation3d(1.0, 1.0, 1.0),
new Rotation3d(0.0, Units.degreesToRadians(-20.0), Units.degreesToRadians(45.0)));
var robotToCamera =
new Transform3d(
new Translation3d(1.0, 0.0, 2.0),
new Rotation3d(0.0, 0.0, Units.degreesToRadians(25.0)));
Pose3d object = robot.plus(robotToCamera).plus(cameraToObject);
var distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
cameraHeight = 1;
targetHeight = 2;
cameraPitch = Units.degreesToRadians(20);
targetPitch = Units.degreesToRadians(-10);
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(5.671, distanceAlongGround, 0.01);
cameraHeight = 3;
targetHeight = 1;
cameraPitch = Units.degreesToRadians(0);
targetPitch = Units.degreesToRadians(-30);
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
cameraHeight = 1;
targetHeight = 3;
cameraPitch = Units.degreesToRadians(0);
targetPitch = Units.degreesToRadians(30);
targetYaw = Units.degreesToRadians(30);
distanceAlongGround =
ComputerVisionUtil.calculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
Assertions.assertEquals(4, distanceAlongGround, 0.01);
}
@Test
void testEstimateFieldToRobot() {
var cameraHeight = 1;
var targetHeight = 3;
var cameraPitch = 0;
var targetPitch = Units.degreesToRadians(30);
var targetYaw = new Rotation2d();
var gyroAngle = new Rotation2d();
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 Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
fieldToTarget,
gyroAngle),
fieldToTarget,
cameraToRobot);
Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1);
Assertions.assertEquals(0, fieldToRobot.getY(), 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 Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
fieldToTarget,
gyroAngle),
fieldToTarget,
cameraToRobot);
Assertions.assertEquals(-3.0, fieldToRobot.getX(), 0.1);
Assertions.assertEquals(1.732, fieldToRobot.getY(), 0.1);
Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1);
Assertions.assertEquals(-30.0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1);
assertEquals(
robot, ComputerVisionUtil.objectToRobotPose(object, cameraToObject, robotToCamera));
}
}

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

@@ -3,95 +3,16 @@
// the WPILib BSD license file in the root directory of this project.
#include "frc/ComputerVisionUtil.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform2d.h"
#include "gtest/gtest.h"
#include "units/angle.h"
#include "units/length.h"
TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
auto cameraHeight = 1_m;
auto targetHeight = 3_m;
auto cameraPitch = 0_deg;
auto targetPitch = 30_deg;
auto targetYaw = 0_deg;
TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
frc::Transform3d cameraToObject{frc::Translation3d{1_m, 1_m, 1_m},
frc::Rotation3d{0_deg, -20_deg, 45_deg}};
frc::Transform3d robotToCamera{frc::Translation3d{1_m, 0_m, 2_m},
frc::Rotation3d{0_deg, 0_deg, 25_deg}};
frc::Pose3d object = robot + robotToCamera + cameraToObject;
auto distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
cameraHeight = 1_m;
targetHeight = 2_m;
cameraPitch = 20_deg;
targetPitch = -10_deg;
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(5.671, distanceAlongGround.value(), 0.01);
cameraHeight = 3_m;
targetHeight = 1_m;
cameraPitch = 0_deg;
targetPitch = -30_deg;
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
cameraHeight = 1_m;
targetHeight = 3_m;
cameraPitch = 0_deg;
targetPitch = 30_deg;
targetYaw = 30_deg;
distanceAlongGround = frc::CalculateDistanceToTarget(
cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
EXPECT_NEAR(4, distanceAlongGround.value(), 0.01);
}
TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
auto cameraHeight = 1_m;
auto targetHeight = 3_m;
auto cameraPitch = 0_deg;
auto targetPitch = 30_deg;
frc::Rotation2d targetYaw;
frc::Rotation2d gyroAngle;
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::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(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::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(2.0, fieldToRobot.Z().value(), 0.1);
EXPECT_NEAR(-30.0, units::degree_t{fieldToRobot.Rotation().Z()}.value(), 0.1);
EXPECT_EQ(robot,
frc::ObjectToRobotPose(object, cameraToObject, robotToCamera));
}