Opracowanie i implementacja algorytmuscalania danych z skanera laserowego

i kamery dookólnej

Marek Majchrowski, Piotr Trojanek, Wojciech Szynkiewicz

Sprawozdanie z wykonania 1-ego etapu prac w ramach grantu MEiN nr

Warszawa, Grudzień 2006

Politechnika WarszawskaInstytut Automatyki iInformatyki Stosowanejul. Nowowiejska 15/1900–665 Warszawa

tel/fax (48) 022 – 825 37 19

Spis treści

1 Algorytm scalania danych ze skanera laserowego i kamery dookólnej 2

1.1 Wstęp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2 Wykorzystywane czujniki . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 Kalibracja kamery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.4 Wizualizacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4.1 Reprezentacje map 3D . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.4.2 Przeprowadzanie skanowania 3D . . . . . . . . . . . . . . . . . . . . 7

1.4.3 Techniki wizualizacji . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.4.4 Wykorzystana technologia . . . . . . . . . . . . . . . . . . . . . . . 11

1.4.5 Silnik graficzny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

1.5 Uzyskane rezultaty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1.6 Podsumowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1

Rozdział 1

Algorytm scalania danych ze skanera laserowegoi kamery dookólnej

1.1 Wstęp

1.2 Wykorzystywane czujniki

Robot Elektron R1 jest wyposażony w kilka niezależnych układów czujników. Do pod-stawowych należą: układ pomiaru odometrycznego oraz układ dalmierzy optycznych roz-mieszczonych dookoła korpusu robota.

Robot jest przystosowany do współpracy z dodatkowym wyposażeniem. Otwarta struktu-ra układu sterowania oraz specjalnie zaprojektowana konstrukcja mechaniczna pozwalająna łatwy montaż dodatkowych modułów.

W opisywanej aplikacji wykorzystano moduł składający się z głowicy do skanowania trój-wymiarowego oraz układu wizji dookólnej. Moduł składa się ze skanera laserowego SICKLMS 200 umieszczonego na obrotowej głowicy. Głowica może obracać skaner wokół osipoprzecznej w zakresie kątowym od −15◦ do +90◦. Do napędu głowicy wykorzystanosilnik elektryczny prądu stałego z przekładnią planetarną. Napęd przekazywany jest zapośrednictwem przekładni z paskiem zębatym, dzięki czemu silnik napędowy został zain-stalowany w podstawie. Do pomiaru kąta obrotu zostały zastosowane dwa przetwornikiobrotowo-impulsowe. Pierwszy z nich, zainstalowany na wale silnika, jest wykorzystywa-ny w układzie regulacji położenia. Drugi dokonuje pomiaru kąta obrotu bezpośrednio naosi obrotu skanera. Dzięki takiej konfiguracji możliwy jest precyzyjny pomiar położeniakątowego głowicy, na który nie mają wpływu luzy występujące w przekładni. Głowicępokazano na rys. 1.1a.

Nad górną częścią skanera został sztywno przykręcony, do specjalnego wspornika zamo-cowanego do korpusu robota, układ wizji dookólnej. Składa się on z kamery umieszczonejpionowo oraz zwierciadła parabolicznego (rys. 1.1b) umieszczonego nad nią w osi optycz-nej. Zaletą czujnika wizji dookólnej jest możliwość obserwacji otoczenia w zakresie 360◦

przez jedną kamerę. Oś optyczna skanera oraz oś kamery pokrywają się.

2

1.3. KALIBRACJA KAMERY 3

a b

Rysunek 1.1: Moduł czujników

1.3 Kalibracja kamery

Układ wizji dookólnej został skalibrowany przy wykorzystaniu algorytmu C. Mei orazP. Rives [3]. Model projekcji wykorzystywany przez ten algorytm, jest rozszerzeniem mo-delu zaproponowanego przez C. Geyer oraz J. P. Barreto w pracach [1,2]. Oba te algorytmysłużą one do kalibracji kamer wykorzystujących zwierciadło (hiperboliczne, parabolicznelub eliptyczne). Wykorzystują one ujednolicony model parametrów (ξ, η), tzn. kamerawraz ze zwierciadłem traktowane są jako jeden element optyczny o nowej uogólnionejogniskowej η oraz parametrze ξ zależnym od rodzaju zwierciadła (zależność tą opisujetabela 1.1).

