Robust Ground Plane Tracking in Cluttered Environments from Egocentric Stereo Vision

Transcription

1 Robust Groun Plane Tracking in Cluttere Environments from Egocentric Stereo Vision Tobias Schwarze an Martin Lauer 1 Abstract Estimating the groun plane is often one of the first steps in geometric reasoning processes as it offers easily accessible context knowlege. Especially unconstraine platforms that capture vieo from egocentric viewpoints can benefit from such knowlege in various ways. A key requirement here is keeping orientation, which can be greatly achieve by keeping track of the groun. We present an approach to keep track of the groun plane in cluttere inner-urban environments using stereo vision in real-time. We fuse a planar moel fit in low-resolution isparity ata with the irection of the vertical vanishing point. Our experiments show how this effectively ecreases the error of plane attitue estimation compare to classic least-squares fitting an allows to track the plane with camera configurations in which the groun is not visible. We evaluate the approach using groun-truth from an inertial measurement unit an emonstrate long-term stability on a ataset of challenging inner city scenes. I. INTRODUCTION A funamental requirement for any robotic system which aims at perceiving an unerstaning unknown environments is the ability to acquire information about the coarse scene structure. Being able to istinguish salient foregroun objects from navigable backgroun space constitutes one of the challenging tasks for such systems. Navigable space can often be seen as a continuous surface in a limite area aroun the point of view. This especially hols in structure environments. Consequently, the groun surface is often expresse through geometric moels. These representations offer simple but effective context knowlege which can be incorporate into subsequent reasoning steps, as e.g. emonstrate in [1]. In this work we treat the measuring an tracking of such moel from a free moving stereo camera system. In mobile robotics plane parameter tracking is most frequently treate from mono-vision by tracking sparse features. The homography inuce by the surface seen from ifferent viewpoints is usually estimate to this en [2] an tracke with particle [3] or Kalman filters [4]. Sometimes the interest is not the plane itself but the camera motion [5], [6]. Using stereo vision or epth ata, groun surface estimation is often foun as preprocessing step towars free-space estimation or obstacle etection, where regions of interest are create base on the elevation over groun. Surface moelling is an important builing block in the context of autonomous vehicles to estimate rivable freespace. A popular metho was introuce by Labayrae et al. 1 The authors are with the Institute of Measurement an Control Systems, Karlsruhe Institute of Technology, Karlsruhe, Germany tobias.schwarze@kit.eu Fig. 1: Two typical inner-urban street scenes. using the v-isparity [7], a isparity row histogram in which planar surfaces can be obtaine by 2D line fitting. In its plain form it requires to compensate the camera roll beforehan, which makes it ifficult to apply in unconstraine camera setups. When vehicle fixe camera attitue is given, the necessary aoptions to compensate changes in roll an pitch over time are comparably small, but often more expressive geometric moels like splines [8] or clothois [9] are fitte here ue to the larger range of interest. In more unconstraine setups planar surfaces or groun planes are usually fitte in the isparity omain [10], [11], or can also be estimate irectly from stereo images [12]. When the camera motion is small enough, the last known plane parameters are a sufficient starting point to fin the new parameters by refinement [13]. In a similar setting to ours, [14] assumes small camera roll an uses the v-isparity omain to fin groun support points for a subsequent leastsquares plane fit. All these approaches still require at least a portion of the groun to be visible in every estimation step. Strong camera motion is not taken into consieration, tracking plane parameters only succees as long as interframe camera pose changes are small enough an the groun is still the ominant plane. In this work we consier vision systems that perceive the environment from an egocentric viewpoint. In our case this is a calibrate stereo camera mounte close to a person s hea. There are various challenges of such unconstraine platforms that existing approaches usually o not eal with: First, camera motion is too strong to use the last known estimate as initial guess for parameter optimization as in [13], [12]. Seconly, a ba camera elevation angle often leas to invisibility of the groun for many consecutive frames. Approaches that require the groun to be the ominant plane cannot eal with such situation an also fail in cases of total occlusion ue to objects an scene clutter. Our goal is to estimate an track the groun from a ense but lowresolution stereo isparity image uner these conitions.

