Surveyor SVS robot navigáció sztereó

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´ eg´ evel ´ am1 , Megyesi Zolt´an2 M´esz´ aros Ad´ 1

2

Kecskem´eti F˝ oiskola G´epipari ´es Automatiz´ al´ asi M˝ uszaki F˝ oiskolai Kar [email protected] Kecskem´eti F˝ oiskola G´epipari ´es Automatiz´ al´ asi M˝ uszaki F˝ oiskolai Kar [email protected]

Absztrakt. Bemutatunk egy, OpenCV-ben ´ırt, 3D rekonstrukci´ on alapul´ o val´ os idej˝ u robot navig´ aci´ os alkalmaz´ ast, amely lehet˝ ov´e teszi, hogy a Surveyor SVS robot s´ık terepen akad´ alyokat tudjon kiker¨ ulni. A robot sztere´ o kamer´ ainak k´epeit vezet´ek n´elk¨ uli h´ al´ ozaton egy szem´elyi sz´ am´ıt´ og´epre tov´ abb´ıtottuk, ahol val´ os idej˝ u sztere´ o rekonstrukci´ os m´ odszert alkalmaztunk. A kapott 3D sz´ınt´er elemz´ese ut´ an a robot a felall´ıtott szab´ ´ alyok alapj´ an d¨ ont, hogy melyik ir´ anyba ker¨ ulje ki az akad´ alyokat, mik¨ ozben folyamatosan friss´ıti a l´ atott sz´ınteret. Mindehhez pontos kamera kalibr´ aci´ ora ´es megb´ızhat´ o s˝ ur˝ u illeszt´esi algoritmus megval´ os´ıt´ as´ ara volt sz¨ uks´eg. Az alkalmazott m´ odszerek le´ır´ asa ut´ an berendezett akad´ alyp´ aly´ an v´egzett k´ıs´erletek eredm´enyeit mutatjuk be.

1.

Bevezet´ es

A 3D sz´ınt´er rekonstrukci´ o alkalmaz´asi ter¨ uletei k¨oz¨ott fontos helyet foglal el a robot navig´aci´ o. Az ´ altalunk kit˝ uz¨ott c´el jelen esetben egy Surveyor SVS robot (1. ´ abra) s´ık terepen t¨ ort´en˝o navig´al´asa volt. Ezekn´el a feladatokn´al a klasszikus keskeny b´ azist´ avols´ag´ u rekonstrukci´o [6] alkalmaz´as´at nehez´ıti, hogy a robotokra szerelt kamer´ ak sokszor gyenge optik´ at ´es alacsony felbont´ as´ u szenzorokat haszn´alnak, tov´abb´a, hogy a robotra integr´alt processzorok sz´am´ıt´asi kapacit´ asa nem el´eg a val´ os idej˝ u s˝ ur˝ u illeszt´es megval´os´ıt´as´ahoz. A sebess´egi probl´em´ ak megold´as´ ara egy lehet˝os´eg a ritka 3D rekonstrukci´o alkalmaz´asa. Ilyenkor csak j´ ol megfeleltethet˝o jellemz˝ o pontok vagy sarokpontok illeszt´es´ere ker¨ ul sor, azonban ebben az esetben kapott ritka pontfelh˝ob˝ol ´all´o sz´ınt´er elemz´ese (els˝ osorban annak eld¨ ont´ese, hogy van-e akad´aly a robot el˝ott, vagy sem) kev´esb´e megb´ızhat´ o. Hogy a s˝ ur˝ u illeszt´es sz´am´ıt´asig´enyeit lek¨ uzdj¨ uk, k¨ uls˝ o sz´ am´ıt´ asi kapacit´ ast haszn´altunk, ´es hat´ekony illeszt´esi algoritmust kerest¨ unk. Az illeszt´esi eredm´enyk´ent kapott s˝ ur˝ u pontfelh˝ot elemezn¨ unk kell, hogy megtudjuk, hol helyezkednek el akad´alyok a robot u ´tj´aban. Megalkottunk egy egyszer˝ u d¨ ont´esi logik´at, amely alapj´an a robot k´epes a pontfelh˝o alapj´an kiker¨ ulni az akad´alyokat. Jelen cikkben el˝ osz¨ or ´ attekintj¨ uk az alkalmazott 3D rekonstrukci´os m´odszert, majd le´ırjuk a haszn´alt navig´aci´ os szab´alyokat. Ezek ut´an berendezett akad´alyp´ aly´akon v´egzett k´ıs´erletek eredm´enyeit mutatjuk be.

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

275

1. ´ abra: A Surveyor SVS (stereo vision system) robot elforgatott fejjel

1.1.

Eszk¨ oz¨ ok bemutat´ asa

Az ´ altalunk haszn´alt Surveyor SVS roboton (1. ´abra) megtal´alhat´o k´et darab Omnivision OV9655 1,3 megapixeles kamera, amik egym´ assal p´ arhuzamosan helyezkednek el 107,5 mm-es t´ avols´agra, k´et darab szervomotor a fej mozgat´as´ara ´es tov´abbi k´et motor a gumil´anctalpak hajt´ as´ara. A kameramodulok nincsenek ler¨ogz´ıtve, az egyik oldalon a foglalat tartja, a m´asik oldalon csak t´ avtart´ok vannak, amiken nem lehet ´ all´ıtani. A PC-vel a kommunik´aci´ o WLAN 802.11g-n t¨ ort´enik. Tov´abbi inform´aci´ ok az SVS-r˝ol megtal´alhat´oak a gy´ art´ o honlapj´an [1, 2]. A k´epek feldolgoz´ asa ´es a robot ir´ any´ıt´asa a PC-r˝ol t¨ ort´enik. Az adatok feldolgoz´ as´ ahoz az ingyenes ´es ny´ılt forr´ask´od´ u OpenCV 2.1-es verzi´oj´ at [3, 5] haszn´altuk.

