Simultaneous camera orientation estimation and road target tracking

Transcription

1 Simulaneous camera orienaion esimaion and road arge racking Per Skoglar and David Törnqvis Linköping Universiy Pos Prin N.B.: When ciing his work, cie he original aricle. Original Publicaion: Per Skoglar and David Törnqvis, Simulaneous camera orienaion esimaion and road arge racking,, Proceedings of h Inernaional Conference on Informaion Fusion, - 7. Posprin available a: Linköping Universiy Elecronic Press hp://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-

2 Simulaneous Camera Orienaion Esimaion and Road Targe Tracking Per Skoglar and David Törnqvis Division of Auomaic Conrol Deparmen of Elecrical Engineering (ISY) Linköpings universie SE- 3 Linköping, Sweden {skoglar, ornqvis}@isy.liu.se Deparmen of Sensor and EW Sysems Swedish Defence Research Agency (FOI) Box, SE- Linköping, Sweden Absrac Airborne surveillance sysems equipped wih a vision/infrared camera require good knowledge abou he posiion and orienaion of he camera for successful racking of ground arges. In paricular, his is essenial when incorporaing prior informaion, like road maps, ha is expressed relaive a global reference sysem. Usually, i is possible o obain good posiioning wih inerial/saellie navigaion sysems, bu esimaing he orienaion is generally more difficul. I migh be possible o use SLAM (Simulaneous Localizaion and Mapping) or image regisraion approaches o suppor he navigaion sysem, bu no always since such approaches require sable feaures in he images. In his paper he problem of simulaneous orienaion error esimaion and road arge racking is considered by assuming ha he arge is consrained o a known road nework. A paricle filer approach is proposed and i is shown ha he resul of his filer is close o he performance of he ideal case where he orienaion error is perfecly known. However, he performance depends on how informaive he road pah is and in rare cases he orienaion error is unobservable. I. INTRODUCTION Uilizing road nework informaion in navigaion and arge racking will reduce he locaion error significanly, compared o he case when he map informaion is ignored []. In surveillance applicaions, road maps have been used for road arges racking based on deecions from a radar sensor (ypically ground moving arge indicaor, GMTI) [], [3] or a vision sensor []. In arge racking, he navigaion error of he observing sensor is ypically negleced and his is reasonable as long as he racks are expressed relaive he sensor iself. However, when exernal informaion, such as road maps, represened relaive a global reference sysem is included, un-modeled navigaion error biases can have severe effecs on he racking performance. In paricular his is a problem when he accuracy of he sensor is similar or beer, in some sense, han he accuracy of he navigaion esimae. For insance, even hough an INS/GPS navigaion sysem is used, here migh be robusness issues for vision/infrared camera based sysems due o he high angular resoluion of cameras. Jus a small camera orienaion error can lead o a large error for a disan objec when an observaion is ransformed o a global reference frame. There is a number of approaches o handle hese robusness issues. For esimaion mehods relying on a represenaion wih bad suppor in low probabiliy areas (such as he paricle filer), increasing he measuremen noise migh be emping o increase he robusness of he filer, bu his is a quesionable soluion since he racking performance will suffer. An alernaive is o use a muliple-model filer wih boh an onroad mode and an off-road mode [] where he off-road mode serves as a fall-back soluion. A hird alernaive is o use rack landmarks in a vision SLAM (Simulaneous Localizaion and Mapping) framework [], [], [7] o esimae he orienaion error, bu his requires ha a number of suiable landmarks are available over ime and his migh no always be he case. A relaed approach is o use image regisraion o esimae he orienaion error by aligning he camera image frame o a known scene model [], [9]. However, his approach also relies on sable feaures in he image frames and parallax effecs are complicaed o handle. Fig.. An infrared image from an airborne surveillance sysem. Three cars on a road are visible. The image is dominaed by rees and i is unlikely ha any visual landmarks can be used o suppor he navigaion. This paper considers an esimaion problem where he orienaion error and he arges are racked simulaneously. Figure shows an image frame from an airborne surveillance sysem wih an infrared sensor. Three cars are raveling on a road hrough a wood, bu here are barely any useful saic landmarks for a SLAM framework o use. Since he