2 The next section gives an overview of the components we use to achieve this goal. In Section III we explain our filter framework which fuses all information into the groun plane estimate. We evaluate the approach in Section IV on atasets of cluttere inner-urban scenes with groun-truth measurements from an inertial measurement unit. II. SYSTEM We moel the groun as a planar surface, which appears to be a vali assumption in our scenarios, consiering a limite range of interest of aroun 15m. To obtain a parameter preiction with strong camera movement we estimate the camera motion by visual oometry. We then use two complementary measurements to correct the preiction. First, we fin an optimize plane using a robust least-squares fit in isparity ata. Seconly, we estimate the vertical vanishing irection, which coincies with the normal vector of the (non-incline) groun plane in a calibrate camera. This secon measurement allows us to carry out a correction without the groun plane being actually visible. The iea is probably most similar to [15], here the vanishing irections are use to constrain a homography estimation from omniirectional images. The iniviual steps are etaile in the following. A. Visual Oometry The process of estimating the motion of a camera solely from image ata is commonly referre to as visual oometry (VO). The general goal is to fin the transformation in all six egrees of freeom that relates the camera poses of frame k 1 an k. Various stanalone implementations exist, an overview can be foun in [16]. In this work we employ libviso2 [17]; the frame to frame transformation is provie as a translation vector t an a 3x3 rotation matrix [ R which ] can be written as an affine R t transformation T = 0 T. Since this estimation is one 1 incrementally it is inevitably subject to rift. B. Iterative Least-Squares plane fitting We represent a plane in uvδ space as αu n + βv n + γ δ = 0 (1) using normalize image coorinates u n = u cu f, v n = v cv f, with camera focal length f an principal point (c u, c v ). The initial groun plane is foun using the RANSAC scheme by repeately sampling planes through 3 ranom points. A plane is evaluate by counting the support points with point-to-plane istance αu n +βv n +γ δ smaller than a isparity margin ɛ aroun the plane. Having obtaine an initial solution we can optimize the parameters using a robust iterative least-squares estimator. The set of uvδ plane support points is selecte by valiating the plane equation αu n + βv n + γ δ ɛ. The optimize uvδ-plane parameters are then result of α β = (H T H) 1 H T y (2) γ with H being the measurement matrix [u n1..n v n1..n 1] an y the corresponing isparity measurements. We apply the least-squares estimation a few iterations until no consierable upate in the parameters remains. C. Vanishing point estimation Given a calibrate camera system, lines in 3D-space are projecte to lines in the image plane. The intersection of projecte parallel space lines is known as vanishing point. In the calibrate camera case, each vanishing point efines a irection vector originating from the camera principal point c u, c v. If we assume a non-incline groun plane, the vertical vanishing irection coincies with the normal vector of the groun plane. We estimate this vanishing point following the approach of Tarif [18]. Base on a Canny ege etector, ege crossings are suppresse an connecte points extracte by floo fill region growing. The ege hypotheses are split into straight segments. The remaining segments longer than 30px are fitte with a line moel an constitute the ege list ξ. From ξ we seek the subset of eges ξ vert, which support the expecte irection of the vertical vanishing point. We initialize this irection with the current groun plane normal vector n. Once the vanishing point is initialize we preict its irection using the visual oometry rotation R between frame k an k 1 as n V P,k = R n V P,k 1 (3) To evaluate the support of an ege ξ j for a given vanishing point n V P we efine its error as the orthogonal istance D j (n V P, ξ j ) of one of the line enpoints to the line connecting the vanishing point with the ege centroi [18]. All eges with D < ɛ V P are consiere when upating the vanishing point through minimizing n + V P,k = min D j (n V P, ξ j ) (4) n V P ξ j ξ vert Internally we represent the vanishing irection n V P with spherical coorinates (, ϕ) an also perform minimization in this omain. III. FILTER The aim of our filter framework is to fuse all measurements escribe in the previous section. Note that these input features are selecte in such a way that they provie stochastically inepenent an complementary information about the groun plane. A recursive estimator is esire to enable access to the current best estimate an retain realtime capabilities. A Kalman filter fullfills these requirement an is chosen here. However, we nee to fuse measurements in camera coorinates (plane fit) with measurements in worl coorinates (vanishing irection, egomotion T ), the transformation between which is non-linear. Hence, we apply an extene Kalman filter (EKF) with state transition an observation moel x k = f (x k 1, u k, w k ), (5) z k = h (x k, v k ), (6)

