POLITECHNIKA WARSZAWSKA Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej

Zakład Sterowania i Systemów Zespół Programowania Robotów i Systemów Rozpoznających

Kamil Niewiarowski

Budowanie map 3D przez roboty mobilne oraz ich wizualizacja

3D map building and visualization by mobile robots

Praca dyplomowa magisterska napisana pod kierunkiem

dr inż. Wojciecha Szynkiewicza

Warszawa, 2006

Podziękowania

Dziękuję mojemu promotorowi, dr inż. Wojciechowi Szynkiewiczowi za

dwuletni okres współpracy, podczas którego wiele się nauczyłem oraz kolegom

Markowi Majchrowskiemu i Piotrowi Trojankowi za to, że w trudnych chwilach

zawsze służyli mi pomocą.

Abstract

This thesis addresses the problem of 3D map building of indoor

environments with mobile robots. The thesis consists of three consecutive parts.

In the first part, a collection of approaches and algorithms, as well as

different 3D map representations, used in 3D map constructing in mobile robotics

is discussed. The most extensively analyzed map representation is a polygonal 3D

mesh map, for which a complete set of relevant algorithms used for data

acquiring, mesh construction and simplification, and scene registering have been

described.

In the second part a self-elaborated implementation of the algorithm used for

polygonal 3D mesh map construction is described in detail. The algorithm’s input

data is acquired using a 3D laser range finder, constructed from a 2D laser range

finder and a standard servo motor. The proposed method is capable of data

acquiring, 3D mesh construction and scene registering .

Third part of the thesis is committed to polygonal 3D scenes visualization

problem. Different approaches are discussed. An implementation of an OpenGL

graphic engine, capable of rendering complex polygonal scenes in real-time is

introduced.

Finally, the experimental results, gathered and visualized with developed

algorithms, are illustrated. Using the obtained results, different configuration

parameters of the elaborated 3D mesh map construction algorithm are discussed

and the optimal values are proposed.

I

Spis treści:

1. WSTĘP......................................................................................................................................................... 1

1.1. CEL I ZAKRES PRACY ............................................................................................................................ 1 1.2. ZAWARTOŚĆ PRACY.............................................................................................................................. 1

2. MAPY TRÓJWYMIAROWE.................................................................................................................... 2

2.1. HISTORIA I ZASTOSOWANIA .................................................................................................................. 2 2.2. ZBIERANIE INFORMACJI O OTOCZENIU 3D ROBOTA............................................................................... 2

2.2.1. Wykorzystywane czujniki................................................................................................................. 3 2.2.2. Wykorzystywane urządzenia pomiarowe......................................................................................... 3

2.3. REPREZENTACJE MAP 3D...................................................................................................................... 5 2.3.1. Mapy złożone z powierzchni płaskich.............................................................................................. 8 2.3.2. Mapy złożone z drzew ósemkowych............................................................................................... 11 2.3.3. Mapy złożone z wielokątów ........................................................................................................... 13

2.3.3.1. Tworzenie reprezentacji sceny złożonej z wielokątów........................................................................ 14 2.3.3.2. Upraszczanie sceny 3D........................................................................................................................ 15 2.3.3.3. Wyznaczanie kolejnych miejsc przeprowadzania pomiarów............................................................... 20 2.3.3.4. Połączenie wielu skanów 3D w jedną spójną mapę............................................................................. 22

3. ALGORYTM BUDOWANIA MAPY 3D................................................................................................ 25

3.1. OPIS ZADANIA..................................................................................................................................... 25 3.2. STANOWISKO BADAWCZE ................................................................................................................... 25 3.3. ŚRODOWISKO PROGRAMISTYCZNE - ARCHITEKTURA PLAYER / GAZEBO............................................. 26 3.4. WYBÓR REPREZENTACJI MAPY ........................................................................................................... 28 3.5. UŻYTE STRUKTURY DANYCH .............................................................................................................. 28 3.6. SCHEMAT ALGORYTMU BUDOWANIA SPÓJNEJ MAPY 3D..................................................................... 29 3.7. PRZEPROWADZANIE SKANU 3D .......................................................................................................... 30 3.8. UTWORZENIE REPREZENTACJI SCENY ZŁOŻONEJ Z TRÓJKĄTÓW.......................................................... 33 3.9. ŁĄCZENIE WIELU SKANÓW 3D W JEDNĄ SPÓJNĄ MAPĘ ....................................................................... 36 3.10. NAPOTKANE PROBLEMY ..................................................................................................................... 36 3.11. MOŻLIWE KIERUNKI ROZWOJU............................................................................................................ 37

4. WIZUALIZACJA SCENY 3D ZŁOŻONEJ Z TRÓJKĄTÓW............................................................ 39

4.1. WSTĘP DO WIZUALIZACJI .................................................................................................................... 39 4.2. ZADANIE PROJEKTOWE ....................................................................................................................... 39 4.3. TECHNIKI WIZUALIZACJI ..................................................................................................................... 39

4.3.1. OpenGL......................................................................................................................................... 41 4.3.2. DirectX .......................................................................................................................................... 42

4.4. IMPLEMENTACJA SILNIKA 3D ............................................................................................................. 43 4.4.1. Wykorzystana technologia............................................................................................................. 43 4.4.2. Środowisko programistyczne......................................................................................................... 43 4.4.3. Schemat działania silnika.............................................................................................................. 44 4.4.4. Widok sceny................................................................................................................................... 46 4.4.5. Praca interaktywna z programem FPP3DMap............................................................................. 47 4.4.6. Oświetlenie sceny .......................................................................................................................... 47 4.4.7. Wkład własny ................................................................................................................................ 49 4.4.8. Możliwe kierunki rozwoju ............................................................................................................. 49

II

5. OBSŁUGA PROGRAMÓW .................................................................................................................... 51

5.1. KONFIGURACJA PROGRAMU BUILD3DMAP ........................................................................................ 51 5.2. URUCHAMIANIE PROGRAMU BUILD3DMAP NA SYMULATORZE .......................................................... 52 5.3. URUCHAMIANIE PROGRAMU BUILD3DMAP NA FIZYCZNYM ROBOCIE ................................................ 53 5.4. KORZYSTANIE Z PROGRAMU BUILD3DMAP........................................................................................ 54 5.5. URUCHAMIANIE PROGRAMU FPP3DMAP ........................................................................................... 55

6. WYNIKI EKSPERYMENTALNE .......................................................................................................... 56

6.1. ANALIZA WYNIKÓW DZIAŁANIA PROGRAMU BUILD3DMAP ............................................................... 56 6.1.1. Test 1 – budowanie mapy 3D ........................................................................................................ 56 6.1.2. Test 2 – wpływ rozdzielczości serwomechanizmu i lasera na złożoność i jakość sceny 3D ......... 63 6.1.3. Test 3 – wpływ maksymalnej długości krawędzi generowanych trójkątów na złożoność i jakość sceny 3D ...................................................................................................................................................... 73

6.2. ANALIZA WYDAJNOŚCI SILNIKA FPP3DMAP ...................................................................................... 81

7. PODSUMOWANIE................................................................................................................................... 84

8. BIBLIOGRAFIA ....................................................................................................................................... 86

1

1. Wstęp

1.1. Cel i zakres pracy

Podstawowym celem pracy było opracowanie i implementacja algorytmu

służącego do budowy mapy trójwymiarowej złożonej z trójkątów przez robota

mobilnego. Zadaniem algorytmu jest akwizycja odczytów z skanera laserowego

3D, skonstruowanego z dalmierza laserowego 2D umieszczonego na mechanizmie

obrotowym napędzanym serwomechanizmem, tworzenie na ich podstawie scen

trójwymiarowych (3D) oraz łączenie wielu scen w spójną mapę trójwymiarową.

Dodatkowym celem było napisanie interaktywnego silnika graficznego,

umożliwiającego wizualizację w czasie rzeczywistym złożonych map

trójwymiarowych zbudowanych z trójkątów.

Praca zawiera także omówienie problematyki budowania oraz wizualizacji

map trójwymiarowych, ze szczególnym naciskiem na ich wykorzystanie w

robotyce, jak również technologii do tych celów stosowanych. Opisane zostały

różne reprezentacje map trójwymiarowych wykorzystywane w robotyce oraz

kolejne etapy ich konstruowania.

Ostatnim elementem pracy jest omówienie otrzymanych wyników – jakości

otrzymanych map 3D oraz wydajności silnika graficznego.

1.2. Zawartość pracy

Praca została podzielona na siedem rozdziałów. W pierwszym rozdziale

scharakteryzowany został cel i zakres pracy, poczynione założenia oraz krótki

opis zawartości pracy. Rozdział drugi został poświęcony omówieniu zagadnienia

budowy map 3D – ich historii i zastosowań oraz na opis wybranych reprezentacji

map 3D, ze szczegółowym uwzględnieniem reprezentacji złożonej z trójkątów.

Rozdział 3 zawiera opis implementacji pierwszej części zadania projektowego –

programu Build3DMap, służącego do budowania map 3D złożonych z trójkątów.

W rozdziale 4 znajduje się opis charakterystyki problemu wizualizacji scen 3D

oraz opis implementacji silnika graficznego FPP3DMap napisanego w środowisku

OpenGL. Szczegółowe informacje dotyczące sposobu uruchamiania oraz

korzystania z narzędzi Build3DMap (zarówno na symulatorze, jak i na

rzeczywistym robocie) oraz FPP3DMap znajduje się w rozdziale 5. W rozdziale 6

znajduje się analiza otrzymanych wyników, zaś ostateczne podsumowanie w

rozdziale 7.

2

2. Mapy trójwymiarowe

2.1. Historia i zastosowania

Temat budowania map 3D budynków (od wewnątrz, jak i od zewnątrz) oraz

innych obiektów od kilku lat cieszy się dużą popularnością. Trójwymiarowe

modele są używane, bądź planuje się ich wykorzystywanie w wielu dziedzinach.

Przykładowo, architekci mogą używać modeli 3D do projektowania i oceny

budynków, a nawet całych miast i regionów, przy użyciu technologii VR. Zespoły

szybkiego reagowania, jak strażacy, czy antyterroryści mogliby wykorzystywać

modele 3D do ćwiczeń, w celu wypracowania najlepszej strategii postępowania w

miejscu zagrożenia. Modele takie byłyby również przydatne dla robotów, dla

których środowiskiem działania byłyby skupiska ludzi. Ostatecznie, modele 3D są

doskonałym uzupełnieniem dla przemysłu gier komputerowych, gdzie są obecnie

najczęściej wykorzystywane.

Na początku lat 90. prognozowano, że w momencie nadejścia nowego

millenium około 50000 niezależnych autonomicznych robotów będzie

wykonywało prace w różnych sektorach – od produkcji po sektor usługowy.

Rzeczywistość przedstawia się inaczej. O ile w sektorze produkcyjnym pojazdy

kierowane przy użyciu magnetycznego bądź optycznego toru są standardem, to

pojazdy nie posiadające z góry ustalonego toru poruszania się są wyjątkowo

rzadkie. Jednym z głównych powodów takiej rozbieżności pomiędzy prognozami

a rzeczywistością jest brak dobrych, szybkich, a przede wszystkim tanich

sensorów, które umożliwiałyby robotom na rozpoznawanie środowiska w czasie

rzeczywistym oraz podejmowaniu decyzji na podstawie zebranych danych.

2.2. Zbieranie informacji o otoczeniu 3D robota

Zbieranie informacji o otoczeniu 3D robota jest podstawową czynnością

umożliwiającą zbudowanie mapy trójwymiarowej. Zadanie to polega na zbieraniu

przez czujniki punktów 3D { } 31 2 3, , ,..., NP p p p p R= ∈ takich, w których wykryte

zostało istnienie przeszkody (i

p należy do przeszkody). Otrzymany zbiór

punktów jest następnie przekształcany przy zastosowaniu rozmaitych

algorytmów na wybraną reprezentację mapy 3D.

Aby zebrane punkty jak najlepiej opisywały otaczające robota środowisko,

stosuje się różne czujniki i konstrukcje, które zostały opisane w poniższych

podrozdziałach.

3

2.2.1. Wykorzystywane czujniki

Najczęściej wykorzystywane urządzenia w robotyce umożliwiające percepcję

otoczenia to:

• skanery laserowe 2D, które odczytują odległości do przeszkód znajdujących

się na jednej płaszczyźnie. Wykorzystywane są następujące ich typy [1]:

o typ AM-CW (ang. Amplitude Modulated Carrier Wave) – mierzą różnicę

w fazie pomiędzy wyemitowaną oraz uzyskaną po odbiciu wiązką.

Wyróżniają się najkrótszym czasem akwizycji próbek, zakres pomiarowy

dochodzi do 50 m. Wraz ze wzrostem odległości przeszkody dość silnie

rośnie niedokładność pomiaru. Są wrażliwe na światło słoneczne, dlatego

używa się ich głównie wewnątrz pomieszczeń.

o typ TOF (ang. Time Of Flight) – mierzą czas od wyemitowania wiązki do

jej powrotu. Mogą działać na większych dystansach, są używane do

akwizycji próbek na terenach odkrytych, za to są wolniejsze od AM-CW.

Niedokładność pomiaru związana z odległością do przeszkody nie rośnie

tak silnie, jak w AM-CW.

o typ FM-CW (ang. Frequency Modulated Carrier Wave) – mierzą różnicę w

częstotliwości wiązki odbitej i wyemitowanej. Cechują się bardzo dużą

precyzją, za cenę skomplikowanej konstrukcji i podatności na

uszkodzenia.

W robotyce wykorzystywane są jedynie AM-CW oraz TOF.

• skanery laserowe 3D, które są w stanie dostarczyć informacje o przeszkodach

znajdujących w dowolnej odległości pod dowolnym kątem w stosunku do

skanera.

• kamery – obrazy odczytane z co najmniej dwóch kamer filmujących otoczenie

z różnych perspektyw mogą zostać przetworzone za pomocą specjalnych

algorytmów na odległości do obserwowanych przeszkód. Ten sposób

akwizycji informacji o przeszkodach cechuje się dużą niepewnością i małą

precyzyjnością pomiaru.

• sonary – zmierzony czas odbicia od przeszkody fali dźwiękowej może

sugerować jej odległość od robota. W praktyce pomiary wykonane przy

użyciu sonaru obciążone są bardzo dużym błędem.

2.2.2. Wykorzystywane urządzenia pomiarowe

Do budowania map 3D wykorzystuje się najczęściej skanery laserowe 2D lub

3D ze względu na dużą dokładność uzyskiwanych pomiarów.

4

Przykładową konstrukcją tego typu, wykorzystaną w pracach [2] i [3], jest

robot z dwoma dalmierzami laserowymi 2D, jednym zamontowanym poziomo,

drugim pionowo. Punkty uzyskane za pomocą drugiego dalmierza przekształcane

są w punkty w trzecim wymiarze, dzięki znajomości pozycji robota, uzyskanej za

pomocą pierwszego skanera. Rysunek 1 przedstawia typową konstrukcję tego

typu. Głównym minusem tego podejścia jest brak możliwości skanowania

bocznych ścian obiektów. Problem ten rozwiązuje konstrukcja, opisana w [4].

Polega ona na zamontowaniu 2 dodatkowych dalmierzy, każdego pod kątem 45˚.

We wszystkich tych podejściach występowały problemy z omijaniem przeszkód z

wystającymi krawędziami.

Rysunek 1. Dwa skanery 2D – jeden zamontowany poziomo, drugi pionowo [3].

Trójwymiarowe modele środowiska próbowano także otrzymywać za

pomocą skanerów laserowych 3D, które generują spójne zbiory punktów 3D.

Rozwiązanie to wykorzystano przy projektach takich, jak RESOLV (modelowanie

wnętrz dla potrzeb VR) [5], bądź AVENUE (tworzenie modeli środowisk

miejskich) [6].

Ponieważ jednak ceny skanerów 3D są o wiele wyższe niż dalmierzy 2D,

podjęto wiele prób skonstruowania lasera 3D przy zastosowaniu lasera 2D.

Pomysł opiera się na skonstruowaniu mechanizmu, zmieniającego kąt skanowania

dalmierza 2D. Przy takim podejściu wykonanie pojedynczego skanu 3D polega na

przeprowadzeniu wielu skanów 2D pod różnymi kątami, a następnie za pomocą

5

jednego z algorytmów połączenie ich w jeden. W tego typu konstrukcjach montaż

skanera odgrywa ważną rolę. Wykorzystywane są konstrukcje, w których laser 2D

zamontowany jest na elemencie obrotowym przy użyciu serwomechanizmu z osią

obrotu Z, przykładowo w [7], [8] i [9], bądź osią obrotu X – [10] i [11], co ukazuje

Rysunek 2.

Rysunek 2. Laser 3D zamontowany na serwomechanizmie a) z osią obrotu OZ [8] b) z osią obrotu OX [10]

2.3. Reprezentacje map 3D

Geometria obiektów trójwymiarowych może być reprezentowana na wiele

sposobów. Najczęściej wykorzystywane konstrukcje w grafice 3D to:

• 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ób można tworzyć proste bryły, albo – jeśli siatka jest dostatecznie