3 arges are visible and hey are on a known road nework, an ineresing quesion is if he orienaion error can be esimaed and he arge can be racked simulaneously. The purpose of his paper is o invesigae his problem. The reason for jus considering he orienaion error is ha his error has much larger impac on he overall resul compared o he posiioning error of he camera ha is assumed o be known wih high accuracy by a saellie navigaion sysem. This esimaion problem is relaed o SLAM, bu a significan difference is ha consrained dynamic arges are used here, unlike SLAM where saic landmarks are considered. The paper is organized as follows. Secion II presens he problem and models in deail. In Secion III a paricle filer is proposed as he esimaor of he problem. Simulaion resuls are presened and discussed in Secion IV and finally he work is summarized and some conclusions are drawn in Secion V. II. PROBLEM DESCRIPTION In his secion he models of he simulaneous orienaion error esimaion and road arge racking problem are given. The orienaion error is represened by a quaernion and he arges are assumed o be consrained o a known road nework. In his work a simplified version of he road arge racking approach in [] is used; jus one single road is here considered and he associaion problem is ignored. The observaion model is based on a pinhole camera model. A. Road Targe Model In his paper i is assumed ha he arge is on he same road all he ime. Road inersecions and arge ransiions o oher roads are no considered in his work. The laeral and verical locaions of he arge relaive he road are also assumed o be zero. However, i should be quie sraighforward o implemen a more general road arge represenaion by following he approach in e.g. []. A curve-linear coordinae sysem is defined for he road. Le x and v be he longiudinal posiion and velociy, respecively, along he road relaive he road sar. The on-road sae vecor is defined as x r, (x v) T and he dynamic arge model can, as long as he arge says on he same road, be expressed as he linear discree-ime model x r + = f r (x r,v r T )= x r T + / w T r () {z } {z } where he process noise is i.i.d. as w r N(,Q r ) and T is he sampling ime. Le g gr ( ) represen he ransformaion ha is ransforming a coordinae x r in he local road aligned sysem o a coordinae x g in a global Caresian reference sysem, i.e., x g = g gr (x r ). B. Orienaion Bias Model The orienaion bias is represened by a quaernion x a, (q q q q 3 ) T and he bias is modeled [] as,f,g x a + = f a (x a,v a )=x a + T S(x a )v a () where he process noise is i.i.d. as w a S(x a )= C. Observaion Model q q q 3 q q 3 q q 3 q q q q q N(,Q a ) and C A. (3) The camera is a saring-array vision sensor wih limied field-of-view and i is assumed ha he pinhole camera model can be used. Thus, an observaion y a ime is a deecion of he arge in he image plane corresponding o he azimuh and elevaion angles from he sensor locaion o he arge locaion under he influence of he orienaion bias. Le y = h(x )+e () where he measuremen noise is i.i.d. as e N(,R). According o he pinhole camera model a poin x c =(x c y c z c ) T expressed in Caresian coordinaes relaive a camera fixed reference sysem, is projeced on a virual image plane ono he image poin (u v) T according o he ideal perspecive projecion formula u h(x )= = x c v z c y c. () The poin x c is compued as he difference beween he camera locaion x s and arge locaion, relaive he global reference sysem, followed by a ransformaion from he global sysem o he camera fixed sysem, i.e., x c = R(x a )R cg (g gr (x r ) x s ) () where R cg is a roaional marix represening he (unbiased) orienaion of he camera. Noe ha boh R cg and x s is assumed o be known in his work. The roaion marix R(x a ) is he sandard roaion marix based on he quaernion and can for insance be found in [, p. ]. D. Augmened Model The model where boh he orienaion error and he arge racking of n arges can be defined as follows. The sae and noise inpu vecors are defined as x a w a x r x, B A, w, w r B A, (7) x rn w rn respecively, and he augmened dynamic model is defined as x + = f(x,v ) T I... S(x a )... F... = C. A x G... + C..... A v.... F... G () An observaion of arge i is modeled as y ri = h(x a,x ri )+e ri. (9)