3 x k R n being state of the system at time t k with associate measurements z k R m an f ( ) an h ( ) being the nonlinear process an measurement functions with w k an v k the process an measurement noises. Our system state x = [, ϕ, ] T consists of the groun plane normal n in spherical coorinates, ϕ an the orthogonal camera to plane istance in Eucliean XY Z space. Preiction We use the visual oometry transformation T between last frame k 1 an current frame k to obtain the preicte plane[ parameters ] for the current view. An XY Z- n plane p k 1 = transforms via where p k = ( T 1) T pk 1, (7) ( T 1 ) [ ] T R 0 = ( R T t) T. (8) 1 The accoring process function f ( ) uses T as control input an is calculate as x k = f([, ϕ, ] T, T ) (9) = g sph ( (T 1 ) T g 1 sph ( [, ϕ, ] T )) (10) where g sph ( ) transforms the Eucliean plane representation into spherical coorinates ([ ]) arccos(n ϕ n z ) = g sph = atan2(n y, n x ) (11) Measurement preiction The current state preiction x k is use as starting point for the iterative least-squares estimation as escribe in section II-B. This requires to transform x to normalize uvδ plane parameters [α, β, γ] via α β = Bf n = g uvδ ϕ = Bf sin () cos (ϕ) sin () sin (ϕ) γ cos () (12) with stereo baseline B an camera focal length f. Correction From (2) we obtain a measurement m LS = [α, β, γ] T. The vanishing point estimation accoring to Section II-C results in a measurement m V P = [, ϕ] T. Using g uvδ (x k ) as filter measurement preiction we apply a correction step with m LS. The vanishing irection measurement m V P is irectly applie as aitional correction step. Alternatively both can be combine in one step. As long as the camera elevation w.r.t. to the groun plane stays below a fixe angle (in our setup 75, generally epening on the vertical camera opening angle) the groun plane is still visible an a least-squares groun plane measurement can be obtaine. In this case we correct the state using both measurements. Whenever the camera is tilte further away we only consier m V P to correct the groun plane attitue. Fig. 2: Measurement setup for parameter variance estimation. Left: plane support points are coloure green, eges supporting the vertical vanishing point are highlighte pink. Right: Corresponing isparity image at half-resolution. We valiate the correction by setting a valiation gate aroun e 2 = r T S 1 r with upate resiual r an measurement preiction covariance S. IV. EXPERIMENTS Our experimental setup consists of a calibrate stereo rig with a baseline of aroun 18 cm an wie-angle lenses of 3.5 mm focal length mounte on a helmet. Having realtime applicability on a mobile platform in min we capture images with 640x480px at 30 fps. We use libviso2 [17] to estimate the camera motion an acquire the isparity with off-the-shelf estimators like the OpenCV semi-global matching (SGBM) at half resolution (320x240px). Visual oometry an isparity estimation are one parallel with aroun 15 fps, the subsequent vanishing point estimation an least-squares fitting are carrie out with the same rate on an i7 ual-core notebook with 2.4GHz. All experiments are one in real-time, i.e. frames are skippe when processing is slower than the capture rate of 30 fps. A. Process an Measurement noise In orer to parameterize the process an measurement noise w k an ɛ k of the EKF we empirically estimate the measurement variances of the least-squares parameter fit for α, β, γ, the vanishing point irection V P, ϕ V P an the uncertainty in estimate camera motion in a set of experiments. For the uvδ-plane fitting we re-estimate the plane parameters 1000 times from a fixe camera position (see Figure 2), using the same initial solution for the support point selection. In the same manner we obtain variances for V P, ϕ V P by re-evaluating (4) from an initial solution. For a uvδ-plane fitte in a half-resolution SGBM isparity image the parameters vary with σ α = σ β = σ γ = The vanishing point irection varies with σ V P = σ ϕv P = The process noise nees to cover the uncertainty in parameter preiction using the estimate camera motion. While there is no straight forwar uncertainty estimation for each single visual oometry estimation, we approximate its stanar eviation of rotation an translation by evaluating short sequences on the KITTI [19] oometry ataset an compare it to grountruth ata. We fin