2.

Sztere´ o rekonstrukci´ o

A megfeleltet´es meg´allap´ıt´ as´ ahoz 320 × 240-es felbont´ as´ u k´epeket haszn´altunk. Enn´el nagyobb felbont´ asn´ al nem teljes¨ ulnek a val´osidej˝ u navig´aci´ ohoz a sebess´eg felt´etelek. A s˝ ur˝ u illeszt´es sor´an az egyik k´epen szerepl˝ o ¨osszes pontnak megkeress¨ uk a p´ arj´ at u ´gy, hogy minden egyes pontot ¨osszehasonl´ıtunk a m´asik k´ep ¨osszes pontj´ aval, a kapott eredm´enyt elmozdul´as t´erk´eppel szoktuk ´abr´ azolni. Ez 320 × 240-es felbont´ as´ u k´epek eset´en k¨ozel 6 · 109 ¨osszehasonl´ıt´as. Mivel els˝odleges

276

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

szempont a sebess´eg, ez´ert sz¨ uks´eg volt arra, hogy cs¨okkenjen a keres´esi t´er. Egy szintbe kell hozni a k´epeket (rektifik´ aci´ o), hogy az azonos k´epp´arok egy sorba essenek, ´ıgy egy pont p´ arj´ at csak a vele azonos sorban l´ev˝o pontok k¨oz¨ott kell megtal´ alni, jelen esetben 320 k¨oz¨ ul, az ¨osszehasonl´ıt´asok sz´ama ´ıgy k¨or¨ ulbel¨ ul 24,5·106-ra cs¨ okken. Ebben a fejezetben ´ attekintj¨ uk a 3D rekonstrukci´ohoz sz¨ uks´eges l´ep´eseket: 1. kamera k¨oz´eppontok, ´es k´eps´ıkok helyzet´enek meghat´aroz´asa (kalibr´ aci´ o) 2. egym´ asnak megfelel˝o pontp´arok keres´ese (illeszt´es) 3. kamera k¨oz´eppont ¨ osszek¨ ot´ese a megfeleltetett vet¨ ulettel (triangul´aci´ o) 2.1.

Kamera kalibr´ aci´ o

A 3D rekonstrukci´ ohoz sz¨ uks´egesek a kamer´ ak bels˝o ´es k¨ uls˝ o param´eterei, ezeket a kalibr´ aci´ o sor´an kapjuk meg. Az OpenCV-ben a cvStereoCalibrate f¨ uggv´eny haszn´alat´aval tudjuk egyszerre kalibr´ alni mindk´et kamer´ at. A f¨ uggv´enynek ehhez objektumpontokra van sz¨ uks´ege. Ezeket a pontokat egy u ´n. kalibr´ aci´ os objektummal gy˝ ujthetj¨ uk ¨ossze, ami egy s´ık, fekete-feh´er n´egyzeth´al´os sakkt´abla, melynek sarokpontjai j´ol meghat´ arozhat´oak. Az objektumpontok mozg´as´at k¨ovetve lehet k¨ovetkeztetni a bels˝o ´es k¨ uls˝ o param´eterekre. Ez az elj´ar´as Zhangt´ol [7, 8] ´es Sturmt´ ol [9] sz´armazik. A sakkt´ abla sarkai k´epenk´ent a cvFindChessboardCorners f¨ uggv´ennyel gy˝ ujthet˝oek ¨ ossze, amely Harris sarokdetekt´ al´ot haszn´al. A t´ abl´ at 3 szabads´agfok ment´en kell forgatni, hogy tudjunk 3D-ben kalibr´ alni. A pontosabb kalibr´ aci´ o ´erdek´eben sz¨ uks´eges min´el t¨ obb felv´etelt csin´alni. Fontos, hogy a k´epp´ar mindk´et k´ep´en l´atsz´odnia kell a teljes sakkt´abl´ anak, hogy k´epp´aronk´ent azonos objektumpontok legyenek (l´ asd 2. ´ abra). A f¨ uggv´eny bemenete 8 bites sz¨ urke´ arnyala-

2. ´ abra: A robot bal ´es jobb kamer´ aj´ aval k´esz´ıtett eredeti, legjobb min˝ os´eg˝ u, 320×240es k´epek. Az OpenCV berajzolta a tal´ alt sarkokat.

tos k´ep, amin szerepel a sakkt´abla. A robott´ ol 3 csatorn´as, csatorn´ank´ent 8 bites sz´ınm´elys´eg˝ u k´ep ´erkezik, ezt kell ´atalak´ıtani a megfelel˝o form´atumra. A

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

277

