3D model-based tracking for UAV indoor localisation

Transcription

1 3D model-based tracng for UAV ndoor localsaton Célne Teulère, Erc Marchand, Laurent Ec Abstract Ths paper proposes a novel model-based tracng approach for 3D localsaton. One man dffculty of standard model-based approach les n the presence of low-level ambgutes between dfferent edges. In ths wor, gven a 3D model of the edges of the envronment, we derve a multple hypotheses tracer whch retreves the potental poses of the camera from the observatons n the mage. We also show how these canddate poses can be ntegrated nto a partcle flterng framewor to gude the partcle set toward the peas of the dstrbuton. Motvated by the UAV ndoor localsaton problem where GPS sgnal s not avalable, we valdate the algorthm on real mage sequences from UAV flghts. Index Terms model-based tracng, partcle flterng, UAV applcaton A. Overvew I. ITRODUCTIO The past ffteen years have seen a major growth of nterest n unmanned aeral vehcles (UAV) [32]. UAV have strong potental applcatons such as survellance, search and rescue, or nspecton for mantenance. For navgaton, exstng systems manly rely on the fuson of GPS and nertal measurements. However, such approaches are usually unsutable for urban or ndoor envronments, where GPS s not avalable or mprecse. For ths reason, recent wors propose to mae use of the vsual nformaton provded by one or two cameras [] [6] [7] for UAV ndoor localsaton and navgaton. Besdes beng passve, lght and cheap sensors, cameras are usually necessary to provde a vsual feedbac n a survellance or nspecton tas, and they provde a rch source of nformaton on the envronment. However, usng vson for UAV control s also partcularly challengng snce strong constrants have to be taen nto account: durng a UAV flght, mage qualty can be poor (see Fgure 2) wth substantal nose and moton blur, and large moton dsturbances occur, especally wth small platforms le those usually consdered for ndoor mssons. To ensure safe vson-based navgaton and control tass, the robustness of the extracton of vsual nformaton s crucal. In partcular, the choce of the vsual features and ther ablty to be robustly matched and traced over tme wll determne the good achevement of the tas. Feature ponts, whch can be C. Teulère was wth CEA, LIST, 8 route du Panorama, Fontenayaux-Roses, F , France. She s now wth the Blase Pascal Unversty, Pascal Insttute, Clermont-Ferrand, France. e-mal: celne.teulere@unvbpclermont.fr E. Marchand s wth Unversté de Rennes, IRISA, Inra, Lagadc Project, Rennes, France. emal: marchand@rsa.fr L. Ec s wth CEA, LIST, Sensoral and Ambent Interfaces Laboratory, 99 - GIF-sur-Yvette CEDEX, France. emal: laurent.ec@cea.fr Ths wor was realzed n the context of the French AR natonal project SCUAV (AR Psrob SCUAV project ref AR-6-ROBO-7-2) extracted and traced n any textured envronment, have been used n varous aeral applcatons, from structure from moton or SLAM [2] [] to optcal flow computaton []. However, n ndoor structured envronments wth plan floors and walls, feature ponts are less frequent, leadng to robustness ssues. They are also senstve to the typcal nose produced by transmsson nterferences (see Fgure 2). Among the dfferent vson-based localsaton approaches, SLAM technques are partcularly nterestng n unnown cluttered envronments, but they are prone to suffer from those ssues related to the use of feature ponts, and they do not provde an absolute localsaton. In ths paper, we propose a robust model-based tracng approach to estmate the 3D pose of the UAV through ts moton. The vson system thus requres a 3D model of the edges of the envronment. Although ths condton s not sutable for unstructured outdoor mssons, t seems reasonable n the context of ndoor nspecton tass, for the mantenance of modelled ndustral stes for example. Moreover, edges are very frequent n such structured envronments, they offer a good degree of nvarance to pose and llumnaton changes and are easy to detect even n presence of nose or blur. Edges have also been already used n the context of outdoor UAV applcatons, n partcular to trac lnear structures such as ppes, or landng runway [26] [2] [3] [24]. In such scenes the lnear structures are generally dstnct n the envronment. In a structured envronment however, one dffculty of usng edges s that they suffer from havng very smlar appearances. Therefore, some ambgutes can occur typcally when dfferent edges get close to each other, whch can lead to wrong matches and tracng falures. To address ths ssue and ensure the robustness of the pose estmaton, our 3D vsual tracng approach taes nto account multple hypotheses. Our frst contrbuton s to propose a novel method to retreve multple pose hypotheses from mage measurements, usng a clusterng algorthm (Secton II). These hypotheses, along wth an assocated confdence measure, provde a smple approxmaton of the probablty densty functon. From ths, we show that choosng the best hypothess provdes us wth a tracng system wth better performances than sngle hypothess regstraton methods as descrbed n [7], [5]. Second, we derve from ths an enhanced approach by combnng top-down and bottom-up methods, n a partcle flter framewor. The hypotheses that result from the mage are used to gude the partcle set towards regons of nterest n the space of 3D poses, allowng to consder fewer partcles (Secton III). We provde smulaton results as well as valdaton on real sequences from UAV flghts. To demonstrate the sutablty of such a vson system n a UAV navgaton tas, we fnally provde expermental results on a quad-rotor

