Model systemu stereowizyjnego

Dopasowanie pikseli lewego i prawego obrazu odbywa si¦ na obrazach

rekty-kowanych. Rektykacja polega na takim przeksztaªceniu pary obrazów, aby odpowiadaªy one obrazom uzyskanym w sytuacji idealnej, w której obrazy le»¡ we wspólnej pªaszczy¹nie, a odpowiadaj¡ce sobie wiersze obu obrazów le»¡ wzdªu» tych samych prostych. W takiej sytuacji linie epipolarne poªo»o-ne s¡ wzdªu» wierszy, dzi¦ki czemu dopasowanie odbywa si¦ tylko w obr¦bie odpowiadaj¡cych sobie wierszy [23].

W celu wyznaczenia parametrów przeksztaªcenia, korzysta si¦ z: para-metrów wewn¦trznych kamer, wspóªczynników znieksztaªce« wprowadzanych

przez obiektywy oraz parametrów zewn¦trznych systemu stereowizyjnego.

Wymagana jest zatem uprzednia kalibracja systemu stereowizyjnego. Model kamery wykorzystywany w algorytmie rektykacji jest oparty na otworkowym (ang. pin-hole) modelu kamery i uwzgl¦dnia dystorsj¦ radialn¡ oraz tangen-cjaln¡. Je±li znane s¡ wspóªrz¦dne punktu w przestrzeni pc = [x_c y_c z_c]^T, to wspóªrz¦dne x, y punktu po rzutowaniu na pªaszczyzn¦ obrazow¡ b¦d¡

