diff --git a/CameraCalibration.npz b/CameraCalibration.npz deleted file mode 100644 index 24ce2ec..0000000 Binary files a/CameraCalibration.npz and /dev/null differ diff --git a/camera calibration/CameraCalibration.npz b/camera calibration/CameraCalibration.npz deleted file mode 100644 index 01de91e..0000000 Binary files a/camera calibration/CameraCalibration.npz and /dev/null differ diff --git a/camera_calibration_000.jpg b/camera_calibration_000.jpg deleted file mode 100644 index a4d19c8..0000000 Binary files a/camera_calibration_000.jpg and /dev/null differ diff --git a/camera_calibration_001.jpg b/camera_calibration_001.jpg deleted file mode 100644 index 5eeb11c..0000000 Binary files a/camera_calibration_001.jpg and /dev/null differ diff --git a/camera_calibration_002.jpg b/camera_calibration_002.jpg deleted file mode 100644 index 956f7e4..0000000 Binary files a/camera_calibration_002.jpg and /dev/null differ diff --git a/camera_calibration_003.jpg b/camera_calibration_003.jpg deleted file mode 100644 index 79521c3..0000000 Binary files a/camera_calibration_003.jpg and /dev/null differ diff --git a/camera_calibration_004.jpg b/camera_calibration_004.jpg deleted file mode 100644 index c132690..0000000 Binary files a/camera_calibration_004.jpg and /dev/null differ diff --git a/camera_calibration_005.jpg b/camera_calibration_005.jpg deleted file mode 100644 index 6d29045..0000000 Binary files a/camera_calibration_005.jpg and /dev/null differ diff --git a/camera_calibration_006.jpg b/camera_calibration_006.jpg deleted file mode 100644 index 84ce223..0000000 Binary files a/camera_calibration_006.jpg and /dev/null differ diff --git a/camera_calibration_007.jpg b/camera_calibration_007.jpg deleted file mode 100644 index 02d1405..0000000 Binary files a/camera_calibration_007.jpg and /dev/null differ diff --git a/track_apriltag.py b/track_apriltag.py index 876d8e3..bf876e9 100644 --- a/track_apriltag.py +++ b/track_apriltag.py @@ -1,21 +1,33 @@ import cv2 from pupil_apriltags import Detector import numpy as np -import matplotlib.pyplot as plt from time import time +from cscore import CameraServer +from networktables import NetworkTablesInstance + +ntinst = NetworkTablesInstance.getDefault() +ntinst.startClientTeam(4903) +ntinst.startDSClient() +nt = ntinst.getTable('SmartDashboard') + +frame_height = 240 +frame_width = 320 + +cs = CameraServer() +CameraServer.enableLogging() +output = cs.putVideo("April Tags", frame_width, frame_height) + +cs.startAutomaticCapture().setResolution(frame_width, frame_height) + +sink = cs.getVideo() + # Edit these variables for config. camera_params = 'camera calibration/CameraCalibration.npz' -webcam = False -video_source = 'Testing_apriltag.mp4' framerate = 30 - output_overlay = True -output_file = 'vision output/test_output.mp4' undistort_frame = True - -show_graph = False debug_mode = True show_framerate = True @@ -25,27 +37,6 @@ aprilCameraMatrix = [cameraMatrix[0][0], cameraMatrix[1][1], cameraMatrix[0][2], cameraMatrix[1][2]] -if webcam: - capture = cv2.VideoCapture(0) -else: - capture = cv2.VideoCapture(video_source) - -# video_fps = capture.get(cv2.CAP_PROP_FPS), -frame_height = capture.get(cv2.CAP_PROP_FRAME_HEIGHT) -frame_width = capture.get(cv2.CAP_PROP_FRAME_WIDTH) - -fourcc = cv2.VideoWriter_fourcc(*"mp4v") -writer = cv2.VideoWriter(output_file, apiPreference=0, fourcc=fourcc, fps=framerate, - frameSize=(int(frame_width), int(frame_height))) - -if show_graph: - fig = plt.figure() - axes = plt.axes(projection='3d') - axes.set_title("3D scatterplot", pad=110, size=100) - axes.set_xlabel("X") - axes.set_ylabel("Y") - axes.set_zlabel("Z") - # options = DetectorOptions(families="tag36h11") detector = Detector( families='tag16h5', @@ -56,14 +47,10 @@ refine_edges=3, ) -# Check if camera opened successfully -if not capture.isOpened(): - print("Error opening video stream or file") - # Read until video is completed -while capture.isOpened(): +while True: # Capture frame-by-frame - ret, frame = capture.read() + ret, frame = sink.grabFrame(None); if ret: start_time = time() @@ -78,18 +65,16 @@ if debug_mode: print("[INFO] detecting AprilTags...") - results = detector.detect(image, estimate_tag_pose=True, camera_params=aprilCameraMatrix, tag_size=5.25) + results = detector.detect(image, estimate_tag_pose=True, camera_params=aprilCameraMatrix, tag_size=0.2032) # print(results) - if debug_mode: + if debug_mode: print(f"[INFO] {len(results)} total AprilTags detected") print(f"[INFO] Looping over {len(results)} apriltags and getting data") # loop over the AprilTag detection results if len(results) == 0: - if not show_graph: - cv2.imshow("Image", inputImage) - writer.write(inputImage) + cv2.imshow("Image", inputImage) for r in results: # extract the bounding box (x, y)-coordinates for the AprilTag @@ -120,6 +105,9 @@ x_centered = cX - frame_width / 2 y_centered = -1 * (cY - frame_height / 2) + + nt.putNumber("x", x_centered); + nt.putNumber("y", y_centered); cv2.putText(inputImage, f"Center X coord: {x_centered}", (ptB[0] + 10, ptB[1] - 20), cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 255, 0), 2) @@ -132,9 +120,9 @@ cv2.circle(inputImage, (int((frame_width / 2)), int((frame_height / 2))), 5, (0, 0, 255), 2) - # pose = detector.detection_pose(detection=r, camera_params=aprilCameraMatrix, tag_size=8) poseRotation = r.pose_R poseTranslation = r.pose_t + poseTranslation = [i*31.9541559133 for i in poseTranslation] if debug_mode: print(f"[DATA] Detection rotation matrix:\n{poseRotation}") @@ -149,12 +137,6 @@ # z axis cv2.line(inputImage, (cX, cY), (cX + int(poseRotation[0][2] * 100), cY + int(poseRotation[1][2] * 100)), (255, 0, 0), 2) - if show_graph: - # only if the id of the tag is 8, plot the 3D graph - if r.tag_id == 0: - axes.scatter(poseTranslation[0][0], poseTranslation[1][0], poseTranslation[2][0]) - plt.pause(0.01) - if debug_mode: # show the output image after AprilTag detection print("[INFO] displaying image after overlay") @@ -164,24 +146,15 @@ cv2.putText(inputImage, f"FPS: {1 / (end_time - start_time)}", (0, 30), cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 255, 0), 2) - if not show_graph: - cv2.imshow("Image", inputImage) - writer.write(inputImage) - + cv2.imshow("Image", inputImage) # Press Q on keyboard to exit - if not show_graph: - if cv2.waitKey(25) & 0xFF == ord('q'): - break + if cv2.waitKey(25) & 0xFF == ord('q'): + break + #320, 240 + output.putFrame(inputImage) + # Break the loop else: break - - -# When everything done, release the video capture object -# save the output video to disk -writer.release() -capture.release() -if show_graph: - plt.show()