From bad453f4b33b8d3f387138268e4ae0fc08fed347 Mon Sep 17 00:00:00 2001 From: Barrett Ray Date: Wed, 14 Jun 2023 22:00:33 -0500 Subject: [PATCH 1/3] Created a `rye` project for the libs folder. This should help a lot with decrusting and understanding each file and its dependencies. :) --- Autonomous/libs/.python-version | 1 + Autonomous/libs/README.md | 11 +++++++++++ Autonomous/libs/pyproject.toml | 24 +++++++++++++++++++++++ Autonomous/libs/requirements-dev.lock | 28 +++++++++++++++++++++++++++ Autonomous/libs/requirements.lock | 28 +++++++++++++++++++++++++++ Autonomous/libs/src/libs/__init__.py | 2 ++ 6 files changed, 94 insertions(+) create mode 100644 Autonomous/libs/.python-version create mode 100644 Autonomous/libs/README.md create mode 100644 Autonomous/libs/pyproject.toml create mode 100644 Autonomous/libs/requirements-dev.lock create mode 100644 Autonomous/libs/requirements.lock create mode 100644 Autonomous/libs/src/libs/__init__.py diff --git a/Autonomous/libs/.python-version b/Autonomous/libs/.python-version new file mode 100644 index 0000000..09dcc78 --- /dev/null +++ b/Autonomous/libs/.python-version @@ -0,0 +1 @@ +3.10.11 diff --git a/Autonomous/libs/README.md b/Autonomous/libs/README.md new file mode 100644 index 0000000..209b9f0 --- /dev/null +++ b/Autonomous/libs/README.md @@ -0,0 +1,11 @@ +# libs + +Some helper classes for all of Autonomous and its operations. + +## ARTracker + +Helps with tracking ARUCO tags and in navigating the environment. YOLO object detection may be used for drawing bounding boxes. (requires CUDA) + +## Others + +TODO! diff --git a/Autonomous/libs/pyproject.toml b/Autonomous/libs/pyproject.toml new file mode 100644 index 0000000..86d7d70 --- /dev/null +++ b/Autonomous/libs/pyproject.toml @@ -0,0 +1,24 @@ +[project] +name = "libs" +version = "0.1.0" +description = "Add a short description here" +dependencies = [ + "opencv-python~=4.7.0.72", + "numpy~=1.24.3", + "darknet~=0.3", + "scikit-image~=0.21.0", + "black~=23.3.0", + "opencv-contrib-python~=4.7.0.72", +] +readme = "README.md" +requires-python = ">= 3.10" + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[tool.rye] +managed = true + +[tool.hatch.metadata] +allow-direct-references = true diff --git a/Autonomous/libs/requirements-dev.lock b/Autonomous/libs/requirements-dev.lock new file mode 100644 index 0000000..fed0ab6 --- /dev/null +++ b/Autonomous/libs/requirements-dev.lock @@ -0,0 +1,28 @@ +# generated by rye +# use `rye lock` or `rye sync` to update this lockfile +# +# last locked with the following flags: +# pre: false +# features: [] +# all-features: false + +-e file:. +black==23.3.0 +click==8.1.3 +darknet==0.3 +imageio==2.31.1 +lazy-loader==0.2 +mypy-extensions==1.0.0 +networkx==3.1 +numpy==1.24.3 +opencv-contrib-python==4.7.0.72 +opencv-python==4.7.0.72 +packaging==23.1 +pathspec==0.11.1 +pillow==9.5.0 +platformdirs==3.5.3 +pywavelets==1.4.1 +scikit-image==0.21.0 +scipy==1.10.1 +tifffile==2023.4.12 +tomli==2.0.1 diff --git a/Autonomous/libs/requirements.lock b/Autonomous/libs/requirements.lock new file mode 100644 index 0000000..fed0ab6 --- /dev/null +++ b/Autonomous/libs/requirements.lock @@ -0,0 +1,28 @@ +# generated by rye +# use `rye lock` or `rye sync` to update this lockfile +# +# last locked with the following flags: +# pre: false +# features: [] +# all-features: false + +-e file:. +black==23.3.0 +click==8.1.3 +darknet==0.3 +imageio==2.31.1 +lazy-loader==0.2 +mypy-extensions==1.0.0 +networkx==3.1 +numpy==1.24.3 +opencv-contrib-python==4.7.0.72 +opencv-python==4.7.0.72 +packaging==23.1 +pathspec==0.11.1 +pillow==9.5.0 +platformdirs==3.5.3 +pywavelets==1.4.1 +scikit-image==0.21.0 +scipy==1.10.1 +tifffile==2023.4.12 +tomli==2.0.1 diff --git a/Autonomous/libs/src/libs/__init__.py b/Autonomous/libs/src/libs/__init__.py new file mode 100644 index 0000000..1336580 --- /dev/null +++ b/Autonomous/libs/src/libs/__init__.py @@ -0,0 +1,2 @@ +def hello(): + return "Hello from libs!" From 345621c64457754382830f2225767c771fa9a621 Mon Sep 17 00:00:00 2001 From: Barrett Ray Date: Wed, 14 Jun 2023 22:04:56 -0500 Subject: [PATCH 2/3] chore: Formatted some deps of ARTracker --- Autonomous/YOLO/darknet/darknet_images.py | 212 +++++++---- Autonomous/findFocalLength.py | 61 +-- Autonomous/libs/ARTracker.py | 440 ++++++++++++++-------- Autonomous/main.py | 94 +++-- 4 files changed, 514 insertions(+), 293 deletions(-) diff --git a/Autonomous/YOLO/darknet/darknet_images.py b/Autonomous/YOLO/darknet/darknet_images.py index ca23d3f..4bfa344 100644 --- a/Autonomous/YOLO/darknet/darknet_images.py +++ b/Autonomous/YOLO/darknet/darknet_images.py @@ -6,50 +6,83 @@ import time import cv2 import numpy as np -import darknet def parser(): parser = argparse.ArgumentParser(description="YOLO Object Detection") - parser.add_argument("--input", type=str, default="", - help="image source. It can be a single image, a" - "txt with paths to them, or a folder. Image valid" - " formats are jpg, jpeg or png." - "If no input is given, ") - parser.add_argument("--batch_size", default=1, type=int, - help="number of images to be processed at the same time") - parser.add_argument("--weights", default="yolov4.weights", - help="yolo weights path") - parser.add_argument("--dont_show", action='store_true', - help="windown inference display. For headless systems") - parser.add_argument("--ext_output", action='store_true', - help="display bbox coordinates of detected objects") - parser.add_argument("--save_labels", action='store_true', - help="save detections bbox for each image in yolo format") - parser.add_argument("--config_file", default="./cfg/yolov4.cfg", - help="path to config file") - parser.add_argument("--data_file", default="./cfg/coco.data", - help="path to data file") - parser.add_argument("--thresh", type=float, default=.25, - help="remove detections with lower confidence") + parser.add_argument( + "--input", + type=str, + default="", + help="image source. It can be a single image, a" + "txt with paths to them, or a folder. Image valid" + " formats are jpg, jpeg or png." + "If no input is given, ", + ) + parser.add_argument( + "--batch_size", + default=1, + type=int, + help="number of images to be processed at the same time", + ) + parser.add_argument("--weights", default="yolov4.weights", help="yolo weights path") + parser.add_argument( + "--dont_show", + action="store_true", + help="windown inference display. For headless systems", + ) + parser.add_argument( + "--ext_output", + action="store_true", + help="display bbox coordinates of detected objects", + ) + parser.add_argument( + "--save_labels", + action="store_true", + help="save detections bbox for each image in yolo format", + ) + parser.add_argument( + "--config_file", default="./cfg/yolov4.cfg", help="path to config file" + ) + parser.add_argument( + "--data_file", default="./cfg/coco.data", help="path to data file" + ) + parser.add_argument( + "--thresh", + type=float, + default=0.25, + help="remove detections with lower confidence", + ) return parser.parse_args() def check_arguments_errors(args): - assert 0 < args.thresh < 1, "Threshold should be a float between zero and one (non-inclusive)" + assert ( + 0 < args.thresh < 1 + ), "Threshold should be a float between zero and one (non-inclusive)" if not os.path.exists(args.config_file): - raise(ValueError("Invalid config path {}".format(os.path.abspath(args.config_file)))) + raise ( + ValueError( + "Invalid config path {}".format(os.path.abspath(args.config_file)) + ) + ) if not os.path.exists(args.weights): - raise(ValueError("Invalid weight path {}".format(os.path.abspath(args.weights)))) + raise ( + ValueError("Invalid weight path {}".format(os.path.abspath(args.weights))) + ) if not os.path.exists(args.data_file): - raise(ValueError("Invalid data file path {}".format(os.path.abspath(args.data_file)))) + raise ( + ValueError( + "Invalid data file path {}".format(os.path.abspath(args.data_file)) + ) + ) if args.input and not os.path.exists(args.input): - raise(ValueError("Invalid image path {}".format(os.path.abspath(args.input)))) + raise (ValueError("Invalid image path {}".format(os.path.abspath(args.input)))) def check_batch_shape(images, batch_size): """ - Image sizes should be the same width and height + Image sizes should be the same width and height """ shapes = [image.shape for image in images] if len(set(shapes)) > 1: @@ -66,17 +99,18 @@ def load_images(images_path): In other case, it's a folder, return a list with names of each jpg, jpeg and png file """ - input_path_extension = images_path.split('.')[-1] - if input_path_extension in ['jpg', 'jpeg', 'png']: + input_path_extension = images_path.split(".")[-1] + if input_path_extension in ["jpg", "jpeg", "png"]: return [images_path] elif input_path_extension == "txt": with open(images_path, "r") as f: return f.read().splitlines() else: - return glob.glob( - os.path.join(images_path, "*.jpg")) + \ - glob.glob(os.path.join(images_path, "*.png")) + \ - glob.glob(os.path.join(images_path, "*.jpeg")) + return ( + glob.glob(os.path.join(images_path, "*.jpg")) + + glob.glob(os.path.join(images_path, "*.png")) + + glob.glob(os.path.join(images_path, "*.jpeg")) + ) def prepare_batch(images, network, channels=3): @@ -86,13 +120,14 @@ def prepare_batch(images, network, channels=3): darknet_images = [] for image in images: image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - image_resized = cv2.resize(image_rgb, (width, height), - interpolation=cv2.INTER_LINEAR) + image_resized = cv2.resize( + image_rgb, (width, height), interpolation=cv2.INTER_LINEAR + ) custom_image = image_resized.transpose(2, 0, 1) darknet_images.append(custom_image) batch_array = np.concatenate(darknet_images, axis=0) - batch_array = np.ascontiguousarray(batch_array.flat, dtype=np.float32)/255.0 + batch_array = np.ascontiguousarray(batch_array.flat, dtype=np.float32) / 255.0 darknet_images = batch_array.ctypes.data_as(darknet.POINTER(darknet.c_float)) return darknet.IMAGE(width, height, channels, darknet_images) @@ -106,17 +141,21 @@ def image_detection(image_path, network, class_names, class_colors, thresh): image = cv2.imread(image_path) image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - image_resized = cv2.resize(image_rgb, (width, height), - interpolation=cv2.INTER_LINEAR) + image_resized = cv2.resize( + image_rgb, (width, height), interpolation=cv2.INTER_LINEAR + ) darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes()) - detections = darknet.detect_image(network, class_names, darknet_image, thresh=thresh) + detections = darknet.detect_image( + network, class_names, darknet_image, thresh=thresh + ) darknet.free_image(darknet_image) image = darknet.draw_boxes(detections, image_resized, class_colors) return cv2.cvtColor(image, cv2.COLOR_BGR2RGB), detections -#The following two methods were actually created by SoRo -#This is the main function call that ARTracker needs + +# The following two methods were actually created by SoRo +# This is the main function call that ARTracker needs def simple_detection(image, network, class_names, thresh): # Darknet doesn't accept numpy images. # Create one with image we reuse for each detect @@ -125,14 +164,18 @@ def simple_detection(image, network, class_names, thresh): darknet_image = darknet.make_image(width, height, 3) image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - image_resized = cv2.resize(image_rgb, (width, height), - interpolation=cv2.INTER_LINEAR) + image_resized = cv2.resize( + image_rgb, (width, height), interpolation=cv2.INTER_LINEAR + ) darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes()) - detections = darknet.detect_image(network, class_names, darknet_image, thresh=thresh) + detections = darknet.detect_image( + network, class_names, darknet_image, thresh=thresh + ) darknet.free_image(darknet_image) return detections - + + def complex_detection(image, network, class_names, class_colors, thresh): # Darknet doesn't accept numpy images. # Create one with image we reuse for each detect @@ -141,21 +184,43 @@ def complex_detection(image, network, class_names, class_colors, thresh): darknet_image = darknet.make_image(width, height, 3) image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - image_resized = cv2.resize(image_rgb, (width, height), - interpolation=cv2.INTER_LINEAR) + image_resized = cv2.resize( + image_rgb, (width, height), interpolation=cv2.INTER_LINEAR + ) darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes()) - detections = darknet.detect_image(network, class_names, darknet_image, thresh=thresh) + detections = darknet.detect_image( + network, class_names, darknet_image, thresh=thresh + ) darknet.free_image(darknet_image) image = darknet.draw_boxes(detections, image_resized, class_colors) return cv2.cvtColor(image, cv2.COLOR_BGR2RGB), detections -def batch_detection(network, images, class_names, class_colors, - thresh=0.25, hier_thresh=.5, nms=.45, batch_size=4): + +def batch_detection( + network, + images, + class_names, + class_colors, + thresh=0.25, + hier_thresh=0.5, + nms=0.45, + batch_size=4, +): image_height, image_width, _ = check_batch_shape(images, batch_size) darknet_images = prepare_batch(images, network) - batch_detections = darknet.network_predict_batch(network, darknet_images, batch_size, image_width, - image_height, thresh, hier_thresh, None, 0, 0) + batch_detections = darknet.network_predict_batch( + network, + darknet_images, + batch_size, + image_width, + image_height, + thresh, + hier_thresh, + None, + 0, + 0, + ) batch_predictions = [] for idx in range(batch_size): num = batch_detections[idx].num @@ -173,8 +238,9 @@ def image_classification(image, network, class_names): width = darknet.network_width(network) height = darknet.network_height(network) image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) - image_resized = cv2.resize(image_rgb, (width, height), - interpolation=cv2.INTER_LINEAR) + image_resized = cv2.resize( + image_rgb, (width, height), interpolation=cv2.INTER_LINEAR + ) darknet_image = darknet.make_image(width, height, 3) darknet.copy_image_from_bytes(darknet_image, image_resized.tobytes()) detections = darknet.predict_image(network, darknet_image) @@ -189,7 +255,7 @@ def convert2relative(image, bbox): """ x, y, w, h = bbox height, width, _ = image.shape - return x/width, y/height, w/width, h/height + return x / width, y / height, w / width, h / height def save_annotations(name, image, detections, class_names): @@ -201,7 +267,11 @@ def save_annotations(name, image, detections, class_names): for label, confidence, bbox in detections: x, y, w, h = convert2relative(image, bbox) label = class_names.index(label) - f.write("{} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f}\n".format(label, x, y, w, h, float(confidence))) + f.write( + "{} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f}\n".format( + label, x, y, w, h, float(confidence) + ) + ) def batch_detection_example(): @@ -210,15 +280,16 @@ def batch_detection_example(): batch_size = 3 random.seed(3) # deterministic bbox colors network, class_names, class_colors = darknet.load_network( - args.config_file, - args.data_file, - args.weights, - batch_size=batch_size + args.config_file, args.data_file, args.weights, batch_size=batch_size ) - image_names = ['data/horses.jpg', 'data/horses.jpg', 'data/eagle.jpg'] + image_names = ["data/horses.jpg", "data/horses.jpg", "data/eagle.jpg"] images = [cv2.imread(image) for image in image_names] - images, detections, = batch_detection(network, images, class_names, - class_colors, batch_size=batch_size) + ( + images, + detections, + ) = batch_detection( + network, images, class_names, class_colors, batch_size=batch_size + ) for name, image in zip(image_names, images): cv2.imwrite(name.replace("data/", ""), image) print(detections) @@ -230,10 +301,7 @@ def main(): random.seed(3) # deterministic bbox colors network, class_names, class_colors = darknet.load_network( - args.config_file, - args.data_file, - args.weights, - batch_size=args.batch_size + args.config_file, args.data_file, args.weights, batch_size=args.batch_size ) images = load_images(args.input) @@ -250,15 +318,15 @@ def main(): prev_time = time.time() image, detections = image_detection( image_name, network, class_names, class_colors, args.thresh - ) + ) if args.save_labels: save_annotations(image_name, image, detections, class_names) darknet.print_detections(detections, args.ext_output) - fps = int(1/(time.time() - prev_time)) + fps = int(1 / (time.time() - prev_time)) print("FPS: {}".format(fps)) if not args.dont_show: - cv2.imshow('Inference', image) - if cv2.waitKey() & 0xFF == ord('q'): + cv2.imshow("Inference", image) + if cv2.waitKey() & 0xFF == ord("q"): break index += 1 diff --git a/Autonomous/findFocalLength.py b/Autonomous/findFocalLength.py index d9454e5..8387177 100755 --- a/Autonomous/findFocalLength.py +++ b/Autonomous/findFocalLength.py @@ -1,6 +1,6 @@ #!/usr/bin/python3 -#This programs returns the camera focal length assuming tag is tag 0, 20cm wide and 100cm away +# This programs returns the camera focal length assuming tag is tag 0, 20cm wide and 100cm away import cv2 import cv2.aruco as aruco import numpy as np @@ -8,24 +8,27 @@ import configparser from time import sleep -#opens the camera -cam = cv2.VideoCapture('/dev/video0') +# opens the camera +cam = cv2.VideoCapture("/dev/video0") if not cam.isOpened(): print("Camera not connected") exit(-1) -#sets up the config stuff +# sets up the config stuff config = configparser.ConfigParser(allow_no_value=True) -config.read(os.path.dirname(__file__) + '/config.ini') +config.read(os.path.dirname(__file__) + "/config.ini") -#sets the resolution and 4cc -format = config['ARTRACKER']['FORMAT'] -cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(format[0], format[1], format[2], format[3])) +# sets the resolution and 4cc +format = config["ARTRACKER"]["FORMAT"] +cam.set( + cv2.CAP_PROP_FOURCC, + cv2.VideoWriter_fourcc(format[0], format[1], format[2], format[3]), +) -dppH = float(config['ARTRACKER']['DEGREES_PER_PIXEL']) -dppV = float(config['ARTRACKER']['VDEGREES_PER_PIXEL']) -width = int(config['ARTRACKER']['FRAME_WIDTH']) -height = int(config['ARTRACKER']['FRAME_HEIGHT']) +dppH = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) +dppV = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) +width = int(config["ARTRACKER"]["FRAME_WIDTH"]) +height = int(config["ARTRACKER"]["FRAME_HEIGHT"]) cam.set(cv2.CAP_PROP_FRAME_WIDTH, width) cam.set(cv2.CAP_PROP_FRAME_HEIGHT, height) @@ -33,33 +36,33 @@ while True: try: - #takes image and detects markers + # takes image and detects markers ret, image = cam.read() corners, markerIDs, rejected = aruco.detectMarkers(image, tagDict) - + tagWidth = 0 if not markerIDs is None and markerIDs[0] == 4: tagWidth = corners[0][0][1][0] - corners[0][0][0][0] - - #IMPORTANT: Assumes tag is 20cm wide and 100cm away + + # IMPORTANT: Assumes tag is 20cm wide and 100cm away focalLength = (tagWidth * 100) / 20 print("Focal length: ", focalLength) - centerXMarker = (corners[0][0][1][0] + corners[0][0][0][0]) / 2 - angleToMarkerH = dppH * (centerXMarker - width/2) - print('Horizontal angle: ', angleToMarkerH) - - centerYMarker = (corners[0][0][0][1] + corners[0][0][2][1]) / 2 - angleToMarkerV = -1 *dppV * (centerYMarker - height/2) - print('Vertical angle: ', angleToMarkerV) + centerXMarker = (corners[0][0][1][0] + corners[0][0][0][0]) / 2 + angleToMarkerH = dppH * (centerXMarker - width / 2) + print("Horizontal angle: ", angleToMarkerH) + + centerYMarker = (corners[0][0][0][1] + corners[0][0][2][1]) / 2 + angleToMarkerV = -1 * dppV * (centerYMarker - height / 2) + print("Vertical angle: ", angleToMarkerV) else: print("Nothing found") - - sleep(.1) - + + sleep(0.1) + except KeyboardInterrupt: break except: - pass #When running multiple times in a row, must cycle through a few times - -cam.release() #releases the camera + pass # When running multiple times in a row, must cycle through a few times + +cam.release() # releases the camera diff --git a/Autonomous/libs/ARTracker.py b/Autonomous/libs/ARTracker.py index 8ee90ae..e07223f 100644 --- a/Autonomous/libs/ARTracker.py +++ b/Autonomous/libs/ARTracker.py @@ -1,36 +1,43 @@ import cv2 import cv2.aruco as aruco -import numpy as np import configparser import sys -from time import sleep import os -''' -darknetPath = os.path.dirname(os.path.abspath(__file__)) + '/../YOLO/darknet/' -sys.path.append(darknetPath) -from darknet_images import * +import darknet +from YOLO.darknet.darknet_images import simple_detection, complex_detection from darknet import load_network -''' + +# FIXME: why is this added to path wtf +darknetPath: str = os.path.dirname(os.path.abspath(__file__)) + "/../YOLO/darknet/" +sys.path.append(darknetPath) + class ARTracker: + """_summary_ + A helper class to aid with tracking ARUCO trackers and navigation. + YOLO object detection may be used for drawing bounding boxes. (requires CUDA) + """ # Constructor - #Cameras should be a list of file paths to cameras that are to be used - #set write to True to write to disk what the cameras are seeing - #set useYOLO to True to use yolo when attempting to detect the ar tags - def __init__(self, cameras, write=False, useYOLO = False, configFile="config.ini"): - self.write=write + def __init__(self, cameras, write=False, useYOLO=False, configFile="config.ini"): + """_summary_ + Cameras should be a list of file paths to cameras that are to be used. + Set write to True to write to disk what the cameras are seeing. + Set useYOLO to True to use yolo when attempting to detect the ar tags. + """ + + self.write = write self.distanceToMarker = -1 self.angleToMarker = -999.9 self.index1 = -1 self.index2 = -1 self.useYOLO = useYOLO self.cameras = cameras - + # Open the config file config = configparser.ConfigParser(allow_no_value=True) if not config.read(configFile): - print(f"ERROR OPENING AR CONFIG:", end="") + print("ERROR OPENING AR CONFIG:", end="") if os.path.isabs(configFile): print(configFile) else: @@ -38,185 +45,226 @@ def __init__(self, cameras, write=False, useYOLO = False, configFile="config.ini exit(-2) # Set variables from the config file - self.degreesPerPixel = float(config['ARTRACKER']['DEGREES_PER_PIXEL']) - self.vDegreesPerPixel = float(config['ARTRACKER']['VDEGREES_PER_PIXEL']) - self.focalLength = float(config['ARTRACKER']['FOCAL_LENGTH']) - self.focalLength30H = float(config['ARTRACKER']['FOCAL_LENGTH30H']) - self.focalLength30V = float(config['ARTRACKER']['FOCAL_LENGTH30V']) - self.knownMarkerWidth = float(config['ARTRACKER']['KNOWN_TAG_WIDTH']) - self.format = config['ARTRACKER']['FORMAT'] - self.frameWidth = int(config['ARTRACKER']['FRAME_WIDTH']) - self.frameHeight = int(config['ARTRACKER']['FRAME_HEIGHT']) - - #sets up yolo + self.degreesPerPixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) + self.vDegreesPerPixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) + self.focalLength = float(config["ARTRACKER"]["FOCAL_LENGTH"]) + self.focalLength30H = float(config["ARTRACKER"]["FOCAL_LENGTH30H"]) + self.focalLength30V = float(config["ARTRACKER"]["FOCAL_LENGTH30V"]) + self.knownMarkerWidth = float(config["ARTRACKER"]["KNOWN_TAG_WIDTH"]) + self.format = config["ARTRACKER"]["FORMAT"] + self.frameWidth = int(config["ARTRACKER"]["FRAME_WIDTH"]) + self.frameHeight = int(config["ARTRACKER"]["FRAME_HEIGHT"]) + + # sets up yolo if useYOLO: os.chdir(darknetPath) - weights = config['YOLO']['WEIGHTS'] - cfg = config['YOLO']['CFG'] - data = config['YOLO']['DATA'] - self.thresh = float(config['YOLO']['THRESHOLD']) - self.network, self.class_names, self.class_colors = load_network(cfg, data, weights, 1) + weights = config["YOLO"]["WEIGHTS"] + cfg = config["YOLO"]["CFG"] + data = config["YOLO"]["DATA"] + self.thresh = float(config["YOLO"]["THRESHOLD"]) + self.network, self.class_names, self.class_colors = load_network( + cfg, data, weights, 1 + ) os.chdir(os.path.dirname(os.path.abspath(__file__))) - + self.networkWidth = darknet.network_width(self.network) self.networkHeight = darknet.network_height(self.network) # Initialize video writer, fps is set to 5 if self.write: - self.videoWriter = cv2.VideoWriter("autonomous.avi", cv2.VideoWriter_fourcc( - self.format[0], self.format[1], self.format[2], self.format[3]), 5, (self.frameWidth, self.frameHeight), False) - + self.videoWriter = cv2.VideoWriter( + "autonomous.avi", + cv2.VideoWriter_fourcc( + self.format[0], self.format[1], self.format[2], self.format[3] + ), + 5, + (self.frameWidth, self.frameHeight), + False, + ) + # Set the ar marker dictionary self.markerDict = aruco.Dictionary_get(aruco.DICT_4X4_50) - + # Initialize cameras - self.caps=[] + self.caps = [] if isinstance(self.cameras, int): self.cameras = [self.cameras] for i in range(0, len(self.cameras)): - #Makes sure the camera actually connects + # Makes sure the camera actually connects while True: cam = cv2.VideoCapture(self.cameras[i]) if not cam.isOpened(): - print(f"!!!!!!!!!!!!!!!!!!!!!!!!!!Camera ", i, " did not open!!!!!!!!!!!!!!!!!!!!!!!!!!") + print( + f"!!!!!!!!!!!!!!!!!!!!!!!!!! \ + Camera {i} did not open!!!!!!!!!!!!!!!!!!!!!!!!!!", + ) cam.release() continue cam.set(cv2.CAP_PROP_FRAME_HEIGHT, self.frameHeight) cam.set(cv2.CAP_PROP_FRAME_WIDTH, self.frameWidth) - cam.set(cv2.CAP_PROP_BUFFERSIZE, 1) # greatly speeds up the program but the writer is a bit wack because of this - cam.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(self.format[0], self.format[1], self.format[2], self.format[3])) - #ret, testIm = self.caps[i].read()[0]: + cam.set( + cv2.CAP_PROP_BUFFERSIZE, 1 + ) # speeds up the program but the writer is a bit wack because of this + cam.set( + cv2.CAP_PROP_FOURCC, + cv2.VideoWriter_fourcc( + self.format[0], self.format[1], self.format[2], self.format[3] + ), + ) + # ret, testIm = self.caps[i].read()[0]: if not cam.read()[0]: cam.release() else: self.caps.append(cam) break - - #helper method to convert YOLO detections into the aruco corners format - def _convertToCorners(self,detections, numCorners): + # helper method to convert YOLO detections into the aruco corners format + def _convertToCorners(self, detections, numCorners): corners = [] xCoef = self.frameWidth / self.networkWidth yCoef = self.frameHeight / self.networkHeight if len(detections) < numCorners: - print('ERROR, convertToCorners not used correctly') + print("ERROR, convertToCorners not used correctly") raise ValueError for i in range(0, numCorners): - tagData = list(detections[i][2]) #Gets the x, y, width, height - - #YOLO resizes the image so this sizes it back to what we're exepcting + tagData = list(detections[i][2]) # Gets the x, y, width, height + + # YOLO resizes the image so this sizes it back to what we're exepcting tagData[0] *= xCoef - tagData[1]*= yCoef + tagData[1] *= yCoef tagData[2] *= xCoef tagData[3] *= yCoef - - #Gets the corners - topLeft = [tagData[0] - tagData[2]/2, tagData[1] - tagData[3]/2] - topRight = [tagData[0] + tagData[2]/2, tagData[1] - tagData[3]/2] - bottomRight = [tagData[0] + tagData[2]/2, tagData[1] + tagData[3]/2] - bottomLeft = [tagData[0] - tagData[2]/2, tagData[1] + tagData[3]/2] - - #appends the corners with the same format as aruco + + # Gets the corners + topLeft = [tagData[0] - tagData[2] / 2, tagData[1] - tagData[3] / 2] + topRight = [tagData[0] + tagData[2] / 2, tagData[1] - tagData[3] / 2] + bottomRight = [tagData[0] + tagData[2] / 2, tagData[1] + tagData[3] / 2] + bottomLeft = [tagData[0] - tagData[2] / 2, tagData[1] + tagData[3] / 2] + + # appends the corners with the same format as aruco corners.append([[topLeft, topRight, bottomRight, bottomLeft]]) - + return corners - - #id1 is the main ar tag to track, id2 is if you're looking at a gatepost, image is the image to analyze + + # id1 is the main ar tag to track, id2 is if you're looking at a gatepost, + # image is the image to analyze def markerFound(self, id1, image, id2=-1): # converts to grayscale - cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) - + cv2.cvtColor(image, cv2.COLOR_RGB2GRAY, image) + self.index1 = -1 self.index2 = -1 - bw = image #will hold the black and white image - # tries converting to b&w using different different cutoffs to find the perfect one for the current lighting + bw = image # will hold the black and white image + + # tries converting to b&w using different different cutoffs to + # find the perfect one for the current lighting for i in range(40, 221, 60): - bw = cv2.threshold(image,i,255, cv2.THRESH_BINARY)[1] - (self.corners, self.markerIDs, self.rejected) = aruco.detectMarkers(bw, self.markerDict) - if not (self.markerIDs is None): - print('', end='') #I have not been able to reproduce an error when I have a print statement here so I'm leaving it in - if id2==-1: #single post - self.index1 = -1 + bw = cv2.threshold(image, i, 255, cv2.THRESH_BINARY)[1] + (self.corners, self.markerIDs, self.rejected) = aruco.detectMarkers( + bw, self.markerDict + ) + if self.markerIDs is not None: + print( + "", end="" + ) # I have not been able to reproduce an error when I have a print statement here so I'm leaving it in + if id2 == -1: # single post + self.index1 = -1 # this just checks to make sure that it found the right marker - for m in range(len(self.markerIDs)): + for m in range(len(self.markerIDs)): if self.markerIDs[m] == id1: - self.index1 = m - break - + self.index1 = m + break + if self.index1 != -1: print("Found the correct marker!") if self.write: - self.videoWriter.write(bw) #purely for debug + self.videoWriter.write(bw) # purely for debug cv2.waitKey(1) - break - + break + else: - print("Found a marker but was not the correct one") - - else: #gate post + print("Found a marker but was not the correct one") + + else: # gate post self.index1 = -1 self.index2 = -1 - if len(self.markerIDs) == 1: - print('Only found marker ', self.markerIDs[0]) + if len(self.markerIDs) == 1: + print("Only found marker ", self.markerIDs[0]) else: - for j in range(len(self.markerIDs) - 1, -1,-1): #I trust the biggest markers the most + for j in range( + len(self.markerIDs) - 1, -1, -1 + ): # I trust the biggest markers the most if self.markerIDs[j][0] == id1: - self.index1 = j + self.index1 = j elif self.markerIDs[j][0] == id2: self.index2 = j if self.index1 != -1 and self.index2 != -1: - print('Found both markers!') + print("Found both markers!") if self.write: - self.videoWriter.write(bw) #purely for debug + self.videoWriter.write(bw) # purely for debug cv2.waitKey(1) - break - - if i == 220: #did not find any AR markers with any b&w cutoff using aruco - #Checks to see if yolo can find a tag + break + + if i == 220: # did not find any AR markers with any b&w cutoff using aruco + # Checks to see if yolo can find a tag if self.useYOLO: detections = [] if not self.write: - #this is a simpler detection function that doesn't return the image - detections = simple_detection(image, self.network, self.class_names, self.thresh) + # this is a simpler detection function that doesn't return the image + detections = simple_detection( + image, self.network, self.class_names, self.thresh + ) else: - #more complex detection that returns the image to be written - image, detections = complex_detection(image, self.network, self.class_names, self.class_colors, self.thresh) - #cv2.imwrite('ar.jpg', image) + # more complex detection that returns the image to be written + image, detections = complex_detection( + image, + self.network, + self.class_names, + self.class_colors, + self.thresh, + ) + # cv2.imwrite('ar.jpg', image) for d in detections: print(d) - + if id2 == -1 and len(detections) > 0: self.corners = self._convertToCorners(detections, 1) - self.index1 = 0 #Takes the highest confidence ar tag + self.index1 = 0 # Takes the highest confidence ar tag if self.write: - self.videoWriter.write(image) #purely for debug - cv2.waitKey(1) + self.videoWriter.write(image) # purely for debug + cv2.waitKey(1) elif len(detections) > 1: self.corners = self._convertToCorners(detections, 2) - self.index1 = 0 #takes the two highest confidence ar tags + self.index1 = 0 # takes the two highest confidence ar tags self.index2 = 1 if self.write: - self.videoWriter.write(image) #purely for debug + self.videoWriter.write(image) # purely for debug cv2.waitKey(1) - print(self.corners) - - #Not even YOLO saw anything - if self.index1 == -1 or (self.index2 == -1 and id2 != -1): + print(self.corners) + + # Not even YOLO saw anything + if self.index1 == -1 or (self.index2 == -1 and id2 != -1): if self.write: - self.videoWriter.write(image) - #cv2.imshow('window', image) + self.videoWriter.write(image) + # cv2.imshow('window', image) cv2.waitKey(1) - self.distanceToMarker = -1 - self.angleToMarker = -999 - return False - + self.distanceToMarker = -1 + self.angleToMarker = -999 + return False + if id2 == -1: - centerXMarker = (self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] + \ - self.corners[self.index1][0][2][0] + self.corners[self.index1][0][3][0]) / 4 + centerXMarker = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel - self.angleToMarker = self.degreesPerPixel * (centerXMarker - self.frameWidth/2) - - ''' + self.angleToMarker = self.degreesPerPixel * ( + centerXMarker - self.frameWidth / 2 + ) + + """ distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker focalLength = focal length at 0 degrees horizontal and 0 degrees vertical focalLength30H = focal length at 30 degreees horizontal and 0 degrees vertical @@ -226,61 +274,133 @@ def markerFound(self, id1, image, id2=-1): + (vertical angle to marker / 30) * (focalLength30V - focalLength) If focalLength30H and focalLength30V both equal focalLength then realFocalLength = focalLength which is good for non huddly cameras Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better - ''' + """ hAngleToMarker = abs(self.angleToMarker) - centerYMarker = (self.corners[self.index1][0][0][1] + self.corners[self.index1][0][1][1] + \ - self.corners[self.index1][0][2][1] + self.corners[self.index1][0][3][1]) / 4 - vAngleToMarker = abs(self.vDegreesPerPixel * (centerYMarker - self.frameHeight/2)) - realFocalLength = self.focalLength + (hAngleToMarker/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker/30) * (self.focalLength30V - self.focalLength) - widthOfMarker = ((self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - self.distanceToMarker = (self.knownMarkerWidth * realFocalLength) / widthOfMarker - + centerYMarker = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + vAngleToMarker = abs( + self.vDegreesPerPixel * (centerYMarker - self.frameHeight / 2) + ) + realFocalLength = ( + self.focalLength + + (hAngleToMarker / 30) * (self.focalLength30H - self.focalLength) + + (vAngleToMarker / 30) * (self.focalLength30V - self.focalLength) + ) + widthOfMarker = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + self.distanceToMarker = ( + self.knownMarkerWidth * realFocalLength + ) / widthOfMarker + else: - centerXMarker1 = (self.corners[self.index1][0][0][0] + self.corners[self.index1][0][1][0] + \ - self.corners[self.index1][0][2][0] + self.corners[self.index1][0][3][0]) / 4 - centerXMarker2 = (self.corners[self.index2][0][0][0] + self.corners[self.index2][0][1][0] + \ - self.corners[self.index2][0][2][0] + self.corners[self.index2][0][3][0]) / 4 - self.angleToMarker = self.degreesPerPixel * ((centerXMarker1 + centerXMarker2)/2 - self.frameWidth/2) - - hAngleToMarker1 = abs(self.vDegreesPerPixel * (centerXMarker1 - self.frameWidth/2)) - hAngleToMarker2 = abs(self.vDegreesPerPixel * (centerXMarker2 - self.frameWidth/2)) - centerYMarker1 = (self.corners[self.index1][0][0][1] + self.corners[self.index1][0][1][1] + \ - self.corners[self.index1][0][2][1] + self.corners[self.index1][0][3][1]) / 4 - centerYMarker2 = (self.corners[self.index2][0][0][1] + self.corners[self.index2][0][1][1] + \ - self.corners[self.index2][0][2][1] + self.corners[self.index2][0][3][1]) / 4 - vAngleToMarker1 = abs(self.vDegreesPerPixel * (centerYMarker1 - self.frameHeight/2)) - vAngleToMarker2 = abs(self.vDegreesPerPixel * (centerYMarker2 - self.frameHeight/2)) - realFocalLength1 = self.focalLength + (hAngleToMarker1/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker1/30) * (self.focalLength30V - self.focalLength) - realFocalLength2 = self.focalLength + (hAngleToMarker2/30) * (self.focalLength30H - self.focalLength) + \ - (vAngleToMarker2/30) * (self.focalLength30V - self.focalLength) - widthOfMarker1 = ((self.corners[self.index1][0][1][0] - self.corners[self.index1][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - widthOfMarker2 = ((self.corners[self.index2][0][1][0] - self.corners[self.index2][0][0][0]) + \ - (self.corners[self.index1][0][2][0] - self.corners[self.index1][0][3][0])) / 2 - - #distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - distanceToMarker1 = (self.knownMarkerWidth * realFocalLength1) / widthOfMarker1 - distanceToMarker2 = (self.knownMarkerWidth * realFocalLength2) / widthOfMarker2 + centerXMarker1 = ( + self.corners[self.index1][0][0][0] + + self.corners[self.index1][0][1][0] + + self.corners[self.index1][0][2][0] + + self.corners[self.index1][0][3][0] + ) / 4 + centerXMarker2 = ( + self.corners[self.index2][0][0][0] + + self.corners[self.index2][0][1][0] + + self.corners[self.index2][0][2][0] + + self.corners[self.index2][0][3][0] + ) / 4 + self.angleToMarker = self.degreesPerPixel * ( + (centerXMarker1 + centerXMarker2) / 2 - self.frameWidth / 2 + ) + + hAngleToMarker1 = abs( + self.vDegreesPerPixel * (centerXMarker1 - self.frameWidth / 2) + ) + hAngleToMarker2 = abs( + self.vDegreesPerPixel * (centerXMarker2 - self.frameWidth / 2) + ) + centerYMarker1 = ( + self.corners[self.index1][0][0][1] + + self.corners[self.index1][0][1][1] + + self.corners[self.index1][0][2][1] + + self.corners[self.index1][0][3][1] + ) / 4 + centerYMarker2 = ( + self.corners[self.index2][0][0][1] + + self.corners[self.index2][0][1][1] + + self.corners[self.index2][0][2][1] + + self.corners[self.index2][0][3][1] + ) / 4 + vAngleToMarker1 = abs( + self.vDegreesPerPixel * (centerYMarker1 - self.frameHeight / 2) + ) + vAngleToMarker2 = abs( + self.vDegreesPerPixel * (centerYMarker2 - self.frameHeight / 2) + ) + realFocalLength1 = ( + self.focalLength + + (hAngleToMarker1 / 30) * (self.focalLength30H - self.focalLength) + + (vAngleToMarker1 / 30) * (self.focalLength30V - self.focalLength) + ) + realFocalLength2 = ( + self.focalLength + + (hAngleToMarker2 / 30) * (self.focalLength30H - self.focalLength) + + (vAngleToMarker2 / 30) * (self.focalLength30V - self.focalLength) + ) + widthOfMarker1 = ( + ( + self.corners[self.index1][0][1][0] + - self.corners[self.index1][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + widthOfMarker2 = ( + ( + self.corners[self.index2][0][1][0] + - self.corners[self.index2][0][0][0] + ) + + ( + self.corners[self.index1][0][2][0] + - self.corners[self.index1][0][3][0] + ) + ) / 2 + + # distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker + distanceToMarker1 = ( + self.knownMarkerWidth * realFocalLength1 + ) / widthOfMarker1 + distanceToMarker2 = ( + self.knownMarkerWidth * realFocalLength2 + ) / widthOfMarker2 print(f"1: {distanceToMarker1}, 2: {distanceToMarker2}") self.distanceToMarker = (distanceToMarker1 + distanceToMarker2) / 2 - - return True - - ''' + + return True + + """ id1 is the marker you want to look for specify id2 if you want to look for a gate cameras=number of cameras to check. -1 for all of them - ''' + """ + def findMarker(self, id1, id2=-1, cameras=-1): if cameras == -1: - cameras=len(self.caps) - + cameras = len(self.caps) + for i in range(cameras): ret, frame = self.caps[i].read() - if self.markerFound(id1, frame, id2=id2): + if self.markerFound(id1, frame, id2=id2): return True return False diff --git a/Autonomous/main.py b/Autonomous/main.py index cfde983..3271e87 100644 --- a/Autonomous/main.py +++ b/Autonomous/main.py @@ -1,5 +1,6 @@ import os -path = (os.path.dirname(os.path.abspath(__file__))) + +path = os.path.dirname(os.path.abspath(__file__)) import argparse import configparser from libs import UDPOut @@ -7,34 +8,51 @@ import threading from time import sleep -mbedIP='10.0.0.101' -mbedPort=1001 +mbedIP = "10.0.0.101" +mbedPort = 1001 flashing = False + def flash(): while flashing: - UDPOut.sendLED(mbedIP, mbedPort, 'g') - sleep(.2) - UDPOut.sendLED(mbedIP, mbedPort, 'o') - sleep(.2) + UDPOut.sendLED(mbedIP, mbedPort, "g") + sleep(0.2) + UDPOut.sendLED(mbedIP, mbedPort, "o") + sleep(0.2) + argParser = argparse.ArgumentParser() -argParser.add_argument("cameraInput", type=int, help="takes a number representing which camera to use") -argParser.add_argument("-id", "--ids", type=int, help="takes either 1 or 2 id values, defaults to -1 if id not assigned", nargs='+') -argParser.add_argument("-ll", "--latLong", type=str, help="takes a filename for a text file, then reads that file for latlong coordinates") +argParser.add_argument( + "cameraInput", type=int, help="takes a number representing which camera to use" +) +argParser.add_argument( + "-id", + "--ids", + type=int, + help="takes either 1 or 2 id values, defaults to -1 if id not assigned", + nargs="+", +) +argParser.add_argument( + "-ll", + "--latLong", + type=str, + help="takes a filename for a text file, then reads that file for latlong coordinates", +) args = argParser.parse_args() -#Gets a list of coordinates from user and drives to them and then tracks the tag -#Set id1 to -1 if not looking for a tag + + +# Gets a list of coordinates from user and drives to them and then tracks the tag +# Set id1 to -1 if not looking for a tag def drive(rover): global flashing - idList = [-1,-1] + idList = [-1, -1] locations = [] if args.ids is not None: for i in range(len(args.ids)): idList[i] = args.ids[i] - + id1 = idList[0] id2 = idList[1] @@ -44,45 +62,57 @@ def drive(rover): for line in f: lineNum += 1 try: - coords = [float(item.replace('\ufeff',"")) for item in line.strip().split()] + coords = [ + float(item.replace("\ufeff", "")) + for item in line.strip().split() + ] except: - print("Parse Error on line " + str(lineNum) + ": Please enter ") + print( + "Parse Error on line " + + str(lineNum) + + ": Please enter " + ) break else: if len(coords) != 2: - print("Error on line " + str(lineNum) + ": Insufficient number of coordinates. Please enter ") - break + print( + "Error on line " + + str(lineNum) + + ": Insufficient number of coordinates. Please enter " + ) + break locations.append(coords) f.close() flashing = False - UDPOut.sendLED(mbedIP, mbedPort, 'r') - found = rover.driveAlongCoordinates(locations,id1, id2) - + UDPOut.sendLED(mbedIP, mbedPort, "r") + found = rover.driveAlongCoordinates(locations, id1, id2) + if id1 != -1: rover.trackARMarker(id1, id2) - - flashing=True + + flashing = True lights = threading.Thread(target=flash) lights.start() - #UDPOut.sendLED(mbedIP, mbedPort, 'g') + # UDPOut.sendLED(mbedIP, mbedPort, 'g') + if __name__ == "__main__": os.chdir(path) print(os.getcwd()) - #user input (args from system) + # user input (args from system) if args.cameraInput is None: print("ERROR: must at least specify one camera") exit(-1) - - #gets the mbed ip and port + + # gets the mbed ip and port config = configparser.ConfigParser(allow_no_value=True) - if not config.read('config.ini'): + if not config.read("config.ini"): print("DID NOT OPEN CONFIG") exit(-2) - mbedIP = str(config['CONFIG']['MBED_IP']) - mbedPort = int(config['CONFIG']['MBED_PORT']) + mbedIP = str(config["CONFIG"]["MBED_IP"]) + mbedPort = int(config["CONFIG"]["MBED_PORT"]) rover = Drive.Drive(50, args.cameraInput) - - drive(rover) \ No newline at end of file + + drive(rover) From 8f36436d01cadae74a9f039b5473790f7e56f65c Mon Sep 17 00:00:00 2001 From: Barrett Ray Date: Thu, 15 Jun 2023 10:03:32 -0500 Subject: [PATCH 3/3] fix: Add correct deps for ARTracker --- Autonomous/config.ini | 2 +- Autonomous/libs/ARTracker.py | 146 ++++++++++++++------------ Autonomous/libs/pyproject.toml | 3 +- Autonomous/libs/requirements-dev.lock | 1 - Autonomous/libs/requirements.lock | 1 - 5 files changed, 83 insertions(+), 70 deletions(-) diff --git a/Autonomous/config.ini b/Autonomous/config.ini index 840201f..6526995 100644 --- a/Autonomous/config.ini +++ b/Autonomous/config.ini @@ -12,7 +12,7 @@ FOCAL_LENGTH=435 FOCAL_LENGTH30H=590 FOCAL_LENGTH30V=470 KNOWN_TAG_WIDTH=20 -FORMAT=MJPG +FORMAT=MJPG # some fourcc format: https://web.archive.org/web/20210130070315/https://www.fourcc.org/codecs.php FRAME_WIDTH=1280 FRAME_HEIGHT=720 MAIN_CAMERA=2.3 diff --git a/Autonomous/libs/ARTracker.py b/Autonomous/libs/ARTracker.py index e07223f..a7b73ab 100644 --- a/Autonomous/libs/ARTracker.py +++ b/Autonomous/libs/ARTracker.py @@ -1,11 +1,11 @@ -import cv2 -import cv2.aruco as aruco -import configparser +from cv2 import aruco, cv2 +from configparser import ConfigParser import sys import os import darknet from YOLO.darknet.darknet_images import simple_detection, complex_detection from darknet import load_network +from typing import List # FIXME: why is this added to path wtf darknetPath: str = os.path.dirname(os.path.abspath(__file__)) + "/../YOLO/darknet/" @@ -19,44 +19,52 @@ class ARTracker: """ # Constructor - def __init__(self, cameras, write=False, useYOLO=False, configFile="config.ini"): + def __init__( + self, + camera_list: List[str], + write_to_disk: bool = False, + use_YOLO_tracking: bool = False, + config_file_path: str = "config.ini", + ): """_summary_ Cameras should be a list of file paths to cameras that are to be used. Set write to True to write to disk what the cameras are seeing. Set useYOLO to True to use yolo when attempting to detect the ar tags. """ - self.write = write - self.distanceToMarker = -1 - self.angleToMarker = -999.9 - self.index1 = -1 - self.index2 = -1 - self.useYOLO = useYOLO - self.cameras = cameras + self.write_to_disk: bool = write_to_disk + self.distanceToMarker: float = -1 + self.angleToMarker: float = -999.9 # i feel this is not a good default ~bray + self.index1 = -1 # the ARUCO tag to track + self.index2 = -1 # (optional) second ARUCO tag used for gates (two qr sticks) + self.use_YOLO_tracking = use_YOLO_tracking + # list of paths to cameras (i.e. /dev/video0) + self.camera_list: List[str] = camera_list # Open the config file - config = configparser.ConfigParser(allow_no_value=True) - if not config.read(configFile): + config = ConfigParser(allow_no_value=True) + if not config.read(config_file_path): print("ERROR OPENING AR CONFIG:", end="") - if os.path.isabs(configFile): - print(configFile) + if os.path.isabs(config_file_path): + print(config_file_path) else: - print("{os.getcwd()}/{configFile}") + print("{os.getcwd()}/{config_file_path}") exit(-2) # Set variables from the config file - self.degreesPerPixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) - self.vDegreesPerPixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) - self.focalLength = float(config["ARTRACKER"]["FOCAL_LENGTH"]) + # TODO: parse to data structure instead (serde-like?) + self.degrees_per_pixel = float(config["ARTRACKER"]["DEGREES_PER_PIXEL"]) + self.vdegrees_per_pixel = float(config["ARTRACKER"]["VDEGREES_PER_PIXEL"]) + self.focal_length = float(config["ARTRACKER"]["FOCAL_LENGTH"]) self.focalLength30H = float(config["ARTRACKER"]["FOCAL_LENGTH30H"]) self.focalLength30V = float(config["ARTRACKER"]["FOCAL_LENGTH30V"]) self.knownMarkerWidth = float(config["ARTRACKER"]["KNOWN_TAG_WIDTH"]) - self.format = config["ARTRACKER"]["FORMAT"] + self.format: str = config["ARTRACKER"]["FORMAT"] self.frameWidth = int(config["ARTRACKER"]["FRAME_WIDTH"]) self.frameHeight = int(config["ARTRACKER"]["FRAME_HEIGHT"]) - # sets up yolo - if useYOLO: + # Set up YOLO tracking (bounds detection) + if use_YOLO_tracking: os.chdir(darknetPath) weights = config["YOLO"]["WEIGHTS"] cfg = config["YOLO"]["CFG"] @@ -71,7 +79,7 @@ def __init__(self, cameras, write=False, useYOLO=False, configFile="config.ini") self.networkHeight = darknet.network_height(self.network) # Initialize video writer, fps is set to 5 - if self.write: + if self.write_to_disk: self.videoWriter = cv2.VideoWriter( "autonomous.avi", cv2.VideoWriter_fourcc( @@ -87,12 +95,12 @@ def __init__(self, cameras, write=False, useYOLO=False, configFile="config.ini") # Initialize cameras self.caps = [] - if isinstance(self.cameras, int): - self.cameras = [self.cameras] - for i in range(0, len(self.cameras)): + if isinstance(self.camera_list, int): + self.camera_list = [self.camera_list] + for i in range(0, len(self.camera_list)): # Makes sure the camera actually connects while True: - cam = cv2.VideoCapture(self.cameras[i]) + cam = cv2.VideoCapture(self.camera_list[i]) if not cam.isOpened(): print( f"!!!!!!!!!!!!!!!!!!!!!!!!!! \ @@ -164,9 +172,9 @@ def markerFound(self, id1, image, id2=-1): bw, self.markerDict ) if self.markerIDs is not None: - print( - "", end="" - ) # I have not been able to reproduce an error when I have a print statement here so I'm leaving it in + # I have not been able to reproduce an error when I have a + # print statement here so I'm leaving it in + print("", end="") if id2 == -1: # single post self.index1 = -1 # this just checks to make sure that it found the right marker @@ -177,7 +185,7 @@ def markerFound(self, id1, image, id2=-1): if self.index1 != -1: print("Found the correct marker!") - if self.write: + if self.write_to_disk: self.videoWriter.write(bw) # purely for debug cv2.waitKey(1) break @@ -200,17 +208,17 @@ def markerFound(self, id1, image, id2=-1): self.index2 = j if self.index1 != -1 and self.index2 != -1: print("Found both markers!") - if self.write: + if self.write_to_disk: self.videoWriter.write(bw) # purely for debug cv2.waitKey(1) break if i == 220: # did not find any AR markers with any b&w cutoff using aruco # Checks to see if yolo can find a tag - if self.useYOLO: + if self.use_YOLO_tracking: detections = [] - if not self.write: - # this is a simpler detection function that doesn't return the image + if not self.write_to_disk: + # this is a simpler detection fn that doesn't return the image detections = simple_detection( image, self.network, self.class_names, self.thresh ) @@ -230,21 +238,21 @@ def markerFound(self, id1, image, id2=-1): if id2 == -1 and len(detections) > 0: self.corners = self._convertToCorners(detections, 1) self.index1 = 0 # Takes the highest confidence ar tag - if self.write: + if self.write_to_disk: self.videoWriter.write(image) # purely for debug cv2.waitKey(1) elif len(detections) > 1: self.corners = self._convertToCorners(detections, 2) self.index1 = 0 # takes the two highest confidence ar tags self.index2 = 1 - if self.write: + if self.write_to_disk: self.videoWriter.write(image) # purely for debug cv2.waitKey(1) print(self.corners) # Not even YOLO saw anything if self.index1 == -1 or (self.index2 == -1 and id2 != -1): - if self.write: + if self.write_to_disk: self.videoWriter.write(image) # cv2.imshow('window', image) cv2.waitKey(1) @@ -259,21 +267,28 @@ def markerFound(self, id1, image, id2=-1): + self.corners[self.index1][0][2][0] + self.corners[self.index1][0][3][0] ) / 4 - # takes the pixels from the marker to the center of the image and multiplies it by the degrees per pixel - self.angleToMarker = self.degreesPerPixel * ( + # takes the pixels from the marker to the center of the image and + # multiplies it by the degrees per pixel + self.angleToMarker = self.degrees_per_pixel * ( centerXMarker - self.frameWidth / 2 ) """ - distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker - focalLength = focal length at 0 degrees horizontal and 0 degrees vertical - focalLength30H = focal length at 30 degreees horizontal and 0 degrees vertical - focalLength30V = focal length at 30 degrees vertical and 0 degrees horizontal - realFocalLength of camera = focalLength - + (horizontal angle to marker/30) * (focalLength30H - focalLength) - + (vertical angle to marker / 30) * (focalLength30V - focalLength) - If focalLength30H and focalLength30V both equal focalLength then realFocalLength = focalLength which is good for non huddly cameras - Please note that the realFocalLength calculation is an approximation that could be much better if anyone wants to try to come up with something better + distanceToAR = + (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker. + + focal_length = focal length at 0 degrees horizontal and 0 degrees vertical. + focalLength30H = focal len at 30 degrees horizontal and 0 degrees vertical. + focalLength30V = focal len at 30 degrees vertical and 0 degrees horizontal. + + realFocalLength of camera = focal_length + + (horizontal angle to marker/30) * (focalLength30H - focal_length) + + (vertical angle to marker / 30) * (focalLength30V - focal_length). + + If focalLength30H and focalLength30V both equal focal_length, + then realFocalLength = focal_length which is good for non huddly cameras. + Please note that the realFocalLength calculation is an approximation that + could be much better if anyone wants to try to come up with something better """ hAngleToMarker = abs(self.angleToMarker) centerYMarker = ( @@ -283,12 +298,12 @@ def markerFound(self, id1, image, id2=-1): + self.corners[self.index1][0][3][1] ) / 4 vAngleToMarker = abs( - self.vDegreesPerPixel * (centerYMarker - self.frameHeight / 2) + self.vdegrees_per_pixel * (centerYMarker - self.frameHeight / 2) ) realFocalLength = ( - self.focalLength - + (hAngleToMarker / 30) * (self.focalLength30H - self.focalLength) - + (vAngleToMarker / 30) * (self.focalLength30V - self.focalLength) + self.focal_length + + (hAngleToMarker / 30) * (self.focalLength30H - self.focal_length) + + (vAngleToMarker / 30) * (self.focalLength30V - self.focal_length) ) widthOfMarker = ( ( @@ -317,15 +332,15 @@ def markerFound(self, id1, image, id2=-1): + self.corners[self.index2][0][2][0] + self.corners[self.index2][0][3][0] ) / 4 - self.angleToMarker = self.degreesPerPixel * ( + self.angleToMarker = self.degrees_per_pixel * ( (centerXMarker1 + centerXMarker2) / 2 - self.frameWidth / 2 ) hAngleToMarker1 = abs( - self.vDegreesPerPixel * (centerXMarker1 - self.frameWidth / 2) + self.vdegrees_per_pixel * (centerXMarker1 - self.frameWidth / 2) ) hAngleToMarker2 = abs( - self.vDegreesPerPixel * (centerXMarker2 - self.frameWidth / 2) + self.vdegrees_per_pixel * (centerXMarker2 - self.frameWidth / 2) ) centerYMarker1 = ( self.corners[self.index1][0][0][1] @@ -340,20 +355,20 @@ def markerFound(self, id1, image, id2=-1): + self.corners[self.index2][0][3][1] ) / 4 vAngleToMarker1 = abs( - self.vDegreesPerPixel * (centerYMarker1 - self.frameHeight / 2) + self.vdegrees_per_pixel * (centerYMarker1 - self.frameHeight / 2) ) vAngleToMarker2 = abs( - self.vDegreesPerPixel * (centerYMarker2 - self.frameHeight / 2) + self.vdegrees_per_pixel * (centerYMarker2 - self.frameHeight / 2) ) realFocalLength1 = ( - self.focalLength - + (hAngleToMarker1 / 30) * (self.focalLength30H - self.focalLength) - + (vAngleToMarker1 / 30) * (self.focalLength30V - self.focalLength) + self.focal_length + + (hAngleToMarker1 / 30) * (self.focalLength30H - self.focal_length) + + (vAngleToMarker1 / 30) * (self.focalLength30V - self.focal_length) ) realFocalLength2 = ( - self.focalLength - + (hAngleToMarker2 / 30) * (self.focalLength30H - self.focalLength) - + (vAngleToMarker2 / 30) * (self.focalLength30V - self.focalLength) + self.focal_length + + (hAngleToMarker2 / 30) * (self.focalLength30H - self.focal_length) + + (vAngleToMarker2 / 30) * (self.focalLength30V - self.focal_length) ) widthOfMarker1 = ( ( @@ -376,7 +391,8 @@ def markerFound(self, id1, image, id2=-1): ) ) / 2 - # distanceToAR = (knownWidthOfMarker(20cm) * focalLengthOfCamera) / pixelWidthOfMarker + # distanceToAR = (knownWidthOfMarker(20cm) * + # focalLengthOfCamera) / pixelWidthOfMarker distanceToMarker1 = ( self.knownMarkerWidth * realFocalLength1 ) / widthOfMarker1 diff --git a/Autonomous/libs/pyproject.toml b/Autonomous/libs/pyproject.toml index 86d7d70..0734a2d 100644 --- a/Autonomous/libs/pyproject.toml +++ b/Autonomous/libs/pyproject.toml @@ -3,12 +3,11 @@ name = "libs" version = "0.1.0" description = "Add a short description here" dependencies = [ - "opencv-python~=4.7.0.72", "numpy~=1.24.3", "darknet~=0.3", "scikit-image~=0.21.0", "black~=23.3.0", - "opencv-contrib-python~=4.7.0.72", + "opencv-python~=4.7.0.72", ] readme = "README.md" requires-python = ">= 3.10" diff --git a/Autonomous/libs/requirements-dev.lock b/Autonomous/libs/requirements-dev.lock index fed0ab6..f8ea519 100644 --- a/Autonomous/libs/requirements-dev.lock +++ b/Autonomous/libs/requirements-dev.lock @@ -15,7 +15,6 @@ lazy-loader==0.2 mypy-extensions==1.0.0 networkx==3.1 numpy==1.24.3 -opencv-contrib-python==4.7.0.72 opencv-python==4.7.0.72 packaging==23.1 pathspec==0.11.1 diff --git a/Autonomous/libs/requirements.lock b/Autonomous/libs/requirements.lock index fed0ab6..f8ea519 100644 --- a/Autonomous/libs/requirements.lock +++ b/Autonomous/libs/requirements.lock @@ -15,7 +15,6 @@ lazy-loader==0.2 mypy-extensions==1.0.0 networkx==3.1 numpy==1.24.3 -opencv-contrib-python==4.7.0.72 opencv-python==4.7.0.72 packaging==23.1 pathspec==0.11.1