mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
65 lines
2.7 KiB
C++
65 lines
2.7 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#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);
|
|
}
|
|
|
|
} // namespace frc
|