Svenska ElektronikForumet

Tjena. På Onsdag är det dags för mig att presentera mitt exjobb. Tänkte att jag kan slänga ihop en liten tråd om det här också. Exjobbet går ut på att använda Visual SLAM för att skapa en 3D karta av en matbutik. Jag har byggt en robot som är utrustad med fyra kameror som den använder för att hitta digitala prislappar i butiken. Det är dessa som jag lokaliserar i 3D för att bygga en karta.

Låt oss börja med hårdvaran. För att spara tid köpte jag mig en Irobot Roomba, robotdammsugare, och använde som grund. Denna har ett seriellt interface "Open Interface" som man kan använda för att via USART (5v nivå RS232) kommunicera med den. Via detta kan man läsa av alla sensorer samt styra alla motorer. Ganska kraftfullt interface, speciellt med tanke på att det är en konsumentprodukt. Robotens hjärna fick bli en Raspberry Pi 3 som kör Linux (Raspian, baserat på Debian) Raspberryn pratar med 3.3V medan roboten snackar 5V system. Detta löser man lätt med en dubelinverterande transistorkoppling: Raspberryn fick också en 7" touchskärm så att man kan interagera med den, men styra roboten gör jag via WiFi. De fyra kamerorna fick jag från Modcam (http://www.modcam.com/). Smidiga små puckar som kör Android. Batteri är också inbyggt. Utanpå dem satte jag extra vidvinkellinser för att få 135 grader FOV (field of view). För att få lite höjd till kamerorna använde jag mig av MakerBeam, http://www.hobbytronik.se/sv/65-makerbeam och byggde ett torn som jag monterade allt på. Kamerorna ger en ganska förvrängd bild. Raka linjer blir böjda. Skall man göra geometriska mätningar i bilderna så underlättar det om man har ett euklidiskt koordinatsystem. Man kan modellera denna distorsion med tex ett 6e grads polynom i radiell led samt ett 2a grads polynom för tangentiell distortation (uppkommer om linsen inte är parallell med bildplanet). Jag har modellerat det såhär: Dessa parametrar hittar man automatiskt. Jag har skrivit ett program som hittar alla hörnor i bilden av schackbrädet och sedan kör först en linjär estimering och efter det iterativ förbättring för att göra schackbrädet fyrkantigt, dvs att de linjer som skall vara raka blir raka. Denna funktionalitet finns också inbakad i OpenCV, så det är väldigt lätt att använda det.

Jag behöver använda mig av inversen till distortationen (för att kunna avdistruera en bild). Att räkna ut den matematiskt är inte praktiskt utan istället är det standard att man gör en itterativ lösning som baseras på Banach's fixed point theorem. Jag har implementerat det såhär: Det brukar konvergera väldigt snabbt, redan efter 30-50 iterationer.

Resultatet om jag tillämpar min invers för att mappa om alla pixlar i bilden blir detta: Mer kommer snart!

Spännande.
Vad är syftet med att kartlägga butiken?

Kartläggningen har väll egentligen tre mål:
- Möjliggöra för en app att visa var produkten finns. Om man går in i en främmande butik är det ofta svårt att hitta de produkter man letar efter. Med en app som automatiskt uppdateras med alla varors position kan man bara skriva in vad man vill ha så får man direkt upp en karta som visar vilken hylla och var på hyllan produkten finns.
- Feedback för personalen som plannerar affärens utseende, detta görs ofta centralt av personer som inte är på plats i butiken.
- Detektera kvarglömda prislappar. När en vara flyttas glöms ibland prislappen kvar på hyllkanten. Detta blir förvirrande för kunderna.

