Chapter １

Transcription

1 Ground Floor Detecton and Ego-Moton Estmaton for Vsual Navgaton of Moble Robots by Young-Geun Km hess Advsor: Hal Km A HESS Submtted to the faculty of NHA UNVERSY n partal fulfllment of the requrements for the degree of MASER OF SCENCE he Graduate School of nformaton echnology and elecommuncaton February 24

2 Ground Floor Detecton and Ego-Moton Estmaton for Vsual Navgaton of Moble Robots by Young-Geun Km A thess submtted to the faculty of nha Unversty n partal fulfllment of the requrements for the degree of Master of Scence n the Graduate School of nformaton echnology and elecommuncaton ncheon, Korea February 24 Approved by Prof. Hal Km Major Advsor

3 hs certfes that the thess of Young Geun Km s approved. Char Referee Referee he Graduate School of nformaton echnology and elecommuncaton nha Unversty February 24

4 ABSRAC Ground Floor Detecton and Ego-Moton Estmaton for Vsual Navgaton of Moble Robots Young Geun Km he Graduate school of nformaton echnology and elecommuncaton nha Unversty hs paper addresses two coupled problems n vsual navgaton for moble robots operatng n unnown structured envronments: local map buldng and ncremental localzaton. Many tradtonal vson based methods can be dvded nto two major classes: Frst, stereo based methods produce a dense dsparty map wth two or more mages and construct the map nto a 3D map. Depth nformaton obtaned from the 3D map may provde 3D vsual cues such as paths or obstacles for navgaton but these methods may fal when partcular types of features are not supported n mages. Secondly, moton based methods compute optcal flows n mage sequences. hese technques are approprate for detectng nterestng objects of domnant motons but do not wor n some types of scenes such as walls. For moble robots n structured envronments, the ground floor s a very nterestng object because t represents a local map of movable paths ecept for other statc or dynamc objects. n spte of the mert, t s not easy to be detected by mage propertes, stereo based or moton based methods. Unle these approaches, the geometrc fact that other statc or dynamc

5 objects are placed on the ground floor perpendcularly can provde an effectve clue to separate the ground floor from scene mages. he purpose of ths thess s to propose ground floor detecton and ego moton estmaton algorthms to solve the two coupled problems. n order to detect the ground floor a geometrc fact observed n the scene and some assumptons about mages are eploted so that a plane normal can be an effectve clue to separate the ground floor from the scene. n order to compute plane normals n mages, two methods are proposed and combned together wth a desgned teratve refnement process so that the ground floor can be detected as accurate as possble although msmatched pont correspondences are obtaned. n order to fnd camera ego moton parameters, a plane, such as the ground floor, s used because the egomoton model does not carry 3D nformaton any more f plane nformaton s gven. Gven the mage two methods are developed whch are the nverse Jacoban method computng a least squared estmate derved from the teratve Newton Raphson formula based on mage Jacoban and the mage Gradent method computng an optmnmal ego moton parameters robustly so that t mnmzes computaton errors although naccurate and nosy mage moton vector are obtaned. he prelmnary eperments use synthetc data and real data to verfy that the proposed algorthms perform ground floor detecton and camera ego moton estmaton correctly and also to show the approprateness and the effectveness of the algorthms for vsual navgaton of moble robots. Key words: Ground floor detecton, plane normal computaton, mage moton estmaton, ego moton estmaton, vsual navgaton, moble robot.

6 국문요약 본논문은비전기반의자율주행로봇이미지의구조화된환경에서동작할때나타나는두가지결합된문제들, 국부환경지도작성과점진적인자기위치인식문제를고려한다. 이러한문제를해결하기위한전형적인비전기반의접근방법들은크게두가지, 스테레오기반의접근방법과모션기반의접근방법으로분류될수있다. 국부환경지도작성을위한스테레오기반의접근방법들은둘이상의영상으로부터조밀한차영상을구하고, 이를 3차원지도로복원한다. 이로부터얻어진깊이정보는주행가능한경로혹은장애물과같은주행시필요한정보로서제공된다. 하지만이러한방법들은구조화된환경의영상에서와같이특정한형태의특징들이잘나타나지않는경우에는조밀한차영상을얻을수없기때문에, 복원된 3차원지도의정확성이낮아지고깊이정보의신뢰도도떨어지게된다. 국부환경지도작성을위한모션기반의접근방법들은연속된영상으로부터움직임정보를추출하고눈에띄는움직임을갖는물체들을검출한다. 이러한방법들은전방에나타내는동적혹은정적의장애물을인식하는용도로는적합할수있지만, 벽과같이움직임이두드러지지않은물체들을검출하기는어렵다. 따라서, 이러한방법들은영상에서특정한형태의특징이잘나타나지않는구조화된환경에는적합하지않다. 구조화된환경에서동작하는로봇에게바닥면은매우흥미로운대상이다. 그이유는바닥면이정적혹은동적물체를제외한주행가능한경로를나타내기때문이다. 이러한장점에도불구하고바닥면영상에는모서리, 에지, 색상또는텍스처와같이특정한공통된특징들이나타나지않기때문에, 영상적특성이나스테레오방법혹은모션기반의방법들로는직접바닥면을검출하기가쉽지않다. 이러한접근방법들과는달리구조화된환경에서관찰될수있는정적혹은동적물체들이바닥면과수직하게놓여진다는기하학적사실은영상에서바닥면만을분리해낼수있는효과적인단서를제공할수있다.

7 본논문의목적은두가지문제를동시에해결하기위해바닥면을검출해서사용하는새로운방법을제안하는것이다. 바닥면검출을위해정적혹은동적물체들이바닥면에수직하게놓여진다는기하학적사실을활용한다. 그리고이러한사실을제안되는알고리즘에적용하기위해영상이작은영역들로분할되고각영역들은공간상에서하나의평면에해당되며적어도 3개이상의점들로구성되어하나의평면을정의할수있다고가정한다. 이러한기하학적사실과가정들은평면의수직벡터가환경영상에서바닥면을분리할수있는효과적인단서가될수있게한다. 제안된바닥면검출방법은연속된영상에서보여지는영상의움직임을검출하고영상을작은영역들로분할한다. 그리고각영역들에대해평면의수직벡터를계산한후반복적인정제과정을거쳐점진적으로바닥면영상을검출한다. 알고리즘개발을위해평면의수직벡터를계산하는두가지방법을제안한다. 첫번째방법은공간상의평면에의해야기되는투영기하학관계식을유도하여 3개의동일점들로부터수직벡터를직접계산해내는방법이다. 두번째방법은잘못정합된동일점들이존재하더라도계산시오류를최소화시킬수있도록최소의최적해를구하는방법이다. 이방법은영상분할기법을기반으로하는반복적인정제과정과혼합되어잘못된정합점들이존재하더라도강인하고정확하게바닥면영상만을분리할수있게한다. 제안된자기위치인식방법은바닥면과같은공간상의하나의평면을사용한다. 그이유는만약하나의평면에대한정보가제공된다면공간상의카메라의운동과영상의모션들사이의관계에는더이상 3차원공간정보를포함하지않게된다. 알고리즘개발을위해카메라의자기운동파라미터를구하는두가지방법들이제안된다. 첫번째방법은영상의모션과카메라의공간상에서의운동과의관계를나타내는영상자코비안을유도하고, 반복적으로해를구하는 Newton Raphson 공식을도입하여카메라의정확한자기운동파라미터의최소자승해를구하는영상의역자코비안에기반한방법이다. 두번째방법은잘못정합된동일점들이존재하더라도계산상의오류를최소화시킬수있도록최소의최적해를구하는방법으로서영상 v

8 Gradent에기반한방법이다. 이방법은첫번째제안된알고리즘에포함되어해를구하는데있어서빠른수렴속도를보인다. 제안된알고리즘들의정당성을입증하기위해인공적으로생성된데이터와실제영상을사용하여실험들을수행한다. 실험결과를통해비전기반의자율주행로봇에대해제안된알고리즘들의적합성과효율성을보인다. v

9 CONENS ABSRAC... 국문요약... CONENS...v LS OF FGURES... v LS OF ALGORHMS... NOMENCLAURE.... NRODUCON..... Motvaton Research Goal Outlne of the hess PRELMNARY Camera Model Normalzed Camera Model General Camera Model Camera Moton n 3 space mage Moton Estmaton mage Brghtness Constancy Equaton GROUND FLOOR DEECON Planar Homography Plane Normal Computaton Case : hree mage Pont Correspondences Case : hree or More Nosy Pont Correspondences Layered Ground Floor Segmentaton Optcal Flow Estmaton teratve Refnement Process Layered mage Representaton Epermental Results mage Moton Estmaton v

10 3.4.2 mage Segmentaton Plane Normal Computaton teratve Refnement Process Layered mage Representaton Comparson Summary CAMERA EGO MOON ESMAON mage Moton Model Dervaton of mage Jacoban Matr Ego Moton Model on the Ground Floor Ego Moton Estmaton Algorthms Dervaton of an nverse mage Jacoban Method Dervaton of an mage Gradent Method Epermental Results Synthetc Case Real Case Comparson and Summary CONCLUSONS AND FUURE WORKS BBLOGRAPHY v

