Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial AprilTag support (#458)

Browse Source 
(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver.

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
Co-authored-by: Chris <chrisgerth010592@gmail.com>
Co-authored-by: mdurrani808 <mdurrani808@gmail.com>
This commit is contained in:
shueja-personal
2022-09-28 18:21:41 -07:00
committed by GitHub
parent a3bcd3ac4f
commit a764ace7f2
114 changed files with 21488 additions and 10851 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -33,13 +33,14 @@
namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose,
double yaw, double pitch, double area, double skew, int id,
const frc::Transform3d& pose,
const wpi::SmallVector<std::pair<double, double>, 4> corners)
: yaw(yaw),
pitch(pitch),
area(area),
skew(skew),
fiducialId(id),
cameraToTarget(pose),
corners(corners) {}
@@ -55,9 +56,13 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().value()
<< target.fiducialId << target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Rotation().Degrees().value();
<< target.cameraToTarget.Translation().Z().value()
<< target.cameraToTarget.Rotation().GetQuaternion().W()
<< target.cameraToTarget.Rotation().GetQuaternion().X()
<< target.cameraToTarget.Rotation().GetQuaternion().Y()
<< target.cameraToTarget.Rotation().GetQuaternion().Z();
for (int i = 0; i < 4; i++) {
packet << target.corners[i].first << target.corners[i].second;
@@ -67,15 +72,19 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.yaw >> target.pitch >> target.area >> target.skew;
packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
target.fiducialId;
double x = 0;
double y = 0;
double rot = 0;
packet >> x >> y >> rot;
double z = 0;
double w = 0;
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.cameraToTarget =
frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
units::degree_t(rot));
target.cameraToTarget = frc::Transform3d(translation, rotation);
target.corners.clear();
for (int i = 0; i < 4; i++) {

18
photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
View File

@@ -34,13 +34,11 @@ namespace photonlib {
SimVisionSystem::SimVisionSystem(const std::string& name,
units::degree_t camDiagFOV,
units::degree_t camPitch,
frc::Transform2d cameraToRobot,
units::meter_t cameraHeightOffGround,
units::meter_t maxLEDRange, int cameraResWidth,
int cameraResHeight, double minTargetArea)
: camPitch(camPitch),
cameraToRobot(cameraToRobot),
: cameraToRobot(cameraToRobot),
cameraHeightOffGround(cameraHeightOffGround),
maxLEDRange(maxLEDRange),
cameraResWidth(cameraResWidth),
@@ -59,11 +57,9 @@ void SimVisionSystem::AddSimVisionTarget(SimVisionTarget tgt) {
}
void SimVisionSystem::MoveCamera(frc::Transform2d newCameraToRobot,
units::meter_t newCamHeight,
units::degree_t newCamPitch) {
units::meter_t newCamHeight) {
cameraToRobot = newCameraToRobot;
cameraHeightOffGround = newCamHeight;
camPitch = newCamPitch;
}
void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
@@ -89,11 +85,17 @@ void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
units::degree_t yawAngle = -units::math::atan2(
camToTargetTrans.Translation().Y(), camToTargetTrans.Translation().X());
units::degree_t pitchAngle =
units::math::atan2(distVertical, distAlongGround) - camPitch;
units::math::atan2(distVertical, distAlongGround);
auto translation = frc::Translation3d(camToTargetTrans.Translation().X(),
camToTargetTrans.Translation().Y(),
units::meter_t(0)); // TODO z height
auto rotation = frc::Rotation3d(units::radian_t(0), pitchAngle, -yawAngle);
frc::Transform3d camToTarget3d{translation, rotation};
if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
PhotonTrackedTarget newTgt = PhotonTrackedTarget(
yawAngle.value(), pitchAngle.value(), area, 0.0, camToTargetTrans,
yawAngle.value(), pitchAngle.value(), area, 0.0, -1, camToTarget3d,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
visibleTgtList.push_back(newTgt);
}