Hirdetés

Sztereó látás Surveyor SVS robottal

Bemutatok egy valós idejű alkalmazást, amivel a Surveyor SVS robot sík terepen akadályokat tud kikerülni

Írta: Elrood
SVS . Surveyor . KEFO . GAMF . OpenCV . SDL
2011-02-22 12:09

Bevezető

Ez a cikk a szakdolgozatomból készült (a gamfinfo számára), de megvágtam és az érthetőség kedvéért egyszerűsítettem. A cikk letölthető innen: http://www.gamfinfo.hu/

Ehhez a témához tartozó blogbejegyzéseim: [link]

Aki a bonyolultabb verziója érdekel, az olvassa el a KÉPAF 2011 konferenciára küldött verzióját: [pdf]. Pár részlet nincs benne, mert néhány dologra a konferencia után jöttünk rá.

Hogy kedvet kapj a cikk végigolvasásához, nézd meg a cikkről készült videót:

Bevezetés

Hirdetés

A Kecskeméti Főiskola Gépipari és Automatizálási Műszaki Főiskolai Kar 2009-es Házi Tudományos Diákköri Konferenciájára elkészített dolgozatom kapcsán ismerkedtem meg a mobil robotokkal, ezek a Lego cég által gyártott NXT-k voltak. A mobil robotoknak, mivel mozognak, szükséges valamilyen szenzor, amivel fel tudják térképezni környezetüket, hogy ki tudják kerülni a mozgásukat akadályozó tárgyakat. Ilyen szenzorok általában lézeren, ultrahangon és elektromágneses hullámon alapulnak.
Bár ezek az eszközök kellően pontosak és hatékonyak, az ember mindig is kutatta, hogyan tudná megtanítani a robotokat "emberi módon" látni.

2009 nyarán az iskola birtokába kerültek a Surveyor Corporation által épített és forgalmazott kamerával rendelkező mobil robotok. Az általam kitűzött cél jelen esetben egy Surveyor SVS robot (1. ábra) sík terepen történő navigálása volt. Ehhez szükséges, hogy a robot semmilyen más érzékelőt, csupán a kameráit felhasználva érzékelni tudja a távolságokat, tehát három dimenzióban (3D) lásson.

1. Eszközök bemutatása

1.1. SVS

Az általam használt Surveyor SVS roboton (1. ábra) megtalálható két darab Omnivision OV9655 1,3 megapixeles kamera, amik egymással párhuzamosan helyezkednek el 107,5 mm-es távolságra, két darab szervomotor a fej mozgatására és további két motor a gumilánctalpak hajtására.
A kamera modulon megtalálható az 500 MHz-es Analog Devices Blackfin BF537 processzor, aminek a teljesítménye 1000 MIPS (millió utasítás per másodperc).
A PC-vel a kommunikáció WLAN 802.11g-n történik, a képeket JPEG-be tömörítve küldi el.
A képek feldolgozása és a robot irányítása a PC-ről történik.

1. ábra

A Surveyor SVS (stereo vision system) robot elforgatott fejjel

1.2. OpenCV

A képfeldolgozáshoz tartozó feladatok elvégzéséhez az OpenCV (Open Source Computer Vision) ingyenes, nyílt forráskódú és keresztplatformos függvénykönyvtárat használtam.
Többnyire a robotikában, automatizált rendszereknél, valamint orvosi képfeldolgozásnál használatos.

1.3. SDL

A robot vezérléséhez szükségem volt egy olyan ingyenes könyvtárra, amit meghívhatok a választott programozási nyelvből és képes TCP/IP-n keresztül üzeneteket küldeni és fogadni.

Az SDL (Simple Directmedia Layer) egy széles körben elterjedt, nyílt forráskódú, C nyelven íródott, keresztplatformos könyvtár, aminek segítségével multimédiás szolgáltatásokkal ruházhatjuk fel programunkat.

Sztereó rekonstrukció

2. Sztereó rekonstrukció

A megfeleltetés megállapításához 320x240-es felbontású képeket használtam. Ennél nagyobb felbontásnál nem teljesülnek a valósidejű navigációhoz a sebesség feltételek.