Roboten kommer att använda sig av en SLAM algoritm, det vill säga den kommer att optimera både omgivningens position och sin egen position baserat på vilka observationer den gör i kamerorna. För att öka stabiliteten och hastigheten i algoritmen kommer jag att använda mig av dödräkning som jag får från rotationsgivare i robotens hjul. Dödräkning har som ni antagligen känner till problem med mycket avdrift när man kör långa sträckor. Framförallt är det vinkeln som blir fel när man svänger med differentialdrift, ett litet vinkelfel ger ju väldigt stort avståndsfel när man kört en längre sträcka. Jag testade min dödräkning i en lite större lokal. Grön markering är var jag faktiskt körde (manuellt uppmätt) medan den rödsträckade linjen är vad dödräkningen har noterat. För att lösa detta har jag valt att också utvärdera ett system där jag kan använda övervakningskamerorna i butiken för att kalibrera in avdriften var gång roboten kör in i en kamers synfält. För att testa detta monterade jag en Axis F44 med två linser i mitt testrum. Detta ger mig en livestream i full HD. Den ena linsen (F1004) syns uppe till vänster i bilden. Som sågs i bilden på roboten i förra inlägget så har jag utrustat roboten med två lysdioder uppe på toppen. Dessa har jag programmerat till att blinka i olika frekvens, 6,5Hz respektive 3,25Hz. För att detektera dessa lysdioder i kamerornas bilder har jag skrivit ihop en algoritm som identifierar högderivata områden och undersöker deras frekvens. I princip går det till såhär:
* Estimera derivatan med backward difference, det vill säga ta varje pixels intensitet i nuvarande bild minus intensiteten i förra bilden. Dividera med tiden mellan de två bilderna.
* Applicera en tröskelfunktion på absolutvärdet av derivatan. Om det är under en viss nivå sätts pixelvärdet i utbilden till noll, annars till derivatans värde.
* Gruppera (spatiellt) dessa aktiva regioner till "bolbs". Varje grupp beskrivs av sin centerpunkt och vilken frame den sågs i.
* Gruppera ihop dessa regioner i tidsskalan. Om den spatiella förflyttningen är mindre än ett vist antal pixlar så anses de komma från samma lysdiod.
* Istället för att spara alla aktiva frames för varje lysdiod så räknar jag ut vilka frames de hade rising-edges i, dvs gick från icke aktiv till aktiv.
Eller lite mer matematiskt: på första raden är p pixelintensitet. Nedan är d är ett aktivt område, p beskriver en unik lysdiod och e är en rising edge i setet Ep för lysdiod p.

* Istället för att ange tiden mellan rising edges i frames så räknar jag om detta till en tid genom att använda tidsstämpel från bilden eller framerate.
* När jag har ett set innehållandes tider mellan rising edges för varje möjlig lysdiod apllicerar jag en kostnadsfunktion på var set.
Kostnadsfunktionen har jag definierat som en övre begränsad sågtand, dvs ett konstant värde med en dal formad som en upp-och-nervänd kon. Detta ger 0 i straff för den förväntade frekvensen. Kostandsfunktionen ser ut såhär Resultatet av att applicera funktionen på en bildsekvens kan ses nedan. Bilden är lite otydlig eftersom jag bara kör algoritmen i den röda kanalen (lysdioderna är röda). Men de två övre bilderna är från samma kamera och de två undre från den andra. Vänster bild skall detektera den främre lysdioden och höger bild skall detektera den bakre lysdioden. Ett högre score innebär lägre kostnad. De gröna prickarna är detektioner som går över tröskelvärdet för en sannolik detektion, de blå prickarna är områden med hög derivata men som inte gick igenom frekvensfiltret. De blå prickarna uppkommer främst på den person som går förbi i bakgrunden. I denna frame lyckades algoritmen alltså hitta tre av de fyra lysdioderna som den letade efter. Nästa steg är att positionera roboten baserat på detektionerna i kamerorna. Först och främst kalibrerade jag dessa kameror också med hjälp av ett schackbräde för att få euklidiska koordinater. Sen mätte jag ut en massa positioner i rummet som ligger i planet som definieras av den höjd som lysdioderna på roboten kommer röra sig i. Genom att manuellt klicka ut dessa positioner i kamerans bilder så fick jag punkt-korrespondens; en massa punkter vars position jag vet både i kamerans bild samt i verkligheten. Har man minst fyra sådana punkter kan man räkna fram en "homography matrix", dvs en prjektionsmatris. För en transformation från 2D till 2D kommer denna vara 3x3 i storlek. Om man har en koordinat i planet i rummet x, som beskrivs som en homogen koordinat, dvs uppskalad till en extra dimension för att få den skaloberoende, gäller det att ap=Hx där p är koordinaten i bilden och H är projektionsmatrisen. a är en godtycklig skala. Eftersom homogena koordinater är icke beroende av skala, dvs p = 2p = np, är H också det. den har alltså bara 8 frihetgrader. Med flera punktkorespondenser kan vi alltså hitta H genom att ställa upp problemet på formatet Ah=0. u,v är bildkoordinater och x,y är koordinater i planet i rummet: Detta kommer inte ha någon exakt lösning pga brus i bilden och icke exakta mätningar. Men man kan hitta ett null-space, dvs en lösning, till detta med hjälp av singularvärdesuppdelning. Nullspacet ges av den egenvektor till A-matrisen som representerar det minsta egenvärdet. Vill man slippa detta så finns även metoder för detta implementerade i OpenCV.

