diff --git a/FRC_Fiducial_Tracking/April_PNP_Live.py b/FRC_Fiducial_Tracking/April_PNP_Live.py index 686023d..c0828ec 100644 --- a/FRC_Fiducial_Tracking/April_PNP_Live.py +++ b/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: diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/0.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/0.jpg new file mode 100644 index 0000000..e613916 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/0.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/1.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/1.jpg new file mode 100644 index 0000000..448830f Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/1.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/10.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/10.jpg new file mode 100644 index 0000000..b70d833 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/10.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/11.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/11.jpg new file mode 100644 index 0000000..0b7182d Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/11.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/12.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/12.jpg new file mode 100644 index 0000000..4de6ba8 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/12.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/13.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/13.jpg new file mode 100644 index 0000000..7ccf210 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/13.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/14.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/14.jpg new file mode 100644 index 0000000..6f6b8de Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/14.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/15.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/15.jpg new file mode 100644 index 0000000..c07d89a Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/15.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/16.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/16.jpg new file mode 100644 index 0000000..c74fbb1 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/16.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/17.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/17.jpg new file mode 100644 index 0000000..280f133 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/17.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/18.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/18.jpg new file mode 100644 index 0000000..fa28eff Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/18.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/19.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/19.jpg new file mode 100644 index 0000000..f950416 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/19.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/2.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/2.jpg new file mode 100644 index 0000000..5728f45 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/2.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/3.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/3.jpg new file mode 100644 index 0000000..3618f9b Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/3.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/4.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/4.jpg new file mode 100644 index 0000000..02dd8dd Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/4.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/5.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/5.jpg new file mode 100644 index 0000000..b781758 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/5.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/6.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/6.jpg new file mode 100644 index 0000000..dba8be7 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/6.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/7.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/7.jpg new file mode 100644 index 0000000..604a512 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/7.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/8.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/8.jpg new file mode 100644 index 0000000..542c0ab Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/8.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/9.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/9.jpg new file mode 100644 index 0000000..5bf4f99 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_1088x720/9.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/1.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/1.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/1.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/1.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/10.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/10.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/10.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/10.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/11.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/11.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/11.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/11.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/12.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/12.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/12.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/12.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/13.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/13.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/13.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/13.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/14.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/14.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/14.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/14.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_640x480/15.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/15.jpg new file mode 100644 index 0000000..9761494 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/15.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_640x480/16.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/16.jpg new file mode 100644 index 0000000..af61974 Binary files /dev/null and b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/16.jpg differ diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/2.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/2.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/2.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/2.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/3.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/3.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/3.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/3.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/4.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/4.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/4.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/4.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/5.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/5.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/5.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/5.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/6.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/6.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/6.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/6.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/7.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/7.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/7.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/7.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/8.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/8.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/8.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/8.jpg diff --git a/FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/9.jpg b/FRC_Fiducial_Tracking/Calibration_Pics_640x480/9.jpg similarity index 100% rename from FRC_Fiducial_Tracking/Calibration_Pics_RPi_Cam/9.jpg rename to FRC_Fiducial_Tracking/Calibration_Pics_640x480/9.jpg diff --git a/FRC_Fiducial_Tracking/Camera_Calibration.py b/FRC_Fiducial_Tracking/Camera_Calibration.py index 354d23d..3caa668 100644 --- a/FRC_Fiducial_Tracking/Camera_Calibration.py +++ b/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)