4 TABLE I: Absolute angular eviation in egrees from IMU grountruth for ifferent isparity estimators compare to conventional least-squares plane fitting as baseline. ϕ SGBM 1 LS baseline 1.25 ± ± 1.21 Filter LS + VP SGBM 0.64 ± ± 0.69 libelas ± ± 0.52 libtoast2 [20] 0.82 ± ± 0.65 libtoast2 w/o subpix 0.81 ± ± 0.59 ϕ Fig. 3: Groun plane parameters (, ϕ, ) preicte by visual oometry without correcting rift. Dashe lines are the IMU groun truth, soli lines the preicte parameters. LS baseline ϕ ϕls baseline σ R = σ t = Using process noise istribute with σ R for an ϕ an σ t for the istance parameter puts much weight onto the preiction. In situations, in which the actual uncertainty is larger this quickly leas to tracking-loss, since the rifting preiction cannot be correcte fast enough. Due to our unconstraine platform the accumulate oometry error as up much higher than in the KITTI benchmark use for evaluation, we therefore increase the process noise to σ R = an σ t = B. Evaluation against IMU Groun-Truth We mounte an inertial measurement unit (IMU) close to the camera setup in orer to evaluate the accuracy of the groun plane attitue ( an ϕ) estimation. The IMU use is an Xsens MTi-300 which fuses the sensor measurements internally into a global orientation in form of a rotation matrix R IMU. From R IMU we can extract the vector of gravity, which correspons to our estimate groun plane normal. As long as the unit is expose only to short translational accelerations as in our case, the gravity reference IMU reaings can be consiere free of rift an are precise enough to be use as basis for comparison here. The IMU is externally calibrate to the left camera through a rotation M, which allows to etermine the vector of gravity in camera coorinates n IMU as the thir row of MR IMU. We apply (11) to convert it to the spherical representation. We capture a ataset of 4740 frames at 30 fps along with the IMU orientation for every camera frame. Some example frames are shown in Figure 6. Filter Preiction: Figure 3 shows the parameter preiction using visual oometry. After initializing the groun plane using RANSAC in frame 3000 we preict the parameters but o not carry out any correction. The parameter rift becomes obvious after a few secons. The rift in attitue leas to a strong rift in the istance parameter, which is cause by the straight horizontal translation that is applie to the camera. If this rift is not correcte, the support point selection for least-squares fitting quickly becomes erroneous an eventually the plane track gets lost Fig. 4: Distribution of angular error in an ϕ with an without inclusion of vanishing irection. The roll parameter ϕ benefits most, since objects in lateral irection often isturb the least-squares fit. Filter Correction: For each filter upate we measure the absolute eviation in egrees from the IMU groun truth for both attitue parameters. We evaluate the tracking over the whole sequence of 4740 frames with ifferent isparity estimators, Table I shows the mean an stanar eviation. As baseline for comparison we use the mere least-squares plane fit on the preicte plane as filter measurement, this correspons to conventional groun plane fitting methos as in [10], [11] or [14]. The results after aing the vertical vanishing irection are liste in the box below. Both attitue parameters benefit significantly from the aitional measurement, particularly the roll parameter ϕ becomes more accurate. Figure 4 shows the istributions of parameter eviations, where this also becomes apparent. This effect can be explaine by ifferent facts. First, the groun plane is often occlue by cars or builings in lateral irection which isturb the least-squares fit ue to imprecise support point selection. Seconly, the planar surface assumption often oes not hol perfectly, e.g., when walking on a slightly elevate pavement, resulting in tilte measurements. See Figure 5 for an example. This section of the ataset lasts aroun 1000 frames. The absolute errors here are 1.57 for an 3.08 for ϕ with the baseline approach an ecrease to 0.73 for an ϕls baseline ϕ 3 +3 Fig. 5: Effect of planar surface moel violation with an without vanishing irection measurement. Groun plane support points are coloure green an the plane is overlaye schematically. The IMU groun-truth virtual horizon is rawn in re, the measure virtual horizon in green. The accoring error istribution for a sequence of 1000 frames aroun the epicte scenario is shown on the left.

