Skip to content

Stufss #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 7 additions & 4 deletions HedgeHogVision/Detector/AdjecencyDetector.py
Original file line number Diff line number Diff line change
@@ -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)))

2 changes: 1 addition & 1 deletion HedgeHogVision/Detector/ConstructorDetector.py
Original file line number Diff line number Diff line change
@@ -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],
10 changes: 6 additions & 4 deletions HedgeHogVision/Detector/Detector.py
Original file line number Diff line number Diff line change
@@ -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])

86 changes: 86 additions & 0 deletions HedgeHogVision/Detector/MegaTagDetector.py
Original file line number Diff line number Diff line change
@@ -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
10 changes: 6 additions & 4 deletions HedgeHogVision/HedgeHogDetector.py
Original file line number Diff line number Diff line change
@@ -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())
11 changes: 11 additions & 0 deletions HedgeHogVision/Tags/Field.py
Original file line number Diff line number Diff line change
@@ -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
33 changes: 33 additions & 0 deletions HedgeHogVision/Tags/ImageTag.py
Original file line number Diff line number Diff line change
@@ -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}")
5 changes: 5 additions & 0 deletions HedgeHogVision/Tags/MegaTag.py
Original file line number Diff line number Diff line change
@@ -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)
37 changes: 37 additions & 0 deletions HedgeHogVision/Tags/RealWorldTag.py
Original file line number Diff line number Diff line change
@@ -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))
)
Loading
Oops, something went wrong.