Med denna metod räcker det alltså med en kamera för att positionera roboten, man vet ju vilket plan den kör i så det är bara en 2D positionering som behövs. Eftersom roboten har två lysdioder som jag detekterar så kan man ju bestämma vilken vinkel/heading som roboten har också.

Med hjälp av detta så har jag skrivit ett kontrollprogram till en dator som jag kan använda för att styra roboten. Sensorinfo streamas live från roboten till denna datorn. Kartan visar dels positionen från dödräkningen (blå) samt positionen från de takkameror som ser roboten. Kollar man noga på den röda cirkeln så ser man att den innehåller två gröna och två röda mindre cirklar. Dessa är detektionerna från de två takkamerorna av den främre och bakre lysdioden.

Ser ut att vara ett häftigt exjobb! Ser fram emot att få läsa mer!

Inser även att det var 5år sedan jag själv presenterade mitt exjobb

Klart intressant!! Du får lägga upp en länk till din rapport senare.

13th.Marine, själv är det 20 år sedan mitt exjobb. Fasiken, man börjar bli gammal!!

He he, det är 30 år sedan jag *inte* gjorde mitt exjobb...

Hur som helst, teorin och matematiken kring det hela ser riktigt kul ut!

Tiden går undan! Känns som om jag precis började plugga och nu är det över för denna gången!

Är det någon i trakterna som är nyfiken så kan ni komma förbi på presentationen och se roboten i egen hög person, det är nu på onsdagen (den 31 augusti) kl. 13.15 i MH:309A, Matematikhuset, Sölvegatan 18, Lund. Jag vågar utlova kaffe och kaka också

Det är inte helt omöjligt att jag dyker upp!

Ett tydligt bevis på att om man är tillräckligt seg med att få igång sina projekt gör någon annan de istället

Jag var dock sugen på en 360 kamera, men det blir kanske för dålig upplösning?

Går det att läsa X-jobbet nånstans? Kunde du hitta etiketterna?

(Jag kanske kommer ner om logistiken går att lösa..)

Kul med intresse!

