From 762163757fa0fc448994b9b2107ff083e260bfc7 Mon Sep 17 00:00:00 2001
From: Tylr-J42 <pi@PilerJac.home>
Date: Wed, 22 Nov 2023 22:27:37 -0500
Subject: [PATCH] rotation okay i gues

---
 FRC_Fiducial_Tracking/PNP_Pose_Estimation.py  |  25 ++++++------------
 .../PNP_Pose_Estimation.cpython-39.pyc        | Bin 2183 -> 2183 bytes
 2 files changed, 8 insertions(+), 17 deletions(-)

diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
index 7080a88..c1494f4 100644
--- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
+++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
@@ -11,24 +11,15 @@ class PNPPose:
         self.orientation = [0, 0, 0]
         self.robo_space_pose = robo_space_pose
     
-    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 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 calculate_coords(self, image_corners, tags_detected):
         tag_corners = self.tag_corners
         PNP_obj_input = np.array([])
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 c5813aa7a15546611614fddb5240815307ca54b4..54c282c38e2542cdffdead9aeaf6a9cc34bfa20d 100644
GIT binary patch
delta 19
YcmZn{Y!~E8<mKgJ00NsM8@XyZ044|o00000

delta 19
ZcmZn{Y!~E8<mKgJ0D@n8H*(c-001gk1h)VH