apriltag detection in working state, file reorganization

FRC_Fiducial_Tracking/April_PNP_Live.py

@@ -21,7 +21,7 @@ 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("--high_res", action='store_true', help="high resolution camera capture")
 parser.add_argument("--pose_estimation", action='store_true', help="estimate pose based on detected tags")
 parser.add_argument("--ip_add", type=str, required=True)
 args = parser.parse_args()
@@ -29,17 +29,18 @@ args = parser.parse_args()
 # 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]
 # 621.5827338
-FOCAL_LEN_PIXELS = 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.]])
+camera_matrix = np.array([[976.16482142,   0.,         771.05155174],
+                        [  0.,         974.47104393, 408.52081949],
+                        [  0.,           0.,           1.        ]])
 
 # from Camera_Calibration.py
-dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00])
+dist = np.array([-0.04790604,  0.08489533, -0.00387366,  0.00616192, -0.03875398])
 
 camera_res = (1536, 864)
 
+'''
 if args.high_res:
     FOCAL_LEN_PIXELS = 991.5391539
     camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0.00000000, 528.420369],
@@ -48,10 +49,11 @@ if args.high_res:
     dist = np.array([[ 2.52081760e-01, -1.34794418e+00,  1.24975695e-03, -7.77510823e-04,
     2.29608398e+00]])
     camera_res = (2304, 1296)
+'''
 
 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)
+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)
 
@@ -127,13 +129,15 @@ def display_features(image, imgpts, totalDist):
 # setting up apriltag detection. Make sure this is OUTSIDE the loop next time
 detector = dt_apriltags.Detector(searchpath=['apriltags'],
                        families='tag36h11',
-                       nthreads=2,
+                       nthreads=3,
                        quad_decimate=2,
                        quad_sigma=0,
                        refine_edges=1,
                        decode_sharpening=0.25,
                        debug=0)
 
+counter = 0
+
 # main vision processing code
 time.sleep(0.1)
 while True:
@@ -152,7 +156,7 @@ while True:
 
     for det in output:
         # if the confidence is less than 30% exclude the tag from being processed.
-        if det[4]>30:
+        if det.decision_margin>30:
             # 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)
 
@@ -198,7 +202,11 @@ while True:
 
     # frame rate for performance
     FPS = (1/(time.time()-frame_start))
-    #print(FPS)
+
+    counter = counter+1
+    if(counter%10):
+        print(FPS)
+
     vision_table.putNumber("FPS", FPS)
 
 cam.stop()

FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py

@@ -4,7 +4,7 @@
 
 import time
 import cv2
-import apriltag
+import dt_apriltags
 import numpy as np
 from math import sqrt
 from math import pi
@@ -12,7 +12,7 @@ import math
 from networktables import NetworkTables
 import argparse
 from TagObj import TagObj
-from PiVid import PiVid
+from Picam2Vid import Picam2Vid
 from PNP_Pose_Estimation import PNPPose
 
 RAD2DEG = 180*pi
@@ -20,7 +20,7 @@ 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("--high_res", action='store_true', help="enable resolution 1088x720 vs 640x480")
 parser.add_argument("--pose_estimation", action='store_true', help="estimate pose based on detected tags")
 parser.add_argument("--ip_add", type=str, required=True)
 args = parser.parse_args()
@@ -28,17 +28,18 @@ args = parser.parse_args()
 # 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]
 # 621.5827338
-FOCAL_LEN_PIXELS = 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.]])
+camera_matrix = np.array([[976.16482142,   0.,         771.05155174],
+                        [  0.,         974.47104393, 408.52081949],
+                        [  0.,           0.,           1.        ]])
 
 # from Camera_Calibration.py
-dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00])
+dist = np.array([-0.04790604,  0.08489533, -0.00387366,  0.00616192, -0.03875398])
  
-camera_res = (640, 480)
+camera_res = (1536, 864)
 