cvCvtColor az ´ atalak´ıt´ ashoz a lum´at (Y) haszn´alja, ami az R, G ´es B s´ ulyozott ´atlagak´ent sz´ amolhat´ o: Gray : Y = 0.299 · R + 0.587 · G + 0.114 · B A Nemzetk¨ozi T´ avk¨ ozl´esi Egyes¨ ulet R´ adi´ ot´avk¨ozl´esi Szektor (ITU-R) 601-es aj´ anl´ asa is tartalmazza ezt az ¨osszef¨ ugg´est a monokr´ om k´ep megjelen´ıt´es´ehez [4]. A cvFindChessboardCorners ´altal visszaadott sarkok csak hozz´ avet˝olegesek. Gyakorlatban ez azt jelenti, hogy csak a kamera fizikai hat´arain bel¨ ul tal´ alja meg a sarkot, ami egy pixel. Egy k¨ ul¨on f¨ uggv´eny sz¨ uks´eges ahhoz, hogy finom´ıtsa a sarkok poz´ıci´oj´ at subpixel pontoss´aggal. Ehhez az eredeti sz¨ urke´ arnyalatos k´epet ´es a sarkok el˝ obb megkapott hozz´ avet˝oleges elhelyezked´es´et kell megadni. Az elj´ ar´ as ´ attekint´es´ehez ´es u ´jabb technik´ak´ert l´asd Lucchese [10] ´es Chen [11]. A t¨ obb felv´etelb˝ ol kapott objektumpontokb´ol cvStereoCalibrate meghat´ arozza a kamer´ ak bels˝o param´etereit k¨ ul¨on-k¨ ul¨on (kamera m´atrix: M, torz´ıt´asi param´eter: D) valamint a k´et kamera kapcsolat´at jellemz˝ o k¨ uls˝ o param´etereket, a forgat´ asi m´atrixot (R) ´es az eltol´ as vektort (T), opcion´alis esetben az esszenci´alis (E) ´es fundament´ alis m´atrixot (F). A bels˝o param´eterek meghat´aroz´as´ara a f¨ uggv´eny Zhang [7] elj´ ar´ as´ an alapul´ o algoritmust, a torz´ıt´asi param´eterekhez pedig Brown [12] m´odszer´en alapul´ o algoritmust haszn´al. A kalibr´ al´as eredm´eny´et l´asd a 3. ´ abr´ an. Ha a kamer´ ak nem mozd´ıthat´oak ´es a bels˝o param´etereik v´altozatlanok, akkor a kalibr´ al´ ast el´eg egyszer elv´egezni, ´ıgy a kapott m´atrixok ´es vektorok k´es˝ obb is felhaszn´alhat´ oak.

3. ´ abra: A kamera kalibr´ aci´ oja ut´ an a 2. ´ abr´ an l´ athat´ o k´epp´ ar rektiline´ aris lett

2.2.

Rektifik´ aci´ o

C´el az, hogy a k´epp´ar egym´ asnak megfelel˝o sorai a rektifik´al´as ut´an egybe essenek, ´ıgy a sztere´o megfeleltet´es megb´ızhat´o ´es megfelel˝oen gyors lesz. Az OpenCV lehet˝ os´egei k¨oz¨ ul Bouguet algoritmuson alapul´ o f¨ uggv´eny´et haszn´altuk

278

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

(cvStereoRectify), mert a rot´ aci´ os m´atrix (R) ´es az eltol´ asi vektor (T) m´ar rendelkez´esre ´ all a kalibr´ aci´ ob´ol. A f¨ uggv´enyt˝ ol megkapjuk a bal ´es a jobb k´epre a rektifik´alt rot´ aci´ os (R1, R2), vet´ıt´esi m´atrixokat (P1, P2) ´es a projekt´ıv m´atrixot (Q), amivel majd a 3D-s koordin´at´akat ki tudjuk sz´amolni. A param´eterben meg kell adni, hogy a k´epen l´ev˝ o nem haszn´alt r´eszeket ne vegye figyelembe, k¨ ul¨onben a kapott Q m´atrixszal nem lehet a val´os t´ avols´agokat kisz´ amolni. A Bouguet algoritmus az el˝ osz¨ or publik´al´ o Tsai [13] ´es Zhang [7, 8] elj´ar´as´anak egy befejez´ese ´es egyszer˝ us´ıt´ese. Megvannak a k´epp´arok, az ´atalak´ıt´as´ahoz sz¨ uks´eges m´atrixok, most m´ar le kell k´epezni a r´egi k´epeket a rektifik´altra. A k´epek ´atalak´ıt´as´ara a cvRemap szolg´ al, ami lek´epezi a r´egi k´ep pontjait egy u ´jra (l´asd 4. ´abra), ´ıgy megkapjuk a rektifik´alt k´epeket. A lek´epez´eshez sz¨ uks´eges a k´eppel megegyez˝o m´eret˝ u m´atrixokat a cvInitUndistortRectifyMap-b´ol nyerhetj¨ uk.

4. ´ abra: A 3. ´ abra a rektifik´ aci´ o ut´ an, ugyanazon pontok egy sorba ker¨ ultek

2.3.

Sztere´ o megfeleltet´ es, BM algoritmus

Az OpenCV implement´ alt egy gyors ´es eredm´enyes mintailleszt˝ o sztere´o algoritmust, a cvFindStereoCorrespondenceBM-t, ami a Kurt Konolige [14] elj´ar´as´an alapul. Az illeszt´es egy ,,hib´ ak abszol´ ut ¨osszege” ablakot haszn´al arra, hogy megtal´ alja az egyez˝o pontokat a bal ´es a jobb rektifik´alt k´epen: X |I1 (i, j) − I2 (x + i, y + j)| (1) SAD : (i,j)∈W