gęsta – dobrze przybliżać skomplikowane obiekty (patrz Rysunek 3).

Rysunek 3. Przykładowa mapa 3D złożona z trójkątów [22].

6

• Woksele1 – obiekt jest budowany z elementarnych sześcianów (trójwymia-

rowych pikseli). Tego rodzaju reprezentacja jest rozpowszechniona

szczególnie w diagnostyce medycznej, gdzie uzyskuje się szereg przekrojów

(obrazów bitmapowych) ciała pacjenta i na ich podstawie tworzy

trójwymiarowe modele. Przykładowy model 3D złożony z wokseli

przedstawia Rysunek 4.

Rysunek 4. Model głowy człowieka złożony z wokseli [38].

• Opis matematyczny – obiekty są określone równaniami. Mogą to być np. kule,

płaszczyzny, oraz szczególnie użyteczne i powszechnie stosowane

powierzchnie parametryczne (płaty powierzchni), np. powierzchnie Beziera,

Hermite'a bądź NURBS2 (patrz Rysunek 5).

Rysunek 5. Model 3D zbudowany z NURBS [37].

1 Woksel (ang. voxel, volumetric pixel) to w grafice trójwymiarowej najmniejszy element przestrzeni, w pewnym sensie odpowiednik piksela w grafice dwuwymiarowej. Przy użyciu voxeli scenę przedstawia się jako trójwymiarową tablicę, na przykład 512×512×512 voxeli. Najczęściej voxeli używa się do przedstawiania i analizy medycznych i naukowych danych. 2 NURBS – (ang. Non-Uniform Rational B-Spline) to popularna nazwa dla dwóch rodzajów obiektów: krzywych i powierzchni. Kształt tych krzywych określany jest za pomocą punktów kontrolnych i znakomicie nadaje się do modelowania złożonych obiektów w programach do tworzenia grafiki 3D. Powierzchnia NURBS jest matematycznie najbardziej elastyczną metodą przedstawienia modelu 3D.

7

Dziedzina robotyki cechuje się pewnymi wymaganiami i ograniczeniami,

które wykluczają niektóre z prezentowanych powyżej reprezentacji obiektów i

map 3D. Wymagania te dotyczą następujących aspektów map 3D:

• Sposób pozyskiwania – jeżeli zadaniem robota jest zbudowanie mapy

nieznanego otoczenia na podstawie odczytów z czujników, należy się liczyć z

dużymi błędami pomiarowymi. Nie ma zatem potrzeby stosowania super-

dokładnych opisów. Powstała mapa powinna charakteryzować się taką

dokładnością, na jaką pozwalają posiadane czujniki.

• Przechowywanie w pamięci – roboty generalnie cechują się dość ograniczoną

pojemnością pamięci, zatem nie należy stosować reprezentacji wymagających

dużych jej ilości.

• Przetwarzanie, uaktualnianie – operacje, takie jak dodawanie do mapy nowo

zbadanych obszarów albo łączenie kilku odrębnych wycinków mapy w jedną

spójną całość nie powinny być bardzo wymagające obliczeniowo.

Wobec powyższych ograniczeń w robotyce nie ma sensu stosowania opisu

matematycznego (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ólnych wielokątów

mogą zależeć od dokładności przeprowadzonych pomiarów) oraz nie są bardzo

wymagające obliczeniowo ani pamięciowo. Nie jest to jednak jedyna

wykorzystywana w robotyce reprezentacja rzeczywistości 3D. Innymi

spotykanymi w robotyce reprezentacjami map 3D są: mapy złożone z drzew

ósemkowych (modyfikacja reprezentacji złożonej z wokseli) oraz mapy

zbudowane z powierzchni płaskich.

W następnych podrozdziałach bardziej szczegółowo opisane zostaną

reprezentacje map 3D używane w robotyce oraz kolejne etapy ich budowy. Opis

ten będzie szczegółowy i wyczerpujący jedynie dla map złożonych z wielokątów,

na których w swojej pracy się skupię.

8

2.3.1. Mapy złożone z powierzchni płaskich

Przedstawienie sceny 3D za pomocą powierzchni płaskich ściśle wiąże się ze

sposobem jej utworzenia (z wykorzystaniem dalmierza laserowego 3D, bądź 2D

zamontowanego na serwomechanizmie). Ten sposób reprezentacji mapy 3D

opiera się na następującym spostrzeżeniu: skoro zbiór punktów 3D uzyskany za

pomocą dalmierza laserowego 2D składa się z wielu skanów (wykonanych przy

różnych kątach nachylenia za pomocą serwomechanizmu), istnieje duże

prawdopodobieństwo, że wiele kolejnych punktów wchodzących w skład

pojedynczego skanu 2D opisuje współrzędne jednego, prostego obiektu (np.

ściany). Ponieważ skany charakteryzują się błędami pomiarowymi, należy

zastosować jeden z istniejących algorytmów do wykrywania odcinków w zbiorze

punktów. W tym celu często wykorzystuje się transformatę Hough’a3, czasami też

inne algorytmy, przykładowo:

Niech 1 2 3( , , ,..., )N

a a a a będzie uszeregowanym zbiorem punktów

wchodzących w skład pojedynczego skanu 2D, znajdujących się na jednej

płaszczyźnie. Dla dowolnych indeksów i i j i> para punktów i

a i 1ja + znajduje

się w jednej linii, jeżeli spełniona jest nierówność:

1

1

,

,

i j

j

t t

t i

a a

a a

ε+

+=

≥

∑,

gdzie ε jest pewną z góry założoną, dobraną eksperymentalnie wartością, a

,x y jest długością odcinka łączącego punkty x i y w metryce Euklidesowej.

Wartość ε musi należeć do przedziału (0,1), przy czym im bliżej wartości 1, tym

kryterium znajdywania linii jest bardziej restrykcyjne (linie będą znajdywane

rzadziej i będą krótsze).

Po wykonaniu tego kroku otrzymywana jest sekwencja linii w kolejno

skanowanych płaszczyznach, przybliżająca kształt obserwowanej sceny.

Powierzchnie płaskie są tworzone ze zbioru linii, jeśli spełnione są trzy kryteria:

• Pierwszy i ostatni punkt linii dodawanej musi być w pewnej odległości, nie

większej niż ϕ , od pierwszego i ostatniego punktu linii do powierzchni już

należącej,

3 Transformata Hough’a – technika służąca do ekstrakcji cech, wykorzystywana najczęściej przetwarzaniu obrazów cyfrowych. Klasyczna transformata znajduje linie w obrazie cyfrowym, można ją łatwo zaadoptować do znajdywania linii w pozornie nieuporządkowanym zbiorze punktów. Znane są rozszerzenia tej techniki służące do identyfikacji bardziej złożonych kształtów.

9

• Kąt pomiędzy łączonymi liniami musi być mały (wartość maksymalna

powinna zostać dobrana eksperymentalnie),

• Linia dodawana do figury utworzonej z wielu połączonych już linii musi

znajdować się na tej samej płaszczyźnie, na której znajduje się ta figura.

Rysunek 6. Kolejne kroki algorytmu budującego mapy 3D złożone z powierzchni płaskich [12]: a) Obraz rzeczywisty, dla którego przeprowadzono testy b) Wykryte linie proste w scenie c) Powierzchnie płaskie uzyskane z linii

Rysunek 7. kroki algorytmu budującego mapy 3D złożone z powierzchni płaskich [13]: a) Obiekt, dla którego przeprowadzono testy b) Zbiór nieprzetworzonych punktów uzyskanych przez skaner c) Wykryte linie dla tego zbioru d) Powierzchnie uzyskane z połączenia odpowiednich linii.

Rysunek 6 i Rysunek 7 przedstawiają przykładowe wyniki uzyskane w

pracach [12] i [13] poprzez zastosowanie opisanej powyżej metody.

Reprezentacja mapy 3D złożonej z powierzchni płaskich może zostać

uzyskana również w zupełnie odmienny sposób, zaproponowany w [14]:

1. Na scenę złożoną z pobranych przez czujniki punktów 3D zostaje

nałożona trójwymiarowa siatka sześcianów o krawędziach o

konfigurowalnej długości (np. 30x30x30 cm), o krawędziach równoległych

10

do osi OX, OY i OZ. Sześciany zawierające mniej niż k (np. 6) punktów

oraz punkty w nich zawarte są usuwane (patrz Rysunek 8).

2. Zawartość każdego z pozostałych sześcianów zostaje przybliżona płaską

powierzchnią na podstawie położenia punktów 3D wewnątrz sześcianu

(patrz Rysunek 9).

3. Płaskie powierzchnie z sąsiednich sześcianów są łączone, o ile spełnione są

wymagania na orientację i na przesunięcie wobec siebie (patrz Rysunek 9).

Rysunek 8. Początkowy krok algorytmu opisanego w [14]: a) Zbiór nieprzetworzonych punktów 3D b) Sześciany zawierające minimum 6 punktów

Rysunek 9. Drugi i trzeci krok algorytmu opisanego w [14]: a) Powierzchnie płaskie powstałe z aproksymacji punktów wewnątrz każdego sześcianu b) Wynik połączenia powierzchni płaskich z punktu a)

Podsumowując, reprezentacja złożona z powierzchni płaskich charakteryzuje

się niską złożonością obliczeniową fazy budowy mapy za cenę niskiej dokładności

mapy z przerwami pomiędzy sąsiednimi elementami. Dodatkowym minusem jest

brak dobrze udokumentowanych i łatwo dostępnych metod obróbki

zbudowanych w ten sposób scen 3D.

11

2.3.2. Mapy złożone z drzew ósemkowych4

Drzewa ósemkowe jest to bardzo wydajna struktura danych, która

zastosowana do budowy sceny 3D daje dość dobre rezultaty, o czym można się

przekonać w pracach [15] i [16].

Rysunek 10. Wyniki działania algorytmu opartego na drzewach ósemkowych [16]: a) Zbiór punktów znaleziony przez skaner laserowy b) Reprezentacja sceny 3D zbudowana z drzewa ósemkowego

Tworzenie sceny 3D zbudowanej z drzewa ósemkowego polega na

następującym schemacie:

1. Pobranie punktów 3D ze sceny przy pomocy dalmierza laserowego.

2. Otoczenie całej sceny pojedynczym dużym sześcianem.

3. Każdy istniejący sześcian w scenie:

a. Usuń, jeżeli nie zawiera żadnych pobranych punktów 3D.

b. Podziel na osiem mniejszych sześcianów w przeciwnym razie.

4. Krok 3. należy powtarzać aż do uzyskania żądanej dokładności

reprezentacji sceny.

Przykładowy wynik opisanego powyżej algorytmu przedstawia Rysunek 10.

Cechami charakterystycznymi map 3D zbudowanych z drzew ósemkowych są:

mała zajętość pamięci i niskie wymagania obliczeniowe, mało skomplikowane

łączenie wielu scen 3D w jedną spójną mapę, ale także niezbyt duża dokładność

reprezentacji fizycznej sceny. Co więcej, nie każdy podział sześcianów na mniejsze

elementy zaowocuje poprawieniem dokładności przybliżenia, co ukazuje

4 Drzewo ósemkowe (ang. octree) – drzewiasta struktura danych, w której każdy element ma 0 lub 8 „dzieci”. Każdy element drzewa ósemkowego reprezentuje fizyczny sześcian, zaś każde „dziecko” każdego elementu reprezentuje sześcian będący ósmą częścią swojego „rodzica”. Poziom dokładności odwzorowania rzeczywistości przez drzewa ósemkowe można zmieniać poprzez zdefiniowanie maksymalnej wysokości drzewa oraz poprzez zdefiniowanie maksymalnej wielkości najmniejszego elementu.

12

Rysunek 11. Efekt może być odwrotny – powstanie lub zwiększy się liczba

przerw, które nie istnieją w fizycznym świecie.

Rysunek 11. Zobrazowanie zagrożenia zbyt dużej liczby podziałów drzewa ósemkowego: a) 3 kolejne wiązki lasera trafiają na przeszkodę. Reprezentacja sceny złożonej z wokseli jest spójna. b) Ta sama sytuacja co w punkcie a), po podziale drzewa ósemkowego. Reprezentacja sceny złożonej

z wokseli przestała być spójna.

13

2.3.3. Mapy złożone z wielokątów

Reprezentacja map i obiektów 3D złożonych z wielokątów jest szeroko

używana w grafice komputerowej, dzięki czemu istnieje zbiór dobrze

zdefiniowanych metod służących do obróbki tak zbudowanych scen oraz ich

wizualizacji. We wszystkich trójwymiarowych grach komputerowych mapy

terenu są przechowywane i wyświetlane w takiej właśnie postaci. Wszystkie

obecnie produkowane karty graficzne posiadają wsparcie sprzętowe i

programowe do obróbki i renderowania w czasie rzeczywistym wielu (do

miliarda na sekundę) wielokątów. Sprawia to, że coraz więcej projektów

naukowych związanych z robotyką wykorzystuje tą właśnie reprezentację do

budowania map 3D badanego terenu, np. [2], [3], [16].

Z wielokątów w praktyce najczęściej używane są jedynie trójkąty, dużo

rzadziej czworokąty, zaś bardziej złożone wielokąty praktycznie nigdy. Wynika to

z dwóch faktów. Po pierwsze dla trójkątów nie zachodzi konieczność

sprawdzania, czy są opisane na płaszczyźnie, zaś dla pozostałych brył taka

konieczność zachodzi. Po drugie z odpowiedniej liczby trójkątów da się złożyć

każdy dowolny wielokąt, co ukazuje Rysunek 12. Biorąc to pod uwagę, w

dalszych rozważaniach na temat map 3D złożonych z wielokątów będę zakładał,

że mapy te są złożone jedynie z trójkątów.

Rysunek 12. Tworzenie dowolnego wielokąta z trójkątów.

Osiągnięcie reprezentacji sceny 3D złożonej z wielokątów (trójkątów) ze

zbioru punktów 3D w przestrzeni polega na połączeniu odpowiednio blisko siebie

położonych punktów krawędziami, a następnie na takim połączeniu krawędzi w

trójkąty, aby te jak najlepiej przybliżały obserwowaną scenę w świecie fizycznym

(szczegółowy opis w rozdziale 2.3.3.1).

Ponieważ otrzymana liczba trójkątów potrafi dochodzić do bardzo dużych

ilości, może zajść konieczność implementacji algorytmu służącego do

zmniejszenia liczby trójkątów wchodzących w skład sceny, ale nie zmieniającego

w sposób istotny samej sceny (szczegółowy opis w rozdziale 2.3.3.2).

14

Aby do zbudowania mapy 3D mogły posłużyć informacje uzyskane w wielu

punktach przeprowadzania pomiaru, należy dodatkowo zaimplementować

metody służące do wyznaczania kolejnych miejsc przeprowadzania pomiaru oraz

do łączenia w spójną mapę wielu otrzymanych w ten sposób scen 3D

(szczegółowy opis w rozdziałach 2.3.3.3 i 2.3.3.4).

Warto w tym momencie zauważyć, że upraszczanie scen 3D i automatyczne

wyznaczanie miejsc pomiarowych, jakkolwiek są bardzo przydatne, nie są

elementami krytycznymi całego procesu budowania mapy i mogą zostać

pominięte.

2.3.3.1. Tworzenie reprezentacji sceny złożonej z wielokątów

Scenę 3D złożoną z wielokątów można zbudować na dwa sposoby, różniące

się wiedzą posiadaną a priori oraz złożonością obliczeniową.

W pierwszym sposobie zakłada się, że posiadany jest ostateczny, globalny w

skali budowanej mapy zbiór punktów zebranych ze wszystkich miejsc

przeprowadzania pomiarów. Jest to podejście uogólnione, polegające na

połączeniu odpowiednio blisko siebie położonych punktów krawędziami, a

następnie na takim połączeniu krawędzi w trójkąty, aby te jak najlepiej przybliżały

obserwowaną scenę w świecie fizycznym. Zaletą tego rozwiązania jest globalnie

spójna reprezentacja mapy wyjściowej (brak zachodzących na siebie elementów

mapy), zaś najważniejsze wady to bardzo duża złożoność obliczeniowa oraz

uzyskanie mapy dopiero po zakończeniu procesu pobierania próbek.

W przeciwieństwie do poprzedniej, druga metoda polega na budowaniu

sceny 3D po każdej operacji przeprowadzenia pomiarów. Kolejne elementy mapy

są dołączane do już istniejącej w miarę poruszania się robota, dzięki czemu w

każdym momencie czasowym robot posiada mapę złożoną ze wszystkich

odwiedzonych już miejsc. Wadą tego podejścia jest konieczność stosowania

metody polegającej na łączeniu wielu scen 3D w jedną.

W obydwu opisanych powyżej podejściach zastosowanie dalmierza

laserowego jako urządzenia pomiarowego silnie obniża złożoność obliczeniową.

Dzieje się tak, ponieważ wzajemne odległości mogą być liczone jedynie dla

punktów uzyskanych z sąsiednich wiązek dalmierza, co bardzo zmniejsza liczbę

potencjalnych krawędzi, z których następnie budowane są trójkąty.

15

2.3.3.2. Upraszczanie sceny 3D