+'''
 if args.high_res:
     FOCAL_LEN_PIXELS = 991.5391539
     camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0.00000000, 528.420369],
@@ -47,10 +48,10 @@ if args.high_res:
     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)
+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)
 
@@ -100,7 +101,7 @@ testing_tag_coords = tag_corners(testing_tags)
 def getTagCoords(tag_id):
     return tag_coords[tag_id]
 
-cam = PiVid(camera_res).start()
+cam = Picam2Vid(camera_res).start()
 
 def connectionListener(connected, info):
     print(info, "; Connected=%s" % connected)
@@ -127,13 +128,19 @@ def display_features(image, imgpts):
     return image
 
 # setting up apriltag detection. Make sure this is OUTSIDE the loop next time
-options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=4,
-quad_decimate=1.0, quad_blur=0.0, refine_edges=True,
-refine_decode=False, refine_pose=False, debug=False, quad_contours=True)
-detector = apriltag.Detector(options)
+detector = dt_apriltags.Detector(searchpath=['apriltags'],
+                       families='tag36h11',
+                       nthreads=3,
+                       quad_decimate=2,
+                       quad_sigma=0,
+                       refine_edges=1,
+                       decode_sharpening=0.25,
+                       debug=0)
 
 pose_estimator = PNPPose(testing_tag_coords, robo_space_pose, camera_matrix, dist)
 
+counter = 0
+
 # main vision processing code
 time.sleep(0.1)
 while True:
@@ -149,7 +156,7 @@ while True:
 
     for det in output:
         # if the confidence is less than 30% exclude the tag from being processed.
-        if det[4]>30:
+        if det.decision_margin>30:
             # points of the tag to be tracked
             image_corners = list(image_corners)
             image_corners = image_corners+(list(det.corners))
@@ -177,7 +184,11 @@ while True:
 
     # frame rate for performance
     FPS = (1/(time.time()-frame_start))
-    #print(FPS)
+
+    counter = counter+1
+    if(counter%10):
+        print(FPS)
+
     vision_table.putNumber("FPS", FPS)
 
 cam.stop()

FRC_Fiducial_Tracking/Aruco_Detection.py

@@ -1,30 +0,0 @@
-import cv2
-import numpy as np
-from Picam2Vid import Picam2Vid
-import time
-
-
-stream = Picam2Vid((1536, 864))
-
-tag36h11 = cv2.aruco.DICT_APRILTAG_36h11
-params = cv2.aruco.DetectorParameters_create()
-dictionary = cv2.aruco.Dictionary_get(tag36h11)
-
-while True:
-    start=time.time()
-
-    stream.update()
-    frame = stream.read()
-    frame = cv2.flip(frame, 1)
-
-    grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-
-    cv2.imshow('frame', frame)
-    cv2.waitKey(1)
-
-    (corners, ids, rejected) = cv2.aruco.detectMarkers(grey, dictionary, parameters=params)
-    #print(corners)
-    print("ids: " + str(ids))
-    #print(rejected)
-
-    print("FPS: "+str(1/(time.time()-start)))

FRC_Fiducial_Tracking/PiVid.py

@@ -1,44 +0,0 @@
-from picamera.array import PiRGBArray
-from picamera import PiCamera
-from threading import Thread
-
-# class for allocating a thread to only updating the camera stream,
-# the other thread is used for detection processing
-class PiVid:
-
-    def __init__(self, camera_res):
-        # RPi camera recording setup with threading crap.
-        #  For specs - https://www.raspberrypi.com/documentation/accessories/camera.html
-        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)
-
-        self.frame = None
-        self.stopped = False
-
-    # Start camera thread
-    def start(self):
-        Thread(target=self.update, args=()).start()
-        return self
-
-    # update camera stream threading
-    def update(self):
-        for frame in self.stream:
-            self.frame=frame.array
-            self.rawCapture.truncate(0)
-            if self.stopped:
-                self.stream.close()
-                self.rawCapture.close()
-                self.camera.close()
-                return
-
-    # output the frame we want
-    def read(self):
-        return self.frame
-
-    # end threading
-    def stop(self):
-        self.stopped = True

FRC_Fiducial_Tracking/PicamThreadingtest.py

@@ -1,10 +0,0 @@
-from Picam2Vid import Picam2Vid
-import cv2
-
-camera = Picam2Vid((640, 480))
-
-while True:
-    frame = camera.read()
-
-    cv2.imshow("worknow", frame)
-    cv2.waitKey(1)

FRC_Fiducial_Tracking/Static_Apriltag_Test.py

@@ -1,45 +0,0 @@
-import dt_apriltags
-import numpy as np
-import cv2
-
-FOCAL_LEN_PIXELS = 976.16482142
-camera_matrix = np.array([[FOCAL_LEN_PIXELS,   0.,         771.05155174],
-    [  0.,         FOCAL_LEN_PIXELS, 408.52081949],
-    [  0.,           0.,           1.        ]])
-    
-dist = np.array([[-0.04790604,  0.08489533, -0.00387366,  0.00616192, -0.03875398]])
-
-detector = dt_apriltags.Detector(searchpath=['apriltags'],
-                       families='tag36h11',
-                       nthreads=2,
-                       quad_decimate=1,
-                       quad_sigma=0,
-                       refine_edges=1,
-                       decode_sharpening=0.25,
-                       debug=1)
-
-#image = cv2.imread("/home/tyler/Desktop/FRC-Apriltag-Pose-Detection/FRC_Fiducial_Tracking/Static_Tag_Pics/apriltagrobots_overlay.jpg")
-image = cv2.imread("/home/tyler/Desktop/FRC-Apriltag-Pose-Detection/FRC_Fiducial_Tracking/Static_Tag_Pics/14.jpg")
-#image = cv2.imread("/home/tyler/Desktop/FRC-Apriltag-Pose-Detection/20240814_043333.jpg")
-
-#h,  w = image.shape[:2]
-#newcameramtx, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, dist, (w,h), 1, (w,h))
-
-# undistort
-#dst = cv2.undistort(image, camera_matrix, dist, None, newcameramtx)
- 
-# crop the image
-#x, y, w, h = roi
-#dst = dst[y:y+h, x:x+w]
-
-grey = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-smaller = cv2.resize(grey, (640, 360))
-
-print(smaller)
-
-cv2.imshow("frame", smaller)
-cv2.waitKey(0)
-
-output = detector.detect(smaller, estimate_tag_pose=True, camera_params=None, tag_size=None)
-
-print(output)

FRC_Fiducial_Tracking/Wide_Cam_Calibration.txt

@@ -1,6 +0,0 @@
-[[550.16284916   0.         323.59646261]
- [  0.         550.72383406 229.56706391]
- [  0.           0.           1.        ]]
-dist : 
-
-[[-0.02760058 -0.01336192 -0.00662915  0.00432453  0.23444395]]

FRC_Fiducial_Tracking/picam2video.py

@@ -1,21 +0,0 @@
-from picamera2 import Picamera2
-from libcamera import Transform
-import cv2
-import time
-
-picam = Picamera2(camera_num=0)
-config = picam.create_video_configuration(main={"size": (640, 480), "format": "RGB888"},
-                                        transform=Transform(hflip=True),
-                                        lores={"size": (640,480)},
-                                        encode='main',
-                                        )
-picam.configure(config)
-picam.set_controls({"FrameRate": 120})
-picam.start()
-while True:
-    start_frame = time.time()
-    cv2.imshow("frame", picam.capture_array())
-    cv2.waitKey(1)
-    frame = 1/(time.time()-start_frame)
-    print(frame)
-

FRC_Fiducial_Tracking/picamera2test.py

@@ -1,40 +0,0 @@
-from picamera2 import Picamera2
-from libcamera import Transform
-from threading import Thread
-
-# class for allocating a thread to only updating the camera stream,
-# the other thread is used for detection processing
-class PiVid:
-
-    def __init__(self, camera_res):
-        # RPi camera recording setup with threading crap.
-        #  For specs - https://www.raspberrypi.com/documentation/accessories/camera.html
-        self.camera = Picamera2()
-        self.resolution = camera_res
-        config = self.camera.create_video_configuration(main={"size": self.resolution}, transform=Transform(hflip=True), lores={"size": (640,480)}, encode='main')
-        self.camera.configure(config)
-        self.camera.controls({"FrameRate": 120})
-        self.frame = None
-        self.stopped = False
-
-    # Start camera thread
-    def start(self):
-        self.camera.start()
-        
-        Thread(target=self.update, args=()).start()
-        return self
-
-    # update camera stream threading
-    def update(self):
-        self.frame=self.camera.capture_array()
-        if self.stopped:
-            self.camera.stop()
-            return
-
-    # output the frame we want
-    def read(self):
-        return self.frame
-
-    # end threading
-    def stop(self):
-        self.stopped = True