Kalibrace kamery pro robotické pracoviště

Transkript

1 5 Bakalářská práce F3 České vysoké učení technické v Praze Fakulta elektrotechnická Katedra kybernetiky Kalibrace kamery pro robotické pracoviště Jaroslav Lištvan Studijní program: Kybernetika a robotika Obor: Robotika Květen 2017 Vedoucí práce: Ing. Pavel Krsek, Ph.D.

2

3

4

5 Poděkování Chtěl bych poděkovat Ing. Pavlu Krskovi, Ph.D. za ochotu a podporu v půběhu psaní této práce. / Prohlášení Prohlašuji, že jsem předloženou práci vypracoval samostatně a že jsem uvedl veškeré použité informační zdroje v souladu s Metodickým pokynem o dodržování etických principů při přípravě vysokoškolských závěrečných prací. V Praze dne v

6 Abstrakt Práce se zabývá kalibrací kamery na robotickém pracovišti. Cílem je odhad vnějších a vnitřních parametrů kamery. Tedy zajištění opravy deformace obrazu poskytnutého kamerou a odhadnutí její polohy a orientace v rámci robotického pracoviště. Účelem je umožnit použití kamery ke snímání pracovního prostoru, aby na základě této informace bylo možné provádět robotickou manipulaci s předměty. Klíčová slova: OpenCV, kalibrace, kamera, Aruco / Abstract Thesis studies camera calibration in robotic workspace. Goal is to estimate extrinsic and intrinsic camera parameters. Thus enable undistortion of image provided by camera, and estimating camera s position and orientation in robotic workspace. Purpuse of thesis is to enable use of camera to gather accurate information about camera workspace to be used for robotic manipulation with objects in camera workspace. Keywords: OpenCV, calibration, kamera, Aruco Title translation: Camera calibration for robotic workspace vi

7 / Obsah 1 Úvod Robotické pracoviště Kamera Objektiv Robot Pracovní prostor Kalibrace kamery Matematický model kamery Kalibrace kamery pomocí OpenCV Záznam obrazů pro kalibraci Provedení kalibrace v OpenCV Vzájemná kalibrace robota a kamery Záznam dat pro vzájemnou kalibraci Výpočet poloh značky Aruco Detekce značky Aruco v obrazech scény Výpočet vnějších parametrů kamery Vyhodnocení výsledků Výsledky kalibrace kamery Výsledky vzájemné kalibrace robota a kamery Závěr Literatura vii

8 / Obrázky 2.1. Robotické pracoviště Parametry fotoaparátu Nikon D Parametry objektivu AF Nikkor 50mm f/1.8d Robotická sestava Schéma rozvržení zařízení na pracovišti Model kamery (Pinhole camera model)...6 viii

9 Kapitola 1 Úvod Tato práce se zabývá kalibrací kamery umístěné na robotickém pracovišti za účelem použití jejich snímků k robotické manipulaci. Aby bylo možné použití obrazu z kamery k robotické manipulaci je nutné znát její vnitřní a vnější parametry. Znalost vnitřních parametrů slouží k opravě obrazu kamery a získání přesné informace o zabírané scéně, zatímco vnější parametry udávají polohu a orientaci kamery vůči scéně samotné. Vnitřní parametry získám pomocí snímání obrazů kalibračního obrazce známých rozměrů a jejich zpracováním pomocí funkce knihovny OpenCV calibratecamera. Vnější parametry získáme pomocí zpracování obrazů značek Aruco se znamými polohami ve světových souřadnicích. Porovnáním poloh značek ve světových souřadnicích s jejich polohami v souřadnicích kamery získámé pozici a orientaci kamery ve světových souřadnicích. S těmito dvěma druhy parametrů bude poté možné získat z obrazů kamery informace pro řízení ramena manipulátoru a provádění manipulace. 1

