3D Model Based Pose Estimation For Omnidirectional Stereovision

Transcription

1 3D Model Based Pose Estimation For Omnidiretional Stereovision Guillaume Caron, Eri Marhand and El Mustapha Mouaddib Abstrat Robot vision has a lot to win as well with wide field of view indued by atadioptri ameras as with redundany brought by stereovision. Merging these two harateristis in a single sensor is obtained by ombining a single amera and multiple mirrors. This paper proposes a 3D model traking algorithm that allows a robust traking of 3D objets using stereo atadioptri images given by this sensor. The presented work relies on an adapted virtual visual servoing approah, a non-linear pose omputation tehnique. The model take into aount entral projetion and multiple mirrors. Results show robustness in illumination hanges, mistraking and even higher robustness with four mirrors than with two. I. INTRODUCTION Robot loalization is a omplex task and vision has shown its interests along last deades. Whatever the kind of amera is, several works have been done to do self-loalization using vision. The hoie of the sensor is ritial for the aimed appliation. Eah of them has pros and ons but some, suh as omnidiretional sensors, have really interesting advantages. Their major interest is to allow to observe a landmark during a long period of time, all around the robot, whih is synonym of preision. Nevertheless, the problem of depth estimation is still present. Stereovision sensors have the interesting property of allowing estimation of the depth. They are, for instane, omposed by two perspetive ameras, as human vision system. Knowing the geometry of the sensor, one an reover a point depth. So an interesting idea is to use a sensor that merges stereovision and omnidiretional vision. This is one of these sensors (a small review of them an be found in [1]) we used for the work presented in this paper. The sensor was designed by Mouaddib et al. in [2]. It is a atadioptri sensor omposed by a unique amera and four paraboli mirrors plaed in a square at the same distane from the amera (Fig. 1). Our aim is to do self-loalization using this sensor sine its potential in terms of robustness and preision is high. We propose to investigate the visual servoing approah to do this task. Visual servoing aims to move a amera usually mounted on the robot end-effetor in order to reah a desired pose using image information. For our problem, one an imagine to virtualize the amera, starting from an initial pose and Guillaume Caron and El Mustapha Mouaddib are with MIS laboratory, University of Piardie Jules Verne, Amiens, FRANCE; {guillaume.aron, mouaddib}@u-piardie.fr Eri Marhand is with INRIA, IRISA, Lagadi, Rennes, Frane; {Eri.Marhand}@irisa.fr Fig. 1. Our sensor: orthographi amera, paraboli mirrors, 90 m height moving to reah the real pose of the amera, still using image information. This is the Virtual Visual Servoing (VVS) framework introdued by Sundareswaran et al. in [4] and then by Marhand et al. in [5], a full sale non-linear optimisation tehnique whih an be used for pose omputation. Some works have been done, for perspetive amera [6], stereo perspetive rig [7] and even for monoular omnidiretional vision [8]. These works also deal with orrupted data, whih is frequent in image feature extration, using the widely aepted statistial tehniques of robust M-estimation [9]. The goal in this paper is to develop robust VVS for omnidiretional stereovision to reover the amera pose and to show qualities and ontributions of the presented sensor for this kind of appliation. We first present sensor modelling before developping the pose estimation approah. After that, the hosen feature type will be takled as well as image traking used method. Finally, experimental results will be presented. II. SENSOR DESCRIPTION AND MODEL A. The Sensor Even if the traking and pose estimation method presented in this paper an be used with any omnidiretional stereovision system, we used a partiular one. It is omposed by a unique amera equipped with a teleentri lens and four paraboli mirrors plaed in a square at the same distane from the amera, so that their axes are parallel to the amera optial axis (Fig. 1). For more details about the design of this sensor, see [3]. Using paraboli mirrors and a teleentri lens, giving an orthographi projetion, allows to keep a entral projetion for eah mirror. So eah individual part of the stereo sensor

