WhyCon

A precise, efficient and low-cost localization system

Presná a spoľahlivá lokalizácia je jedným z kľúčových problémov mobilnej robotiky. Obvykle sa tento problém rieši metódami simultánnej lokalizácie a mapovania (SLAM). Napriek tomu sa však často používajú aj externé lokalizačné systémy, či už pre riadenie v uzavretej slučke, alebo pre meranie polohy. WhyCon je rýchly a presný vizuálny softvér pre lokalizáciu viacerých robotov. Jadrom je nový a efektívny algoritmus pre rozpoznávanie čierno-bielych vzorov. Metóda je robustná a odolná voči zmenám osvetlenia, má presnosť na úrovni pixelov a jej výpočtová zložitosť nezávisí od veľkosti spracovaného obrazu. Je dostatočne rýchla a zvláda spracovať stovky snímkov za sekundu pomocou bežného počítača a lacnej kamery, pričom dosahuje až milimetrovú presnosť.

Vstup: farebný alebo čiernobiely obrázok z jednej kamery

Výstup: pozície kruhových značiek v priestore

Algoritmus:

1. segmentácia prehľadávaním do šírky,
2. filtrácia výsledkov (kruhovosť, sústrednosť, pomer farieb atď),
3. presné parametre 2d vzoru,
4. reprojekcia do 3d,
5. transformácia do daného súradnicového systému.

• inicializácia hľadania na poslednej pozíci v obraze,
• spracovanie iba relevantných pixelov,
• uložené medzivýsledky vo fronte pozícií.

Podrobný opis algoritmu je zachytený na prednáške T. Krajníka

ku ktorej sú k dispozícii aj

• slajdy
• github repo
• KRAJNÍK, Tomáš, et al. A practical multirobot localization system. Journal of Intelligent & Robotic Systems, 2014, 76.3: 539-562.

WhyCon detekcia je v podstate séria filtrov, kde každý krok zúži počet možných kandidátov. Celý postup je vidno na nasledovnom obrázku:

• Krok 1 — Šedotónový + blur. Farba nás nezaujíma (marker je čiernobiely), Gaussian blur odstráni šum.
• Krok 2 — Adaptívny prah. Každý pixel porovná so svojím okolím (51×51 px okno) a rozhodne: tmavší než priemer → biela, svetlejší → čierna. Preto to funguje aj pri nerovnomernom osvetlení — na rozdiel od globálneho prahu, ktorý by v tmavom rohu videl "všetko čierne".
• Krok 3 — Kontúry s hierarchiou. OpenCV nájde hranice všetkých bielych oblastí (v tomto snímku 1021). Kľúčové je RETR_TREE — zachová informáciu, ktorá kontúra je vnútri ktorej (rodič → dieťa).
• Krok 4 — Filter na cirkularitu. Z 1021 kontúr zostanú len 3 kruhové. Cirkularita = 4π × plocha / obvod². Dokonalý kruh = 1.0, stena bludiska ≈ 0.1. Filtrujeme na ≥ 0.55.
• Krok 5 — Rodič-dieťa test. Toto je jadro WhyConu. Hľadáme kruhovú kontúru, ktorá má vnútri seba ďalšiu kruhovú kontúru — čiže prstenec s kruhovým stredom. Navyše kontrolujeme pomer plôch: pri pomere priemerov 2:1 je pomer plôch * 0.25. V tvojom snímku vyšiel 0.24 — presne sedí.
• Krok 6 — Najlepší kandidát. Ak prešlo viacero, vyberie sa ten s najvyšším skóre (kombinácia cirkularity oboch kruhov a veľkosti).

Jednotlivé kroky algoritmu WhyCon.

WhyCon Tracker — Kompletný návod (Ubuntu Linux) podľa Claude.AI

Tento návod ťa prevedie celým procesom od inštalácie softvéru, cez kalibráciu kamery, až po spracovanie videa, vykreslenie trajektórie a generovanie videa s postupne rastúcou stopou robota.

1. Inštalácia základných závislostí

Nainštaluj Python 3 a pip:

sudo apt update
sudo apt install python3 python3-pip python3-venv ffmpeg

Vytvor si virtuálne prostredie (odporúčané):

python3 -m venv ~/whycon-env
source ~/whycon-env/bin/activate

Nainštaluj Python knižnice:

pip install opencv-python numpy matplotlib

Ak by opencv-python hlásil chýbajúce systémové knižnice:

sudo apt install libgl1-mesa-glx

Priprav pracovný priečinok:

mkdir -p ~/whycon-tracking/output
cd ~/whycon-tracking

Skopíruj sem súbor whycon_tracker.py, tvoje videá a prípadne calibration.npz.

2. Inštalácia GPU akcelerácie (voliteľné)

