attempting to get euler angles

FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py

@@ -83,6 +83,7 @@ def tag_corners(tag_coords):
         x_offset = (b/2)*math.cos(math.radians(z_rotation))
         y_offset = (b/2)*math.sin(math.radians(z_rotation))
         coordinates[0] = tag_coords[i][0]
+        
         coordinates[1] = [x-x_offset, y+y_offset, z+b/2]
         coordinates[2] = [x+x_offset, y+y_offset, z+b/2]
         coordinates[3] = [x+x_offset, y+y_offset, z-b/2]
@@ -140,7 +141,7 @@ while True:
 
     #detecting april tags
     tagFrame = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-    
+
     output = detector.detect(tagFrame)
 
     for det in output:

FRC_Fiducial_Tracking/PNP_Pose_Estimation.py

@@ -10,18 +10,35 @@ class PNPPose:
         self.camera_matrix = camera_matrix
         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 calculate_coords(self, image_corners, tags_detected):
         tag_corners = self.tag_corners
         PNP_obj_input = np.array([])
 
-        for i in range(len(tags_detected)):
-            if(PNP_obj_input.size == 0):
-                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]])
-            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]])])
+        if len(tags_detected)>0:
 
+            for i in range(len(tags_detected)):
+                if(PNP_obj_input.size == 0):
+                    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]])
+                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, "tags_detected: ", tags_detected)
-        ret, rvecs, tvecs = cv2.solvePnP(PNP_obj_input, image_corners, self.camera_matrix, self.dist, flags=0)
-        print(tvecs,"poop")
+            #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)
+            
+            for i in range(len(rvecs)):
+                rvecs[i] = math.degrees(rvecs[i])
+
+            euler_angles = self.rot_params_rv(rvecs)
+
+            print("euler ZYX: ", euler_angles, "rvecs: ", rvecs)

FRC_Fiducial_Tracking/PiVid.py

@@ -12,6 +12,7 @@ class PiVid:
         self.camera = PiCamera()
         self.camera.resolution = camera_res
         self.camera.framerate = 60
+        self.camera.rotation = 180
         self.rawCapture = PiRGBArray(self.camera, size=camera_res)
         self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True)