Designspecifikation. LIPs. Emanuel W Viklund Version 1.0. LiTH 7 oktober imap. Status Granskad Godkänd Jonas Callmer

Transkript

1 imap Emanuel W Viklund Version 1.0 Status Granskad Godkänd Jonas Callmer

2 imap imap Namn Ansvar Telefon E-post Hanna Pettersson Projektledare (PL) Sanna Nilsson Informationsansvarig (IA) Daniel Karlsson Testansvarig (TA) Marcus Arvidsson Dokumentansvarig (DOK) Emanuel W Designansvarig emawa633@student.liu.se Viklund (DA) Johan Wågberg johwa782@student.liu.se Jacob Bernhard jacbe807@student.liu.se Epostlista för hela gruppen: imaptsrt10@googlegroups.com Hemsida: Beställare: Jonas Callmer, Linköpings universitet, , callmer@isy.liu.se Kund: Malin Ingerhed, Saab Dynamics, , malin.ingerhed@saabgroup.com Kursansvarig: David Törnqvist, Linköpings universitet, , tornqvist@isy.liu.se Handledare: Johan Dahlin, Linköpings universitet, , johan.dahlin@isy.liu.se 2

3 Dokumenthistorik imap Version Datum Utförda förändringar Utfört av Granskad av Första utkast Emanuel Daniel Språkliga ändringar och Jacob utvecklat positioneringsavsnitt Ändringar enligt Alla kommentarer från beställare och handledare Version 1.0 Marcus Daniel 3

4 INNEHÅLL Innehåll imap 1 Inledning 5 2 Systemöversikt Grov systembeskrivning Ingående delsystem Masterenheten Kartering Bibliotek Skapa djupbild ELAS Generera punktmoln Ihopsättning av punktmoln Skapa polygoner Städa polygonvärld Texturera polygoner Dataobjekt från kartering Användargränssnitt Tidigare funktionalitet Vidareutveckling D-världen Bibliotek Kravuppföljning Texturering Ruttplanering Förenklingar Exempel på algoritmstruktur för diskret modell Scoutenheten Positionering Bibliotek Visuell odometri Hitta punkter Matcha punkter Behandling av outliers GPS Odometer Fusionering Från globala koordinater till pixelkoordinater Mätuppdatering Dataobjekt Kollisionshantering Kollisionshantering för hinder Kollisionshantering för förbjudna områden A Konventioner 26 A.1 Dokumentkonventioner A.2 Kodkonventioner

5 1 Inledning imap Saab Dynamics vill utveckla en bandvagn som ska kunna kartlägga olika miljöer autonomt. Ett flertal projekt har genomförts för att uppnå målet. Ett första projekt genomfördes våren 2009 av projektgruppen O hara s med utveckling av specifikationer, kontrollprogram, nätverkskommunikation, navigationstekniker med mera. Under hösten 2009 fortsatte gruppen Carpe Locus och utrustade bandvagnen med fjärrstyrning samt motorreglering för framdrivning. Bandvagnen utrustades även med en GPS för att kunna följa en fördefinierad brytpunktsbana. Senaste projektet gjordes hösten 2010 av gruppen 8Yare som förde över tidigare funktionalitet till en industridator och installerade en stereokamera av typen Bumblebee 2. Målet för detta projekt är att utöka bandvagnens funktionalitet genom att med hjälp av kamerans stereobilder och sensorer bygga upp en 3D-karta av ett område på upp till 100x100 meters storlek. Detta dokument behandlar hur systemet ska designas för att uppnå de nya krav som ställts på det. Utöver den nya funktionalitet som ska implementeras kommer även en del av de tidigare gruppernas arbete behöva göras om eller förbättras. 2 Systemöversikt Systemet är uppbyggt av två delsystem, masterenheten och scoutenheten. Masterenheten används för att kontrollera och övervaka scoutenheten. I Figur 1 ses en översiktlig bild över hur systemet är uppbyggt, där scoutenheten definieras av bandvagnen och dess tillhörande delsystem. Figur 1: Översikt av systemet 5

6 2.1 Grov systembeskrivning imap 2.1 Grov systembeskrivning Det första delsystemet (masterenheten) är en laptop som används för att styra bandvagnen. Det andra delsystemet (scoutenheten) är själva bandvagnen. Dessa två delsystem kommunicerar med varandra via ett trådlöst nätverk (WLAN). Scoutenheten är i sin tur uppbyggd av två delsystem: navigationsenheten och framdrivningsenheten ARM. De sensorer som ingår är en GPS och en stereokamera som är kopplad till navigationsenheten och två odometrar som är kopplade till ARM-processorn. I Figur 2 visas informationsflödet inom systemet. Det tidigare systemet utökas för att utföra kartering, dvs. uppbyggnad av punktmolnsvärld och polygonvärld, samt för att nyttja visuell odometri. Masterenhet Användargränssnitt Status GPSData WheelData OdometryData Images, Polygon World, Point Cloud CameraSettings Automatic Manual Nätverksmodul ImageData Modul för bildbehandling och kartering CameraData Status CameraData Status GPSData WheelData OdometryData CameraSettings Automatic Manual ImageData GPS Odometrar CameraSettings GPSData WheelData Sensormodul CameraData GPSData WheelData OdometryData CameraData Nätverksmodul Status WheelData GPSData OdometryData Manual Automatic ImageData Mobilitetsmodul Stereokamera CameraSettings Rörelsemodell Scoutenhet Figur 2: Informationsflöde inom systemet Nedan följer en kort beskrivning på vad de olika modulerna gör. Användargränssnitt Modulen hanterar uppritandet av det grafiska, samt möjliggör kommunikation mellan användare och system. Modul för bildbehandling och kartering Modulen sköter karteringen och all bildbehandling som behövs till användargränssnittet. 6

