From 3b85b4f9cc351880042aefce176036ed017c24df Mon Sep 17 00:00:00 2001
From: Tylr-J42 <pi@PilerJac.home>
Date: Wed, 22 Nov 2023 01:42:56 -0500
Subject: [PATCH] Field position localization works WOOOO

---
 .../Apriltag_Pose_Localization.py             |   3 ++-
 FRC_Fiducial_Tracking/PNP_Pose_Estimation.py  |   1 -
 FRC_Fiducial_Tracking/WTFTEST.py              |   2 +-
 .../PNP_Pose_Estimation.cpython-39.pyc        | Bin 1284 -> 1257 bytes
 4 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
index 80b9d73..b4e5e59 100644
--- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
+++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
@@ -159,7 +159,8 @@ while True:
                 imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, camera_matrix, dist)
                 image = display_features(image, imgpts)
 
-    pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected)
+    if(len(tags_detected) > 0):
+        pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected)
 
     #Showing image. use --display to show image
     if args.display:
diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
index 4c4666e..0433ec6 100644
--- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
@@ -21,7 +21,6 @@ class PNPPose:
             else: 
                 PNP_obj_input = np.stack([PNP_obj_input, np.array([tag_corners[tags_detected[i]][1], tag_corners[tags_detected[i]][2], tag_corners[tags_detected[i]][3], tag_corners[tags_detected[i]][4]])])
 
-        image_corners = image_corners.astype("float")
 
         print("PNP_obj_input: ", PNP_obj_input, ", image_corners: ", image_corners, "tags_detected: ", tags_detected)
         ret, rvecs, tvecs = cv2.solvePnP(PNP_obj_input, image_corners, self.camera_matrix, self.dist, flags=0)
diff --git a/FRC_Fiducial_Tracking/WTFTEST.py b/FRC_Fiducial_Tracking/WTFTEST.py
index 588476c..5861637 100644
--- a/FRC_Fiducial_Tracking/WTFTEST.py
+++ b/FRC_Fiducial_Tracking/WTFTEST.py
@@ -13,7 +13,7 @@ dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e
 
 # 2023 Field Apriltag Coordinates index = tag id
 # format = [id, x, y, z, z-rotation] in inches
-tag_coords = [[0, 69.0, 420.0, 69.0, 0.0], [1, 610.77, 42.19, 18.22, 180], [2, 610.77, 108.19, 18.22, 180], [3, 610.77, 174.19, 18.22, 180],
+tag_coords = [[0, 0.0, 0.0, 0.0, 0.0], [1, 610.77, 42.19, 18.22, 180], [2, 610.77, 108.19, 18.22, 180], [3, 610.77, 174.19, 18.22, 180],
 [4, 636.96, 265.74, 27.38, 180], [5, 14.25, 265.74, 27.38, 0], [6, 40.45, 174.19, 18.22, 0], [7, 40.45, 108.19, 18.22, 0],
 [8, 40.45, 42.19, 18.22, 0]]
 
diff --git a/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc
index cfe240cffb486958a6a4530370077dccb3500585..375a50631e22a6f29c0d1bd2ce7828e4a5c3fcc8 100644
GIT binary patch
delta 159
zcmZqSddbO~$ji&c00b-xVpET9<bBS_^owEgH^zMqCG06|H7qIYHH;}7HB8NHj0`0l
zS)4U2HH<Y3SzJKOox+&H*~=QkT+3F=UdvI!Uc-^X)y%}mkirnmpvgV?ACuH%d1hBe
zp2?}qT9c<UPZ1IVS;WZ3$i*bU$i?Ww<i|YOp2eF{esVX9Ij=ZS?iOoaX>LJfk-+5h
GET#ZXU?rOX

delta 217
zcmaFK*}}z}$ji&c00fWTM5q4V$org;iG^|UH^zP0HH-_`Q`i<V)-slGq_Eepq;S+Q
zrf}9UHM21?lyGKo)v(ks)-Yso12InuV+vO<YYcNOTP=GnM+rv_M+$c{6C*<kLokCT
zk6#fZ(1Mp>f(gW81`#Yk;udRKPJUv^WIkqBM&8LG%v$VhiNz(A1*wy3nWu_~097zB
zvN3Wo2{3Xox-j`MaWPF+Vew{EoLs<SE+h#yAg?sHpt491$Sz_63X4y^z#<6%=YTR}