4 III. ESTIMION APPROACH If he uncerainy is relaively low, he esimaion problem will be close o linear and could be handled wih an Exended Kalman Filer (EKF). However, wih a larger uncerainy and for an arbirary road nework he esimaed sae disribuion is likely o be mulimodal and hus an EKF would no longer work. Here, we have insead used he paricle filer for esimaion. The paricle filer is a very flexible esimaion approach where i is relaively sraighforward o use non-linear model like road maps. However, one disadvanage of he paricle filer is is bad suppor in low probabiliy areas which someimes makes i sensiive o un-modeled errors. Anoher problem is when he number of racked arges increases. Then he sae dimension increases and he compuaional load rapidly becomes infeasible o handle. To alleviae his, i is proposed o do he esimaion wih a Rao-Blackwellized paricle filer which reduces he sae dimension handled by he paricle filer. A. Boosrap Paricle Filer As he non-linear esimaion problem reaed in his aricle does no have an analyical soluion, an approximae soluion wih he paricle filer is used. The paricle filer was inroduced in [] and a good uorial can be found in [3]. To use he paricle filer, he dynamic model is rewrien as condiional disribuions. The dynamic model in () and () can wih knowledge abou he process noise disribuion be reformulaed ino he condiional disribuion p(x x )=p(x r x r )p(x a x a ), where and p(x r x r ) =N (Fx r, GQ r G T ) () p(x a x a ) =N x a, T S(x a )Q a a S(x ) T. () In he same way, he measuremen model () can be rewrien as he densiy p(y x )=N h(x ),R. () Wih knowledge abou hese disribuions, he paricle filer in Algorihm can be applied. B. Rao-Blackwellized Paricle Filer To reduce he dimension handled by he paricle filer, he sae space is pariioned ino wo pars x =(x pt x k T ) T. The poserior disribuion can hen also be pariioned ino wo pars as p(x p,x k y : )=p(x p y : )p(x k x p,y : ). () Now, if he sysem is linear given he saes x p (i.e., condiionally linear) hen he second densiy in () can be compued by a Kalman filer. The paricle filer is hen only needed for compuing he densiy over x p, hence he dimension reducion. This pariioning is referenced o as he Rao-Blackwellized Paricle Filer and more informaiom can be found in [], [], [7]. Algorihm Paricle Filer ) Iniialize he paricles according o x (i) p(x ) and se appropriae weighs for all i =,...,N. ) Time updae: Generae new paricles according o he proposal disribuion x (i) (x x (i),y ), i =,...,N. (3) Updae he weighs according o = p(x(i) (x (i) x (i) ) which in case (x x (i) = q(i). x (i),y ) q(i), i =,...,N, (),y )=p(x x (i) ) simplifies o 3) Measuremen updae: Calculae imporance weighs according o = P N j= q(j) p(y x (i) ) p(y x (j) ), i =,...,N. () ) Resampling: Use a mehod of choice, in his work sysemaic resampling is used []. ) Se := + and repea from sep. In his applicaion, he saes are divided as x p = x and x k =(v x at ) T. The dynamics for x k will be close o linear since small orienaion errors are expeced and arges ypically move far away from he sensor. Hence, his par can be linearized. Mulimodaliy is expeced for x p depending on he shape of he road nework, hence his par is esimaed wih he paricle filer. In case of muliple arges, he sae space is divided as x p =(x r x rn ) T and x k =(v r v rn x at ) T. IV. RESULTS In his secion a number of simulaion examples are presened. The road pahs ha are used are defined by x g = r(x) cos(x) y g = r(x)sin(x) z g = + sin(cx) r(x) = + sin(cx) (7) where c is a consan. Three differen cases will be used, c =,,, and he hese pahs are illusraed in Figure. y x x x Fig.. Three differen road pahs (7) used in he simulaions. Lef: c =. Middle: c =. Righ: c =.