11 LS OF FGURES Fgure.. A sample mage of an ndoor scene Fgure 2.. Projecton onto the normalzed mage plane π at f Fgure 2.2. Projecton onto an mage plane wth f Fgure 2.3. wo normalzed cameras n 3 psace... Fgure 2.4. Dfference between the mage moton and the optcal flow Fgure 3.. Planar homography nduced by a plane... 6 Fgure 3.2. he concept of a homography Fgure 3.3. Central projecton Fgure 3.4. Planar homography between two vews Fgure 3.5. A scene plane normal wth 3 mage ponts Fgure 3.6. Geometrc meanng of m Fgure 3.7. A scene plane normal wth n 3 nosy mage ponts Fgure 3.8. Framewor of the layered ground floor segmentaton Fgure 3.9. Mult scale coarse to fne estmaton Fgure 3.. Reference mages for two mage sequences Fgure 3.. mage moton estmaton Fgure 3.2. mage segmentaton Fgure 3.3. Plane normal computaton Fgure 3.4. teratve refnement process Fgure 3.5. ntermedate processes for the case of no obstacles Fgure 3.6. ntermedate processes for the case of statc obstacles Fgure 3.7. he foreground layers Fgure 3.8. Ground truth layers vs. estmated layers... 4 Fgure 4.. Projecton of 3D moton of onto the normalzed mage Fgure 4.2. Camera ego moton estmaton usng ground floor detecton... 5 Fgure 4.3. Bas and senstvtes on translatonal, angular veloctes and mage transfer error Fgure 4.4. Epermental results for pure translaton v

12 Fgure 4.5. Epermental results for pure rotaton Fgure 4.6. Epermental results for general moton... 73

13 LS OF ALGORHMS Algorthm 4.. Summary of the nverse mage Jacoban based algorthm..58 Algorthm 4.2. Summary of the mage gradent based algorthm

14 NOMENCLAURE o enhance the readablty the notatons used throughout the thess are summarzed here. For matrces and vectors bold face fonts are used. Scalar values wll be represented by talc fonts. {C} : camera coordnate systems {}: mage coordnate systems π : normalzed mage plane f : camera focal length X : scene pont n 3 space 3 vector X ~ : homogeneous coordnates of : mage pont of X X 4 vector n normalzed mage coordnates 3 vector proj p : projecton operator onto a plane p P : normalzed camera projecton matr 3 4 matr p : mage of X p ~ : homogenous coordnates of n pel coordnates 2 vector p 3 vector, : mage ponts n the st and 2 nd vews, respectvely 3 vector l, l : eppolar lnes for, n the st and 2 nd vews 3 vector e, e : eppoles of the st and 2 nd vews 3 vector c : camera center 3 vector K : camera calbraton matr 3 3 matr p, p y : prncple pont n mage coordnates m, m y : the number of pels per unt dstance n mage coordnates : homogeneous transformaton matr 4 4 matr E : spatal mage gradent 2 vector E t : temporal mage dfference H : homography matr 3 3 matr M, m : sub matrces of the camera projecton matr Π : scene plane n 3 space 4 vector

15 v : plane normal 3 vector * v : optmnmum plane normal v : estmate of the plane normal of a regon v : plane normal of the ground floor G θ : angle dfference L, L : the bacground and foreground layers α : alpha map defnng transparency of L X & : lnear velocty of a scene pont X 3 vector X & z : z as component of the lnear velocty t : translatonal velocty n 3 space 3 vector ω : angular velocty n 3 space 3 vector & : mage moton vector 3 vector & t : translatonal component of & & : rotatonal component of & r J : mage Jacoban matr at an mage pont 3 6 matr J. t : contrbuton of the relatve translaton velocty t 3 3 matr J. ω : contrbuton of the relatve angular velocty ω 3 3 matr Φ : camera ego moton vector 6 vector * Φ : desred camera ego moton vector Φ : current appromaton of δ Φ * Φ : dfferental camera ego moton vector 6 vector δ Φ : current appromaton of the dfferental camera ego moton vector : the unt vector along to the z as 3 vector J : accumulated mage Jacoban matr for 3 N 6 matr R N X & mage ponts J : current appromaton of the accumulated mage Jacoban matr v : accumulated mage moton vectors for N mage ponts 3 N vectors δ v : current appromaton of the accumulated mage moton vectors 3 N vectors δ &, : current appromaton of the dfferental mage moton vector H : current appromaton of the planar homography matr 3 3 matr

16 P : current appromaton of the camera projecton matr 3 4 matr R : rotaton matr 3 3 matr R : current appromaton of the rotaton matr δ R : dfferental rotaton matr 3 3 matr δ R : current appromaton of the dfferental rotaton matr t : current appromaton of the translaton vector 3 vector d : dfferental translatonal velocty 3 vector d : current appromaton of the dfferental translatonal velocty δ : dfferental angular velocty 3 vector δ : current appromaton of the dfferental angular velocty ε : an error functon : spatal mage gradent vector 3 vector, : current appromaton of the spatal mage gradent vector δ : temporal mage dfference δ, : current appromaton of the temporal mage dfference H : mage Hessan matr 6 6 matr H : current appromaton of the mage Hessan matr, m : mage msmatch vector 6 vector m, : current appromaton of the mage msmatch vector

17 macro 있음 까지 : 지우지말것 v

18 CHAPER. NRODUCON.. Motvaton n vsual navgaton, the purpose of local map buldng s to solve the problem of Where should go? and the coupled problem s localzaton often mentoned by the queston of Where am? Many tradtonal vson based methods can be classfed nto two major classes: Frst, stereo based methods produce sparse or dense dsparty maps wth two or more mages and they are constructed nto 3D maps. Depth nformaton obtaned from 3D maps may provde 3D vsual cues such as paths or obstacles for navgaton [4,23]. And these methods are also used n ncremental localzaton by detectng specfc features such as corners or edges, called natural landmars and tracng them n consecutve mages. But they may fal when partcular types of features are not supported n mages. n ths case, the accuracy of reconstructed 3D maps s decreased and the confdence of the map s not also guaranteed. Secondly, moton based methods compute optcal flows called the mage moton feld n consecutve mages. Moton nformaton s used for detectng nterestng objects of domnant motons [9,2] and mtatng the centerng behavors of honeybees wthn walls [8] and also tme to contact estmaton for navgaton [3]. hese methods are approprate for detectng other statc or movng objects but they do not wor n some types of scenes such as walls havng no domnant motons. For moble robots n structured envronments, the ground floor s a very nterestng object because t represents a local map of movable paths

19 ecept for other statc or dynamc objects. n spte of the mert, t s not easy to be detected by mage propertes because common types of features, such as corners, edges, colors or tetures, are not supported n ground floor mages. n stereo approaches t cannot be found drectly from 3D maps because of dfferent depth. n moton approaches t s also dffcult because of the moton s not domnant aganst the surroundngs. Unle these approaches, a geometrc fact that other statc or dynamc objects are placed on the ground floor perpendcularly can provde a clue to fnd the ground floor n mages..2. Research Goal hs paper consders vsual navgaton n structured envronments such as ndoor scenes and focus on a method to detect obstacles and paths wthn mages smultaneously. Fgure. llustrates the concept wth an mage of an ndoor scene that contans statc objects such as walls, dess and a boo on the floor. he ey dea n the fgure s to detect statc objects on the ground floor as obstacles Fgure.b and to detect the ground floor as a movable path that robots can navgate Fgure.c. n order to detect the ground floor n mages, ths paper eplots the geometrc fact that other statc or dynamc objects are placed on the ground floor perpendcularly. n order to use the fact n the proposed algorthm to detect the ground floor, t s assumed that an mage conssts of small patches and each small patch corresponds to a plane n 3 space so that t can defne a plane wth at least three mage ponts. t s because at least three pont correspondences n two or more mages can defne a plane n 3 space [7]. Such the geometrc fact and assumptons provde the followng advantages: Plane normals can be an effectve clue to separate the ground floor from the scene; o compute a plane normal for all mage patches can ncrease the precson of the detected ground floor mage. But 2

20 ths approach contans the followng problems: he frst, nown as correspondence problem, s determnng whch pont n the frst mage corresponds to whch pont n the second mage [22] and the second determnng the shape and sze of a patch so that a patch corresponds to a flat surface n 3 space. n order to solve the problems and satsfy the above geometrc fact and assumptons, the followng methods are proposed.. Compute mage moton feld from consecutve mages to obtan dense mage pont correspondences nstead of usng stereo mages provdng only sparse pont correspondences because a patch should be have at least three mage pont correspondences although t s small. 2. Adopt mult scale coarse to fne estmaton algorthm, such as Lucas Kanade estmaton algorthm, so that mage pont correspondences should be accurate as possble. a an ndoor scene b obstacles Fgure.. A sample mage of an ndoor scene. c the ground floor 3