Az algoritmus csak az er˝ osen illeszked˝o (er˝ osen text´ ur´alt) pontokat tal´ alja meg a k´et k´ep k¨oz¨ott. ´Igy a gyeng´ebben text´ ur´alt helysz´ınen, p´eld´ aul egy folyos´on kevesebb p´ aros´ıt´ ast fog megtal´alni. A sztere´o megfeleltet´esi mintailleszt˝ o algoritmus v´egrehajt´asa h´ arom l´ep´esben t¨ ort´enik:

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

279

1. El˝ osz˝ ur´es, normaliz´ alja a k´ep f´enyess´eg´et ´es kiemeli a textur´alts´ag´at. 2. Egy SAD ablakkal megfeleltet´eseket keres az egym´ asnak megfelel˝o sorok ment´en. 3. Ut´osz˝ ur´es, ahol t¨ orli a rosszul megfeleltetett tal´ alatokat. Az els˝o l´ep´esben normaliz´ alja a k´epeket u ´gy, hogy cs¨okkenti a f´enyess´egbeli k¨ ul¨onbs´egeket ´es fokozza a k´ep text´ ur´alts´ag´at. Ezt u ´gy teszi, hogy v´egigmegy a k´epen egy 5 × 5-¨ os, 7 × 7-es (ez az alap´ertelmezett) vagy ak´ ar 21 × 21-es (ez a maximum) ablakkal a k´epen. Az ablak k¨ozep´en tal´ alhat´o pontot (Ic ) lecser´eli a (2) k´epletb˝ ol kisz´ am´ıtott ´ert´ekre, ahol a I¯ az ablakban szerepl˝ o ´ert´ekek ´atlaga, az Icap pedig a megadott pozit´ıv sz´am, aminek az alap´ertelmezett ´ert´eke 30. 
 ¯ −Icap , Icap (2) Ic = min max Ic − I,

x

x

Bal

minDisparity

Jobb

y

y (x0,y0)

(x0,y0)

numberOfDisparities

5. ´ abra: A BM algoritmus 2. l´ep´ese

Az 5. ´ abr´ an l´athat´o, mi t¨ ort´enik a m´asodik l´ep´esben. Egy cs´ usz´ o SAD ablak seg´ıts´eg´evel sz´ amoljuk a megfeleltet´eseket. Minden egyes saj´atoss´ aghoz a bal k´epen megkeress¨ uk a legjobb egyez˝os´eget a jobb k´epen ugyanabban a sorban. A minDisparity param´eterrel ´all´ıthat´o, hogy a jobb k´epen hol kezdje el a keres´est. Seg´ıts´eg´evel a nem sz¨ uks´eges t´ avoli t´ argyak kisz˝ urhet˝oek. Alap´ertelmezetten ez az ´ert´ek 0, ilyenkor a bal k´epen szerepl˝ o (x0 , y0 ) pontot a jobb k´epen is ugyanabban a (x0 , y0 )-ban kezdi el keresni. Ha ez a sz´am negat´ıv, akkor a jobbra, ha pozit´ıv akkor balra tol´ odik a jobb k´epen ez a pont. A m´asik param´eter a numberOfDisparities pedig az adja meg, hogy a keres´es sor´an maximum h´ any pixelt mozdulhat el a SAD ablak. A param´eternek pozit´ıvnak ´es 16-tal oszthat´onak kell lennie, alap´ertelmezett ´ert´eke 64. Kamera param´etereit˝ ol f¨ ugg, de ha t´ ul alacsonyra ´all´ıtjuk be ezt az ´ert´eket, akkor a f¨ uggv´eny a k¨ozeli t´ argyakat nem tal´ alja meg. Min´el kisebb a numberOfDisparities ´es a minDisparity k¨ ul¨onbs´ege, ann´al kevesebb ¨ osszehasonl´ıt´ as sz¨ uks´eges, ´ıgy gyorsabban jutunk eredm´enyhez.

280

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

A megfeleltet´es ut´an a harmadik l´ep´esben az ut´osz˝ ur´es k¨ovetkezik. Gyakran el˝ ofordulhat, hogy egy zajos k´epen a f¨ uggv´eny egy er˝os k¨ozponti cs´ ucsot k¨or¨ ulvev˝ o r´eszeken tal´ al p´ aros´ıt´ asokat. Ezeknek a hib´as p´ aroknak kisz˝ ur´es´ere az egyik lehet˝ os´eg az uniquenessRatio param´eter. Ez egy sz´azal´ekban kifejezett ´ert´ek, ami meghat´arozza a legjobb ´ert´ek (min match) ´es az aktu´alis ´ert´ek (match val) k¨ozti maxim´ alis k¨ ul¨onbs´eget. Ha egy ´ert´ek a (3) ¨osszef¨ ugg´es alapj´an a param´etert˝ ol nagyobb sz´ azal´ekban t´er el a legjobb ´ert´ekt˝ol, az t¨ orl˝odik. uniquenessRatio >

match val − min match min match

(3)

