diff --git a/HedgeHogVision/Detector/AdjecencyDetector.py b/HedgeHogVision/Detector/AdjecencyDetector.py index d1086ad..e80cf44 100644 --- a/HedgeHogVision/Detector/AdjecencyDetector.py +++ b/HedgeHogVision/Detector/AdjecencyDetector.py @@ -6,7 +6,7 @@ from .Detector import Detector from .Utils import LinePair import math -def process_tag(arg: list[LinePair, LinePair, Calibration, int, int]) -> list[FoundTag]: +def process_tag(arg: list[LinePair, LinePair, Calibration, int, int, int]) -> list[FoundTag]: line1 = arg[0] line2 = arg[1] calibration = arg[2] @@ -35,12 +35,15 @@ def process_tag(arg: list[LinePair, LinePair, Calibration, int, int]) -> list[Fo imagePoints, calibration.mtx, calibration.dist, - flags=cv2.SOLVEPNP_IPPE) + flags=cv2.SOLVEPNP_ITERATIVE) if not success: return tag_x = (line2.real_line.top.x + line1.real_line.top.x)/2 - print(tag_x) tag_y = (line1.real_line.top.y + line1.real_line.bot.y)/2 known_tag = MegaTag(tag_x, tag_y, (line1.parent.z + line2.parent.z)/2, math.degrees(rotation), width) + print(imagePoints) + tagWidth = line2.image_line.bot.x - line1.image_line.bot.x + tagHeight = line1.image_line.top.y - line1.image_line.bot.y + print(f"T1: {line1.parent.id}, T2: {line2.parent.id}: TW: {tagWidth}, TH: {tagHeight}") to_append = [] for (r, t) in zip(rotation_vectors,translation_vectors): rotation_matrix = cv2.Rodrigues(r)[0] @@ -77,7 +80,7 @@ def create_tags(self, tags) -> list[list[FoundTag]]: args = [] for i in range(len(lines)-1): j = i + 1 - args.append([lines[i], lines[j], self.calibration, i, j]) + args.append([lines[i], lines[j], self.calibration, i, j, self.solveType]) pool = Pool(4) detected = list(filter(lambda a : a != None, pool.map(process_tag, args))) diff --git a/HedgeHogVision/Detector/ConstructorDetector.py b/HedgeHogVision/Detector/ConstructorDetector.py index 85b02cc..42ee1c9 100644 --- a/HedgeHogVision/Detector/ConstructorDetector.py +++ b/HedgeHogVision/Detector/ConstructorDetector.py @@ -20,7 +20,7 @@ def process_tag(arg: list[LinePair, LinePair, Calibration, int, int]) -> list[Fo if(width == 0): return if(line1.parent.z != line2.parent.z): return success, rotation_vectors, translation_vectors, _ = cv2.solvePnPGeneric( - np.array([ + np.array([lambda pl : pl[0]) [-width/2, height/2, 0.0], [width/2, height/2, 0.0], [width/2, -height/2, 0.0], diff --git a/HedgeHogVision/Detector/Detector.py b/HedgeHogVision/Detector/Detector.py index e4ac125..039504e 100644 --- a/HedgeHogVision/Detector/Detector.py +++ b/HedgeHogVision/Detector/Detector.py @@ -3,7 +3,7 @@ import pupil_apriltags from numpy.typing import ArrayLike from HedgeHogVision.Camera.Calibration import Calibration -from HedgeHogVision.Tags.Tags import FoundTag, KnownTag +from HedgeHogVision.Tags.Tags import FoundTag, KnownTag, Field from HedgeHogVision.math_stuff.math_stuff import Transform3d from HedgeHogVision.math_stuff.rotation3d import Rotation3d from HedgeHogVision.math_stuff.translation3d import Translation3d @@ -20,7 +20,7 @@ class Detector(ABC): """Used to find Apriltags in an image and return a position on the field (ABSTRACT CLASS, DO NOT INSTANTIATE)""" - def __init__(self, calibration: Calibration, field: list[KnownTag]): + def __init__(self, calibration: Calibration, field: Field): self.lastKnownPosition: Transform3d = Transform3d.zero() self.roborioPosition: Translation3d = None self.time_since_last_update = 0 @@ -28,7 +28,7 @@ def __init__(self, calibration: Calibration, field: list[KnownTag]): self.calibration = calibration self.detector = pupil_apriltags.Detector(families="tag16h5", nthreads=4) # TODO test thread count self.field = field - + self.solveType = cv2.SOLVEPNP_ITERATIVE """tag_half = tag_width_m / 2 self.tag_half = tag_half self.object_points = np.array([ @@ -49,7 +49,7 @@ def find_tags(self, image: ArrayLike): """ gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) found = self.detector.detect(gray) - firstFilter = list(filter(lambda item: not (item.hamming > 0 or item.decision_margin < 0.7 or item.tag_id >= len(self.field)), found)) + firstFilter = list(filter(lambda item: not (item.hamming > 0 or item.decision_margin < 0.7), found)) return list(filter(lambda item : self.field[item.tag_id] is not None, firstFilter)) def __get_cluster(self, startPair: list[FoundTag,FoundTag], pairList: list[list[FoundTag,FoundTag]]) -> tuple[list[FoundTag], float]: first_tag_pos = startPair[0].robot_position @@ -88,6 +88,8 @@ def trimmed_tags(self, tags: list[list[FoundTag,FoundTag]]) -> list[FoundTag]: return [tags[0][tags[0][0].robot_position.translation.y < 0]] return [tags[0][self.lastKnownPosition.field_distance(tags[0][0].robot_position) > self.lastKnownPosition.field_distance(tags[0][1].robot_position)]] + if(len(tags[0]) == 1): + return list(map(lambda i : i[0], tags)) firstSolve, firstError = self.__get_cluster(tags[0], tags[1:]) secondSolve, secondError = self.__get_cluster(tags[-1],tags[:-1]) diff --git a/HedgeHogVision/Detector/MegaTagDetector.py b/HedgeHogVision/Detector/MegaTagDetector.py new file mode 100644 index 0000000..1181512 --- /dev/null +++ b/HedgeHogVision/Detector/MegaTagDetector.py @@ -0,0 +1,86 @@ +import cv2 +import numpy as np +from ..Camera.Calibration import Calibration +from ..Tags.Tags import FoundTag, MegaTag +from multiprocessing import Pool +from .Detector import Detector +from .Utils import LinePair +import math + +class AdjecencyDetector(Detector): + """Used to find Apriltags in an image and return a position on the field + MegaTagDetector: Uses corners of tags to create new tags, and find positions of those fake tags""" + def create_tags(self, tags) -> list[list[FoundTag]]: + """ + Finds edges of the tag, finds out their real-world position. + Then it reconstructs the tags, and uses SolvePNP to find the real-world distance from the camera to the tags. + Because there is ambiguity across the x axis on SolvePNP's position, this function returns a list pairs of tags, one of them correct, and another one on the wrong side of the tag + To throw out the incorrect tags, use a function such as "trimmed_tags" + :param tags: List of Detections to find the corners of + :return: List of tag pairs + """ + if len(tags) == 0: return [] + + lines = [] + + for item in tags: + image_points = item.corners + + tag = self.field[item.tag_id] + + lines.append(LinePair.create_from_info("LEFT", tag.object_points, image_points, tag)) + lines.append(LinePair.create_from_info("RIGHT", tag.object_points, image_points, tag)) + + line1 = arg[0] + line2 = arg[1] + calibration = arg[2] + height = line1.real_line.top.y - line1.real_line.bot.y + width = line2.real_line.bot.x - line1.real_line.bot.x + #print(line1.real_line.bot.x) + if line1.parent.z != line2.parent.z: return + rotation = line1.parent.rotation + z1 = 0.0 + z2 = 0.0 + objectPoints = np.array([ + [-width/2, height/2, z1], + [width/2, height/2, z2], + [width/2, -height/2, z2], + [-width/2, -height/2, z1], + ], dtype=np.float64) + imagePoints = np.array([ + line1.image_line.top.arr2d, + line2.image_line.top.arr2d, + line2.image_line.bot.arr2d, + line1.image_line.bot.arr2d + ], + dtype=np.float64) + success, rotation_vectors, translation_vectors, _ = cv2.solvePnPGeneric( + objectPoints, + imagePoints, + calibration.mtx, + calibration.dist, + flags=self.solveType) + if not success: return + tag_x = (line2.real_line.top.x + line1.real_line.top.x)/2 + tag_y = (line1.real_line.top.y + line1.real_line.bot.y)/2 + known_tag = MegaTag(tag_x, tag_y, (line1.parent.z + line2.parent.z)/2, math.degrees(rotation), width) + print(imagePoints) + tagWidth = line2.image_line.bot.x - line1.image_line.bot.x + tagHeight = line1.image_line.top.y - line1.image_line.bot.y + print(f"T1: {line1.parent.id}, T2: {line2.parent.id}: TW: {tagWidth}, TH: {tagHeight}") + to_append = [] + for (r, t) in zip(rotation_vectors,translation_vectors): + rotation_matrix = cv2.Rodrigues(r)[0] + found_tag = FoundTag(known_tag, t, rotation_matrix, f"T1: {line1.parent.id}, T2: {line2.parent.id}") + to_append.append(found_tag) + return to_append + args = [] + for i in range(len(lines)-1): + j = i + 1 + args.append([lines[i], lines[j], self.calibration, i, j, self.solveType]) + pool = Pool(4) + + detected = list(filter(lambda a : a != None, pool.map(process_tag, args))) + + if detected is None: return [] + return detected \ No newline at end of file diff --git a/HedgeHogVision/HedgeHogDetector.py b/HedgeHogVision/HedgeHogDetector.py index 36a8b6d..07246ce 100644 --- a/HedgeHogVision/HedgeHogDetector.py +++ b/HedgeHogVision/HedgeHogDetector.py @@ -2,10 +2,10 @@ from HedgeHogVision.math_stuff.math_stuff import Translation3d, Transform3d from HedgeHogVision.SmartDashboard.dashboard import VisionNetworkTable from numpy.typing import ArrayLike -from HedgeHogVision.Tags.Tags import KnownTag +from HedgeHogVision.Tags.Tags import Field from HedgeHogVision.Detector.AdjecencyDetector import AdjecencyDetector from HedgeHogVision.Detector.IndividualDetector import IndividualDetector - +import cv2 from enum import Enum class DetectorType(Enum): @@ -16,10 +16,11 @@ class DetectorType(Enum): class HedgeHogDetector: def __init__(self, detector: DetectorType, calibration, - field: list[KnownTag], + field: Field, camera = None, cameraOffset: Transform3d = Transform3d.zero(), - networkTable: VisionNetworkTable = None): + networkTable: VisionNetworkTable = None, + solveType = cv2.SOLVEPNP_ITERATIVE): """ :param detector: The detector class of the method you want to use. The recommended detector is the AdjecencyDetector (Do not instantiate the class) :param calibration: The calibration of the camera you are using. @@ -30,6 +31,7 @@ def __init__(self, detector: DetectorType, self.camera = camera self.cameraOffset = cameraOffset self.networkTable = networkTable + self.detector.solveType = solveType def solveImage(self, image: ArrayLike): if(self.networkTable != None): self.detector.roborioPosition = self.networkTable.getRoborioPosition() return self.detector.get_world_pos_from_image(image).transform_by(self.cameraOffset.inverse()) diff --git a/HedgeHogVision/Tags/Field.py b/HedgeHogVision/Tags/Field.py new file mode 100644 index 0000000..964bbd0 --- /dev/null +++ b/HedgeHogVision/Tags/Field.py @@ -0,0 +1,11 @@ +from .Tags import KnownTag + +class Field: + def __init__(self, *tags: KnownTag): + self.tags = {} + for i in tags: + self.tags[i.id] = i + + def __getitem__(self, idx): + if idx in self.tags.keys(): return self.tags[idx] + return None diff --git a/HedgeHogVision/Tags/ImageTag.py b/HedgeHogVision/Tags/ImageTag.py new file mode 100644 index 0000000..f57ce04 --- /dev/null +++ b/HedgeHogVision/Tags/ImageTag.py @@ -0,0 +1,33 @@ +from HedgeHogVision.math_stuff.math_stuff import Transform3d, Translation3d, Pose3d, Rotation3d +from numpy.typing import ArrayLike +from .RealWorldTag import KnownTag +import numpy + +class FoundTag: + def __get_robot_location(self) -> Pose3d: + """ + :return: Robots real world position + :rtype: Pose3d + """ + object_to_camera = self.tag_transform.inverse() + return self.parent_tag.pose.transform_by(object_to_camera) + + def __init__(self, parent_tag: KnownTag, translation: ArrayLike, rotation: ArrayLike, id: int = 0): + self.id = id + """The ID of the apriltag""" + self.parent_tag: KnownTag = parent_tag + translation3d: Translation3d = Translation3d.from_matrix(translation) + """Translation of the camera from the apriltag""" + rotation3d: Rotation3d = Rotation3d.from_matrix(rotation) + """Rotation matrix of the apriltag from the matrix""" + self.tag_transform: Transform3d = Transform3d(translation3d, rotation3d) + self.robot_position = self.__get_robot_location() + self.first_guess = numpy.matmul(rotation, translation) + + def __str__(self): + return f"Tag: {self.tag_transform}" + + def info(self): + print(f"Pos: {self.robot_position}, \n" + f"Transform: {self.tag_transform}, \n" + f"ID: {self.id}") diff --git a/HedgeHogVision/Tags/MegaTag.py b/HedgeHogVision/Tags/MegaTag.py new file mode 100644 index 0000000..c1e8640 --- /dev/null +++ b/HedgeHogVision/Tags/MegaTag.py @@ -0,0 +1,5 @@ +from .RealWorldTag import KnownTag + +class MegaTag(KnownTag): + def __init__(self, x: float, y: float, z: float, rotation_degrees: float, tag_witdth: float): + KnownTag.__init__(self, 99, x, y, z, rotation_degrees) \ No newline at end of file diff --git a/HedgeHogVision/Tags/RealWorldTag.py b/HedgeHogVision/Tags/RealWorldTag.py new file mode 100644 index 0000000..4a37dca --- /dev/null +++ b/HedgeHogVision/Tags/RealWorldTag.py @@ -0,0 +1,37 @@ +import numpy +from HedgeHogVision.math_stuff.math_stuff import Transform3d, Translation3d, Pose3d, Rotation3d +from pyquaternion import Quaternion +import math +class KnownTag: + """Contains the position and rotation of the tag on the field""" + + @staticmethod + def from_inches(id, x_inches: float, z_inches: float, y_inches: float, rotation_degrees: float): + return KnownTag(id, x_inches * 0.0254, y_inches * 0.0254, z_inches * 0.0254, rotation_degrees) + + def __init__(self, id, x: float, y: float, z: float, rotation_degrees: float, tag_witdth: float = 0.1524): + self.id = id + self.x: float = x + """X position of the tag relative to a corner of the field in meters""" + self.y: float = y + """Y position of the tag relative to a corner of the field in meters""" + self.z: float = z + """The width of the tag in meters""" + self.tag_witdth = tag_witdth + """Z position of the tag relative to a corner of the field in meters""" + self.rotation: float = math.radians(rotation_degrees) + self.rotationDegrees: float = rotation_degrees + + tag_half = tag_witdth / 2 + self.object_points = numpy.array([ + [-tag_half, tag_half, 0.0], + [ tag_half, tag_half, 0.0], + [ tag_half, -tag_half, 0.0], + [-tag_half, -tag_half, 0.0] + ], dtype=numpy.float64) + + """Rotation of the tag relative to the center of the field in radians.""" + self.pose = Pose3d( + Translation3d(x, y, z), + Rotation3d(Quaternion(axis=[1.0, 0.0, 0.0], radians=self.rotation)) + ) \ No newline at end of file diff --git a/HedgeHogVision/Tags/Tags.py b/HedgeHogVision/Tags/Tags.py index a2252eb..1591fbb 100644 --- a/HedgeHogVision/Tags/Tags.py +++ b/HedgeHogVision/Tags/Tags.py @@ -1,111 +1,16 @@ -from dataclasses import dataclass - -import numpy - -from HedgeHogVision.math_stuff.math_stuff import Transform3d, Translation3d, Pose3d, Rotation3d -from pyquaternion import Quaternion -from numpy.typing import ArrayLike -import math - -@dataclass -class KnownTag: - """Contains the position and rotation of the tag on the field""" - - @staticmethod - def from_inches(id, x_inches: float, z_inches: float, y_inches: float, rotation_degrees: float): - return KnownTag(id, x_inches * 0.0254, y_inches * 0.0254, z_inches * 0.0254, rotation_degrees) - - def __init__(self, id, x: float, y: float, z: float, rotation_degrees: float, tag_witdth: float = 0.1524): - self.id = id - self.x: float = x - """X position of the tag relative to a corner of the field in meters""" - self.y: float = y - """Y position of the tag relative to a corner of the field in meters""" - self.z: float = z - """The width of the tag in meters""" - self.tag_witdth = tag_witdth - """Z position of the tag relative to a corner of the field in meters""" - self.rotation: float = math.radians(rotation_degrees) - self.rotationDegrees: float = rotation_degrees - - tag_half = tag_witdth / 2 - self.object_points = numpy.array([ - [-tag_half, tag_half, 0.0], - [ tag_half, tag_half, 0.0], - [ tag_half, -tag_half, 0.0], - [-tag_half, -tag_half, 0.0] - ], dtype=numpy.float64) - - """Rotation of the tag relative to the center of the field in radians.""" - self.pose = Pose3d( - Translation3d(x, y, z), - Rotation3d(Quaternion(axis=[1.0, 0.0, 0.0], radians=self.rotation)) - ) - - -class MegaTag(KnownTag): - def __init__(self, x: float, y: float, z: float, rotation_degrees: float, tag_witdth: float): - KnownTag.__init__(self, 99, x, y, z, rotation_degrees) - - -class FoundTag: - def __get_robot_location(self): - """ - - :return: Robots real world position - :rtype: Pose3d - """ - object_to_camera = self.tag_transform.inverse() - return self.parent_tag.pose.transform_by(object_to_camera) - def __get_guess_location(self): - """ - :return: Robots real world position - :rtype: Pose3d - """ - object_to_camera = self.tag_transform.inverse() - return Pose3d.zero().transform_by(object_to_camera) - - def __init__(self, parent_tag: KnownTag, translation: ArrayLike, rotation: ArrayLike, id: int = 0): - self.id = id - self.parent_tag: KnownTag = parent_tag - """The ID of the apriltag""" - translation3d: Translation3d = Translation3d.from_matrix(translation) - """Translation of the camera from the apriltag""" - rotation3d: Rotation3d = Rotation3d.from_matrix(rotation) - """Rotation matrix of the apriltag from the matrix""" - self.tag_transform: Transform3d = Transform3d(translation3d, rotation3d) - self.robot_position = self.__get_robot_location() - #self.first_guess = numpy.matmul(rotation, translation) - def __str__(self): - return f"Tag: {self.tag_transform}" - def info(self): - print(f"Pos: {self.robot_position}, \nTransform: {self.tag_transform}, \nID: {self.id}") - -"""field = ( - KnownTag.from_inches(0.0, 0.0, 0.0, 180), # 0 - KnownTag.from_inches(42.19, 610.77, 18.22, 180), # 1 - KnownTag.from_inches(108.19, 610.77, 18.22, 180), # 2 - KnownTag.from_inches(147.19, 610.77, 18.22, 180), # 3 - KnownTag.from_inches(265.74, 636.96, 27.38, 180), # 4 - KnownTag.from_inches(265.74, 14.25, 27.38, 0), # 5 - KnownTag.from_inches(147.19, 40.45, 18.22, 0), # 6 - KnownTag.from_inches(108.19, 40.45, 18.22, 0), # 7 - KnownTag.from_inches(42.19, 40.45, 18.22, 0) # 8 -)""" -field = ( - None, # 0 - KnownTag.from_inches(1, 42.19, 610.77, 18.22, 0), # 1 - KnownTag.from_inches(2, 108.19, 610.77, 18.22, 0), # 2 - KnownTag.from_inches(3, 147.19, 610.77, 18.22, 0), # 3 - KnownTag.from_inches(4, 265.74, 636.96, 27.38, 0), # 4 - KnownTag.from_inches(5, 265.74, 14.25, 27.38, 180), # 5 - KnownTag.from_inches(6, 147.19, 40.45, 18.22, 180), # 6 - KnownTag.from_inches(7, 108.19, 40.45, 18.22, 180), # 7 - KnownTag.from_inches(8, 42.19, 40.45, 18.22, 180) # 8 +from .RealWorldTag import KnownTag +from .MegaTag import MegaTag +from .ImageTag import FoundTag +from .Field import Field + +ChargedUp2023 = Field( + KnownTag.from_inches(1, 42.19, 610.77, 18.22, 0), + KnownTag.from_inches(2, 108.19, 610.77, 18.22, 0), + KnownTag.from_inches(3, 147.19, 610.77, 18.22, 0), + KnownTag.from_inches(4, 265.74, 636.96, 27.38, 0), + KnownTag.from_inches(5, 265.74, 14.25, 27.38, 180), + KnownTag.from_inches(6, 147.19, 40.45, 18.22, 180), + KnownTag.from_inches(7, 108.19, 40.45, 18.22, 180), + KnownTag.from_inches(8, 42.19, 40.45, 18.22, 180) ) -"""field = ( - None, - KnownTag(0 + 0.655, 0, 0, 180), - KnownTag(0.915 + 0.655, 0, 0, 180), - KnownTag(0.915 + 0.84 +0.655, 0, 0, 180), -)""" + diff --git a/main.py b/main.py index c8f4e56..62403bb 100644 --- a/main.py +++ b/main.py @@ -5,10 +5,11 @@ from HedgeHogVision.math_stuff.math_stuff import Transform3d, Translation3d, Rotation3d import pickle as pkl +from HedgeHogVision.Tags.Tags import KnownTag, Field, ChargedUp2023 + from HedgeHogVision.HedgeHogDetector import HedgeHogDetector, DetectorType import cv2 -from HedgeHogVision.Tags.Tags import field #VisionNetworkTable.fromString("10.28.98.2","Vision","SmartDashboard/Odometry") #mtx, dist = pkl.load(open("calib-picam-4", "rb")) @@ -18,26 +19,34 @@ #cv2.QT_QPA_PLATFORM = "wayland" #tag_finder = AdjecencyDetector(calibration) _robotToCamera = Transform3d(Translation3d(-0.1397,0,-0.16), Rotation3d.zero()) -img = cv2.imread("/home/ozy/Documents/Tag/test5_7.png") +def getImage(fileName): + return cv2.imread(f"/home/ozy/Documents/Tag/{fileName}.png") + +img = getImage("test5_7") -print(field[5].pose) +print(ChargedUp2023[5].pose) + +field = Field( + KnownTag(1, 0, 0, 10, 0), + KnownTag(2, 2, 0, 10, 0) +) detectorA = HedgeHogDetector( DetectorType.ADJACENCY, calibration, - field, + ChargedUp2023, cameraOffset=_robotToCamera ) detectorB = HedgeHogDetector( DetectorType.INDIVIVIDUAL, calibration, field, - cameraOffset=_robotToCamera + cameraOffset=_robotToCamera, + solveType=cv2.SOLVEPNP_IPPE ) - HedgeHogDetector.compare_detectors(img, detectorA, detectorB, Translation3d(5, 0, 7), - method1Name = "Adjacency", method2Name = "Individual") + method1Name = "Iterative", method2Name = "IPPE") #start(tag_finder) #debug(tag_finder)