10 Kapitola 2 Robotické pracoviště Robotické pracoviště je prostor ve kterém se nacházejí používaná zařízení a kde je s nimi prováděna činnost. V této práci je pracoviště tvořeno kamerou s objektivem, robotem CloPeMa [1] a pracovní plochou. V tomto prostoru budeme provádět kalibraci kamery a vzájemnou kalibraci robota a kamery. Rozmístění těchto zařízení je zobrazeno na následujícím Obrázku 2.1. Obrázek 2.1. Model robotické pracoviště v simulátoru Rviz Robot CloPeMa je umístěn ve středu robotického pracoviště. Na tomto místě si v základně robota definujeme počátek světových souřadnic. Orientace a poloha světového souřadného systému je uvedena na obrázku2.1 a je také souřadnicovou soustavou robota. Pracovní prostor je tvořen plochou tácu, umistěného na stole ve směru osy x světových souřadnic ve vzdálenosti přibližně 1.3 metru. Stůl je umístěn symetricky vůči ose x. Kamera je umístěna ve svislém směru nad pracovním prostorem, tak aby jej zabírala pokud možno celou jeho plochu. 2.1 Kamera Použitou kamerou je fotoaparát Nikon Digital SLR Camera D Kamera je napájena z robotické setavy. Řízení a komunikace probíhá pomocí USB. Kamera je umístěna nad pracovní plochou pomocí stojanu, tak aby snímala celou pracovní plochu s jejím středem uprostřed obrazu.2.5. Základní vlastnosti fotoaparátu jsou2.2. 2

11 ... PARAMETR Typ Rozlišení Velikost senzoru Typ senzoru 2.2 Objektiv HODNOTA Nikon Digital SLR Camera D x 3264 px 23.6 x 15.6 (ASP-C Nikon) CMOS Obrázek 2.2. Parametry fotoaparátu Nikon D5100 [2] 2.2 Objektiv Jedná se o objektiv AF Nikkor 50mm f/1.8d2.3. Tento objektiv má pevné ohnisko ve vzdálenosti 50 mm. U zaostřovacího objektivu by docházelo pokaždé k různé deformaci obrazu a tan by jej nebylo možné opravit pomocí jedné kalibrace kamery, ale kalibrace by se musela provést při kažném novém zaostření. Objektiv byl zaostřen na zachycení pracovní plochy. Takto zaostřen již zůstavá a pro tento stav provedeme kalibraci. Parametry jsou následující2.3. PARAMETR Typ Ohnisková vzdálenost Nejkratší zaostřitelná vzdálenost HODNOTA AF Nikkor 50mm f/1.8d 50 mm 0.45 m Obrázek 2.3. Parametry objektivu AF Nikkor 50mm f/1.8d[3] 2.3 Robot Robot CloPeMa [1] je umístěn nad počátkem světových souřadnic. Sestává ze základny, otočné torza MOTOMAN R-750 [4] se dvěmi otočnými rameny MOTOMAN MA1400 [5]. Obrázek 2.4. Robotická sestava 3

12 2. Robotické pracoviště Pracovní prostor Pracovním prostorem je plocha kovového tácu o velikosti 90x60 cm. Tác je umístěn na stole ve výšce 60.5 cm nad zemí. Střed tácu se nachází pod kamerou, tak aby zabírala pokud možno celou jeho plochu. Přibližné rozvržení zařízení v prostoru je na následujícím schématu [6]. Vzhledem k tomu, že objekty robotického pracoviště, s vyjímkou robota, nejsou pevně připevněny k zemi, nelze zaručit jejich přesnou polohu, či jejich setrvaní v dané poloze v průběhu práce na robotockém pracovišti. Obrázek 2.5. Schéma rozvržení zařízení na pracovišti 4

13 Kapitola 3 Záznam obrazu Tato kapitola popisuje záznam obrazu pracovního prostoru kamerou k provedení kalibrace kamery a vzájemné kalibrace robota a kamery. V zadání práce je určeno provést záznam snímků v prostředí ROS[7]. Toto řešení boužel nebylo možné v přiměřené době provést. Z důvodu složitosti takového řešení a vzniklých komplikací vzhledem k jednoduchosti samotného úkonu jsem se rozhodl provést záznam obrazu pomocí knihovny libgphoto2. Knihovna libgphoto2 [8] umožnuje s pomocí příkazového řádku gphoto2 snadný a rychlý záznam obrazu u značného počtu modelů fotoaparátů. K záznamu jsem navrhl program v prostředí C++, který bude provádět záznam vybraných snímků potvrzených uživatelem. Uživatel může s programem nakládat dle po spuštění zobrazeného manuálu a zobrazený náhled scény uložit, či nahradit aktuálním. Tento program také po dokončení záznamu vytvoří seznam snímků ve formátu pro jejich snadné zpracování pomocí knihovny OpenCV [9]. Program se nachází v použitém kódu, pod názvem capture.cpp. Jeho kompilaci lze provést v adresáři kde se nachází pomocí příkazu g++ -o capture capture.cpp pkg-config opencv cflags libs. Program se volá pomocí příkazu./capture. 5