A m´asodik lehet˝ os´eg a textureThreshold, ami egy hat´art szab meg a SAD ablaknak, hogy a nagy cs´ ucsokat jelent˝ o zajos r´eszeket ne vegye figyelembe. A harmadik lehet˝ os´eg pedig az u ´gynevezett folt (speckle) haszn´alata. Minden mintailleszt´est haszn´al´ o algoritmusn´al probl´ema l´ep fel, ha egy objektum hat´ ar´ ahoz ´er. Egy lok´alis ter¨ ulet egyik r´esz´en kicsik a m´asik r´esz´en pedig magasak az elmozdul´asi ´ert´ekek. A hat´ arvonal menti p´ arok megel˝oz´es´ere haszn´alhatjuk a folt detekt´al´ ot. A BM f¨ uggv´eny v´egeredm´enye a bal k´epre elk´esz´ıtett s˝ ur˝ u elmozdul´as t´erk´ep, az egyes pixelekhez az x-ben m´ert elmozdul´as ´ert´eke ker¨ ul: xl − xr . Ahol nem siker¨ ult p´ aros´ıt´ ast tal´ alni, ott a pixel ´ert´eke minDisparity − 1 lesz. 2.4.

Bal-jobb konzisztencia

El˝ oz˝oekben megismerhett¨ uk, hogyan k´esz¨ ul el a bal k´epre az elmozdul´as t´erk´ep, de a megfelel˝o be´ all´ıt´ asok ellen´ere is el˝ofordul, hogy olyan p´ aros´ıt´asokat tal´ al, ami nem l´etezik az eredeti k´epeken. Ezeknek a hib´aknak egy kik¨ usz¨ ob¨ol´esi lehet˝os´ege a bal-jobb konzisztencia (left-right consistency, LRC), amihez ki kell sz´amolni a bal majd a jobb k´epre is az elmozdul´as t´erk´epet. A BM f¨ uggv´eny csak bal k´epre tudja megcsin´ alni az elmozdul´as t´erk´epet, ´ıgy sz¨ uks´eges az alapbe´ all´ıt´ asok megv´ altoztat´ asa. A 6. ´abr´ an l´athat´oak a k´ezzel m´ert elmozdul´asok, amik megegyeznek a 7. ´abr´ an l´athat´o sz´amolt elmozdul´asokkal. A bal elmozdul´as t´erk´ep sz´ am´ıt´as´ahoz a numberOfDisparities ´ert´ek´et 96-ra vett¨ uk, a minDisparity ´ert´eket pedig hagytuk 0-n, minden m´as az alap´ertelmezett maradt. A jobbhoz a BM param´eterekn´el felcser´elt¨ uk a jobb ´es a bal k´epet, a numberOfDisparities ´ert´ek maradt 96, a minDisparitity ´ert´eket pedig −96-ra ´all´ıtottuk, ´ıgy az eredeti pontt´ol a keres´est 96 pixellel jobbra kezdi el. A 7. ´abr´ an l´athat´o, milyen elmozdul´asokat sz´amolt. A bal ´es jobb elmozdul´as t´erk´epek csak negat´ıv el˝ojelben t´erhetnek el egym´ ast´ ol. Minden egyes pixelt meg kell vizsg´alni, ha a (4) egyenl˝ otlens´eg teljes¨ ul, akkor a tal´ alt p´ aros´ıt´ as j´ o (8. ´ abra). disp left(x, y) + disp right(x − disp left(x, y), y) < threshold

(4)

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

281

6. ´ abra: A k´ezzel megm´ert pixelt´ avols´ agok ´ert´ekei. A jobb oldali k´epen l´ev˝ o X-ek b˝ ol 320-at ki kell vonni.

bal

jobb

7. ´ abra: A 6. ´ abra alapj´ an a bal ´es jobb k´epekre k´esz´ıtett elmozdul´ as t´erk´epek

8. ´ abra: A 7. ´ abr´ an l´ athat´ o elmozdul´ as t´erk´epek felhaszn´ al´ as´ aval a bal elmozdul´ as t´erk´ep LRC-vel megsz˝ urt eredm´enye

282

2.5.

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

3D rekonstrukci´ o

Miut´ an megvan a normaliz´ alt elmozdul´as t´erk´ep, 3D-s rekonstrukci´ora, vagy m´elys´egt´erk´epre van sz¨ uks´eg. A cvReprojectImageTo3D az elmozdul´as t´erk´ep ´es a Q m´atrix seg´ıts´eg´evel kisz´ amolja minden egyes pixelhez (x, y) az X, Y ´es Z koordin´ at´akat: T

[X Y Z W ] = Q · [x y disparity(x, y) 1] 3DImage(x, y) = (X/W, Y /W, Z/W )

T

A kapott koordin´ at´akb´ ol (9. ´ abra) csak a Z-t haszn´altuk fel, mert egy akad´aly meg´allap´ıt´ as´ ahoz ez is elegend˝o.

9. ´ abra: A 8. ´ abr´ an l´ athat´ o p´ aros´ıtott pontok elmozdul´ as t´erk´epb˝ ol k´esz´ıtett 3D-s rekonstrukci´ onak a Z ´ert´ekei mm-ben

3.

Navig´ aci´ o

Az el˝ oz˝o fejezetben ismertetett, megfelel˝oen param´eterezett elj´ar´asokat felhaszn´ alva egy megb´ızhat´ o algoritmust kellett kidolgozni, amivel a robot elker¨ ulheti az u ¨tk¨ oz´est. 3.1.

M´ er´ esek