7 2.2 Ingående delsystem imap Nätverksmodul Modulen sköter kommunikationen mellan masterenheten och scoutenheten. Sensormodul Modulen samlar ihop all sensordata och gör en första bearbetning av denna. Sensordatat kommer kunna användas av exempelvis mobilitetsmodulen för att positionera sig. Mobilitetsmodul Modulen sköter dels förflyttningen utav scoutenheten utifrån styrkommandon från masterenheten samt sensordata, dels bearbetning av sensordata för skattning av position. Rörelsemodell Via rörelsemodellen får sensormodulen nödvändig information om hur scoutenheten rör sig, vilket kan förbättra bearbetning av sensordata. 2.2 Ingående delsystem De ingående delsystemen är: Masterenheten Scoutenheten Navigationsenheten Framdrivningsenheten ARM 3 Masterenheten Masterenheten utgörs av en laptop, där ett användargränssnitt körs under ett Linuxbaserat operativsystem. Från användargränssnittet skickas kommandon till bandvagnen (scoutenheten). Bandvagnen kan endera köras i autonomt eller manuellt läge. I autonomt läge skickas en av användaren specificerad brytpunktsbana till bandvagnen. Bandvagnen följer sedan denna bana så bra som möjligt. I manuellt läge skickar användaren styrkommandon direkt till bandvagnen. Figur 3: Masterenheten kommunicerar med scoutenheten via WLAN. 3.1 Kartering Det som i detta dokument kallas kartering kallas i litteraturen ofta för Surface reconstruction, Structure from motion eller 3D reconstruction se till exempel [11] och [10]. 7

8 3.1 Kartering imap Många algoritmer finns implementerade men alla löser i stort sett samma delproblem på vägen fram till den slutliga kartan. Nedan beskrivs en väg att skapa en representation av omvärlden utifrån stereobilder. Många av de delproblem som måste lösas har flera olika lösningar. Avgörande för vilken metod som är bäst är beräkningstiden i jämförelse med resultatets kvalitet. Många lösningsmetoder har hög beräkningskapacitet m.a.p. antalet analyserade stereobilder. Detta måste utvärderas utifrån verkliga bilder från kameran. En översiktlig bild över de olika stegen i framtagandet av kartan visas i Figur 4. Ta in nytt stereobildpar Skatta djup Bilda punktmoln Position och orientering ja Första punktmolnet? nej Fusionera nytt punktmoln med befintligt Figur 4: Översiktlig bild av stegen i bildandet av 3D-kartan Bibliotek Flertalet bibliotek för hantering av bilder finns att hämta gratis på internet. Till exempel finns Cimg som implementerar primitiver för att lagra och visa bilder. Ett annat kraftfullt bibliotek är Cgal. Cgal implementerar många funktioner i C++ för beräkningar inom geometri. Många, om inte alla, av de delmoment som krävs vid karteringen, kan använda funktioner ur Cgals bibliotek Skapa djupbild Ett första steg mot en 3D-karta är att utifrån det senaste stereobildparet skatta en djupbild, det vill säga en bild där avståndet från kameran till omvärlden skattas för varje pixel i bilden. Den algoritm som idag finns implementerad av tidigare projektgrupper ger troligen en för gles bild där det inte går att bestämma något djup för allt för många pixlar. En bättre metod som kallas ELAS [6] (Efficient LArge-scale Stereo). Den kan ge en tät djupbild där djupet för nästan alla pixlar kan skattas i båda ursprungsbilderna och samtidigt ge en snabb beräkningstid. Algoritmen är avancerad men finns implementerad i det fria biblioteket libelas [5] ELAS För att generera en tät djupbild över hela bilden, skattar ELAS djupet i två steg. Först identifieras ett mindre antal punkter som är enkla att känna igen för att på så sätt 8

9 3.1 Kartering imap minimera risken för felmatchningar mellan bilderna. Djupet för dessa punkter skattas därefter med hjälp av den kända kamerageometrin och punkternas pixelkoordinater i respektive bild. Utifrån dessa säkra punkter bildas sedan en a priori-fördelning för djupet i varje pixel i båda bilderna. Detta sker genom en triangularisering av 2D-bilden utifrån de matchade punkternas pixelkoordinater där djupet i varje triangel bildar ett plan utifrån djupet i dess hörnpunkter. Djupet i hela bilden skattas alltså som en styckvis linjär funktion med stöd i redan beräknade punkterna. Utifrån denna a priori-fördelning kan sedan djupet i resterande pixlar beräknas genom en maximum a postiori skattning Generera punktmoln Utifrån den genererade djupbilden kan ett punktmoln skapas där varje punkt placeras ut i ett koordinatsystem fixt i omvärlden i vilket bandvagnen rör sig. För att göra detta krävs dels djupbilden men även kamerans position och orientering i det fixa koordinatsystemet då kortet tagits samt en modell av kameran. Genom att utgå från kamerans position kan kameramodellen användas för att baklänges räkna fram en punkt i det fixa koordinatsystemet där det borde finnas ett objekt. Beroende på hur skattningen görs kan även ett konfidensintervall beräknas för punkten. Detta skulle användas för att sålla bort allt för osäkra punkter. Senare borde det även gå att användas detta då det nya punktmolnet skall sammanfogas med det sedan tidigare erhållna punktmolnet för att ge en bättre sammanvägning mellan säkra och osäkra punkter.. Utöver positionen hos punkten skall även en del övrig information sparas. För det första ska en färg sättas till punkten genom att spara den färg som finns i bilden för den specifika pixeln. Dessutom ska ett index som beskriver vilken bild punkten har skapats ifrån sparas. Denna information används senare då 3D-världen ska textureras. Ett alternativ till att beräkna punktmolnet utifrån den täta djupbilden är att använda samma teknik som kommer att användas för den visuell odometrin. Där identifieras utmärkande punkter i bilden med hjälp av algoritmerna SIFT [9] eller SURF [1]. Metoderna användas för att identifiera samma punkt i två olika bilder. Matchas punkterna ihop i de två bilderna från ett stereobildpar kan punktens världskoordinater skattas utifrån den kända kamerageometrin. Dessa punkter kan sedan användas i punktmolnet. Detta bör ge en snabbare beräkningstid och kanske en enklare algoritm, men i gengäld kommer punktmolnet inte innehålla lika många punkter Ihopsättning av punktmoln Efter att ett nytt punktmoln har skattats, ska det vägas samman med det gamla. På detta sätt byggs världen upp med mer och mer information. Det enklaste sättet att hantera detta på är att lägga till de nya punkterna och spara alla punkter som skattats under körningen. Detta fungerar väl för små områden med ett litet antal bilder men när området blir större och antalet bilder ökar kommer denna metod att kräva mycket minne. Dessutom erhålls ingen förbättring av kartering av objekt som observeras flera gånger. Problemet att fusionera punktmoln brukar i litteraturen ofta kallas bundle adjustment [12]. Bäst resultat erhålls genom att minimera en olinjär förlustfunktion över punktmoln från alla djupbilder, men detta kräver att även det så kallade associationsproblemet är löst. Det vill säga att punkter i punktmolnet som härstammar från samma punkt i verkligheten har identifierats. Används tekniken att bilda punktmoln utifrån en tät djupbild kan detta inte göras. Detta är dock möjligt ifall punktmolnen skattas med hjälp av någon av SIFT eller SURF algoritmerna. En annan metod för att lösa detta problem föreslås av Geiger 9