Rzut punktu z przestrzeni trójwymiarowej na płaszczyznę zdjęcia jest realizowane w na-stępujących krokach (proces ten jest także przedstawiony na rys. 1.2).

1. Punkt z przestrzeni trójwymiarowej jest rzutowany na jednostkową kulę:

(X )Fm = (X s)Fm =X‖X‖ = (Xs, Ys, Zs)

2. Współrzędne tak zrzutowanego punktu przeliczane są do układu współrzędnych,który ma środek w punkcie Cp = (0, 0, ξ):

(X s)Fm = (X s)Fp = (Xs, Ys, Zs + ξ)

1.4. WIZUALIZACJA 4

3. Następnie punkt jest rzutowany na znormalizowaną płaszczyznę zdjęcia:

mu =(XsZs + ξ

,YsZs + ξ

, 1)= ~ (X s)

4. Potem brane są pod uwagę zniekształcenia elementów optycznych:

md =mu +D(mu, V )

5. Na końcu brana jest pod uwagę uogólniona macierz kamery (opisująca uogólnionąogniskową η, rzeczywisty środek obrazu (u0, v0), współczynnik przekrzywienia s orazwspółrzynnik powiększenia r)

p = Km =

η ηs u00 ηr v00 0 1

m = k(m)

Rodzaj zwierciadła Parametry modeluξ η

Paraboliczne 1 −2pfHiperboliczne df√

d2+4p2−2pf√d2+4p2

Eliptyczne df√d2+4p2

2pf√d2+4p2

Planarne 0 -fPerspektywiczne 0 f

Tabela 1.1: Parametry ujednoliconego modelu

Na rysunku 1.3 przedstawiono zdjęcie wykonane z omnikamery, które następnie poddanotransformacji wg algorytmu podanego wyżej. Rezultat kalibracji oraz prostowania zdjęciaprzedstawiają rysunki 1.4 oraz 1.5.

1.4 Wizualizacja

Właściwe techniki wizualizacji danych są jednym z ważniejszych czynników decydującycho właściwym zrozumieniu danych. Techniki wizualizacji mogą być stosowane jako jednaze skuteczniejszych form oceny uzyskanych wyników, która może powiedzieć na ich tematwięcej niż same liczby.

1.4.1 Reprezentacje map 3D

Geometria obiektów trójwymiarowych może być reprezentowana na wiele sposobów. Naj-częściej wykorzystywane konstrukcje w grafice 3D to:

1.4. WIZUALIZACJA 5

Znieksztalcenia

p

K

~zm

~ym

Cm~xm

πmd

Xs

X

πmu

mu

ξ

1

md

~xs

~zs

~ys

Cp

Fp

Fm

πp

Xs = X‖X‖

= (Xs, Ys, Zs)

Fm

Xs = (Xs, Ys, Zs + ξ)

Fp

p = Km

πp

πmd

πmu

md = mu + D(mu, V )

mu = ℏ(Xs) = ( Xs

Zs+ξ,

Ys

Zs+ξ,1)

Rysunek 1.2: Pełny model rzutu

• Siatka wielokątów – obiekt jest budowany z płaskich wielokątów (najczęściej trójką-tów lub czworokątów), które mają wspólne wierzchołki i krawędzie. W ten sposóbmożna tworzyć proste bryły, albo – jeśli siatka jest dostatecznie gęsta – dobrzeprzybliżać skomplikowane obiekty.

• Woksele obiekt – jest budowany z elementarnych sześcianów (trójwymiarowych pik-seli). Tego rodzaju reprezentacja jest rozpowszechniona szczególnie w diagnostycemedycznej, gdzie uzyskuje się szereg przekrojów (obrazów bitmapowych) ciała pa-cjenta i na ich podstawie tworzy trójwymiarowe modele.

1.4. WIZUALIZACJA 6