2 2 aeral vehcle (Secton IV). Ths paper extends our shortest presentatons of our tracer n [3] wth addtonal detals and experments. B. Related wor Our wor s manly related to the lterature 3D pose estmaton n computer vson. The problem of estmatng the pose of a movng camera wth respect to ts modelled envronment n real-tme has been wdely nvestgated n the past years (see [8] for a survey) and dfferent approaches have been proposed to address t. Most of these approaches can be dvded nto two categores: Regstraton methods use non lnear optmsaton technques (ewton mnmsaton, vrtual-vsual servong,) to fnd the pose whch mnmses a gven reprojecton error between the model and the mage edges [9], [7], [5]. The robustness of these methods has been mproved by usng robust estmaton tools [2], [7], [5]. In the UAV context [5] appled modelbased tracng to UAV navgaton. On man dffculty of such approaches alone s that they can fal n case of large dsplacements or wrong edge matchng, especally n cluttered envronment. Bayesan methods, on the other hand, have been used to perform the same tas by estmatng the probablty densty assocated to the pose. Ths can be acheved by Kalman flterng when the probablty densty functon (p.d.f.) can be represented by an un-modal Gaussan dstrbuton. More recently, the mprovement of computatonal performances has allowed to consder partcle flterng approaches [25], [6], [23]. Instead of gong from the low level edges to retreve the camera pose, partcle flterng uses a set of hypotheses on the possble camera poses (the partcles). The lelhood of each partcle s then measured n the mage. Snce the space of all possble poses s large, the man ssue s to eep a far representaton of the dfferent modes of the state probablty dstrbuton whle usng few partcles. To overcome the edge matchng ssue of regstraton-based methods, [3] proposed to nclude multple low level hypotheses n the robust regstraton method, showng mproved performances. However t stll mantan a unque hypothess on the camera pose. Also motvated by the robustness requred for UAV applcaton, [4] proposed to use RASAC to mantan a mult-modal representaton of the posteror densty n a partcle-flter nspred way. Our tracng approach dffers from [4] on two man ponts: Frst, we propose a novel method for generatng multple edge hypotheses, based on -mean clusterng prncple (see Secton II). Second, whle n [4] all the pose hypotheses are derved from the mage, n our approach we propose to combne hypotheses generated at the pose level, as propagated through a classc condensaton scheme [3], wth the multple hypotheses generated thans to our -mean based regstraton process (see Secton III). The next two sectons descrbe our vson-based approach n detals. II. MULTIPLE HYPOTHESES REGISTRATIO Our multple hypotheses regstraton algorthm reles on a smlar bass as the ones used n [5], [7] and [3]. Assumng the camera parameters and an estmate of the pose are nown, the 3D model s frst projected nto the mage accordng to that pose, whch can be the prevous one or a predcton obtaned from a flter. Formally, the projecton of an edge L of the 3D model accordng to the pose c M w wll be denoted by E = L ( c M w ). Each projected edge E s sampled, gvng a set of ponts {e,j } (see Fgure ). From each sample pont e,j a search s performed along the edge normal to fnd strong gradents. Fg.. In classc edge based tracng, the model s projected nto the mage plane and ponts are sampled on the projected edges. A search s performed along the normal (top). When multple strong edges are close n the mage, the exploraton of the normal can lead to ambgutes (bottom). As llustrated n Fgure, two close edges can lead to matchng ambgutes. In [5] the pont of maxmum lelhood wth regard to the ntal pont e,j s selected from the exploraton step. It s denoted by e,j n the followng. A non lnear optmsaton approach s then used to estmate the camera pose whch mnmses the errors between the selected ponts and the projected edges [9], [5], [7], that s: c M w = arg mn c M w,j d (E, e,j) () where d (E, e,j ) = d (L ( c M w ), e,j ) s the squared dstance between the pont e,j and the projecton E of the lnear segment L of the model. The quantty to mnmse s then expressed by: S = ρ ( d (E, e,j) ) (2) e j c M w SE(3) s an homogeneous matrx that gves the poston of the camera n the scene frame. It s gven by [ c c R M w = c ] w t w where c R w SO(3) s a rotaton matrx and c t w R 3 s a translaton vector.