2 an be modelled using the equivalent sphere projetion model. B. Unified Central Projetion Model Sine the work of Barreto et al. [10], a unified projetion model for entral projetion ameras was designed. This model desribes a family of ameras from perspetive to atadioptri ones with partiular shape mirrors. The paraboloidal mirror we use is one of them. Aording to this model, a entral projetion amera an be modelled by a first projetion on a sphere with oordinates (0, 0, ξ) in the amera frame followed by a perspetive projetion on the image plane. Suh a model an be defined using parameter ξ whih depends intrinsially of the atadioptri amera mirror parameters. Knowing intrinsi parameters γ = {p x, p y, u 0, v 0, ξ}, a 3D point X = (X, Y, Z) is first projeted on a unitary sphere and then in the image plane as x = (x, y, 1). The relationship between X and x an be expressed as: X x = x = pr γ (X) with Z + ξ X 2 + Y 2 + Z 2 Y y = Z + ξ X 2 + Y 2 + Z 2 (1) x is the point on the virtual normalized plane and the image point in pixelli oordinates is obtained by: C. Stereo Model u v = 1 p x 0 u 0 0 p y v x y 1 (2) We hose to model our stereo sensor inluding one amera and four paraboli mirrors as four ameras, one for eah mirror. Eah of these ameras handles a set of intrinsi parameters γ i. To model our rig, we fix the first amera/mirror as the amera frame origin. Hene, the three other ameras are plaed relatively to the first one. We note 2 M 1, 3 M 1, 4 M 1 these relative poses whih are part of the full alibrated stereo system. One an note this model is expendable with more ameras, knowing eah j M 1. A. Overview III. POSE ESTIMATION APPROACH The proposed approah is based on works of Marhand et al. [7], [8] where the pose omputation problem is defined as a virtual visual servoing one. This virtual servoing proess is similar to a full sale non-linear estimation method of the pose. This method assumes the stereo system alibration parameters are known. B. Method The virtual monoular amera is defined by its projetion funtion pr γ () and its position in the objet frame by an homogeneous matrix M o. A 3D objet have several features o P defined in its own frame. The method will estimate the real pose by minimizing the error between the observed data, or deteted features, s and the position s of the same features omputed by forward-projetion aording to the urrent pose. So with k features, we have = k (pr γ ( M o, o P i ) s i ) 2 (3) i=1 With this formulation, a virtual amera with inital pose 0 M o, is moved using a visual servoing ontrol law to minimize the error. At onvergene, the virtual amera reahes the pose M o whih minimizes. Assuming this non-linear estimation proess onverges, this pose is the real amera pose. C. Stereo Extension As stated before, we want to apply this method to an omnidiretional stereovision sensor onsidered as a rig of four atadioptri ameras. Dionnet et al. in [7], modelled the virtual visual servoing problem for a two perspetive ameras stereo rigid system saying that knowing the system alibration (two sets of intrinsi parameters and the seond amera pose w.r.t. the first), we an rewrite the riterion to take into aount two ameras. It an be generalized to N ameras knowing eah of N sets of intrinsi parameters and eah N 1 relative amera poses w.r.t. the first one. So, we an write k N j = (pr γj ( j M 1 1 M o, o P i ) j s i ) 2 (4) j=1 i=1 with 1 M 1 = I 4 4. With this formulation, only 6 parameters have to be estimated, as for the monoular pose estimation problem. For instane, if N = 2, we retrieve the two ameras ase. In our four ameras stereo ase, N = 4. Anyway, assuming that r is a vetor representation of the pose 1 M o, this remains to minimize a residual defined as k = (s i (r) s i ) 2 = s(r) s 2 (5) i=1 The error to be regulated is hene e = s(r) s. The primitives motion is linked to the virtual amera veloity by ė = λe, only depending of ṡ whih an be written as ṡ = ds dt = s dr r dt = L sv. (6) L s is alled the interation matrix and links the feature motion in the image to the amera veloity v. D. Robust Stereo Pose Estimation In order to have a preise estimation, s must have a suffiient preision. So, as in [7], we use a M-estimator [9] to deal with outliers. A lot of funtions have been proposed in the literature. They allow unertain measures to be less likely onsidered and in some ases ompletely rejeted. To add robust estimation to our objetive funtion, it is modified by k R = ρ(s i (r) s i ), (7) i=1