Upraszczanie sceny 3D złożonej z trójkątów jest obecnie dobrze opisanym w

literaturze problemem, w praktyce stosuje się wiele różnych wariantów, zależnie

od specyficznych właściwości sceny. Celem tej operacji jest uzyskanie wyjściowej

reprezentacji sceny 3D złożonej z mniejszej ilości trójkątów, przy zachowaniu

poziomu szczegółowości pierwotnej reprezentacji. W robotyce krok ten jest

szczególnie przydatny, ponieważ wszystkie pomiary mają ograniczoną

dokładność, można by rzec, że cechują się szumem losowym. Algorytmy

upraszczania scen 3D złożonych z trójkątów uśredniają pomiary w scenie

wyjściowej, działając jak filtr medianowy. Przykładowy efekt działania algorytmu

upraszczającego ukazuje Rysunek 13.

Istnieją dwie klasy takich algorytmów:

• Algorytmy zależne od obserwatora [18] – Wynik uproszczenia jest zależny od

pozycji obserwatora oraz kąta patrzenia. Proces upraszczania musi być

częściowo, bądź całkowicie powtórzony, jeśli obserwator zmieni swoją

pozycję. Główną zaletą algorytmów z tej klasy jest fakt, że elementy sceny

odległe od obserwatora, bądź dla niego niewidoczne mogą być o wiele

bardziej uproszczone, niż elementy znajdujące się blisko niego. Przy użyciu tej

samej liczby trójkątów do opisania sceny, daje to dużo lepsze wizualnie

rezultaty, niż drugie podejście.

• Algorytmy niezależne od obserwatora – Wynik uproszczenia jest niezależny

od pozycji i kąta patrzenia obserwatora. Obliczenia nie muszą być

powtarzane, jeśli położenie obserwatora się zmieni. Elementy sceny

znajdujące się blisko obserwatora są traktowane identycznie jak te znajdujące

się daleko od niego.

Rysunek 13. Fragment modelu ściany, uzyskanego z zeskanowania dalmierzem laserowym korytarza w Carnegie Mellon University, przed i po uproszczeniu [3].

16

Dla potrzeb budowania map 3D przez autonomiczne roboty mobilne

naturalne jest drugie podejście. Algorytmy niezależne od obserwatora można dalej

podzielić na 3 podklasy, w zależności od sposobu przeprowadzania uproszczenia.

Są to:

• algorytmy polegające na usuwaniu wierzchołków

• algorytmy polegające na grupowaniu trójkątów

• algorytmy polegające na usuwaniu krawędzi

Algorytmy polegające na usuwaniu wierzchołków5

Ta klasa algorytmów została szczegółowo opisana w [19] i [20]. Schemat

działania polega na iteracyjnym wykonywaniu kroków:

• Wybór wierzchołka do usunięcia

• Usunięcie wybranego wierzchołka oraz wszystkich stycznych z nim trójkątów

• Wypełnienie powstałej luki przez powiększenie trójkątów z nią graniczących

Algorytmy z tej klasy różnią się jedynie pierwszym krokiem, czyli kryterium

wyboru wierzchołka do usunięcia. Rysunek 14 przedstawia najczęściej używane

kryteria.

Rysunek 14. Najczęściej używane kryteria wyboru wierzchołka do usunięcia [19]: a) Jeżeli wierzchołek jest w odległości co najwyżej d do przybliżonej płaszczyzny tworzonej przez pozostałe punkty, może być usunięty. b) Jeżeli wierzchołek jest graniczny, to algorytm oblicza jego odległość do prostej powstałej z połączenia wierzchołków tworzących granicę. Jeżeli odległość ta jest mniejsza niż d, wierzchołek może być usunięty.

5 ang. vertex decimation

17

Algorytmy polegające na grupowaniu wierzchołków6

Algorytmy z tej grupy dzielą całą scenę 3D siatką złożona z sześcianów o

określonej wielkości. Wszystkie trójkąty znajdujące się w obrębie pojedynczego

sześcianu są zamieniane na pojedynczy trójkąt, którego położenie jest zależne od

wag i położeń wierzchołków, wchodzących w skład danego sześcianu. Jakość

wyniku zależy od wielkości użytych sześcianów. Rysunek 15 przedstawia

przykładowy efekt działania algorytmu z tej grupy.

Główne minusy tego algorytmu to niska jakość przybliżenia (w porównaniu

z innymi metodami), brak możliwości zachowania topologii i ograniczenia

maksymalnego błędu oraz niedeterministyczność (dwa identyczne obiekty

znajdujące się w różnych miejscach na scenie – inaczej podzielone przez siatkę,

zostaną w różny sposób uproszczone). Z drugiej strony, bardzo duża szybkość i

stabilność są jego istotnymi zaletami.

Rysunek 15. Wyniki uzyskane po zastosowaniu algorytmu grupującego trójkąty w [21]: a) Scena 3D zawierająca 4804 trójkąty b) Ta sama scena po uproszczeniu metodą grupowania trójkątów – 218 trójkątów i jedna krawędź

6 ang. vertex clustering

18

Algorytmy polegające na usuwaniu krawędzi7

Ta klasa algorytmów została szczegółowo opisana w [22] i [23]. Schemat

działania (patrz Rysunek 16) polega na iteracyjnym wykonywaniu kroków:

• Wybór krawędzi do usunięcia

• Usunięcie wybranej krawędzi oraz wszystkich stycznych z nią trójkątów

• Wypełnienie powstałej luki przez powiększenie trójkątów z nią graniczących

Rysunek 16. Działanie typowego algorytmu bazującego na usuwaniu krawędzi [22].

Rysunek 17. Wynik działania algorytmu upraszczającego model 3D poprzez usuwanie krawędzi [22]: a) Model złożony z 5408 trójkątów b) Model złożony z 1000 trójkątów c) Model złożony z 500 trójkątów

Jak widać (Rysunek 16), algorytm zaproponowany w [22] działa jedynie w

obrębie zamkniętych zbiorów (ang. manifold surfaces). W opracowaniu [23]

zaproponowano modyfikację tego algorytmu, dzięki której możliwe się staje

łączenie wielu scen w jedną. Jest to możliwe dzięki uogólnieniu algorytmu

zaproponowanego przez Hoppe’go w [22], polegającym na łączeniu dowolnych

dwóch, znajdujących się odpowiednio blisko siebie wierzchołków, nawet takich,

które niekoniecznie połączone są krawędzią. Dzięki tej właściwości algorytm

opisany w [23] można stosować, przy niewielkich modyfikacjach, także do

łączenia kilku scen 3D w jedną (bardzo przydatne dla potrzeb robotyki, gdzie

ostateczna postać mapy może być w szczególności sklejeniem wielu scen 3D).

7 ang. iterative egde contraction

19

Efekt działania zmodyfikowanego algorytmu usuwania krawędzi przedstawia

Rysunek 18.

Rysunek 18. Algorytm polegający na usuwaniu krawędzi po modyfikacji opisanej w [23] zyskuje możliwość łączenia nie zamkniętych powierzchni 3D8

Podsumowanie algorytmów upraszczających scenę 3D

Podsumowując, metody polegające na usuwaniu wierzchołków są dość

szybkie i produkują wyniki wysokiej jakości, zachowując topologię sceny. Metody

polegające na grupowaniu wierzchołków są najszybsze, ale produkują wyniki

najniższej jakości spośród opisanych algorytmów oraz nie zachowują topologii

sceny. Dodatkowo nie istnieje możliwość ograniczenia maksymalnego błędu

przybliżenia oraz maksymalnej liczby trójkątów w wynikowej scenie (ustalanie

rozmiarów siatki nie przekłada się jednoznacznie na ilość trójkątów po

uproszczeniu – zależy to także od rozmieszczenia obiektów na scenie). Algorytmy

polegające na usuwaniu krawędzi ustępują trochę szybkością algorytmom

usuwającym wierzchołki, ale za to jako jedyne umożliwiają modyfikację, dzięki

której możliwe jest łączenie wielu scen 3D w jedną. Dodatkowo wyniki przez nie

produkowane są wysokiej jakości, choć w wersji zmodyfikowanej nie jest możliwe

ścisłe zachowanie topologii sceny. Dodatkową cechą algorytmów usuwających

krawędzie lub wierzchołki jest ich iteracyjność – wszystkie zmiany mają charakter

lokalny, możliwe jest zatem zapisanie zmian do oddzielnego pliku po

każdorazowym lokalnym uproszczeniu, dzięki czemu można otrzymać zbiór o

stopniowo malejącej złożoności wersji pierwotnej sceny.

8 ang. non-manifold surfaces

20

2.3.3.3. Wyznaczanie kolejnych miejsc przeprowadzania pomiarów

W celu zbudowania spójnej i dokładnej mapy 3D autonomiczny robot

mobilny musi wykonywać skany 3D w odpowiednio wyznaczonych punktach.

Optymalnym punktem do przeprowadzenia kolejnego skanu 3D jest pozycja

( , , )x z θ , która ma wysoki spodziewany współczynnik przyrostu informacji i jest

osiągalna dla robota. Co więcej, ścieżka prowadząca do tego punktu nie powinna

być zbyt długa.

Standardowe metody wykorzystywane w grafice komputerowej zakładają,

że sensor zbierający dane może się dowolnie obracać, a informacje przez niego

uzyskane są idealne [24], tak więc i tym razem należy zastosować podejście

alternatywne, utworzone z myślą o robotyce (ograniczony stopień swobody

sensora, fizyczna obecność sensora w skanowanej scenie, ograniczona precyzja

skanu, jedynie przybliżona informacja o aktualnym położeniu sensora). Do

rozwiązania tego zadania w praktyce stosuje się wiele różnych podejść.

Przykładowo, w [25] zauważono, że wyznaczenie punktów obserwacyjnych,

tj. miejsc w których należałoby wykonać skany 3D w celu uzyskania kompletnych

danych o obiekcie jest podobne do problemu galerii sztuki . Problem ten (opisany

dokładnie w [26]) polega na odpowiednim ustawieniu jak najmniejszej liczby

obserwatorów tak, aby widzieli oni całe wnętrze obiektu, przy znajomości mapy

obiektu. Utworzono model, aproksymujący galerię sztuki, rozbijając scenę 3D na

wiele płaskich scen 2D, odpowiadającym płaszczyznom przecinającym scenę 3D

na różnych wysokościach. W skrócie, problem ten został rozwiązany poprzez

wygenerowanie pewnej liczby losowych punktów – kandydatów, a następnie dla

każdego z nich obliczony został spodziewany przyrost informacji. Wybierany jest

punkt o największym przyroście. Rysunek 19 przedstawia przykładowy wynik

działania algorytmu opisanego w [25].

Inne podejście zaproponowano w [27]. Wprowadzono tu pojęcie punktów

orientacyjnych, jako całkowicie rozpoznawalnych i możliwych do zlokalizowania

z dużym przybliżeniem obiektów w przestrzeni 3D. Obiekt może być traktowany

jako punkt orientacyjny, jeżeli da się go z dużym prawdopodobieństwem

sklasyfikować jako jeden z obiektów ze zdefiniowanej wcześniej bazy danych oraz

jeżeli da się z dużym przybliżeniem określić jego pozycję w przestrzeni. W

początkowym kroku określa się przybliżony rozmiar niezbadanej przestrzeni,

odległości do najbliższych krawędzi, ścian i innych przeszkód, oraz wartość

niepewności w poszczególnych rozważanych punktach (wartość niepewności

rośnie proporcjonalnie do odległości, którą robot musi przebyć do wyznaczonego

punktu). Następnie robot przemieszcza się do pewnej (ograniczonej z góry) liczby

21

najlepszych punktów, w każdym wykonując wstępne pomiary – estymując

przyrost informacji, w razie wykonania pełnego skanu 3D w tym punkcie.

Następnie przemieszcza się do najlepszego punktu, wykonuje pełny skan 3D,

dodaje otrzymane informacje do mapy i ponawia cały proces. Wybór najlepszego

punktu jest złożoną operacją, wybierającą największą spośród wyznaczonych dla

każdego punktu wartości liczbowych reprezentujących spodziewany przyrost

informacji. Wyznaczanie owych wartości opisane jest funkcją 10 zmiennych,

takich jak: liczba punktów orientacyjnych w badanym regionie, liczba krawędzi,

odległość do najbliższej krawędzi, skumulowana wartość niepewności, itd.

Rysunek 19. Kolejne 6 iteracji wyznaczania następnego optymalnego punktu skanowania wraz z odkrytymi obszarami, uzyskane przy użyciu algorytmu opisanego w [25].

22

2.3.3.4. Połączenie wielu skanów 3D w jedną spójną mapę

Każdorazowo po przeprowadzeniu pomiarów w środowisku 3D, robot musi

uaktualnić mapę o uzyskane informacje. Teoretycznie, posiadając informację o

wektorze przesunięcia [ , ]x z oraz o kącie obrotu θ pomiędzy poprzednim a

aktualnym punktem skanowania, można bezbłędnie połączyć starą mapę z

danymi uzyskanymi w wyniku przeprowadzenia ostatniego skanu 3D w nową

mapę. W praktyce jednak, ponieważ rzeczywista odometria robota odbiega od

idealnej, konstruowanie mapy 3D bez kompensacji błędów, prowadzi do ich

kumulacji i w konsekwencji do niespójności mapy (jako przykład może posłużyć

Rysunek 20).

Rysunek 20. Mapa fragmentu korytarza powstała przez złożenie 20 skanów 3D bez kompensacji błędów odometrii [12].

Metody dopasowywania do siebie skanów 3D można podzielić na dwie

kategorie:

• Dopasowywanie jako problem optymalizacyjny – do określenia jakości

dopasowania używana jest pewna forma funkcji kosztu. Dopasowanie

odbywa się poprzez obrót i translację nowego fragmentu mapy o wartości

minimalizujące funkcję kosztu.

• Dopasowywanie jako problem ekstrakcji wybranych cech – do obliczenia

dopasowania wykorzystywane jest porównanie położenia wybranych cech w

mapie oraz w dodawanym fragmencie.

23

Najczęściej używanym algorytmem należącym do klasy optymalizacyjnych

jest, opracowany w 1991 roku, algorytm ICP9, opisany szczegółowo w [29], [30] i

[31]. Polega on na minimalizacji funkcji kosztu E(R,t):

2

,1 1

( , ) ( )m dN N

i j i j

i j

E R t w m Rd t= =

= − +∑∑

W powyższej funkcji połączyć chcemy 2 zbiory uzyskanych niezależnie

punktów – M (zbiór modelowy, m

M N= ) oraz D (zbiór nowych danych, d

D N= ).

Wartością szukaną jest obrót R oraz przesunięcie t zbioru D, dzięki którym zbiór

D będzie możliwie najlepiej dopasowany do zbioru M. Wielkość ,i jw jest równa 1,

jeżeli i-ty punkt ze zbioru M opisuje ten sam punkt w przestrzeni, co j-ty punkt ze

zbioru D, w przeciwnym razie wielkość ta wynosi 0. Iteracyjnie wykonane są

następujące obliczenia:

• Wybór par odpowiadających sobie punktów w zbiorach M i D (zgodnie z

kryterium odległości – najbliższe sobie punkty tworzą parę). Dla wybranych

par punktów ,i jw = 1.

• Transformacja zbioru D o wielkości R i t, minimalizujące funkcję kosztu

E(R, t) dla wybranych par punktów.

Zostało udowodnione, że proces iteracyjny zawsze się zakończy w lokalnym

minimum. W założeniu, w ostatnim kroku iteracji wybrane pary punktów w

rzeczywistości sobie odpowiadają. Złożoność algorytmu ICP w zależy głównie od

metody wyboru par najbliższych punktów (złożoność 2( )O n w przypadku

metody brute force). Znane są różne udoskonalenia tej metody, jak na przykład

redukcja ilości porównywanych punktów [12] oraz stosowanie drzew kD [32].

Innym algorytmem służącym do dopasowywania skanów jest algorytm

przedstawiony w [28]. Założeniem tej metody jest spójność w obrębie

pojedynczego skanu 3D (zaszumienie jest dopuszczalne), wynikiem działania zaś

prawdziwy kąt θ oraz wektor [ , ]x z przesunięcia między dwoma kolejnymi

skanami. Algorytm ten operuje na skanach 2D, ale jeżeli zapewni się spójność

dwóch skanów 2D (należących do dwóch różnych skanów 3D), wykonanych przy

zerowym kącie wychylenia serwomechanizmu kontrolującego pozycję lasera,

zapewni się również spójność dwóch skanów 3D. Dzieje się tak dzięki założeniu

9 ang. Iterative Closest Points

24

spójności odczytów w obrębie pojedynczego skanu 3D. Metoda polega na

połączeniu poszczególnych uzyskanych punktów odcinkami, a następnie na

sporządzeniu histogramu ilości wystąpień poszczególnych kątów między

odcinkami a osią x, co przedstawia Rysunek 21.

Rysunek 21. Działanie metody dopasowywania skanów opisanej w [28]: a) Wynik skanu laserowego, z punktami połączonymi odcinkami. Dla każdej „ściany” podano średni

kąt odcinków względem osi x. b) Histogram ilości wystąpień kątów odczytanych z punktu a