A sűrű illesztés során az egyik képen szereplő összes pontnak megkeressük a párját úgy, hogy minden egyes pontot összehasonlítunk a másik kép összes pontjával, a kapott eredményt elmozdulás térképpel szoktuk ábrázolni.
Mivel elsődleges szempont a sebesség, ezért szükség volt arra, hogy csökkenjen a keresési tér. Egy szintbe kell hozni a képeket (rektifikáció), hogy az azonos képpárok egy sorba essenek, így egy pont párját csak a vele azonos sorban lévő pontok között kell megtalálni.

Ebben a fejezetben áttekintjük a 3D rekonstrukcióhoz szükséges lépéseket:
1. kamera középpontok, és képsíkok helyzetének meghatározása (kalibráció)
2. egymásnak megfelelő pontpárok keresése (illesztés)
3. kamera középpont összekötése a megfeleltetett vetülettel (trianguláció)

2.1. Kamera kalibráció

A kalibrációhoz objektumpontokra van szükség. Ezeket a pontokat egy ún. kalibrációs objektummal gyűjthetjük össze, ami egy sík, fekete-fehér négyzethálós sakktábla, melynek sarokpontjai jól meghatározhatóak. Az objektumpontok mozgását követve lehet következtetni a belső és külső paraméterekre. A belső paraméterek segítségével a különböző torzításokat lehet kijavítani (pl. a 2. ábrán látható párnatorzítást), míg a külső paraméterek segítségével tudunk majd 3D-t számolni.

A pontosabb kalibráció érdekében szükséges minél több felvételt csinálni. Fontos, hogy a képpár mindkét képén látszódnia kell a teljes sakktáblának, hogy képpáronként azonos objektumpontok legyenek (lásd 2. ábra).

2. ábra

A robot bal és jobb kamerájával készített eredeti, legjobb minőségű, 320x240-es képek. Az OpenCV berajzolta a talált sarkokat.

A kalibrálás eredményét lásd a 3. ábrán.

Ha a kamerák nem mozdíthatóak és a belső paramétereik változatlanok, akkor a kalibrálást elég egyszer elvégezni, így a kapott mátrixok és vektorok később is felhasználhatóak.

3. ábra

A kamera kalibrációja után a 2. ábrán látható képpár rektilineáris lett

2.2. Rektifikáció

Cél az, hogy a képpár egymásnak megfelelő sorai a rektifikálás után egybe essenek, így a sztereó megfeleltetés megbízható és megfelelően gyors lesz. Az eljárás után többek közt megkapjuk azt a mátrixot (Q), amivel majd a 3D-s koordinátákat ki tudjuk számolni.

Megvannak a képpárok, az átalakításához szükséges mátrixok, most már le kell képezni a régi képeket a rektifikáltra. A képek átalakítására után megkapjuk a rektifikált képeket (4. ábra).

4. ábra

A 3. ábra a rektifikáció után, ugyanazon pontok egy sorba kerültek

2.3. Sztereó megfeleltetés, BM algoritmus

Az illesztés egy "hibák abszolút összege"' (SAD) ablakot használ arra, hogy megtalálja az egyező pontokat a bal és a jobb rektifikált képen.
Az algoritmus csak az erősen illeszkedő (erősen textúrált) pontokat találja meg a két kép között. Így a gyengébben textúrált helyszínen, például egy folyosón kevesebb párosítást fog megtalálni.

A sztereó megfeleltetési mintaillesztő algoritmus végrehajtása három lépésben történik:
1. Előszűrés, normalizálja a kép fényességét és kiemeli a texturáltságát.
2. Egy SAD ablakkal megfeleltetéseket keres az egymásnak megfelelő sorok mentén.
3. Utószűrés, ahol törli a rosszul megfeleltetett találatokat.

Az első lépésben normalizálja a képeket úgy, hogy csökkenti a fényességbeli különbségeket és fokozza a kép textúráltságát.

5. ábra

A BM algoritmus 2. lépése

Az 5. ábrán látható, mi történik a második lépésben. Egy csúszó SAD ablak segítségével számoljuk a megfeleltetéseket. Minden egyes sajátossághoz a bal képen megkeressük a legjobb egyezőséget a jobb képen ugyanabban a sorban.

