Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

57
wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
View File

@@ -4,61 +4,16 @@
#include "frc/ComputerVisionUtil.h"
#include "frc/geometry/Rotation3d.h"
#include "units/math.h"
namespace frc {
units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch,
units::radian_t targetYaw) {
return (targetHeight - cameraHeight) /
(units::math::tan(cameraPitch + targetPitch) *
units::math::cos(targetYaw));
}
frc::Pose3d EstimateFieldToRobot(
units::meter_t cameraHeight, units::meter_t targetHeight,
units::radian_t cameraPitch, units::radian_t targetPitch,
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose3d& fieldToTarget, const frc::Transform3d& cameraToRobot) {
auto distanceAlongGround =
CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
targetPitch, targetYaw.Radians());
auto range =
units::math::hypot(distanceAlongGround, targetHeight - cameraHeight);
return EstimateFieldToRobot(
EstimateCameraToTarget(
Translation3d{range,
Rotation3d{0_rad, targetPitch, targetYaw.Radians()}},
fieldToTarget, gyroAngle),
fieldToTarget, cameraToRobot);
}
frc::Pose3d EstimateFieldToRobot(const frc::Transform3d& cameraToTarget,
const frc::Pose3d& fieldToTarget,
const frc::Transform3d& cameraToRobot) {
return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
.TransformBy(cameraToRobot);
}
frc::Transform3d EstimateCameraToTarget(
const frc::Translation3d& cameraToTargetTranslation,
const frc::Pose3d& fieldToTarget, const frc::Rotation2d& gyroAngle) {
// Map our camera at the origin out to our target, in the robot reference
// frame. Gyro angle is needed because there's a circle of possible camera
// poses for which the camera has the same yaw from camera to target.
return Transform3d{cameraToTargetTranslation,
Rotation3d{0_rad, 0_rad, -gyroAngle.Radians()} -
fieldToTarget.Rotation()};
}
frc::Pose3d EstimateFieldToCamera(const frc::Transform3d& cameraToTarget,
const frc::Pose3d& fieldToTarget) {
auto targetToCamera = cameraToTarget.Inverse();
return fieldToTarget.TransformBy(targetToCamera);
frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
const frc::Transform3d& cameraToObject,
const frc::Transform3d& robotToCamera) {
const auto objectToCamera = cameraToObject.Inverse();
const auto cameraToRobot = robotToCamera.Inverse();
return objectInField + objectToCamera + cameraToRobot;
}
} // namespace frc