Łatwo można zauważyć zależność, że zmiana orientacji robota ze skanerem

o kąt ϕ zaowocuje zmianą średnich kątów odcinków względem osi x o ϕ i tym

samym spowoduje przesunięcie w fazie wartości na histogramie o ϕ . Ponieważ

odległości pomiędzy szczytowymi wartościami na histogramie są stałe dla danego

środowiska niezależnie od kąta skanowania, ani od dokładnego położenia skanera

na płaszczyźnie xz, kąt θ obrotu pomiędzy poprzednim a aktualnym punktem

skanowania można odczytać z wartości przesunięcia w fazie maksimów na

histogramach. W analogiczny sposób znajdywane jest przesunięcie – po obróceniu

wszystkich punktów w dodawanym skanie 3D o kąt θ , w celu zniwelowania kąta

obrotu między poprzednim, a aktualnym punktem skanowania, tworzone są

histogramy odległości punktów od osi x i y. Na podstawie porównania tych

histogramów można wyznaczyć przesunięcie, dzięki czemu ostatecznie możliwe

jest niemal bezbłędne połączenie dwóch skanów 3D, a w konsekwencji

zbudowanie spójnej mapy.

Rysunek 22. Poprawnie połączone 20 skanów w jedną spójną reprezentację 3D [12].

25

3. Algorytm budowania mapy 3D

3.1. Opis zadania

Zadanie polegało na opracowaniu i implementacji algorytmu

umożliwiającego budowanie map 3D złożonych z trójkątów przez robota

mobilnego. Zrealizowane zostały wszystkie elementy konieczne do utworzenia

reprezentacji 3D – odczyt informacji o otoczeniu 3D robota z dalmierza

laserowego zamontowanego na serwomechanizmie, zbudowanie reprezentacji

sceny 3D po wykonaniu każdego skanu oraz złożenie wszystkich scen 3D do

pojedynczej reprezentacji mapy 3D. Błędy odometryczne kompensowane są przy

pomocy zmodyfikowanego sterownika lodo10.

Drugą częścią zadania projektowego było napisanie w OpenGL silnika

graficznego służącego do wyświetlania w czasie rzeczywistym map 3D

zbudowanych z dużej liczby trójkątów (dokładny opis implementacji znajduje się

w rozdziale 4).

Celem pracy jest umożliwienie osobie kontrolującej robota zbudowania

mapy 3D a następnie możliwość jej wizualizacji. Elementy zadania projektowego

były pisane z myślą o przyszłych rozszerzeniach, np. implementacji algorytmów

upraszczających scenę 3D bądź automatycznego wyznaczania kolejnych punktów

pobierania pomiarów, zaś ostatecznie o całkowitym uniezależnieniu procesu

budowania map 3D od operatora. Dokładny opis możliwych rozszerzeń znajduje

się w rozdziale 0).

3.2. Stanowisko badawcze

Robot mobilny, będący stanowiskiem badawczym (Rysunek 23), składa się z

następujących podzespołów (wymienione są jedynie istotne z punktu widzenia

tematyki map 3D):

• Dalmierz laserowy typu TOF (Time Of Flight) Sick LMS 200

• Serwomechanizm o zakresie obrotu od –13º do 90º

o Silnik elektryczny prądu stałego z przekładnią planetarną

o 2 przetworniki obrotowo-impulsowe (łączna rozdzielczość 4000 / 360º)

• Komputer pokładowy

o Procesor Pentium 900 MHz

o Pamięć RAM 256 MB

o System operacyjny Linux (dystrybucja Gentoo)

10 ang. laser-stabilized odometry

26

Rysunek 23. Zdjęcia robota laboratoryjnego

3.3. Środowisko programistyczne - architektura Player / Gazebo

W trakcie części projektowej pracy często zachodziła potrzeba

przetestowania danego rozwiązania bądź wybranej części kodu. Ponieważ w

początkowej fazie stanowisko badawcze nie było jeszcze gotowe do

uruchomienia, testy odbywały się w środowisku symulacyjnym Player / Gazebo.

Player jest to serwer służący do kontrolowania robotów. Jego

najważniejszymi cechami są:

• Działanie w środowisku sieciowym TCP/IP. Umożliwia to działanie systemu

w rozproszonym środowisku oraz ułatwia zorganizowanie przepływu

informacji między rzeczywistym robotem a komputerem nadzorującym bądź

obserwującym jego pracę.

• Dostarczenie szerokiego zbioru interfejsów służących do sterowania sensorów

i efektorów, takich jak laser lub position.

• Wspieranie dużej liczby istniejących urządzeń (m.in. Sick LMS 200).

• Modularna architektura umożliwiająca pisanie własnych interfejsów i

sterowników urządzeń, bądź rozszerzanie funkcjonalności już istniejących.

• Działanie w architekturach wielorobotyczych.

• Dostępność kodu. Użytkownicy mają możliwość ingerowania w kod serwera,

naprawiając błędy i adaptując go do swoich potrzeb.

27

Gazebo jest to symulator dowolnej liczby robotów działający w środowisku

3D. Jego najważniejszymi cechami są:

• Kompatybilność z serwerem Player.

• Możliwość symulacji wielu robotów naraz.

• Poprawna symulacja praw fizyki. Każdy obiekt ma swoją masę, pęd,

poprawnie symulowana jest grawitacja, zderzenia obiektów itp.

• Dostępność kodu.

Gazebo musi implementować te same interfejsy dla swoich symulowanych

urządzeń, co rzeczywisty robot dla swoich rzeczywistych urządzeń. Dzięki

takiemu rozwiązaniu serwer Player (w zależności od pliku konfiguracyjnego, z

którym zostanie uruchomiony) łączy się bądź z symulatorem, bądź z

rzeczywistym robotem, przy czym w żaden sposób ich nie rozróżnia. Tak pobrane

interfejsy Player udostępnia programom klienckim. Schemat ten przedstawia

Rysunek 24.

Rysunek 24. Schemat współdziałania serwera Player oraz symulatora Gazebo z programem klienckim.

Istotne z punktu widzenia zadania projektowego interfejsy udostępniane

zarówno przez fizycznego robota jak i przez symulator to:

• laser – służy do ustawienia parametrów dalmierza laserowego oraz do

odczytów pobranych przez niego danych

28

• position – umożliwia odczyt położenia oraz orientacji robota dzięki

informacjom odometrycznym

• ptz – służy do kontrolowania i odczytu kąta ustawienia serwomechanizmu.

3.4. Wybór reprezentacji mapy

Jako reprezentację mapy 3D wybrałem reprezentację złożoną z wielokątów,

ponieważ jest ona szeroko używana w grafice komputerowej, dzięki czemu

istnieje zbiór dobrze zdefiniowanych metod służących do obróbki tak

zbudowanych scen oraz ich wizualizacji. Dodatkowo, co ukazałem w rozdziale

2.3, reprezentacja ta jest z powodzeniem stosowana w robotyce.

Spośród wszystkich wielokątów do reprezentowania mapy 3D

wykorzystywać będę jedynie trójkąty, będące podstawowym elementem

składowym wszystkich wielokątów oraz elementem najłatwiej poddającym się

wszelkim modyfikacjom.

3.5. Użyte struktury danych

Jako struktury danych użyte do przechowywania oraz przetwarzania

elementów składowych skanów 3D jak również samych skanów wykorzystane

zostały klasy w języku C++. Jest to naturalne podejście zważywszy na strukturę

budowy skanów 3D. Utworzone zostały następujące klasy (patrz też Rysunek 25):

• Vertex11 – klasa opisująca położenie punktu w przestrzeni 3D (x, y, z).

• Polygon12 – klasa opisująca pojedynczy trójkąt. Składa się z 3 elementów klasy

Vertex.

• Scan2D – klasa zawierająca informacje o położeniach wszystkich punków

zebranych podczas pojedynczego skanu przez dalmierz laserowy, jak również

o położeniu i orientacji robota oraz o wychyleniu serwomechanizmu podczas

wykonywania pomiaru. Dla każdego punktu zdefiniowane jest pole valid,

mówiące, czy punkt ten został zebrany w polu zasiągu skanera (valid = 1, jeśli

odległość punktu od miejsca przeprowadzania pomiaru < maksymalny zasięg

urządzenia), czy nie. Maksymalny zasiąg urządzenia jest wielkością

konfigurowalną, domyślnie wynosi 8 m.

• Scan3D – klasa zawierająca zestaw wielu klas Scan2D zebranych przy

wszystkich kolejnych wychyleniach serwomechanizmu z pojedynczego

punku pomiarowego. Dodatkowo posiada ona wektor obiektów klasy

11 ang. vertex – wierzchołek 12 ang. polygon – fragment powierzchni płaskiej, wielokąt

29

Polygon, w którym przechowywane są trójkąty otrzymane ze wszystkich

punktów zebranych w danym miejscu pomiarowym.

• Scene – klasa zawierająca całą mapę 3D. Składa się z wektora obiektów klasy

Scan3D.

Rysunek 25. Graficzne przedstawienie użytych struktur danych.

3.6. Schemat algorytmu budowania spójnej mapy 3D

Poniższy schemat ukazuje sposób działania programu Build3DMap,

służącego do budowania map 3D złożonych z wielu scen 3D.

Rysunek 26. Schemat działania programu Build3DMap.

Czekaj na polecenie

START

Subskrybuj wszystkie wykorzystywane interfejsy

Przeprowadź skan 3D

STOP

Otrzymano żądanie przeprowadzenia skanu 3D

Wygeneruj trójkąty dla wszystkich przeprowadzonych

skanów 3D Zapisz wszystkie trójkąty do pliku scena.m oraz wszystkie znalezione punkty 3D do pliku

scena.mm

Otrzymano żądanie zakończenia programu

30

3.7. Przeprowadzanie skanu 3D

Poniższy schemat ukazuje sposób przeprowadzania pojedynczego skanu 3D

przy użyciu dalmierza laserowego zamontowanego na serwomechanizmie.

Rysunek 27. Schemat pobierania z otoczenia zbioru punktów 3D.

Minimalne oraz maksymalne wychylenie serwomechanizmu są wielkościami

konfigurowalnymi i w szczególności mogą być sobie równe. Wartość kroku

wychylenia powinna być tak dobrana, żeby jego całkowita wielokrotność była

równa różnicy maksymalnego i minimalnego wychylenia serwomechanizmu.

Ustawienie serwomechanizmu na żądanym poziomie wychylenia jest

czynnością zależną od trzech zmiennych czasowych – minimalnego minT oraz

maksymalnego maxT czasu oczekiwania na zmianę, okresu sprawdzania stanu

wychylenia waitT oraz maksymalnego dopuszczalnego błędu δ . Schemat

ukazujący zmianę kąta wychylenia serwomechanizmu przedstawia Rysunek 28.

Przeprowadzenie skanu 2D, obliczenie wartości (x, y, z)

uzyskanych punktów.

START

NIE

TAK

minα α=

maxα α== stepα α α= +

STOP

α – kąt ustawienia serwomechanizmu

min

max

step

α

α

α

wartości konfigurowalne

31

Rysunek 28. Schemat ukazujący sposób zmiany kąta serwomechanizmu.

Ponieważ serwomechanizm praktycznie nigdy nie ustawi się dokładnie na

żądanym kącie, zwracane jest rzeczywiste ustawienie serwomechanizmu, co

pozwala zminimalizować błędy wynikające z ograniczonej dokładności enkodera.

Wprowadzenie czasu minimalnego jako parametru umożliwia uniknięcie

sytuacji, w której wybierany jest kąt wychylenia serwomechanizmu będący

wystarczająco bliski kątowi docelowemu, a serwomechanizm jest wciąż w ruchu.

Odpowiedni wybór tego parametru umożliwi serwomechanizmowi

ustabilizowanie się na żądanym kącie, zanim zostanie przeprowadzony pomiar.

Bezpiecznymi wartościami minT są 350 milisekund przy półstopniowej

precyzji serwomechanizmu ( stepα = 0.5) oraz 700 milisekund przy jednostopniowej

precyzji. Przy tak dobranych wartościach przeprowadzenie jednego pełnego

skanu 3D (przy minα = 0˚ i maxα = 90˚) zajmuje około 90 sekund.

Dalmierz laserowy zwraca jako wynik pomiaru odległość wykrytej

przeszkody dla każdej ze swoich wiązek. Wielkość ta nie jest istotna z punktu

widzenia budowania mapy 3D i należy ją przekształcić na współrzędne ( , , )k k k

x y z

START

Wyślij do sterownika serwomechanizmu żądanie

ustawienia kąta na destα

startT = now();

Odczytaj obecne wychylenie

serwomechanizmu curα

sleep ( waitT )

NIE

now() - startT > minT

now() - startT > maxT

TAK

dest curα α δ− <

NIE

TAK

BŁĄD

TAK

NIE

STOP (zwróć curα )

now() – chwila obecna

min

max

wait

T

T

T

δ

wartości konfigurowalne

32

punktu, od którego wiązka się odbiła. W takim przekształceniu wartościami

znanymi są:

• Długość 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) ξ

• pozycja robota ( , , )x y z

Rysunek 29. Rysunek wyjaśniający znaczenie kątów α, β, ξ

Współrzędne ( , , )k k k

x y z punktu, od którego wiązka się odbiła, obliczane są

w dwóch krokach. W pierwszym kroku obliczane są współrzędne punktu

( , , )f f f

x y z , dla którego nie jest uwzględniana orientacja oraz pozycja robota.

Przekształcenie to obrazuje Rysunek 30.

Rysunek 30. Obliczanie współrzędnych (xf, yf, zf)

W następnym kroku rzut punktu ( , , )f f f

x y z na płaszczyznę OXY jest

poddawany przekształceniom obrotu wokół osi OZ o kąt ξ oraz przesunięcia o

33

pozycję robota ( , , )x y z (patrz Rysunek 31). Wynikiem obliczeń są szukane

współrzędne ( , , )k k k

x y z punktu, na który trafiła badana wiązka.

Rysunek 31. Obliczanie współrzędnych (xk, yk, zk)

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)

• serwomechanizm (maksymalny błąd wynikający z dokładności enkodera

wynosi ~ 0.1˚)

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

3.8. Utworzenie reprezentacji sceny złożonej z trójkątów

Zadanie utworzenia reprezentacji sceny 3D złożonej z trójkątów można

sformułować jako zadanie rozpięcia spójnej siatki trójkątów na powierzchni

ograniczonej przez znalezione punkty 3D tak, aby trójkąty te jak najdokładniej

przybliżały obserwowaną powierzchnię. Ponieważ topologia mapy powinna jak

najdokładniej odzwierciedlać topologię sceny obserwowanej, trójkąty powinny

mieć wszystkie wierzchołki w znalezionych punktach 3D. Im więcej trójkątów

zostanie użytych (przy czym nie powinny się one nawzajem przecinać ani

zasłaniać), tym lepsze przybliżenie zostanie uzyskane. Podsumowując, ostateczna

reprezentacja sceny 3D powinna mieć następujące cechy:

• Jak najdokładniejsze przybliżenie obserwowanej powierzchni

• Zachowanie topologii obserwowanej powierzchni

• Trójkąty nie powinny się zasłaniać ani przecinać

• Siatka powinna być możliwie jak najbardziej spójna (minimalizacja grup

trójkątów nie posiadających wspólnych krawędzi)

34

Postulaty te można spełnić korzystając z wiedzy a priori uzyskanej podczas

przeprowadzania pomiarów. Przykładowo, nie powinno się tworzyć trójkątów z

wierzchołków, z których jeden został uzyskany ze skanu na poziomie

serwomechanizmu = 1˚, zaś drugi na wysokości 3˚, ponieważ trójkąt taki dałoby

się podzielić na dwa mniejsze (i tym samym lepiej przybliżające obserwowaną

powierzchnię) trójkąty wykorzystując wierzchołek z wysokości kątowej13 2˚. Taką

samą zasadę można zastosować do szerokości kątowej14. Innymi słowy

obserwowaną powierzchnię można podzielić siatką wysokości i szerokości

kątowych (kąty α i β), gdzie na każdym przecięciu dowolnej istniejącej szerokości i

wysokości kątowej została wyemitowana wiązka lasera, co zaowocowało

powstaniem punktu 3D. Sytuację tą przedstawia Rysunek 32. Trójkąty ze

wszystkimi wierzchołkami w znalezionych punktach powinny być tworzone

jedynie w obrębie powstałych na siatce „kwadratów”.

Rysunek 32. Fragment siatki złożonej z wysokości i szerokości kątowych wiązek lasera padających na obserwowany obiekt,

Należy zauważyć, że niektóre uzyskane punkty 3D nie będą mogły zostać

wykorzystane (wiązka lasera nie natrafiła na żadną przeszkodę lub został

przekroczony maksymalny zasięg urządzenia pomiarowego). Część krawędzi

również nigdy nie będzie mogła wejść w skład żadnego trójkąta – dotyczy to

13 Wysokość kątowa jest to kąt wychylenia serwomechanizmu od poziomu (kąt α) 14 Szerokość kątowa jest wyznaczana na podstawie numeru wiązki w skanie 2D; β ∈ (-90°, 90°)

35