14 Kapitola 4 Kalibrace kamery 4.1 Matematický model kamery Ke zjištění stavu scény zabírané kamerou je nutné, aby z kamery získaný obraz byl její co nejpřesnější reprezentací. Obraz získaný kamerou je ovlivněn značnou deformací, kterou lze popsat vnitřními parametry kamery. Kalibrace kamery slouží ke zjištění těchto vnitřních parametrů. Se znalostí těchto parametrů je možné zpětně upravit získaný obraz, tak, aby nebyl ovlivněn deformací kamery a byl použitelnou reprezentací scény. Výpočet těchto vnitřních parametrů je prováděn pomoci modelu jednoduché kamery bez čočky znamé jako pinhole camera model[10]. Jde o model kamery, kde je obraz tvořen projekcí bodů z trojrozměrného prostoru do zobrazovací roviny. Obrázek 4.1. Model kamery (Pinhole camera model) sm = A [R t] M (4.1) X x f x 0 c x r 11 r 12 r 13 t 1 s y = 0 f y c y r 21 r 22 r 23 t 2 Y Z (4.2) z r 31 r 32 r 33 t 3 1 (X,Y,Z) - souřadnice bodů v trojrozměrném prostoru ve světových souřadnicích (u,v) - souřadnice místa projekce v pixelech A - je matice kamery, neboli matice vnitřních parametrů (c x, c y ) - souřadnice bodu v pixelech, který bývá ve středu obrazu. f x, f y - jsou vzdálenosti ohniska v pixelech 6

15 Kalibrace kamery pomocí OpenCV Matice vnitřních parametrů nezávisí na poloze, či orientaci kamery v prostoru. Závislost polohy a natočení kamery vůči světovým souřadnicím určuje transformační matice [R t]. Transformační matice popisuje převod souřadnic bodů v trojrozměrném prostoru ze světovýh souřadnic do souřadnic kamery.[11] V případě reálné čočky bude výpočet složitější. Přepočet souřadnic a výpočet souřadnic promítaných bodů s reálnou čočkou, pro z 0 vypadá následovně. x X y = R Y + t (4.3) z Z x = x z (4.4) y = y z (4.5) x = x 1 + k 1r 2 + k 2 r 4 + k 3 r k 4 r 2 + k 5 r 4 + k 6 r 6 + 2p 1x y + p 2 (r 2 + 2x 2 ) (4.6) 1 + k y 1 r 2 + k 2 r 4 + k 3 r 6 = y 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + p 1(r 2 + 2y 2 ) + 2p 2 x y (4.7) r 2 = x 2 y 2 (4.8) u = f x x + c x (4.9) v = f y y + c y (4.10) Kde k 1, k 2, k 3, k 4, k 5 a k 6 jsou radiální koeficienty deformace. Zatímco p 1 a p 2 jsou tangenciální koeficienty deformace. 4.2 Kalibrace kamery pomocí OpenCV K provedení kalibrace kamery použijeme knihovnu OpenCV[9]. Ta je k tomuto účelu vybavena několika funkcemi, sloužicími pro získání potřebných dat ze zaznamenaných obrazů. Jaké obrazy je třeba kamerou zaznamenat, a jak je zpracuji funkcemi knihovny OpenCV popíši v následujících podsekcích Záznam obrazů pro kalibraci K záznamu obrazů použiji program capture, který je popsán v Kapitole 3 Záznam obrazu4. Pro získání potřebných dat z obrazu musí být v obraze viditelně umístěn kalibrační obrazec o známých rozměrech. K této kalibraci jsem použil kalibrační obrazec šachovnice o rozměrech 10x7 a velikosti kostky 2.44 cm. Obrazů kalibračního obrazce musí být kvůli dostatečnému množství dat vyžší počet. Kvůli zmapování deformace obrazu kamery jsem zaznamenal 103 obrazů kalibračního obrazce v různých polohách, orientacích a vzdálenostech od kamery pokrývající celou viditelnou pracovní plochu. 7