A minDisparity paraméterrel állítható, hogy a jobb képen hol kezdje el a keresést. Segítségével a nem szükséges távoli tárgyak kiszűrhetőek. Ha ez a szám negatív, akkor jobbra, ha pozitív akkor balra tolódik a jobb képen ez a pont.

A másik paraméter, a numberOfDisparities pedig azt adja meg, hogy a keresés során maximum hány pixelt mozdulhat el a SAD ablak, tehát mekkora tartományban keresse az adott pixel párját. Ha túl alacsonyra állítjuk be ezt az értéket, akkor a függvény a közeli tárgyakat nem találja meg.

Minél kisebb a numberOfDisparities és a minDisparity különbsége, annál kevesebb összehasonlítás szükséges, így gyorsabban jutunk eredményhez.

A megfeleltetés után a harmadik lépésben az utószűrés következik, amivel a zajok miatt talált párosításokat lehet törölni.

A BM függvény végeredménye (6. ábra) a bal képre elkészített sűrű elmozdulás térkép, az egyes pixelekhez az x-ben mért elmozdulás értéke kerül: xbal-xjobb.
Ahol nem sikerült párosítást találni, ott a pixel értéke minDisparity-1 lesz.

6. ábra

A kézzel megmért pixeltávolságok értékei. A jobb oldali képen lévő X-ek ből 320-at ki kell vonni.

bal

jobb

A felső képekből készített bal és jobb elmozdulás térképek. A színek jelzik az adott pixelen az elmozdulás értékét. A sötétkék jelzi azokat a pontokat, amiket nem tudott párosítani.

2.4. Bal-jobb konzisztencia

Előzőekben megismerhettük, hogyan készül el a bal képre az elmozdulás térkép, de a megfelelő beállítások ellenére is előfordul, hogy olyan párosításokat talál, ami nem létezik az eredeti képeken. Ezeknek a hibáknak egy kiküszöbölési lehetősége a bal-jobb konzisztencia (left-right consistency, LRC), amihez ki kell számolni a bal majd a jobb képre is az elmozdulás térképet, amik csak negatív előjelben térnek el egymástól. Ha ugyanannak a fizikai pontnak a bal és jobb elmozdulás térképen található elmozdulásának összege nagyobb 1-nél, akkor az hibás találatnak minősül és töröljük.
Az eredmény a 7. ábrán látható.

7. ábra

A 6. ábrán látható elmozdulás térképek felhasználásával a bal elmozdulás térkép LRC-vel megszűrt eredménye

2.5. 3D rekonstrukció

Miután megvan a normalizált elmozdulás térkép, 3D-s rekonstrukcióra, vagy mélységtérképre van szükség. A rektifikáció során kapott Q mátrixot meg kell szorozni az elmozdulás térképpel, így minden pixelre megkapjuk a 3D-s koordinátákat (X, Y, Z).

A kapott koordinátákból (8. ábra) csak a Z-t használtam fel, mert egy akadály megállapításához ez is elegendő.

8. ábra

A felső képpáron a 7. ábrán látható párosított pontok elmozdulás térképből készített 3D-s rekonstrukciónak a Z értékei mm-ben. A valóságban a bal oldali doboz 53 cm-re, míg a jobb 83 cm-re volt a robottól. Az alsó képpáron látható, hogy a merőleges sarkokat is érzékelni tudja.

Navigáció és eredmények

3. Navigáció

A navigálás úgy történik, hogy miután meglettek a 3D-s adatok, a program a tartományban megszámolja, hogy mennyi Z érték túl kicsi.
Ha ez a szám meghaladja a küszöböt, akkor előtte akadály van, el kell fordulni.
Ciklus Közeli pontok számának meghatározása Ha közeli pontok száma túl sok Állj Fejet balra, közeli pontok számának meghatározása Fejet jobbra, közeli pontok számának meghatározása Fejet egyenesbe fordítani Ha balra megfelelőbb, fordulj balra Ha jobbra megfelelőbb, fordulj jobbra Egyébként Előre Ciklus vége

