Kalibrierung der Kamera

Zur Kalibrierung der Kamera machen wir als erstes mit dem Programm ueyedemo ca. 9 Bilder vom Labyrinth, wobei sich jeweils an verschiedenen Stellen im Labyrinth eine große Tafel mit Schachbrettmuster befindet.

Dazu wird zuerst eine uEye Konfigurationsdatei geladen (Abbildung 1) oder erstellt, die dann auch später zum Tracken der Roboter genutzt werden soll, am besten wird das AOI (Area of Interest) so festgelegt, dass nur das Labyrinth und ein ca. 20 cm breiter Randbereich sichtbar sind, ich verwende hier die Datei UI124xLE-C_conf_aoi.ini aus dem Verzeichnis laustracker/share/laustracker/.

alternate text

Abbildung 1: Menü: Laden der uEye Konfigurationsdatei

Anschließend werden die Testaufnahmen in einem Unterordner von laustracker-src/laustracker/testdata/, in dem sich noch keine Bilder befinden, gespeichert, in diesem Fall calibrate_aoi/

Hier ein Beispielfoto:

alternate text

Abbildung 2: Originalbild von der Kamera, verzerrt

Dann wechseln wir zurück in den Ordner calibrate und erstellen eine Liste der Bilder in einer Textdatei:

