Real Time On Board Stereo Camera Pose through Image Registration*

Transcription

1 28 IEEE Intelligent Vehicles Symposium Einhoven University of Technology Einhoven, The Netherlans, June 4-6, 28 Real Time On Boar Stereo Camera Pose through Image Registration* Fai Dornaika French National Geographical Institute (IGN) 946 Saint-Mané, France Angel D. Sappa Computer Vision Center, UAB 893 Bellaterra, Barcelona, Spain Abstract This work aresses the real time estimation of on-boar stereo hea s position an orientation. Unlike existing works which rely on feature extraction either in the image omain or in 3D space, our approach irectly estimates the unknown parameters from the stream of stereo pairs brightness. The pose parameters are estimate by registering the right an left images, which is solve using two optimization techniques: the Differential Evolution algorithm an the Levenberg-Marquart algorithm. We provie experiments an evaluations of performance. Keywors: on-boar stereo camera pose, image registration, ifferential evolution algorithm, non-linear optimization I. INTRODUCTION In recent years, several techniques to on-boar vision pose estimation have been propose [], [2], [3]. Vision system pose estimation is require for any avance river assistance application. The real-time estimation of on-boar vision system pose position an orientation is a challenging task since i) the sensor unergoes motions ue to the vehicle ynamics an the roa imperfections, an ii) the viewe scene is unknown an continuously changing. Of particular interest is the estimation of on-boar camera position an orientation relate to the 3D roa plane. Note that since the 3D plane parameters are expresse in the camera coorinate system, the camera position an orientation are equivalent to the 3D plane parameters. Algorithms for fast roa plane estimation are very useful for river assistance applications as well as for augmente reality applications. The ability to use continuously upate plane parameters (camera pose) will consierably make the tasks of obstacles an objects etection more efficient [4], []. However, ealing with an urban scenario is more ifficult than ealing with highways scenario since the prior knowlege as well as visual features are not always available in these scenes [6]. In general, monocular vision systems avoi problems relate to 3D Eucliean geometry by using the prior knowlege of the environment as an extra source of information. Although prior knowlege has been extensively use to tackle the river assistance problem, it may lea to wrong results. Hence, consiering a constant camera position an orientation is not a vali assumption to be use in urban scenarios, since both of them are easily affecte by roa imperfections or artifacts (e.g., rough roa, spee bumpers), car s accelerations, uphill/ownhill riving, among others. *This work was partially supporte by the Government of Spain uner MEC project TRA /AUT an research programme Consolier Ingenio 2: MIPRCV (CSD27-8). The secon author was supporte by The Ramón y Cajal Program. In [7], authors use a single mounte camera. An extene Kalman filter has been use in orer to infer a state vector incluing the vehicle rigi motion (six egrees of freeom) an the camera pose where the measurements are given by the 8-parameter planar motion fiel an the reaings of the velocity an yaw rate sensors. In the literature, many application-oriente stereo systems have been propose. For instance, the ege base v-isparity approach propose in [3], for an automatic estimation of horizon lines an later on use for applications such as obstacle or peestrian etection (e.g., [8], [9]), only computes 3D information over local maxima of the image graient. A sparse isparity map is compute in orer to obtain a real time performance. Recently, this v-isparity approach has been extene to a u-v-isparity concept in []. In this work, ense isparity maps are use instea of only relying on ege base isparity maps. In [], we have propose an approach for on-line stereo camera pose estimation. Although the propose technique oes not require the extraction of visual features in the images, it is base on ense epth maps an on the extraction of a ominant 3D plane that is assume to be the roa plane. This technique has been teste on ifferent urban environments. The propose algorithm took, on average, 3 ms per frame. As can be seen, existing works aopt the following main stream. First, features are extracte either in the image space (optical flow, eges, riges, interest points) or in the 3D Eucliean space (assuming the 3D ata are built online). Secon, an estimation technique is then invoke in orer to recover the unknown parameters. In this paper, we propose a novel paraigm for on-boar camera pose tracking through the use of image registration. We solve the featureless registration by using two optimization techniques: the Differential Evolution algorithm (a stochastic search) an the Levenberg-Marquart algorithm (a irecte search). Moreover, we propose two tracking schemes base on these optimizations. The avantage of our propose paraigm is twofol. First, it can run in realtime. Secon, it provies goo results even when the roa surface oes not have reliable features such as roa an lane markings, i.e., the approach can be use for both highways an urban scenarios. The rest of the paper is organize as follows. Section II escribes the problem we are focusing on as well as some backgrouns. Section III presents the propose approach in etails. Section IV gives some experimental results an metho comparisons. Section V conclues the paper /8/$2. 28 IEEE. 84