Lehetett volna bonyolultabb algoritmus is, de az idő nagy részében a 3D-t próbáltam megjavítani.

4. Eredmények

Sikerült egy olyan programot írnom, amivel a robot ki tudja kerülni az elé került akadályokat.

4.1. Sebesség

Mivel a robot folyamatosan halad előre, ezért szükséges, hogy az akadály számolása real-time legyen. Sebességteszteket végeztem egy szálon, majd több szálon futó programmal, hogy a robot milyen sebességgel dolgozza fel a képpárokat. A fekete képnél a robot csak feketeséget látott, így a küldött kép a lehető legkisebb volt, statikusnál egy tárgyra irányultak a kamerák.
Az 1. táblázatban a másodpercenként feldolgozott képpárok száma szerepel.

1. táblázat

Az akadályszámolás sebességtesztje képpár/másodpercben. A továbbított kép: 320x240, legjobb minőség

4.2. Z értékek

A 2. táblázatban látható a számított, a valódi távolságok és a közöttük lévő eltérés.

2. táblázat

A mért és a tényleges távolságértékek, a Z tengely ellenkező irányba mutat, ezért negatívak az értékek. A 13. mérésnél a numberOfDisparities értéke 128 volt, a többinél 96. 2 méter után jelentősen nő az eltérés a mért és a valós távolságok között.

4.3. Pályabejárás

A program tudását kipróbáltam egy akadálypályán. Egy L alakú csatornán (9. ábra) kellett végighaladnia ütközés nélkül. A pályát teljesítette, a kihelyezett akadályokat fél méteren belül észlelte.

9. ábra

Az L alakú pálya bejárása. Először 1-es és 2-es irányba néz szét, majd a 2-es irány felé fordul. Ez is túl közel van, 3-as és 4-es irányba néz szét, majd 4-es irányba fordul. Továbbhalad, itt a falat távolabbról észreveszi, így 5-ös irányba is halad egy kicsit, mielőtt újra érzékelné a falat.

Videó

9. ábra videón

4.4. Szűk látótér

4.4.1. A probléma

A 7. ábrán látható, hogy az elmozdulás térkép bal és jobb oldaláról is hiányoznak párosítások. Az elmozdulásokat kereső függvény csak abban az esetben keresi meg egy pont párját, ha a megadott teljes pixeltartományban (numberOfDisparities, lásd 2.3. fejezet) végig tudja nézni, ellenkező esetben annak a pixelnek "nem párosított"' értéket ad.
A bal-jobb konzisztencia miatt tovább szűkül a látótér (10. ábra).

A 3. és a 4. táblázatból kiolvasható, hogy ha széles látószöget akarunk, akkor közelre nem látunk, ha közelre akarunk látni, akkor keskeny a látószög. Ha szűk a látótér, akkor nincs elegendő információ ahhoz, hogy akadály esetén kiválasszuk a megfelelő irányt.

3. táblázat

A bal kamera előtt fél méterre, oldalra ellátható maximális távolság különböző numberOfDisparities értékeknél

4. táblázat

Különböző numberOfDisparities értékeknél érzékelhető legkisebb távolság cm-ben

10. ábra

Minél nagyobb a numberOfDisparities értéke, annál szűkebben sávban lát a robot. A képeken elmozdulás térképek láthatóak fekete-fehérben (minél fehérebb, annál közelebb van).

4.4.2. Fejmozgatás

A kamerák egy olyan fejen helyezkednek el, ami egy két szervóból álló fejmozgató egységhez kapcsolódik, így a fejet horizontálisan és vertikálisan is lehet forgatni 45-45 fokban. A 11. ábrán látható, hogy a mozgatás segítségével akadály esetén már elengedő információ áll rendelkezésünkre, hogy eldöntsük, hogy merre menjen tovább a robot.

11. ábra

Fejmozgatás nélkül

Fejmozgatással
Az oldalra látás különböző numberOfDisparities értéknél (kék: 96, sárga: 80, zöld: 64). A robot felőli körív jelzi, hogy milyen közelre képes látni az adott értéknél, a távoli pedig kvázi végtelen.

4.4.3. A kép bővítése