10 3.1 Kartering imap et al. i [7]. Idén går ut på att projicera det gamla punktmolnet på den nya djupbilden och för varje punkt kontrollera om denna hamnar inom ett giltigt djupintervall. Är detta fallet vägs punkterna samman genom deras medelvärde, annars betraktas punkten som ny. Ett flödesschema över detta visas i Figur 5. Geiger visar i sin rapport att detta ger snabba beräkningar och ett tillräckligt bra resultat. På samma sätt kan även färgen vägas samman som ett medelvärde. Slutligen utökas listan med vilka bilder som punkten har observerats i. Ta in nytt punktmoln Projicera gammalt punktmoln på aktuell kameraposition. Ordna punkterna i en given ordning. Välj den första. Jämför den projicerade punktens djup med senaste djupbild Välj nästa punkt i listan Punktens djup inom given tolerans? nej ja Inför punkten som en ny punkt i molnet Väg samman de två punkterna med deras medelvärde nej Alla punkter gjorda? ja Klart Figur 5: Flödesschema för fusionen av ett nytt punktmoln med det gamla Skapa polygoner Utifrån det skapade punktmolnet skapas en 3D-värld av polygoner. Polygonerna genereras genom Delaunay-triangularisering med hjälp av Cgal-biblioteket. Denna algoritm 10

11 3.1 Kartering imap spänner upp trianglar så att inga punkter ligger inuti någon triangels omskrivna sfär, på samma gång som smala trianglar undviks [2]. Genom att studera vilka bildpar som gav upphov till polygonens ingående punkter, kan ett visst bildpar väljas som källa till polygonen. I det fall att alla polygonens punkter härstammar från samma bildpar står det klart att detta skall användas vid texturering samt städning. Då ett bildpar tas nära eller vinkelrätt mot en viss yta kommer punkterna i punktmolnsvärlden om härstammar från dessa bilder att bli tätare än de från bildpar tagna långt bort ifrån eller från sned vinkel. Sannolikheten att majoriteten av en potentiell polygons punkter kommer från det bildpar som har bäst betraktelseförhållande, dvs. indexeras till detta, blir då större än för de bildpar med sämre förhållande. Punkterna polygonen ursprungligen spänns upp av innehåller även färginformation. Ett snitt av deras färginformation kan användas för att bestämma en specifik färg till varje polygon. Betraktas även punkternas osäkerhet kan färginformationen även viktas vid sammanvägningen; står det klart att en viss punkt med stor sannolikhet inte går att lita på gällande exempelvis färg kan den i större grad bortses ifrån Städa polygonvärld Ett problem som uppstår då punktmolnet triangulariseras är att felaktiga och överflödiga polygoner skapas. För att göra poygonvärlden mer tilltalande måste den rensas. Detta sker genom att jämföra polygonernas position gentemot de djupbilder de sägs tillhöra. Vid jämförelse mellan en djupbild och de polygoner som är indexerade till just den djupbilden kan man detektera om någon polygon spänts upp framför det djup som djupbilden visar. Om så är fallet ska den polygonen raderas. Genom att endast jämföra med de för djupbilden tillhörande polygonerna erhålls att ingen polygon borde dölja en från djupbilden teoretisk punkt mer än ett visst tröskelvärde. En del i städningsproblemet är att hitta det tröskelvärde som ger bäst resultat. Vid städning av polygonvärlden bestäms även varje polygons normal. Genom att numrera de ingående punkterna, sett från kameran, kommer det grafiska gränssnittet tolka det som att polygonens normal pekar mot kameran. Vid vetskap om åt vilket håll varje yta vänder sig kan uppritning i 3D snabbas upp, samt grafiska effekter som skuggor och reflektioner enkelt beräknas. Normalriktningen erhålls genom kryssprodukten mellan två vektorer från polygonens första punkt med var och en av de två övriga. Polygonerna för en given bild ska vara vända mot kameran; är en polygon uppspänd från en viss bild ska den även synas från samma position i 3D-världen. Då normalen för en polygon ej är i rätt riktning givet en viss bild numreras punkterna om Texturera polygoner Texturering sker genom att projicera varje polygon på den bild polygonen är indexerad till. Polygonens hörn i 3D kommer att få relaterade koordinater i 2D-bilden, som sedemera kan användas vid uppritning av världen. Projiceringen sker genom att beskriva polygonernas koordinater, {x p, y p, z p }, i koordinatsystemet med kamerans position som origo, riktat åt det håll bilden togs. Titthålskameramodell kan då användas, och givet koordinaterna, {x k, y k, z k }, i detta system samt 11

12 3.2 Användargränssnitt imap fokaldjupet, f, ges koordinaterna, u i, i bilden som { {u x, u y } = f x k, f y } k. (1) z k z k Dataobjekt från kartering Efter karteringen erhålls en polygonvärld som skapas ur de punkter som tillsammans bildar punktmoln. Här följer beskrivning av de ingående objekten och deras viktigaste datamedlemmar. Namnen är givna på engelska enligt gällande kodkonvention. Bildindex betyder i detta fall det bildpar som en punkt eller polygon sägs tillhöra. Point En punkt bestående av koordinater, färg, bildindex och kovarians. pointcoordinates Array med punkens koordinater [x, y, z]. pointcolor Array med punktens färgkod [R, G, B]. imageindex Punktens bildindex. pointcovar Osäkerhet i punktens position. Point Cloud Hanterar lista med Points. listofpoints Array med punktvärldens alla Points. Polygon En triangel bestående av punkter, färg, bildindex och relaterade koordinater i bilden. polygonpoints Array med polygonens tre Points. polygoncolor Array med polygonens färgkod [x, y, z]. imageindex Polygonens bildindex. imagepolygoncoordinates Flerdimensionell array med varje punkts motsvarande koordinat i bilden [[x1, y1],[x2, y2], [x3, y3]]. Polygon World En lista med Polygons. listofpolygons Array med polygonvärldens alla Polygons. 3.2 Användargränssnitt Det användargränssnitt (GUI) som används på masterenheten har utvecklats av tidigare projektgrupper(8yare, O hara s och Carpe Locus). Användaren kan med hjälp av användargränssnittet ansluta till bandvagnen och därefter skicka och ta emot data samt presentera denna på olika sätt. Programmet är utvecklat i C++ och använder Qt-biblioteket för det grafiska gränssnittet Tidigare funktionalitet Vid start av programmet möts användaren av ett anslutningsfönster där IP-adressen till navigationsenheten skrivs in. Efter att anslutningen är upprättad kan styrkommandon skickas till roboten. Dessutom kan en brytpunktsbana definieras i användargränssnittet och överföras till roboten som då utför förflyttning efter banan. 12