Det är Kinect du tänker på, AndLi? RGB-kameran i den är 1280x960 pixlar, så det är definitivt inga problem, jag kör bara på 640x320 i mina kameror på roboten. Sen är det ju någon IR-projektor i den som projecerar ett rutmönster, grid med prickar eller dyl tror jag för att få djupseende. Denna upplösningen är nog inte särskilt bra alls, speciellt inte på den gammla Kinect. Ett stort problem är nog också att den har bara 43x57 grader field of view, så det blir väldigt mycket svepande fram och tillbaks. Jag kör ju med 4 kameror på 135 grader fov, så att jag täcker hela varvet runt plus lite överlappning. Sen kan man väll säga att stereokamera/djupkamera är inte nödvändigt när du kör SLAM, i och med att du flyttar på roboten så att du får flera vyer behöver du inte ha flera vyer från samma position. Troligtvis blir det betydligt bättre precision med mono-SLAM än vad en stereo kamera ger på grund av de större vinklarna som man triangulerar över. Sen är det väll klart att om man kombinerar det bästa från båda och kör stereo-SLAM så borde man kunna öka prestandan lite. Frågan är om det är värt det.... eller förresten.... vi är på EF, ett projekt behöver inte vara värt att byggas för att byggas

Jag kan länka till rapporten så småningom, 112 sidor nöjesläsning! Jodå, roboten hittar etiketterna. Inte 100% träffsäkerhet men relativt bra. Största problemet är att jag detekterar lysdiodernas blinkande på dem och ibland reflekteras det i en vägg eller ett fönster, det gör att jag får två detektioner varav den ena måste filtreras bort. Inget jätteproblem dock om man implementerar en robust SLAM algoritm som klarar lite outliers.

Funderade på vad du fick Kinect ifrån, sen insåg jag Nitendo 360...
Men med 360 tänkte jag på de där extrema vidvinkellinserna som tex M3007 har...

Blir ingen lund-resa för mig, så jag får vänta på rapporten

Jasså, en sån kamera är ju mer intressant! Jag har ingen erfarenhet alls av dem men det kan nog fungera. Vad jag har upplevt i projektet så är upplösningen i kamerorna inget stort problem. Högre upplösning är definitivt bättre, men i slutet hade jag ett medelreprojektionsfel på flera pixlar (runt 5 tror jag) och trots det så blev precisionen cirka 30 millimeter vilket lär vara mer än vad som behövs. Däremot lär linskalibreringen på den kameran vara en mardröm, jag hade problem med mina 135 graders kameror att få det bra. Finns speciella algoritmer för 360 kameror av olika slag så det är inte omöjligt idag.

Hela rapporten finns uppladdad nu:
http://da-robotteknik.se/publications/m ... re-sfm.pdf

Tack för att du lade upp rapporten. Har hastigt läst igenom den. Även om man kan tycka att sånt här
låter sig göras med modern elektronik så är det ändå ett hästarbete och verkligen få det gjort.
Bara att författa texten är ju en hel det jobb.
Krävs det verkligen så här mycket för ett examensarbete på mastersnivå nu för tiden?
Eller har du jobbat mer än vad som krävs bara för att det var kul?

Såg att du hade Kalle Åström som examinator. Han är trevlig. För ca 12 år sedan skrev han, jag, en av hans doktorander och en exjobbare hos oss ihop ett patent rörande högupplöst 3D rekonstruktion vid flygburen LiDAR där man har överlappande footprints.

SvenW: Jo, när jag började med projektet hade jag en övergripande plan för hur jag skulle gå tillväga, men inte mycket djupkunskap hur man faktiskt implementerade dessa stegen. Blev lite överraskad sen av komplexiteten, det gick inte att göra så grundläggande som jag hade tänkt mig . Nä, det är väll lite mer än vad som krävs. Det är nog en kombination av att jag är intresserad och aldrig vill missa en chans att lära mig mer samt att jag har väldigt motiverade/motiverande handledare. Vi bestämde en tid från början, sen har det blitt att jag jobbat på i fullt tempo hela tiden trots att jag troligtvis hade uppnåt målen efter halva tiden.

Andax: Kalle Åström verkar vara väldigt duktig och intresserad inom ämnet! Jag har inte haft så mycket kontakt med honom genom exjobbets gång, men han har skrivit många intressanta artiklar som har hjälpt mig.