21 3. Splt an mage nto sub regons as small as possble by usng mage splttng technques based on color homogenety so that each regon s close to a plane n 3 space. 4. Derve an optmnmal estmate of the plane normal for a patch so that computaton errors are mnmzed although msmatched mage pont correspondences are obtaned. 5. Desgn an teratve refnement process based on regon growng and mergng to detect and segment the only ground floor wthn an mage. 6. Represent the segmented mage wth two layers n a proper form for vsual navgaton. General ego moton model s represented by a functon of mage moton vectors and 3D nformaton, but t cannot carry 3D nformaton any more f a plane s nown, such as the ground floor. n order to develop egomoton estmaton algorthms, ths paper uses plane nformaton provded n the ground floor detecton algorthm. Gven the ground floor mages and the mage moton vectors on the ground floor the ego moton model can be computed by at least three mage moton vectors. n ths paper, the followng methods are proposed.. he frst proposed algorthm s the nverse Jacoban method whch s derved from the teratve Newton Raphson formula based on mage Jacoban and a least squared soluton of the ego moton parameters s computed. 2. he second proposed algorthm s the mage Gradent method computng an optmnmal weghted least squared estmate to determne the ego moton parameters robustly so that t mnmzes computaton errors although naccurate and nosy mage moton vector are obtaned. 4

22 .3. Outlne of the hess he remander of the paper s organzed as follows: Chapter 2 ntroduces some termnologes and basc concepts for developng the proposed ground floor detecton and ego moton estmaton algorthms. Chapter 3 presents the proposed ground floor detecton algorthm n more detal, shows the epermental results wth two real scenes and compares them wth the ground truth data. Chapter 4 presents the proposed ego moton estmaton algorthm, shows the smulaton and real epermental results. Chapter 5 concludes the thess wth the valdty for vsual navgaton of moble robots, and the effectveness of the proposed algorthms. 5

23 CHAPER 2 2. PRELMNARY hs chapter ntroduces some termnologes and basc concepts for developng the ground floor detecton and ego moton estmaton algorthms. Frst, vrtual mage plane and normalzed camera model s presented. Second, basc concepts for mage moton estmaton are ntroduced brefly. 2.. Camera Model hs secton descrbes the mathematcal model of projectve cameras, whch represents a mappng between the 3D scene space and a 2D mage plane. t can be consdered as the central projecton of ponts n space onto a plane. Although the model has been derved by a number of authors [2,7,22,25], t s rewrtten n an approprate form to develop the proposed algorthms, whch s consdered n the normalzed mage plane. 2.. Normalzed Camera Model he basc pnhole camera model s consdered as the central projecton of ponts n space onto a plane. Fgure 2. shows a vrtual mage plane π at a focal length f n the camera coordnate {C}. Let X X, Y, Z be a scene pont n 3 space and, y, ts correspondng pont n the vrtual mage plane π. hen the central projecton of a scene pont X n the vrtual mage plane s defned by 6

24 proj X 2. π where the term proj p denotes a projecton operator whch project a scene pont onto a plane p and the mage pont n the mage plane π s gven by X y Z Y. 2.2 f the world and mage ponts are represented by usng homogeneous coordnates, then the central projecton s epressed as a matr vector form and the above equaton may be wrtten as s sy s X Y Z 2.3 where s s a scale factor between the homogeneous coordnates and nonhomogeneous coordnates. f X X, Y, Z, represents the ~ homogeneous coordnates of X, the above equaton becomes a compact form as X z c f {C } c π y c c Fgure 2.. Projecton onto the normalzed mage plane π at f. 7

25 where the matr P [ ] the vrtual mage plane ~ s PX 2.4 s a camera projecton matr wth respect to π at a focal length f. hs s called the normalzed camera projecton matr because t does not carry camera nformaton. he center of projecton s called the camera center. he lne from the camera center perpendcular to the mage plane s called the prncple as and the pont where the prncple as meets the mage plane s called the prncple pont General Camera Model Now consder a real mage plane not a vrtual mage plane. Fgure 2.2 shows an mage pont onto a real mage plane p u, v that s projecton of a scene pont at a focal length f. By consderng the focal length, Eq. 2.2 s wrtten as X X z c p f {C} c v u π y c c Fgure 2.2. Projecton onto an mage plane wth f. 8

26 Y X Z f v u 2.5 and Eq. 2.3 becomes. 2.6 Z Y X f f s sv su f the mage pont s represented by the homogeneous coordnate p,, ~ v u p, then Eq. 2.4 has the concse form PX p ~,, ~ f f dag s. 2.7 n the above equaton, the focal length s represented as a dstnct term from the normalzed camera projecton matr because the focal length has dfferent values accordng to dfferent camera. n the case of CCD cameras, there are addtonal parameters related to the specfcaton of cameras. he pnhole camera model just derved assumes that the orgn of the mage coordnates s located at the prncple pont, but the mage pont has an offset correspondng to the prncple pont. f denotes the poston of the prncple pont n the mage coordnate, Eq. 2.6 becomes p, y p p. 2.8 Z Y X p f p f s sv su y he mage coordnates s measured n pel unt. hus converson factors between pel and world coordnates must be consdered. f the 9

27 number of pels per unt dstance n mage coordnates are n the and m and m y drectons, then the transformaton from world coordnates to pel coordnates s obtaned by multplyng on the left of the above equaton: y dag m, m y, su fm sv s fm y p p y m m y X Y Z 2.9 and the above equaton s smply p ~ ~ KPX s 2. where fm K fm y p p y m m y α α y y. 2. he term K s a matr contanng nternal parameters dependng on camera specfcatons, called the camera calbraton matr and the terms α, α y, and y are represented n pel coordnates. he converson from mage coordnates to the normalzed mage coordnates s gven by K ~ p. 2.2 he above equaton can be useful to elmnate the effect of camera nternal parameters when detectng the ground floor and estmatng the ego moton.

28 2..3 Camera Moton n 3 space Camera moton translaton and rotaton n 3 space changes the form of the camera projecton matr and t can be obtaned by usng two cameras wth dfferent postons and orentatons. Fgure 2.3 shows two normalzed cameras. Suppose that two frames { } and { } are the coordnates of the frst and second cameras wth the normalzed camera projecton matrces P [ ] and P [ ], respectvely. f the poston of a scene pont s represented by X and X wth respect to each frame { } and { }, the correspondng mage ponts are gven by s s P P ~ ~ X [ ] X ~ ~. 2.3 X [ ] X X he relaton between X and X s represented by 4 4 homogeneous transformaton matr consstng of rotaton and translaton between two coordnates: X c z π π z c { } { } y R,t y Fgure 2.3. wo normalzed cameras n 3-psace.

