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

[wpimath] Improve accuracy of ComputerVisionUtil.calculateDistanceToTarget() (#4215)

Browse Source
This commit is contained in:
chen perach
2022-05-06 18:36:58 +03:00
committed by GitHub
parent b1abf455c1
commit ef7ed21a9d
5 changed files with 56 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
View File

@@ -32,15 +32,17 @@ public final class ComputerVisionUtil {
* Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
* values up.
* @param targetYawRadians The yaw of the target in the camera's lens in radians.
* @return The estimated distance to the target in meters.
*/
public static double calculateDistanceToTarget(
double cameraHeightMeters,
double targetHeightMeters,
double cameraPitchRadians,
double targetPitchRadians) {
double targetPitchRadians,
double targetYawRadians) {
return (targetHeightMeters - cameraHeightMeters)
/ Math.tan(cameraPitchRadians + targetPitchRadians);
/ (Math.tan(cameraPitchRadians + targetPitchRadians) * Math.cos(targetYawRadians));
}
/**
@@ -76,7 +78,11 @@ public final class ComputerVisionUtil {
estimateCameraToTarget(
new Translation2d(
calculateDistanceToTarget(
cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians),
cameraHeightMeters,
targetHeightMeters,
cameraPitchRadians,
targetPitchRadians,
targetYaw.getRadians()),
targetYaw),
fieldToTarget,
gyroAngle),

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

@@ -11,9 +11,11 @@ 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 targetPitch,
units::radian_t targetYaw) {
return (targetHeight - cameraHeight) /
units::math::tan(cameraPitch + targetPitch);
(units::math::tan(cameraPitch + targetPitch) *
units::math::cos(targetYaw));
}
frc::Pose2d EstimateFieldToRobot(
@@ -22,11 +24,12 @@ frc::Pose2d EstimateFieldToRobot(
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
return EstimateFieldToRobot(
EstimateCameraToTarget(frc::Translation2d{CalculateDistanceToTarget(
cameraHeight, targetHeight,
cameraPitch, targetPitch),
targetYaw},
fieldToTarget, gyroAngle),
EstimateCameraToTarget(
frc::Translation2d{
CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch,
targetPitch, targetYaw.Radians()),
targetYaw},
fieldToTarget, gyroAngle),
fieldToTarget, cameraToRobot);
}

4
wpimath/src/main/native/include/frc/ComputerVisionUtil.h
View File

@@ -28,13 +28,15 @@ namespace frc {
* Positive values up.
* @param targetPitch The pitch of the target in the camera's lens. Positive
* values up.
* @param targetYaw The yaw of the target in the camera's lens.
* @return The estimated distance to the target.
*/
WPILIB_DLLEXPORT
units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch);
units::radian_t targetPitch,
units::radian_t targetYaw);
/**
* Estimate the position of the robot in the field.