From b82ef749508e283ee229b84b7aa3d9ea370016ac Mon Sep 17 00:00:00 2001
From: Tylr-J42 <Tyler.Jacques00@gmail.com>
Date: Mon, 19 Aug 2024 20:04:57 -0400
Subject: [PATCH] fixed up pose localization (needs further testing)

---
 .../Apriltag_Pose_Localization.py                |  15 ++++++++++++++-
 FRC_Fiducial_Tracking/PNP_Pose_Estimation.py     |   2 +-
 .../PNP_Pose_Estimation.cpython-311.pyc          | Bin 0 -> 4031 bytes
 3 files changed, 15 insertions(+), 2 deletions(-)
 create mode 100644 FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-311.pyc

diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
index ef87621..35191b4 100644
--- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
+++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
@@ -49,6 +49,12 @@ if args.high_res:
     2.29608398e+00]])
     camera_res = (1088, 720)
 '''
+
+data_array = []
+
+pose_coords = []
+z_line_offset = 0
+
 b=6.5
 # 3d object array. The points of the 3d april tag that coresponds to tag_points which we detect
 objp = np.array([[0,0,0], [-b/2, -b/2, 0], [b/2, -b/2, 0], [b/2, b/2, 0], [-b/2, b/2, 0]], dtype=np.float32)
@@ -101,7 +107,7 @@ testing_tag_coords = tag_corners(testing_tags)
 def getTagCoords(tag_id):
     return tag_coords[tag_id]
 
-cam = Picam2Vid(camera_res).start()
+cam = Picam2Vid(camera_res)
 
 def connectionListener(connected, info):
     print(info, "; Connected=%s" % connected)
@@ -145,6 +151,7 @@ counter = 0
 time.sleep(0.1)
 while True:
     frame_start = time.time()
+    cam.update()
     image = cam.read()
     image_corners = np.array([])
     tags_detected = []
@@ -173,6 +180,12 @@ while True:
     if(len(tags_detected) > 0):
         pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected)
 
+        vision_table.putNumber("global_pose_x", pose_coords[0])
+        vision_table.putNumber("global_pose_y", pose_coords[1])
+        vision_table.putNumber("global_pose_z", pose_coords[2])
+
+        vision_table.putNumberArray("visibleTags", tags_detected)
+    
     #Showing image. use --display to show image
     if args.display:
         image = cv2.putText(image, "FPS: "+str(round(FPS, 4)), (25,440), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 2, cv2.LINE_AA)
diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
index 5ae2562..8d1ba57 100644
--- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
@@ -54,4 +54,4 @@ class PNPPose:
             print("euler XYZ: ", euler_angles, "tvecs: ", tvecs, "total_dist: ", total_dist)
 
             world_angleXYZ = [-euler_angles[0], -euler_angles[1], -euler_angles[2]]
-            z_line_offset_coord = []
\ No newline at end of file
+            return world_angleXYZ
\ No newline at end of file
diff --git a/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-311.pyc b/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..6658856935bf109dc6acfb012018f25fe3c4c87c
GIT binary patch
literal 4031
zcmd5<T}&I<6}~f`!2`DWjT68S%q}K15wPS(UD9Z`5c1PBPzXtr5TfbCGZ+(Nn>%9?
zoUxjReegpbywXbXN~~m6G<B3z;$d6yP^n1Whdj<iBh?x;Qq(FBys^smWuJQPj4|V6
zljmOB=iYOF&N=s-bH982C!enYLHYN;{;}|94x#@LX0I@n#+y%}@d$}Xq~d7KuBkc7
z;^{dTjm{&H{sM`v$23Au;b)gQMx<s@nAsuhRHV{tRaAv}(+`bDNJ2!~94%5`pg9+j
z1#4YES&;!+C$b=?&U8;tO;0CfDZ)Br5p^#3y_vQ|sg#QN>afZHx2FuLF5s%>MPWOC
zAhXG8tEe<^dz*%Bf>L_wi3^h@`y|Yo9z}@qkt9w?ST-3kCM%{dA}mW-;Fkpj$L^V)
zB#uc5MNneNgxQ3Xx0Ae_5+V|x0{cM%BTMlG>?VRegwzuP{zo#|Tac6==Z6=Q%hIs2
z7MJkwge2cllBwZym(L82rEn|`CJYj13{FUj6d`)Z7=JD%rXw*S&R@Yo<W4LR9R{cI
zWFmi7R$^ct%p6LsnH<l@5;2A6GtCvZ4(;_2v*hDI9-wldKgSqcd-*_nj(J#L?j9g)
zXF1SU7%hz2Lp?*J?$WI98(4-SRVM>e2R|!7pFtoXiztzfqh%^fFVk7qvMb9_NFm#=
zl%;BLRq8RUcv=mgNOkN@LRnUXyn}#3hV1u>?0(yW-WqevtIg94XjJJ8#35T}NoC$z
z>Cjtikz=L1YLCMPr^S{%4oS|j_u9j$b*fwCqONScT2Icvl$D2Jmr18$CMzgH;+V+@
zaxAg)E%;`K_<e^s3{DFJv0lV2z~ELw+JKm@$m%h({&G^pv1nS7!wuL=x>>v`MdY_9
zQ<qH!C*yIGO~sVRqUl-_?wSoasqiTQ3(K-hpu@^li(nrS<=Y?U@oL7ums~a7jWARG
zI}pe)u3vlq_ocV#;bbACdxM5ISZp!8ALpoYQ)~WY{$!!IFt*-TSXn<(XfKWwXEsih
zsEwcNZAXo^qk7ZP(zxCPiCJ!L%TMGd3N3|^^#cWA{m_?_Urp-Gy~U68=Dr+TZfwbi
z@}d3V0$1!Uj&1Z6S2m95tpi5ufZjMzI;J-c=3HfeM<G;nX$Sgp4dtc|jqAWou!l$$
zK!73HYh`;18jp|-GFexpW|V3+BeYa)t8gavGgowF012Sak+F7P24vP|V3t+k{J=S2
z7HG&1b_$c@W`mWVv<?CZ)A$fn06HsJ3D;FZu8@$MUMUUm!V8ILT#`)=;3;Nd76<XN
z(W^m$R-7j&Clldz!0ljr0rhw#$uGpDxR^Px7i(uP(JWs80-<i}%8%wpb6mCY+TZcj
zjY7IOvwpAC^7Z}F)y<J-*Ed(5+|+wM)%~M}e-u2;HRrfjO@|8YT2rsa^}g$Kf}57l
zHK31>ik6_mF2wU$MsY+)rE5JXQg<xK&r|rhQpFyGKD4G*!UVeYmn>T|pQ)VzIxIxQ
zzkx{6sskY$QFmm(2WGkUSA1ZWBXd45%V9^9$ubW`xGw*1?IXDlHvUy1w^OAoS&<`*
z`j*Eb=Hy<jlqHh4R)HPDmA_P#0d0~!RuQRZb&nwfo>sG99`Ytzmvv{k_hyE>Hqx~m
zWJV8U20`9RS+ZVRgaKT4_`h}?89NPg9tKrZbnK%d#9CduB3oahg@C$Me!5rXoVO2f
zj@+xgi(t%=akXRF1|_hk2O-g|){7j>uU8u$gTJ3veGd3k@6-o3VZBqM#XGck5Zrej
z)iY~9!5Zv4&q6XxFl~pN<{c9HFlDj}aUm*aJQj=vuiuzE70kFTXoH4#AG%Hj!;RK^
z%XG!11YCH4c#=scQgDl6EUe*nkY{poGQKKJC#LrwWHxlVMJb9UNyb4kja^(zB^9_9
zaV(*Py*8lw;Bgg|s?S0nJkVrbv>s=YXsh;-w^;J_61==pX~m>srVC37?gW|mV?z1}
zIYLx4-c91TXyY>MN+uDtk=Kd8%@H9UNyi0+=uKi#&b04iaP3GC1juQCA;95-oyNib
zoTuF0ne)B&H0H9pr`zy!7lap{9^KRP+TZ!j@j_M)3>tw!-9KdbhjtO;%hEQ@Q!m>N
z=CgWR$Y=|dT3)mbXl(=K&?lSSTPw!Mm=PM^9x*~^x9@ASzte=<#%#nmFKW_)aXzZW
zlwE{YsWTu7P0(lQ9|(7jR7=!((s_aYmCaow+$H)Nsjkx!sTSxc8M{s|lIjv2Bb`h1
z9h-|2F0@SNC-al9`bIWyZg)RlF~(+$lb5yGE5^yI+O^wSB&DrjBO)8u6fM1KT)PWv
zGSpeJ^c+1)sw?zWBJUjiTT*4HiUBulZjMZu2Roozq|>BYrSFolMfx78=IJ%knW697
zoJzQpS^9ghEo<5Kvg`2rh2kx}>$uT%JU98@I@H^@;Vs?TntXo3`1LJ~j~M)-mRiw*
zSPx<YsJA<JChsq>c3je5pISeq`}+-l|1P3@$1DOd3G}R=F0m!y8K(zEjKD~q%?n?0
zdG1x9=bwSW7lFZ&qz6tIffKn)FFU%|JO4Pbaj}%%oOyO{%fGeqw|2d6+~^zMKKs1)
zy9;{9q|q^%yZGAMk{>K~mDaXG+lRH8o4R-2@Xl-A`ByzhOJnQ5*A5SDc5MmY_Gq8{
za{IJ4J@eh?+W2Lydp0*!ZtK;&y}L(|H>i1ftV??;>?VIZ67Wxn=gkJ5Urvhf`hd2V
z=U37~+#bQ?b!Z(l{4>(}1&}Aml3=|$Fgf}bAwVrp19?#W>@qAnyo;)&(am=5qAF>1
zvmX(*LRvdW>s?|iBYMKD1pqcYu!;0?Y7GzX>Bpaw7I_0&hW*H$ws-Uy9)mveypyj2
p*>zDARYsl%)~Adb9$23;YSs3>yUYm+EP0oFPx^lNi2+OP{{TYqZ({%e

literal 0
HcmV?d00001