3 3 (a) (b) Fg. 2. (a) Example of 3D model, (b) correspondng scene (undstorted) and (c) tracng result. where e s the total number of sampled ponts, and ρ s a robust estmator. To be more robust to ambguous cases, our regstraton method consders multple low level hypotheses {e,j,l } correspondng to local extrema of the mage gradent along the edge normal n e,j. Instead of performng one sngle mnmsaton from these ponts as n [3] resultng n one sngle pose, we go from these multple low level hypotheses to multple hypotheses on the camera pose tself. Ths process s descrbed n the next secton. A. Determnng the underlyng edges In order to retreve multple hypotheses for the camera pose from the detected low level hypotheses, we frst determne the underlyng lnes from the set of ponts {e,j,l }. The dea s to use the nowledge we have about the lnear components of the model, to assgn each detected pont to a potental edge and to assocate a confdence crteron to these canddate edges (see Fgure 3). In [4] ths was acheved va a RASAC approach. Here, we express ths as a typcal classfcaton problem, and propose to tacle t usng a -mean clusterng algorthm []. In our case the clusterng algorthm s slghtly modfed to group the canddate ponts nto edge hypotheses. For each projected edge E, the algorthm segments the canddate ponts {e,j,l } nto sets of ponts or classes (C,, C ). The mean of each of the classes corresponds n our case to the lne obtaned by a least square mnmsaton on the ponts of that class. To ntalse the algorthm, the number of classes for the edge E s set to the maxmum number of canddate ponts detected, that s: = max j {n,j }. The classes (C,, C ) are ntalsed usng the order n whch the hypotheses have been found on the normal. That s, for each class Cm: Cm = {e,j,m } j. Ths ntalsaton s often close to the correct segmentaton, allowng the algorthm to converge faster (see Fgure 3 (a)). At each teraton of the algorthm, the mean lne of each class s computed (Fgure 3 (b)). Each pont s then assgned to the class wth the nearest mean lne. (c) Snce the potental edges are not supposed to be normal to the ntal edge, we add the constrant that two hypotheses e,j,l and e,j,l 2 of a same ntal sample pont e,j cannot belong to the same class. The process s summarzed n the algorthm. In -mean algorthms the local convergence s ensured by the fnte number of classes and the decrease of the cost functon at each teraton. Other endng crtera such as a maxmum number of teratons and/or a threshold on the cost functon decrease rate are often added to set a maxmum computatonal tme. In our case, we do not have an analytcal proof of convergence and base the choce of ths clusterng component on expermental performances we observe. The algorthm s deemed to have converged when the assgnments no longer change or the teraton number reaches a gven threshold whch gves an upper lmt n terms of computaton tme (ths threshold was set to 3 n our experments). -mean algorthm for edge clusterng. Let {e,j,l } be the canddate ponts correspondng to the projected edge E and = max j {n,j } the maxmum number of canddates detected for a sample pont. The algorthm s ntalsed by respectng the order of detecton of the canddates: Cl = {e,j,l } j. Then the process s teratvely run as follows:. For each class Cl {C,..., C } compute the nterpolaton lne of the ponts n the class. Let rl denote the resdual of the nterpolaton. 2. For each, j : compute the dstance from the ponts e,j,l to each computed lne, the ponts e,j,l (for, j fxed) are then assocated to dfferent classes, wth hgher prorty for the ponts closer to the closest lne. The algorthm runs untl the assgnments no longer change n step 2 or the teraton number exceeds the maxmum teraton number. Algorthm : -mean algorthm for edge clusterng. Fnally, the -mean algorthm correspondng to the ntal edge E provdes us wth a set of classes Cm = ({e,j,m } j, rm) where rm s the resdual of the least square mnmsaton. Ths resdual gves a measurement of the confdence we have n the correspondng canddate edge. ote also that only the lnes wth a suffcent number of ponts are taen nto account. Ths allows to avod cases where 2 or 3 well algned ponts would then form a canddate edge consdered as very lely wthout ths class beng meanngful. We thus elmnate classes wth less than fve ponts. Fgure 3 shows a smple example of the process. Although the contours consdered have been restrcted to lnes n ths study, the approach can be easly adapted to other nds of contours. In most cases, the number of classes does not exceed two or three. Fgure 5 gves an example of the lnes detected from the teabox sequence.