Rysunek 1.3: Zdjęcie do kalibracji z omnikamery

Rysunek 1.4: Zdjęcie po rozprostowaniu

Rysunek 1.5: Zdjęcie po rozprostowaniu oraz uwzględnieniu zniekształceń

1.4. WIZUALIZACJA 7

• Opis matematyczny – obiekty są określone równaniami. Mogą to być np. kule, płasz-czyzny, oraz szczególnie użyteczne i powszechnie stosowane powierzchnie parame-tryczne (płaty powierzchni), np. powierzchnie Beziera, Hermite’a bądź NURBS2.

Dziedzina robotyki cechuje się pewnymi wymaganiami i ograniczeniami, które wyklucza-ją niektóre z prezentowanych powyżej reprezentacji obiektów i map 3D. Wymagania tedotyczą następujących aspektów map 3D:

• Sposób pozyskiwania – jeżeli zadaniem robota jest zbudowanie mapy nieznanegootoczenia na podstawie odczytów z czujników, należy się liczyć z dużymi błędamipomiarowymi. Nie ma zatem potrzeby stosowania super-dokładnych opisów. Po-wstała mapa powinna charakteryzować się taką dokładnością, na jaką pozwalająposiadane czujniki.

• Przechowywanie w pamięci – roboty generalnie cechują się dość ograniczoną pojem-nością pamięci, zatem nie należy stosować reprezentacji wymagających dużych jejilości.

• Przetwarzanie, uaktualnianie – operacje, takie jak dodawanie do mapy nowo zbada-nych obszarów albo łączenie kilku odrębnych wycinków mapy w jedną spójną całośćnie powinno być bardzo wymagające obliczeniowo.

Wobec powyższych ograniczeń w robotyce nie jest wskazane stosowanie opisu matematycz-nego (super-dokładna reprezentacja rzeczywistości oraz duże wymagania obliczeniowe)oraz wokseli w formie tablicy 3D (duże wymagania pamięciowe) do opisu otoczenia 3D.Z przedstawionych powyżej konstrukcji jedynie mapy 3D złożone z wielokątów spełniająwymagania stawiane przez robotykę. Mogą być dowolnie dokładne (wielkości poszczegól-nych wielokątów mogą zależeć od dokładności przeprowadzonych pomiarów) oraz nie sąbardzo wymagające pod względem obliczeniowym ani pamięciowym. Nie jest to jednakjedyna wykorzystywana w robotyce reprezentacja rzeczywistości wurtualnej. Innymi spo-tykanymi reprezentacjami map 3D są: mapy złożone z drzew ósemkowych (modyfikacjareprezentacji złożonej z wokseli) oraz mapy zbudowane z powierzchni płaskich.

1.4.2 Przeprowadzanie skanowania 3D

Dalmierz laserowy zwraca jako wynik pomiaru odległość wykrytej przeszkody dla każdejze swoich wiązek. Wielkość ta nie jest istotna z punktu widzenia budowania mapy 3Di należy ją przekształcić na współrzędne (xk, yk, zk) punktu, od którego wiązka się odbiła.W takim przekształceniu wartościami znanymi są (1.6):

• długość promienia odbitego wiązki ν

• kąt wychylenia serwomechanizmu od poziomu α

• kąt odchylenia badanej wiązki od wiązki środkowej β

• orientacja pozioma robota (w stosunku do osi x) ξ

1.4. WIZUALIZACJA 8

Rysunek 1.6: Oznaczenia kątów wykorzystywanych do przeliczania położeń punktów

• pozycja robota (x, y, z)

Współrzędne punktu (xk, yk, zk), od którego wiązka się odbiła, obliczane są w dwóchkrokach. W pierwszym kroku obliczane są współrzędne punktu (xf , yf , zf ), dla któregonie jest uwzględniana orientacja oraz pozycja robota. Przekształcenie to obrazuje rysu-nek 1.7. W następnym kroku rzut punktu (xf , yf , zf ) na płaszczyznę OXY jest podda-

Rysunek 1.7: Obliczanie współrzędnych (xf , yf , zf )

xf = ν · cosα · sin βyf = ν · cosα · cos βzf = ν · sinα