16 4. Kalibrace kamery Provedení kalibrace v OpenCV Po nasbírání obrazců pomocí programu použije kalibrační program calibration[12] poskytnutý knihovnou OpenCV v rámci samples. Kód programu zkopírujeme z knihovny do adresáře se zaznamenanými obrazy a vegenerovaným seznamem obrazů. Zkompilujeme jej příkazem g++ -o calibration calibration.cpp pkg-config opencv cflags libs, a zavoláme s příkazem s následujícími argumenty./calibration -w=10 -h=7 -s= o=camera.yml imagefile.xml. V argumentech předáváme rozměry kalibračního obrazce, název výstupního souboru s parametry kamery a název seznamu zaznamenaných obrazů. Výsledkem bude soubor s maticí kamery, deformačními koeficienty a průměrnou chybou reprojekce. 8

17 Kapitola 5 Vzájemná kalibrace robota a kamery Vzájemnou kalibrací robota a kamery je myšleno získání vnějších parametrů kamery. Těmito vnějšími parametry kamery je poloha a orientace perspektivy kamery ve světových souřadnicové soustavě robotického pracoviště. Dále také použijeme modul Aruco. Modul Aruco je součástí dodatečných modulů knihovny OpenCV. Modul Aruco slouží k vytváření a detekci značek určených ke sledování polohy předmětů, na kterých jsou připevněné v obraze kamery. K získání těchto dat je nutné provést následující kroky. 5.1 Záznam dat pro vzájemnou kalibraci K provedení kalibrace je nutné vytvořit řadu obrazů značky Aruco v pozicích se známými polohami ve světových souřadnicích nad pracovním prostorem. Toto je docíleno nastavovaním ramena s pevně připevněnou značkou Aruco do různých kamerou viditelných poloh o různých výškách nad pracovním prostorem. Postup je podobný provedení záznamu obrazů pro kalibraci vniřních parametrů v Kapitole Při tomto kroku provádím nastavování ramena robora pomocí uživatelského rozhraní prostředí ROS s názvem Rviz. V tomto prostředí pro kažkou polohu ramena zaznamenám poskytnutou informaci o poloze a orientaci sledované osy části ramena r2 gripper ve světových souřadnicích. Vůči této ose je mi známa přesná poloha a orientace značky Aruco v jejich souřadnicích. Souřadnicemi je poloha osy r2 gripper ve světových souřadnicích a orientace v podobě kvaternionů. Polohu značky Aruco následně přepočítám to světových souřadnic. 5.2 Výpočet poloh značky Aruco K přepočtu poloh značky Aruco ze souřadné soustavy manipulátoru r2 gripper, do světové souřadné soustavy použiji náležitou transformační matici. K vytvoření transformační matice musím nejprve získat matici rotační. Rotační matici získám ze zaznamenaných kvaterniónů pomocí funkcí knihovny Eigen[13]. Použité funkce jsou.normalized() k normalizaci kvaternionu a.torotationmatrix() pro převedení na rotační matici. S pomocí knihovny Eigen získanou matici je nutné upravit tak, že když je realná části kvaterniónu q.w záporná, provedu ještě její transpozici. Výpočet polohy značky Aruco ve světových souřadnicích je následující. x y = T z X x y = R Y Z z (5.1) (5.2) 9