3 where ρ(u) is a robust funtion that grows subquadratially and is monotonially non-dereasing with inreasing u. Iterative Re-Weighted Least Squares is a ommon method of applying the M-Estimator. Thus, the error to be regulated to 0 is defined, in a matriial form, as: e = D(s(r) s ), (8) where D is a diagonal weighting matrix given by D = diag(w 1,..., w k ). Eah w i is a weight given to speify the onfidene in eah feature loation. The omputation of these weights is desribed in [11]. A simple ontrol law that allows to move a virtual amera an be designed to try to ensure an exponential deoupled derease of e. It is given by: v = λ(dl s ) + D(s(r) s ), (9) where v = (v, ω) is the virtual amera veloity with v, the instantaneous linear veloity and ω the intantaneous angular amera veloity. The interation matrix L s is defined in equation (6) and λ is a gain that tunes the onvergene rate. The omputation of interation matrix will be disussed in setion IV. Considering the minimisation of equation (4), with N = 4, sine we know the relation between mirrors poses ( j M 1 ), we an operate a frame hange of the pose veloity vetor, in order to express it in eah mirror frame. For instane, mirror 1 has a veloity vetor v 1 and mirror j, a veloity vetor v j. We an express v j w.r.t. v 1 : v j = j V 1 v 1, j = 2..4 (10) where j V 1 is the twist transformation matrix: [ j ] j R V 1 = 1 [ j t 1 ] 0 j. (11) R 1 So the feature veloity in image j an be related to the motion of amera 1 by and, for four ameras, we have: ṡ j = L j v j = L j j V 1 v 1, (12) ṡ 1 L 1 ṡ 2 ṡ 3 = L 2 2 V 1 L 3 3 V 1 v 1. (13) ṡ 4 L 4 4 V 1 Finally, we get the following ontrol law, with only six parameters to estimate: + D 1 L 1 D 1 s 1 (r 1 ) s 1 v 1 = λ L 2 2 V 1 s 2 (r 2 ) s 2 D 3 L 3 3 V 1 D 3 s 3 (r 3 ) s. (14) 3 D 4 L 4 4 V 1 D 4 s 4 (r 4 ) s 4 The pose 1 M o is then updated using the exponential map of se(3) (see [12] for details) 1 M t+1 o = 1 M t oe [v1], (15) and poses of the three other ameras are then updated using estimated stereo rig alibration parameters j M 1 : j M o = j M 1 1 M o and will be used in equation (14) to ompute s j (r j ) and j V 1. The feature type hoie and hene the interation matrix expression is a key point of this algorithm and is desribed in setion IV. E. Pose And Calibration Parameters Estimation In equation (6), the system is supposed to be alibrated but it is possible to relax this knowledge adding the alibration parameters to the estimation proess with ṡ = ds dt = s dr r dt + s dγ γ dt. (16) This time, there are two veloity vetors, still the pose one but the intrinsi parameters veloity vetor too, meaning it is possible to vary the intrinsi parameters in the same optimisation proess. Following the same idea, it is possible to relax relative poses j M 1 between ameras and inserting them in the optimisation proess to have a full stereo alibration method. This method is used to alibrate our stereo rig offline using points. A. Feature Type IV. VISUAL FEATURES The traked objet is defined as a 3D model made of 3D lines. So as it has been done in [8] and other papers, we onsider as visual features the distane between a point p, deteted in the image and the projetion of a line, a oni (r) in the image plane, for a given pose. The vetor s(r) will therefore be defined by:. s(r) = s i (r). with s i(r) = d a (x, (r)) (17) where d a () defines the algebrai distane (detailled in setion IV.D.) between point x and the projetion (r) of the 3D line. B. Projetion of a 3D Straight Line As in [8], we modelled a 3D straight line by the intersetion of two planes, one inluding the equivalent sphere enter and the other perpendiular to the first. This is not the only possible 3D line representation, Pluker oordinates or a 3D point and a vetor are other possible representations. These two planes P 1, P 2 are defined by: P 1 : A 1 X + B 1 Y + C 1 (Z ξ) = 0 P 2 : A 2 X + B 2 Y + C 2 Z + = 0 with the following onstraints on the 3D parameters: A B1 2 + C1 2 = 1 A B2 2 + C2 2 = 1 A 1 A 2 + B 1 B 2 + C 1 C 2 = 1 (18) (19) so that the two planes with unit normals N 1 = (A 1, B 1, C 1 ) and N 2 = (A 2, B 2, C 2 ) are orthogonals. Following the projetion model defined by equation (1), the projetion of a straight line in the image is the perspetive projetion of the irle defined as the intersetion between