krawędzi dłuższych niż pewna konfigurowalna wartość (np. 30 cm). Sytuację tą

ilustruje Rysunek 33.

Rysunek 33. Pewne krawędzie i wierzchołki nie mogą zostać wykorzystane do budowy siatki z trójkątów.

Wszystkie pozostałe krawędzie i wierzchołki wejdą w skład siatki 3D

złożonej z trójkątów. Jeżeli dla wybranego zbioru 4 wierzchołków tworzących na

siatce kwadrat wszystkie krawędzie są dostępne, do utworzenia dwóch trójkątów

wybierana jest krótsza przekątna (patrz Rysunek 33). Dla powyższych rysunków

ostateczna reprezentacja w formie siatki 3D może przyjąć poniższą postać:

Rysunek 34. Ostateczna postać siatki 3D złożonej z trójkątów.

36

Uzyskana siatka 3D spełnia wszystkie postawione powyżej wymagania. Z

pojedynczego skanu 3D przy rozdzielczości 1˚ dla α ∈ (-30˚, 90˚) oraz dla β ∈ (-90˚,

90˚), czyli z co najwyżej 21901 wierzchołków, powstaje średnio 15000 trójkątów

(wielkość wyznaczona eksperymentalnie). Samo obliczenie współrzędnych

trójkątów dla takiego zbioru punktów 3D zajmuje ułamek sekundy.

3.9. Łączenie wielu skanów 3D w jedną spójną mapę

Problem łączenia wielu skanów 3D w jedną spójną reprezentację mapy 3D

jest bardzo szerokim problemem, ja w swojej pracy zaproponowałem jedynie

sposób ominięcia tego problemu, wykorzystując do tego celu zmodyfikowany

sterownik lodo. Sterownik ten działa z poziomu serwera Player i subskrybuje

interfejsy laser, position oraz ptz, udostępniając jako interfejsy wyjściowe laser i

position, w których jest już poprawiona odometria.

Poprawianie odometrii w sterowniku lodo wiąże się z dość prostą

czynnością. Kilka razy w ciągu sekundy sterownik pobiera skan 2D z lasera i za

każdym razem porównuje go z poprzednim. Na podstawie tego porównania

sterownik wyznacza rzeczywiste przesunięcie i kąt obrotu pomiędzy punktami, w

których dane skany były przeprowadzane. Dzięki wprowadzonej modyfikacji

sterownik nie działa w momencie wykonywania skanu 3D (α ∉ (-0.2˚, 0.2˚)).

Sterownik lodo ma jednak swoje ograniczenia. Jest to przede wszystkim

konieczność poruszania się robota przy kącie wychylenia serwomechanizmu

ustawionym na 0. Dodatkowo wszelkie ewentualne błędy w estymacji

przesunięcia i kąta obrotu nie są nigdy poprawiane, co sprawia że wszystkie błędy

się kumulują. Ostatecznie, jak łatwo się można domyślić, rozwiązanie to jest dość

wymagające obliczeniowo.

Podsumowując, zastosowany przez mnie sposób ominięcia problemu

generuje poprawne wyniki, ale docelowo powinien być on zastąpiony przez

bardziej zaawansowany algorytm o mniejszej liczbie ograniczeń.

3.10. Napotkane problemy

W trakcie tworzenia opisanego powyżej rozwiązania oraz jego implementacji

na fizycznym robocie natknąłem się na poniżej opisane problemy natury

technicznej:

• Liczne błędy i niedociągnięcia w serwerze Player, np. brak możliwości

subskrybowania więcej niż jednego urządzenia korzystając z dostarczonej

37

biblioteki C oraz nieprawidłowa reprezentacja kątów w interface’ie ptz itp.

Dopiero wykorzystanie biblioteki dla C++ rozwiązało problem

subskrybowania wielu urządzeń, zaś kod interface’u ptz musiał zostać

poprawiony.

• Błędy w skryptach Python’owych w Gazebo uniemożliwiające wyświetlenie

przebiegu symulacji na ekranie. Problem ten został naprawiony dzięki

przepisaniu funkcji wyświetlającej tak, aby korzystała z biblioteki Zlib.

• Bardzo duże błędy w odometrii robota, szczególnie widoczne przy skrętach

(błędy ponad 60%). Rozwiązaniem było zastosowanie sterownika lodo.

• Duże błędy wynikające z budowy serwomechanizmu – oś obrotu

serwomechanizmu zamontowana jest na osi optycznej Sicka, nie zaś na jego

osi środka ciężkości. Dodatkowo silnik obracający laser zamontowany jest

tylko z jednej strony, z drugiej jest jedynie zawias. Cała konstrukcja nie jest

sztywna, łatwo poddaje się nawet niewielkiemu naciskowi. Wszystkie te

cechy sprawiają, że odczyty pobierane z lasera na różnych wysokościach

kątowych serwomechanizmu cechują się dużymi błędami pomiarowymi, co

widać dokładnie w rozdziale 6.1.

3.11. Możliwe kierunki rozwoju

Część projektowa powyższej pracy została napisana z myślą o przyszłych

rozszerzeniach, które poprawią jej działanie lub rozszerzą oferowaną

funkcjonalność. Są to:

• Algorytm upraszczający sceny 3D złożone z trójkątów, zachowujący topologię

sceny.

• Algorytm automatycznie wyznaczający następny punkt przeprowadzenia

pomiarów oraz kod umożliwiający przemieszczenie w to miejsce robota. Ten

krok zapewni robotowi autonomiczność.

• Algorytm służący do łącznia wielu skanów 3D w jedną spójną mapę 3D, nie

kumulujący błędów z odometrii (nawet poprawionej), potrafiący

minimalizować globalny błąd dopasowania. Powinien on umożliwiać spajanie

w jedną całość rozłącznych siatek trójkątów oraz usuwanie pokrywających się

fragmentów siatki. Umożliwi to zrezygnowanie z wymagającego

obliczeniowo sterownika lodo.

Dodatkowym kierunkiem rozwoju, który warto rozważyć jest pokrywanie

sceny złożonej z trójkątów teksturami, utworzonymi ze zdjęć wykonywanych w

38

miejscu przeprowadzania każdego skanu 3D. Zwiększyło by to bardzo czytelność

mapy i jej przydatność dla człowieka.

39

4. Wizualizacja sceny 3D złożonej z trójkątów

4.1. Wstęp do wizualizacji

Pierwotnie wizualizacja w ogóle nie była rozważana jako etap mojej pracy

dyplomowej, jednak już w trakcie analizy tematyki map 3D doszedłem do

wniosku, że bez możliwości wizualizacji wykonanie pierwszej części projektowej

byłoby niemożliwe.

Generalnie, aby mógł zachodzić proces budowy trójwymiarowej mapy

środowiska przez robota wyposażonego w dalmierz laserowy oraz aby robot

mógł pozyskaną informację przetwarzać i wykorzystywać, wizualizacja jest

całkowicie zbędna. Za to, aby w którymkolwiek momencie człowiek mógł

zobaczyć i ocenić wyniki przeprowadzonych przez robota obliczeń w postaci

sceny 3D, zaraz po jej zbudowaniu bądź już po jej odpowiednim przetworzeniu,

niezbędne staje wprowadzenie intuicyjnej metody prezentacji graficznej owej

sceny. Umożliwia to obserwatorowi ocenę jakości uzyskanych wyników oraz

ewentualne skorygowanie różnych ustawień, zarówno sprzętowych jak i

softwareowych, od których owe wyniki zależą. W fazie tworzenia

oprogramowania służącego do generowania bądź przetwarzania scen 3D,

wizualizacja staje się niezbędna do oceny poprawności zastosowanych rozwiązań

oraz porównania jakości generowanych wyników.

4.2. Zadanie projektowe

Ta część zadania projektowego polegała na stworzeniu wydajnego,

działającego w czasie rzeczywistym (interaktywnego) silnika graficznego

umożliwiającego użytkownikowi wizualizację mapy 3D złożonej z trójkątów.

4.3. Techniki wizualizacji

W nowoczesnych aplikacjach komputerowych grafika trójwymiarowa

znajduje wiele zastosowań. Istnieją dwa zasadnicze sposoby prezentacji scen 3D

przez aplikacje komputerowe:

• Generowanie grafiki w czasie rzeczywistym

• Generowanie grafiki wsadowo

Trójwymiarowa grafika czasu rzeczywistego jest animowana i pozwala na

interakcję z użytkownikiem. Jednym z pierwszych zastosowań grafiki 3D czasu

rzeczywistego były wojskowe symulatory lotu. Dzisiaj najpowszechniejszym

zastosowaniem tego trybu prezentacji grafiki 3D są gry komputerowe, choć jest on

40

wykorzystywany również w wizualizacjach naukowych, aplikacjach

inżynieryjnych oraz biznesowych oraz wszędzie tam, gdzie konieczne jest

stosowanie wydajnego i atrakcyjnego graficznego interfejsu użytkownika (ang.

GUI – Graphical User Interface). Generowanie grafiki trójwymiarowej w czasie

rzeczywistym wymaga dużych nakładów obliczeniowych w ograniczonym czasie,

dzięki czemu zmiany obserwowanej scena zachodzą płynnie. Współczesne karty

graficzne potrafią wyświetlać obiekty trójwymiarowe zbudowane z wielokątów,

wykonując dużą część obliczeń związanych z generowaniem grafiki 3D (obrót,

skalowanie, rzutowanie perspektywiczne, cieniowanie wielokątów, proste modele

oświetlenia itp.), odciążając tym samym procesor.

Aplikacje korzystające z grafiki komputerowej w czasie rzeczywistym

wymagają pewnych kompromisów. Posiadając więcej czasu na przetwarzanie

danych, można wygenerować grafikę komputerową o znacznie wyższej jakości. W

aplikacjach generujących grafikę 3D w trybie wsadowym użytkownik projektuje

modele 3D obiektów oraz złożoną z nich scenę, a następnie program służący do

śledzenia promieni (ang. ray tracer) generuje wysokiej jakości obraz

trójwymiarowy. Ta technologia znajduje zastosowanie głównie z aplikacjach typu

CAD (ang. Computer Aided Design) oraz w kinematografii do tworzenia złożonych

efektów 3D. Wygenerowanie jednej klatki w trybie wsadowym nawet bardzo

szybkim komputerom zajmuje nawet do kilku godzin, w zależności od złożoności

sceny.

Na podstawie powyższego zestawienia widać wyraźnie, że aplikacja

prezentująca mapę 3D utworzoną przez robota powinna wyświetlać grafikę w

czasie rzeczywistym, ponieważ:

• Umożliwi użytkownikowi „poruszanie się po scenie” – oglądanie jej z

różnych perspektyw

• Użytkownik będzie mógł obejrzeć wyniki natychmiast po utworzeniu przez

robota sceny, nie będzie musiał czekać dodatkowo na jej wyświetlenie

• Mapa utworzona przez robota będzie złożona z dużej ilości prostych

elementów, nie będzie potrzeby wyświetlania złożonych geometrycznie

obiektów oraz wykorzystywania czasochłonnych obliczeniowo efektów

specjalnych (odbijanie światła, rzucanie przez obiekty cieni, przezroczystość,

mgła, itp.)

W chwili obecnej istnieją dwa najbardziej rozpowszechnione zestawy

interfejsów umożliwiające generowanie grafiki 3D w czasie rzeczywistym –

OpenGL oraz DirectX. Obydwie te technologie wspierają sprzętową akcelerację

renderowania grafiki 3D.

41

4.3.1. OpenGL

OpenGL (Open Graphics Library) został zdefiniowany jako „interfejs

programowy dla sprzę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 przez firmę 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 organizację 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 aplikacja wykorzystująca OpenGL, która zleca operacje graficzne do

wykonania, a serwerem - aktualnie używana implementacja OpenGL (np. w

sterowniku karty graficznej). Zwykle klient i serwer znajdują się na tej samej

maszynie, jednak nie jest to konieczne - biblioteka zaprojektowana tak, aby

możliwe było np. wyświetlanie grafiki OpenGL na zdalnym terminalu.

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 aplikacja.

Jedną z podstawowych cech OpenGL jest to, że jest on maszyną stanów. Na

stan OpenGL w 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średni lub pośredni wpływ na otrzymany rezultat

renderingu. Raz ustawiony parametr lub tryb dział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ż na przykład funkcje pozwalające na

ustawienie obserwatora w pewnym punkcie patrzącego na inny punkt (funkcja

gluLookAt) bądź narysowanie kuli (funkcja gluSphere) realizowane są 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 i nie zawiera funkcji związanych z tworzeniem kontekstu

42

graficznego, obsługą środowiska okienkowego, czy obsługą zdarzeń takich, jak

naciśnięcie klawisza na klawiaturze lub ruch kursora myszy. Funkcjonalność tę

można dodać poprzez biblioteki takie jak GLUT (GL Utility Toolkit), GLUI, czy

dedykowane dla poszczególnych platform GLX (w przypadku środowiska X

Window System) lub WGL (w przypadku Windows).

4.3.2. DirectX

DirectX to zestaw funkcji API wspomagających generowanie grafiki (dwu i

trójwymiarowej), dźwięku oraz innych zadań związanych zwykle z grami i

innymi aplikacjami multimedialnymi. Komponentem DirectX odpowiedzialnym

za generowanie grafiki 3D jest Direct3D. 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 systemach operacyjnych zgodnych z Windows. Akceleracja

sprzętowa, oferowana przez większość współczesnych kart graficznych oraz

bogaty zbiór dostępnych narzędzi umożliwia programowanie efektownej grafiki

3D. Direct3D dostarcza programiście gotowych interfejsów, uwalniając go

jednocześnie od konieczności zaznajamiania się ze wewnętrznymi funkcjami

sprzętu.

Pierwotnie struktura Direct3D różniła się od standardu OpenGL, ale z

biegiem czasu jej budowa coraz bardziej zaczynała go przypominać. Direct3D jak i

cały pakiet DirectX jest oparty na interfejsach COM, co oznacza, że programista

musi tworzyć wskaźniki do klas i potem z nich korzystać. Jest to dość zawiła

operacja, która dodatkowo zwiększa nieczytelność kodu programu. Microsoft

odchodzi od modelu COM i chce przyjąć rozwiązania zawarte 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 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ła tylko pod kontrolą tego systemu, co jest jego największą wadą.

43

4.4. Implementacja silnika 3D

4.4.1. Wykorzystana technologia

W rozdziale 4.3 uzasadniłem wybór sposobu generowania grafiki 3D

(generowanie w czasie rzeczywistym). Pozostał zatem wybór konkretnej

implementacji – OpenGL lub Direct3D.

Przy wyborze technologii do zbudowania interaktywnego silnika 3D

kierowałem się kilkoma względami: przede wszystkim wykorzystana technologia

powinna być wydajna, przenośna oraz mało skomplikowana. Zarówno Direct3D,

jak i OpenGL oferują zbliżone możliwości i wydajność, jednakże OpenGL jest

biblioteką wysoce przenośną – istnieją jej implementacje dział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żo

przejrzystszy od kodu pisanego w Direct3D – 10 linijek kodu w OpenGL

odpowiada około 2-3 stronom kodu w Direct3D. Biorąc pod uwagę powyższe

argumenty, zdecydowałem się na wybór biblioteki OpenGL do napisania kodu

silnika 3D.

Dodatkowym argumentem przemawiającym za tym wyborem była

dostępność narzędzi i bibliotek rozszerzających podstawową funkcjonalność

OpenGLa, takich jak np. biblioteka GLUT [34].

4.4.2. Środowisko programistyczne

Program silnika 3D został napisany w języku C z wykorzystaniem bibliotek

OpenGL i GLUT oraz innych standardowych bibliotek, takich jak studio.h, math.h

oraz string.h. Język C wybrałem ze względu na szybkość działania oraz na

przejrzystość kodu. W przyszłości, jeżeli zajdzie taka potrzeba, będzie można

niewielkim nakładem pracy przepisać program na język C++.

Silnik pisałem w systemie operacyjnym Gentoo Linux korzystając z

linuksowych implementacji bibliotek OpenGL oraz GLUT, jednakże dzięki

wysokiej przenośności tych bibliotek oraz standardowych bibliotek języka C, kod

bez większych zmian można będzie skompilować w dowolnym innym systemie

operacyjnym.

Kompilacja oraz konsolidacja wszystkich niezbędnych plików odbywa się za

pomocą polecenia make.

44

4.4.3. Schemat działania silnika

Schemat działania silnika 3D obrazuje rysunek poniżej:

Rysunek 35. Schemat działania silnika graficznego 3D.

Model sceny jest w rzeczywistości dynamiczną listą trójkątów. Dla każdego z

wczytanych trójkątów należy obliczyć wektor normalny. Polega to na obliczeniu

wektora o długości jednostkowej, prostopadłego do każdego z wczytanych

trójkątów. Wektory te są następnie wykorzystywane do określenia, pod jakim

kątem światło z danego źródła oświetlenia pada na obiekt (jak silnie obiekt

zostanie oświetlony).

Dokładna implementacja głównej pętli przetwarzania i renderowania

biblioteki GLUT jest ukryta przed programistą. Programista w fazie inicjalizacji

deklaruje, które funkcje będą odpowiedzialne za które zdarzenia, przykładowo:

