better calibration

FRC_Fiducial_Tracking/April_PNP_Live.py

@@ -15,34 +15,45 @@ import argparse
 from threading import Thread
 
 
-# translation vector units to inches: tvec/71.22 this constant will differ
-# according to your camera. Space an apriltag at intervals, note the distance
-# in pixels and divide it by the real world distance
-TVEC2IN = 1
 # Rotational vector radians to degrees
 RAD2DEG = 180/pi
 
+# To show display of camera feed add --display in terminal when running script. To set IP address use --ip_add.
+parser = argparse.ArgumentParser(description="Select display")
+parser.add_argument("--display", action='store_true', help="enable a display of the camera")
+parser.add_argument("--high_res", action='store_true', help="enable resolution 1088x720 vs 640x480")
+parser.add_argument("--ip_add", type=str, required=True)
+args = parser.parse_args()
 
-# focal length in pixels. You can use Camera_Calibrate.py or calculate using a camera spec sheet for more accuracy
+# focal length in pixels. You can use Camera_Calibrate.py and take at least 10 pics of a chess board or calculate using a camera spec sheet
 # focal_length [mm] / imager_element_length [mm/pixel]
-FOCAL_LEN_PIXELS = 528.6956522
+# 621.5827338
+FOCAL_LEN_PIXELS = 621.5827338
 # camera matrix from Calibrate_Camera.py.
 camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0., 308.94165115],
  [0., FOCAL_LEN_PIXELS, 221.9470321],
  [0., 0.,1.]])
 
-b=7.15
+# from Camera_Calibration.py
+dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00])
+ 
+camera_res = (640, 480)
+
+if args.high_res:
+    FOCAL_LEN_PIXELS = 991.5391539
+    camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0.00000000, 528.420369],
+    [0.00000000, FOCAL_LEN_PIXELS, 342.737594],
+    [0.00000000, 0.00000000, 1.00000000]])
+    dist = np.array([[ 2.52081760e-01, -1.34794418e+00,  1.24975695e-03, -7.77510823e-04,
+    2.29608398e+00]])
+    camera_res = (1088, 720)
+
+b=6.5
 # 3d object array. The points of the 3d april tag that coresponds to tag_points which we detect
 objp = np.array([[0,0,0], [b/2, b/2, 0], [-b/2, b/2, 0], [-b/2, -b/2, 0], [b/2, -b/2, 0]], dtype=np.float32)
 # 2d axis array points for drawing cube overlay
 axis = np.array([[b/2, b/2, 0], [-b/2, b/2, 0], [-b/2, -b/2, 0], [b/2, -b/2, 0], [b/2, b/2, -b], [-b/2, b/2, -b], [-b/2, -b/2, -b], [b/2, -b/2, -b]], dtype=np.float32)
 
-# To show display of camera feed add --display in terminal when running script. To set IP address use --ip_add.
-parser = argparse.ArgumentParser(description="Select display")
-parser.add_argument("--display", action='store_true', help="enable a display of the camera")
-parser.add_argument("--ip_add", type=str, required=True)
-args = parser.parse_args()
-
 # network tables + RoboRio IP
 NetworkTables.initialize(server=args.ip_add)
 vision_table = NetworkTables.getTable("Fiducial")
@@ -51,6 +62,15 @@ FPS = 0
 
 TARGET_ID = 1
 
+# 2023 Field Apriltag Coordinates index = tag id
+# format = [x, y, z, z-rotation]
+tag_coords = [0, [610.77, 42.19, 18.22, 180], [610.77, 108.19, 18.22, 180], [610.77, 174.19, 18.22, 180],
+[636.96, 265.74, 27.38, 180], [14.25, 265.74, 27.38, 0], [40.45, 174.19, 18.22, 0], [40.45, 108.19, 18.22, 0],
+[40.45, 42.19, 18.22, 0]]
+
+def getTagCoords(tag_id):
+    return tag_coords[tag_id]
+
 # class for allocating a thread to only updating the camera stream,
 # the other thread is used for detection processing
 class PiVid:
@@ -59,9 +79,9 @@ class PiVid:
         # RPi camera recording setup with threading crap.
         #  For specs - https://www.raspberrypi.com/documentation/accessories/camera.html
         self.camera = PiCamera()
-        self.camera.resolution = (640, 480)
+        self.camera.resolution = camera_res
         self.camera.framerate = 60
-        self.rawCapture = PiRGBArray(self.camera, size=(640,480))
+        self.rawCapture = PiRGBArray(self.camera, size=camera_res)
         self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True)
 
         self.frame = None
@@ -118,7 +138,7 @@ def display_features(image, imgpts, totalDist):
     return image
 
 # setting up apriltag detection. Make sure this is OUTSIDE the loop next time
-options = apriltag.DetectorOptions(families='tag16h5', border=1, nthreads=4,
+options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=4,
 quad_decimate=2.0, quad_blur=0.0, refine_edges=True,
 refine_decode=False, refine_pose=False, debug=False, quad_contours=True)
 detector = apriltag.Detector(options)
@@ -138,8 +158,7 @@ while True:
     for det in output:
         # points of the tag to be tracked
         tag_points = np.array([[det.center[0], det.center[1]], [det.corners[0][0], det.corners[0][1]], [det.corners[1][0], det.corners[1][1]], [det.corners[2][0], det.corners[2][1]], [det.corners[3][0], det.corners[3][1]]], dtype=np.float32)
-        # from Camera_Calibration.py
-        dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00])
+
 
         ret,rvecs, tvecs = cv2.solvePnP(objp, tag_points, camera_matrix, dist, flags=0)
 
@@ -161,9 +180,6 @@ while True:
         data_array.append([det.tag_id, tvecDist, rvecDeg, totalDist])
         
 
-    # writing data to networktables and ordering tags
-    target_detected = False
-
     for i in range(len(data_array)):
         orderVal = 0
         for d in range(len(data_array)):
@@ -178,11 +194,8 @@ while True:
         vision_table.putNumber("tag"+str(data_array[i][0])+"rvecY("+str(orderVal)+")", rvecDeg[1])
         vision_table.putNumber("tag"+str(data_array[i][0])+"rvecZ("+str(orderVal)+")", rvecDeg[2])
 
-        if TARGET_ID == data_array[0]:
-            target_detected = True
 
     vision_table.putNumber("numberOfTags", len(data_array))
-    vision_table.putBoolean("targetDetected", target_detected)
 
     #Showing image. use --display to show image
     if args.display:

FRC_Fiducial_Tracking/Camera_Calibration.py

@@ -21,7 +21,7 @@ objp[0,:,:2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)
 prev_img_shape = None
 
 # Extracting path of individual image stored in a given directory
-images = glob.glob('/home/pi/Desktop/Fudicial_Stuff/FRC_Fiducial_Tracking/Calibration_Pics/*.jpg')
+images = glob.glob('/home/pi/Desktop/apriltag-pose-frc/FRC_Fiducial_Tracking/Calibration_Pics_640x480/*.jpg')
 for fname in images:
     img = cv2.imread(fname)
     gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)