13 3.2 Användargränssnitt imap I manuellt läge skickas styrkommandon direkt till roboten. Användaren kan via ett hastighetsreglage ställa in önskad hastighet och med hjälp av tangentbordet styra roboten. Användaren kan även välja vilken data från roboten som ska visas i användargränssnittet: GPS-data, odometerdata eller bilder från kameran. Kamerainformationen kan representeras som singel-bilder, stereobilder eller färglagda djupbilder. Det går även välja att följa ett mål. I autonomt läge följer roboten en bana given av ett antal brytpunkter. Den bana som roboten ska följa kan användaren själv bestäma genom att klicka på skärmen så att brytpunkter placeras ut och därmed bildar ett spår att följa. Det går även att öppna en redan befinlig bana eller att spara den nuvarande banan. Ett antal olika styrknappar finns för att göra det möjligt att bestämma vad roboten ska göra och vilken information om Scoutenheten som ska visas. Samma sensorinformation som för det manuella läget går även att se i det autonoma. Utöver dessa lägen finns ett satellitläge. Där är det möjligt att få upp en satellitbild över området roboten ska köra i. För tillfället existerar bara en karta i programmet och det är en bild över och omkring B-huset på Linköpings universitet. I denna vy går det som i den autonoma menyn att ladda in en brytpunktsbana eller att skapa en egen. Klasser som redan finns och vilken funktion de har kan ses i Tabell 1 samt ett klassdiagram kan ses i Figur 6. Klassnamn UserInterface GLWidget AutomaticGUI ManualGUI NetworkGUI SensorGUI ScoutData SatellitePanel LoadCameraPositionGUI BreakGUI TrackGUI StatusGUI MapGUI Tracker CameraGUI CameraSettingsGUI AboutUsGui Beskrivning Skapar huvudfönstret Huvudklass för att rita ut satellitkartan Gränssnitt för automatisk styrning Gränssnitt för manuell styrning Skapar fönster för nätverksanslutning Tar in och visar sensordata Sparar information om roboten så som position, GPS-data, bildpositioner och brytpunktskartan Visar knapparna i satellitvyn Laddar in en bana från en textfil Sköter inmatning av brytpunktslista och sparning av den Skapar ett fönster så det går att ladda in en förgenererad bana Visar statusinformation Ritar ut robotens position relativt sin startpunkt Klass för identifiering av rörliga objekt Visar kamerabilder i olika versioner Sköter inställningar för kameran Visar information om projektgruppen Tabell 1: Beskrivning av befintliga klasser. 13

14 3.2 Användargränssnitt imap Vidareutveckling Användargränssnittet ska vidareutvecklas så att det finns ett läge där användaren kan se och röra sig i den 3D-värld som roboten har skapat efter karteringen. Världen ska kunna vara uppbyggd av antingen polygoner eller punktmoln. Växling mellan dessa metoder ska kunna göras. För att kunna markera förbjudna områden eller markera det område som ska karteras i användargränsnittet ska det implementeras en Click-and-Drag funktion vilket det redan finns stöd för i biblioteket Qt. En knapp för att välja vilket typ av område som ska väljas ska finnas. När användargränssnittet utökas kommer den kodkonvention som projektgruppen Carpe Locus tagit fram att följas. Även objekthanteringen kommer följa den bestämda strukturen. De nya klasser/metoder som kommer att skapas är: Interface3d - Klass som ska hantera ett fönster som innehåller en 3D-vy. Denna kommer vara nära kopplad till OpenGl-paketet och hanterar användarinteraktion både med 3D-världen och med gränssnittet. World3d - Klass som beskriver alla funktioner som kommer behöva användas av de olika OpenGL renderade världarna. PointWorld - Subklass till World3d. PointWorld innehåller de funktioner som är specifika för punktmolnsvärlden. PolygonWorld - Subklass till World3d. PolygonWorld innehåller de funktioner som är specifika för polygonvärlden. En större del av existerande klasser och metoder från tidigare grupper kommer behöva utökas med ytterligare funktionalitet. Då vissa filer är otillräckligt kommenterade kommer dessa att kompletteras i mån av tid för att hjälpa nästkommande grupper. De nya klassernas relationer kan ses i Figur 6. 14

15 3.3 3D-världen imap Figur 6: Klassdiagram - Blåa rutor är befintliga klasser och gröna är de som ska skapas för detta projekt D-världen Utvecklingen av användargränssnittet är mycket beroende av resultatet från karteringen. För att tjäna in tid kan en annan enkel 3D-värld skapas där funktionerna för Interface3dklassen kan testas. Den information som antas erhållas från karteringen beskrivs i avsnitt I detta avsnitt används ordet objekt som beskrivning av en geometrisk form som är uppbyggd av ett antal polygoner eller punkter. 3D-världen ska bestå av antingen polygoner eller punkter. Detta val ska kunna göras av användaren i användargränssnittet. När världen är uppbyggd av punkter benämns den som en punktmolnsvärld. I punktmolnsvärlden ska punkterna representeras av små sfärer med en fast storlek. När en punkt skapas ska den ges samma färg som den motsvarar i stereobilden och denna information ges från karteringsgruppen. För att få en uppfattning om varifrån en bild är tagen, ska det finnas pilar som representerar detta strax ovanför marken i punktmolnsvärlden. När världen är uppbyggd av polygoner benämns den som en polygonvärld. Från karteringsfasen erhålls en lista med polygoner som ska vara en del av polygonvärlden. Då prestandan hos masterenheten kan vara begränsad är det viktigt att funktioner som frustum culling (se nedan för förklaring) samt back-face culling stöds och kan implementeras om behov finns. Därför antags polygonernas Points vara angivna i den ordning som ger polygonen rätt normal. Då kan back-face culling enkelt implementeras, vilket innebär att alla polygoner som är vända bort från kameran ignoreras. För att minska beräkningskraften för frustum culling och kollisionshantering för OpenGLkameran, är det viktigt dela in världen i mindre delar. OpenGL-kameran kan med fördel representeras som en sfär för att förenkla kollisionshanteringen och förhindra att kameran tillåts komma för nära objekt. Istället för att behöva testa om kameran kolliderar med 15