4 4 Low-level canddates Classes One draw Edge E e, e,2 = C,w C p (a) Edge E e,e,2 Edge E n e n, e n,2 e,j,2 e,j, =3 n =2 C n,wn C n 2,wn 2 C,w C2,w 2 C p C3,w 3 C n pn Mnmsaton One camera pose (b) Fg. 3. An example of the -mean computaton, wth = 3. Each class s represented by a dfferent color. (a) Intalsaton of the classes of ponts. (b) Mean lnes computaton. (c) The fnal segmentaton s obtaned n one step. (c) Fg. 4. From low level hypotheses, classes of ponts are extracted. For each projected edge a random weghted draw s performed among the classes to determne the ponts that wll be used for the mnmsaton process. The mnmsaton provdes an hypothess on the camera pose. A dfferent draw would lead to another canddate pose. B. From edge hypotheses to pose hypotheses Once canddates have been obtaned for each edge n the form of sets of ponts assocated to a resdual, random weghted draws are performed. We compute weghts wm for each canddate as a functon of the resduals. Several choces for computng the weghts are possble and we use the followng expresson that gves satsfyng performances: w m = e λ ( ) rm 2 r mn rmax r mn f rmax rmn otherwse. where λ s a parameter that can be tuned accordng to the selectvty that s desred. A value of λ = has been used n our experments. One weghted draw wll denote here the draw of one canddate per edge, that s, for each edge E a class Cp s drawn from the classes. From each draw, a numercal nonlnear mnmsaton s performed accordng to (4), usng the set of ponts correspondng to the pced classes, resultng n a camera pose. We use the vrtual vsual servong mnmzaton process [5] but any other non-lnear mnmsaton method could be used for ths step. S = ( E (e,j,l) ) (4) e e,j,l C p ρ Snce the optmsaton s determnstc, t s only computed when the sets of canddates are dfferent. The weghted draw allows to favour, among all the possble combnatons, the ones wth the canddates of lowest resdual, whch are more lely to correspond to a real edge. Several hypotheses on the camera pose are thus obtaned from the low level detected hypotheses. The process s summarzed n Fgure 4. In practce, snce the number of canddate lnes per edge s small, so wll be the number of optmsatons to be performed and thus the number of pose canddates obtaned. In our experments we set the number of optmsatons (and thus pose hypotheses) to 3. (3) C. Experments To llustrate the benefts of ths approach n terms of robustness, Fgure 5 shows an example where the multple hypotheses approach allows to avod falure. At ths partcular frame, extracted from a vdeo sequence, a sngle hypotheses tracer fals. Whle runnng the multple hypotheses algorthm, t appears (see Fgure 5) that only one canddate s found for almost each projected edge, except the top bac one. For ths edge, two canddates have been found, whch lead to two dfferent camera poses. Whereas the sngle hypothess tracer fals due to a wrong match, the multple hypotheses tracer fnds the correct pose (Fgure 5-(2-b)). To valdate the proposed approach, we also used a smulated sequence, for whch the ground truth s nown. The comparatve results between the classc regstraton method and our multple hypotheses method are shown n Fgure 6. In the sngle hypothess case, only the maxmum lelhood pont s selected n the search along the normal. The tracng fals when confronted to ambgutes, that s especally when two edges get close to each other, or when a new face appears (Fgure 6-(a) and (c)). In the multple hypotheses case, the output consdered at ths stage s the camera pose whch gves the lowest resdual n the mnmsaton process. The object s successfully traced even n cases of ambgutes. However, at some ambguous frames where two canddates gve almost the same resdual, the tracer selects the wrong one as the best, whch results n some jtter on the camera trajectory (Fgure 6 (d)). In the same way, the mnmsatons whch lead to Fgure 5 (-b) and (2- b) correspond to mnma gvng almost the same resduals. By selectng only the best one, some nformaton gven by other canddates can be lost. Moreover the tracer stll needs frame to frame moton to be small to converge and could beneft from a predcton. The next secton presents an enhanced algorthm of our pose estmaton tracer that deals wth these ssues and ensures

5 5 (Detected hypotheses) (-a) (2-a) (-a) (-b) (-b) Trajectory (2-b) Trajectory MH (2-a) (2-b) Fg. 5. Example of multple hypotheses ambguty. On the top frame, all the canddate lnes and ther correspondng ponts have been represented. The gray level of the lne corresponds to ts lelhood. When tracng the topbac edge, two hypotheses have been found, one of them correspondng to the top-front edge. (-a) and (2-a) show two dfferent draws from the ntal set of canddates, resultng n two dfferent camera poses (-b) and (2-b). In the frst draw, the bac edge has been mxed up wth the front one, leadng to a tracng falure. Usng multple hypotheses allows to be more robust to such stuatons. temporal coherence of our pose estmaton. III. ORIETED PARTICLE FILTER To tacle the above ssues, we consder partcle flterng. Partcle flterng offers a very nterestng framewor n that t provdes both temporal coherence through the flterng process and mult-hypotheses handlng through the partcle representaton. More specfcally the man nterests of the partcle flterng framewor at ths pont are: the temporal flterng whch can ncorporate easly some pror nowledge on the UAV moton and prevents the system to fal n case of large occluson. the possblty to handle mult-modal representatons, whch means concretely that, f 2 poses have smlar resdual, the partcle flter wll eep partcles correspondng to both hypotheses, whle the optmsaton-based approach presented n the Secton II wll detect several, choose one, and forget about the others. The remander of ths secton presents how we combned the optmsaton process presented above wth the partcle flterng framewor (c) Trajectory Ref Fg. 6. Comparatve results on a smulated sequence wth auto-occluson. The basc algorthm wth sngle hypothess (-a), (-b) fals when a new face appears. Whle consderng multple hypotheses (2-a), (2-b), the object s successfully traced. The ground truth for the camera pose s shown n (c). A. Overvew As mentoned n the ntroducton, partcle flterng approaches [3] have been recently ntroduced n model-based tracng as an alternatve to numercal optmsaton methods, showng promsng performances [25], [6], [23]. Our approach reles on the same bass as these wors, that s the classc CODESATIO algorthm [2], whch represent the probablty densty functon p(x z : ) of the state x whch n our case wll be the pose at frame, by a fnte set {(s (), π() assocated wth the weghts π () )} =.. of samples, or partcles, s (). Each partcle s() represents a potental camera pose and z : are the observatons untl frame, deduced from mage measurements. For each new frame, the partcles frst evolve accordng to a gven dynamc model. Then, the lelhood of every partcle s measured n the mage and a weght s derved. The output consdered s usually the weghted mean of the resultng set of partcles. The partcle set s updated by performng a random weghted draw among the partcles. It s nterestng to note that whereas the tracer presented n the prevous secton was a bottom-up approach, n whch multple hypotheses on the camera pose were derved from low level hypotheses, partcle flterng does the opposte. Multple hypotheses are made on the camera pose at start, and the lelhood of these hypotheses s measured on the low level, n the mage.