5 Three differen filers are applied o he single road arge racking problem. The RMSE (roo mean square error) resul of Mone-Carlo (MC) simulaions wih runs are presened for evaluaion of he esimaion resuls. The filer algorihm, for all cases, is he boosrap paricle filer presened in Secion III, bu he models and iniial condiions are differen. The dynamic model of filer () (as in Ideal Tracking) only conains he arge racking par in (), and he orienaion error is assumed o be perfecively known. This filer is providing an ideal esimaion resul of he arge racking and gives a lower bound of he arge racking performance. Filer () (as in Tracking Only) is also jus considering he arge racking par (), bu unlike he ideal filer (), his filer does no know he orienaion error and acs as if his error is zero. Thus, filer () will show he impac of un-modeled orienaion error. Filer () (as in Augmened Tracking) is using he model in () which includes boh he arge racking and he orienaion error pars. The camera is assumed o be locaed in he origin and i is aiming downwards. The iniial orienaion error is a roaion around he poining direcion vecor. The orienaion error hen evolves according o (). For each MC run is sampled uniformly from he inerval [.,.] (rad). The iniial posiion and velociy errors of he arge are sampled uniformly from [, ] (m) and [, ] (m/s), respecively. The sampling ime is. s and he number of paricles is in each filer. The rue sae rajecory is generaed by using he following covariance marices: Q r =., Q a =. I, R =. I, bu in he filers wice as big Q r and R are used. Figures 3, and show he filer resuls for he road cases c =, c =and c =, respecively. The more curved he pah is, he easier i is o esimae he orienaion error and, hence, he arge posiion. This can be seen by noing ha he posiion error of he arge x of filer () (black line) is close o he posiion error of filer () (dashed gray line) quicker in Figure 3 compared o Figure (posiion error is shown in he second plo from he boom in each figure). The case c =in Figure is a special case where he orienaion error is no observable in filer () since he circle pah is invarian w.r.. a roaion around a verical axis. For his paricular case, he posiioning error of filer () is similar o he error of filer () where he orienaion error is ignored. However, even for less informaive pahs filer () usually performs beer han filer (). This is illusraed by anoher example of he case c =in Figure where he orienaion error is around a vecor in he xy-plane insead. Alhough he posiion error of he arge in filer () does no approach he ideal case ha much, he resul is beer han for filer () where he orienaion error is ignored. Noe ha he impac of he orienaion error on he posiion error of he arge in filer () is dependen on he arge locaion on he circle pah. Jus a few examples are presened here, bu he overall behavior of he filers is similar for oher condiions, e.g. oher iniial orienaion errors. Filer () performs good and, if he pah is informaive, he posiion error of he arge is close o he ideal case. The impac of un-modeled orienaion errors in filer () are also dependen on he pah and also he direcion of he orienaion error, bu in all circumsances he errors have a severe effec on he arge racking performance. q q q x Fig. 3. RMSE for pah c =, he righ case in Figure. Black line shows he RMSE errors (denoed by he ilde symbol on each sae variable) of filer () wih simulaneous orienaion error and arge racking. Dashed dark gray line shows he RMSE errors of filer () wih he arge racking par only and perfec orienaion error knowledge. Dashed-doed ligh gray line shows he RMSE errors of filer () wih he arge racking par only and ignored orienaion error. The filer () is successfully esimaing he orienaion error and he posiioning error x is close o he ideal case (second plo from he boom). V. DISCUSSION AND CONCLUSIONS This paper reas he problem of simulaneous orienaion error esimaion and road arge racking. The applicaion in mind is an airborne surveillance sysem wih a vision/infrared camera for road arge racking and an INS/GPS sysem for navigaion. Alhough he navigaion sysem provides he posiion and orienaion of he camera here migh be errors

6 x x q q q. q q. q Fig.. RMSE for pah c =, he middle case in Figure. Here he pah is less informaive and he resul of filer () close o he ideal case slower, compared o he case in Figure 3. ha can have severe effecs on he racking performance since observaions are incorrecly aligned wih prior informaion expressed in a global reference frame. In his work i assumed ha SLAM and image regisraion echniques could no be used due o bad/unsable image feaures. Insead a filer is proposed ha esimaes he orienaion error and he road arge sae simulaneously by exploiing he knowledge abou he road map. The posiion error is negleced since i is assumed o be very small compared wih he range o he arge. The filer is based on he well-known boosrap paricle filer, bu if several arges are racked he Rao-Blackwellized version is a beer choice o handle he curse-of-dimensionaliy. Paricle filers are very flexible and relaively sraighforward o implemen and adap o non-linear sysems, like road maps in his work. However, i is imporan o remember ha i has bad suppor in low probabiliy areas. In he simulaion examples i is shown ha he racking performance of a road arge is close o he ideal case where he Fig.. RMSE for pah c =, he lef case in Figure. This pah is jus a circle and he orienaion error is no observable in filer () for his special case when he orienaion error is an angle around a verical vecor from he sensor in he origin o he circle cener. This causes he posiioning error of filer () o be similar o he resul of filer () where he orienaion error is ignored. orienaion error is known. However, observabiliy is always an issue in vision based arge racking. In fac, he arge racking problem for a saionary angle-only sensor, e.g. a camera, is no observable and some exernal informaion is needed o suppor he esimaion process. In his work he knowledge abou he road pah is used, bu as seen in one example, here exis cases where he orienaion error is unobservable. Such cases are quie rare, bu sill differen pahs can be more or less informaive from an esimaion poin of view. Basically, curvy pahs are beer han sraigh or very smooh pahs, and if he sensor plaform is moving he condiions for arge racking is usually beer. REFERENCES [] F. Gusafsson, U. Orguner, T. B. Schön, P. Skoglar, and R. Karlsson, Handbook of Inelligen Vehicles. Springer-Verlag London Limied,