29 ~ X R ~ X t ~ X 2.4 where s 4 4 homogeneous transformaton matr whch represents the second frame { } wth respect to the frst frame }, the matr represents the orentaton of the second frame wth respect to the frst frame } and t s the poston of the second frame { } epressed n the frst frame {}. By substtutng the above equaton n Eq. 2.3, the central projecton of X n the frst frame {} wth respect to the second frame { {} s {} { R s [ ] [ R ~ X R t ] ~. 2.5 X Snce the above equaton s represented n the same coordnates {}, the subscrpt can be removed. he 3D moton of a camera may be wrtten wth the above equaton: ~ ~ s P X [ R R t] X 2.6 where the matr P s the normalzed camera projecton matr and the pont s the normalzed mage pont of a scene pont X when a camera s rotated and translated by R and t mage Moton Estmaton mage moton feld s defned as the projecton of the 3D velocty feld on the mage plane n vew of the vew geometry and t s also defned as the 2D vector feld of the mage correspondences n mage sequences, nduced 2

30 by the relatve moton between the camera and the scene. mage moton estmaton s to determne the moton feld observed n the mage plane, but the mage moton feld cannot really be observed. nstead, the spatal and temporal varatons of the mage brghtness can be estmated, called the optcal flow feld. he optcal flow feld s an appromaton of the mage moton feld, but they are not same. Fgure 2.4 shows the dfference between them. Consder a smooth, lambertan and unform sphere rotatng around a dameter n front of a camera as shown n Fgure 2.4a. n ths case, the mage moton feld s not zero because the ponts on the sphere are movng, but the optcal flow feld s zero because there are not any movng patterns n the mages. Consder a stll, smooth, specular and unform sphere n front of a camera and a movng lght source as shown n Fgure 2.4b. n ths case, the mage moton feld s zero snce the ponts on the sphere are not movng, but the optcal flow feld s not zero snce there are movng patterns n the mages. n order to estmate the optcal flow feld, t s bascally assumed that 3D mage a a rotatng sphere 3D mage mage2 b a stll sphere and a movng lght source Fgure 2.4. Dfference between the mage moton and the optcal flow. 3

31 the brghtness of movng objects reman constant, called the mage brghtness constancy assumpton [22]. And methods to obtan the optcal flow are classfed nto gradent based methods and flter based methods. Gradents based methods compute optcal flow from spatal and temporal dervatves of mage ntensty [9,2,5] Flter based methods computes optcal flow n the frequency doman [26,8,6,5]. For obtanng accurate optcal flow felds, mult scale, coarse to fne, refnement technques are also used wth them mage Brghtness Constancy Equaton mage brghtness constancy equaton s a fundamental equaton to obtan optcal flow n the gradent based methods. Consder a movng pont n 3 space. Let P X t, Y t, Z t be a movng scene pont at tme t and p t, y t be ts mage pont at tme t. f E t, y t, t represents the brghtness at p at tme t, the brghtness constancy assumpton s wrtten as de dt 2.7 and the total temporal dervatve may be wrtten by the chan rule of dfferentaton: de dt E d dt E + y dy dt E + t. 2.8 Denote E E E 2.9 y 4

32 s the spatal mage gradent, d dy v dt dt 2.2 s the optcal flow and E t E 2.2 t s the temporal mage dfference. hen Eq. 2.8 becomes a compact form E v + Et Methods to obtan the optcal flow v can be dvded two major classes: dfferental technques and matchng technques. Dfferental technques use least squared or weght least squared estmatons derved from the brghtness constancy equaton based on mage brghtness constancy assumpton and these methods assumes that the optcal flow v s well appromated by a constant vector wthn any small regon of the mage plane [22]. Matchng technques use correlaton or bloc matchng methods n whch each small patch of the frst frame s compared wth nearby patches n the net frame [2]. n matchng technques, two ey components must be consdered: accuracy and robustness. Accuracy s related to the local sub pel accuracy and robustness s related to senstvty wth respect to lght change, sze of mage moton, etc. Small wndow s preferable at occludng areas and t doesn t smooth out mage values but t cannot handle large motons. hus t s requred a tradeoff between local accuracy and robustness when selectng wndow sze. As a soluton to the problem, a pyramdal mplementaton can handle large motons wth small wndow. 5

33 CHAPER 3 3. GROUND FLOOR DEECON hs chapter proposes the ground floor detecton algorthm estmatng plane normals based on planar homography, shows the epermental results wth two real scenes and compares them wth the ground truth data. 3.. Planar Homography Consder a plane n a scene and suppose the plane s projected nto two vews. n ths case, the relaton between the mages projected from the scene plane s a projectve relaton, called planar homography nduced by a plane [7]. Fgure 3. llustrates the concept wth two mages. n the fgure the three regons between two mages correspond three dfferent planes n H H 2 M H n Fgure 3.. Planar homography nduced by a plane. 6

34 3 space and each regon has own planar homography nduced by ts correspondng scene plane. A homography s also called a projectvty, or a collneaton, or a projectve transformaton [7]. A homography shown n Fgure 3.2 s defned by an nvertble mappng h from a 2D projectve space 2 P to tself such that three ponts, and le on a lne l f and only f 2 3 h, h and h le on another lne 2 3 l. n other words, a mappng h s a homography f and only f there ests a non sngular 3 3 matr H represented by a vector such that for any pont n a 2D projectve space, t s true that h H. hus any nvertble lnear transformaton of homogeneous coordnates s a homography and a planar homography s a lnear transformaton on homogeneous 3 vectors represented by a non sngular 3 3 matr: 2 P H ' h y' h h 4 7 h h h h3 h y hs planar projectve transformaton s smply a lnear transformaton of R 3,, and t has 8 degree of freedom. Whle, shown n Fgure 3.3, 3 lne-to-lne mappng 2 2 P ' P l l ' h ' 2 2 h 2 ' h 3 3 pont-to-pont mappng Fgure 3.2. he concept of a homography. 7

35 consecutve mage planes has central projecton mappng ponts to ponts and also lnes to lnes and t s called a central projectvty or a perspectvty. t has 6 degree of freedom. hus a planar projectve transformaton can be specfed by four pont correspondences, or by two correspondng lnes. Whle a perspectvty can be specfed by three mage pont correspondences, or by one mage pont correspondence and one correspondng lne Plane Normal Computaton Ground floor s a very nterestng object for moble robot navgaton n structure envronments, whch represents movable paths ecept for other statc or dynamc objects. Snce such objects are on the ground floor perpendcularly, plane normals can be a clue to separate the ground floor from the scene. Consder a plane n a scene and suppose the plane s projected onto two vews. hen the relaton between two mages whch s projecton of the ' l ' l ' π π ' H Fgure 3.3. Central projecton. 8

36 scene plane s a planar homography nduced by a plane. Fgure 3.4 shows a scene pont X on a scene plane Π n 3 space and ts two normalzed mage ponts and whch are projected onto the normalzed mage planes π and π. Let P [ ] and P [ M m] be the frst and second normalzed camera projecton matrces. Usng the normalzed pnhole camera model, the mage ponts and are ~ ~ s PX [ ] X ~ ~ s P X [ M m] X 3.2 where the matrces M and m are sub matrces of the second camera projecton matr n a canoncal form. For the frst vew π, the scene pont X s on the ray passng through ts mage pont and the camera center c : X λ λ c + λ 3.3 X Π {C} c y c z c π c H R,t π ' ' z c ' c' {C'} ' y c ' c Fgure 3.4. Planar homography between two vews. 9

37 Snce the locaton of the frst camera center c n the frst vew s c P ~ c [ ] 3.4 the camera center c s gven by c. 3.5 From Eq. 3.3 the scene pont X becomes X λ λ c + λ λ 3.6 and t can be wrtten n homogeneous coordnates ~ X. 3.7 / λ f the scene plane Π s represented by scene pont X on the scene plane Π satsfes Π [ n d ] n 3 space, the ~ Π X v / λ 3.8 where the vector v s the plane normal of the scene plane Π and t s parameterzed by n / d. hus the scalar value s gven by λ v 3.9 and the scene pont X may be rewrtten as ~ X v. 3. 2

38 For the second vew π, the normalzed mage pont from Eq. 3.2 and 3.: s obtaned ~ s P X [ M m] v. 3. M mv Snce the relaton between two mage ponts and s a planar homography as n Eq. 3. and the homography matr s defned up to scale, the 3 3 homography matr H H M mv becomes. 3.2 he above equaton represents that the homography matr can be specfed wth the matr M and two vectors m, v Case : hree mage Pont Correspondences A scene n 3 space can be specfed by three mage pont correspondences. Refer to Fgure 3.5. Suppose that three mage pont correspondences are gven between two vews. hen the homography nduced by the plane of the three mage ponts s π π H for, L,3 where,. 3.3 Snce the left and rght terms should be zero: and H are parallel ther cross product H M mv

39 n order to compute the plane normal, mang a lnear equaton wth respect to v yelds m v M 3.5 and t s wrtten n a compact form as v b 3.6 where b m M. 3.7 m m Summng up the above equaton for three mage pont correspondences yelds a lnear equaton wth respect to the plane normal: X X 2 Π X 3 {C} c y c 2 z c 3 c π H R,t ' 2 ' z c π ' ' ' 3 c' {C'} ' y c ' c Fgure 3.5. A scene plane normal wth 3 mage ponts. 22

40 2 3 b v b2 b3 Av b. 3.8 hus the plane normal pont correspondences. v Note that f the matr be obtaned because three mage ponts the plane normal correspondences. v wo mage ponts can be computed drectly from three mage A s not of full ran, a plane normal cannot are collnear. he accuracy of depends on the accuracy of the three mage pont and for two vews are assumed to be n the normalzed mage coordnates because the camera projecton matrces are P [ ] and P [ M m]. But t s necessary to consder ther pel coordnates for plane normal computaton. f two mage ponts and are represented n pel coordnates, ther normalzed mage coordnates K : and are obtaned by usng camera calbraton matrces K and p p K p ~ K p ~ 3.9 Geometrc meanng of m Let e and e be the eppoles of the frst and second vews as shown n Fgure 3.6. he eppole e of the second vew s defned by an mage pont that s projecton of the frst camera center c onto the second vew: e P ~ c

41 Snce the normalzed projecton matr P of the second camera s P [ M m] above equaton becomes and the center of the frst camera s zero from Eq. 3.5, the e m. 3.2 he above equaton means that the rght most column vector second camera projecton matr corresponds to the eppole second camera. hus the term m may be wrtten as m e of the of the m e l 3.22 where the lne l s the eppolar lne of the mage pont n the second vew. Eq. 3.7 s rewrtten as b M l / l X Π {C} c z c e π e ' π ' ' z c ' ' c c' {C'} y c c l l ' ' y c Fgure 3.6. Geometrc meanng of m. 24

42 3.2.2 Case : hree or More Nosy Pont Correspondences he accuracy of the plane normal depends on the accuracy of computng the correspondng mage pont n another mage for a pont n the reference mage. Although the mult scale coarse to fne estmaton algorthm produces accurate and dense mage pont correspondences, lessmatched mage ponts may est n case of large moton vectors and msmatched mage ponts may also est n case of the apparent brghtness changes are not observed n mages. For these cases t s necessary to compute an optmnmal plane normal whch mnmzes computaton error. Assume that a regon R s the sub mage that s projecton of a scene plane n 3 space as shown n Fgure 3.7. An approprate error functon s the sum of squared dfference of the resdue for a plane normal v n Eq.3.6 for all pels n that regon R. v X 2 X 2 X {C} c y c 2 R z c 3 c π Π ' 2 ' z c π ' X 3 ' ' 3 c' {C'} ' y c ' c Fgure 3.7. A scene plane normal wth n 3 nosy mage ponts. 25

43 R b e v v At the optmnmum plane normal, the partal dervatves of wth respect to should be zero: * v e v * vv v v e After epanson of the dervatves, Eq becomes R R b e v v v Denote R G 3.27 R b b hen Eq s wrtten as 2 * * b Gv v v v v e herefore the optmnmum plane normal s gven by * v b. 3.3 G v * Note that the optmal estmate of a plane normal whch mnmzes the error s a least squared soluton and t s vald when the matr s nvertble. G 26

44 3.3. Layered Ground Floor Segmentaton hs secton descrbes the framewor to segment the ground floor n mage sequences. As mentoned earler, t s eploted the fact that other statc or movng objects are on the ground floor and they are perpendcular to the ground. Also t s assumed that an mage conssts of small patches or regons and each corresponds to a plane n 3 space and has at least three mage ponts so that t can defne a plane. t s because at least three pont correspondences n two or more mages can defne a plane n 3 space. Such a fact and assumptons provde the followng advantages: Plane normals can be an effectve clue to separate the ground floor from the scene; o compute a plane normal for all mage ponts n a patch ncreases ts computatonal accuracy. But ths approach contans the followng problems: he frst, nown as correspondence problem, s determnng whch pont n the frst mage corresponds to whch pont n the second mage [23]. he second problem s determnng the shape and sze of a patch so that a patch corresponds to a flat regon n 3 space. n order to solve the problems and satsfy the above fact and assumptons, the followng method s suggested: A patch should have at least three pont correspondences although t s small. hus mage moton feld s computed to produce dense pont correspondences nstead of usng stereo mages provdng only sparse pont correspondences; Pont correspondences should be accurate as possble to provde accurate plane normals. hus the mult scale coarse to fne estmaton s adopted, such as Lucas Kanade optcal flow estmaton [2]; Each patch should be closest to a plane n 3 space. hus an mage s splt nto sub regons as small as possble by usng mage splttng technques based on color homogenety [7]; v Estmated plane normals should be optmnmal wth respect to pont correspondence errors. hus the optmal estmate of the plane normal, whch s just derved n the prevous secton, s used so that the estmaton 27

45 error s mnmzed although msmatched mage pont correspondences are obtaned, v he patches correspondng to the ground floor should be refned wthn mages. hus an teratve refnement process s desgned to detect and segment the ground floor by usng regon growng and mergng technques, v he ground floor mage should be represented n a proper form for vsual navgaton. hus the segmented mages s represented wth two layers [24,]. he proposed algorthm conssts of three stages as shown n Fgure 3.8:. Optcal flow estmaton and mage segmentaton: Compute optcal flow to obtan accurate and dense mage pont correspondences n consecutve mages by usng mult scale coarse to fne estmaton. Splt mages nto small regons. 2. teratve refnement process: Select a seed regon and grow t to connected regons usng a Queue structure. For each regon, estmate the plane normal and merge t nto the ground floor f t s close to the ground plane normal. 3. wo layered representaton: Decompose the mage nto the foreground layer and the bacground layer. mage sequence optcal flow estmaton plane normal estmaton regon classfcaton ground floor mage regon splttng regon growng regon mergng Fgure 3.8. Framewor of the layered ground floor segmentaton. 28

46 3.3. Optcal Flow Estmaton he performance of detectng the ground floor s manly affected by both accuracy and densty of mage pont correspondences. n stereo approaches accurate pont correspondences can be only obtaned n the partcular types of features such as corners, edges, etc. Snce large dsparty may produce many msmatched ponts t s not guaranteed that patches should be have at least three mage pont correspondence although t s small. n moton approaches, accurate and dense pont correspondences can be obtaned wth mult scale coarse to fne estmaton algorthms based on gradent approaches [2,]. radtonal algorthms produce only moton vectors, nown as optcal flows, whch represent the drectons to the correspondng ponts n the net mage. But the vector can represent the locaton of the correspondng ponts n the net mage usng mult scale coarse to fne estmaton algorthms because they can handle large motons. Fgure 3.9 shows the mult scale coarse to fne moton estmaton algorthm based on the Gaussan pyramd usng the Lucas Kanade equaton [2]. Estmate optcal flow at each pel usng the L-K equaton warp & upsamplng Estmate optcal flow at each pel usng the L-K equaton Repeat untl converge Gaussan pyramd Fgure 3.9. Mult-scale coarse-to-fne estmaton. 29

47 3.3.2 teratve Refnement Process n the teratve refnement process, regon growng and mergng rules are used wth a classfer, that s, angle dfference between an estmated plane normal of a regon and the plane normal of the ground floor. he classfer decdes whether a plane normal s smlar to the ground plane normal: θ v, v cos v v / v v 3.3 G G G where v s the estmate of the plane normal for a regon R and v s the plane normal of the ground floor. he mergng rule s that a regon regon R G s merged wth the ground floor f the angle dfference s smaller than a threshold value: R G R G R R θ v, v <τ G G f 3.32 where τ s a threshold for angle dfference. n the growng rule, when a regon s merged nto the ground floor regon R, new regons connected wth R are found. hs process G s repeated usng a Queue structure untl t converges. After the teratve refnement usng regon growng and mergng va classfcaton, an mage s dvded nto the ground floor and the rest. R 3

48 3.3.3 Layered mage Representaton n order to represent the segmented mages n a proper form for vsual navgaton, we decompose the mage nto two layers, descrbed by []. he frst layer s the foreground layer representng the ground floor as a path that robots can navgate and the second layer s the bacground layer representng other statc or movng objects as obstacles:, y L, y α, y + L, y α, 3.33 y where and L are bacground and foreground layers, respectvely and L α s an alpha map defnng transparency of. L 3

49 3.4. Epermental Results hs secton descrbes the epermental results for two mage sequences wthout obstacles and wth statc obstacles, respectvely, as shown n Fgure 3.. Each mage corresponds to the frst frame of two mage sequences when a robot moves forward on the ground floor. a no obstacles b statc obstacles Fgure 3.. Reference mages for two mage sequences. 32

50 3.4. mage Moton Estmaton Fgure 3. shows the moton feld that was estmated for all pels, but appeared at every 5 pels. For mage moton estmaton three level Gaussan pyramds and coarse to fne algorthms were used. Note that any nd of domnant moton does not appear and wrong motons appeared n the fgures. a no obstacles b statc obstacles Fgure 3.. mage moton estmaton. 33

51 3.4.2 mage Segmentaton Fgure 3.2 shows the mage segmentaton result n whch the mage was splt nto a number of regons as small as possble to satsfy our assumptons. Almost 45 regons were detected and each regon was represented by a non overlapped unque color for vsualzaton. Note that the ground floor mage s splt nto several regons a no obstacles regon D b statc obstacles regon D Fgure 3.2. mage segmentaton. 34

52 3.4.3 Plane Normal Computaton n order to show that the plane normal s an effectve measure to detect the ground floor, Fgure 3.3 shows the result of plane normal computaton wthout teratve refnement process. he rght color ndces represent the angle between the ground floor and a color regon. he blue colors ndcate the correspondng regons are close to the ground floor and the red colors ndcate the correspondng regons are perpendcular to the ground floor a no obstacles angle dff b statc obstacles angle dff. Fgure 3.3. Plane normal computaton. 35

53 3.4.4 teratve Refnement Process Fgure 3.4 shows the result of teratve refnement process to detect the only ground floor mage. he regon growng and mergng rules were used wth a classfer, that s, angle dfference between an estmated plane normal of a regon and the plane normal of the ground floor. Fgure 3.5 and Fgure 3.6 shows ntermedate processes to detect the ground floor. a no obstacles b statc obstacles Fgure 3.4. teratve refnement process. 36

54 a st teraton b 7 th teraton c th teraton d 4 th teraton e 5 th teraton f 88 th teraton Fgure 3.5. ntermedate processes for the case of no obstacles. 37

55 a st teraton b th teraton c 9 th teraton d 32 th teraton e 52 th teraton f 92 th teraton Fgure 3.6. ntermedate processes for the case of statc obstacles. 38

56 3.4.5 Layered mage Representaton Fgure 3.7 shows the foreground layers of layered mage representaton, whch correspond to the ground floor. Note that the only ground floor was segmented ecept for the other statc objects perpendcular to the ground n mages. a no obstacles b statc obstacles Fgure 3.7. he foreground layers. 39

57 3.4.6 Comparson Fgure 3.8 shows the ground truth layers and the estmated layers generated by the proposed algorthm. he ground truth layers were produced by usng mage processng pacages such as Adobe Photoshop or Pant Shop Pro. As we can see, the estmated foreground layer contaned the ground floor and the estmated bacground layer contaned the other statc objects such as walls, dess, a char, etc. and an statc obstacle on the floor. he wrong parts n each layer were caused by msmatched mage pont correspondences n moton estmaton and the mssng parts were due to mssng mage pont correspondences. a no obstacles b statc obstacles Fgure 3.8. Ground truth layers vs. estmated layers. 4

58 3.5. Summary he proposed ground floor detecton algorthm eploted the geometrc fact and the assumptons as mentoned n chapter. n order to specfy a plane n 3 space, planar homography nduced by a plane was consdered and two methods to compute plane normal were derved, drect estmaton wth three mage pont correspondences and optmnmal estmaton wth nosy three or more mage pont correspondences. As a result a robust plane normal estmaton algorthm that mnmzes the computaton error was developed although msmatched mage pont correspondences were obtaned. n order to show the valdty of the proposed algorthm to detect the ground floor, the mult scale coarse to fne estmaton method was adopted to obtan accurate and dense pont correspondences, the teratve refnement process was desgned to segment the ground floor from the scene and the layered mage representaton was used for descrbng the ground floor mage n a proper form for vsual navgaton. As a result, the proposed algorthm detected the only ground floor although msmatched mage pont correspondences were obtaned n real eperments. 4

59 CHAPER 4 4. CAMERA EGO MOON ESMAON hs chapter proposes two ego moton estmaton algorthms nduced by a plane n 3 space and verfes the performance by eperments on synthetc data and real scenes. 4.. mage Moton Model hs secton descrbes a mathematcal model of the mage moton feld whch represents mage changes observed n tme varyng mage sequences caused by the relatve 3D moton between a scene and the camera. An mage sequence s a seres of mages acqured at dscrete tme nstants wth fed tme ntervals. And the relatve 3D moton between a scene and a camera s caused by a movng camera n a statc scene, or movng objects n front of a fed camera, or both the camera and objects havng dfferent motons. hus, the mathematcal model of the mage moton feld represents a mappng between the 3D moton of a camera or a robot relatve to a scene, and t represents the mage changes observed n the magng sensor plane. Also t can be thought of as the projecton of 3D moton on the mage plane. Fgure 4. shows the projecton of the 3D moton of a scene pont to a normalzed mage plane. Let X [ X, Y, Z] be a scene pont n 3 space. he lnear velocty of the pont X s a functon of the translatonal and rotatonal veloctes of the pont relatve to the camera: X & t + ω X 4. 42

60 c & z c X & X c y c Fgure 4.. Projecton of 3D moton of onto the normalzed mage. where t t, t, t ] s the translatonal velocty of the pont X wth respect to the camera and pont. [ y z ω ω, ω, ω ] [ y z s the angular velocty of the he normalzed pnhole camera projects the scene pont normalzed mage plane at the focal length f as n Eq. 2.2: X onto the X 4.2 Z where [, y,] denotes the mage of the scene pont X. ang tme dervatve of the mage pont moton vector of the pont X : n the above equaton leads to the mage & 2 Z X& Z XZ&. 4.3 z By substtutng Eq. 4. n the above equaton and wrtng Z & as Z & X & to represent the z as component of the lnear velocty X &, the mage moton vector & becomes 43

61 & X& X X& 2 Z Z Z z Z t + ω X X t + ω X z and by wrtng X Z to elmnate X, the mathematcal model of the mage moton vector can be rewrtten as & t t z + ω ω z 4.5 Z Z where both z t t and ω ω y ω are scalars. he mage z z moton vector s called the mage moton feld. Eq. 4.5 dscloses two mportant propertes of the mage moton feld: Frst, the moton feld s a functon of the translatonal and angular veloctes of the camera wth respect to the scene and depth. And t s represented as the sum of two components, one of whch depends on the translatonal velocty and the other depends on the rotatonal velocty. hus the mage moton feld may be wrtten as & & t + & r 4.6 y where & t t t z 4.7 Z Z s the translatonal component and r ω z & ω 4.8 s the rotatonal component. Second, the rotatonal component does not depend on depth nformaton [22]. 44

62 4.2. Dervaton of mage Jacoban Matr he relatonshp between the 3D moton of a scene pont relatve to the camera and the mage moton feld observed n an magng sensor plane s represented by a matr, called the mage Jacoban, whch transforms the relatve 3D moton to the mage moton feld. he mage Jacoban matr s one of the mportant terms for mage moton analyss, whch s useful for estmatng the 3D moton of the camera or vsual servong. he mage Jacoban matr can be represented n a matr vector form from the mathematcal model of the mage moton feld whch s just derved. Let be the mage Jacoban matr at an mage pont and J Φ be a vector representng the relatve moton of the scene pont wth respect to the camera. hen the mage moton feld of the mage pont s a functon of the mage Jacoban matr and the relatve 3D moton of the scene pont: & J Φ 4.9 where the vector Φ [ t ω ] contans the relatve translatonal and angular veloctes of the scene pont X wth respect to the camera. he mage Jacoban matr can be parttoned nto two sub matrces representng the contrbuton of each velocty t and ω as & t ω [ J J ]. 4.. t. ω he term J. t represents the contrbuton of the relatve translatonal velocty t of the scene pont X wth respect to the camera to the mage moton at an mage pont and the term represents the J. ω contrbuton of the relatve angular velocty ω between them to the mage moton. 45

63 he contrbuton of each velocty to the mage moton can be computed by settng the other component to zero n Eq. 4.5 and 4.. he contrbuton of the translatonal velocty t to the mage J. t moton can be computed by settng ω : & t Z Z J t, t t z. 4. Let be the unt vector along to the z as. he contrbuton of the translatonal velocty t J. t can be obtaned n a matr vector form as t Z Z t Z t t J, t o t t. 4.2 hen the contrbuton becomes J, t Z. 4.3 Equvalently, the contrbuton of the angular velocty ω to the J. ω mage moton can be computed by settng ω t : & ω o. 4.4 J ω, ω Snce the term ω o o ω and dot products, by the relatonshp between cross J, ω ω ω ω 46

64 [ [ ] [ ] ] [[] [] ] ω [] [ ]ω ω 4.5 where the term [] the contrbuton becomes represents the matr form of the cross product. hen [ ] J, ω. 4.6 hus the mathematcal model of the mage moton feld can be wrtten as & J Φ [ J J ], t [] Z, ω t. 4.7 ω t ω 4.3. Ego Moton Model on the Ground Floor he mage moton feld of an mage pont n Eq. 4.7 s a functon of the translatonal and angular veloctes of the scene pont X wth respect to the camera and the depth Z between the scene pont and the camera: & f t, ω, Z. 4.8 f the depth Z s nown the mage moton feld becomes a functon of the only relatve motons t and ω : & f t, ω

65 and the relatve 3D moton wth respect to the camera can be recovered from the mage moton feld: t, ω f &. 4.2 he ground floor detecton algorthm was proposed n the prevous chapter, whch can detect and segment the ground floor regon usng mage moton felds n consecutve mages. Snce the dstance from the camera to the ground floor s already nown, the relatve 3D moton wth respect to the camera can be recovered from the mage moton felds on the ground floor regon. Suppose that Π G [ n d] represents the ground floor n the camera coordnate. A scene pont X on the ground floor Π satsfes G Π G ~ X n X + d 4.2 where the term d represents the negatve dstance from the camera to the ground floor. Wrtng X as X Z from the basc pnhole camera model n Eq. 4.2 to get an epresson wth respect to the depth yelds Z n d n order to elmnate Z n Eq. 4.3, substtutng the above equaton n Eq. 4.3 yelds J, t Z n d hus the fnal mage moton feld model becomes a functon of the only translatonal and angular veloctes t and ω : 48

66 n t & [] 4.24 d ω for proj X where X Π G π. he above equaton represents the relatonshp between the relatve translatonal and angular veloctes of a scene pont on the ground floor and the mage moton feld of the correspondng mage pont. f a scene pont s on the ground floor, then the relatve 3D moton between the ground floor and the camera s caused by the camera moton. hus the above equaton has to be modfed so that the vectors t and ω represent the translatonal and angular veloctes of the camera relatve to the scene: n t & [] d ω he above equaton s called the camera ego moton model on the ground floor, whch s one of the most mportant terms for camera moton analyss and control. t represents the relaton between mage moton and the 3D moton of the camera relatve to a scene and provdes a method to recover the relatve 3D moton of the camera from mage moton felds wthout depth nformaton. hen the mage Jacoban matr may be wrtten as [ ] v [ ] J t transforms the camera moton to the mage moton feld on the ground floor, whch s called the mage Jacoban matr nduced by the ground floor Π and the vector v s the plane normal of the ground floor, G whch s parameterzed by n / d. 49

67 4.4. Ego Moton Estmaton Algorthms Ego moton estmaton s to determne the camera moton parameters gven the mage moton feld. he relaton between the mage moton observed n the magng sensor plane and the camera moton n 3 space s represented by the mage Jacoban matr. f the ground floor mage s gven the mage Jacoban matr does not carry depth nformaton as n Eq hs secton proposes two algorthms to fnd the camera ego moton parameters usng the ground floor mage provded n the ground floor detecton algorthm whch s derved n the prevous chapter. he frst s an nverse mage Jacoban method whch s to determne the camera egomoton parameters usng the derved mage Jacoban matr. t s derved from the Newton Raphson formula because Eq. 4.2 s not a well posed problem n whch the functonal relatonshp between the camera moton parameters and mage moton s not one to one. And he planar homography s used for constructng recursve formulaton and for verfyng teratve solutons. Also t can be computed from at least three mage moton vectors. he second s an mage Gradent method whch s to determne the parameters robustly although naccurate and nosy mage moton vectors are obtaned, whch fnds an optmnmal estmate so that t mnmzes computaton errors. Ground floor detecton Ego-moton estmaton ω t Fgure 4.2. Camera ego-moton estmaton usng ground floor detecton. 5

68 4.4. Dervaton of an nverse mage Jacoban Method Eq represents the relatonshp between the camera ego moton relatve to a scene and the mage moton feld observed n an magng sensor plane by the mage Jacoban matr. Snce t has s unnown camera ego moton parameters Φ,,,6 and the mamum ran of the 3 6 mage Jacoban matr Φ s two, the camera ego moton vector can be determned f three mage moton vectors are gven whch are not collnear. J Now suppose that three mage moton vectors & for,,3 are gven n the normalzed mage plane. From Eq. 4.9 t s possble to mae a set of lnear equatons wth respect to the camera moton vector Φ : JΦ v 4.27 where J J J J s the accumulated mage Jacoban matr and & v & & s the accumulated mage moton vector. hus the camera moton vector Φ can be obtaned by Φ J v

69 he above equaton dscloses an mportant property. f the functonal relatonshp between the camera moton vector and the mage moton vector s one to one then a unque soluton wll be est. But the same moton feld can be produced by two dfferent camera moton vectors [22]. t means that t s mpossble to recover unquely the camera moton vector from the mage moton feld alone. hus the above equaton s not a well posed problem and may have multple solutons. mage Jacoban based Newton Raphson Formula Newton Raphson formula s used for fndng a soluton recursvely f an ntal estmate for the desred soluton s nown. t uses the tangental lnes analytcally evaluated and may be etended to fnd comple solutons of smultaneous nonlnear equatons [3]. n ths paper an camera ego moton estmaton algorthm s derved usng the Newton Raphson formula. n the algorthm the derved mage Jacoban matr s used for fndng the dfferental camera moton vector and the planar homography s used for verfyng the solutons. * Let Φ be the desred camera ego moton vector and Φ be a current appromaton of the desred camera ego moton vector. f the currently appromated ego moton vector s close to the desred camera ego moton vector, then the desred camera ego moton vector s represented by the sum of the current appromaton and the dfferental camera ego moton: Φ * Φ + δφ 4.3 where d δ δ Φ d d δ δ d, y, z,, y, δ z,

70 s the current appromaton of the dfferental camera ego moton vector and the terms d and δ denotes the current appromatons of the dfferental angular and translatonal veloctes, respectvely. f the camera ego moton s small then the current dfferental camera ego moton vector can be obtaned from the dfferental mage moton vectors of mage ponts by usng the mage Jacoban matr as n Eq. 4.3: δφ J δv he term J represents the current appromaton of the accumulated mage Jacoban matr. For N mage ponts J J L J L J,, N, 4.34 where J [ ] v [ ],,,, 4.35 s the mage Jacoban matr of the currently appromated mage pont, obtaned from Eq he term δ v represents the current appromaton of the accumulated dfferental mage moton vector: δ v δ& L δ& L δ&,, N, he current appromaton of a dfferental mage moton vector δ &, may represent the dfference between the correspondng mage pont and the current appromaton of an mage pont: δ&,,

71 where denotes the th correspondng mage pont and, the current appromaton of the th mage pont. he current appromaton of an mage pont can be obtaned by the current appromaton of the planar homography f the mage motons are restrcted so that they are occurred wth respect to a plane n 3 space such as the ground floor. From Eq. 3.34, t s wrtten as, H 4.38 where H M m v s the current planar homography matr from Eq. 3.2 and the matrces M P and m n a canoncal form of are the sub matrces of the current camera projecton matr [ M m ] P. 4.4 Snce the camera projecton matr P for the net mage plane s represented by the rotaton and translaton of the camera wth respect to the reference mage coordnates n Eq. 2.6, the current camera projecton matr P for the net mage plane s wrtten as [ R R t ] P 4.4 where the terms R and t are the current appromatons of the rotaton matr and translaton vector n the reference mage coordnates. By substtutng Eq. 4.4 and 4.4 n Eq. 4.39, the current appromaton of the planar homography matr H can be obtaned by 54

72 H M R m v + t v he current appromaton of the rotaton matr can be computed from the prevous appromaton R : R δ 4.43 R R where the matr δ R denotes the prevous appromaton of the dfferental rotaton matr. f dfferental rotaton s small, the matr may be represented by the dfferental angular velocty δ n Eq. 4.32: δr R δ δ z, δ y, δ δ z,, δ δ y,, he above equaton s vald when dfferental angular velocty approaches zero and for angles less than. radans 5.7, then error s less than.2% for sne functons and.5% for cosne functons. But the epresson cannot be used n teratve algorthms because the errors accumulate [4]. n ths case the dfferental rotaton matr can be found usng Rodrguez s formula: snθ cosθ δ R [ ] [ ] + δ + δ θ θ where θ 4.46 δ s the sze of the current dfferental angular velocty. hs s called the 55

73 ncremental rotaton matr. he current appromaton of the translaton vector can be also represented n recursve formulaton whch s the product of the prevous appromaton translaton vector t and the prevous appromaton of the dfferental δ t : t δ t t Snce the dfferental translaton vector can be obtaned from Eq. 4.32, the current appromaton t may be rewrtten as t t δ f R K s close enough to the camera ego moton parameters after teratons, the fnal camera angular velocty can be obtaned from by usng the RPY angles whch are represented by composton of elementary rotatons, Roll Ptch Yaw motons about z, y and aes, respectvely: ω R K R K Rot z, ω Rot y, ω Rot, ω cω zcω y cω zcω y sω y z cω sω sω sω cω z sω sω sω cω cω z y y y y cω sω z z cω sω cω sω sω z sω sω cω cω sω z y y y cω cω z z f the rotaton matr s gven by R K r r r 2 3 r r r r r r then the fnal angular velocty ω can be obtaned by comparng Eq. 4.5 wth Eq hus 56

74 ω ω ω z y Atan2 r Atan2 r Atan2 r 2 32, r 3,, r 33 r r he soluton degenerates when cω y. n ths case, t s possble to determne only the sum or dfference of ω and ω [2]. z 57

75 Algorthm 4.. Summary of the nverse mage Jacoban based algorthm. Assumpton: Objectve:. nternal camera calbraton matr K s nown 2. An mage of the ground floor s gven 3. mage ponts are normalzed wth K : K ~ p 2.2 Gven normalzed mage ponts and mage moton vectors & for, L, N n the ground floor regon, the ground plane normal v the unt vector along the parameters Φ [ ω, t ] z as and, fnd the camera ego moton Correspondng mage ponts: ntalzaton of teratve guesses: + & R, t,, for to K wth step of Dfferental mage moton vector: δ& 4.37,, δ&, δ& 2, Accumulated mage moton vector: δv 4.36 M δ& N, [ ] mage Jacoban matr: J v [ ],,,, 4.35 J,, J, 2, Accumulated mage Jacoban matr: J 4.34 M J, N, 58

76 Dfferental camera ego moton vector: δφ J δv 4.33 ncremental rotaton matr: [ ] [ ] Rotaton matr: ranslaton vector: snθ cosθ δ + δ + δ θ θ R 2 θ R δ R δr 4.47 t t δ Planar homography: H R t v Warp by H : H, 4.38 f ma, < τ then stop end of for loop on Fnal camera translaton velocty: t t K r r2 r3 Fnal camera rotaton matr: R K 4.5 r2 r22 r23 r 3 r32 r33 ω z Atan2 r 2, r Fnal camera angular velocty: ω y Atan2 r 3, r r ω Atan2 r 32, r 33 Φ Soluton: he fnal camera ego moton parameters ω t 59

77 4.4.2 Dervaton of an mage Gradent Method he accuracy of estmatng the camera ego moton depends on the accuracy of mage moton estmaton and moton based ground floor detecton. Although the mult scale coarse to fne estmaton algorthm produces accurate mage moton nformaton, msmatched mage moton vectors may be obtaned. n ths case t s necessary to fnd an optmal camera egomoton vector whch mnmzes errors. Let and be the reference and the net mages, respectvely and π G the ground floor mage observed n the reference mage. hen an error functon wth respect to the camera ego moton vector can be defned for some mage ponts n the ground floor mage π G and t s represented by the sum of squared dfference between ther mage values n the reference mage and the correspondng mage values n the net mage, whch are ndcated by ther mage moton vectors: + ε Φ & π G 2 Substtutng Eq. 4.9 n the above equaton yelds ' + J Φ ε Φ π g 2 At the optmnmum camera ego moton vector of ε wth respect to Φ should be zero: * Φ, the partal dervatves ε Φ dφ ΦΦ * After epanson of the dervatves, Eq becomes 6

78 + + G π ε Φ J Φ Φ J Φ Φ f the camera ego moton vector s small, then the term can be appromated by ts frst order aylor epanson about Φ Φ J + Φ : Φ J Φ J and ts partal dervatve wth respect to s wrtten as Φ + J Φ J Φ Φ Φ J By substtutng Eq. 4.56, 4.57 n Eq. 4.55, the partal dervatve of ε becomes G π ε Φ J J Φ Φ and t should be zero at optmnmum G π δ ε Φ Φ Φ J J Φ Φ 2 * * 4.59 where 6

79 4.6 s spatal mage gradent vector at an mage pont of the net mage and δ 4.6 s temporal mage dfference at that pont between two mages. hus the soluton of Eq s gven by. G G π π δ J J J Φ * 4.62 Let 4.63 G π J J H be the mage Hessan matr and G π δ J m 4.64 be the mage msmatch vector. hen Eq becomes a concse form H m Φ * Snce the least squared soluton of Eq. 4.3 can be wrtten as v J J J Φ *

80 the soluton n Eq s the weghted least squared soluton by the spatal mage gradent. hus the convergence rate of Eq s faster than the nverse mage Jacoban method derved n Algorthm 4.. he weght least squared soluton s also used wth the nverse mage Jacoban method. n Eq. 4.33, the dfferental camera ego moton vector may be computed by the weght least squared soluton n Eq. 4.65: 4.67,, m H Φ δ where 4.68 G π J J H,,,,, denotes the current appromaton of the mage Hessan matr n Eq and G π δ J m,,,, 4.69 denotes the current appromaton of the mage msmatch vector n Eq And the current appromatons of the spatal mage gradent and the temporal mage dfference may be rewrtten as,,, 4.7,, δ

81 Algorthm 4.2. Summary of the mage gradent based algorthm. Assumpton: Objectve:. nternal camera calbraton matr K s nown 2. An mage of the ground floor s gven 3. mage ponts are normalzed wth K : K ~ p 2.2 Gven normalzed mage ponts and mage moton vectors & for, L, N n the ground floor regon, the ground plane normal v the unt vector along the parameters Φ [ ω, t ] z as and, fnd the camera ego moton Correspondng mage ponts: ntalzaton of teratve guesses: + & R, t,, for to K wth step of emporal mage dfference: δ 4.7 Spatal mage gradent:,,,, 4.7 [ ] mage Jacoban matr: J v [ ],,,, 4.35 mage Hessan matr: H J J, π G,,,, mage msmatch vector: m J δ, π G,,, Dfferental camera ego moton vector: δφ H 4.67, m, 64

82 snθ cosθ δ + δ + δ θ θ R 2 ncremental rotaton matr: [ ] [ ] Rotaton matr: ranslaton vector: θ R δ R δr 4.47 t t δ Planar homography: H R t v Warp by H :, H 4.38 f ma, < τ then stop end of for loop on Fnal camera translaton velocty: t t K r r2 r3 Fnal camera rotaton matr: R K 4.5 r2 r22 r23 r 3 r32 r33 ω z Atan2 r 2, r Fnal camera angular velocty: ω y Atan2 r 3, r r ω Atan2 r 32, r 33 Φ Soluton: he fnal camera ego moton parameters ω t 65

83 4.5. Epermental Results hs secton descrbes epermental results on planar ego moton estmaton usng the proposed nverse Jacoban methods. ests used both synthetc and real data to verfy that the algorthm performed ego moton estmaton correctly Synthetc Case n all of smulatons, synthetc data conssted of a random cloud of ponts whch were placed on a plane n 3D space, the ground floor, n front of the smulated camera. Each set of pont conssted of 5 randomly chosen sample ponts. he focal length was set to. he dstance to the ground floor was m along to the download y as. he depth range was.5 to 6 m along to the forward z as. And the mamum wdth was 2 m. Varous combnatons of translatonal and angular veloctes were chosen. A camera was consdered to be postoned on the top of a moble robot. hus the camera motons were composed of translatons n the y plane parallel to the ground floor and rotatons around y as. Zero mean Gaussan noses of varous levels were added to each component of mage moton vectors by consderng nosy data n mage moton estmaton. nose levels, n pels, were consdered n tests. n order to chec the correctness and accuracy of the algorthm, three bas and three senstvtes were measured for each nose level as the mean and the standard devaton of the estmates: bas and senstvtes on translatonal, angular veloctes and mage transfer error. One thousand trals were performed wth 5 mage moton vectors. ranslaton bas b t was computed for each nose level as the mean of the Eucldean dstances between the true translatonal veloctes t and 66

84 the estmates tˆ over M trals: M b ˆ t t t 4.72 M where M. Rotaton bas mght be equvalently wrtten as M b ˆ ω ω ω 4.73 M where ω, ωˆ denote the th true angular velocty and the th estmate. And mage transfer bas was computed as the mean of Eucldean mage dstances between the true correspondng mage ponts ndcated by the mage moton vectors and the estmated mage ponts ˆ j warped by the planar homography matr composed of tˆ and ωˆ over M trals and N mage ponts: j & j j M N b ˆ j j 4.74 MN where + & 4.75 j j j s the j th true correspondng mage pont n the th tral and ˆ j R ω ˆ [ tˆ ] j 4.76 s the j th estmated mage pont n the th tral, from Eq ranslaton senstvty for each nose level was computed as the standard devaton of the translaton bas over M trals: 67

85 M 2 bt b t σ t 4.77 M where b t t tˆ 4.78 s the th translaton bas. And rotaton bas and mage transfer bas could be computed n ths way. Fgure 4.3 plots bas and senstvtes on translatonal, angular veloctes and mage transfer error. he performance of the algorthm s proportonal to the nose levels. n an etensve seres of prelmnary smulatons n whch a set of smulatons was preformed wth rotaton about the y as and another set of smulatons wth rotaton about the z as, the as of rotaton had no mpact on bas and senstvtes of the proposed algorthms. hus the performance of the algorthms s nvarant wth respect to the rotaton as Real Case he eperments on real data were performed planar ego moton estmaton wth nown camera ego moton parameters when a camera moves on the ground floor for the cases of pure translaton, pure rotaton and general moton. For pure translaton an mage sequence of 8 mages are obtaned n ntervals, 3 cm, when a camera s movng forward on the ground floor. For pure rotaton an mage sequence of 9 mages are obtaned n ntervals, 8/32about 5.6 degrees, when a camera s rotatng on the ground floor. For general moton, an mage sequence of 4 mages are obtaned n ntervals, 8/6about.3 degrees and 4 cm when a camera s 68

86 rotated and translated on the ground floor. Fgure 4.4a shows the frst mage of the mage sequence for pure translaton, whch shows the mage ponts on the ground floor and the mage moton vectors. Fgure 4.4b plots the bas on translatonal and angular veloctes. Fgure 4.4c shows the reconstructed camera postons and orentatons. Fgure 4.5 and Fgure 4.6 are epermental results for pure rotaton and general moton. 69

87 angular bas degree translatonal bas m m m age transfer bas pels nose pels nose pels nose pels tra n s la to n a l s e n s tv ty m m angular senstvty degree m age transer senstvty pels nose pels nose pels nose pels Fgure 4.3. Bas and senstvtes on translatonal, angular veloctes and mage transfer error. 7

88 a mage ponts and mage moton vectors translaton bas mm rotaton bas degree mage sequence mage sequence b bas on translatonal and angular veloctes ymm mm 5 z mm c camera postons and orentatons Fgure 4.4. Epermental results for pure translaton. 7

89 a mage ponts and mage moton vectors translaton bas mm rotaton bas degree mage sequence mage sequence b bas on translatonal and angular veloctes ymm mm 5 5 z mm c camera postons and orentatons 5 Fgure 4.5. Epermental results for pure rotaton. 72

90 a mage ponts and mage moton vectors translaton bas mm rotaton bas degree mage sequence 2 3 mage sequence b bas on translatonal and angular veloctes ymm mm 5 5 z mm c camera postons and orentatons 5 Fgure 4.6. Epermental results for general moton. 73