• Zdarzenia polegające na przyciśnięciu klawisza klawiatury zostały

przydzielone funkcji processNormalKeys.

• Zdarzenia polegające na przyciśnięciu i przytrzymaniu klawisza klawiatury

zostały przydzielone funkcji pressKey.

• Zdarzenia polegające na puszczeniu klawisza klawiatury zostały przydzielone

funkcji releaseKey.

• Zdarzenia polegające na poruszaniu kursora myszki zostały przydzielone

funkcji processMousePassiveMotion.

Wczytanie modelu sceny z pliku, obliczenie dla każdego trójkąta jego wektora normalnego

START

Inicjalizacja sceny: • ustawienie oświetlenia • ustawienie modelu cieniowania • deklaracja funkcji odpowiedzialnych za

obsługę klawiatury i myszki • deklaracja funkcji renderującej obraz • ukrycie kursora myszki

Uruchomienie głównej pętli przetwarzania i renderowania biblioteki GLUT. STOP

wciśnięto ESC

45

Spora część możliwości oferowanych przez bibliotekę GLUT nie została

wykorzystana, przykładowo nie zostały zdefiniowane funkcje obsługujące

zdarzenia takie jak: wciśnięcie lub puszczenie przycisku myszki, obsługa

specjalnych klawiszy klawiatury (takich jak F1), itp.

Schemat blokowy głównej pętli przetwarzania i renderowania obrazu

przedstawia Rysunek 36.

Rysunek 36. Schemat blokowy głównej pętli przetwarzania i renderowania obrazu.

START

Renderuj obraz Klawiatura

Myszka

Nie wciśnięto przycisku

STOP wciśnięto ESC

Aktualizuj zmienne określające obecne

położenie obserwatora oraz zmienne

określające szybkość poruszania się i obrotu

wciśnięto jeden z ↑ ↓ ← → [ ] , .

Nie poruszono kursorem

Aktualizuj zmienne określające kierunek

patrzenia obserwatora

Poruszono kursorem

Kursor blisko krawędzi okna

NIE

Przesuń kursor na pozycję środkowego

punktu okna

TAK

46

4.4.4. Widok sceny

Obserwator widzi scenę z perspektywy pierwszej osoby – jest to sposób

prezentacji otoczenia często stosowany w grach komputerowych FPP (ang. FPP –

First Person Perspective).

Zastosowany widok sceny jest bardzo intuicyjny, dodatkowe przydatne

informacje wyświetlane są w górnym obszarze okna prezentacji. Są to:

• Obecne położenie obserwatora w przestrzeni (X, Y, Z)

• Kąty, pod którymi obserwator ogląda scenę

• Obecne ustawienia dotyczące prędkości poruszania się i obracania

• Szybkość generowania sceny w klatach na sekundę (ang. FPS – Frames Per

Second)

Rysunek 37. Przykładowa scena wyświetlona przez program FPP3DMap.

47

4.4.5. Praca interaktywna z programem FPP3DMap.

Generowanie grafiki 3D w czasie rzeczywistym umożliwia nadanie aplikacji

cech interakcyjności. Zdecydowałem się na wprowadzenie tego elementu,

ponieważ interakcyjność bardzo silnie zwiększa użyteczność całego silnika, w

szczególności w połączeniu z widokiem FPP. Obserwator, używając klawiatury

może dowolnie poruszać się po scenie oraz obracać się na wszystkie strony przy

pomocy myszki. Prędkość poruszania się oraz prędkość obrotu są wielkościami

regulowanymi. Do poruszania się po scenie służą następujące klawisze:

• ↑ - idź przed siebie

• ↓ - idź w tył

• ← - idź w lewo

• → - idź w prawo

• Myszka w lewo – obrót w lewo

• Myszka w prawo – obrót w prawo

• Myszka do góry – spojrzenie w górę

• Myszka do dołu – spojrzenie w dół

• [ - zmniejsz prędkość obrotu

• ] – zwiększ prędkość obrotu

• , - zmniejsz prędkość poruszania

• . – zwiększ prędkość poruszania

• Esc – wyjście z programu

Podczas działania silnika graficznego kursor myszy jest ukryty,

zastosowałem także metodę, dzięki której użytkownik nie „wyjedzie” kursorem

myszy poza obszar okna, w którym obserwowana jest scena.

4.4.6. Oświetlenie sceny

Odpowiednie oświetlenie oraz cieniowanie sceny jest konieczne, aby

wygenerowany obraz był czytelny. Biblioteka OpenGL posiada rozbudowaną

funkcjonalność zapewniającą dobre odwzorowanie rzeczywistego wyglądu

obiektów pod względem warunków oświetlenia. Każdy obiekt może być

oświetlany przy pomocy maksymalnie ośmiu różnych źródeł światła, z czego

każde jest mieszanką trzech rodzajów światła [35]:

• Światło otaczające (ang. ambient) – nie pochodzące z żadnego konkretnego

kierunku. Wszystkie obiekty oświetlone są równomiernie, niezależnie od ich

położenia i kąta patrzenia obserwatora.

48

• Światło rozproszone (ang. diffuse) – pada na obiekt z określonego kierunku,

ale jest na nim rozpraszane we wszystkich kierunkach. Mimo to powierzchnie

obiektu oświetlone bezpośrednio będą jaśniejsze od powierzchni ustawionych

w stosunku do kierunku padania światła pod pewnym kątem.

• Światło odbite (ang. specular) – pada na obiekt z określonego kierunku i jest

odbijane w jednym konkretnym kierunku. Mocno odbite światło najczęściej

powoduje wystąpienie na powierzchni oświetlanej jasnej plamy nazywanej

odbłyskiem (ang. specular highlight).

Przeprowadziłem wiele prób z różnymi liczbami źródeł światła,

oświetlających scenę różnymi rodzajami światła. Najlepsze efekty uzyskałem przy

wykorzystaniu 3 źródeł światła, gdy każde składało się ze światła otaczającego i

rozproszonego. Przykładowe rezultaty dla różnych ustawień oświetlenia ukazuje

Rysunek 38.

Rysunek 38. Rysunek ukazujący wpływ oświetlenia na widok mapy 3D:

a) Tylko światło otaczające b) Jedno źródło światła, częściowo otaczające, częściowo rozproszone c) Dwa źródła światła, częściowo otaczające, częściowo rozproszone d) Trzy źródła światła, częściowo otaczające, częściowo rozproszone

49

Ostateczna jasność każdego trójkąta zależy od jego ustawienia względem

wszystkich źródeł światła. Ustawienie to jest przez OpenGL uwzględniane przy

pomocy wektorów normalnych.

Próba użycia zbyt małej ilości świateł (albo ich nieprawidłowe

rozmieszczenie) sprawia, że trójkąty są nierozróżnialne, co z kolei powoduje

nieczytelność mapy 3D.

4.4.7. Wkład własny

W literaturze można znaleźć bardzo dużo poradników i artykułów na temat

wykorzystania możliwości OpenGL. Należy jednak zauważyć, że specyficzna

konstrukcja mapy 3D zbudowanej przez robota mobilnego sprawia, że artykuły te

najczęściej nie znajdują w robotyce zastosowania. Nie wykorzystuję tu żadnych

efektów, takich jak przezroczystość, czy wygładzanie krawędzi, stawiam za to na

czytelność oraz wysoką wydajność silnika przy dużej złożoności sceny (stąd

analiza oświetlenia). Dodatkowo postanowiłem zaimplementować

interaktywność, umożliwiając użytkownikowi poruszanie się po scenie.

Podsumowując, opisany powyżej silnik graficzny jest rozwiązaniem

skonstruowanym pod kątem specyfiki map 3D budowanych przez roboty

mobilne.

4.4.8. Możliwe kierunki rozwoju

Opisany powyżej silnik graficzny sprawuje się poprawnie nawet przy

wyświetlaniu bardzo dużej ilości trójkątów w jednym momencie (dokładna

analiza wydajności znajduje się w rozdziale 0). Da się jednak tę wydajność

zwiększyć poprzez zastosowanie trójkątów sklejanych (ang. triangle strips). W

chwili obecnej każdy trójkąt przechowywany jest jako zbiór trzech wierzchołków.

Przy zastosowaniu trójkątów sklejanych każdy trójkąt definiowany jest przez

tylko jeden wierzchołek (poza pierwszym ze zbioru). Rysunek 39 przedstawia

kolejne etapy rysowania trzech sklejonych trójkątów, określanych przez definicje

pięciu wierzchołków ponumerowanych od v0 do v4.

50

Rysunek 39. Kolejne etapy rysowania trzech sklejonych trójkątów.

Używanie trójkątów sklejonych wiąże się z dwoma zaletami w porównaniu

do definiowania każdego trójkąta z osobna. Po pierwsze jest to oszczędność

miejsca przeznaczonego na przechowywanie 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łaniu danych (mniej

wierzchołków oznacza szybsze przesłanie danych z pamięci komputera do karty

graficznej, a także mniej przekształceń wierzchołków). Jedyną wadą tego

podejścia jest konieczność konwersji sceny złożonej z trójkątów zwykłych na scenę

złożoną z trójkątów sklejanych. Polega to na obliczeniu jak najmniejszej ilości

zbiorów trójkątów sklejonych z pierwotnego zbioru trójkątów tak, aby każdy

trójkąt ze zbioru pierwotnego wchodził w skład dokładniej jednego zbioru

trójkątów sklejanych. Jest to dość skomplikowany i złożony obliczeniowo

problem.

Innym kierunkiem rozwoju jest umożliwienie wyświetlania sceny złożonej z

trójkątów pokrytymi teksturami Byłoby to konieczne do umożliwienia

wizualizacji scen 3D wykonanych przez rozszerzony o opcję teksturowania

program Build3DMap.

51

5. Obsługa programów

5.1. Konfiguracja programu Build3DMap

Wszystkie opcje konfiguracyjne programu Build3DMap znajdują się w pliku

nagłówkowym global_config.h. Przykładowa konfiguracja:

//Player specific

#define PLAYER_PORT 6665

#define PLAYER_HOST "pcm1"

//Laser specific

#define STEP_LASER_ANGLE 1 //in degrees

#define LASER_TRUTH_Z_OFFSET 50 //in milimeters (value always added to laser's Z

truth position)

#define LASER_MAX_DISTANCE 8000 //in milimeters (used for points invalidation)

#define USE_INVALID_POINTS 0 //use points which exceeded laser max_distance

//PTZ specific

#define MIN_TILT_ANGLE -10 //in degrees

#define MAX_TILT_ANGLE 90 //in degrees

#define STEP_TILT_ANGLE 1 //in degrees

#define TILT_ANGLE_POLL_MS 100 //in miliseconds

#define TILT_ANGLE_WAIT_TIMEOUT 60000 //in miliseconds

#define TILT_ANGLE_WAIT_MIN_TIME 1000 //in miliseconds

#define TILT_ANGLE_ACCEPTABLE_ERROR 0.2 //in degrees

//Polygon output

#define MAX_EDGE_LENGTH 300 //in milimeters

//Logs

#define WRITE_LOGS_TO_FILE 1

#define WRITE_LOGS_TO_STDOUT 1

#define LOG_LEVEL_FILE 5 //4-debug, 3-warnings, 2-errors, 1-fatal, 0-info

#define LOG_LEVEL_STDOUT 4 //4-debug, 3-warnings, 2-errors, 1-fatal, 0-info

#define LOG_FILE "simulator.log"

Opcje „player specific” określają, na jakiej maszynie (nazwa maszyny oraz

numer portu) działa serwer Player, podłączony do robota bądź symulatora.

Opcje „laser specific” określają rozdzielczość lasera (STEP_LASER_ANGLE)

określoną w odległości kątowej pomiędzy kolejnymi wiązkami, wysokość

elementu optycznego lasera nad ziemią (LASER_TRUTH_Z_OFFSET) oraz

maksymalny zasięg urządzenia (LASER_MAX_DISTANCE). Opcja

USE_INVALID_POINTS określa, czy punkty uzyskane z wiązek o długości

przekraczającej maksymalny zasięg urządzenia mają być brane pod uwagę

podczas budowy mapy, czy nie.

Opcje „PTZ specific” określają minimalny minα oraz maksymalny maxα kąt

wychylenia serwomechanizmu od poziomu (MIN_TILT_ANGLE i

MAX_TILT_ANGLE), wartość kroku stepα (STEP_TILT_ANGLE), jak również

52

minimalny minT i maksymalny maxT czas oczekiwania na osiągnięcie zadanego kąta

(TILT_ANGLE_WAIT_MIN_TIME i TILT_ANGLE_WAIT_TIMEOUT),

maksymalny dopuszczalny błąd ustawienia serwomechanizmu δ

(TILT_ANGLE_ACCEPTABLE_ERROR) oraz częstotliwość odczytywania

pomiarów z enkodera serwomechanizmu waitT (TILT_ANGLE_POLL_MS).

Opcja MAX_EDGE_LENGTH definiuje maksymalną dopuszczalną długość

krawędzi trójkąta.

Opcje typu „logs” określają, jakie informacje mają być logowane oraz w jaki

sposób. Informacje mogą być wyświetlane na standardowym wyjściu

(WRITE_LOGS_TO_STDOUT), do pliku (WRITE_LOGS_TO_FILE) o określonej

nazwie (LOG_FILE), bądź w obydwa te miejsca. Można również zdefiniować,

jakiego typu informacje mają być wyświetlane (LOG_LEVEL_STDOUT) i

zachowywane (LOG_LEVEL_FILE). Zdefiniowałem pięć poziomów logowania, w

zależności od wagi komunikatów:

• S-DEBUG (5) – najbardziej szczegółowy poziom. Wszystkie, nawet bardzo

mało znaczące komunikaty będą logowane.

• DEBUG (4) – Wszystkie komunikaty będą logowane, z pominięciem tych

najmniej znaczących.

• WARNING (3) – Logowane będą tylko ostrzeżenia i błędy.

• ERROR (2) – Logowane będą tylko błędy.

• FATAL (1) – Logowane będą tylko błędy, po których wystąpieniu program

musi zakończyć pracę.

• INFO (0) – Logowane będą jedynie najistotniejsze informacje dotyczące stanu

progamu.

Każdorazowo po zmianie jednego z paramentów konfiguracyjnych należy

program przekompilować wykonując polecenie make.

5.2. Uruchamianie programu Build3DMap na symulatorze

Do poprawnego działania programu Build3DMap na symulatorze potrzebne

są skompilowane z poprawionych źródeł program Gazebo oraz serwer Player

(poprawione źródła są w katalogu patched_source). Do poprawnej kompilacji

wymagają one między innymi biblioteki GDAL, silnika ODE (Open Dynamics

Engine), Pythona z modułem wxpython oraz biblioteki GLUT.

53

Proces uruchamiania składa się z następujących kroków:

1. Uruchomienie środowiska symulacyjnego

W katalogu Protonek znajdują się skrypty run, wxrun oraz wxrun2.

Każdy z nich uruchamia środowisko Gazebo z jednym z dwóch

dostarczonych plików konfiguracyjnych symulatora.

2. Uruchomienie serwera Player

W katalogu Protonek znajduje się skrypt player_run_gazebo. Uruchamia

on serwer Player z dostarczonym plikiem konfiguracyjnym (dzięki któremu

serwer podłącza się do symulatora Gazebo).

3. Uruchomienie aplikacji Build3DMap

Aplikacja Build3DMap znajduje się w katalogu Simulate. Do

uruchomienia aplikacji służy polecenie ./Build3DMap.

5.3. Uruchamianie programu Build3DMap na fizycznym robocie

Do poprawnego działania programu Build3DMap na fizycznym robocie

potrzebny jest skompilowany z poprawionych źródeł serwer Player (poprawione

źródła są w katalogu patched_source).

Proces uruchamiania składa się z następujących kroków:

1. Uruchomienie robota

2. Uruchomienie serwera Player na robocie

W katalogu Protonek znajduje się skrypt player_run_robot. Uruchamia

on instancję serwera Player służącą do komunikacji z czujnikami oraz

efektorami robota.

3. Uruchomienie serwera Player na komputerze stacjonarnym

W katalogu Protonek znajduje się skrypt player_run_cs. Uruchamia on

instancję serwera Player służącą do komunikacji z instancją działającą na

robocie. Na komputerze stacjonarnym uruchomiony jest sterownik lodo,

dzięki czemu nie obciąża on procesora robota.

4. Uruchomienie aplikacji Build3DMap

Aplikacja Build3DMap znajduje się w katalogu Simulate. Do

uruchomienia aplikacji służy polecenie ./Build3DMap.

54

5.4. Korzystanie z programu Build3DMap

Po uruchomieniu aplikacja Build3DMap subskrybuje wszystkie wymagane

interfejsy a następnie wchodzi w stan oczekiwania na ustawienie robota w

odpowiedniej pozycji do przeprowadzenia skanu 3D: Enter - get scan. Anything else and enter - exit program.

Your command:

W momencie, gdy pożądana pozycja zostanie osiągnięta, należy wcisnąć

Enter. Po przeprowadzeniu skanu 3D aplikacja ponownie wchodzi w stan

oczekiwania na osiągnięcie kolejnej pozycji robota.