Ahogy az előző fejezetben említettem, ahol nem tud végigfutni a párosítást kereső függvény, oda "nem párosított" érték kerül. Felmerül a kérdés, mi lenne, ha kibővítenénk a képeket, hogy beleférjen a numberOfDisparities-s sáv, és így nem vágna le fontos részeket.

Még az elmozdulások keresése előtt, a rektifikált képek bal és jobb oldalát kibővítem a numberOfDisparities értékének megfelelő fehér sávval (12. ábra). A sávbővítés nem okoz plusz párosításokat, mert nem elég textúrált, de a toldalékok határán mesterséges függőleges élek jöhetnek létre, amiket viszont észrevesz a bal-jobb konzisztencia algoritmusa.

12. ábra

A kibővített rektifikált kép, ha a numberOfDisparities értéke 96. A láthatóság kedvéért a fehér sávot feketére színeztem.

A 13. ábrán látható képek a 10. ábra képéből készültek, így jól összehasonlítható az eredmény. Ezzel az eljárással a numberOfDisparities értékétől függetlenül mindig ugyanolyan széles elmozdulás térképet kapunk. A jobb elmozdulás térképen látható, hogy megtalálta a mesterséges élt, de a bal-jobb konzisztencia kiszűrte.

13. ábra

Bal elmozdulás térkép, numberOfDisparities=96

Jobb elmozdulás térkép, numberOfDisparities=96

Javított, numberOfDisparities=96

Javított, numberOfDisparities=160
A kibővített képek felhasználásával készített elmozdulás térképek. A 96-os és 160-as értékekkel is ugyanakkora területet lehet belátni.

Bár a fejmozgásra továbbra is szükség van, de szinte bármilyen numberOfDisparities értéknél belátjuk a teljes tartományt (bal 20 cm, jobb 35 cm, lásd 3. táblázat). A számításigény is nőtt, de maradt a 3-4 FPS közötti érték.

A bővítést felhasználva a 8. ábra felső képpárjára 14. ábrát kapjuk.

14. ábra

A 8. ábra felső képpárja a bővítés alkalmazása után

Konklúzió és a többi

5. Konklúzió és további lépések

Bemutattam egy sztereó rekonstrukción alapuló robot navigációs alkalmazást, amely jól hozzáférhető elemekre épül, de számos módosítást tartalmaz. A megbízhatóság érdekében alkalmaztam a bal-jobb konzisztencia megszorítást, és a párhuzamos implementáció segítségével valós idejű feldolgozást értem el. Megmutattam, hogy a robot képes a megfelelően textúrált tárgyak határát felismerni és elnavigálni A pontból B pontba. Az elmozdulások és a 3D-s adatok nem voltak teljesen pontosak, de a biztos navigáláshoz ez is megfelelő volt.

További lépések lehetnek az erősen textúrált padlók kiszűrése, hogy minél több helyen képes legyen navigálni. Szeretném, ha fel tudná majd ismerni, hogy milyen akadályon tud még áthaladni. Tervezem még, hogy mozgás közben letárolom a 3D-s adatokat és a hozzájuk tartozó távolságokat, majd ezekből rekonstruálom és megjelenítem a bejárt pályát. Másik célom az, hogy kihasználom a robot mozgékonyságát arra, hogy egy helyben, minden lehetséges irányból felvett képből rekonstruáljam a színteret.

Tervezem még, hogy mivel már nem áll a rendelkezésemre a robot, készítek egy emulátort. Így a már lementett képek segítségével tovább tudom javítani, bővíteni a programomat.

Köszönetnyilvánítás

A dolgozatom végén szeretném megköszönni Dr. Kovács Tamás és Pásztor Attila oktatóknak, hogy a Surveyor SVS robototot a munka idejére a rendelkezésemre bocsájtották.

Továbbá köszönöm Megyesi Zoltán oktatónak, hogy az elmélet jobb megértése érdekében a segítségemre volt.

Ami kimaradt

Megjavult 3D további következményei

Ha még emlékeztek, az eddigi 3D-s képeknél csak a Z (távolság) értéket vettem figyelembe. Most, hogy megjavult a 3D, megnéztem a teljes X,Y,Z értékeket, hogy mit adnak: