From ebcc5285309b715a919d3b90c48911b16622dc0d Mon Sep 17 00:00:00 2001
From: Tylr-J42 <pi@PilerJac.home>
Date: Mon, 20 Nov 2023 18:39:08 -0500
Subject: [PATCH] PNP for Pose almost works - errors

---
 .../Apriltag_Pose_Localization.py             |   2 +-
 FRC_Fiducial_Tracking/PNP_Pose_Estimation.py  |   4 +++-
 FRC_Fiducial_Tracking/WTFTEST.py              |   9 +--------
 .../PNP_Pose_Estimation.cpython-39.pyc        | Bin 1236 -> 1284 bytes
 4 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
index 231dfdc..80b9d73 100644
--- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
+++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
@@ -62,7 +62,7 @@ FPS = 0
         
 # 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/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
index 834bca4..4c4666e 100644
--- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
@@ -21,6 +21,8 @@ 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]])])
 
-        print("PNP_obj_input: ", PNP_obj_input, ", image_corners: ", image_corners)
+        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)
         print(tvecs,"poop")
diff --git a/FRC_Fiducial_Tracking/WTFTEST.py b/FRC_Fiducial_Tracking/WTFTEST.py
index 1972b93..588476c 100644
--- a/FRC_Fiducial_Tracking/WTFTEST.py
+++ b/FRC_Fiducial_Tracking/WTFTEST.py
@@ -45,14 +45,7 @@ def tag_corners(tag_coords):
     return corners
 
 field_tag_coords = tag_corners(tag_coords)
-robo_space_pose = [0, 0, 0, 0, 0, 0]
 
 pose_estimator = PNPPose(field_tag_coords, robo_space_pose, camera_matrix, dist)
 
-pose_estimator.calculate_coords( np.array([[ 65.75, 420.,    72.25],
- [ 72.25, 420.,    72.25],
- [ 72.25, 420.,    65.75],
- [ 65.75, 420.,    65.75]]), np.array([[398.38015747, 154.20915222],
- [260.04067993, 150.67947388],
- [212.12528992 , 77.55628204],
- [387.06124878,  77.81692505]]))
\ No newline at end of file
+pose_estimator.calculate_coords(np.array([[341.3, 157.0], [220.99, 152.41], [140.93 , 83.28], [284.02,  81.93]]), np.array([0]))
\ No newline at end of file
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 2c7d6824da166f40d6acebc4befed412a5835427..cfe240cffb486958a6a4530370077dccb3500585 100644
GIT binary patch
delta 221
zcmcb@*}}z}$ji&c00fWTM5q4V$org;iG^|UH^zNlHH-_`Q`i<V)-slGq_Eepq;S+Q
zrf}9UHM21?lyGKo)v(ks)-Yso12InuV+vO<YYcNOTP=GnM+rv_M+$c{6C*<kLokCT
z&*TqGQtY=_({l0?OD1zMt0-0Rmn5ba$ET#0q$Zc7rdTOxGJ>RmVo@vw`S}H!yp!#j
zwb<Dbi%TjCQYU9HPZbpbxsj2Lk&8)yk&DrV$&ZPPNocYFix;Ef<Y*RiUP++zE!Mo!
N+=9v?!O8PkOaaZZIRO9w

delta 172
zcmZqSy28nu$ji&c00g(~f>Rf5<bBS_^o?QiH^zN7CG06|H7qIYHH^(Hj0`0lS)4U2
zHH<Y3SzJKOox+&H(aRdcT+3F=UdvI!Uc-^X+04Yqkirnmpvg7)ACuH%d1e(sO~zZS
zX*r4M#ZfE;`S}H!Jd;zIwI)wzo+2y+vXqgHk&8)yk&DrV$&ZO|vIC13qx|G<7IR*4
TpyVyqywco)$|8Zu=UGev(YPuv