rotation estimation works probably

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)