6 6 The man dffculty wth these top-down approaches, as proposed n [6] and [23], results from the great sze of the consdered state space. For the tracng to be accurate enough, a large number of partcles s needed. [6] and [23] use partcle annealng method as a herarchcal approach to reduce the partcle number. However, the lelhood functons proposed stll need to be very fast to compute, snce they have to be called for each partcle. These functons may not dstngush enough between low level ambgutes. In ths paper, we propose to use partcles resultng from our multple hypotheses tracer to gude the partcle set towards the local maxma of the dstrbuton. The coherence s ensured usng the so-called mportance samplng method [3]. ) State space: The state space consdered s the specal Eucldean group SE(3) of all possble pose matrces whch transform ponts from homogeneous world coordnates to the camera coordnate frame. [ c ] c R M w = c w t w where c R w SO(3) s a rotaton matrx and c t w R 3 s a translaton vector. SE(3) s the group of rgd body transformatons. 2) Partcles propagaton on SE(3): As n [6], the propagaton model that has been consdered n the experments s a smple Gaussan nose centered on the prevous pose. Snce SE(3) s a Le group, there exsts an exponental map between SE(3) and ts Le algebra se(3). Gaussan nose s frst added on the canoncal exponental coordnates and the resultng pose matrx s computed usng the exponental map. Formally, x pred = M σ.x (5) where M σ = exp(v), v,σ 2 I 6, v se(3) and σ s the vector of the covarances assocated to the components of v. 3) Mean and averagng on SE(3): Snce the addton s not a bnary operaton on SO(3) (and thus SE(3)), the arthmetc mean R = = R of a set of rotaton matrces R s usually not a rotaton. It s however possble to defne a meanngful average as the pont n SO(3) whch mnmses the sum of squared dstances to the consdered ponts. Ths dstance can be extrnsc when we use the vector space embeddng SO(3) or ntrnsc when a Remannan dstance s consdered [9]. In ths wor an extrnsc dstance was used, followng [22]. The average rotaton s thus computed as the arthmetc mean R, followed by the unque projecton onto SO(3) gven by the unque polar factor n the polar decomposton of R. Let R = UΣV be the sngular value decomposton of R, then the mean rotaton R m s gven by: { VU f det(r) > R m = VHU (6) otherwse, where H = dag(,, ). Fnally, the weghted mean of the partcle set s computed usng ths average rotaton and the arthmetc mean of the translatons. 4) Lelhood evaluaton: Each partcle represents a potental camera pose whch has to be evaluated accordng to mage measurements. In [6] the contours are projected accordng to the partcle s to evaluate, and the rato between the number n of pxels of the projected contours whch do correspond to an edge n the mage, and the total number v of pxels on the vsble contours s computed. The lelhood of the partcle s s then derved from ths rato by: p(z x = s) = e λ n v (7) where λ s a parameter to be tuned. To decde whether a pxel do correspond to an edge n the mage, a dstance map [8] s frst computed, provdng for each pxel of the mage the dstance to the closest edge and ts drecton (see Fgure 7). Then a threshold on the dstance has to be set to determne the nler/outler count. The dstance map has to be computed only once per frame, whch mae the lelhood value very fast to compute. [6] showed the computaton can be performed n real tme on a graphcs processng unt (GPU). In ths paper, the dstance map s drectly used to compute a mean dstance: d(s) = d (8) where d s the dstance gven by the dstance map for the pxel, that s the dstance between the pxel and the closest contour n the mage. The pxels are the pxels of the projected edges. The use of the drecton to the nearest edge could mprove the dscrmnatve power of the dstance functon. However, we found that ths measure was accurate enough n our experments. Fgure 8 shows the shape of the dstance d wth respect to n-plane translatons for the frame of Fgure 7. The lelhood s derved from ths dstance by: p(z x = s) { ( ) e λ d(s) dmn 2 dmax d mn f d max d mn otherwse. Fg. 7. Wndow frame (left) and ts dstance map (rght). The darest values correspond to the smallest dstances. B. Importance samplng In our wor we propose to use the regstraton method presented n Secton II to gude the partcle set towards the regons of nterests. The approach s nspred from hybrd partcle flters le [28] [4] [27] where some partcles are moved to local maxma of the lelhood by a local optmsaton. In our case, the optmsaton corresponds to the multple hypotheses tracer descrbed n Secton II. To reduce computatonal complexty, the optmsaton s only appled to a subset of partcles whom lelhood s above a gven percentage of the maxmum lelhood. The resultng set of (9)

7 y (mm) x (mm) Fg. 8. Dstance functon wth respect to x and y translatons for the wndow frame. The zero poston corresponds to the true camera pose. partcles stll provdes a good representaton of the man modes of the densty, but t s not drectly sampled from the pror dstrbuton f (x ) = p(x z : ) as requred by Bayesan flterng theory. However, the new partcles {(s () )} =.. can be regarded as sampled from an mportance functon g (x ). A correctve term f/g can then be appled to the weghts of the partcles accordng to the mportance samplng theory [3] to mantan a far dstrbuton. As n [4], f (x ) and g (x ) are approxmated by Gaussan mxtures to evaluate the correctve term: f (x ) = (s (), Σ)(x ) () g (x ) = + ( (s (), Σ)(x )+ = = ) (s (), Σ)(x ) () where (s (), Σ) denotes the 6-dmensonal normal dstrbuton of covarance Σ centered on the exponental coordnates of the pose s. The whole algorthm s summarzed n the algorthm 2. ote that to avod a loss of dversty n the partcles, nown as the sample mpovershment ssue, the ntal partcles are not removed but combned wth the optmsed ones. Besdes, the addton of optmsed partcles generated from a bottom-up optmsaton process as well as the resamplng step also prevent the approach from degeneracy ssues. C. Experments ) Comparatve results: To underlne the mprovement n robustness brought by partcle flterng framewor, the tracer was tested on dfferent sequences taen from a UAV. Comparatve results are presented n the Fgures 9 and. The wndow sequence presents mportant frame to frame moton and occlusons, as much as great llumnaton changes. Results are shown n Fgure 9. The regstraton process alone fals when the occluson s too mportant (Fgure 9 -c). Algorthm summary. Gven the set { (s (), )} of partcles of equal =.. weghts at frame, the algorthm goes as follow: Propagaton { of the partcles accordng to (5), gvng the new set: (s (), }=.. ). Dstance measurement for each partcle, computed from equaton (8), to determne whch partcle to optmse Optmsaton of the best partcles: the multple hypotheses tracer s appled from the camera poses correspondng to the best partcles (wth a dstance below a gven threshold). One optmsaton can lead to { several hypotheses. } A set of optmsed partcles (s (), ) s obtaned. =.. Dstance measurement for the optmsed partcles. Combnaton { of } the partcles s (s (), + ) =..+. () and s () to get a set Weght computaton for each partcle usng a correctve term: π () f (s () ) g (s () )p(z x = s () ), wth + = π () =. { See III-A4. It gves the set (s (), π() }=..+ ) Estmaton of the tracng result as the weghted mean of the partcle set (see III-A3). Resamplng by performng a weghted draw of partcles among the + partcles Algorthm 2: Algorthm summary. Embedded n a partcle flterng framewor, the wndow s traced all along the sequence. Ths experment shows the nterest of ntroducng temporal flterng, wth respect to the optmsaton-based approach of Secton II. Thans to the optmsaton of some of the partcles, a small number of partcles s needed. For the wndow sequence of Fgure 9, only partcles were used. Fgure shows comparatve results for a complex structured sequence wth ambgutes (see rows 3 and 5), nose (2nd row for nstance) and sometmes few nformaton when few edges are vsble n the mage (4th row). The st column presents the results from the sngle optmsaton approach. On the 2nd column a partcle flter wth 25 partcles has been appled, whch s not enough to trac the camera pose. Wth 2 partcles the flter does not dverge anymore but the accuracy s stll poor (3rd column). Our hybrd approach (4th column) allows good estmaton wth very few partcles (25 partcles only were used n ths example). 2) Computatonal complexty and executon tme: In the dfferent tracng approaches consdered n ths paper, for a gven mage and model, the computatonal complexty depends on dfferent parameters: the number K of optmsatons, the average number M of teratons per optmsaton, the number of partcles when partcle flterng s used. If M denotes the average number of teratons necessary for the classc regstraton method, then the complexty of ths