16 3.3 3D-världen imap varje polygon i hela världen, räcker det att undersöka i vilken del av världen kameran befinner sig och endast testa kollisioner med de polygoner som är i samma världsdel. Hur stora delar världen ska splittas i undersöks senare i projektet. Frustum culling innebär att man undersöker vilka objekt som kan ses av OpenGLkameran och renderar endast dessa. För att det inte ska uppstå artefakter (fel i den grafiska representationen) i utkanterna av vyn, kommer objekt runt om synfältet också att renderas. Objekt som är placerade i resten av världen kommer ändå inte att ses och kan därför ignoreras Bibliotek Då det tidigare programmet är utvecklat i C++, under Qt-biblioteket, är det logiskt att fortsätta att utnyttja Qt och därmed använda Qt:s inbyggda stöd för OpenGL. Detta innebär att det inte behövs importera en OpenGL-instans utan istället ha den integrerad i Qt. Det är viktigt att GLUT-paketet finns tillgängligt då detta erbjuder en stor mängd förenklade funktioner som kommer vara till stor hjälp Kravuppföljning För att möjliggöra för användaren att titta på en specifik position kommer ett formulär finnas i användargränssnittet där användaren kan ställa in önskad kamera position eller position som kameran ska titta på. Om inget annat anges antas användaren börja i koordinaterna (0,0,0) och titta i den riktning som roboten inledde sin färd. För att möjliggöra att användaren ska kunna följa bandvagnens bana, ska informationen om bildernas position och vinkel användas. Banan ritas redan nu ut av gränssnittet och genom att göra om denna till en kamerabana, kan kameran ställas in att följa banan i mer eller mindre mjuka rörelser. Ett stöd för att hantera instruktioner från användaren via tangentbord och mus finns redan i GLUT-paketet i OpenGL. Där kan varje tangent kopplas till en funktion och på detta sätt kan användaren röra sig omkring i 3D-världen. I nuläget är det tänkt att följande rörelsekontroller ska finnas: w - rörelse framåt s - rörelse bakåt a - rörelse rakt åt vänster d - rörelse rakt åt höger pil upp - titta gradvis uppåt pil ner - titta gradvis nedåt pil vänster - rotera runt sin egen axel åt vänster pil höger - rotera runt sin egen axel åt höger Den 2D-karta som genereras är tänkt att vara ett genomskinligt färglager som kan placeras över den översiktskarta som redan finns implementerad i gränssnittet. Den genomskinliga egenskapen ges genom att använda alpha-parametern i Qt-klassen QColor. 16

17 3.4 Ruttplanering imap Texturering Texturering är enbart möjligt i polygonvärlden då punkterna i punktmolnsvärlden är oberoende av varandra när de skickas till användargränssnittet. Dock borde det ge tillräckligt bra resultat med att endast tilldela varje punkt i punktmolnsvärlden en färg. Om punkterna är tillräckligt täta borde detta ge samma effekt som en texturerad polygon i polygonvärlden. För polygonvärlden kommer dock textureringen att medföra en stor förbättring av helhetsintrycket. Till varje polygon som erhålls från karteringen kommer det att anges vilka koordinater i originalbilden som motsvaras. Innehållet i originalbilden kan rättframt extraheras och mappas på polygonen. 3.4 Ruttplanering För att kunna hantera helt okända omgivningar är det önskvärt att roboten kan klara av att kartera ett givet område automatiskt. Detta kräver att roboten själv vet var bilder behövs tas och hur den transporterar sig dit. För att kunna göra detta krävs en algoritm som beskriver vilka mål som ska uppnås, vilka alternativ som ges (e.g. förflytta sig framåt, svänga 90 grader åt höger, ta en bild) och vilken prioritetsordning som finns mellan dessa alternativ. Om en senare projektgrupp fokuserar mer på detta område, skulle antagligen stor vikt läggas på att denna algoritm är relativt effektiv med avseende på tid eller information per tagen bild eller liknande. Dock är huvudmålet med ruttplaneringen för projektgrupp imap, att endast skapa en fungerande algoritm som inte behöver vara effektiv Förenklingar Ett problem med ruttplaneringen är att man inte på förhand vet exakt var hinder befinner sig. Detta innebär att förutom en bra positionering, är en fungerande kollisionshantering viktig. Om vi antar att roboten kan detektera hinder och placera ut dessa på den interna kartan är ruttplaneringsproblemet ett i stort sett iterativt, dynamiskt, kontinuerligt problem, vilket gör det beräkningskrävande. En förenkling kan vara att diskretisera de positioner och vinklar som man behöver ta bilder i för att kartera ett område. Då skulle det exempelvis räcka med att ta en bild varje meter i fyra ortogonala riktningar. På ett m stort område skulle det isåfall krävas st bilder tagna av roboten. Genom att förenkla problemet ytterligare tex. genom att säga att: Har du tittat på en yta utan hinder från EN riktning så finns inget hinder där från de övriga riktningarna eller: Det är ingen mening att ta bilder ut från området, kan förmodligen lösningen snabbas upp avsevärt. Det är dock inte säkert att diskretiseringen är nödvändig. Diskretiseringen är ett sätt att sätta ut möjliga brytpunkter offline, innan körningen startar. Ett annat sätt att hantera att ett hinder stoppar robotens ursprungliga plan är att online beräkna vilka områden som är i behov av fler bilder och ifrån vilka vinklar de behövs och därefter beräkna en möjlig rutt. Scoutenheten skulle då i princip kunna beräkna nästa brytpunkt och under tiden den rör sig dit. Ta bilder och beräkna fram en möjlig nästnästa brytpunkt. Om ett hinder hittas på vägen skulle en ny rutt planeras utifrån denna information. 17

