From 93a7a6f7c6417e9ba1652a325a6178f08957aabe Mon Sep 17 00:00:00 2001
From: Tylr-J42 <pi@PilerJac.home>
Date: Wed, 22 Nov 2023 22:11:15 -0500
Subject: [PATCH] rotation estimation works probably

---
 FRC_Fiducial_Tracking/PNP_Pose_Estimation.py  |  28 ++++++++++++------
 .../PNP_Pose_Estimation.cpython-39.pyc        | Bin 1710 -> 2183 bytes
 2 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
index e5ca434..7080a88 100644
--- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
@@ -11,14 +11,22 @@ class PNPPose:
         self.orientation = [0, 0, 0]
         self.robo_space_pose = robo_space_pose
     
-    def rot_params_rv(self, rvecs):
-        from math import pi,atan2,asin
-        R = cv2.Rodrigues(rvecs)[0]
-        roll = 180*atan2(-R[2][1], R[2][2])/pi
-        pitch = 180*asin(R[2][0])/pi
-        yaw = 180*atan2(-R[1][0], R[0][0])/pi
-        rot_params= [roll,pitch,yaw]
-        return rot_params
+    def axisToEuler(self, rvecs):
+        R = cv2.Rodrigues(rvecs)
+
+        sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
+ 
+        singular = sy < 1e-6
+
+        if  not singular :
+            x = math.atan2(R[2,1] , R[2,2])
+            y = math.atan2(-R[2,0], sy)
+            z = math.atan2(R[1,0], R[0,0])
+        else :
+            x = math.atan2(-R[1,2], R[1,1])
+            y = math.atan2(-R[2,0], sy)
+            z = 0
+        
 
 
     def calculate_coords(self, image_corners, tags_detected):
@@ -39,6 +47,8 @@ class PNPPose:
             for i in range(len(rvecs)):
                 rvecs[i] = math.degrees(rvecs[i])
 
+            #cameraMatx, rotMatx, transVect, S
+
             euler_angles = self.rot_params_rv(rvecs)
 
-            print("euler ZYX: ", euler_angles, "rvecs: ", rvecs)
+            print("euler ZYX: ", euler_angles, "rvecs: ", rvecs,"tvecs: ", tvecs)
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 b7e5810e09b1406beba380bb488283d358ec90cb..c5813aa7a15546611614fddb5240815307ca54b4 100644
GIT binary patch
delta 617
zcmZ`#L2DC17@hB%$?WVlNo^V%BNXbv;zF<|554H2coITDPdQjZCX~|9;LH}1xG@Js
zPd$|R11SjhBueq<Rs9Wn^5o5fC&827Bw6XjfqC=h?R$@J=Y8qTTDemyMU2*;&rf^5
z)LFR?jkD*si(0*6ZK=b@ERH_D{f1NYrsYy}O4@t0?FB)?W{|=;OfbP2Pf<giEwJiO
zoZkqmtj5U2Q?B_Dn}}4{AteNP#N1P;vvZsjmYS@y6_#>R<a8-5s;HjC;JC3=gSip4
z`d?g!lRz{7GtC$c(8zNjel6z*r$0Z<et-FJpTt!!`Y$<OonE>4c%Jmmod2te3#LN4
zS@*C;kO}+DB&O)i$^>4ndbm!|MmuUNdw6_s((jlIh8c`97#sh{3kZol$W}VTL;Ga#
z@TA`}HBzi^U=ZOpDilzK3T)s8;ds5(Y_$fqclS!d9r2scaIg6m+_`+m5z0DD)k_I%
z@ht9A?DBDtxF6y>#O`Zhb4x@xzL8gTJN+)rm-P0#gMrbu83#_t9l1#}m>u^**4!f*
V75&f(RiHvuar?3kx7<kH`wI)bex(2a

delta 137
zcmZn{T*u3o$ji&c00cc1ajBo#Ci3Mls!Xg_WeH}`<k-081k+?gR!K&U$=<9Aj4GRR
zSeY2PnpqebQW${poRe$VEGO?{yTvFzc_zCoBh%#V>~f53ldrMsGipud=1^2&1Deah
j#KF$NS;PcnYO)py0;wV)AaRQ|uQa!yvPfof5QixMnhYL7