2 II. PROBLEM FORMULATION AND BACKGROUND A. Experimental setup A commercial stereo vision system (Bumblebee from Point Grey ) was use. It consists of two Sony ICX84 color CCDs with 6mm focal length lenses. Bumblebee is a precalibrate system that oes not require in-fiel calibration. The baseline of the stereo hea is 2cm an it is connecte to the computer by a IEEE-394 connector. Figure shows an illustration of the on-boar stereo vision system. The problem we are focusing on can be state as follows. Given a stream of stereo pairs provie by the on-boar stereo hea we like to recover the parameters of the roa plane for every capture stereo pair. Since we o not use any feature that is associate with roa structure, the compute plane parameters will completely efine the pose of the onboar vision sensor. This pose is represente by the 3D plane parameters, that is, the height an the plane normal u = (u x,u y,u z ) T from which two inepenent angles can be inferre (See Figure ). In the sequel, the pitch angle will refer to the angle between the camera optical axis an the roa plane; an the roll angle will refer to the angle between the camera horizontal axis an the roa plane (See Figure ). Due to the reasons mentione above, these parameters are not constant an shoul be estimate online for every time instant. Note that the three angles (pitch, yaw, an roll) associate with the stereo hea orientation can vary. However, only the pitch an roll angles can be estimate from the 3D plane parameters. Camera coorinate system Stereo pair Roa plane u Normal vector Fig.. On-boar stereo vision sensor. The time-varying roa plane parameters an u. θ enotes the pitch angle an ρ the roll angle. B. Image transfer function Before going into the etails of the propose approach, this section will escribe the geometric relation between roa pixels belonging to the same stereo pair the left an right images. It is well-known [2] that the image coorinates of the projections of 3D points belonging to the same plane onto two ifferent images are relate by a 2D projective transform having 8 inepenent parameters homography. In our setup, the right an left images are horizontally [ Left image w * t- Right image Roa segmentation Image registration w * t Roa region (right image) Roa plane parameters Fig. 2. The propose approach consists of two stages: A rough roa segmentation followe by image registration. rectifie 2. Let p r (x r,y r ) an p l (x l,y l ) be the right an left projection of an arbitrary 3D point P belonging to the plane (,u x,u y,u z ). In the case of a rectifie stereo pair where the left an right cameras have the same intrinsic parameters, the right an left coorinates of corresponing pixels belonging to the roa plane are relate by the following linear transform (the homography reuces to a linear mapping) x l = h x r + h 2 y r + h 3 () y l = y r (2) where h, h 2, an h 3 are function of the intrinsic an extrinsic parameters of the stereo hea an of the plane parameters. For our setup (rectifie images with the same intrinsic parameters), those coefficients are given by: h = + b u x (3) h 2 = b u y (4) u x h 3 = bu bv u y + α b u z () where b is the baseline of the stereo hea, α is the focal length in pixels, an (u,v ) is the image center (principal point). Let w be the 3-vector encapsulating the 3D plane parameters, that is, w = u. w = (w x,w y,w z ) T = ( u x, u y, u z )T (6) Note that the vector w fully escribes the current roa plane parameters. The problem can be state as follows. Given the current stereo pair estimate the corresponing 3D roa plane parameters an u or equivalently the vector w. III. APPROACH Since the goal is to estimate the roa plane parameters w for every stereo pair (equivalently the 3D pose of the stereo 2 The use of non-rectifie images will not have any impact on the theory of our evelope metho. However, the image transfer function will be given by a general homography. 8

3 hea), the whole process is invoke for every stereo pair. Figure 2 illustrates the tracking of the stereo hea pose over time. The inputs to the algorithm are the current stereo pair as well as the estimate roa plane parameters associate with the previous frame. The algorithm is split into two consecutive stages. First, a rough roa region segmentation is performe for the right image. Let R enotes this region a set of pixels. Secon, recovering the plane parameters from the rawbrightness of a given stereo pair will rely on the following fact: if the parameter vector w correspons to the actual plane parameters the istance an the normal u then the registration error between corresponing roa pixels in the right an left images over the region R shoul correspon to a minimum. In our work, the registration error is set to the Sum of Square Differences (SSD) between the right image an the corresponing left image compute over the roa region R. The registration error is given by 3 : e(w) = (I r(xr,y r) I l(h x r+h 2 y r+h 3,y r)) 2 (7) (x r,y r) R The corresponing left pixels are compute accoring to the linear transform given by () an (2). The compute x l = h x r +h 2 y r +h 3 is a non-integer value. Therefore, the greylevel, I l (x l,y l ), is set to a linear interpolation of the greylevel of two neighboring pixels the ones whose horizontal coorinates bracket x l. A. Roa segmentation In this section, we briefly escribe how the roa region R is etecte in the right images. Roa segmentation is the focus of many research works [3], [4]. In our stuy, the sought segmentation shoul meet two requirements: (i) it shoul be as fast as possible, an (ii) it shoul be as generic as possible (both urban roas an highways). Thus our segmentation scheme will be a color-base approach which works on the hue an saturation components. The segmentation stage is split into two phases. The first phase is only invoke every T frames for upating the statistical color moel an for obtaining a real-time performance. The secon phase exploits the roa color consistency over short time. Figure 3 shows the segmentation results obtaine with the propose scheme. Detecte roa pixels are shown in white within the ROI of two ifferent frames. Fig. 3. Rapi roa segmentation associate with two frames. 3 This functional error is justifie by the fact that the two images are acquire by two ientical cameras having the same orientation. B. Image registration The optimal current roa parameters are given by w = arg min e(w) (8) w e(w) = (I r(xr,y r) I l(h x r+h 2 y r+h 3,y r)) 2 (x r,y r) R where e(w) is a non-linear function of the parameters w = (w x,w y,w z ) T. In the sequel, we escribe two minimization techniques: i) the Differential Evolution minimization, an ii) the Levenberg-Marquart minimization. The first one is a stochastic search metho an the secon one is a irecte search metho. Moreover, we present two tracking schemes. ) Differential Evolution minimization: The Differential Evolution algorithm (DE) is a practical approach to global numerical optimization that is easy to implement, reliable an fast []. We use the DE algorithm [6], [7] in orer to minimize the error (8). This is carrie out using generations of solutions population. The population of the first generation is ranomly chosen aroun a rough solution. We point out that even the exact solution for the first frame is not known, the search range for the camera height as well as for the plane normal can be easily known. For example, in our experiments, the camera height an the normal vector are assume to be aroun m an (,,) T, respectively. The optimization aopte by the DE algorithm is base on a population of N solution caniates w n,i (n =,...,N) at iteration (generation) i where each caniate has three components. Initially, the solution caniates are ranomly generate within the provie intervals of the search space. The population improves by generating new solutions iteratively for each caniate [7]. The solution is set to the best caniate among the last generation caniates. Calibration. Since the stereo camera is rigily attache to the car, the ifferential evolution algorithm can also be use as a calibration tool by which the camera pose can be estimate off-line. To this en, the car shoul be at rest an shoul face a flat roa. Whenever the car moves, the offline calibration results can be use as a starting solution for the whole tracking process. Note that the calibration process oes not nee to run in real-time. 2) Levenberg-Marquart minimization: Minimizing the cost function (8) can also be carrie out using the Levenberg- Marquart technique [8], [9] a well-known non-linear minimization technique. One can notice that the Jacobian matrix only epens on the horizontal image graient since the right an left images are rectifie. C. Tracking schemes Since the unknown parameters (roa parameters/camera pose) shoul be estimate for every stereo pair, we propose two tracking schemes which are illustrate in Figure 4. The first scheme (Figure 4) is only base on the Differential Evolution minimization. In other wors, the solution for every stereo frame is compute by invoking the whole algorithm where the first generation is generate by iffusing 86