18 5. Vzájemná kalibrace robota a kamery... (X,Y,Z) - souřadnice osy r2 gripper ve světových souřadnicích (x,y,z) - souřadnice středu značky Aruco ve světových souřadnicích R - rotační matice z osy r2 gripper do světových souřadnic. ( , 0, ) - poloha středu značky Aruco v souřadnicích osy r2 gripper 5.3 Detekce značky Aruco v obrazech scény K detekci značek Aruco poskytuje modul knihovny OpenCV funkci detectmarkers. Tato funkce v zadaném obrazu zbaveném deformace způsobené kamerou nalezne rohy značky Aruco v souřadnicích pixelů obrazu u,v. Střed značky vzhledem k jejímu čtvercovitému tvaru získáme jako průměru souřadnic jejích rohů. 5.4 Výpočet vnějších parametrů kamery K získání vnějších parametrů použijeme souřadnice středu značky Aruco ve světových souřadnicích a v souřadnicích pixelů jejich obrazu. Tyto dvě odpovídající sady souřadnic zpracujeme pomocí funkce knihovny OpenCV s názvem solvepnp. Funkce vyžaduje jak souřadnice bodu ve světových souřadnicích tak v součadnicích pixelů obrazu. Dále funkce využívá matici kamery a deformační koeficienty. Funkce solvepnp vrací translační vektor z osy kamery do světových souřadnic a rotační vektror druhu rodrigues knihovny OpenCV z kamery do světových souřadnic. Rotační matici z rotačního vektoru rodriques získáme pomocí funkce Rodriques(rotVector, rotmatrix)[12]. Translační vektor ve směru ze světových souřadnic do polohy kamery, tedy polohový vektor kamery ve světových souřadnicích získáme následovně. X x Y = R T y (5.3) Z z (X,Y,Z) - souřadnice kamery ve světových souřadnicích (x,y,z) - translační vektor ze soustavy kamery do světových souřadnic v. R - rotační matice ze osy kamery do světové souřadnicové osy 10