5 ϕ Fig. 6: Groun plane parameters (, ϕ, ) for the evaluation ataset correcte with our filter combining least-squares groun plane fit (LS) an vanishing irection (VP). The top row contains exemplary frames from the sequence for ϕ after aing the vanishing irection. Finally, the filtere parameters for the whole ataset are plotte in Figure 6. To test the long-term stability we ran the groun plane estimation on a ataset consisting of 45 minutes walking through inner-urban scenes, which was mostly capture on narrow sie-walks between house facaes an cars, but also contains some vast spaces with persons an objects frequently occluing the free view onto the groun. The lighting conitions were challenging with consierably over- an unerexpose image areas that lack isparity measurements. Figures 7, 8 an 9 contain some example shots. The ap- proach enables the system to keep track of the groun plane throughout the whole sequence (except for major violations of the continuous, non-incline surface assumption, as e.g. on stairways). The ataset we use for quantitative comparison in Section IV-B oes not contain scenes with total occlusion or the groun plane out of view. This is ifferent here an leas to various situations in which the baseline least-squares approach looses track. In the right scene of Figure 8 a passing car blocks the view onto the street for a couple of frames, the isparity clutter causes the plane to rift away. The vanishing irection effectively helps to keep the correct attitue in such cases. Figure 9 emonstrates a sequence with the groun out of view ue to ba camera inclination. The rift in preiction Fig. 7: Example shots from the city ataset. Fig. 8: Typical failure examples without vanishing point correction. C. City ataset