4 Initialization (t=) DE algorithm Tracking (t>) DE algorithm Initialization (t=) DE algorithm Tracking (t>) LM algorithm Fig. 4. Parameter tracking using two schemes. The tracking is only base on the Differential Evolution search. The tracking is base on the Differential Evolution search an on the Levenberg-Marquart search. the previous solution using a normal istribution. A uniform istribution is use for the first stereo frame. The secon scheme (Figure 4) uses the Differential Evolution minimization for the first stereo frame only. It utilizes the Levenberg-Marquart for the rest of the frames where the initial solution for a given frame is provie by the solution w t associate with the previous frame. Although the first scheme might have better convergence properties than the secon scheme, the latter one is better suite for real-time performance since the Levenberg- Marquart algorithm is faster than the Differential Evolution search (the corresponing CPU times are illustrate in Section IV.B). In both tracking schemes, the pose parameters associate with the first stereo pair are estimate by the DE algorithm. IV. EXPERIMENTAL RESULTS The propose technique has been teste on ifferent urban environments since they correspon to the most challenging scenarios. In this section, we provie results obtaine with two ifferent vieos associate with ifferent urban roa structures. Moreover, we provie a performance stuy using synthetic vieos with groun-truth ata. A. Tracke pose parameters The first experiment has been conucte on a sequence corresponing to an uphill riving. The stereo pairs are of resolution Figure epicts the estimate camera height as a function of the sequence frames. Figures epicts the estimate pitch angle as a function of the sequence frames. The otte curves correspon to the first scheme that is base on the Differential Evolution minimization. The soli curves correspon to the secon scheme which is base on both the Differential Evolution algorithm an the Levenberg-Marquart algorithm. As can be seen, the estimate parameters are almost the same for the two propose schemes. However, as we will show, the secon scheme is much faster than the first scheme (the stochastic search). a) Differential Evolution convergence: Figure 6 illustrates the behavior of the Differential Evolution algorithm associate with the first stereo pair of the above stereo sequence. This plot epicts the best registration error (SSD per pixel) obtaine by every generation. The three curves correspon to three ifferent population sizes. The first generation (iteration ) has been built using a uniform sampling aroun the solution = m an u = (u x,u y,u z ) T = (,,) T. The algorithm has converge in five iterations (generations) when the population size was 3 an in two iterations when the population size was 2. At convergence the solution was =.2m an u = (u x,u y,u z ) T = (.3,.99,.2) T. Note that even the manually provie initial camera height has 2cm iscrepancy from the current solution, the DE algorithm has rapily converge to the actual solution. Also, we have run the Levenberg-Marquart algorithm with the same starting solution but we get at convergence =.9m an u = (u x,u y,u z ) T = (.,.99,.2) T. b) Horizon line: In the literature, the pose parameters plane parameters can be use to compute the horizon line. In our case, since the roll angle is very small, the horizon line can be represente by an horizontal line in the image. Once the 3D plane parameters an u = (u x,u y,u z ) T are compute, the vertical position of the horizon line will be given by v h = v + α u y Z α u z u y v α u z u y (9) The above formula is erive by projecting a 3D point (,Y p,z ) belonging to the roa plane an then taking the vertical coorinate v = α Yp Z + v. Z is a large epth value. The right-han expression is obtaine by using the fact that u y is close to one an Z is very large. Figure 7 illustrates the compute horizon line for frames an 99. The whole vieo illustrating the compute horizon line can be foun at asappa/horizonline.avi. c) Occlusions an segmentation errors: In orer to stuy the algorithm behavior in presence of significant occlusions or significant segmentation errors, we conucte the following experiment. We use a vieo sequence corresponing to a flat roa (see Figure 3). We run the propose tracking technique escribe in Section III.B twice (the secon tracking scheme (DE-LM)). The first was a straightforwar run. In the secon run, the right images were moifie to simulate a significant registration error. To this en we set the vertical half of a set of 2 right images to a fixe color. The left images were not moifie. Figure 8 compares the pose parameters obtaine in the two runs. The soli curves were obtaine with the non corrupte images. The otte curves were obtaine when the right images of the same sequence are artificially corrupte. The simulate corruption starts at frame 4 an ens at frame 6. The upper part of the Figure illustrates the stereo pair 4. As can be seen, the only significant iscrepancy has affecte the camera height. Moreover, one can see that the algorithm resumes to provie the correct parameters once the corruption has isappeare. Figure 9 shows the registration error obtaine at convergence as a function of the sequence frames. Figure illustrates the registration error (8) as a function of the camera height while the orientation is kept fixe. Figure illustrates the registration error as a function of the camera pitch angle for four ifferent camera heights. In both figures, the epicte error is the SSD per 87