wany przekształceniom obrotu wokół osi OZ o kąt ξ oraz przesunięcia o pozycję robota(x, y, z) (1.8). Wynikiem obliczeń są szukane współrzędne punktu, na który trafiła badanawiązka. Dane otrzymane na podstawie powyższych obliczeń z czujników charakteryzująsię błędami wniesionymi przez:

• odometrię robota (błędy te są minimalizowane przez sterownik lodo)

1.4. WIZUALIZACJA 9

Rysunek 1.8: Obliczanie współrzędnych (xk, yk, zk)

xk = x+ xf · cos ξ + yf · sin ξyk = y + yf · cos ξ − xf · sin ξzk = z + zf

• luzy na osi serwomechanizmu obracania głowicy lasera (maksymalny błąd wynika-jący z dokładności enkodera wynosi 0.1◦)

• skaner laserowy (według opracowania maksymalny błąd wynosi 17mm)

1.4.3 Techniki wizualizacji

W chwili obecnej istnieją dwa najbardziej rozpowszechnione zestawy interfejsów umożli-wiające generowanie grafiki 3D w czasie rzeczywistym OpenGL oraz DirectX. Obydwiete technologie wspierają sprzętową akcelerację renderowania grafiki 3D.

OpenGL

OpenGL (Open Graphics Library) został zdefiniowany jako interfejs programowy dlasprzętu graficznego. Zasadniczo jest to uniwersalna, bardzo szybka i doskonale przeno-śna biblioteka umożliwiająca modelowanie i tworzenie grafiki trójwymiarowej.

Początkowo biblioteka korzystała z algorytmów rozwijanych i optymalizowanych przezfirmę Silicon Graphics światowego lidera w grafice i animacji komputerowej. Z czasem,dzięki wkładowi różnych dostawców oprogramowania, OpenGL rozwinęło się znacznie.Obecnie OpenGL jest wciąż rozwijającym się, otwartym standardem tworzonym przez or-ganizację ARB (Architectural Review Board) zrzeszającą 10 firm (w tym SGI, IBM, Intel,Microsoft, 3DLabs), które spotykają się raz na 3 miesiące, aby głosować i podejmowaćdecyzje dotyczące dalszego rozwoju standardu.

OpenGL działa w architekturze klient-serwer. Klientem, w tym przypadku, jest aplika-cja wykorzystująca OpenGL, która zleca operacje graficzne do wykonania, a serwerem -

1.4. WIZUALIZACJA 10

aktualnie używana implementacja OpenGL (np. w sterowniku karty graficznej). Zwykleklient i serwer znajdują się na tej samej maszynie, jednak nie jest to konieczne - bibliote-ka zaprojektowana tak, aby możliwe było np. wyświetlanie grafiki OpenGL na zdalnymterminalu. Jednocześnie, na skutek zastosowania zunifikowanego protokołu komunikacji,wyświetlanie może odbywać się na zupełnie innej platformie, niż ta, na której działa apli-kacja.

Jedną z podstawowych cech OpenGL jest to, że jest on maszyną stanów. Na stan OpenGLw danym momencie składa się szereg parametrów i trybów działania, które można ustawićlub zapamiętać na stosie i później odtworzyć. Ich konfiguracja będzie miała bezpośrednilub pośredni wpływ na otrzymany rezultat renderingu. Raz ustawiony parametr lub trybdziałania pozostaje zachowany aż do następnej zmiany.

Mimo dużych możliwości, samo OpenGL jest interfejsem niskopoziomowym. Oznacza to,że zawiera ono np. funkcje rysujące pojedyncze wielokąty lub serie wielokątów, ale już naprzykład funkcje pozwalające na ustawienie obserwatora w pewnym punkcie patrzącegona inny punkt (funkcja gluLookAt) bądź narysowanie kuli (funkcja gluSphere) realizowanesą za pomocą biblioteki pomocniczej GLU (ang. GL Utility Library).