4 the plane P 1 and the unitary sphere S entered in (0, 0, ξ). Following the proess presented in [8], it is possible to define the oni equation with P 1, P 2 and S parameters by: Q(x, y) = a 0 x 2 + a 1 y 2 + 2a 2 xy + 2a 3 x + 2a 4 y + 1 (20) with a 0 = A2 1 C 2 1 a 2 = A1B1 C1 2 a 4 = B1 (1 ξ 2 ) ξ 2 a 1 = B2 1 (1 ξ 2 ) ξ 2 C1 2 (1 ξ 2 ) a 3 = A1 (21) C 1 C 1 C. Interation Matrix for a Line To ompute the interation matrix, [8] show it is possible to rewrite a 0, a 1 and a 2 using a 3 and a 4 so that their time derivative are funtions of ȧ 3 and ȧ 4. Hene, determining interation matries L a3 and L a4 will lead to the three others. Following [13], and defining α = A2 + C2 a 3, β = B2 + C2 a 4, we obtain for a line and one amera: ] L a4 = [βa 3 βa 4 β 1 + a 24 a 3 a 4 a 3 ] L a3 = [αa 3 αa 4 α a 3 a 4 1 a 23 a 4 L a2 = (1 ξ 2 )(a 3 L a4 + a 4 L a3 ) L a1 L a0 = 2a 4 (1 ξ 2 )L a4 = 2a 3 (1 ξ 2 )L a3 D. Interation Matrix for Point-Coni Distane Feature (22) The hosen feature is the distane between a point and the projetion of a 3D line, a oni in the used projetion system. We deided to use the algebrai distane. Considering a point (x, y), its algebrai distane from a oni is: d a = Q(x, y) (23) with Q(x, y) defined in equation (20). The interation matrix L da related to this distane is given by [8], onsidering the time derivative of d a, d a = ȧ 0 x 2 + ȧ 1 y 2 + 2ȧ 2 xy + 2ȧ 3 x + 2ȧ 4 y (24) we immediately obtain: x 2 y 2 L da = 2xy 2x 2y T L a0 L a1 L a2 L a3 L a4 (25) So there will be four L da, one for eah amera and they will be ombined as shown in equation (14). V. IMAGE PROCESSING To find orresponding edges, we use the moving edge algorithm [14]. The idea is to sample ontours at a regular step and to use an oriented gradient mask to find orresponding ontour by onvolution maximization along a range searh (see [8] for an explanation of this proess). To sample ontours, it is possible to sample regularly the 3D straight line and then projet these 3D sample points in the image. But it is more effiient to diretly sample the oni. Sturm et al. [15] expressed a relationship between a oni and a irle by an eigendeomposition of a oni matrix. Coni parameters are known from equation (20). So we an form its matrix representation, a 0 a 2 a 3 C = a 2 a 1 a 4 (26) a 3 a 4 1 and deompose it C = RΣR T. (27) Then, using Σ and R, the oni to unitary irle transformation T is given by T = Σ 1 R 1 (28) whih first rotates the oni to fit the standard 2D frame and then normalizes its axes to fit the unitary irle. Starting and ending points of the edge are also known so that the range to sample is known. The inverse transformation T 1 is finally applied to eah sample to ome bak on the oni. Eah sample is the starting point of orresponding ontour searh. The moving edge algorithm searhes this orrespondene along the ontour normal at eah sample. To ompute this normal, we an diretly use the partial derivates of Q(x, y) as: Q x 2a 0 x + 2a 2 y + 2a 3 Q y = (29) 2a 1 y + 2a 2 x + 2a 4 VI. RESULTS The algorithm was applied on real image sequenes. We used the ViSP library [16] to develop the algorithm. Frames are aquired at 10 fps with a resolution of pixels and mean proessing time is about 300ms. A. Traking a Box In this experiment, the sensor is immobile and a box (30m 25m 20m) is handheld and moves in a large part of the sensor field of view (Fig. 2), at a distane from the sensor between 35m and 75m. Despite disturbing image gradients in the bakground and illumination variations, the traking is ahieved all along the sequene of 312 images. Of ourse, some sample points are influened by the strong bakground gradients but thanks to robust estimation and robustness indued by stereovison, these ones are rejeted. We also applied traking and pose estimation with our method to only two mirrors of the sensor, the ones with the widest baseline (the top-left and the bottom-right mirrors). These two mirrors should be the ones used in a more lassial stereo sensor: a two mirrors/ameras rig. But with two mirrors the objet is lost from image 277 (Fig. 2(d)). This shows the higher robustness, due to information redundany, brought by the two additional mirrors. The video of this experiment is available from the researh setion in website g-aron/en/.

5 (a) Image 1 (b) Image 149 () Image 312 Fig. 2. (d) Image 312 with two mirrors pose estimation. The objet is lost from image 277. To be ompared with Fig. 2() Images of pose estimation for the handheld box sequene. B. Auto-olusion Another experiment has been made in order to show the higher robustness indued by the four mirrors stereo sensor w.r.t. a two mirrors stereo sensor. A problem of autoolusion an appear when plaing paraboli mirrors on the same plane. They reflet on eah other. This phenomenon reates problemati image zones and these are generally withdrawn thanks to an image mask. We also used this method to avoid traking problems and we made a seond experiment in whih the box starts outside the inter-refletion zone between the two diagonal mirrors, moving through it and ome bak near starting position. The four mirrors stereo pose estimation and traking proess sueed (Fig. 3(a) and 3()) while with two mirrors, the box is lost in the autoolusion zone (Fig. 3(b) and 3(d)). VII. CONCLUSION We have presented a robust 3D model-based pose estimation method using omnidiretional stereovision. Results with our four mirrors sensor show even higher robustness than a two mirrors approah. The ombination of VVS and four mirrors omni-stereo sensor has proven to be robust to disturbing bakground or even to auto-olusion thanks to redundany, all around the sensor. Future works will be foused on the full utilization of the stereo sensor relaxing the onstraint of knowing the 3D model. REFERENCES [1] G. CARON, E. M. MOUADDIB, Vertial Line Mathing for Omnidiretional Stereovision Images, IEEE Int. Conf. on Robotis and Automation, Kobe, Japan, may 2009.

6 (a) Four mirrors : Image 15 (b) Two mirrors : Image 15 () Four mirrors : Image 153 (d) Two mirrors : Image 153 Fig. 3. Images of pose estimation in presene of auto-olusion. The robustness indued by the four mirrors rig allows to deal with auto-olusions. [2] E. MOUADDIB, R. SAGAWA, T. ECHIGO AND Y. YAGI, Stereo Vision with a Single Camera and Multiple Mirrors, IEEE Int. Conf. on Robotis and Automation, Barelona, Spain, april [3] G. DEQUEN, L. DEVENDEVILLE, E. MOUADDIB, Stohasti Loal Searh for Omnidiretional Catadioptri Stereovision Design, Iberian Conferene on Pattern Reognition and Image Analysis, Estoril, Portugal, may [4] V. SUNDARESWARAN, R. BEHRINGER, Visual servoing-based augmented reality, IEEE Int. Workshop on Augmented Reality, San Franiso, USA, nov [5] E. MARCHAND, F. CHAUMETTE, Virtual visual servoing: a framework for real-time augmented reality, EUROGRAPHICS, Saarbrüken, Germany, sept [6] A.I. COMPORT, E. MARCHAND, F. CHAUMETTE, Robust modelbased traking for robot vision, IEEE Int. Conf. on Inteligent Robots and Systems, Sendai, Japan, sept [7] F. DIONNET, E. MARCHAND, Robust Stereo Traking for Spae Appliations, IEEE Int. Conf. on Inteligent Robots and Systems, San Diego, USA, nov [8] E. MARCHAND, F. CHAUMETTE, Fitting 3D Models on Central Catadioptri Images, IEEE Int. Conf. on Robotis and Automation, Roma, Italy, april [9] P.-J. HUBER, Robust Statistis, Wiler, New York, USA, [10] J.P. BARRETO, H. ARAUJO, Issues on the Geometry of Central Catadioptri Images, Int. Conf. on Computer Vision and Pattern Reognition, Hawai, USA, de [11] A.I. COMPORT, E. MARCHAND, M. PRESSIGOUT, F. CHAUMETTE, Real-time markerless traking for augmented reality: the virtual visual servoing framework, IEEE Trans. on Visualization and Computer Graphis, july/aug [12] Y. MA, S. SOATTO, J. KOŠECKÁ, S. SASTRY, An invitation to 3-D vision, Springer, [13] B. ESPIAU, P. RIVES, Closed-loop reursive estimation of 3d features for a mobile vision system, IEEE Int. Conf. on Robotis and Automation, Raleigh, USA, mar [14] P. BOUTHEMY, A maximum likelihood framework for determining moving edges, IEEE Trans. on Pattern Analysis and Mahine Intelligene, may [15] P. STURM, P. GARGALLO, Coni Fitting Using the Geometri Distane, Asian Conferene on Computer Vision, Tokyo, Japan, nov [16] E. MARCHAND, F. SPINDLER, F. CHAUMETTE, ViSP for visual servoing: a generi software platform with a wide lass of robot ontrol skills, IEEE Robotis and Automation Magazine, de 2005.