nast¦puj¡ce: [ S¡ one nast¦pnie poddawane dystorsji, opisanej równaniami:

[ x_d pro-cesie kalibracji, a r jest odlegªo±ci¡ punktu od ±rodka obrazu:

r =√

x²+ y². (5.3)

Ostatecznie wspóªrz¦dne obrazowe na obrazie znieksztaªconym obliczane s¡

nast¦puj¡co: [ gdzie fx i fy to ogniskowa, wyra»ona w liczbie pikseli (odpowiednio w kie-runku poziomym i pionowym), a cx i cy to wspóªrz¦dne punktu gªównego kamery.

Wszystkie wspóªczynniki wykorzystywane w równaniach (5.1)  (5.4) wy-znaczane s¡ w procesie kalibracji kamery. Dzi¦ki kalibracji ukªadu stereowi-zyjnego znane jest równie» poªo»enie ukªadu wspóªrz¦dnych prawej kamery w ukªadzie wspóªrz¦dnych lewej kamery. Poªo»enie to wyra»one jest przy pomocy 6 parametrów zewn¦trznych: Tx, Ty, Tz, θx, θy, θz. Pierwsze trzy wspóªczynniki wchodz¡ w skªad wektora pozycji, natomiast pozostaªe trzy okre±laj¡ orientacj¦ w postaci o±-k¡t i mog¡ by¢ przeliczone do macierzy obrotu przy u»yciu formuªy Rodrigues'a. Na podstawie tych 6 wspóªczynni-ków mo»na wyznaczy¢ macierze rotacji, pozwalaj¡ce na rektykacj¦ obrazów.

Najpierw ka»dy obraz obracany jest w kierunku drugiego obrazu o poªow¦

obrotu danego przez wspóªczynniki θx, θy, θz. Je±li wi¦c jako Rod() oznaczy-my formuª¦ Rodrigues'a, to macierze obrotu lewego i prawego obrazu b¦d¡

nast¦puj¡ce:

R_cL = Rod([θ_x θ_y θ_z]^T/2), R_cR = Rod(−[θx θ_y θ_z]^T/2) . (5.5)

Zastosowanie tych macierzy obrotu sprowadzi obrazy do wspóªplanarnego poªo»enia. Nast¦pnie nale»y wykona¢ przeksztaªcenie zapewniaj¡ce wspóª-wierszowe poªo»enie obrazów. To przeksztaªcenie obraca oba obrazy w taki sposób, aby ich wiersze byªy równolegªe do odcinka ª¡cz¡cego pocz¡tki le-wego i prale-wego obrazu. O± obrotu mo»e by¢ zatem wyznaczona jako iloczyn wektorowy wektora le»¡cego wzdªu» wiersza obrazu oraz wektora t, ª¡cz¡cego pocz¡tki ukªadów wspóªrz¦dnych obu wspóªplanarnych obrazów (wyra»onych w ukªadzie wspóªrz¦dnych lewego wspóªplanarnego obrazu):

l = t× [−1 0 0]^T, (5.6) gdzie

t = R⁻¹_cL · [Tx T_y T_z]^T. (5.7) K¡t obrotu mo»na obliczy¢ korzystaj¡c z denicji iloczynu skalarnego tych dwóch wektorów:

Ostatecznie macierz obrotu zapewniaj¡cego wspóªwierszow¡ orientacj¦, liczy si¦ korzystaj¡c z formuªy Rodrigues'a, jako:

R_rect = Rod Macierz okre±laj¡c¡ zªo»enie dwóch obrotów, zapewniaj¡ce wspóªplanarne i wspóªwierszowe poªo»enie obrazów, liczy si¦ zatem jako iloczyn odpowied-nich macierzy:

R_L = R_rectR_cL, R_R = R_rectR_cR. (5.11) Punkt we wspóªrz¦dnych kamery pozbawionych dystorsji mo»e by¢ przenie-siony przy pomocy powy»szych macierzy do wspóªrz¦dnych rektykowanych x_rp, yrp, zrp.

W przypadku lewego obrazu korzysta si¦ z macierzy RL:

[x_rp y_rp z_rp]^T = R_L· [xd y_d 1]^T, (5.12) natomiast w przypadku obrazu prawego  macierzy RR:

[x_rp y_rp z_rp]^T = R_R· [xd y_d 1]^T. (5.13) Ten punkt jest nast¦pnie rzutowany na pªaszczyzn¦ obrazow¡, a zrzutowane wspóªrz¦dne przeksztaªcane s¡ do wspóªrz¦dnych obrazowych obrazu

rekty-kowanego: [

gdzie xr = ^x_z^rp

rp i yr = ^y_z^rp

rp. Parametry wewn¦trzne dla obrazu rektykowanego:

f_rx, fry, crx, cry s¡ dobierane arbitralnie aby zmaksymalizowa¢ wspólne pole widzenia dla lewego i prawego rektykowango obrazu. Najcz¦±ciej wybiera si¦ frx = f_ry [17].

Teoretycznie przebieg procesu rektykacji powinien obejmowa¢ usuni¦cie znieksztaªce«, a nast¦pnie obrót obrazu do postaci rektykowanej. W prak-tyce jednak kolejno±¢ dziaªa« jest odwrotna. Jest to podyktowane skompliko-wan¡ form¡ równa« (5.2), okre±laj¡cych znieksztaªcenie wprowadzane przez obiektyw, powoduj¡c¡, »e wyznaczenie zale»no±ci pozwalaj¡cej na usuni¦cie dystorsji jest trudne. Zamiast przeksztaªca¢ piksele obrazu znieksztaªconego do postaci rektykowanej, dla ka»dego piksela obrazu rektykowanego obli-cza si¦ zatem odpowiadaj¡ce mu wspóªrz¦dne na obrazie znieksztaªconym.

Na potrzeby analizy niepewno±ci nale»aªo jednak okre±li¢ zale»no±¢ usuwaj¡-c¡ dystorsj¦. W tym celu skorzystano z liniowego przybli»enia równa« (5.2).

Po sprowadzeniu ich do postaci uwikªanej:

0 = (1 + k1r²+ k2r⁴+ k3r⁶)x + 2p1xy + p2(r²+ 2x²)− xd = fx

W powy»szych równaniach wektor p zawiera warto±ci parametrów znieksztaª-cenia kamery, a tak»e wspóªrz¦dne punktu na obrazie z dystorsj¡ oraz na obrazie bez dystorsji. Oznacza to, »e dla ka»dego piksela linearyzacja musi

by¢ wykonywana osobno. Po wprowadzeniu nast¦puj¡cych oznacze«:

z ukªadu równa« (5.16) mo»na wyznaczy¢ wspóªrz¦dne na obrazie bez znie-ksztaªce« w funkcji wspóªrz¦dnych na obrazie znieksztaªconym:

[ x_u Poªo»enie punktu w przestrzeni trójwymiarowej uzale»nione jest od jego lokalizacji na obrazie rektykowanym, wyznaczonej warto±ci dysparycji oraz parametrów zewn¦trznych systemu stereowizyjnego. Jako pierwsza wyzna-czana jest wspóªrz¦dna z:

z_c = T_rx· fr

d . (5.19)

W powy»szym równaniu fr jest ogniskow¡ przyj¦t¡ dla rektykowanego ob-razu, d to warto±¢ dysparycji wyznaczona dla danego piksela, a Trx to odle-gªo±¢ pomi¦dzy ukªadami wspóªrz¦dnych rektykowanych obrazów. Dyspa-rycj¦ wyznacza si¦ jako ró»nic¦ poªo»enia w osi u na lewym i prawym obrazie:

d =|urR− urL| , (5.20)

natomiast Trx obliczane jest na podstawie wzajemnego poªo»enia pocz¡tków kamer, poddanego rektykacji:

W równaniu (5.21) Tx, Ty, Tz to parametry otrzymane z kalibracji systemu stereowizyjnego, a Rr dane jest wzorem (5.11). Po obliczeniu wspóªrz¦dnych punktu w osi zc mo»na wyznaczy¢ pozostaªe wspóªrz¦dne:

x_c= ^(vr−c_f ^xr)

r z_c, y_c= ^(ur−c_f ^yr)

r z_c. (5.22)

W powy»szym równaniu vr i ur to wspóªrz¦dne punktu na lewym rektyko-wanym obrazie, a fxr, fyr, cxr, cyr s¡ parametrami wewn¦trznymi przyj¦tymi dla lewego obrazu rektykowanego.

W zadaniu budowy mapy terenu dla robota krocz¡cego przydatne b¦dzie równie» wyznaczenie wysoko±ci (elewacji). Aby obliczy¢ wysoko±¢ punktu w ukªadzie wspóªrz¦dnych zwi¡zanym z map¡, nale»y zna¢ poªo»enie robota w ukªadzie mapy (otrzymywane dzi¦ki odometrii oraz przy u»yciu sensora orientacji IMU), a tak»e poªo»enie kamery w ukªadzie robota, otrzymywane z kalibracji (procedura kalibracji poªo»enia sensora w ukªadzie robota zo-staªa opisana szczegóªowo w rozdziale 6.3). Je±li wyrazi¢ poªo»enie robota w ukªadzie mapy przy pomocy kolejnych obrotów wokóª osi xm, ym, zm ukªa-du mapy, odpowiednio o k¡ty αr, βr, γr (rys. 5.2) oraz za pomoc¡ poªo»enia w ukªadzie mapy xpr, ypr, zpr, wówczas mo»na wyznaczy¢ macierz poªo»enia robota w ukªadzie mapy, w postaci:

mT_r =

gdzie Rotxm, Rotym, Rotzm oznaczaj¡ macierze obrotu wokóª odpowied-nich osi ukªadu mapy, a wspóªrz¦dne xpr, ypr, zpr okre±laj¡ poªo»enie ukªadu wspóªrz¦dnych robota w ukªadzie mapy. W analogiczny sposób mo»na

okre-±li¢ poªo»enie kamery w ukªadzie robota:

rT_c=

W powy»szym równaniu Rotxr, Rotyr, Rotzr oznaczaj¡ macierze obrotu wokóª odpowiednich osi ukªadu robota o k¡ty αc, βc, γc , natomiast xpc, ypc, z_pc okre±laj¡ poªo»enie kamery w ukªadzie robota.

Po wyznaczeniu poªo»enia kamery w ukªadzie mapy jako:

mT_c=^mT_r·^rT_c, (5.25) mo»liwe jest przeksztaªcenie zmierzonego punktu z ukªadu kamery do ukªadu mapy:

[xm ym zm 1]^T =^mTc· [xc yc zc 1]^T, (5.26) Elewacja jest w tym ukªadzie jednoznaczna z poªo»eniem punktu w osi zm:

e = z_m. (5.27)

z _m

y _m

a

c

b

ym yr

zr

α

xm

zr xr

xm

yr xr

y _m

x _m

Rysunek 5.2: Obroty ukªadu wspóªrz¦dnych robota wzgl¦dem ukªadu wspóª-rz¦dnych mapy