Poza GL i GLU, do wykorzystania OpenGL w aplikacji potrzebne są zwykle równieżinne biblioteki. Wynika to z faktu, że OpenGL zajmuje się tylko renderingiem grafiki inie zawiera funkcji związanych z tworzeniem kontekstu graficznego, obsługą środowiskaokienkowego, czy obsługą zdarzeń takich, jak naciśnięcie klawisza na klawiaturze lub ruchkursora myszy. Funkcjonalność tę można dodać poprzez biblioteki takie jak GLUT (GLUtility Toolkit), GLUI, czy dedykowane dla poszczególnych platform GLX (w przypadkuśrodowiska X Window System) lub WGL (w przypadku Windows).

DirectX

DirectX to zestaw funkcji API wspomagających generowanie grafiki (dwu i trójwymiaro-wej), dźwięku oraz innych zadań związanych zwykle z grami i innymi aplikacjami mul-timedialnymi. Komponentem DirectX odpowiedzialnym za generowanie grafiki 3D jestDirect3D. Pozwala on na zaawansowany, niskopoziomowy dostęp do karty graficznej.

Dzięki Direct3D można tworzyć nowoczesne gry, symulacje czy programy multimedialne.Jest on podstawowym standardem programowania grafiki trójwymiarowej w systemachoperacyjnych zgodnych z Windows. Akceleracja sprzętowa, oferowana przez większośćwspółczesnych kart graficznych oraz bogaty zbiór dostępnych narzędzi umożliwia progra-mowanie efektownej grafiki 3D. Direct3D dostarcza programiście gotowych interfejsów,uwalniając go jednocześnie od konieczności zaznajamiania się ze wewnętrznymi funkcjamisprzętu.

Pierwotnie struktura Direct3D różniła się od standardu OpenGL, ale z biegiem czasu jejbudowa coraz bardziej zaczynała go przypominać. Direct3D jak i cały pakiet DirectX jestoparty na interfejsach COM, co oznacza, że programista musi tworzyć wskaźniki do klasi potem z nich korzystać. Jest to dość zawiła operacja, która dodatkowo zwiększa nieczy-telność kodu programu. Microsoft odchodzi od modelu COM i chce przyjąć rozwiązaniazawarte w OpenGL, które opierają się na funkcjach i przepuszczaniu przez nie zmiennych.Takie rozwiązanie daje znacznie bardziej czytelny kod, co wpływa na zmniejszenie trudno

1.4. WIZUALIZACJA 11

wykrywalnych drobnych błędów, ale również na niewielki spadek wydajności.

Direct3D jest użyteczny jedynie przy pisaniu aplikacji pod Windows, ponieważ działatylko pod kontrolą tego systemu, co jest jego największą wadą.

1.4.4 Wykorzystana technologia

Przy wyborze technologii do zbudowania interaktywnego silnika 3D kierowano się kilkomawzględami: przede wszystkim wykorzystana technologia powinna być wydajna, przenośnaoraz mało skomplikowana. Zarówno Direct3D, jak i OpenGL oferują zbliżone możliwości iwydajność, jednakże OpenGL jest biblioteką wysoce przenośną istnieją jej implementacjedziałające na niemal każdym systemie operacyjnym, w przeciwieństwie do Direct3D, dzia-łającego jedynie pod systemem Windows. Dodatkowo kod pisany w OpenGL jest dużoprzejrzystszy od kodu pisanego w Direct3D 10 linijek kodu w OpenGL odpowiada około2-3 stronom kodu w Direct3D. Biorąc pod uwagę powyższe argumenty, zdecydowano sięna wybór biblioteki OpenGL do napisania kodu silnika 3D.

Dodatkowym argumentem przemawiającym za tym wyborem była dostępność narzędzi ibibliotek rozszerzających podstawową funkcjonalność OpenGLa, takich jak np. bibliotekaGLUT.

1.4.5 Silnik graficzny

Zaimplementowany silnik graficzny sprawuje się poprawnie nawet przy wyświetlaniu bar-dzo dużej ilości trójkątów w jednym momencie. Wymaganą wydajność uzyskano przezwykorzystanie trójkątów sklejanych (ang. triangle strips). Przy zastosowaniu trójkątówsklejanych każdy trójkąt definiowany jest przez tylko jeden wierzchołek (poza pierwszymze zbioru). Rysunek 1.9 przedstawia kolejne etapy rysowania trzech sklejonych trójkątów,określanych przez definicje pięciu wierzchołków ponumerowanych od v0 do v4.