calibrate]$ ls -d -1 $PWD/../../testdata/calibrate_aoi/*.* \
              >image_list.txt

Jetzt können wir aus den Bildern mit dem Programm laustracker-calibrate eine Intrinsics-Matrix erstellen:

calibrate]$ ../../bin/laustracker-calibrate 10 6 \
              image_list.txt intrinsics_new_cam_aoi.xml

In Abbildung 3 ist ein Screenshot von laustracker-calibrate, in dem das Labyrinth automatisch gefunden wurde, zu sehen. Die Punkte auf dem Schachbrettmuster, die zur Berechnung der Verzerrung sowie der Intrinsischen Parameter genutzt werden, sind hierbei mit farbigen Markierungen versehen.

alternate text

Abbildung 3: Das Labyrinth wurde automatisch im Bild gefunden.

Um das nächste Bild anzuzeigen, muss man einfach immer die Lehrtaste drücken, wenn dann alle Bilder von dem Programm ausgewertet sind, beendet es sich und die Kamera-Matrix und die Distortion-Koeffizienten werden in der Datei intrinsics_new_cam_aoi.xml gespeichert.

alternate text

Abbildung 4: Über Kamera-Matrix und Distortion Koeffizienten entzerrtes Bild

Das Ergebnis in Abbildung 4 ist schon ziemlich gut, das AOI kann noch etwas verschoben werden, so dass sich das Labyrinth mittig im Bild befindet, dann muss die XML-Datei unter dem Namen intrinsics.xml in das Verzeichnis laustracker-src/laustracker/share/laustracker/ kopiert werden, damit liblaustracker die neuen Kalibrierungsdaten nutzt.

Finden der Labyrinth Begrenzungen

Variante 1: Manuell

Die Laustracker Programme kann man auch ohne direkten Zugriff auf die Kamera benutzen, dazu wird beispielsweise ein Bild von der Festplatte verwendet. Zum Ausprobieren der einzelnen Algorithmen bietet sich das Kommandozeilenprogramm laustracker-cli an, hierzu wechselt man in das Verzeichnis laustracker/src/cli/ und führt es mit den entsprechenden Optionen, die auch in der README Datei im selbigen Verzeichnis erwähnt sind, aus.

Für mehr Informationen zu den unterstützten Kommandozeilenoptionen sei auf den Eintrag Unterstützte Optionen unter Laustracker CLI verwiesen.

Projects]$ cd laustracker-src/laustracker/src/cli/
cli]$ ../../bin/laustracker-cli --walls-img=\
        ../../testdata/aoi_labyrinth_empty.jpg --manual

Es öffnet sich dann ein Fenster mit den Titel please_click_on_the_labyrinth_edges, wie in Abbildung 5 dargestellt. In diesem muss man dann nacheinander die Ecken des Labyrinthes anklicken.

alternate text

Abbildung 5: Manuelles Lokalisieren des Labyrinthes.

Nach dem Lokalisieren des Labyrinthes werden noch vorhandene trapezförmige Verzerrungen ausgeglichen, so dass das Labyrinth ein Rechteck ergibt und die äußere Begrenzung bestimmt.

alternate text

Abbildung 6: Perspektivische Transformation (Trapez Ausgleich)

In Abbildung 6 sind keine großen Unterschiede zum Ausgangsbild zu erkennen. Das liegt daran, dass der Blickwinkel der Kamera nahezu senkrecht auf das Labyrinth ist. Weitere Informationen zur Funktionsweise der Perspektivischen Transformation und warum sie notwendig sein kann, befinden sich unter Grundlagen.

Variante 2: Automatisch

Um den Algorithmus zum automatishcen Finden des Labyrinths zu testen, führt man im gleichen Verzeichnis wie in der manuellen Variante den folgenden Befehl aus:

Projects]$ cd laustracker-src/laustracker/src/cli/
cli]$ ../../bin/laustracker-cli --walls-img=\
        ../../testdata/aoi_labyrinth_empty.jpg

Um weitere Zwischenschritte anzuzeigen, kann man auch noch die Option --debug bei dem obigen Befehl verwenden.

Ablauf:

  1. Mit Hilfe der Funktion improve_image(walls_img) wird das Kamerabild zu erst entzerrt.

  2. Über die Funktion findSquares(img, squares, contours_img, debuglevel); werden Quadrate im Bild gesucht. Dabei werden die gefundenen Konturen aufgrund von Winkeln, Seitenverhältnissen und Größe gefiltert, je nachdem ob sie zum Labyrinth gehören können.

    alternate text

    Abbildung 7: Automatische Suche von Quadraten

  3. Jetzt wird mithilfe der Quadrate die Position des Labyrinthes ermittelt (findLabyrinth(contours_img, 80, labyrinth_pos, debuglevel);). Dabei werden in die Grafik mit den gefunden Quadraten (Abbildung 8) mithilfe des Hough-Lines Algorithmus aus der OpenCV Bibliothek Linien gelegt.

    alternate text

    Abbildung 8: Schwarz-Weiß-Bild mit den Quadraten

Die Linien befinden sich dann in einem Array aus jeweils 2 mal 2 Werten, X- und Y-Koordinaten von 2 Punkten auf den Linien. Als nächstes werden dann die Anstiegswinkel der Linien berechnet und somit die Linien in horizontale, vertikale und diagonale Linien unterteilt.

alternate text

Abbildung 9: Hough Lines

Zwischendurch werden jeweils die horizontalen und vertikalen Linien gespeichert, die sich am weitesten am Bildrand befinden. Jetzt müssen nur noch die Schnittpunkte dieser Linien berechnet werden und das Labyrinth ist gefunden.

  1. Zum Schluss wird noch eine perspektivische Transformation durchgeführt, damit das Labyrinth auch quadratisch und nicht trapezförmig ist. Die Position des Labyrinthes wird dann in der Variable labyrinth_pos gespeichert.
alternate text

Abbildung 10: Automatische Suche abgeschlossen

Man könnte jetzt natürlich einen effektiveren Algorithmus entwickeln, um aus den Quadraten auf die Labyrinth Begrenzungen zu kommen. Aber da diese Operation nur einmal zu Programmstart ausgeführt werden muss und die Hough Transformation momentan gut funktioniert, war das nicht nötig.

Mögliche Probleme

  • Wenn keine oder nicht genug Quadrate gefunden wurden, wird evt. ein Teil des Labyrinths weggeschnitten. Die Position des Labyrinths kann aber auch manuell durch Anklicken der Ecken festgelegt werden.

Variante 3: Über rote Kreise an den Labyrinthecken

Wie in Abbildung 11 dargestellt, wurden an den Labyrinthecken rote Kreise zur Lokalisierung angebracht. Das PDF zum Ausdrucken der Kreise in dieser Farbe liegt mit im Projektverzeichnis unter hardware/red_dot.pdf.

alternate text

Abbildung 11: Rote Kreise an den Labyrinthecken zur besseren Lokalisierung

Danach wurden aus dem Bild die HSV Farbbereiche für die roten Kreise ermittelt, wie unter Bestimmen der Grenzwerte zum Finden der LEDs beschrieben, um dann über diese Grenzwerte nur die roten Bereiche zu erhalten.

In Abbildung 12 sieht man noch eine ältere Version zum Lokalisieren des Labyrinthes als noch kein AOI festgelegt war und die Grenzwerte für die roten Bereiche haben auch noch nicht perfekt gepasst, da vor allem außerhalb des Labyrinthes einige Bereiche als rot detektiert wurden, die eigentlich gar nicht rot sind.

Sehr von Vorteil ist auf jeden Fall, dass innerhalb des Labyrinthes kaum Störfaktoren in Form von roten Bereichen vorhanden sind.

alternate text

Abbildung 12: Farbbereiche noch nicht ganz passend

Nachdem die Grenzwerte angepasst und der Bildausschnitt verkleinert wurde, lassen sich leicht die Positionen der Eckpunkte bestimmen. Dafür werden in Abbildung 13 alle weißen Konturen gesucht, alle Kombinationen aus jeweils 4 roten Punkten gebildet, überprüft ob diese Punkte eine Fläche mit annähernd rechten Winkel bilden und ob die Fläche des Rechtecks mindestens einen bestimmten Prozentanteil der Gesamtbildfläche einnimmt.

Der etwas kompliziertere Algorithmus resultiert daraus, da anfangs noch mehr rote Konturen gefunden wurden und das Labyrinth zweifelsfrei bestimmt werden musste. In Abbildung 13 kann dagegen wenig schief gehen.

alternate text

Abbildung 13: Grenzwerte angepasst und Bild weiter bearbeitet

Die Grenzwerte sind in der Konfigurationsdatei labyrinth.conf definiert. Falls andere Eckmarkierungen genutzt werden sollten, können sie dort leicht angepasst werden.

Wer will kann das ganze auch selber ausprobieren, der Befehl dazu lautet:

laustracker-src]$ cd laustracker/src/cli/
cli]$ ../../bin/laustracker-cli --walls-img=\
        ../../testdata/aoi_labyrinth_red_dots.jpg \
        --reddots --debug
alternate text

Abbildung 14: Labyrinth wurde über die roten Kreise gefunden und zugeschnitten

Es ist auch noch die Option --labyrinth-conf implementiert. So lässt sich zum Beispiel abends bei schlechten Lichtverhältnissen eine Konfigurationsdatei speziell für das Leuchtstoffröhren-Licht verwenden, da die Kreise unter dem Licht einen anderen Farbton aufweisen können wie in Abbildung 15 dargestellt. Die Defaultwerte sollten aber relativ zuverlässig funktionieren.

alternate text

Abbildung 15: Laustracker Server Labyrinth über rote Kreise gefunden, andere Lichtverhältnisse

Mögliche Probleme

Selbst mit den roten Kreisen ist es manchmal nicht möglich, das Labyrinth ohne manuelle Eingriffe zu detektieren. Ungleichmäßige Ausleuchtung, wie Abbildung 16, verhindert hier das Finden des Labyrinths.

alternate text

Abbildung 16: Labyrinth kann durch ungleichmäßige Beleuchtung nicht gefunden werden.

Herausfinden wo sich Wände befinden

Variante 1: Suchen der weißen Wände

Nachdem die Position des Labyrinthes im Bild gefunden ist, können alle Positionen überprüft werden, an denen sich theoretisch Wände befinden können. Die Ergebnisse werden dann in eine Karte des Labyrinthes übertragen.

Zum Finden der Wände sind 2 Verfahren implementiert, durch die Kommandozeilenoption --mask kann jeweils eine gewählt werden.

Variante 1: Mit berechneten Wandpositionen:

cli]$ ../../bin/laustracker-cli \
        --walls-img=../../testdata/aoi_labyrinth_empty.jpg

Variante 2: Mit Maske von den Wandpositionen:

cli]$ ../../bin/laustracker-cli \
        --walls-img=../../testdata/aoi_labyrinth_empty.jpg --mask

In der Funktion locate_walls(walls_img) wird dann folgendes ausgeführt:

  1. Es werden weiße Regionen im Bild gesucht.

In Abbildung 17 ist eine frühere Variante dazu abgebildet. Es lässt sich leicht erkennen, dass die Wände teilweise nicht vollständig und sehr dünn sind. Deshalb muss das Bild noch weiter bearbeitet werden.

alternate text

Abbildung 17: Ausgangsbild: Weiße Regionen zum Finden der Wände (frühere Version)

Eine verbesserte Variante ist in Abbildung 18 dargestellt. Jetzt müssen nur noch die weißen Flecken innerhalb der Felder entfernt werden und Löscher in den Wänden geschlossen werden. Dazu verwende ich die Funktion edm() aus der Datei image_manipulation.cpp, die verschiedene Funktionen aus der OpenCV Bibliothek ausführt (erode(), dilate(), morphologyEx()). Das Endergebnis ist in Abbildung 19 zu sehen.

alternate text

Abbildung 18: Ausgangsbild: Weiße Regionen zum Finden der Wände (aktuelle Version)

alternate text

Abbildung 19: Endergebnis: Weiße Regionen nach edm()

  1. Bereiche, in denen sich voraussichtlich die Wände befinden, werden mit oder ohne Hilfe der Maske berechnet.

2.1. Ohne Maske (getGrid(), Abbildung 20)

  • Aus den vier Eckpunkten und der Anzahl der Felder wird die Feldbreite berechnet
  • Dann werden horizontale und waagerechte Linien mit Hilfe der Feldbreite berechnet, an deren Schnittpunkten sich dann die Wandelemente befinden
  • Es werden Rechtecke gebildet, deren Schwerpunkt den Schnittpunkten entspricht
  • Diese Rechtecke dienen dann als ROI (Region of Interest) für die Auswertung, ob sich in dem Bereich eine Wand befindet
alternate text

Abbildung 20: Raster zum Erkennen der Wände

2.2. Mit Maske (getGrid2(), Abbildung 26)

  • Die Grafik mit der Maske (Abbildung 21) wird geladen

    alternate text

    Abbildung 21: Grafik mit der Maske

    Die Maske ließ sich einfach mit Gimp erstellen, indem in ein Foto vom Labyrinth für die Bereiche, in denen sich Wände befinden, weiße Rechtecke eingezeichnet werden und zum Schluss alles andere schwarz gefärbt wird.

  • Die Maske wird auf die Größe des Labyrinthes skaliert

    Zu Demonstrationszwecken ist in Abbildung 22 eine halb transparente Überlagerung aus Maske und Kamerabild, sowie in Abbildung 23 eine Überlagerung des resultierenden Schwarz-Weiß-Bildes mit der Maske dargestellt.

    alternate text

    Abbildung 22: Überlagerung der Maske mit dem Farbbild von der Kamera

    alternate text

    Abbildung 23: Überlagerung der Maske mit dem Schwarz-Weiß-Bild mit den Wänden

  • Mit Hilfe der OpenCV Funktion findContours(gray, contours, hierarchy, RETR_LIST, CHAIN_APPROX_SIMPLE), werden dann die Koordinaten der weißen Rechtecke in der Maske bestimmt, nach Position sortiert und in dem Array walls zur weiteren Verwendung gespeichert.

  1. In den Bereichen für die Wände werden dann im Schwarz-Weiß-Bild die weißen Pixel gezählt und, wenn ein bestimmter Grenzwert von z.B. 40% der Fläche der ROI weiß ist, gilt die Wand als vorhanden. Die gefunden Wände werden im Anschluss in die Karte vom Labyrinth übertragen.

    alternate text

    Abbildung 24: Ergebnis: Rote Quadrate für erkannte Wände

In Abbildung 23 lässt sich auch gut erkennen, dass sich die Flächen für die Wände je nach Position im Labyrinth unterscheiden. Aber durch die Maske werden die Wände ziemlich gut getroffen und man kann ohne komplizierte Änderungen am Quelltext einfach mit einem Zeichenprogramm die Maske bearbeiten. In der Fertigung könnte man so schnell auf Produktänderungen reagieren, ohne dass erst komplizierte Änderungen am Quelltext durchgeführt werden müssen.

Mögliche Probleme

  • Schwarze Bojen auf weißen Wänden könnten dazu führen, dass eine Wand nicht als vorhanden erkannt wird. Mögliche Gegenmaßnahmen wären zum einen, einen Streifen weißes Klebeband mittig über die Boje zu kleben, zweitens das Rechteck in der Maske für diese Wand etwas zu vergrößern (wenn die Boje nicht die gesamte Wand abdeckt).

    Dieser Fehler trat häufig bei der in Abbildung 17 dargestellten Variante zum Finden der weißen Regionen auf. Inzwischen haben die Bojen aber kaum noch Einfluss auf das Finden der Wände, wie in Abbildung 25 und Abbildung 26 zu sehen ist. Schlimmer sind Roboter, die sich während der Initialisierung der Laustracker Software an Wandpositionen befinden. Wenn man etwas genauer hinsieht kann man erkennen, dass der oberste Roboter in Abbildung 26 als Wand erkannt wird.

    alternate text

    Abbildung 25: Bojen und Roboter, Schwarz-Weiß

    alternate text

    Abbildung 26: Bojen und Roboter, erkannte Wände

  • Die Beleuchtung das Labyrinths erfolgt nicht gleichmäßig, so dass größere weiße Bereiche entstehen.

    Wie in Abbildung 27 zu sehen ist, verursacht die Deckenbeleuchtung im Praktikumsraum abends eine sehr ungleichmäßige Ausleuchtung des Labyrinthes. Da der Labyrinthboden relativ glatt ist, wird auch viel Licht reflektiert, so dass im Schwarz-Weiß-Bild (Abbildung 28) große weiße Regionen entstehen und die Wände in Abbildung 29 praktisch nicht mehr korrekt erkannt werden können. Für diesen Fall ist die Option --load implementiert, um eine Karte vom Labyrinth, die tagsüber erzeugt wurde, zu laden.

    alternate text

    Abbildung 27: Leuchtstoffröhren: Ausgangsbild

    alternate text

    Abbildung 28: Leuchtstoffröhren: Schwarz-Weiß-Bild

    alternate text

    Abbildung 29: Leuchtstoffröhren: Richtige Erkennung der Wände ist unmöglich

Erstellung einer neuen Maske

Wenn die Maske einmal erstellt ist muss sie eigentlich nicht mehr angepasst werden, da sie automatisch auf die Größe des Labyrinthes skaliert wird. Es ist aber einfach möglich, die Auswahlflächen in Form und Größe mit einem Zeichenprogramm zu verändern, ohne den Sourcecode anpassen zu müssen.

Hier möchte ich kurz erklären, wie die Erstellung einer neuen Maske funktioniert.

Zuerst wird das Programm laustracker-cli mit einem Bild vom Labyrinth im Debug Modus --debug gestartet und von dem Fenster mit dem Titel cv_white_overlay ein Screenshot gemacht. Dieser dient dann als Ausgangsbild zu Erstellung einer neuen Maske.

alternate text

Abbildung 30: Neue Maske, Schritt 1, cv_white_overlay

In Schritt 2 wird dann das Bild auf das Labyrinth zugeschnitten und es werden in den Bereichen, in denen sich Wände befinden können, grüne Linien eingezeichnet.

alternate text

Abbildung 31: Neue Maske, Schritt 2, Zuschnitt

alternate text

Abbildung 32: Neue Maske, Schritt 2, fertig gestellt

Diese Linien werden dann in Schritt 3 durchtrennt:

alternate text

Abbildung 33: Neue Maske, Schritt 3, Linien durchtrennt

Anschließend werden in Schritt 4 mit dem Auswahlwerkzeug alle grünen Bereiche selektiert, die Selektion wird invertiert und alles andere wird schwarz eingefärbt. Danach werden noch die grünen Bereiche weiß gefärbt und die Maske ist fertig.

alternate text

Abbildung 34: Neue Maske, Schritt 4, Maske ist fertig

Die Maske wurde unter share/laustracker/mask.png gespeichert. Sie wird verwendet, wenn die Kommandozeilenoption --mask benutzt wird.

Das Endergebnis kann weiter oben in Abbildung 21 bewundert werden.

Variante 2: Suchen der schwarzen Linien

Da das Lokalisieren der weißen Wände unter dem Licht der Leuchtstoffröhren einige Probleme bereitete, soll hier noch ein zweites Verfahren implementiert werden, das nicht nach den Wänden sucht, sondern nach den schwarzen Linien auf dem Labyrinthboden. Da die Linien von den Wänden verdeckt werden, können wir so auf des Vorhandensein der Wände schließen.

Zum Testen des neuen Verfahren wird einfach die Kommandozeilenoption --black benutzt.

Dabei war das Verfahren zum Finden der schwarzen Linien einfacher zu implementieren als zunächst gedacht, da wir schon eine Funktion getGrid2 haben, die uns die Bereiche für die Wände innerhalb des Bildes anhand einer Maske bestimmt, musste nur die Maske angepasst werden, so dass sie besser auf die schwarzen Linien passt (Bereiche sehr viel kleiner, damit sie innerhalb der Wände liegen) und dann die Suche angepasst werden, um nicht die weißen Pixel zu zählen, sondern die schwarzen.

In in Abbildung 35 und Abbildung 36 ist die Lokalisierung über das neue Verfahren dargestellt.

alternate text

Abbildung 35: Neues Verfahren: Weiße Bereiche überlagert mit Maske

alternate text

Abbildung 36: Neues Verfahren mit Auswertung der Wände

Wie in Abbildung 35 zu sehen ist, passt die Maske noch nicht perfekt. Aber selbst wenn sie hundertprozentig passen würde, habe ich momentan keine Idee, wie sich die weißen Wände bzw. die schwarzen Linien von den, vom Störlicht verursachten, weißen Flecken unterscheiden lassen. Selbst ein Kriterium wie – wenn 10 Prozent der interessanten Region (ROI) schwarz sind, befindet sich an dieser Stelle keine Wand – führt nicht zum Ziel, da die Wände nicht fest montiert sind und man somit nicht immer nur die Wand trifft. Da Variante 1 momentan zuverlässiger funktioniert, bleibe ich bei der Lokalisierung über die weißen Wände.

Finden der Roboter im Labyrinth

Implementiertes Verfahren: Lokalisierung über farbige LEDs

Das Lokalisieren der Roboter über farbige LEDs ist die einfachste und robusteste Möglichkeit, die Roboter im Labyrinth mit der Kamera zu finden.

Durch 2 LEDs mit verschiedenen Farben kann sowohl die Position als auch die Blickrichtung des Roboters einwandfrei aus einem einzigen Kamerabild bestimmt werden.

Um einen ersten Eindruck von der Funktionsweise zu bekommen, kann man einfach das Laustracker Kommandozeilenprogramm mit 2 Bildern von der Festplatte aufrufen:

cli]$ ../../bin/laustracker-cli --fast \
        --walls-img=../../testdata/aoi_labyrinth_empty.jpg \
        --robots-img=../../testdata/aoi_robots_dark.jpg --debug

Durch die Kommandozeilenoption --robots-img wird das angegebene Bild von der Festplatte geladen und darin dann nach Robotern gesucht.

Ablauf

  1. Der Funktion lt.update_robot_positions(frame); wird ein Bild von der Kamera mit kurzer Belichtungszeit übergeben, so dass nur starke Lichtquellen, wie die LEDs, gut sichtbar sind.
alternate text

Abbildung 37: Ausgangsbild zum Finden der Roboter (aoi_robots_dark.jpg)

  1. Bei dem Aufruf von lt.update_robot_positions(frame); wird im Hintergrund die Funktion bool RobotTracker::processNewFrame(Mat image) aus der Datei laustracker/src/liblaustracker/robots.cpp ausgeführt.
  2. Falls die Position des Labyrinths bekannt ist, werden alle Bereiche außerhalb des Labyrinths durch Festlegen einer Region von Interesse (ROI), die in unserem Fall dem Labyrinth entspricht, von der Suche ausgeschlossen, so dass dort keine Roboter erkannt werden können und nur ein kleinerer Bildbereich ausgewertet werden muss, was die Framerate erhöht. Meine Versuche zur Erhöhung der Laustracker Framerate habe ich in diesem Blogpost festgehalten.
  3. In dem Bild werden alle blauen und roten Bereiche gesucht und in zwei separaten Matrizen gespeichert. In der Matrix für Blau sind dann beispielsweise alle blauen Bereiche weiß und der Rest schwarz.
alternate text

Abbildung 38: Bild mit den blauen Bereichen

  1. Über die OpenCV Funktion findContours() werden in den Bildern, mit den blauen und roten Bereichen, zusammenhängende Flächen gesucht. Über eine FOR-Schleife wird dann über diese Flächen iteriert, der minimale Kreis berechnet, der die jeweilige Fläche komplett umschließt. Der Mittelpunkt des Kreises, der Radius, und die Farbe der LED werden dann in einem Array gespeichert. Anschließend wird ein blauer oder roter Kreis an der Stelle der LED in das Bild eingezeichnet.
alternate text

Abbildung 39: Blaue und rote LEDs eingekreist.

  1. Jetzt haben wir alle blauen LEDs in einem Array und alle roten LEDs in einem anderen. Nun werden alle Kombinationen aus blauen und roten LEDs gebildet und die Entfernung beider LEDs berechnet. Wenn die Entfernung kleiner ist als 0,6 mal die Feldbreite ist, haben wir vermutlich einen Roboter gefunden.

    Wenn es ein Roboter ist, berechnen wir den Mittelpunkt zwischen der blauen und der roten LED, berechnen im Mittelpunkt den Normalenvektor auf die Verbindungsstrecke zwischen der blauen und der roten LED und anschießend den Mittelpunkt des Roboters sowie den Orientierungswinkel des Roboters.

alternate text

Abbildung 40: Gefundene Roboter mit Mittelpunkt und Radius in Blickrichtung.

  1. Wenn die Labyrinthposition bekannt ist, wird die Position jedes Roboters mit dem Mittelpunkt jeden Feldes verglichen, und so der Roboter dem nächstliegenden Feld zugeordnet.
alternate text

Abbildung 41: Feld Mittelpunkte

Außerdem werden zu jedem Roboter die absolute Position in Relation zur linken oberen Labyrinthecke in Pixel und cm sowie die relative Position zum Bezugsfeld gespeichert. Weitere Informationen zu Positionsangaben und zum Koordinatensystem befinden sich unter Das Koordinatensystem.

  1. Über die Liste der Roboter wird iteriert und alle Roboter die keinen Sinn ergeben, werden gelöscht. Solche Roboter entstehen zum Beispiel, wenn eine rote oder blaue LED so hell leuchtet, dass sie in der Mitte nicht als Rot oder Blau erkannt wird, sondern als weiß und so aus einer LED zwei LEDs entstehen oder wenn sich eine LED an der Alu Platte des Roboters spiegelt, dann kann es passieren, dass 2 Roboter erkannt werden, die sich sehr nah beieinander befinden und die Mittelpunkte nur wenige Millimeter voneinander entfernt sind, dann wird der Roboter gelöscht, bei dem die LEDs weniger deutlich erkennbar sind, also die erkannte LED Fläche geringer ist.
  2. Das ganze wird regelmäßig wiederholt, falls neue Roboter ins Labyrinth gebracht wurden.

Mögliche Probleme

  • Wenn der Roboter nah an einer Wand fährt und sich auf dieser Wand eine Boje befindet, kann es in den äußeren Labyrinthregionen passieren, dass sich die Boje zwischen Kamera und einer LED des Roboters befindet und der Roboter so kurzzeitig nicht erkannt wird.

Weitere Möglichkeiten

Es wurde auch drüber nachgedacht, die Roboter über Mustererkennung (Feature Detection) zu lokalisieren. Erste Versuche wurden jedoch schnell verworfen, da das Lokalisieren über LEDs einfacher und zuverlässiger funktionierte. Hier sind noch ein paar weiterführende Links zu dem Thema:

Da inzwischen eine Kamera mit höherer Auflösung verwendet wird, könnte man mal zu Testzwecken große farbige Dreiecke aus Papier auf den Robotern anbringen und diese dann im Kamerabild suchen, um die Roboter zu lokalisieren. Über verschiedene Farben ist dann auch eine Unterscheidung der Roboter möglich. Allerdings muss das Labyrinth dazu gut und gleichmäßig ausgeleuchtet werden, damit auch abends die Dreiecke im Bild gefunden werden können.

Unterscheiden der Roboter

Um Teams zu bilden oder um besser zu erkennen, wo man sich selber befindet, wäre es hilfreich, den Robotern Namen oder Nummern zuzuordnen.

Implementiertes Verfahren: Über Position einer dritten LED

Die Unterscheidung der Roboter erfolgt durch eine weiße 8 mm LED hinten am Roboter, die sich an verschieden Positionen befinden kann.

Ablauf:

  1. Zusätzlich zu den blauen und roten Bereichen werden auch die weißen Bereiche gesucht und in einer separaten Matrix gespeichert.

  2. Genauso wie bei den blauen und roten LEDs werden über die Funktion findContours() alle zusammenhängenden weißen Flächen gesucht und in einem Array gespeichert.

  3. Da wir schon eine Liste aller Roboter haben, müssen wir jetzt nur noch den Robotern die weißen LEDs zuordnen. Dazu wird der Mittelpunk eines jeden Roboters mit den Positionen aller weißen LEDs verglichen, die LED, die sich innerhalb des Radius des Roboters befindet und sich in einem Abstand von mindestens einem Viertel des Roboterradius hinter der Verbindungslinie von blauer und roter LED befindet, wird gewählt.

    alternate text

    Abbildung 42: Gefundene weiße LEDs sind eingekreist.

  4. Jetzt wird die Distanz von der weißen LED zur blauen und roten berechnet und jeweils in einer Variable gespeichert. Aus dem Verhältnis beider Strecken wird dann über die Funktion String RobotTracker::ratio2name(float ratio) der Name des Roboters bestimmt.

Dafür sind in der Konfigurationsdatei robots.conf die Namen und Verhältnisse für die Roboter definiert:

[robots]
names = Andi Pete
ratios = 1.396 1.92
max_ratio_diff = 0.8

Oder können auch direkt im Programm gesetzt werden:

vector<String> names = {"Bernd", "Paul", "Karli",
                        "Heinz", "Manu"};
vector<float> ratios = {1.42, 1.25, 1.03, 0.92, 0.81};
lt.robots.setRobotNames(names, ratios, 0.4);

Es wird der Name gewählt, bei dem die Differenz des berechneten Verhältnisses zu dem aus der Liste, am geringsten und nicht größer als max_ratio_diff ist.

Das Programm zur 3D-Visualisierung blendet dann über den Robotern den Namen ein, bzw. robot_0, robot_1 und so weiter, wenn der Roboter noch nicht in der obigen Liste vorhanden ist. Danach folgt das Verhältnis in runden Klammern und zum Schluss steht die absolute Position in cm in eckigen Klammern.

alternate text

Abbildung 43: Name über den Robotern in Laustracker3D

Inzwischen werden alle Roboter ohne weiße LED als ungültig eingestuft. Das könnte aber leicht durch Ändern einer einzigen Zeile im Quelltext angepasst werden.

Mögliche Probleme

  • Wenn sich die Positionen der weißen LEDs am Roboter nicht stark genug unterscheiden, können den Robotern nicht zuverlässig Namen zugeordnet werden. Dass ist bei der momentanen Aluminium Platte mit festen Bohrungen aber nicht mehr der Fall.

Weitere Möglichkeiten

  • Über Farbkodierung der LEDs

    Da auch noch eine grüne 8 mm LED vorhanden ist, kann die Anzahl der unterscheidbaren Roboter leicht von 4 auf 8 erhöht werden, in dem man die weiße LED durch eine grüne ersetzt und noch weiter, wenn man 2 weiße, 2 grüne oder eine Mischung aus beidem und diese noch an verschieden Positionen bestückt.

  • Wenn die dritte LED mit einem freien I/O-Pin des Mikrocontroller steuerbar ist, können auch noch andere Möglichkeiten, wie das Blinken oder das Ein-/Ausschalten auf Anfrage getestet werden.

    • 2 LEDs leuchten dauerhaft, eine blinkt mit bestimmter Frequenz

      Nachteil: es sind mehrere Bilder erforderlich, um die Roboter zu unterscheiden. Außerdem liefert die Kamera die Bilder nicht mit konstanter Frequenz und der PC kann die Bilder manchmal nicht schnell genug verarbeiten.

    • 2 LEDs leuchten dauerhaft, 3. LED wird kurz eingeschaltet, wenn eine Anfrage per Funk an den Roboter gesendet wurde wie z.B.: “Wo ist Bernd?”

    • Alle 10 Sekunden blinkt die 3. LED mit einer bestimmten Anzahl

      • 3 mal Blinken = Bernd, 4 = Karli, 5 = Heinz

Verfolgen der Roboter

Momentan werden die Roboter in jedem Bild aufs Neue gesucht. Man könnte aber jeweils die Position eines jeden Roboters im letzten Bild (Frame) speichern und diese dann mit der Position im momentanen Bild vergleichen.

So könnte man einen Bereich festlegen, in dem sich der Roboter im nächsten Bild befinden kann und müsste nicht das gesamte Spielfeld durchsuchen und könnte die Framerate weiter steigern.

Allerdings muss das gesamte Spielfeld trotzdem ca. alle 30 Sekunden komplett durchsucht werden, um zu überprüfen, ob neue Roboter in das Spielfeld gebracht wurden.