Kalman filter correction with rational non-linear functions: Application to Visual-SLAM

Transcription

1 1 Kalman filte coection with ational non-linea functions: Application to Visual-SLAM Thomas Féaud, Roland Chapuis, Romuald Aufèe and Paul Checchin Clemont Univesité, Univesité Blaise Pascal, LASMEA UMR 6602, CNRS, AUBIERE, FRANCE, Abstact This aticle deals with the divegence of the Kalman filte when used on ational non-linea obsevation functions in the Visual SLAM famewok. The context objective is to localize a vehicle and simultaneously to build a map accoding to envionment peceived by a camea. Thee ae many ways to fuse all data fom sensos and the usual one is the Kalman filte. A main poblem of this appoach is the divegence due to an impope lineaization of the obsevation model. It leads to wong estimation which distubs all the pocess. The pesented method allows, unde weak constaint on the obsevation function, to educe the divegence effect without modifying the obsevation noise. In the Visual SLAM context, this method dastically impoves esults and gives moe stability to monocula system in ode to initialize landmaks. Index Tems Visual SLAM, depth estimation, Kalman filteing I. INTRODUCTION Simultaneous Localization and Mapping (SLAM) has become well defined in the obotics community to tackle the issue of a moving senso platfom constucting a epesentation of its envionment on the fly, while concuently estimating its ego-motion [8, 2]. Stochastic appoaches have solved the SLAM poblem in a consistent way, because they explicitly deal with senso noise [7, 13]. The implementation of a featue-based SLAM appoach encompasses the following functionalities: the peception pocess which depends on the kind of envionment and on the sensos the obot is equipped with. It consists in detecting fom the peceived data, featues of the envionment that ae salient, easily obsevable and whose elative position to the obot can be estimated. the obsevation pocess which concens the estimation of the featues locations elative to the obot s pose fom which they ae obseved. the pediction pocess which deals with the estimation of the obot s motion between two featue obsevations. This estimate can be povided by sensos, by a dynamic model of obot s evolution fed with the motion contol inputs, o based on simple assumptions such as a constant velocity model. the data association pocess in which the obsevations of landmaks must be popely associated (o matched), othewise the obot s position can become totally inconsistent. the estimation pocess which consists in integating the vaious elative measuements to estimate the obot and landmaks positions in a common efeence fame. Fig. 1 gives an example of a monocula SLAM pocess. Histoically, SLAM elied on lase telemetes such as Range-and-Beaing (RB) sensos allowing the obots to build plana maps of the envionment. The oiginal solution [12] utilized an Extended Kalman Filte (EKF) to fuse data acquied by a lase ange scanne o othe ange and beaing sensos, leading to Range and Beaing EKF-SLAM. Recently, the use of vision sensos has put the wold within each but has given ise to new difficulties due, on the one hand, to the ichness of the infomation povided and, on the othe hand, to the loss of the depth dimension in the measuements inheent to the pojective natue of vision. Visual SLAM with a single camea is moe challenging than visual SLAM based on steeo vision, but a single camea will always be cheape, moe compact and easie to calibate than a multi-camea ig. Fig. 1. Top: two images taken at steps k and k + 1 and the coesponding extacted featues. Bottom: the distance between the two obseve s positions (black tiangula) based on the tajectoy in blue.