8 8 (-a) (-b) (-c) (-d) (2-a) (2-b) (2-c) (2-d) Fg. 9. Wndow sequence. The regstraton method alone (frst row) fals when large occlusons occur (-c). The multple hypotheses tracer embedded n partcle flterng framewor (second row) tracs the object successfully. method s C = O(M). The multple hypotheses optmsaton process of Secton II requres the computaton of several optmsatons when ambgutes are met. In practce, the number K of optmsatons needed for a real robustness mprovement does not exceed 4 or 5, and ths approach s stll sutable for real tme: C = O(M). In the case of partcle flterng ntegraton III, the complexty manly depends on the number of partcles and the optmsatons appled: C = O( M K). However, the number of partcles requred s sgnfcantly lower than for classc partcle flterng. Moreover, the mean number of teratons requred by the optmsaton process s reduced thans to the predcton of the partcle flter, and the fact that the optmsaton s only appled to the best partcles. In the example of Fgure the executon tme of the hybrd approach was frames per second, wthout specfc optmsaton, on a standard PC computer. Snce partcle flterng s well adapted to parallelsaton, the approach s sutable for real-tme applcatons. such smplfcaton, the pose estmaton s stll better than the classcal approach, as shown n Secton II, snce t stll retreves several poses from the mages. However, compared to the partcle flter, Kalman flterng s mono-modal and does not eep a memory of all the canddate poses when one s selected. The man objectve s to test our vson-based approach n a real UAV experment, faced wth the challenges of flght condtons, to valdate the feasblty of the overall system. A. Expermental setup The experments have been conducted on the quad-rotor (X4-flyer) developed by the CEA LIST (Fgure ). IV. SUITABILITY OF THE APPROACH FOR UAV POSITIOIG TASKS. In Sectons II and III we presented a multple hypotheses framewor to estmate the pose of a camera and we showed how t allows to mprove the tracng robustness wth regard to the constrants assocated wth UAV flght. Ths secton presents an expermental valdaton of the sutablty of the presented approach n the UAV ndoor navgaton context. Snce our current mplementaton of the algorthm of Secton III s stll slow for UAV control, we consder here a smplfed verson to valdate the feasblty of the global approach. We thus estmate the pose from our multple hypotheses tracng (Secton II) wth 3 optmsatons, and fuse t wth nertal data n a Kalman flter nstead of partcle flterng. Wth Fg.. Quad-rotor UAV. The UAV sends the mages from ts embedded camera to the ground staton (PC) va a wreless analogcal ln of 2.4GHz. Images are processed on the ground staton wth a framerate of 2Hz. A scene was bult, combnng planar and 3D objects (see Fgure 2). The tracng ntalsaton has not been consdered n ths paper. Durng the experment, the tracng s automatcally ntalsed by detectng a blac dot n ts frst poston (Fgure 2). Then the vehcle can locate tself thans to the model-based tracng, wthout usng the dot anymore.