19 Kapitola 6 Vyhodnocení výsledků V této kapitole provedu vyhodnocení výsledků zadaných úkonů. 6.1 Výsledky kalibrace kamery Výsledkem kalibrace kamery je matice kamery a deformační koeficienty. Kalibraci jsem provedl voláním programu knihovny OpenCV calibration. Program zkompilujeme pomocí příkazu g++ -o calibration calibration.cpp pkg-config opencv cflags libs a zavolaným příkazem s argumenty./calibration -w=10 -h=7 -s= o=camera.yml imagefile.xml. Kde imagefile sestává ze 103 obrazů kalibračního obrazce šachovnine. Program volaný s těmito parametry vypočítal následující matici kamery a koeficienty deformace. Hodnoty jsou matice kamery jsou v pixelech uvedu je tedy zaokrouhlené na celé pixely. f x 0 c x f y c y = (6.1) c x, c y - souřadnice bodu v pixelech, který bývá ve středu obrazu. f x, f y - jsou vzdálenosti ohniska v pixelech Hodnoty matice kamery lze ověřit výpočtem ohniskové vzdálenosti ze vzdáleností ohniska v pixelech a znalosti počtu pixelů v rozměru čipu společně s rozměrem čipu. Výpočet je následující. F x = f x (W/w) = 11687(0.0236/4928 = m (6.2) F y = f y (H/h) = 11687(0.0156/3264) = m (6.3) f x, f y - jsou vzdálenosti ohniska v pixelech W, H - rozměry výšky a šířky senzoru CMOS v metrech. w, h - šířka a výška pixelového pole CMOS senzoru v pixelech Výsledné hodnoty za předpokladu, že rozměry čipu jsou kvůli lemu na okrajích čipu větší než samotné pixelové pole, odpovídá ohniskové vzdálenosti objektivu 50 mm. Souřadnice c x = 2463 a c y = 1617 označují bod projekce na čipové pole odpovídají středu kamerou zabírané scény. Pixelové pole čipu má velikost 4928x3264. Střed tohoto pole odpovídá přibližně pixelu u = 2464 a v = V tomto případě leží střed scény téměř zcela přesně ve středu pixelového pole. Kalibrační program dále vypočítal reprojekční chybu, chybu polohy změřených vůči vypočítaným polohám bodů daných dosažený získanými kalibračními hodnotami. Tato chyba je udávaná v pixelech a pro tuto kalibraci je pixelu. Vzhledem k výsledkům výpočtu z hodnot matice kamery a hodnotě reprojekční chyby menší než jeden pixel považuji kalibraci kamery za použitelnou. 11

20 6. Vyhodnocení výsledků Výsledky vzájemné kalibrace robota a kamery Výsledkem vzájemné kalibrace robota a kamery je poloha a rotace perspektivy kamery vůči světové souřadné ose použivané robotem. Kalibrace je dosažena zpracováním pomocí dvou sad souřadnic stejných bodů, ve světových souřadnicích a rovinných souřadnicích obrazu kamery pomocí programu robcamcalib. Program se kompiluje povelem g++ -o robcamcalib robcamcalib.cpp pkg-config opencv cflags libs a volá se povelem./robcamcalib. Při použití funkce solvepnp s metodou ITERATIVE, která funguje na principu minimalizace reprojekční chyby, bylo dosaženo těchto výsledků R R C = (6.4) tvec = (6.5). R R C - rotační matice kamery ze souřadnic robota do kamery. tvec - translační vektor polohy perspektivy kamery vůči světovým souřadnicím Výsledky odhadu pozice kamery jsou velmi podobné hodnotám naměřeným při ručním měřením. Rotační matice opovídá změnám v orientaci os. Kde je poloha kamery odhadnuta přibližně na souřadnice (x,y,z) = (1.25, 0, 2.5) metrů. Absolutní odchylky jsou přibližně (1, 2, 3.4) centimetrů. Vzhledem k nepřesnosti dostupných ručních měřících zařízení a tomu, že konstrukce držící fotoaparát není tuze spojená se souřadnou soustavou robota, se také nelze spolehnout že ručně měřený odhad polohy kamery odpovídá skutečnosti. Tento výsledek povařuji za přijatelný odhad, pro další kalibraci na spolehlivějším provedení konstrukce robotického pracoviště. 12

21 Kapitola 7 Závěr V rámci práce jsem provedl kalibraci kamery pro použití na daném robotickém pracovišti. K záznamu obrazu jsme místo prostředí ROS, z důvodů komplikované implementace jsem zvolil použití knihovny libgphoto2. Ta umožnila záznam obrazu mnohem uživatelsky přívětivějším způsobem. Pro podporu snímání jsem vytvořil program umožnují náhled a následné uložení obrazu s následnou automatickou tvorbou seznamu obrazů pro zpracování pomocí knihovny OpenCV. Dále jsem určil vnitřní parametry kamery pomocí snímání obrazů kalibračního obrazce. Výsledkem byla matice kamery a koeficienty popisující deformaci optické cesty umožnující opravu obrazu k získání věrohodné reprezentace pracovního prostoru, nad kterým je kamera umístěna. V dalším kroku jsem s takto kalibrovanou kamerou provedl záznam značek Aruco. Po provedení porovnání jejich poloh ve světové souřadnicové soustavě a souřadnicové soustavě kamery jsem získal vnější parametry kamery. Výsledkem je odhad polohy a orientace kamery vůči světové souřadnicové soustavě. Výsledný odhad přesně neodpovída ručnímu měření konstrukce, ale vzhledem k faktu že, konstrukce držící kameru není tuhá a provádáme porovnání s velmi nepřesnými prostředky jde o přijatelný odhad. Se získanými vnitřními a vnějšími parametry je dále možné provést robotickou manipulaci s objety nalezajícími se na pracovní ploše. Za pomocí algoritmů pro detekci a lokalizaci objektů v obrazu lze určit z obrazu kamery polohy objektu na pracovní ploše. 13

22 Literatura [1] Home. CloPeMa - Clothes Perception and Manipulation. [online]. [cit ]. [2] Technická specifikace. Nikon. [cit ]. http: / / www. nikon. cz / cs_cz / product / discontinued / digital-cameras / 2015 / d5100#tech_specs. [3] Technická specifikace. Nikon. [cit ] d/JAA013DA/details#specificationstab. [4] R-series. YASKAWA. [online]. [cit ]. ositioner_pi1%5buid%5d=182&chash=6405ccea1d8f18088ed550b0e [5] MA YASKAWA. [online]. [cit ]. %5Buid%5D=2499&cHash=e9e a15a5cf9d2ace0a65545e9. [6] Home. RadioRoSo. [online]. [cit ]. [7] Morgan Quigley, Brian Gerkey a William D. Smart. Programming Robots with ROS. 1. vydání. O Reilly Media, ISBN [8] gphoto. libgphoto2 library. [online]. [cit ]. [9] OpenCV. Open Source Computer Vision Library. [online]. [cit ]. [10] Camera Calibration a 3D Reconstruction. OpenCV. [online]. [cit ]. econstruction.html. [11] Milan Šonka, Václav Hlaváč a Roger Boyle. Image Processing, Analysis and Machine Vision. 2. vydání. PWS, ISBN X. [12] [13] Gaël Guennebaud, Benoît Jacob a others. Eigen v