Po przeprowadzeniu wszystkich pomiarów należy wcisnąć dowolny

klawisz, a następnie Enter. Spowoduje to zapisanie wyników działania programu

do dwóch plików wyjściowych oraz zakończenie programu. Pliki wyjściowe to:

• scena.m – plik zawierający zbudowaną mapę 3D złożoną ze wszystkich

przeprowadzonych skanów.

• scena.mm – plik zawierający wszystkie punkty 3D uzyskane za pomocą

dalmierza laserowego, zanim na ich podstawie została zbudowana mapa.

Przykładową zawartość plików scena.m i scena.mm ukazuje Rysunek 40.

Rysunek 40. Przykładowa zawartość plików wyjściowych programu Build3DMap, wyświetlonych przez program FPP3DMap: a) scena.m b) scena.mm

55

5.5. Uruchamianie programu FPP3DMap

Do prawidłowego działania program ten wymaga biblioteki GLUT,

komponentu GLX (rozszerzenie odpowiedzialne za rendering OpenGL w

środowisku X-Windows) oraz sprzętowego wsparcia do generowania grafiki 3D

w czasie rzeczywistym. Jeżeli po wykonaniu polecenia glxinfo jedna z linijek

będzie następująca:

direct rendering: Yes

to znaczy, że wszystkie wymagania zostały spełnione.

Program uruchamiany jest z jednym parametrem – nazwą pliku, z którego

ma być odczytany model środowiska 3D:

./FPP_3Dmap scena.m

Plik zawierający model sceny 3D powinien mieć następującą składnię:

beginmodel 83600

2.000000, 2.200000, 1.500000, 2.045455, 2.200000, 1.500000, 2.000000, 2.156000, 1.500000

2.000000, 2.156000, 1.500000, 2.045455, 2.200000, 1.500000, 2.058918, 2.151842, 1.511286

2.000000, 2.156000, 1.500000, 2.058918, 2.151842, 1.511286, 2.000000, 2.112000, 1.500000

...

Pierwsza linijka pliku mówi, z ilu trójkątów będzie się składać scena, każda

następna linijka określa współrzędne pojedynczego trójkąta – współrzędne X, Y, Z

każdego z trzech wierzchołków.

Przechowywanie modelu sceny w pliku w postaci zwykłego tekstu

umożliwia przeprowadzanie dowolnych transformacji sceny (np. łączenie w jedną

scenę kilku, upraszczanie poprzez zmniejszanie ilości trójkątów itp.) w zależności

od potrzeb użytkownika, a dopiero późniejsze jej wyświetlanie.

56

6. Wyniki eksperymentalne

6.1. Analiza wyników działania programu Build3DMap

Do oceny wyników działania programu Build3DMap posłużyłem się trzema

testami. Pierwszy test polegał na utworzeniu mapy 3D wybranego fragmentu

piwnicy w gmachu Elektroniki Politechniki Warszawskiej. Oceniane były: jakość i

złożoność powstałej mapy oraz wpływ sterownika lodo na dopasowanie

poszczególnych skanów 3D w spójną mapę. Drugi test polegał na ocenie wpływu

parametrów rozdzielczości dalmierza laserowego i serwomechanizmu na

złożoność i jakość sceny. W trzecim teście sprawdziłem wpływ parametru

maksymalnej długości krawędzi trójkąta na jakość sceny.

6.1.1. Test 1 – budowanie mapy 3D

W poniższym teście zostało wykonanych 6 skanów 3D, połączonych

następnie w jedną mapę 3D fragmentu piwnicy gmachu Elektroniki PW. Skany

były wykonywane przy następujących ustawieniach:

• Maksymalny zasięg dalmierza 8 metrów

• Rozdzielczość lasera 1˚ (181 próbek pobranych w każdym skanie 2D)

• Rozdzielczość serwomechanizmu 1˚

• Minimalne wychylenie serwomechanizmu –13˚

• Maksymalne wychylenie serwomechanizmu 90˚

• Maksymalna długość krawędzi trójkąta 30 cm

Przy powyższych ustawieniach pobieranie próbek trwało 9 minut i 18

sekund (93 sekundy dla każdego z 6 skanów 3D). Łącznie zostało zebranych

111814 punktów 3D, na podstawie których zostało wygenerowane 206369

trójkątów.

Poniższe rysunki obrazują fragmenty uzyskanej mapy 3D. Obok każdego

fragmentu mapy znajduje się zdjęcie przedstawiające badany obszar.

57

Rysunek 41. Fragment 1 utworzonej mapy 3D.

Rysunek 42. Zdjęcie obszaru przedstawionego na powyższym skanie.

58

Rysunek 43. Fragment 2 utworzonej mapy 3D.

Rysunek 44. Zdjęcie obszaru przedstawionego na powyższym skanie.

59

Rysunek 45. Fragment 3 utworzonej mapy 3D.

Rysunek 46. Zdjęcie obszaru przedstawionego na powyższym skanie.

60

Rysunek 47. Fragment 3 utworzonej mapy 3D,

Rysunek 48. Zdjęcie obszaru przedstawionego na powyższym skanie.

61

Rysunek 49. Widok z góry na zbudowaną mapę, złożoną z 6 skanów 3D.

Jak widać na załączonych powyżej rysunkach, algorytm budowania mapy

bardzo dobrze sobie radzi z mapowaniem powierzchni płaskich, takich jak ściany,

czy sufity. Dużo gorzej radzi sobie z podłogą, gdyż minimalnym kątem

wychylenia serwomechanizmu jest –13˚. Sprawia to, że przy obecnej konstrukcji

(wysokość elementu optycznego dalmierza nad ziemią wynosi 33 cm) robot

„widzi” podłogę dopiero około 1,5 metra przed sobą. Co więcej, nie cały fragment

podłogi, będący w zasiągu robota jest dodawany do mapy, ponieważ istnieje

ograniczenie na maksymalną długość krawędzi dodawanych trójkątów. Przy

ustawieniu tego parametru na 30 cm przy każdym skanie 3D do mapy dodawane

jest około 1 metra podłogi (w odległości od około 1,5 do 2,5 metra od miejsca

przeprowadzania skanu). Wynika z tego, że przy użytych ustawieniach robot

musiałby zatrzymywać się co 1 metr i wykonywać dość czasochłonny skan 3D,

aby podłoga na ostatecznej mapie 3D była spójna (bez przerw). Problem ten

można rozwiązać na kilka sposobów:

• Zwiększenie parametru maksymalnej długości krawędzi trójkąta.

Dokładniejsza analiza wpływu tego parametru na jakość wynikowej mapy 3D

znajduje się w rozdziale 6.1.3.

62

• Zwiększenie rozdzielczości serwomechanizmu (zmniejszenie stepα ). Wpływ

rozdzielczości serwomechanizmu na wielkość dodanego do mapy fragmentu

podłogi obrazuje następująca tabela (przy założeniu, że maksymalna długość

krawędzi trójkąta = 30 cm):

stepα minα maxα Dodany do mapy fragment podłogi

1˚ -13˚ -8˚ Od 1,44 do 2,36 m. od miejsca przeprowadzenia skanu

0,5˚ -13˚ -5,5˚ Od 1,44 do 3,45 m. od miejsca przeprowadzenia skanu

0,2˚ -13˚ -3,6˚ Od 1,44 do 5,28 m. od miejsca przeprowadzenia skanu

0,1˚ -13˚ -2,5˚ Od 1,44 do 7,60 m. od miejsca przeprowadzenia skanu

Tabela 1. Tabela ukazująca wpływ rozdzielczości serwomechanizmu na dodany do mapy 3D fragment podłogi.

Powyższe wartości zostały uzyskane z prostych obliczeń geometrycznych.

W rzeczywistości serwomechanizm nigdy nie ustawia się z dowolną

dokładnością na zadany mu kąt, co wynika z ograniczonej dokładności

enkodera, zatem dodane do mapy 3D fragmenty podłogi będą nieco mniejsze,

niż by to wynikało z powyższej tabeli.

• Poprawienie konstrukcji serwomechanizmu, umożliwiając osiągnięcie kątów

mniejszych niż -13˚. Przykładowo, możliwość wychylenia serwomechanizmu

do -30˚ umożliwiłaby robotowi zeskanowanie podłogi w odległości 60 cm od

robota.

Sterownik lodo bardzo skutecznie poprawia odometrię robota i zapewnia

dość dobrą spójność skanów 3D, co obrazuje Rysunek 49. Należy jednak pamiętać,

że mapa 3D została złożona z niewielkiej ilości skanów 3D. Jest prawdopodobne,

że przy większej ich ilości błędy zaczęłyby być wyraźnie widoczne.

Powstała mapa składa się z ponad 200 tysięcy trójkątów, mimo iż

zmapowany został niewielki obszar. Przy tak dużym wolumenie bardzo wyraźna

staje się potrzeba implementacji algorytmu służącego do upraszczania scen 3D, w

szczególności jeżeli mapowane będą większe obszary, albo jeżeli mapy będą

budowane w wyższych rozdzielczościach.

63

6.1.2. Test 2 – wpływ rozdzielczości serwomechanizmu i lasera na złożoność i

jakość sceny 3D

W poniższym teście zostały wykonane 4 skany 3D, przy różnych

ustawieniach rozdzielczości serwomechanizmu i lasera, za każdym razem w tym

samym miejscu. Porównywane były jakość przybliżenia obserwowanej sceny (na

podstawie porównania ze zdjęciem), złożoność sceny wynikowej oraz czas

niezbędny do jej uzyskania. Skany były wykonywane przy następujących

ustawieniach:

• Maksymalny zasięg dalmierza 8 metrów

• Minimalne wychylenie serwomechanizmu –13˚

• Maksymalne wychylenie serwomechanizmu 90˚

• Maksymalna długość krawędzi trójkąta 30 cm

Rozdzielczość serwomechanizmu ( stepα ) określa różnicę kątową pomiędzy 2

kolejnymi ustawieniami serwomechanizmu. Testowane były ustawienia 1˚, 0,5˚,

0,2˚ i 0,1˚. Należy zauważyć, że dla wartości 0,2˚ i 0,1˚ może się zdarzyć sytuacja,

że dla dwóch kolejnych wartości zadanych serwomechanizm nie zmieni swojej

pozycji. Przykładowo, dla wartości zadanych 0,4˚ i 0,5˚ serwomechanizm może

ustawić się na 0,45˚ i w obydwu przypadkach będzie to poprawna wartość

(wielkość maksymalnego dopuszczalnego błędu δ ustawiłem na 0,15˚).

Rozdzielczość lasera określa liczbę pobranych próbek podczas jednego skanu

2D. Przy rozdzielczości 1˚ jest to 181 próbek, zaś przy 0,5˚ jest to 361 próbek.

Najwyższa możliwa rozdzielczość lasera 0,25˚ jest rozdzielczością „sztuczną”,

uzyskiwaną poprzez interpolację, w związku w czym nie uwzględniłem jej w

poniższym zestawieniu (Tabela 2).

stepα l. próbek / skan 2D Czas skanu 3D [s] l. punktów 3D l. trójkątów

1˚ 181 1 min 33 sec 18618 34064

0,5˚ 361 2 min 44 sec 73903 142635

0,2˚ 361 6 min 20 sec 182851 357016

0,1˚ 361 12 min 38 sec 368078 716959

Tabela 2. Tabela obrazująca uzyskane wyniki dla skanów wykonywanych przy różnych rozdzielczościach serwomechanizmu oraz lasera.

64

Rysunek 50. Skan 3D uzyskany przy rozdzielczości serwomechanizmu i lasera 1°.

Rysunek 51. Zdjęcie obszaru przedstawionego na powyższym skanie.

65

Rysunek 52. Skan 3D uzyskany przy rozdzielczości serwomechanizmu i lasera 1°.

Rysunek 53. Zdjęcie obszaru przedstawionego na powyższym skanie.

66

Rysunek 54. Skan 3D uzyskany przy rozdzielczości serwomechanizmu i lasera 0,5°.

Rysunek 55. Zdjęcie obszaru przedstawionego na powyższym skanie.

67

Rysunek 56. Skan 3D uzyskany przy rozdzielczości serwomechanizmu i lasera 0,5°.

Rysunek 57. Zdjęcie obszaru przedstawionego na powyższym skanie.

68

Rysunek 58. Skan 3D uzyskany przy rozdzielczościach serwomechanizmu 0,2°i lasera 0,5°.

Rysunek 59. Zdjęcie obszaru przedstawionego na powyższym skanie.

69

Rysunek 60. Skan 3D uzyskany przy rozdzielczościach serwomechanizmu 0,2°i lasera 0,5°.

Rysunek 61. Zdjęcie obszaru przedstawionego na powyższym skanie.

70

Rysunek 62. Skan 3D uzyskany przy rozdzielczościach serwomechanizmu 0,1°i lasera 0,5°.

Rysunek 63. Zdjęcie obszaru przedstawionego na powyższym skanie.

71

Rysunek 64. Skan 3D uzyskany przy rozdzielczościach serwomechanizmu 0,1°i lasera 0,5°.

Rysunek 65. Zdjęcie obszaru przedstawionego na powyższym skanie.

72

Porównując powyższe wyniki można zauważyć, że jednostopniowa

rozdzielczość zarówno lasera jak i serwomechanizmu jest wystarczająca do

uzyskania sceny 3D, na której widoczna będzie topologia pomieszczeń, korytarzy

itp., zaś niewielkie obiekty, takie jak klamki czy rury, mogą być niewidoczne bądź

niewyraźne. Zaletą stosowania takich ustawień jest niska w porównaniu z innymi

złożoność sceny 3D (34064 trójkąty w porównaniu do 142635 – 716959 trójkątów)

oraz czas jej tworzenia (93 sekundy w porównaniu do 164 – 758 sekund).

Z drugiej strony, jeżeli priorytetem jest dokładność odwzorowania detali

obserwowanego obszaru na wyjściowej scenie 3D, najlepszym rozwiązaniem jest

stosowanie rozdzielczości półstopniowej, zarówno w przypadku lasera, jak i

serwomechanizmu. Otrzymane przy takich ustawieniach sceny 3D nie ustępują

jakością odwzorowania otoczenia scenom uzyskanym przy większej

rozdzielczości serwomechanizmu, za to cechują się dużo niższą złożonością, a ich

akwizycja jest mniej czasochłonna.

Wykorzystywanie rozdzielczości serwomechanizmu mniejszych niż 0,5˚

przy akwizycji skanów 3D nie przynosi żadnych wymiernych efektów. Korzyści

płynące z bardzo dokładnego odwzorowania obserwowanej sceny są nieznaczne

w porównaniu do problemów związanymi z bardzo dużymi wymaganiami

obliczeniowymi związanymi z przetwarzaniem i wyświetlaniem ogromnej liczby

wygenerowanych trójkątów.

73

6.1.3. Test 3 – wpływ maksymalnej długości krawędzi generowanych trójkątów

na złożoność i jakość sceny 3D

W poniższym teście zostały wykonane 3 skany 3D, przy różnych

ustawieniach maksymalnej długości krawędzi generowanych trójkątów.

Wszystkie skany były wykonywane w tym samym miejscu. Oceniane były jakość

przybliżenia obserwowanej sceny (na podstawie porównania ze zdjęciem) oraz

złożoność sceny wynikowej. Skany były wykonywane przy następujących

ustawieniach:

• Maksymalny zasięg dalmierza 8 metrów

• Minimalne wychylenie serwomechanizmu –13˚

• Maksymalne wychylenie serwomechanizmu 90˚

• Rozdzielczość lasera 0,5˚ (361 próbek pobranych w każdym skanie 2D)

• Rozdzielczość serwomechanizmu 0,5˚

Max. długość krawędzi [mm] l. punktów 3D l. trójkątów

70 74726 130696

300 74726 146404

600 74726 147287

Tabela 3. Tabela przedstawiająca złożoność sceny 3D w zależności od parametru maksymalnej długości krawędzi trójkąta.

Jak widać na powyższym maksymalna długość boków trójkątów ma wpływ

na złożoność wygenerowanej sceny. Zgodnie z oczekiwaniami, wraz ze wzrostem

badanego parametru rośnie też złożoność sceny wyjściowej. Maksymalną,

możliwą do uzyskania liczbą trójkątów jest :

max

max min

step step

1802l

α α

β α∆

−=

W tym konkretnym przypadku jest to:

max

max min

step step

180 180 90 132 2 148320

0,5 0,5l

α α

β α∆

− += = =

Do porównania jakości odwzorowania obserwowanej sceny wykorzystałem

zamieszczone poniżej rysunki.

74

Rysunek 66. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 7 cm.

Rysunek 67. Zdjęcie obszaru przedstawionego na powyższym skanie.

75

Rysunek 68. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 7 cm.

Rysunek 69. Zdjęcie obszaru przedstawionego na powyższym skanie.

76

Rysunek 70. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 30 cm.

Rysunek 71. Zdjęcie obszaru przedstawionego na powyższym skanie.

77

Rysunek 72. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 30 cm.

Rysunek 73. Zdjęcie obszaru przedstawionego na powyższym skanie.

78

Rysunek 74. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 60 cm.