Rysunek 1.9: Kolejne etapy rysowania trzech sklejonych trójkątów

1.5. UZYSKANE REZULTATY 12

Używanie trójkątów sklejonych wiąże się z dwoma zaletami w porównaniu do definiowaniakażdego trójkąta z osobna. Po pierwsze jest to oszczędność miejsca przeznaczonego naprzechowywanie danych, szczególnie, gdy obiekt składa się z wielu trójkątów. Drugą zaletąjest poprawa wydajności obliczeń matematycznych oraz oszczędności przy przesyłaniudanych (mniej wierzchołków oznacza szybsze przesłanie danych z pamięci komputera dokarty graficznej, a także mniej przekształceń wierzchołków).

1.5 Uzyskane rezultaty

Na chwilę tworzenia opracowania nie były dostępna możliwość uzyskania pełnych danychpomiarowych – zawierających jednocześnie dane ze skanów trójwymiarowych oraz ob-raz z kamery dookólnej zainstalowanej na robocie. Poniżej przedstawiono jedynie wynikiwizualizacji dostępnych danych ze skanowania w przykładowym pomieszczeniu laborato-ryjnym.

Działanie opisanego silnika graficznego zostało przetestowane przy wizualizacji scen takichjak przedstawiona (rys. 1.10), składających się z kilkuset tysięcy trójkątów.

Na przedstawionej wizualizacji kolor wszystkich trójkątów został sztucznie podany jakoszary – w docelowej implementacji barwa każdego trójkąta zostanie podana jako wynikuśrednienia składowych RGB punktów obrazu z kamery dookólnej, które odpowiadająwspółrzędnym wierzchołków każdego trójkąta.

1.6 Podsumowanie

W opracowaniu przedstawiono sposób tworzenia trójwymiarowych, barwnych map oto-czenia przez robota mobilnego, wyposażonego w skaner laserowy na obracanej głowicyoraz kamerę dookólną. Opisany został proces kalibracji czujnika wizyjnego metodą, którauwzględnia specyfikę zastosowanego zwierciadła parabolicznego. Omówiony został procesprzekształcenia współrzędnych (x, y, z) punktu przestrzeni uzyskanego jako pomiar z czuj-nika laserowego do współrzędnych punktu w obrazie z kamery dookólnej. Ostateczniezaprezentowano wynik wizualizacji sceny przykładowego pomieszczenia laboratoryjnegoz wykorzystaniem stworzonego silnika graficznego.Na chwilę tworzenia opracowania niebyły dostępne pełne dane pomiarowe – jako para odczytów z lasera oraz obraz z kamery.Przedstawiono jedynie sposób uzyskiwania barwy pojedyńczego trójkąta (jako elementar-nego obiektu w wizualizowanej scenie) na podstawie obrazu wizyjnego. Jest to ostatniwymagany krok w procesie budowania barwnej, trójwymiarowej reprezentacji środowiskadziałania robota.

1.6. PODSUMOWANIE 13

Rysunek 1.10: Obraz rzeczywistego pomieszczenia oraz wizualizacja pomiarów ze skanera

Bibliografia

[1] J. P. Barreto and H. Araujo. Issues on the geometry of central catadioptric imageformation. In IEEE Computer Vision and Pattern Recognition or CVPR, pages II:422–427, 2001.

[2] Christopher Geyer and Konstantinos Daniilidis. A unifying theory for central pano-ramic systems and practical applications. In David Vernon, editor, Computer Vision- ECCV 2000, 6th European Conference on Computer Vision, Dublin, Ireland, June26 - July 1, 2000, Proceedings, Part II, volume 1843 of Lecture Notes in ComputerScience, pages 445–461. Springer, 2000.

[3] C. Mei and P. Rives. Calibration between a central catadioptric camera and a laserrange finder for robotic applications. In ICRA, May 2006.

14