2 2 The EKF estimation algoithm used in Davison s system has been shown to be inconsistent [1]. The algoithm tends to be oveconfident in the estimate calculated due to lineaization eos. As well as impoving the computational efficiency of mapping lage envionments, sub-mapping also impoves the quality of the estimate by deceasing these lineaization eos [1], [5]. Othe monocula SLAM systems have also attempted to impove the quality of the estimate by using diffeent estimation algoithms. Chekhlov et al. [6] used the Unscented Kalman Filte (UKF) fo thei monocula SLAM system. This algoithm impoves the consistency of the estimate by making moe accuate appoximations to the non-lineaities in the obsevation pocess. The impovement in consistency comes at a cost in complexity (O(N3) vs. O(N2) fo the EKF). Recently, Holmes et al. [10] showed that using the squae oot UKF algoithm [14] a moe consistent estimate can be obtained at O(N 2) complexity in a monocula SLAM system. Howeve, the actual pocessing time equied fo the implementation was an ode of magnitude geate than fo the EKF on easonably sized maps due to a lage constant facto on the computational cost in thei implementation. In this pape, a camea-centeed Extended Kalman Filte is used to pocess a monocula sequence. The sensocenteed Extended Kalman Filte was fist intoduced in the context of lase-based SLAM [4]. Contay to the standad EKF-SLAM, whee estimates ae always efeed to a wold efeence fame, the senso-centeed appoach epesents all featue locations and the camea motion in a efeence fame attached to the cuent camea. The typical coelated featuesenso uncetainty aising fom the uncetain camea location is tansfeed into uncetainty in the wold efeence fame, esulting in lowe vaiances fo the camea and map featues, and thus in smalle lineaization eos [5]. This pape pesents a method to educe the divegence in the specific case of ational non-linea obsevation models. At each step, compaisons between solutions with and without coection in the non-linea case demonstate the effectiveness of the poposed appoach. In Section II, we begin by intoducing the camea-centeed Extended Kalman Filte. Section III pesents the solution poposed in this pape. Finally Section IV shows expeimental esults of this wok. Section V concludes the pape. A. Pediction step II. ISSUES OF EKF VISUAL SLAM The SLAM algoithm begins with standad EKF pediction using popioceptive sensos and the known dynamic model associated to the vehicle. Thus, the cuent position of the obseve can be pedicted: X k+1 k = f(x k k, u k ) (1) whee X k is the state vecto at step k and u k is the vecto of contol inputs. The estimation step of the vaiance-covaiance matix is then expessed as: P k+1 k = F X P k k F T X + Q k (2) whee Q k is the covaiance of pocess noise, F X is the Jacobian of the evolution model f(x k k ) [9], with espect to X k. At this point we obtain the best estimation of the position accoding to the popioceptive infomation. B. Update step In the case of SLAM, the extacted featues ae used to update the vehicle s position and the map. At time k, a set of featues constitutes the map which can be augmented with new obseved featues. On the othe hand, if this is a featue peviously obseved, its location can be pedicted in the obsevation space. The Kalman gain G k+1 and the update pocess in this case can be witten fo each obsevation y as follows: G k+1 = P k+1 k H T X (H X P k+1 k H T X + R obs ) 1 (3) ) X k+1 k+1 = X k+1 k + G k+1 (y h(x k+1 k ) (4) P k+1 k+1 = P k+1 k G k+1 H X P k+1 k (5) whee h is the obsevation model and H X is its Jacobian with espect to the state vecto. R obs is the covaiance of the noise associated with the obsevations. Localization accuacy depends on the numbe of points, the pecision of the initial positioning and the econstuction model. At this stage, the gain has two behavios vesus the obsevation model. If it is linea, the Kalman gain is optimum. Othewise, it is suboptimal and may even lead to divegence mainly due to inappopiate lineaization. C. Visual SLAM In the case of Visual SLAM, the dimension of obsevation space is lowe than that of the state space and the associated model is non-linea. To undestand the consequences, Fig. 2a shows the esult of a 2D featue s position updated with a 1D obsevation. The divegence obseved is the esult of an impope lineaization. The poblem of negative depth was addessed fo invese depth paametization in [11]. The pesented solution includes the case of negative depth. We popose in this aticle to intoduce a coection in the update pocess by putting a constaint on the ational obsevation function in ode to make it moe consistent and to avoid divegence phenomenon. D. Fom 2D to A fist appoach would be to see that if the obsevation model in the space is linea and equal to identity, then fo each 2D obsevation, a vitual point is ceated along the line of sight. Fo each point, a vaiance-covaiance matix is associated, dealing with uncetainty of the pose and the obsevation. With this vitual point, only the coesponding point in the state vecto is updated. The esult of this method is pesented in Fig. 2c. This method has the advantage of woking with a linea obsevation function but at the cost of inceased uncetainty associated with the data. Indeed, consideing the uncetainty as Gaussian in the image, its igoous eto-pojection in the wold is not Gaussian as shown in Fig. 3.

3 3 (a). Update pocess in the 2D space showing divegence Fig. 3. Fo a 2D Gaussian uncetainty in the image (left pat), the epojection in the wold is not Gaussian and coesponds to a cone (ight pat). Fig. 4. point uncetainty appoximation by using the sum of thee Gaussian functions. (b). A zoom on the eal point pove the integity (c). Update step pocessed in the wold with a linea obsevation function Fig. 2. Simulation: let the geen sta be a landmak of the envionment being obseved fom a fist obot s position (left blue tiangle). The position of a featue is initialized along the line of sight (ed coss) with its uncetainty in blue. (a) When obseved fom a second position (ight blue tiangle), the location of the featue (geen cicle) is estimated afte the update pocess in the 2D space showing divegence due to the nonlinea obsevation function. (b) A zoom on the eal point pove it is contained in the uncetainty of the vitual point. (c) The new point (black coss with its uncetainty) is ceated fom the second obsevation but this time the Kalman update step is pocessed in the wold with a linea obsevation function equal to identity. The esult is the black cicle with the uncetainty in ed. One appoach commonly used in this case consists in an appoximation by using the sum of Gaussian functions, each centeed on a diffeent point of the line of sight and tangent to the cone esulting fom the pojection of the measued uncetainty (see Fig. 4). III. PROPOSED APPROACH In ode to diectly initialize the new point, it is placed into the wold efeence fame fom its fist obsevation with an uncetainty coheent with the obsevation eo in the image. Howeve, despite being diect this method has a poblem of lineaization: gap between the ceated and the eal point may be significant. We popose hee to compute a coection in the update step of the Kalman pocess in ode to attenuate the filte divegence (see Section III-A). Its application in a Visual SLAM context is intoduced next in Section III-B. A. Weighted Coefficient When the measuement matches the pedicted obsevation, we have the following elation: whee obsevation = h( ) (6) ( ) = P (k) +Gpt k+1 y h(p (k) ) with Gpt k+1 the pat of the Kalman gain egading to the point. Hypothesis: Let the obsevation function be defined as a atio of two polynomial functions, N and D of P : h( [ N ) = [ D ] ] (7) with is not a oot of D. Using (6) and (7) we have a polynomial sum with espect to : [ obsevation D ] [ N ] = 0 (8) Let us define the gain Ω which satisfies (8). As the 2D- tansfomation is not well-conditioned, we intend to seek a simple popotional elationship between Ω and Gpt k+1. This

4 4 eseach equies that Ω satisfies the same existing conditions as the Kalman gain Gpt: Ω = Gpt = P P t H T ( P t HP t P P t H T P t + R ) 1 ( = P P t H T P P t P t H P t HT P t + R ) 1 = P P t H T ( P t HP t P H T P t + R ) 1 (9) with P = P P t and R = R (10) and whee P P t and H P t ae the pats of the covaiance and the Jacobian matices elated to the point and the obsevation model espectively. As P P t and R ae positive-definite matices, P and R must also be the same. So cannot be negative. Futhemoe, as R is the best estimate of the noise associated with the obsevation, thus R cannot be impoved. It leads to R R 1 and (10) to a polynomial function in, which is the coefficient of popotionality sought between Ω and Gpt k+1. This can be summaized by the following elation: Ω k+1 = Gpt k+1 and min(, 1) (11) Once the value of is found, the coected update step can be pocessed: Ω k+1 = Gpt k+1 ) P k+1 = P k + Ω k+1 (y h(p k ) P P k+1 = P k + Ω k+1 = P P k Ω k+1 H X P P k (12) whee = obsevation pediction is the innovation. When the dimension of the obsevation space is geate than 1, then consideing each coodinate as independent, we can find coections fo them. We will then choose the smallest of them to avoid any oveweighted updates. Finally, the method is summaized in Algoithm 1. B. Obsevation function in Visual SLAM Conside a vehicle moving and obseving its envionment using a camea. Data association povides infomation of matching between the 2D image points (u obs, v obs ) T and the points P of the map. Let us define the igid tansfomation between the wold and the efeence fame of the vehicle as: R = R z (θ k )R y (φ k )R x (ψ k ) and T = (T x k, T y k, T z k ) T. As in [9], P ae pojected in the image, on u est and v est, with the following non-linea elationship: u est = h u (P ) = K 1R T (P T ) K 3 R T (P T ) = N u D e (13) v est = h v (P ) = K 2R T (P T ) K 3 R T (P T ) = N v D e (14) whee K i is the i th line fom the camea matix and h u, h v ae the pat of the obsevation function elated to the u and v coodinates. Algoithm 1: Kalman coection fo non-linea obsevation model Data: Obsevation function h = N[X p ] D[X p ] is a division of two polynomial functions in X p, pat of the state vecto we want to update by the Kalman filte. Result: State vecto updated and coected accoding to specific constaints begin Pediction step: totox k+1 k = f(x k k, u k ) totop k+1 k = F X P k k F T X + Q k toto Kalman gain: totog k+1 = P k+1 k H T X (H XP k+1 k H T X + R obs ) 1 toto Coection: fo each dimension of the obsevation space do )) X p k+1 k+1 = X p k+1 k +G k+1 (y h (X p k+1 k i is the smallest positive oot] of: ] obsevation D [X p k+1 k+1 N [X p k+1 k+1 = 0 Ω k+1 = min( i, 1) G k+1 toto Update step: )) totox p k+1 k+1 = X p k+1 k + Ω k+1 (y h (X p k+1 k totop p k+1 k+1 = P p k+1 k Ω k+1 H XP p k+1 k IV. EXPERIMENTS AND RESULTS In this section, the coection is calculated in the Visual SLAM famewok and esults ae given in thee cases: fistly fo a 2D obseve and a 1D obsevation model, secondly fo a obseve with a 2D obsevation function in simulation and finally, fo a obseve with a 2D obsevation on eal data. A. Coection calculation The update of a point can be witten as follows: The pojection can be expessed as: = P (k) + Gpt k+1 (15) u (k+1) = K 1R T ( + Gpt k+1 T ) K 3 R T ( + Gpt k+1 T ) (16) The obsevation function is witten as a atio of two linea polynomials with espect to Gpt. We can apply the elationship (16) and extact a popotionality coefficient which satisfies the elation between the pojection of the updated point and the cuent obsevation: Ω = u Gpt (17) u obs = K 1R T (P (k) + Ω T ) K 3 R T (P (k) + Ω T ) (18)

5 5 The esolution of this equation can be achieved by solving a fist ode polynomial function. The oot of (18) with espect to u obs coodinate is: u = (u obs u est )D e (K 1 u obs K 3 )R T Gpt (19) Fo a 1D obsevation function, u is the coefficient sought. Fo a 2D obsevation function, a coefficient can be defined, in the same way, with espect to v obs coodinate: v = (v obs v est )D e (K 2 v obs K 3 )R T Gpt (20) Finally, fo 2D obsevation model, the smallest value (less than 1) between u and v will be chosen. B. Results: 2D obseve, 1D obsevation In this section, the obsevation function is defined as the fist component of the 2D obsevation and the function is then simila to h = y/x. The obseve is defined by a position (x, y) and an oientation Θ and the obseved landmak by two coodinates (x p, y p ). Fig. 2a shows the case without coection, and Fig. 5 eveals the esults of the fist update with coection. This figue allows to compae the Kalman update esults afte 10 iteations with and without coection. Fo moe claity, the odinate axis is in logaithmic scale and give the Euclidian distance between the updated point and the eal one. The middle gaphic shows the esult without the coection and confims the divegence at the fist iteation. This divegence is coected and the convegence can be seen in the gaphic below. C. Results: obseve, 2D obsevation in simulation As the obsevation model is a 2D function, two coefficients can be found. The obseve is now defined by a position (x, y, z) and an oientation (Ψ, Φ, Θ) and the obseved landmak by thee coodinates (x p, y p, z p ). In this case, each coodinate in the image allows to compute one coection coefficient. The expeimental scenaio is simple: fom the fist position, a vitual point is ceated in the wold and the state vecto is augmented. This new featue just indicates a line of sight along the eal point which will be placed using tiangulation with the next obsevation when the obseve eaches anothe position. At each innovation given in the image, we assume that it coesponds a point in the fist line of sight by tiangulation. By this way, we can incement the innovation to test the coection. Fig. 6 shows the esult of this expeiment fom 1 to 100 pixels of innovation. D. Results: obseve, 2D obsevation on eal data Fom popioceptive data and the coesponding sequence of images acquied on platfom PAVIN (a small uban ealistic envionment fo intelligent vehicle tests) by a vehicle, the convegence of the localization of a single point is analyzed. Fig. 7 shows in a fist pat a point in an image and the initialization done in the efeence wold, and in a second pat, the convegence of the Kalman pocess with and without coection fo uncetainty at 1σ. The divegence which places Fig. 5. Top: the blue tiangles ae still the two base consecutive obsevation bases. The vitual point ceated fom the obsevation of a eal point (geen sta) on the fist base is still the ed coss with the uncetainty in blue. The obsevation fom the second base leads to an updated point (black cicle) with its uncetainty (ed flat ellipse). The divegence is educed and the integity is guaanteed. Only the fist Kalman update is shown and esults below pove the convegence afte 10 steps. Middle: the distance between each update and the eal 2D point without coection. Bottom: the distance between each coected update and the eal 2D point. the divegence point behind the obseve while it continues to be obseved, is evident hee. This divegence is coected by the popotionality sought. V. CONCLUSION As pat of a Visual SLAM poject, we faced a ational non-linea obsevation function which is simila to h = y/x on each coodinate in the obsevation space, whateve its dimension. The acquisition conditions ae those that we would obtained with low cost senso. The pesented method allows, in the case whee the obsevation function diveges, to calculate a coefficient of popotionality to be applied on the Kalman gain. This coection bings all its impotance since the obsevation function becomes clea while the estimate of the obseved object is impecise. Moeove, in the case of moving at modeate speed, the fame ate of low cost cameas (15 fps) apidly geneates this kind of divegence and is coected by the popotionality sought. The coection method is not specific to the Visual-SLAM and we want to addess the issue in geneal, giving the limits of use when the obsevation function is non-linea and satisfies some weak constaints. ACKNOWLEDGMENTS This wok is suppoted by the Agence Nationale de la Recheche (ANR - the Fench national eseach agency) (ANR

6 6 (a). Desciption of eal envionment Fig. 6. Top: the wold epesentation of the expeiment. Fom a fist base (left blue tiangle), a vitual point (black coss) is ceated on a line of sight (geen line). Fom a second base (ight blue tiangle), the fist line of sight is pojected in the image (geen line in the black ectangle in the bottom ight cone). On this line is chosen the obsevations (ed cosses) and the coesponding point is find by tiangulation (ed line of sight on the main gaphic). Middle: the distance between each update and the eal point without coection. Bottom: the distance between each coected update and the eal point. Rinavec PsiRob ANR-07-ROBO-0006). The authos would like to thank L. Malatee, S. Alizon and all the othe membes of LASMEA who contibuted to this poject. REFERENCES [1] Bailey T., Nieto J., Guivant J., Stevens M., and Nebot E. Consistency of the EKF-SLAM algoithm. In Poc. IEEE Intenational Confeence on Intelligent Robots and Systems, [2] T. Bailey and H.F. Duant-Whyte, Simultaneous Localization and Mapping (SLAM): Pat II - State of the At. Robotics and Automation Magazine, 10 p., [3] Bay H., Tuytelaas T., and Van Gool L. SURF: Speeded up obust featues. In Poc. Euopean Confeence on Compute Vision, [4] J. Castellanos, J. Neia, and J. Tados. Limits to the consistency of EKF-based SLAM. In 5th IFAC Symposium on Intelligent Autonomous Vehicles, [5] Castellanos J., Matinez-Cantin R., Tados J. D., and Neia J.. Robocentic map joining: Impoving the consistency of EKF-SLAM. In Robotics and Autonomous Systems, 55(1), [6] Chekhlov D., Pupilli M., Mayol-Cuevas W., and Calway A. Real-time and obust monocula SLAM using pedictive multi-esolution desciptos. In Poc. 2nd Intenational Symposium on Visual Computing, [7] G. Dissanayake, P. Newman, H.F. Duant-Whyte, S. Clak, M. Csoba, A solution to the simultaneous localization and map building (SLAM) poblem. IEEE Tans. Robotics and Automation, 17(3), pp , [8] H.F. Duant-Whyte, T. Bailey, Simultaneous Localization and Mapping (SLAM): Pat I - The Essential Algoithms. Robotics and Automation Magazine, 9 p., [9] Féaud T., Checchin P., Aufèe R. and Chapuis R. Communicating Vehicles in Convoy and Monocula Vision-based Localization, 7th IFAC Symposium on Intelligent Autonomous Vehicles, Septembe, 7-9, Lecce, Italy [10] Holmes S., Klein G., and Muay D. A squae oot unscented Kalman filte fo visual monoslam. In Poc. Intenational Confeence on Robotics and Automation, (b). Convegence without coection (c). Convegence with coection Fig. 7. Results of convegence in a Visual-SLAM context. 7a: On the left, the envionment and the point we will pecise duing SLAM pocess. On the ight, the initialization of the point in the wold. 7b: Result of the localization of the point by a Kalman filte without coection. Afte the 4 th iteation, the point is always obseved while its localization is behind the obseve. 7c: Result of the localization of the point by a Kalman filte with coection. At the 4 th iteation, the Kalman gain is coected to obtain a best localization. [11] Pasley Matin P. and Julie Simon J. Avoiding Negative Depth in Invese Depth Beaing-Only SLAM, in IEEE/RSJ Int. Conf. on intelligent Robots an Systems, Nice, Fance, [12] R. Smith and P. Cheeseman, On the epesentation and estimation of spatial uncetainty, The Intenational Jounal of Robotics Reseach, vol. 5, no. 4, pp , [13] Thun, S Robotic mapping: A suvey. In Exploing Atificial Intelligence in the New Millenium G. Lakemeye and B. Nebel (eds.), Mogan Kaufmann. [14] Van de Mewe R. and Wan E. The squae-oot unscented Kalman filte fo state and paamete estimation. In Poc. IEEE Intenational Confeence on Acoustics, Speech, and Signal Pocessing, 2001.