5 pixel. From the slop of the error function we can see that the camera height will not be recovere with the same accuracy as the plane orientation. This will be confirme in the accuracy evaluation section (see Section IV.C) Scheme (DE) Scheme 2 (DE LM) Camera s pitch angle (egrees) Scheme (DE) Scheme 2 (DE LM) No occlusion Temporary occlusion Stereo pair 4 Camera s pitch angle (egrees) No occlusion Temporary occlusion Fig.. Camera height an orientation compute by the propose tracking schemes Fig. 8. Comparing the pose parameters when a significant occlusion occurs. The soli curves are obtaine with the non corrupte images. The otte curves are obtaine when 2 frames of right images of the same sequence are artificially corrupte. The occlusion is simulate by setting the vertical half of the right images to a fixe color. This occlusion starts at frame 4 an ens at frame 6. Population size = 3 Population size = 6 Population size = No occlusion Temporary occlusion SSD per pixel (SSD per pixel) / Generation Fig. 6. The evolution of the best registration error obtaine by the Differential Evolution algorithm associate with the first stereo pair. Fig. 9. The registration error obtaine at convergence as a function of the sequence frame. The secon tracking scheme is use SSD per pixel Fig. 7. The estimate horizon line associate with frames an 99. The sequence correspons to an uphill riving. B. Metho comparison The secon experiment has been conucte on a short sequence of stereo pairs corresponing to a typical urban environment (see Figure 3). The stereo pairs are of resolution Here the roa is almost flat an the changes in the pose parameters are mainly ue to the car s accelerations an ecelerations. Figures an epict the estimate camera height an orientation as a function of the sequence frames using two ifferent methos. The soli curves correspon to the evelope irect approach (DE-LM) an the ashe curves correspon to a 3D ata base approach []. This approach uses a ense 3D reconstruction followe by a RANSAC-base estimation of the ominant 3D plane the roa plane. One can see that espite some iscrepancies the SSD per pixel = m =. m =.2 m =.3 m Pitch angle (Deg.) Fig.. The registration error as a function of the camera pose parameters. epicts the error as a function of the camera height with a fixe orientation. epicts the error as a function of the camera pitch angle associate with four ifferent camera heights. 88