A c´el az volt, hogy a robot egyenletes sebess´eggel haladjon el˝ore, ´es mikor akad´alyt ´eszlel maga el˝ ott, akkor ´alljon meg, v´alassza ki a megfelel˝o ir´ anyt, forduljon arra, ami megfelel˝o ´es haladjon tov´abb. A robot k¨ ul¨onb¨ oz˝o sebess´egekkel tud haladni, de ha t´ ul gyorsan megy, akkor a fejneh´ez robot csak mos´odott k´epeket tud tov´abb´ıtani az er˝os r´azk´od´as miatt, amit nem lehet felhaszn´alni a feladathoz. A helyes sebess´eg az alap´ertelmezett˝ ol 6 egys´egnyivel lassabb volt.

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

283

Teszteket v´egezt¨ unk ahhoz, hogy meg´allap´ıtsuk, hogy a bal k´epre k´esz´ıtett 3D-s k´epb˝ol a Z ´ert´ekeket, amik mm-ben vannak megadva, hogyan ´ertelmezze. Az elmozdul´as t´erk´epekb˝ ol meg´allap´ıthat´o volt, hogy a robot, a t´ avoli kamer´ak miatt, k¨or¨ ulbel¨ ul f´el m´etern´el t´ avolabb l´ev˝ o t´ argyaknak a k´ep´et tudja p´ aros´ıtani, ez´ert a f´el m´eteren t¨ ort´en˝o fordul´as lett a c´el. A −500-n´al kisebb Z ´ert´ekkel rendelkez˝o pixeleket fogja sz´amolni a program. Meg kellett tov´abb´a ´ allap´ıtani, hogy a k´epen szerepl˝ o pixelek k¨oz¨ ul melyek tartoznak a robottal szemben l´ev˝ o t´erre. A m´er´eseink szerint (l´asd pl. 6. ´abra) az X ´ert´ek´enek 130 ´es 210 k¨oz´e kell esnie, az Y-ra nem tett¨ unk felt´etel. T¨obb terept´ arggyal t¨ ort´en˝o k´ıs´erlet ut´an, ha 3500 darab −500-n´al kisebb Z ´ert´eket tal´ al a megadott k´eptartom´anyban, akkor az akad´alynak min˝os¨ ul. Ha a numberOfDisparities ´ert´eke 96, akkor 0–102 k¨oz¨otti X-en nem lesz p´ aros´ıt´ as, 128 ´ert´ek eset´en pedig 0–134 pixelen. A 8. ´ abr´ an ´eszrevehet˝ o, hogy az LRC sz´amol´ as ut´an a k´ep bal ´es jobb sz´el´en szinte alig tal´ alhat´ o p´ aros´ıt´ as. Ahhoz, hogy a robot meg´allap´ıtsa, balra vagy jobbra induljon el, el kell forgatnia a fej´et. Balra ´es jobbra maximum 45◦ –45◦-s sz¨ ogbe tudja elford´ıtani a kamer´ akat, ez´ert erre a sz¨ogre kellett meg´allap´ıtani azokat a motorutas´ıt´ as ´ert´eket, amelyre pont ennyit tud fordulni. Ezeket az ´ert´ekeket k¨ ul¨onb¨ oz˝o padl´ okn´ al mindig u ´jra ki kell m´erni. A fejforgat´as parancs kiad´ asa ut´an egy kis sz¨ unetet kell beiktatni, mert a mozgat´as is id˝obe telik, ha t´ ul hamar h´ıv le k´epet a program, akkor az eredm´eny ugyancsak elmos´odott k´ep lesz, amin alig tal´ alhat´ o p´ aros´ıt´as. 3.2.

Algoritmus

A navig´al´ as u ´gy t¨ ort´enik, hogy miut´an meglettek a 3D-s adatok, a program a tartom´ anyban megsz´amolja, hogy mennyi Z ´ert´ek t´ ul kicsi. Ha ez a sz´am meghaladja a k¨ usz¨ ob¨ot, akkor el˝otte akad´aly van, el kell fordulni. Ciklus K¨ ozeli pontok sz´ am´ anak meghat´aroz´asa Ha k¨ozeli pontok sz´ ama t´ ul sok ´ Allj Fejet balra, k¨ozeli pontok sz´am´ anak meghat´aroz´asa Fejet jobbra, k¨ozeli pontok sz´am´ anak meghat´aroz´asa Fejet egyenesbe ford´ıtani Ha balra megfelel˝obb, fordulj balra Ha jobbra megfelel˝obb, fordulj jobbra Egy´ebk´ent El˝ ore Ciklus v´ege

4.

Eredm´ enyek

Siker¨ ult egy olyan programot ´ırnunk, amivel a robot ki tudja ker¨ ulni az el´e ker¨ ult akad´alyokat.

284

4.1.

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

Elmozdul´ asok

A BM algoritmus seg´ıts´eg´evel siker¨ ult megfelel˝o sz´am´ u elmozdul´ast tal´ alni, ´ıgy lehets´egess´e v´alt a navig´aci´ o. A k¨ozelebb l´ev˝ o t´ argyak elmozdul´asa nagyobb lett, mint a t´ avolabb l´ev˝ o t´ argyaknak (10. ´abra), de nem csak a t´ avols´aguk, hanem az alakjuk is kivehet˝ o (11. ´ abra). A haszn´alt 96 ´ert´ek˝ u numberOfDisparities eset´en 35 cm-re l´ev˝ o t´ argyat is k´epes volt ´eszlelni, 128 ´ert´ek eset´en ez a t´ avols´ag 25 cm-re cs¨okkent.