9 9 (-a) (-b) (-c) (-d) (2-a) (2-b) (2-c) (2-d) (3-a) (3-b) (3-c) (3-d) (4-a) (4-b) (4-c) (4-d) (5-a) (5-b) (5-c) (5-d) Sngle optmsaton FP 25 partcles FP 2 partcles Hybrd approach 25 partcles Fg.. Comparatve results for the apartment sequence. B. Accuracy of the estmaton To evaluate the accuracy of the estmaton of the poston of the UAV, we compared t to a ground truth obtaned usng a metrologc laser tracer Leca 2 of mcrometrc precson. Fgures 2 and 3 show the comparatve results obtaned for the pose estmaton for a flght n the modelled scene. The standard devaton for the pose estmaton s about 6cm n translaton whch s much better than the localsaton acheved wth standard GPS. The man lmtng factor here s 2 Poston (m) Tme (s) Fg. 2. Poston estmated wth the model-based tracng (blue) and ground truth (red). Y Leca Y Vson

10 Poston Z (m) Fg. 3. Poston estmated wth the model-based tracng (blue) and ground truth (red). Tme (s) the accuracy of the 3D model, whch was obtaned by coarsely measurng the dfferent elements. Ths experment also shows the robustness of the approach to typcal nterference nose that was present n the flght (see Fgure 2 (b)). C. avgaton In [29] we show that such a localsaton method can be used for the UAV control. We do not focus here on the control strategy whch s not the am of ths paper. We show however that the proposed approach was successfully valdated on a way-pont navgaton trajectory. The tas consdered was to autonomously reach several set ponts successvely: frst the vehcle s stablsed 2 meters above the dot target (p = (,, 2)). Then the set ponts are successvely set to (, 2, 2), ( 2, 2, 2), (, 2, 2), (,, 2), ( 2,, 2) (see Fgure 4b). (a) Fg. 4. Example of camera vew wth model reprojecton (a) and requred trajectory (b). Fgure 5, shows the estmated and ground truth trajectores. The system was able to localse tself despte nose and large nterframe moton. Z (m) Y (m) 2 X (m) Fg. 5. UAV trajectory as estmated by our vson algorthm (blue) and ground truth (red). (b) Z Leca Z Vson V. COCLUSIOS In ths paper, we proposed a novel model-based tracng system. Our multple hypotheses regstraton process provdes us wth multple pose hypotheses by performng several mnmzatons, correspondng to dfferent sets of ponts. We show how these hypotheses can be ntegrated nto a partcle flter. The multple hypothess tracer s appled to the best partcles to move them to the local maxma of the lelhood functon. The partcle set s therefore guded toward the canddate poses emergng from the multple hypothess tracer. Although the state space s large, a small number of partcles are needed. In that case, the tracng approach benefts from both the temporal coherence and mult-modal representaton of the Bayesan framewor as well as the accuracy of a regstraton process. The resultng approach has been valdated on dfferent sequence from UAV flghts. A smplfed approach has been tested on postonng tass on a quad-rotor UAV. The accuracy of the estmates has been evaluated and the experment shows the feasblty of the proposed approach n ndoor structured envronments. REFERECES [] A. Angel, D. Fllat, S. Donceux, and J.-A. Meyer. 2D smultaneous localzaton and mappng for mcro aeral vehcles. In Proceedngs of the European Mcro Aeral Vehcles (EMAV 26) conference, 26. [2] M. Armstrong and A. Zsserman. Robust object tracng. In Proc. Asan Conference on Computer Vson, volume, pages 58 6, 995. [3] O. Bourquardez and F. Chaumette. Vsual servong of an arplane for auto-landng. In Intellgent Robots and Systems, 27. IROS 27. IEEE/RSJ Internatonal Conference on, pages 34 39, 27. [4] M. Bray, E. Koller-Meer, and L. Van Gool. Smart partcle flterng for 3D hand tracng. IEEE Internatonal Conference on Automatc Face and Gesture Recognton, :675 68, 24. [5] A.I. Comport, E. Marchand, M. Pressgout, and F. Chaumette. Real-tme marerless tracng for augmented realty: The vrtual vsual servong framewor. IEEE Trans. on vsualzaton and computer graphcs, 2(4):65 28, July/August 26. [6] J. Courbon, Y. Mezouar,. Guenard, and P. Martnet. Vson-based navgaton of unmanned aeral vehcles. Control Engneerng Practce, 8(7): , 2. Specal Issue on Aeral Robotcs. [7] R. Drummond, T. Cpolla. Real-tme vsual tracng of complex structures. IEEE Trans. on Pattern Analyss and Machne Intellgence, 24(7): , 22. [8] R. Fabbr, L. Da F. Costa, J. C. Torell, and O. M. Bruno. 2D eucldean dstance transform algorthms: A comparatve survey. ACM Comput. Surv., 4(): 44, 28. [9] P.T. Fletcher, C. Lu, and S. Josh. In IEEE Int. Conf. on Computer Vson and Pattern Recognton. [] J.A. Hartgan. Clusterng algorthms. John Wley & Sons, 975. [] B. Hérssé, T. Hamel, R. Mahony, and F.-X. Russotto. A terranfollowng control approach for a VTOL unmanned aeral vehcle usng average optcal flow. Autonomous Robots, pages 9, 2. [2] M. Isard and A. Blae. CODESATIO: condtonal densty propagaton for vsual tracng. Int. Journal of Computer Vson, 29():5 28, 998. [3] M. Isard and A. Blae. ICODESATIO: Unfyng low-level and hgh-level tracng n stochastc framewor. In European Conf. on Computer Vson, volume, pages , 998. [4] C. Kemp and T. Drummond. Mult-modal tracng usng texture changes. Image and Vson Computng, 26(3):442 45, 28. [5] Chrstopher Kemp. Vsual control of a mnature quad-rotor helcopter. PhD thess, Unversty of Cambrdge, 26. [6] G. Klen and D. Murray. Full-3d edge tracng wth a partcle flter. In Brtsh Machne Vson Conf., volume 3, pages 9 28, 26. [7] S. Klose, J. Wang, M. Achtel, G. Pann, F. Holzapfel, and A. Knoll. Marerless, vson-asssted flght control of a quadrocopter. In IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, Tape, Tawan, October 2.