18 imap Exempel på algoritmstruktur för diskret modell Vilket upplägg som än används kommer det att behövas en intern representation av omgivningen och information om vilket tillstånd roboten befinner sig i. I det diskreta fallet kan omgivningen representeras av en lista med alla möjliga brytpunkter. Varje position betecknas som oupptäckt, innehåller ett objekt, blockerad eller fri. Detta kan innebära att om en brytpunkt är fri finns det inget objekt på den positionen och den behöver inte fotograferas, om den är oupptäckt måste den minst fotograferas en gång från en vinkel och om den innehåller ett objekt måste den fotograferas från fyra vinkelräta vinklar. När en brytpunkt som innehåller ett objekt blir fotograferad från fyra håll räknas den som blockerad och behöver inte fotograferas mer. Genom att vid en given brytpunkt ta en bild i en viss riktning, erhålls information om brytpunkten i den riktningen. Denna information används sedan vid uppdateringen av listan. De möjliga åtgärderna som finns för roboten kan vara att t.ex. svänga 90 grader åt höger/vänster, ta en bild, åka framåt en meter. Målet för roboten kan vara att alla brytpunkter ska vara antingen fria eller blockerade. Det som bestämmer vad roboten ska göra härnäst är en sökfunktion som söker igenom de möjliga kommandon som roboten har och värdesätter vilket kommando som leder snabbast till målet. Det finns ett flertal sökalgoritmer att välja som använder olika strategier. För att förenkla kan Breadth-first search användas som väljer det alternativ som är närmast. Om man inte viktar kostnaden av kommandona så att det är bättre att köra rakt fram än att svänga kan detta dock resultera i att roboten snurrar ett helt varv på varje brytpunkt, vilket kanske inte är det tidsmässigt mest effektiva sättet. 4 Scoutenheten Scoutenheten består av en bandvagn av typ MMP30 samt en dator som använder operativsystemet Linux. På bandvagnen sitter två stycken likströmsmotorer, en på vardera sida, som driver enheten. Plattformen innehåller även två batterier som via en spänningsregulator tillhandahåller spänning till motorer och övrig elektronik i bandvagnen. Scoutenheten är utrustad med odometrar på motorerna, en GPS och en stereokamera. Scoutenheten består av två stycken delsystem: Navigationsenheten samt framdrivningsenheten ARM. Kommunikation mellan scoutenheten och omvärlden sker genom navigationsenheten till masterenheten via WLAN. 18

19 4.1 Positionering imap Figur 7: Översikt av scoutenheten. 4.1 Positionering För att utifrån stereobilderna kunna skapa en 3D-värld behövs en bättre positionering än tidigare. Gruppen 8Yare från 2010 estimerade bandvagnens position genom att behandla data från Odometer och GPS och med hjälp av ett Extended Kalman Filter (EKF) skatta bandvagnens tillstånd och dynamik. Den estimeringen kommer utökas med att även skatta positionen utifrån information från stereobilderna. Detta görs med Visuell Odometri (VO). Informationen från de tre sensorerna kommer sedan att fusioneras ihop med hjälp av EKF Bibliotek OpenCv och MRPT är två programbibliotek som är bra för bildanalys och filtrering. Dessa bibliotek innehåller bland annat algoritmerna nedan. De är gratis och går att ladda ner från internet och kan inkluderas i C++. Även Rob Hess har användbar kod som finns att hitta gratis på internet [13] Visuell odometri VO är en metod för att bestämma position och orientering genom analys av associerade kamerabilder. På det sättet kan också bandvagnens rutt bestämmas. Metoden går ut på att hitta förändringar i bilden och på så vis kunna identifiera samma omgivning i andra bilder. Genom det kan man sedan bestämma kamerans relativa position när en viss bild togs gentemot en annan bild. 19

20 4.1 Positionering imap Figur 8: Flödesschema över visuell odometri Hitta punkter Det första steget i VO är att hitta intressanta punkter, landmärken, i en bild. Landmärken är pixlar som är väldefinierade och innehåller mycket information om sin position. Det innebär att de distinkt skiljer sig från sin omgivning och alltså är lätta att identifiera, till exempel hörn. En metod för att hitta dessa är Harris corner detector [3]. Fördelen med denna metod är att den är invariant för rotation, skalning, belysning och brus i bilden. Metoden bygger på en lokal auto-korrelationsfunktion som mäter lokala förändringar i intensitet i olika riktningar. Detta genom att placera ut små fönster (ca 5 5 pixlar) och studera deras intensiteter. När ett fönster flyttas runt i bilden kommer intensiteten att förändras. Genom att maximera intensitetsändringen i två riktningar kan hörn detekteras. Autokorrelationsfunktionen som ska maximeras är c(u, v) = W [I(u i, v i ) I(u i + u, v i + v)] 2 (2) där W är fönstret med centrum i punkten (u, v), I intensiteten och u, v förflyttningen i u- respektive v-led. Genom Taylor approximation och utveckling av funktionen erhålls Harrisoperatorn c(u, v) = [ u v ] ( [ ]) [ ] I 2 u I u I v u = [ u v ] [ ] u C(u, v). (3) I u I v v v W I 2 v Genom att sedan studera egenvärdena λ 1, λ 2 till matrisen C(u, v) kan tre fall identifieras: λ 1, λ 2 är små - små förändringar i omgivningens intensitet och alltså inte en intressant punkt. 20