6 Fig. 9: Sequence with groun plane not in free view ue to camera inclination. Only the vanishing irection is consiere here to correct the plane parameters. uring this situation is too strong to remeasure the plane successfully afterwars again, when the vanishing irection is not consiere. Our filter approach ismisses the erroneous plane fit an uses only the vanishing irection in such case. V. CONCLUSION In this work we present an approach to track a planar groun plane from an unconstraine stereo camera platform in inner-urban environments. The propose framework is able to eal with challenging situations in which existing approaches fail. These situations inclue strong camera motion an frequent invisibility of the groun ue to ba camera orientation or heavy occlusions. We propose to take visual oometry into the tracking loop in orer to obtain a parameter preiction for the current estimate. To correct the preiction rift we combine two complementary measurements: A traitional least-squares plane fit in isparity ata when the groun is visible, augmente by the irection of vertical scene structures to provie a measurement of the groun plane normal vector. Note that the irection of vertical scene structures oes not epen on the visibility of the groun an can therefore be extracte even if the groun is occlue or out of sight. Moreover, even if the groun is visible an a least-squares fit can be calculate the aitional feature of vertical scene structures provies a secon, stochastically inepenent cue for estimation of the groun plane an thus, improves the groun plane estimation accuracy. On a sequence of cluttere inner-urban street scenes we unerpin this by comparing our results quantitatively to groun-truth taken from an inertial measurement unit an successfully test the long term stability on a 45 min sequence without loosing track of the groun. Current work focuses on extening the framework to unstructure environments an situations which violate the non-incline an continuous groun surface assumption, as for instance in case of rop-offs or stairways. ACKNOWLEDGMENT The work was supporte by the German Feeral Ministry of Eucation an Research within the project OIWOB. The authors woul like to thank the Karlsruhe School of Optics an Photonics for supporting this work. REFERENCES [2] J. Zhou an B. Li, Homography-base groun etection for a mobile robot platform using a single camera, in Robotics an Automation, ICRA Proceeings 2006 IEEE International Conference on, May 2006, pp [3] A. Linarth, M. Brucker, an E. Angelopoulou, Robust groun plane estimation base on particle filters, in Intelligent Transportation Systems, ITSC th International IEEE Conference on, Oct 2009, pp [4] V. Chari an C. Jawahar, Multiple plane tracking using unscente kalman filter, in Intelligent Robots an Systems (IROS), 2010 IEEE/RSJ International Conference on, Oct 2010, pp [5] G. Simon, A. Fitzgibbon, an A. Zisserman, Markerless tracking using planar structures in the scene, in Augmente Reality, (ISAR 2000). Proceeings. IEEE an ACM International Symposium on, 2000, pp [6] D. Cobzas, M. Jägersan, an P. Sturm, 3D SSD tracking with estimate 3D planes, Image an Vision Computing, vol. 27, no. 1, pp , Jan [7] R. Labayrae, D. Aubert, an J.-P. Tarel, Real time obstacle etection in stereovision on non flat roa geometry through v-isparity representation, in Intelligent Vehicle Symposium, IEEE, vol. 2, june 2002, pp vol.2. [8] A. Weel, H. Baino, C. Rabe, H. Loose, U. Franke, an D. Cremers, B-spline moeling of roa surfaces with an application to free-space estimation, Trans. Intell. Transport. Sys., vol. 10, no. 4, pp , Dec [9] S. Neevschi, R. Danescu, D. Frentiu, T. Marita, F. Oniga, C. Pocol, T. Graf, an R. Schmit, High accuracy stereovision approach for obstacle etection on non-planar roas, in in IEEE Inteligent Engineering Systems (INES, 2004, pp [10] S. Se an M. Bray, Groun plane estimation, error analysis an applications. Robotics an Autonomous Systems, vol. 39, no. 2, pp , [11] N. Chumerin an M. M. Van Hulle, Groun Plane Estimation Base on Dense Stereo Disparity, in The Fifth International Conference on Neural Networks an Artificial Intelligence (ICNNAI), Minsk, Belarus, May 2008, pp [12] J. Corso, D. Burschka, an G. Hager, Direct plane tracking in stereo images for mobile navigation, in Robotics an Automation, Proceeings. ICRA 03. IEEE International Conference on, vol. 1, Sept 2003, pp vol.1. [13] P. Lombari, M. Zanin, an S. Messeloi, Unifie stereovision for groun, roa, an obstacle etection, in Intelligent Vehicles Symposium, Proceeings. IEEE, June 2005, pp [14] T.-S. Leung an G. Meioni, Visual navigation ai for the blin in ynamic environments, in Computer Vision an Pattern Recognition Workshops (CVPRW), 2014 IEEE Conference on, June 2014, pp [15] J. C. Bazin, P. Laffont, I. Kweon, C. Demonceaux, an P. Vasseur, An original approach for automatic plane extraction by omniirectional vision, in 2010 IEEE/RSJ International Conference on Intelligent Robots an Systems, October 18-22, 2010, Taipei, Taiwan, 2010, pp [16] D. Scaramuzza an F. Fraunorfer, Visual oometry [tutorial], Robotics Automation Magazine, IEEE, vol. 18, no. 4, pp , Dec [17] A. Geiger, J. Ziegler, an C. Stiller, Stereoscan: Dense 3 reconstruction in real-time, in IEEE Intelligent Vehicles Symposium, Baen- Baen, Germany, June [18] J.-P. Tarif, Non-iterative approach for fast an accurate vanishing point etection, in Computer Vision, 2009 IEEE 12th International Conference on, Sept 2009, pp [19] A. Geiger, P. Lenz, an R. Urtasun, Are we reay for autonomous riving? the kitti vision benchmark suite, in Conference on Computer Vision an Pattern Recognition (CVPR), [20] B. Ranft an T. Strauß, Moeling arbitrarily oriente slante planes for efficient stereo vision base on block matching, in Intelligent Transportation Systems, 17th International IEEE Conference on, Qingao, China, Oct [1] B. Leibe, N. Cornelis, K. Cornelis, an L. Van Gool, Dynamic 3 scene analysis from a moving vehicle, in Computer Vision an Pattern Recognition, CVPR 07. IEEE Conference on, June 2007, pp. 1 8.