Rysunek 75. Zdjęcie obszaru przedstawionego na powyższym skanie.

79

Rysunek 76. Fragment sceny 3D uzyskanej przy ustawieniu maksymalnej długości boku trójkąta na 60 cm.

Rysunek 77. Zdjęcie obszaru przedstawionego na powyższym skanie.

80

Podsumowując kwestię doboru parametru maksymalnej długości krawędzi

trójkąta – wartość ta powinna być każdorazowo dobierana w zależności od

opisanych poniżej czynników.

Przy doborze tego parametru trzeba się kierować pewnym kompromisem

pomiędzy dokładnością sceny a wielkością obszaru dodanego do sceny 3D. Im

mniejsza wartość zostanie ustawiona, tym dokładniejsza będzie wyjściowa scena

3D. Z drugiej strony, zwiększanie tej wartości prowadzi do dodania większego

obszaru do wyjściowej sceny.

Zezwolenie na tworzenie zbyt dużych trójkątów prowadzi do powstawiania

na wyjściowej scenie 3D obiektów, które w rzeczywistości nie istnieją. Zjawisko to

dobrze obrazuje Rysunek 74 i Rysunek 75.

Zbyt silne ograniczenie maksymalnej długości krawędzi trójkątów prowadzi

z kolei do sytuacji, w której obiekty będące w zasięgu skanera nie zostaną dodane

do wyjściowej sceny 3D.

Przy doborze akceptowalnej wartości badanego parametru należy brać pod

uwagę następujące czynniki:

• Charakterystyka dodawanych do mapy obiektów – jeżeli budowana jest mapa

pomieszczeń mało skomplikowanych, prostych brył itp., długość

maksymalnej krawędzi można zwiększyć, umożliwiając dodanie większego

obszaru do mapy 3D. W przeciwnym razie wartość tą należy zmniejszyć,

minimalizując ilość dodanych do mapy niepożądanych trójkątów.

• Rozdzielczość serwomechanizmu i lasera – zwiększanie tych parametrów

powinno iść w parze ze zmniejszaniem maksymalnej długości krawędzi.

Najczęściej uzyskanie satysfakcjonujących wyników wiąże się z

przeprowadzeniem kilku skanów 3D z różną wielkością badanego powyżej

parametru w celu wyboru akceptowalnej jego wartości. Po przeprowadzeniu

licznych doświadczeń doszedłem do wniosku, że przy poszukiwaniu

akceptowalnego ustawienia parametru dość dobrą wartością wyjściową jest 30 cm.

81

6.2. Analiza wydajności silnika FPP3DMap

Pomiary wydajności wykonywane były na komputerze PC z procesorem

AMD Athlon 2500+, 512 MB RAM, Radeon 9600 Pro 128 MB przy włączonej opcji

sprzętowej akceleracji karty graficznej.

W celu zmierzenia wydajności silnika generowałem kolejne sceny składające

się z coraz większej liczby trójkątów i porównywałem liczbę wyświetlanych klatek

na sekundę.

Tabela 4. Liczba wyświetlanych klatek oraz trójkątów na sekundę w zależności od liczby trójkątów w scenie 3D

Wartości te można przedstawić w postaci poniższych trendów:

0

1000000

2000000

3000000

4000000

5000000

6000000

1300

10100

18900

27700

36500

45300

54100

62900

71700

80500

89300

98100

106900

115700

124500

133300

142100

150900

159700

168500

177300

186100

194900

203700

212500

221300

230100

238900

Liczba trójkątów wchodzących w skład sceny

Lic

zb

a w

yśw

ietl

an

ych

tró

jkątó

w n

a s

eku

ndę

Rysunek 78. Liczba wyświetlanych trójkątów na sekundę w zależności od ich liczby na scenie.

Liczba trójkątów Klatki na sekundę Trójkąty na sekundę

1320 320 422400

6600 294 1940400

15000 205 3075000

30000 135 4050000

66000 76 5016000

90000 58 5220000

240000 23 5520000

82

0

50

100

150

200

250

300

350

1300

1630

0

3130

0

4630

0

6130

0

7630

0

9130

0

1063

00

1213

00

1363

00

1513

00

1663

00

1813

00

1963

00

2113

00

2263

00

Liczba trójkątów wchodzących w skład sceny

Lic

zb

a w

yśw

ietl

an

ych

tró

jkątó

w n

a s

eku

ndę

Rysunek 79. Liczba klatek na sekundę w zależności od liczby wyświetlanych trójkątów.

Powyższe wyniki zostały wygenerowane podczas wyświetlania scen o

zróżnicowanej liczbie trójkątów, oświetlonych za pomocą trzech źródeł świateł. Za

każdym razem w obszarze widzenia były wszystkie trójkąty. To jest ważna

właściwość tego podsumowania, gdyż rzadko się zdarza, żeby wszystkie

elementy sceny były zawsze widoczne. Zwykle duża część trójkątów będzie się

znajdować poza polem widzenia obserwatora. Można zatem założyć, że powyższe

podsumowanie jest najgorszym możliwym scenariuszem, w rzeczywistości

można spodziewać się nieco lepszych wyników.

Animację uznaje się za płynną, jeżeli liczba klatek na sekundę wynosi co

najmniej 25. Wynika z tego, że aby na sprzęcie zbliżonym pod względem

wydajności do testowego otrzymać płynną animację, w polu widzenia

obserwatora może znajdować się w każdej chwili co najwyżej 200 – 250 tysięcy

trójkątów.

83

Rysunek 80. Scena złożona z 1320 trójkątów.

Rysunek 81. Scena złożona z 30000 trójkątów.

84

7. Podsumowanie

Opracowano i zaimplementowano algorytm umożliwiający budowanie

dokładnych map trójwymiarowych przez roboty mobile wyposażone w skaner

laserowy 3D skonstruowany ze skanera laserowego 2D zamontowanego na

mechanizmie obrotowym napędzanym serwomechanizmem. Algorytm ten służy

do akwizycji danych z lasera 3D, odpowiednim ich przekształceniu do

współrzędnych 3D obserwowanych przeszkód, utworzenia reprezentacji sceny 3D

złożonej z trójkątów, a następnie połączenia dowolnej liczby otrzymanych scen 3D

w jedną spójną reprezentację mapy 3D.

Dodatkowo opracowano i zaimplementowano w środowisku OpenGL

interaktywny silnik graficzny 3D, umożliwiający wyświetlanie otrzymanych map

3D w czasie rzeczywistym. Program ten umożliwia wyświetlenie dowolnych,

nawet bardzo złożonych scen 3D złożonych z trójkątów, dzięki zastosowaniu

różnych technik optymalizacji zwiększających jego wydajność.

Opisany został aktualny stan wiedzy o budowaniu map 3D przez roboty

mobilne na podstawie dostępnej literatury poświęconej temu zagadnieniu.

Scharakteryzowane zostały wykorzystywane w robotyce reprezentacje map 3D

oraz kolejne etapy ich konstruowania. Najwięcej uwagi zostało poświęcone

opisowi reprezentacji map 3D złożonych z trójkątów – szczegółowo opisano

szeroki zestaw wykorzystywanych podejść do budowania i obróbki tak złożonych

scen.

Przeprowadzono serię testów w celu zbadania wpływu różnych parametrów

konfiguracyjnych algorytmu służącego do budowania mapy 3D na jakość

otrzymanej mapy, czas potrzebny do przeprowadzenia pomiarów oraz złożoność

mapy 3D. Badanymi parametrami były: rozdzielczość dalmierza laserowego

(liczba próbek w pojedynczym skanie 2D), rozdzielczość serwomechanizmu

(różnica kątowa pomiędzy kolejnymi przeprowadzanymi skanami 2D) oraz

maksymalna długość boku tworzonych trójkątów. W ramach testów sporządzona

została mapa 3D niewielkiego fragmentu korytarzy gmachu Wydziału Elektroniki

i Technik Informacyjnych Politechniki Warszawskiej. Testy dowiodły, że przy

wykorzystaniu zaimplementowanego algorytmu możliwe jest stworzenie

dokładnej mapy 3D badanego środowiska.

Przeprowadzono również testy wydajności zaimplementowanego silnika

graficznego. Zbadano wpływ liczby trójkątów w wyświetlanej scenie 3D na liczbę

wyświetlanych klatek na sekundę (a tym samym liczbę wyświetlanych trójkątów

85

na sekundę). Na przeciętnym komputerze klasy PC otrzymano wydajność rzędu

5,5 miliona wyświetlanych trójkątów na sekundę, oświetlanych przez 3 różne

źródła światła. Dowodzi to, że przy wykorzystaniu zaimplementowanego

rozwiązania możliwe jest wyświetlenie w czasie rzeczywistym nawet bardzo

złożonych scen 3D.

Dalszymi kierunkami prac nad algorytmem służącym do budowania map

3D powinny być przede wszystkim: zaimplementowanie metody upraszczania

skanów 3D (wykorzystującej zmodyfikowany algorytm usuwania krawędzi) oraz

zaimplementowanie metody łączenia wielu scen 3D w jedną spójną

(minimalizującej globalny błąd). Dodatkowo, aby możliwe było w pełni

autonomiczne budowanie mapy 3D przez robota mobilnego należy

zaimplementować algorytm wyznaczania następnego punktu przeprowadzania

pomiarów na podstawie już uzyskanych.

Z innych możliwych kierunków rozwoju zaimplementowanych slgorytmów

warto wymienić dodanie możliwości akwizycji oraz wyświetlania map 3D

pokrytych teksturami. Wymagałoby to modyfikacji algorytmów budowania oraz

wyświetlania map 3D, jak również wyposażenie robota w odpowiedni sprzęt –

kamerę bądź aparat cyfrowy.

86

8. Bibliografia

[1] M. Herbert, “Active and passive range sensing for robotics”, Proceedings of the

IEEE International Conference on Robotics and Automation (ICRA '00), tom 1, str.

102-110, 2000.

[2] S. Thurn, D. Fox, W. Burgard, “A real-time algorithm for mobile robot

mapping with application to multi robot and 3D mapping”, Proceedings of the

IEEE International Conference on Robotics & Automation (ICRA), str. 321-328, 2002

[3] S. Thurn, D. Hahnel, W. Burgard, “Learning Compact 3d Models of Indoor

and Outdoor Environments with a Mobile Robot”, Robotics and Autonomous

Systems, tom 44, str. 15-27, 2003

[4] H. Zhao, R. Shibasaki, “Reconstructing Textured CAD Model of Urban

Environment using Vehicle-borne Laser Range Scanners and Line Cameras”,

Second International Workshop on Computer Vision System, strony 284 - 297, 2001

[5] V. Sequeira, J. Goncalvez, D. Hogg, “Automated 3D reconstruction of interiors

with multiple scan-views”, SPIE, Electronic Imaging '99, Society for Imaging

Science and Technology, strony 106-117, 1999

[6] P. Allen, I. Stamos, A. Gueorguiev, E. Gold, P. Blaer, “AVENUE: Automated

Site Modeling in Urban Environments”, Third International Conference on 3D

Digital Imaging and Modeling, strony 357–364, 2001

[7] S. Kristensen, P. Jensfelt, “Active global localisation for a mobile robot using

multiple hypothesis tracking”, Transactions on Robotics and Automation tom 17,

strony 748-760, 2001

[8] A. Walthelm, A. Madany Momlouk, “Multisensoric active spatial exploration

and modeling”, Dynamische Perzeption: Workshop der GI-Fachgruppe 1.0.4

Bildverstehen, Ulm., strony 141-146, 2000

[9] O. Wulf, K. Arras, H. Christensen, B. Wagner “2D Mapping of Cluttered

Indoor Environments by Means of 3D Perception”, Conference on Robotics and

Automation (ICRA), strony 4204-4209, 2004

[10] A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg, “Consistent 3D Model

Construction with Autonomous Mobile Robots”, KI 2003: Advances in Artificial

Intelligence. 26th Annual German Conference on AI, Proceedings Springer LNAI tom

2821, strony 550-564, 2003

87

[11] S. Fekete, R. Klein, A. Nüchter, “Online Searching with an Autonomous

Robot”, Workshop on Algorithmic Foundations of Robotics (WAFR), strony 350-365,

2004

[12] H. Surmann, A. Nüchter, J. Hertzberg, “An autonomous mobile robot with a

3D laser range finder for 3D exploration and digitalization of indoor

environments”, Robotics and Autonomous Systems, tom 45 nr 3-4, str. 181-198,

2003

[13] H. Surmann, K. Lingemann, A. Nüchter, J. Hertzberg, “A 3D laser range finder

for autonomous mobile robots”, Proceedings of the 32nd ISR(International

Symposium on Robotics), 2001

[14] J. Weingarten, G. Gruener, R. Siegwart, “A Fast and Robust 3D Feature

Extraction Algorithm for Structured Environment Reconstruction”, Proceedings

of 11th International Conference on Advanced Robotics (ICAR), 2003

[15] A. Nüchter, H. Surmann, J. Hertzberg, “Automatic Model Refinement for 3D

Reconstruction with Mobile Robots”, Proceedings of the 4th IEEE International

Conference on Recent Advances in 3D Digital Imaging and Modeling, str. 394 - 401,

2003

[16] A. Nüchter, K. Lingemann, J. Hertzberg, “Extracting Drivable Surfaces in

Outdoor 6D SLAM”, Proceedings of the 37nd International Symposium on Robotics

(ISR '06) and Robotik 2006, 2006

[17] D. Huber, M. Hebert, “A New Approach to 3-D Terrain Mapping”, Proceedings

of the 1999 IEEE/RSJ International Conference on Intelligent Robotics and Systems

(IROS '99), IEEE, str. 1121-1127, 1999

[18] D. Luebke, C. Erikson, “View-dependent simplification of arbitrary polygonal

environments”, Proc. of the 24rd International Conference on Computer Graphics

and Interactive Techniques (SIGGRAPH), str. 197-208, 1997

[19] W. Schroeder, J. Zargea, W. Lorenson, “Decimation of triangle meshes”,

SIGGRAPH 92 Conference Proceedings, volume tom 26, str. 65-70, 1992

[20] M. Soucy, D. Laurendeau, “Multiresolution surface modeling based on

hierarchical triangulation”, Computer Vision and Image Understanding, tom 63, nr

1, str. 1-14, 1996

88

[21] J. Rossignac, P. Borrel, “Multi-resolution 3D approximations for rendering

complex scenes”, Geometric Modeling in Computer Graphics, str. 455-465, 1993

[22] H. Hoppe, “Progressive meshes”, SIGGRAPH ’96 Proceedings, str. 99–108, 1996.

http://www.research.microsoft.com/research/graphics/hoppe/

[23] M. Garland, P. Heckbert, “Surface Simplification Using Quadric Error Metrics”

SIGGRAPH '97 Proceedings, str. 209-216, 1997

[24] L. Wong, C. Dumont, M. Abidi, “A “Best-Next-View” Algorithm for Three-

Dimensional Scene Reconstruction Using Range Images”, SPIE (Intelligent

Robots and Computer Vision XIV: Algorithms), tom 2588, str. 418--429, 1995

[25] A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg, “Planning Robot Motion

for 3D Digitalization of Indoor Environments”, 11th International Conference on

Advanced Robotics (ICAR), str. 222-227, 2003

[26] J. O'Rourke , “Art Gallery Theorems and Algorithms”, Oxford University Press,

1987

[27] B. Tovar, C. Esteves, R. Murrieta-Cid, “Robot Motion Planning for Map

Building”, International Conference on Intelligent Robots and Systems (IROS), 2002

[28] G. Weiß, C. Wetzler, E. Puttkamer, “Keeping Track of Position and Orientation

of Moving Indoor Systems by Correlation of Range-Finder Scans”, IROS tom 1,

str. 595-601, 1994

[29] P. Besl, N. McKay, “A method for registration of 3D shapes”, IEEE Transactions

on Pattern Analysis and Machine Intelligence 14, str. 239–256, 1992

[30] Y. Chen, G. Medoni, “Object modelling by registration of multiple range

images”, Proceedings of the IEEE Conference on Robotics and Automation

(ICRA’91), str. 2724–2729, 1991

[31] Z. Zhang, “Iterative point matching for registration of free-form curves”,

Technical Report No. RR-1658, INRIA, 1992

[32] D. Simon, M. Hebert, T. Kanade, “Real-time 3D pose estimation using a high-

speed range sensor”, Proceedings of the IEEE International Conference on Robotics

and Automation (ICRA’94), tom 3, str. 2235–2241, 1994

[33] C. Ye, J. Borenstein, “Characterization of a 2-D Laser Scanner for Mobile Robot

Obstacle Negotiation”, Proceedings of the 2002 IEEE International Conference on

Robotics and Automation, str. 2512-2518, 2002

89

[34] www.lighthouse3D.com – “GLUT Tutorial”

[35] R. Wright, B. Lipchak, “OpenGL – Księga Eksperta”, rok wyd. 2000.

[36] J. Neider, T. Davis, M. Woo, “OpenGL Programming Guide”, rok wyd. 1993.

[37] wiele innych stron, serwisów oraz for internetowych zajmujących się tematyką

OpenGL oraz GLUT

[38] www.wikipedia.com – The Free Encyclopedia