Stufss

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)))
 

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],

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,15 +20,15 @@
 
 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
 
         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])
 

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

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())

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

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}")

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)

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))
+        )