Ak máš NVIDIA GPU, môžeš urýchliť dekódovanie videa. Toto je úplne voliteľné — skript funguje aj bez GPU, len pomalšie.

Predpoklady

Potrebuješ nainštalovaný NVIDIA driver. Over si:

nvidia-smi

Ak príkaz funguje a vidíš svoju GPU, máš driver v poriadku.

FFmpeg s NVDEC podporou

Štandardný FFmpeg z Ubuntu repozitárov zvyčajne podporuje NVDEC. Over si:

ffmpeg -hwaccels

V zozname by si mal vidieť cuda. Ak tam nie je, nainštaluj FFmpeg s NVIDIA podporou:

sudo apt install ffmpeg

Ak ani potom cuda nevidíš, skús nainštalovať FFmpeg zo snap alebo z oficiálneho buildu:

# Snap verzia (zvyčajne obsahuje NVDEC):
sudo snap install ffmpeg

# Alebo stiahni statický build:
# https://johnvansickle.com/ffmpeg/

Overenie funkčnosti

# Test GPU dekódovania na tvojom videu:
ffmpeg -hwaccel cuda -i tvoje_video.mp4 -f null - 2>&1 | tail -5

Ak prebehne bez chyby, GPU akcelerácia funguje.

3. Príprava markera a šachovnice

WhyCon marker

WhyCon marker je čierny disk s bielym kruhovým stredom. Pomer vonkajšieho a vnútorného priemeru by mal byť približne 2:1 (napr. vonkajší 122 mm, vnútorný 61 mm).

Hotový marker na stiahnutie:

• https://github.com/jiriUlr/whycon-ros (súbor id/test.pdf, priemer 122 mm)

Vytlač na matný papier. Zmeraj a zapíš si presný vonkajší priemer v milimetroch.

Kalibračná šachovnica

Stiahni si šachovnicový vzor:

• https://markhedleyjones.com/projects/calibration-checkerboard-collection (SVG/PDF, rôzne veľkosti)
• https://calib.io/pages/camera-calibration-pattern-generator (vlastné rozmery)
• https://github.com/opencv/opencv/blob/4.x/doc/pattern.png (9×6 z OpenCV)

Vytlač na A4 a nalep na tuhý podklad (kartón, clipboard). Pri tlači vypni škálovanie — tlač na 100%.

Zapamätaj si:

• Počet vnútorných rohov (nie štvorčekov!) — šachovnica 11×8 štvorčekov = 10×7 rohov
• Veľkosť štvorčeka v mm — odmeraj po vytlačení

4. Nahrávanie videí

Nastavenie kamery

Upevni kameru na stabilné miesto nad bludisko (statív, konzola, strop). Nastav:

• Najvyššie dostupné rozlíšenie (ideálne 4K)
• Pevný fokus (zamkni autofokus) na vzdialenosť bludiska
• Rovnaké nastavenie pre kalibráciu aj nahrávanie robota

Kalibračné video (60–90 sekúnd)

Vezmi šachovnicu na tuhom podklade a pomaly ju presúvaj a nakláňaj pred kamerou:

• V rôznych častiach obrazu (stred, rohy, hrany)
• Pod rôznymi uhlami (naklonená do všetkých strán)
• V rôznych vzdialenostiach
• Pomaly — nesmie byť rozmazaná

Video s robotom

Spusti nahrávanie, potom robota. Uisti sa, že WhyCon marker na robote je viditeľný z pozície kamery.

5. Spracovanie — príklady použitia

Pred spustením sa uisti, že máš aktivovaný virtuálny environment a si v správnom priečinku:

source ~/whycon-env/bin/activate
cd ~/whycon-tracking

Kompletný pipeline (kalibrácia + tracking + trail video)

python3 whycon_tracker.py \
    --calib kalibracia.mp4 \
    --video kobuki1.mp4 \
    --board-size 9,6 \
    --square-size 25.0 \
    --marker-diam 122.0 \
    --trail-video

Len tracking s uloženou kalibráciou

python3 whycon_tracker.py \
    --video kobuki1.mp4 \
    --use-calib calibration.npz

Tracking bez kalibrácie (len pixelové súradnice)

python3 whycon_tracker.py \
    --video kobuki1.mp4 \
    --no-calib

S GPU akceleráciou

python3 whycon_tracker.py \
    --video kobuki1.mp4 \
    --use-calib calibration.npz \
    --gpu \
    --trail-video

Dlhé video — zrýchlenie spracovania

# Každý 3. snímok (10 fps pri 30fps videu):
python3 whycon_tracker.py \
    --video dlhe_video.mp4 \
    --use-calib calibration.npz \
    --every-n 3 \
    --trail-video \
    --gpu

