Field position localization works WOOOO

FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py

@@ -159,7 +159,8 @@ while True:
                 imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, camera_matrix, dist)
                 image = display_features(image, imgpts)
 
-    pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected)
+    if(len(tags_detected) > 0):
+        pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected)
 
     #Showing image. use --display to show image
     if args.display:

FRC_Fiducial_Tracking/PNP_Pose_Estimation.py

@@ -21,7 +21,6 @@ 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]])])
 
-        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)

FRC_Fiducial_Tracking/WTFTEST.py

@@ -13,7 +13,7 @@ dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e
 
 # 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]]