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] 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

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