21 4.1 Positionering imap λ 1 är stort och λ 2 är litet eller viceversa - Intensiteten förändras i en riktning vilket betyder att fönstret visar en kant. λ 1, λ 2 är stora - stora förändringar i omgivningens intensitet i båda riktningar vilket visar på ett hörn. Tröskeln som bestämmer om egenvärdet är stort eller litet måste givetvis bestämmas och kommer att vara betydande för om punkterna är intressanta eller inte Matcha punkter När ett antal intressanta punkter har identifierats på två olika bildpar (höger och vänster stereobild) så är nästa problem att matcha ihop dessa mellan de olika bildparen. Det finns ett antal olika algoritmer för att göra detta. I detta projekt kommer algoritmen Scale- Invariant Feature Transform (SIFT), [9], att användas eftersom att den är oberoende av rotation, skala och belysning. Dessutom bygger den på Harris corner detecter som används för att hitta intressanta punkterna. För att på ett bra sätt kunna matcha ihop de intressanta punkterna så görs fönstret till varje intressant punkt om till en beskrivningsvektor som kan ses som varje enskild punkts fingeravtryck. Beskrivningsvektorerna i de ena bildparen jämförs sedan med de andra för att detektera matchande punkter. Detta görs genom att undersöka hur väl de olika beskrivningsvektorerna matchar varandra. Eftersom det kommer att finnas brus i bilderna så kommer beskrivningsvektorerna inte att vara exakt lika. Därför undersöks hur bra den bästa matchningen är i jämförelse med den näst bästa. Jämförelsen av matchningarna används sedan för att skatta positionen i ett Extended Kalman Filter Behandling av outliers Outliers, alltså felaktiga matchningar som uppstår i samband med SIFT kommer att behandlas med den iterativa algoritmen RANSAC (RANdom SAmple Consensus)[4]. RAN- SAC går ut på att en delmängd av all data väljs ut slumpmässigt och sedan används för att skatta tillstånden med modellen. Dessa tillstånd testas sedan med all data. Skulle passningen som erhålls inte vara tillräckligt bra upprepas algoritmen. En tröskel måste bestämmas som avgör hur bra matchning som krävs och algoritmen måste begränsas med ett fixt antal iterationer GPS Kommunikationen med GPS-enheten kommer inte förändras något från vad tidigare projektgrupper skapat där X t = [ x t y t ] T + et Odometer Odometerdatat kommer att användas på samma sätt som tidigare års projektgrupp har gjort, det vill säga estimera hastigheten genom att veta längden på bandvagnens band. Detta kan läsas om i förra årets tekniska dokumentation [8] under avsnitt och

22 4.1 Positionering imap Fusionering För att skatta bandvagnens tillstånd kommer som sagt ett Extended Kalman Filter (EKF) att användas. Data från odometrar, GPS och visuell odometri kommer tillsammans med rörelsemodellen nedan att användas i filtret. Att inte ett vanligt kalmanfilter används beror på att vinklarna kommer in olinjärt i dynamiken och därför behövs EKF istället. Tillståndsvektorn som skattas i filtret kommer bestå av dels positionen på bandvagnen, men även positionen för landmärken i bandvagnens synfält, cirka 10 stycken. Detta gör att tillståndsvektorns storlek kommer att variera vid varje tidsuppdatering. Om ett landmärke predikteras ligga utanför bildramens storlek, tas de bort från tillståndsvektorn. Tillståndsvektorn kommer alltså se ut som, X = [ X B X 1 L... Xn L] T där X B är bandvagnens globala position och X i L = [ x i y i z i] T landmärkenas globala position. Till att börja med kommer X B = [ x y ψ ] T där ψ är girvinkeln. Denna kommer sedan utökas så att även höjden, z, tippvinkeln, θ, och rollvinkeln φ estimeras. Rörelsemodellen som kommer att användas ser ut som x t+1 = x t + T s v t cos(ψ + ω ψt s 2 ), y t+1 = y t + T s v t sin(ψ + ω ψt s 2 ), z t+1 = z t + T s v t cos(θ + ω θt s 2 ), ψ t+1 = ψ t + ω ψt T s, θ t+1 = θ t + ω θt T s, φ t+1 = φ t + ω φt T s, ω ψt+1 = ω ψt, ω θt+1 = ω θt, ω φt+1 = ω φt, (4) där hastigheten, v t, och vinkelhastigheten, ω ψ, ses som insignaler från odometrarna. Tillstånden beräknas genom att först göra en tidsuppdatering. Då predikteras alla landmärkens och bandvagnens position med hjälp av rörelsemodellen. Denna position översätts sedan till pixalkoordinater. Vidare görs en mätuppdatering där de predikterade tillstånden jämförs med mätdata och skattas i ett EKF Från globala koordinater till pixelkoordinater För att kunna jämföra landmärkenas predikterade koordinater med de uppmätta, måste dessa vara i samma koordinatsystem. De globala koordinaterna måste därför göras om till pixelkoordinater. Inledningsvis översätts landmärkena till bandvagnens koordinater. Det görs med ekvationen där M i = R(ψ)(X i L X B ) (5) 22