7 q q q x [9] B. Ziova and J. Flusser, Image regisraion mehods: a survey, Image Vis. Compu., vol., no., pp. 977, 3. [] D. Törnqvis, Esimaion and deecion wih applicaions o navigaion, Disseraions No, Dep. Elecr. Eng, Linköpings universie, Sweden, SE- 3 Linköping, Sweden, Nov.. [] J. B. Kuipers, Quaernions and Roaion Sequences. Princeon Univeriy Press, 999. [] N. J. Gordon, D. J. Salmond, and A. F. M. Smih, Novel approach o nonlinear/non-gaussian Bayesian sae esimaion, IEE Proc.-F, vol., no., pp. 7 3, Apr [3] S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, A uorial on paricle filers for on-line non-linear/non-gaussian Bayesian racking, IEEE Trans. Signal Processing, vol., no., pp. 7, Feb.. [] G. Kiagawa, Mone Carlo filer and smooher for non-gaussian nonlinear sae space models, J. Compu. Graph. Sa., vol., no., pp. pp., 99. [Online]. Available: hp:// [] R. Chen and J. S. Liu, Mixure Kalman filers, J. Royal Sais. Soc. B, vol., no. 3, pp. 93,. [] C. Andrieu and A. Douce, Paricle filering for parially observed Gaussian sae space models, J. Royal Sais. Soc. B, vol., no., pp. 7 3,. [7] T. Schön, F. Gusafsson, and P.-J. Nordlund, Marginalized paricle filers for mixed linear/nonlinear sae-space models, IEEE Trans. Signal Processing, vol. 3, no. 7, pp. 79 9, Jul Fig.. RMSE for pah c =. The same case as in Figure, bu he orienaion error is insead around a vecor in he xy-plane. Alhough he informaion richness of he pah is very low, he filer () performs significanly beer han he filer ()., ch. Navigaion and Tracking of Road-Bound Vehicles Using Map Suppor, o appear. [] M. Ulmke and W. Koch, Road-Map assised ground arge racking, IEEE Trans. Aerosp. Elecron. Sys., vol., no. 3, pp. 7, Oc.. [3] B. Risic, S. Arulampalam, and N. Gordon, Beyond he Kalman Filer - Paricle filers for racking applicaions. Arech House,. [] P. Skoglar, U. Orguner, D. Törnqvis, and F. Gusafsson, Pedesrian racking wih an infrared sensor using road nework informaion, EURASIP J Adv. Signal Process., vol., no., p.,. [Online]. Available: hp://asp.eurasipjournals.com/conen/// [] A. J. Davison, I. Reid, N. Molon, and O. Srasse, MonoSLAM: Realime single camera SLAM, IEEE Trans. Paern Anal. Machine Inell., vol. 9, no., pp. 7, Jun. 7. [] H. Durran-Whye and T. Bailey, Simulaneous localizaion and mapping (SLAM): Par I, IEEE Robo. Auoma. Mag., vol. 3, no., pp. 99, Jun.. [7] T. Bailey and H. Durran-Whye, Simulaneous localizaion and mapping (SLAM): Par II, IEEE Robo. Auoma. Mag., vol. 3, no. 3, pp. 7, Sep.. [] D.-G. Sim, R.-H. Park, R.-C. Kim, S. U. Lee, and I.-C. Kim, Inegraed posiion esimaion using aerial image sequences, IEEE Trans. Paern Anal. Machine Inell., vol., no., pp., jan.