6 propose irect metho is proviing the same behavior of changes. On a 3.2 GHz PC, the propose approach processes one stereo pair in about 2 ms assuming that the ROI size is 9 9 pixels an the number of the etecte roa pixels is pixels (3 ms for the fast color-base segmentation an about 7 ms for the Levenberg-Marquart minimization). One can notice that this is much faster than the 3D ata base approach, which nees 3 ms. Moreover, the Levenberg- Marquart algorithm is faster than the DE algorithm which nees 2 ms assuming that the number of iterations is an the population number is 3 (the number of pixels is ). Obviously, evoting a very small CPU time for estimating the roa parameters/camera pose is avantageous for realtime systems since the CPU power can be use for extra tasks such as peestrian or obstacle etection. C. Convergence stuy In orer to stuy the convergence behavior of the two optimization techniques we use a synthetic stereo sequence containing stereo frames. For each stereo frame the groun-truth pose parameters are known. Gaussian image is ae to the grey-levels of all images. The stanar eviation of the noise is kept fixe to 4. For every stereo frame in the sequence the starting solution was shifte from the groun-truth solution by 2 cm for the camera height an by egrees for the plane normal. This shifte solution is use as the starting solution for the Levenberg-Marquart technique an as the center of the first generation for the Differential Evolution technique. Table I epicts the average height an orientation errors obtaine with the LM an DE minimizations. As can be seen, the DE minimization has better convergence properties than the LM minimization which essentially looks for a local minimum. This is reason why the two propose tracking schemes use the DE algorithm for estimating the pose parameters associate with the first vieo frame (See Figure 4). stereo frames LM DE Ave. height error (%) Ave. orientation error (egrees).9.4 TABLE I AVERAGE CAMERA POSE ERRORS. V. CONCLUSION A featureless technique for real time estimation of onboar stereo hea pose has been presente. The metho aopts a registration scheme that uses images brightness. The avantages of the propose technique are as follows. First, it oes nee any specific visual feature extraction neither in the image omain nor in 3D space. Secon, it is very fast compare to almost all propose stereo-base techniques. The propose featureless registration is carrie out using two optimization techniques: the Differential Evolution algorithm (a stochastic search) an the Levenberg-Marquart algorithm (a irecte search) Direct metho Direct metho 3D 3D ata ata base metho 3D ata base metho.9 2 Fig.. Camera s pitch angle (egrees) Direct metho 3D ata base metho 2 Camera height an orientation using two ifferent methos. REFERENCES [] X. Liu an K. Fujimura, Peestrian etection using stereo night vision, IEEE Trans. on Vehicular Technology, vol. 3, no. 6, pp , November 24. [2] G. Stein, O. Mano, an A. Shashua, A robust metho for computing vehicle ego-motion, in IEEE Intelligent Vehicles Symposium, 2. [3] R. Labayrae, D. Aubert, an J. Tarel, Real time obstacle etection in stereovision on non flat roa geometry through V-isparity representation, in Proc. IEEE Intelligent Vehicles Symposium, Versailles, France, June 22, pp [4] P. Viola, M. Jones, an D. Snow, Detecting peestrians using patterns of motion an appearance, International Journal of Computer Vision, vol. 63, no. 2, pp. 3 6, 2. [] G. Toulminet, M. Bertozzi, S. Mousset, A. Bensrhair, an A. Broggi, Vehicle etection by means of stereo vision-base obstacles features extraction an monocular pattern analysis, IEEE Trans. on Image Processing, vol., no. 8, pp , 26. [6] U. Franke, D. Gavrila, S. Görzig, F. Linner, F. Paetzol, an C. Wöhler, Autonomous riving approaches owntown, IEEE Intelligent Systems, vol. 3, no. 6, pp. 4, 999. [7] T. Suzuki an T. Kanae, Measurement of vehicle motion an orientation using optical flow, in IEEE Intelligent Vehicles Symposium, 999. [8] M. Bertozzi, E. Binelli, A. Broggi, an M. Del Rose, Stereo visionbase approaches for peestrian etection, in Procs. Computer Vision an Pattern Recognition, San Diego, USA, June 2. [9] R. Labayrae an D. Aubert, A single framework for vehicle roll, pitch, yaw estimation an obstacles etection by stereovision, in Proc. IEEE Intelligent Vehicles Symposium, Columbus, OH, USA, June 23, pp [] Z. Hu an K. Uchimura, U-V-Disparity: An efficient algorithm for stereovision base scene analysis, in Procs. IEEE Intelligent Vehicles Symposium, Las Vegas, USA, June 2, pp [] A. Sappa, D. Gerónimo, F. Dornaika, an A. López, On-boar camera extrinsic parameter estimation, Electonics Letters, vol. 42, no. 3, pp , June 26. [2] O. Faugeras an Q. Luong, The Geometry of Multiple Images. The MIT Press, 2. [3] P. Lombari, M. Zanin, an S. Messeloi, Switching moels for vision-base on-boar roa etection, in IEEE Intelligent Transportation Systems, 2. [4] P. Jansen, W. v.. Mark, J. v.. Heuvel, an F. Groen, Colour base off-roa environment an terrain type classification, in IEEE Intelligent Transportation Systems, 2. [] K. V. Price, J. A. Lampinen, an R. M. Storn, Differential Evolution: A Practical Approach To Global Optimization. Springer, 2. [6] S. Das, A. Konar, an U. Chakraborty, Two improve ifferential evolution schemes for faster global search, in Genetic an Evolutionary Computation, 2. [7] R. Storn an K. Price, Differential evolution A simple an efficient heuristic for global optimization over continuous spaces, Journal of Global Optimization, vol., pp , 997. [8] R. Fletcher, Practical Methos of Optimization. New York: Wiley, 99. [9] W. H. Press, S. A. Teukolsky, W. T. Wetterling, an B. P. Flannery, Numerical Recipes, The Art of Scientific Computing. New York: Cambrige University Press,