apriltag_tracking.py
# Dieses Werk ist unter der MIT-Lizenz lizenziert.
# Copyright (c) 2013-2023 OpenMV LLC. Alle Rechte vorbehalten.
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# Dieses Beispiel zeigt, wie man mit der Steuerung des Auslesefensters einen kleinen Teil eines Kamera
# Sensor-Pixel-Arrays mit sehr hoher Geschwindigkeit ausliest und dieses Auslesefenster bewegt.
# Dieses Beispiel wurde für die OpenMV Cam H7 Plus mit dem OV5640-Sensor entworfen und getestet.
import csi
import time
# Dieses Beispielskript erzwingt für die gesamte Zeit einen konstanten Belichtungswert. Sie möchten jedoch
# möglicherweise die Belichtung dynamisch anpassen, wenn das Auslesefenster auf eine kleine Größe schrumpft.
EXPOSURE_MICROSECONDS = 20000
SEARCHING_RESOLUTION = csi.QVGA
TRACKING_RESOLUTION = csi.QQVGA # oder csi.QQQVGA
TRACKING_LOW_RATIO_THRESHOLD = (
0.2 # Zu einem kleineren Auslesefenster wechseln, wenn die Tag-Seite im Verhältnis zur Auflösung kleiner ist.
)
TRACKING_HIGH_RATIO_THRESHOLD = (
0.8 # Zu einem größeren Auslesefenster wechseln, wenn die Tag-Seite im Verhältnis zur Auflösung größer ist.
)
csi0 = csi.CSI()
csi0.reset() # Sensor zurücksetzen und initialisieren.
csi0.pixformat(csi.GRAYSCALE) # Pixelformat auf GRAYSCALE setzen
csi0.framesize(SEARCHING_RESOLUTION)
csi0.snapshot(time=1000) # Warten, bis die Einstellungen wirksam werden.
clock = time.clock() # Ein Clock-Objekt erstellen, um die FPS zu verfolgen.
csi0.auto_gain(False) # Ausschalten, da es sonst oszilliert.
csi0.auto_exposure(False, exposure_us=EXPOSURE_MICROSECONDS)
csi0.snapshot(time=1000)
# sensor_w und sensor_h sind die rohen Pixel-Breite/-Höhe des Bildsensors (x/y sind anfangs 0).
x, y, sensor_w, sensor_h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)
while True:
clock.tick()
img = csi0.snapshot()
# Verfolgt standardmäßig TAG36H11.
tags = img.find_apriltags()
if len(tags):
best_tag = max(tags, key=lambda x: x.decision_margin)
img.draw_rectangle(best_tag.rect)
# Dies muss kleiner sein als die Standard-Sensorausgabe, damit wir es bewegen können.
readout_window_w = ((sensor_w // csi0.width()) * csi0.width()) / 2
readout_window_h = ((sensor_h // csi0.height()) * csi0.height()) / 2
def get_mapped_centroid(t):
# Standardmäßig ist das Auslesefenster auf das gesamte Sensor-Pixel-Array mit x/y==0 eingestellt.
# Die Auflösung, die Sie sehen, wird erzeugt, indem Pixel aus dem Auslesefenster der
# Kamera entnommen werden. Die x/y-Position ist relativ zur Sensormitte.
x, y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)
# Der Kameratreiber versucht, die von Ihnen übergebene Auflösung an die maximale
# Breite/Höhe anzupassen, die auf den Sensor passt, während das Seitenverhältnis beibehalten wird.
ratio = min(w / float(csi0.width()), h / float(csi0.height()))
# cx() auf die Mitte des Viewports beziehen und dann auf das Auslesefenster skalieren.
mapped_cx = (t.cx - (csi0.width() / 2.0)) * ratio
# Da das Seitenverhältnis beibehalten wird, kann es einen Versatz in x geben.
mapped_cx += (w - (csi0.width() * ratio)) / 2.0
# Unsere Verschiebung von der Sensormitte hinzufügen
mapped_cx += x + (sensor_w / 2.0)
# cy() auf die Mitte des Viewports beziehen und dann auf das Auslesefenster skalieren.
mapped_cy = (t.cy - (csi0.height() / 2.0)) * ratio
# Da das Seitenverhältnis beibehalten wird, kann es einen Versatz in y geben.
mapped_cy += (h - (csi0.height() * ratio)) / 2.0
# Unsere Verschiebung von der Sensormitte hinzufügen
mapped_cy += y + (sensor_h / 2.0)
return (mapped_cx, mapped_cy) # X/Y-Position auf dem Sensor-Array.
def center_on_tag(t, res):
global readout_window_w
global readout_window_h
mapped_cx, mapped_cy = get_mapped_centroid(t)
# Zur Auflösung res wechseln (wenn res unverändert war, geschieht nichts).
csi0.framesize(res)
# Auslesefenster konstruieren. x/y sind Versätze von der Mitte.
x = int(mapped_cx - (sensor_w / 2.0))
y = int(mapped_cy - (sensor_h / 2.0))
w = int(readout_window_w)
h = int(readout_window_h)
# Auf den Schwerpunkt fokussieren.
csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (x, y, w, h))
# Prüfen, ob wir den Rand erreichen.
new_x, new_y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)
# Sie können diese Fehlerwerte verwenden, um Servos anzusteuern und die Kamera zu bewegen, wenn Sie möchten.
x_error = x - new_x
y_error = y - new_y
if x_error < 0:
print("-X Limit Reached ", end="")
if x_error > 0:
print("+X Limit Reached ", end="")
if y_error < 0:
print("-Y Limit Reached ", end="")
if y_error > 0:
print("+Y Limit Reached ", end="")
center_on_tag(best_tag, TRACKING_RESOLUTION)
loss_count = 0
# Diese Schleife verfolgt den Tag mit einer viel höheren Auslesegeschwindigkeit und niedrigerer Auflösung.
while True:
clock.tick()
img = csi0.snapshot()
# Verfolgt standardmäßig TAG36H11.
tags = img.find_apriltags()
# Wenn wir den Tag verlieren, müssen wir einen neuen finden.
if not len(tags):
# Einige fehlerhafte Frames aufgrund von Tag-Flackern behandeln.
if loss_count < 2:
loss_count += 1
continue
# Auflösung zurücksetzen.
csi0.framesize(SEARCHING_RESOLUTION)
csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (sensor_w, sensor_h))
break
loss_count = 0
# Die Blob-Liste eingrenzen und den Blob hervorheben.
best_tag = max(tags, key=lambda x: x.decision_margin)
img.draw_rectangle(best_tag.rect)
print(clock.fps(), "TAG cx:%d, cy:%d" % get_mapped_centroid(best_tag))
w_ratio = best_tag.w / csi0.width()
h_ratio = best_tag.h / csi0.height()
# Das Tracking-Fenster verkleinern, bis der Tag hineinpasst.
while (w_ratio < TRACKING_LOW_RATIO_THRESHOLD) or (
h_ratio < TRACKING_LOW_RATIO_THRESHOLD
):
readout_window_w /= 2
readout_window_h /= 2
w_ratio *= 2
h_ratio *= 2
# Das Tracking-Fenster vergrößern, bis der Tag hineinpasst.
while (TRACKING_HIGH_RATIO_THRESHOLD < w_ratio) or (
TRACKING_HIGH_RATIO_THRESHOLD < h_ratio
):
readout_window_w *= 2
readout_window_h *= 2
w_ratio /= 2
h_ratio /= 2
center_on_tag(best_tag, TRACKING_RESOLUTION)
print(clock.fps())