11 [8] V. Lepett and P. Fua. Monocular model-based 3D tracng of rgd objects: A survey. In Foundatons and Trends n Computer Graphcs and Vson, pages 89, 25. [9] D.G. Lowe. Fttng parameterzed three-dmensonal models to mages. IEEE Trans. on Pattern Analyss and Machne Intellgence, 3:44 45, 99. [2] R. Mahony and T. Hamel. Image-based vsual servo control of aeral robotc systems usng lnear mage features. IEEE Transactons on Robotcs, 2(2): , 25. [2] M. Mengast, C. Geyer, and S. Sastry. Vson based terran recovery for landng unmanned aeral vehcles. In IEEE Conf. on Decson and Control, volume 2, pages , dec. 24. [22] M. Moaher. Means and averagng n the group of rotatons. SIAM journal on matrx analyss and applcatons, 24: 6, 22. [23] S. use, J. Roberts, and G. Wyeth. Vsual localsaton n outdoor ndustral buldng envronments. In IEEE Int. Conf. on Robotcs and Automaton, pages , 28. [24] A. Pett, E. Marchand, and K. Kanan. Tracng complex targets for space rendezvous and debrs removal applcatons. In IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, 22. [25] M. Pupll and A. Calway. Real-tme camera tracng usng nown 3D models and a partcle flter. In Int. Conf. on Pattern Recognton, pages 99 23, August 26. [26] S. Rathnam, Zu Km, A. Soghan, and R. Sengupta. Vson based followng of locally lnear structures usng an unmanned aeral vehcle. In Decson and Control, 25 and 25 European Control Conference. CDC-ECC 5. 44th IEEE Conference on, pages , 25. [27] C. Shan, T. Tan, and Y. We. Real-tme hand tracng usng a mean shft embedded partcle flter. Pattern Recognton, 4(7):958 97, 27. [28] J. Sullvan and J. Rttscher. Gudng random partcles by determnstc search. In IEEE Int. Conf. on Computer Vson, volume, pages , 2. [29] C. Teulère, L. Ec, E. Marchand, and. Guenard. 3D model-based tracng for UAV poston control. In IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, Tape, Tawan, 2. [3] C. Teulère, E. Marchand, and L. Ec. Usng multple hypothess n model-based tracng. In IEEE Int. Conf. on Robotcs and Automaton, Anchorage, Alasa, May 2. [3] L. Vacchett, V. Lepett, and P. Fua. Combnng edge and texture nformaton for real-tme accurate 3d camera tracng. In IEEE Int. Symposum on Mxed and Augmented Realty, pages 48 57, 24. [32] K. Valavans. Advances n Unmanned Aeral Vehcles. State of the Art and the Road to Autonomy. Sprnger-Verlag, 27.