Trail video s mizajúcou stopou

Namiesto celej stopy od začiatku sa zobrazí len posledných N sekúnd:

# Stopa mizne po 10 sekundách:
python3 whycon_tracker.py \
    --video kobuki1.mp4 \
    --no-calib \
    --trail-video \
    --trail-fade 10

Trail video s bodkami namiesto čiar (odolné voči chybným detekciám)

Ak detekcia občas "preskočí" na nesprávne miesto, spojité čiary vytvárajú rušivé skoky naprieč celým obrazom. Bodkový režim kreslí každú detekciu ako samostatný bod — správne detekcie vytvoria kvázi spojitú stopu, zatiaľ čo ojedinelé chybné detekcie budú len nevýrazné izolované body:

python3 whycon_tracker.py \
    --video kobuki1.mp4 \
    --no-calib \
    --trail-video \
    --trail-dots

# Kombinovateľné s GPU a fade:
python3 whycon_tracker.py \
    --video dlhe_video.mp4 \
    --use-calib calibration.npz \
    --trail-video \
    --trail-dots \
    --trail-fade 15 \
    --gpu

6. Prehľad všetkých parametrov

Parameter	Predvolená hodnota	Popis
--video	(povinný)	Cesta k videu s robotom
--calib	—	Cesta ku kalibračnému videu
--use-calib	—	Cesta k uloženej kalibrácii (.npz)
--no-calib	—	Preskočiť kalibráciu
--board-size	9,6	Vnútorné rohy šachovnice (šírka,výška)
--square-size	25.0	Veľkosť štvorčeka šachovnice v mm
--marker-diam	122.0	Vonkajší priemer WhyCon markera v mm
--every-n	1	Spracovať každý N-tý snímok
--output-dir	./output	Výstupný priečinok
--trail-video	—	Vygenerovať video s rastúcou stopou
--trail-dots	—	Kresliť bodky namiesto čiar (odolné voči chybám)
--trail-fade	0	Dĺžka mizajúcej stopy v sekundách (0 = celá stopa)
--gpu	—	Použiť NVIDIA GPU na dekódovanie videa

7. Výstupné súbory

Po spracovaní nájdeš v output/ priečinku:

Súbor	Popis
trajectory_overlay.png	Trajektória na obrázku bludiska, farba = čas
trajectory_timeseries.png	Priebehy X, Y a rýchlosti v čase
trajectory_xy.png	Trajektória zhora (XY graf)
trajectory_data.csv	Surové dáta: snímok, čas, pixel X, pixel Y
positions.npy	Dáta v NumPy formáte pre ďalšie spracovanie
calibration.npz	Kalibrácia kamery (ak bola spustená)
trail_video.mp4	Video so stopou z čiar (ak --trail-video bez --trail-dots)
trail_video_dots.mp4	Video so stopou z bodiek (ak --trail-video --trail-dots)

8. Riešenie problémov

Skript nenašiel šachovnicu

• Skontroluj --board-size — počítaj vnútorné rohy, nie štvorčeky
• Šachovnica je príliš malá — drž ju bližšie ku kamere
• Skús rôzne rozmery: 9,6 potom 10,7 potom 8,5

Skript nenašiel marker

• Marker musí mať dostatočný kontrast (biele centrum, čierny prstenec)
• Pri slabom osvetlení zvýš jas pri nahrávaní
• Ak je marker malý v obraze, skús znížiť min_area v zdrojovom kóde

Trajektória má výpadky

• Robot bol zakrytý stenou — to je normálne
• Pri šikmom pohľade sa marker v niektorých častiach detekuje horšie
• Medzery sa dajú vyplniť interpoláciou v post-processingu

Trajektória má náhodné skoky

• Falošné detekcie kruhových prvkov v scéne
• Skús zvýšiť min_area alebo znížiť max_area v zdrojovom kóde
• Pomôže aj mediánový filter na výstupné súradnice

Trail video je príliš veľké

• Použi --every-n 2 alebo --every-n 3 na zníženie počtu spracovaných snímkov
• FFmpeg re-encoding (automatický) by mal veľkosť výrazne znížiť
• Pre menšie rozlíšenie výstupu uprav zdrojový kód (pridaj resize pred zápisom)

GPU problémy

• nvidia-smi nefunguje → nainštaluj NVIDIA driver: sudo apt install nvidia-driver-535
• FFmpeg nemá cuda → preinštaluj FFmpeg alebo použi snap verziu
• GPU dekódovanie padá → skús bez --gpu, CPU verzia je stabilnejšia

Import error cv2

# Alternatíva bez GUI závislostí:
pip install opencv-python-headless

Skús si sám...

Celý projekt nájdete na https://github.com/baloghr/kobuki-whycon