23 4.1 Positionering imap cos(ψ) sin(ψ) 0 R(ψ) = sin(ψ) cos(ψ) 0 (6) och M i det i:te landmärkets koordinater i bandvagnens koordinatsystem. Dessa 3D koordinater måste sedan kunna tolkas och översättas till 2D i bildernas koordinatsystem. Detta för att kunna prediktera vilken pixel i bilden landmärket borde befinna sig i. Projiceringen till bildplanet ser ut som [ xk y k ] = f M iz [ Mix M iy ], (7) där x k, y k är normaliserade bildkoordinater i kameraplanet. z i det här fallet syftar alltså på djupet i bilden. f är kamerans fokallängd som på Bumblebee 2 är 6 mm. Dessa transformeras till pixelkoordinater genom [û ] = ˆv [ ] [ ] [ ] fx 0 xk hx x + k 0 f y y k h y y k där û, ˆv är pixelkoordinater, f x = s x /f, f y = s y /f och s x, s y är pixelstorleken och har värdet 7,4 µm. h x och h y är mittpunkten av bildramen. (8) Mätuppdatering När ovanstående beräkningar har genomförts och tillstånden har predikterats, uppdateras de nya tillstånden med mätningarna i ett EKF, κ, enligt ([ ] ]) ut [ût+1 t ˆx t+1 t+1 = ˆx t+1 t + κ. (9) v t ˆv t+1 t För att ett landmärke ska kunna jämföras med motsvarande prediktering i de olika bilderna, betraktas dess beskrivningsvektorer, se avsnitt Sammanfattningsvis ser proceduren ut enligt Figur 9 Figur 9: Översiktsbild av fusionering. 23

24 4.2 Kollisionshantering imap De skattade koordinaterna och vinklarna skickas sedan vidare tillsammans med bilder till karteringen där de sedan används vid skapandet av en 3D-värld Dataobjekt Efter positioneringen kommer sex tillstånd tillsammans med en bild att kunna skickas vidare till karteringen. Nedan följer en beskrivning på de dataobjekt som positioneringen använder sig av. Features En intressant punkt bestående av koordinater och vilken bild punkten ligger i. featurescoordinates Array med punktens koordinater [x,y]. featuresimage Vilken bild punkten ligger i, höger eller vänster. Key Points Ett fönster runt den intressanta punken som innehåller koordinater och egenskaper. keypointcoordinates Array med mittpunktens koordinater [x,y]. keypointsfingerprint Array med fönstrets egenskaper. Key Point List Hanterar en lista där en bilds alla intressanta fönster finns. listofkeypoints Array med en bilds alla intressanta fönster. Matching Key Points Två matchande Key Points, innehåller koordinater och avstånd. matchingkeypointscoordinates Array med de två matchande punkternas key points Matching Key Points List Ett fönster runt den intressanta punken som innehåller koordinater och egenskaper. listofmatchingkeypoints Array med en bilds alla matchande keypoints. Estimated Data De skattade koordinaterna och vinklarna. estimatedcoordinates Array med de skattade koordinaterna [x,y,z] estimatedangles Array med de skattade vinklarna [θ, φ, ψ] 4.2 Kollisionshantering En viktig del att ta hänsyn till under körning av roboten är möjligheten att detektera hinder och kunna välja en lämplig motaktion. Det finns två olika typer av kollisionshantering. En som behandlar detektion och motaktion vid hinder som blockerar färdvägen samt en annan som gäller vid kollision med förbjudna områden. Förbjudna områden väljs av användaren i användargränssnittet Kollisionshantering för hinder Om scoutenheten närmar sig ett hinder kommer det att kunna detekteras med hjälp av djupbilderna. Då ett hinder är närmare än en meter bör systemet planera och utföra en motaktion. De motaktioner som ska implementeras är att planera om rutten enligt det som anges i avsnitt 3.4 alternativt stanna helt om det inte är möjligt att undvika hindret på annat sätt. 24

25 4.2 Kollisionshantering imap Kollisionshantering för förbjudna områden För att detektera att roboten är innanför ett förbjudet område används dess skattade position. Då ruttplaneringen planerar en säker väg ska detta scenario aldrig ske. Om roboten mot all förmodan hamnar innanför skall den köra samma väg ut som den kom in då detta är den enda vägen som med säkerhet är körbar. 25

26 A Konventioner imap För att få enhetligt utseende på kod och dokument kommer föregående gruppers konventioner användas. A.1 Dokumentkonventioner Följande dokumentkonventioner används: Inget stycke får sakna inledande text. A.2 Kodkonventioner Följande kodkonventioner används: Engelska används vid all namngivning. Klasser har formen av substantiv i singularis, t.ex. Odometer eller namnet på modulen t.ex. NetworkModule. Metoder har formen Verb alternativt VerbSubstantiv, t.ex. Send, GetAngle. Klass- och metodnamn använder PascalCase, vilket innebär att första tecknet i varje ord är versalt. Datamedlemmar, lokala variabler samt parametrar använder camelcase, t.ex. operationlist. Inga understreck i namn på varken klasser, datamedlemmar eller metoder. Indentering är två positioner, mellanslag (space) inte tab. Curly-braces placeras på egen rad med samma indentering som föregående instruktion, sk. Allman indent style. Ett mellanrum runt operatorer, dock inte före eller efter parenteser i funktionsdefinitioner och funktionsanrop. För integrala typer används direkt anrop med värde, för större typer används konstantreferenser eller pekare. Samma för returtyper. I kodruta 1 följer ett exempel på hur kod i C++ har formateras. class MyClass : public ABase { public : int MyMethod( int thisisaparameter, const AnotherClass &anotherclassinput ) ; private : AnotherClass data ; } int MyClass : : MyMethod( int thisisaparameter, const AnotherClass &anotherclassinput ) { data = anotherclassinput ; 26

27 A.2 Kodkonventioner imap } i f ( thisisaparameter <= 4) { return thisisaparameter 2 ; } return thisisaparameter + 4 ; 27

28 REFERENSER Referenser imap [1] Herbert Bay, Tinne Tuytelaars, and Luc Van Gool. Surf: Speeded up robust features. In Axel Pinz Ales Leonardis, Horst Bischof, editor, Computer Vision - ECCV 2006, volume 3951 of Lecture Notes in Computer Science, pages Springer Berlin / Heidelberg, / [2] Boris Delaunay. Sur la sphère vide. a la mémoire de georges voronoï. Bulletin de l Académie des Sciences de l URSS. Classe des sciences mathématiques et na, 6: , [3] Konstantinos G. Derpanis. The harris corner detector. Technical report, Computer Science, York University, October CompVis_Notes/harris_detector.pdf. [4] Konstantinos G. Derpanis. Overview of the ransac algorithm. Technical report, Computer Science, York University, May CompVis_Notes/ransac.pdf. [5] Andreas Geiger. Libelas. C++ library. libelas.html. [6] Andreas Geiger, Martin Roser, and Raquel Urtasun. Efficient large-scale stereo matching. In Asian Conference on Computer Vision (ACCV), Queenstown, New Zealand, November ttic.uchicago.edu/~rurtasun/publications/geiger_ et_al_accv10.pdf. [7] Andreas Geiger, Julius Ziegler, and Christoph Stiller. Stereoscan: Dense 3d reconstruction in real-time. In IEEE Intelligent Vehicles Symposium, Baden-Baden, Germany, June [8] Gustav Hanning. Teknisk dokumentation, [9] David G. Lowe. Object recognition from local scale-invariant features. In Computer Vision, The Proceedings of the Seventh IEEE International Conference on, volume 2, pages vol.2, jsp?tp=&arnumber= [10] Andrew Zisserman Richard Hartley. Multiple View Geometry in Computer Vision. Cambridge University Press, West Nyack, NY, USA, 2 edition, March [11] Nader Salman and Mariette Yvinec. Surface Reconstruction from Multi-View Stereo. Lecture notes in computer science, September [12] Bill Triggs, Philip McLauchlan, Richard Hartley, and Andrew Fitzgibbon. Bundle adjustment - a modern synthesis. In Bill Triggs, Andrew Zisserman, and Richard Szeliski, editors, Vision Algorithms: Theory and Practice, volume 1883 of Lecture Notes in Computer Science, pages Springer Berlin / Heidelberg, / [13] Rob Hess. Library for SIFT computations. code/sift/ 28