10. ´ abra: A 8. ´ abr´ an l´ athat´ o elmozdul´ as t´erk´ep ,,fentr˝ ol”, az elmozdul´ as fel˝ ol n´ezve. J´ ol kivehet˝ o a robotra mer˝ oleges k´et s´ık doboz oldala ´es k¨ ozt¨ uk l´ev˝ o t´ avols´ ag

Ellen˝ orz´esk´ent r´ahelyezt¨ uk az elmozdul´as t´erk´epet az eredeti k´epre, hogy mennyire tal´ alta meg pontosan a dobozokat (12. ´abra). 4.2.

Sebess´ egtesztek

Mivel a robot folyamatosan halad el˝ore, ez´ert sz¨ uks´eges, hogy az akad´aly sz´amol´asa real-time legyen. Sebess´egteszteket v´egezt¨ unk egy sz´alon, majd t¨ obb sz´alon fut´o programmal, hogy a robot milyen sebess´eggel dolgozza fel a k´epp´arokat. A fekete k´epn´el a robot csak feketes´eget l´atott, ´ıgy a k¨ uld¨ott k´ep a lehet˝o legkisebb volt, statikusn´ al egy t´ argyra ir´ anyultak a kamer´ ak. Az 1. t´ abl´ azatban a m´asodpercenk´ent feldolgozott k´epp´arok sz´ama szerepel. 4.3.

Z´ ert´ ekek

A 2. t´ abl´ azatban l´athat´o a sz´am´ıtott, a val´odi t´ avols´agok ´es a k¨oz¨ott¨ uk l´ev˝ o elt´er´es.

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

285

1. t´ abl´ azat: Az akad´ alysz´ amol´ as sebess´egtesztje k´epp´ ar/m´ asodpercben. A tov´ abb´ıtott k´ep: 320 × 240, legjobb min˝ os´eg. 1,6 GHz-es, egy magos Egy sz´ al ´ o k´ep Fekete k´ep El˝ Csak k´ep k¨ uld´es 11 FPS 3,9–4 FPS Navig´ aci´ o 6,2–6,3 FPS 2,9–3 FPS 3,1 GHz-es, k´et magos Egy sz´ al ´ o k´ep Fekete k´ep El˝ Csak k´ep k¨ uld´es 10–10,2 FPS 3,8 FPS Navig´ aci´ o 6,8–6,9 FPS 3,2–3,3 FPS

CPU T¨ obb sz´ al ´ o k´ep Fekete k´ep El˝ 11 FPS 3,9–4 FPS 9,9–10 FPS 3,8–3,9 FPS CPU T¨ obb sz´ al ´ o k´ep Fekete k´ep El˝ 10–10,2 FPS 3,8 FPS 10–10,2 FPS 3,7–3,8 FPS

2. t´ abl´ azat: A m´ert ´es a t´enyleges t´ avols´ ag´ert´ekek, a Z tengely ellenkez˝ o ir´ anyba mutat, ez´ert negat´ıvat az ´ert´ekek. A 13. m´er´esn´el a numberOfDisparities ´ert´eke 128 volt, a t¨ obbin´el 96. 2 m´eter ut´ an jelent˝ osen n˝ o az elt´er´es a m´ert ´es a val´ os t´ avols´ agok k¨ oz¨ ott. M´er´es T´ avols´ ag 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13.

(mm) Z ´ert´eke (mm) Elt´er´es (mm) 1590 −1670 80 1400 −1480 80 1290 −1335 45 1210 −1250 40 1100 −1140 40 1020 −1070 50 910 −940 30 830 −860 30 720 −745 25 640 −664 24 530 −543 13 450 −470 20 250 −275 25

286

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

bal k´ep

LRC-vel tiszt´ıtott bal elmozdul´ as t´erk´ep 11. ´ abra: A bal oldalon l´ athat´ o k´epre k´esz´ıtett elmozdul´ as t´erk´epen j´ ol kivehet˝ o a mer˝ oleges sarok

bal

jobb

LRC-s bal

12. ´ abra: A megtal´ alt elmozdul´ asok r´ ahelyezve a 6. ´ abr´ an l´ athat´ o k´epekre

4.4.

P´ alyabej´ ar´ as

A program tud´as´ at k´et akad´alyp´aly´an pr´ob´altuk ki. Az egyik esetben egy f´elk¨or alak´ u (14. ´ abra) p´ aly´at kellett k¨ovetnie, a m´asik esetben pedig egy L alak´ u csatorn´ an kellett v´egighaladnia u ¨tk¨ oz´es n´elk¨ ul. Mindk´et p´ aly´at teljes´ıtette, a kihelyezett akad´alyokat f´el m´eteren bel¨ ul ´eszlelte. A tesztek sor´an kider¨ ult, hogy a robotnak a hagyom´ anyos l´amp´ ak f´enye s¨ot´eted´es ut´an nem mindig elegend˝o. A homog´en fel¨ ulet˝ u t´ argyakat rendszerint nem l´atta meg, a p´ aly´at j´ol text´ ur´alt elemekb˝ol ´ep´ıtett¨ uk fel, pl. k¨onyvekb˝ ol, mint´ as dobozokb´ ol.

5.

Konkl´ uzi´ o´ es tov´ abbi l´ ep´ esek

Bemutattunk egy sztere´o rekonstrukci´on alapul´ o robot navig´aci´ os alkalmaz´ast, amely j´ ol hozz´ af´erhet˝ o elemekre ´ep¨ ul, de sz´amos m´odos´ıt´ast tartalmaz. A megb´ızhat´ os´ ag ´erdek´eben alkalmaztuk a bal-jobb konzisztencia megszor´ıt´ast, ´es a

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

287

13. ´ abra: A 9. ´ abr´ at r´ ahelyezve az eredeti k´epre, a nem p´ aros´ıtott ´ert´ekek nem l´ atszanak

Start Stop

Fot´ o a p´ aly´ ar´ ol

A p´ aly´ an bej´ art u ´t

14. ´ abra: Az O alak´ u p´ alya bej´ ar´ asa

p´ arhuzamos implement´ aci´ o seg´ıts´eg´evel val´os idej˝ u feldolgoz´ ast ´ert¨ unk el. Megmutattuk, hogy a robot k´epes a megfelel˝oen text´ ur´alt t´ argyak hat´ar´at felismerni ´es elnavig´alni A pontb´ol B pontba. Az elmozdul´asok ´es a 3D-s adatok nem voltak teljesen pontosak, de a biztos navig´al´ashoz ez is megfelel˝o volt. Tov´abbi l´ep´esek lehetnek az er˝osen text´ ur´alt padl´ ok kisz˝ ur´ese, hogy min´el t¨ obb helyen k´epes legyen navig´alni. Szeretn´enk, ha fel tudn´a majd ismerni, hogy milyen akad´alyon tud m´eg ´athaladni. Tervezz¨ uk m´eg, hogy mozg´as k¨ozben let´ aroljuk a 3D-s adatokat ´es a hozz´ ajuk tartoz´o t´ avols´agokat, majd ezekb˝ ol rekonstru´ aljuk ´es megjelen´ıtj¨ uk a bej´ art p´ aly´at. M´ asik c´elunk az, hogy kihaszn´ aljuk a robot mozg´ekonys´ ag´at arra, hogy egy helyben, minden lehets´eges ir´ anyb´ol felvett k´epb˝ol rekonstru´ aljuk a sz´ınteret.

288

´ am, Megyesi Zolt´ M´esz´ aros Ad´ an

7 1

3

2

5 8

4 6

Fot´ o a p´ aly´ ar´ ol

A p´ aly´ an bej´ art u ´t

15. ´ abra: Az L alak´ u p´ alya bej´ ar´ asa. El˝ osz¨ or 1-es ´es 2-es ir´ anyba n´ez sz´et, majd a 2-es ir´ any fel´e fordul. Ez is t´ ul k¨ ozel van, 3-as ´es 4-es ir´ anyba n´ez sz´et, majd 4-es ir´ anyba fordul. Tov´ abbhalad, itt a falat t´ avolabbr´ ol ´eszreveszi, ´ıgy 5-¨ os ir´ anyba is halad egy kicsit, miel˝ ott u ´jra ´erz´ekeln´e a falat.

K¨ osz¨ onetnyilv´ an´ıt´ as A cikk v´eg´en szeretn´enk megk¨ osz¨ onni Dr. Kov´acs Tam´ as ´es P´ asztor Attila oktat´ oknak, hogy a Surveyor SVS robototot a munka idej´ere a rendelkez´es¨ unkre bocs´ ajtott´ ak.

Irodalom 1. 2. 3. 4. 5. 6. 7. 8.

9.

10.

11.

http://www.surveyor.com/stereo/stereo info.html http://www.surveyor.com/blackfin/OV9655-datasheet.pdf http://opencv.willowgarage.com/ http://inst.eecs.berkeley.edu/~cs150/Documents/ITU601.PDF Gary Bradski and Adrian Kaehler: Learning OpenCV: Computer Vision with the OpenCV Library. O’Reilly Media, Inc., 1st edition, October 2008. Milan Sonka, Vaclav Hlavac, and Roger Boyle: Image Processing, Analysis, and Machine Vision. Cengage-Engineering, March 2007. Z. Zhang: A flexible new technique for camera calibration. IEEE Transactions on Pattern Analysis and Machine Intelligence 22 (2000): 1330–1334 Z. Zhang: Flexible camera calibration by viewing a plane from unknown orientations. Proceedings of the 7th International Conference on Computer Vision (pp. 666–673), Corfu, September 1999. P. F. Sturm and S. J. Maybank: On plane-based camera calibration: A general algorithm, singularities, applications. IEEE Conference on Computer Vision and Pattern Recognition, 1999. L. Lucchese and S. K. Mitra: Using saddle points for subpixel feature detection in camera calibration targets. Proceedings of the 2002 Asia Pacific Conference on Circuits and Systems (pp. 191–195), December 2002. D. Chen and G. Zhang: A new sub-pixel detector for x-corners in camera calibration targets. WSCG Short Papers (2005): 97–100.

Surveyor SVS robot navig´ aci´ o sztere´ o rekonstrukci´ o seg´ıts´eg´evel

289

12. D. C. Brown: Close-range camera calibration. Photogrammetric Engineering 37 (1971): 855–866. 13. R. Y. Tsai: A versatile camera calibration technique for high accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses. IEEE Journal of Robotics and Automation 3 (1987): 323–344. 14. K. Konolige: Small vision system: Hardware and implementation. Proceedings of the International Symposium on Robotics Research (pp. 111–116), Hayama, Japan, 1997.