L A Li

Diosi

S Šegvić

F Chaumette
email: francois.chaumette@irisa.fr

Experimental Evaluation of Autonomous Driving

Based on Visual Memory and Image-Based Visual Servoing

Albert Diosi, Siniša Šegvić, Anthony Remazeilles, and François Chaumette Abstract-In this paper, the performance of a topologicalmetric visual-path-following framework is investigated in different environments. The framework relies on a monocular camera as the only sensing modality. The path is represented as a series of reference images such that each neighboring pair contains a number of common landmarks. Local 3-D geometries are reconstructed between the neighboring reference images to achieve fast feature prediction. This condition allows recovery from tracking failures. During navigation, the robot is controlled using image-based visual servoing. The focus of this paper is on the results from a number of experiments that were conducted in different environments, lighting conditions, and seasons. The experiments with a robot car show that the framework is robust to moving objects and moderate illumination changes. It is also shown that the system is capable of online path learning. Index Terms-Localization, mapping, path following, visual memory, visual servoing.

I. INTRODUCTION

I NTELLIGENT autonomous vehicles have performed amazing feats outdoors. They have driven thousands of kilometers on freeways [START_REF] Pomerleau | Rapidly adapting machine vision for automated vehicle steering[END_REF], navigated on the surface of Mars [START_REF] Cheng | Visual odometry on the Mars exploration rovers-A tool to ensure accurate driving and science imaging[END_REF], and driven more than 200 km on a challenging desert route [START_REF] Thrun | Stanley, the robot that won the DARPA grand challenge[END_REF]. Systems based on visual odometry, stereo vision, and inertial measurement units have achieved significantly high precision, e.g., an error of only 9 m after a 9-km travel [START_REF] Konolige | Large-scale visual odometry for rough terrain[END_REF]. Even monocular-vision-based map building is in the realm of mapping whole suburbs [START_REF] Milford | Mapping a suburb with a single camera using a biologically inspired SLAM system[END_REF] or rapidly performing loop closure detection on images collected over a 1000-km path [START_REF] Cummins | Highly scalable appearance-only SLAM-FAB-MAP 2.0[END_REF]. However, reliable autonomous navigation outdoors using one camera and no other sensor still remains an exciting challenge.

One of the approaches for autonomous navigation using monocular vision is visual path following. In visual path following, a path to follow can be represented by a series of reference images and corresponding robot actions (e.g., go forward, turn left, and turn right), as discussed in [START_REF] Matsumoto | Visual navigation using viewsequenced route representation[END_REF], where a mobile robot navigated through indoor corridors by applying template matching to current and reference images and using the stored actions. However, storing the robot actions is not necessary for navigation. In [START_REF] Royer | Outdoor autonomous navigation using monocular vision[END_REF], a robot navigates a 127-m-long path outdoors while saving only a series of images from a camera with a fish-eye lens. To enable pose-based control of the robot in a global metric coordinate frame, a precise 3-D reconstruction of the camera poses is performed of the frequently (approximately every 70 cm) saved reference images. In the 3-D reconstruction process applied to feature points of the reference images, global bundle adjustment is used, which results in a long (1-h) learning phase unsuitable for online use. The length of the path measured by odometry is used to correct the scale of the map. After learning the path, the robot can very accurately reproduce it at a 50-cm/s velocity.

It turns out that reconstructing the robot's path or having 3-D information is not necessary. In [START_REF] Chen | Qualitative vision-based mobile robot navigation[END_REF], a robot navigated 140 m outdoors at a speed of 35 cm/s with only 2-D image information. During mapping, image features were tracked, and their image patches together with their x-image coordinates were approximately saved every 60 cm traveled. During navigation, the robot control was based on simple rules applied to the tracked feature coordinates shared between the next reference and the current image. The robot, however, relied on frequent reference image switches to recover from occlusions due to moving objects. A person who walks across the camera's field of view between two reference image switches could have caused a problem due to covering up each tracked feature. In a later work [START_REF] Chen | Qualitative vision-based path following[END_REF], the authors in [START_REF] Chen | Qualitative vision-based mobile robot navigation[END_REF] added odometry to compensate for roll on a nonflat terrain.

The work described in [START_REF] Goedeme | Feature-based omnidirectional sparse visual path following[END_REF] aimed at indoor navigation and can deal with occlusion but at the price of using 3-D information. A local 3-D reconstruction is done between two reference omnidirectional images. During navigation, tracked features that have been occluded get projected back into the current image. The recovered pose of the robot is used to guide the robot toward the target image.

Similar to [START_REF] Goedeme | Feature-based omnidirectional sparse visual path following[END_REF], in the work described in [START_REF] Booij | Navigation using an appearance-based topological map[END_REF], indoor navigation is performed using omnidirectional vision. However, the epipolar geometry is only calculated to validate that the scale-invariant feature transform (SIFT) descriptor [START_REF] Lowe | Distinctive image features from scale-invariant keypoints[END_REF] matches between the current and reference image keypoints and to calculate the required heading direction. In a thorough experimental evaluation, the authors demonstrated that their system can plan and execute motions in pure-appearance-based topological maps, whereas significant part of the robot's view was covered up by moving people.

Recently, Courbon et al. in [START_REF] Courbon | Autonomous navigation of vehicles from a visual memory using a generic camera model[END_REF] have successfully demonstrated outdoor visual path following on a 754-m-long outdoor track using a pose-based control strategy. Their topological map consisted of reference images. During navigation, the robot pose was estimated using homography recovery applied to images covering a 185 • field of view. A step toward commercialization is presented in [START_REF] Courbon | Indoor navigation of a nonholonomic mobile robot using a visual memory[END_REF], because a similar framework is applied to an indoor robot with a potentially inexpensive processing unit that entails an ARM9 microprocessor and a field-programmable gate array (FPGA).

Not all robots in the visual-path-following literature use manually controlled map acquisition. In [START_REF] Fontanelli | Visual servoing in the large[END_REF], the robot generated an indoor topological-metric map by performing random motions. During mapping, the 3-D positions of point features of individual reference images were estimated using visual and odometry measurements fused together in a Kalman filter. The estimation of point feature positions continued during navigation.

Convincing experimental results for outdoor visual path following using omnidirectional vision and odometry are presented in [START_REF] Zhang | Robust appearance-based visual route following in large scale outdoor experiments[END_REF]. In the simple effective approach, 1-D localization along the path is performed using a particle filter in conjunction with odometry and an effective patch-normalized implementation of correlation-based image matching. In the thorough experimental results, the accuracy and the effects of illumination were investigated.

Building an accurate and consistent 3-D representation of the environment can also be done using monocular simultaneous localization and mapping (SLAM) [START_REF] Davison | MonoSLAM: Real-time single-camera SLAM[END_REF]. For example, in [START_REF] Lemaire | Vision-based SLAM: Stereo and monocular approaches[END_REF], a robot mapped a 100-m path outdoors using a monocular camera and odometry. There were only 350 features in the map, which may approach the limit that a simple Kalman filter SLAM implementation can handle in real time on current PCs. However, the simulation result in [START_REF] Frese | Closing a million-landmarks loop[END_REF] of closing millions of landmark loops and building large hierarchical maps with monocular SLAM [START_REF] Clemente | Mapping large loops with a single hand-held camera[END_REF] predicts that monocular SLAM may be a viable choice for creating accurate maps with large numbers of landmarks.

In this paper, a visual-path-following framework is presented to the research community in intelligent transportation systems. General concepts such as representing paths as a series of images and extracting these series of images from an image database were presented in [START_REF] Remazeilles | Image-based robot navigation from an image memory[END_REF]. This paper, on the other hand, is oriented toward applying the same general idea for controlling real autonomous cars. An account of the employed vision system has previously been presented in [START_REF] Segvic | A mapping and localization framework for scalable appearance-based navigation[END_REF]. In this paper, the presented experiments describe the behavior of the system in several different outdoor environments and thus provide qualitative and quantitative insights into the feasible range of navigation performance. In addition, this paper presents a more advanced implementation of the system, with a refined control law and an improved implementation of the vision system. Consequently, the system presented in this paper exhibits faster, smoother, and safer motions and can perform online mapping.

The contribution of this paper, based on [START_REF] Diosi | Outdoor visual path following experiments[END_REF], 1 is the application of the vision system to a robotic vehicle using an imagebased visual servoing strategy and the experimental exploration of the implementation's limits. 2 Experiments were mostly carried out on roads using an autonomous electric vehicle that can carry two passengers.

The presented framework is similar to [START_REF] Goedeme | Feature-based omnidirectional sparse visual path following[END_REF], because only local 3-D reconstruction is used, and occluded features get projected back into the image. However, the rest of the details are different. For example, in this paper, a standard camera is used instead of an omnidirectional camera, tracking is used for mapping instead of matching, experiments are done outdoors and not indoors, and the centroid of image features is used to control the robot.

This paper is organized as follows. A description of the framework is given in Section II. More details about the vision system for the interested reader are given in Section III, followed by a description of the experiments. After a discussion of the results, this paper ends with conclusions.

II. VISUAL NAVIGATION

This section briefly describes the visual navigation framework that we implemented. The teaching of the robot (mapping) is first described, followed by the description of the navigation process that consists of localization and robot control.

A. Mapping

Learning a path (i.e., mapping) starts with the manual driving of the robot on a reference path while processing (or storing for offline mapping) the images from the robot's camera. Based on the images, an internal representation of the path is created, as summarized in Fig. 1. The mapping starts with finding Harris points in the first image, initializing a Kanade-Lucas-Tomasi (KLT) feature tracker [START_REF] Shi | Good features to track[END_REF] and saving the first image as the first reference image. A version of the KLT3 tracker was modified, as proposed in [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF], to improve performance in outdoor sequences acquired from a moving car. In the tracker, the position, scale, and contrast parameters of features are tracked. In the next step, a new image is acquired, and the tracked features are updated. The tracking of features with a large appearance change compared to their reference image appearance is abandoned. The rest of the features are then used to estimate the 3-D geometry between the previous reference and the current image. In the 3-D geometry estimation, the essential matrix is recovered using the calibrated five-point algorithm4 [START_REF] Nister | An efficient solution to the five-point relative-pose problem[END_REF] used in the maximum likelihood estimation sample consensus (MLESAC) [START_REF] Torr | MLESAC: A new robust estimator with application to estimating image geometry[END_REF] random sampling framework. The inlier points are then used in a final 3-D geometry calculation using the eight-point algorithm [START_REF] Torr | MLESAC: A new robust estimator with application to estimating image geometry[END_REF]. If the 3-D reconstruction error is low and there are enough tracked features, a new image is acquired. Otherwise, the previous image is saved as the new reference image. The relative pose of the previous image with respect to the previous reference image and the 2-D and 3-D coordinates of the point features shared with the previous reference image are also saved. Then, the tracker is reinitialized with new Harris points added to the old points, and the processing loop continues with a new image acquired by the camera.

To handle gaps in the image sequence and to close a loop between the first and the last images of the teaching sequence, wide-baseline matching is utilized, as described in Section III.

The resulting map (see Fig. 2) is used during autonomous navigation in the localization module to provide stable image points for image-based visual servoing.

B. Localization

The localization process during navigation is depicted in Fig. 3. The navigation starts with initial localization, where the user selects a pair of reference images close to the robot's current location. Then, an image is acquired and matched to the selected reference images. Wide-baseline matching is done using a correlation-based approach [START_REF] Zhang | A robust technique for matching two uncalibrated images through the recovery of the unknown epipolar geometry[END_REF] and by matching SIFT descriptors [START_REF] Lowe | Distinctive image features from scale-invariant keypoints[END_REF] applied to difference of Gaussians (DoG) [START_REF] Lowe | Distinctive image features from scale-invariant keypoints[END_REF], multiscale Harris [START_REF] Mikolajczyk | Scale and affine invariant interest point detectors[END_REF], and maximally stable extremal regions (MSER) [START_REF] Matas | Robust wide-baseline stereo from maximally stable extremal regions[END_REF] keypoints. The estimation of the camera pose using the matched points enables us to project map points from the reference images into the current image. The projected points are then used to initialize a KLT tracker.

After the initial localization, a new image is acquired, and the point positions are updated by the tracker. Using the tracked points, a three-view geometry calculation (see Section III) is performed between the current image, the previous reference, and the next reference image (see Fig. 2). If the current image is found to precede the next reference image, then points from the map are projected into the current image using the estimated local pose. The projected points enable us to restart the tracking of points that are currently not tracked and to stop the tracking of points that are far from their projections. A new image is acquired next, and the whole cycle continues. However, if it is found that the current image comes after the next reference image, a topological transition is made, i.e., the second next reference image (see Fig. 2) becomes the next reference image. The tracker is then reinitialized with points from the map, and the process continues with acquiring a new image. Similar to forward transitions, a topological transition is performed backward if the current image precedes the previous reference image.

To achieve seamless switching between nodes, points from the second next, previous, and second previous reference images are also tracked in the current image (see Fig. 2).

Wide-baseline matching is only used outside the initial localization phase if most features are lost, e.g., due to a total obstruction of the camera's field of view. In such a case, the robot stops, and automatic reinitialization is carried out by matching with the nearest reference images.

C. Motion Control

In the motion control scheme, the robot is not required to accurately reach each reference image of the path or to accurately follow the learned path, because this condition may not be useful during navigation. In practice, the exact motion of the robot should be controlled by an obstacle avoidance module, which will constitute future work. Therefore, a simple control algorithm was implemented, where the difference in the x-coordinates (assuming that the forward-facing camera's horizontal axis is orthogonal to the axis of robot rotation) of the centroid of features in the current (x c ) and the next reference image (x n ) are fed back into the motion controller of the robot as the steering angle Φ. We have

Φ = -a(x c -x n ) ( 1
)
where a is the gain. To smoothen rapid steering actions when switching reference images, a feedforward part is added to the steering angle. The calculation of the feedforward part is based on the centroids of the shared features in the current and second next (x nn ) reference images. Thus, the final equation is

Φ = -a(x c -x n ) -b(x c -x nn ) (2)
where b is the feedforward gain. The translational velocity is set to a constant value, except during turns, where it is reduced to a smaller constant value to ease the tracking of rapidly moving features in the image. Such turns are automatically detected during navigation by thresholding the difference in the feature centroids in the current, next, and second next images.

The decision of when to stop when reaching the goal position is carried out similar to the reference-image-switching strategy in [START_REF] Chen | Qualitative vision-based mobile robot navigation[END_REF] by detecting when the variance of the difference between the current and last reference image feature coordinates starts to rise.

III. VISION TECHNIQUES AND ALGORITHMS

This section describes the main details about the vision techniques and algorithms employed. A key ability in vision path following is to correctly locate mapped features in images that were acquired during navigation. Therefore, we first introduce the following two basic approaches for establishing point correspondences between images: 1) wide-baseline matching in Section III-A and 2) tracking in Section III-B. Section III-C presents an empirical performance evaluation of the two correspondence approaches. The experiments indicate that correspondences recovered by tracking provide considerably more accurate 3-D reconstructions (aside from being several times faster to obtain). Consequently, we employ wide-baseline matching only for obtaining initial localization at the beginning of the navigation session (see the top of Fig. 3). Other localization steps and the whole mapping stage employ tracking, as detailed in Sections II-B and A, respectively.

The proposed localization subsystem often needs to (re)start the tracking of features that, for various reasons, were not tracked in the previous frame (see the right branch of the flowchart in Fig. 3). During the typical forward motion, tracked features gradually leave the field of view and need to be replaced by new features. In addition, tracked features may be lost in any moment due to local disturbances such as occlusion, motion blur, illumination effects, noise, or any combination thereof. A suitable geometric procedure has therefore been devised to predict the locations of mapped features that are currently not tracked. This procedure is described in Section III-D. Note that a part of this procedure (caching the recovered twoview geometries between the key images) is performed during the mapping stage (see Fig. 1), whereas the actual prediction is employed during navigation (see Fig. 3, predict point positions).

A. Keypoint Detection for Wide-Baseline Matching

The purpose of wide-baseline matching is to detect correspondences without any prior knowledge about the relative orientation of the two views. Our images are acquired from a moving car; therefore, we particularly require robustness to appearance distortion along the scale axis. The desired robustness can be achieved by matching invariant feature descriptors [START_REF] Mikolajczyk | A performance evaluation of local descriptors[END_REF], which are independently extracted in both images. This approach is based on recent advances in robust and repeatable detection of characteristic image locations called keypoints [START_REF] Tuytelaars | Local Invariant Feature Detectors: A Survey[END_REF]. Oftentimes, the detected keypoints are locally distinctive with respect to position, scale, and rotation, whereas some approaches even address affine invariance [START_REF] Mikolajczyk | Scale and affine invariant interest point detectors[END_REF]. The descriptors obtained are exhaustively compared to the descriptors from the other image, typically with respect to the L2 distance. The correspondences are usually established as distinctive pairs, for which the best distance is less than 60% of the distance of the second best match [START_REF] Lowe | Distinctive image features from scale-invariant keypoints[END_REF].

We evaluated the following three keypoint detectors: 1) the maxima of the DoG [START_REF] Lowe | Distinctive image features from scale-invariant keypoints[END_REF]; 2) multiscale Harris corners [START_REF] Mikolajczyk | Scale and affine invariant interest point detectors[END_REF]; and 3) MSER [START_REF] Matas | Robust wide-baseline stereo from maximally stable extremal regions[END_REF]. The three detectors extract different kinds of features (blobs, corners, and regions, respectively [START_REF] Tuytelaars | Local Invariant Feature Detectors: A Survey[END_REF]) and complement each other with more or less success, depending on the scene. Our final procedure combines the correspondences obtained by individually matching the descriptors extracted by all three algorithms.

B. Point Feature Tracking

When approximate current feature locations are known (as is often the case when processing an image sequence), correspondences can be established by tracking. Two main pointfeature-tracking approaches are given as follows: 1) iterative first-order differential approximation [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF], [START_REF] Shi | Good features to track[END_REF] and 2) exhaustive matching of lightweight point features [START_REF] Ma | An Invitation to 3-D Vision: From Images to Geometric Models[END_REF], [START_REF] Nistér | Visual odometry[END_REF], e.g., Harris corners. We believe that the former approach is better suited to appearance-based navigation, because it tends to be less susceptible to association errors and provides more accurate point tracks.

To tolerate significant interframe displacements, the features are first tracked between the previous and the current images across a multilevel resolution pyramid. 5 Then, to avoid drift accumulation, the current appearance is warped to achieve optimal resemblance with the stored template image or reference. 6This alignment can be achieved by minimizing the norm of the error image obtained by subtracting the warped current feature from the reference [START_REF] Baker | Lucas-Kanade 20 years on: A unifying framework[END_REF]. Shi and Tomasi [START_REF] Shi | Good features to track[END_REF] have described the warp as a 2-D affine transform. An extended warp, which, in addition, compensates for affine photometric deformations of the gray-level value in the image, has been proposed in [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF].

In the rest of this section, we first provide a formulation of the general point feature tracker [START_REF] Baker | Lucas-Kanade 20 years on: A unifying framework[END_REF], [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF], [START_REF] Shi | Good features to track[END_REF] in Section III-B1, and then, in Section III-B2, we describe our variant of the concept, with which we obtained best results. The main changes of our final implementation with respect to the public KLT library are outlined in Section III-B3, whereas in Section III-B4, we summarize some computational considerations.

1) General Differential Tracker With Warp Correction: Let the feature in the current frame be given by I(x), its appearance after a warp with parameters p by I W (x, p), and the corresponding reference by I R (x). Then, the differential tracking consists of finding p, which minimizes the norm of the error over the feature window. We have p = arg min

p x I W (x, p) -I R (x) . ( 3
)
The minimization is performed in a Gauss-Newton style, by employing a first-order Taylor expansion of the warped feature around the previous approximation of p. This formulation can be expressed in different ways [START_REF] Baker | Lucas-Kanade 20 years on: A unifying framework[END_REF], and here, we present a "forward-additive" formulation, with which the best accuracy has been obtained. In this formulation, the current feature warped with a sum of the previous parameter vector p, and an unknown additive improvement Δp is approximated as

I W (x, p + Δp) ≈ I W (x, p) + ∂I W ∂p • Δp. (4)
The scalar residual norm that appears in (3) can now be represented as

R(Δp) = x I W (x, p + Δp) -I R (x) ≈ x I W (x, p) + ∂I W ∂p • Δp -I R (x) . ( 5
)
For clarity, we omit the arguments, denote the previous error image as e, and introduce g as the transposed warped feature gradient over the warp parameters. We have

R(Δp) ≈ x e + g Δp . ( 6
)
The requirement (3) can be enforced by finding a Δp for which the gradient of the residual vanishes. In case of the L2 norm, this approach is easy to perform, i.e.,

∂R(Δp) ∂Δp ≈ x 2 • (e + g Δp) • g = 0 . (7)
After transposing both ends of [START_REF] Clemente | Mapping large loops with a single hand-held camera[END_REF], we arrive at the final expression for an iteration in the context of a general warp (note that e is a scalar function) as follows:

x (ge + gg Δp) = 0. (8)
Thus, in each iteration, the additive improvement is calculated by solving a linear system of equations. The procedure stops when the norm of the improvement Δp falls below a threshold, when the new feature position falls outside the image bounds, or when the determinant |gg | becomes very small.

2) Differential Tracker With Isotropic Scaling and Contrast Compensation:

To mitigate the danger that a physically unrelated image patch might be well transformed toward the reference, a tradeoff between modeling power and tracking security should carefully be chosen. For our application, a good balance is obtained by a 5-D warp that consists of a 2-D translational offset (d), isotropic scaling (m), and the affine contrast compensation model (λ, δ) [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF]. It is convenient to express the warp in terms of geometric and photometric components as p = (q, r), where q = (m, d), and r = (λ, δ). The warped feature is then obtained as

I W (x, p) = λ • I(m * x + d) + δ = U (I(T (x, q)) , r). (9)
To use the general formulation given in [START_REF] Courbon | Indoor navigation of a nonholonomic mobile robot using a visual memory[END_REF], an expression for ∂I W /∂p = [∂U/∂q ∂U/∂r] must be derived using the chain rule. The second term is simpler to obtain. We have

∂U ∂r (I(T (x, q)) , r) = [I T 1] (10)
where I T is the current feature warped with T , I T = I(T (x, q))). The derivative of the first term is more involved as

∂U ∂q (I (T (x, q)) , r) = ∂U ∂I (I (T (x, q)) , r) • ∂I ∂T (T (x, q)) • ∂T ∂q (x, q) = λ • I x T • x 1 1 0 x 2 0 1 = λ I x T x I x1 T I x2 T ( 11
)
where I x T is the gradient in the feature warped by T , I x T = ∂I ∂T (T (x, q))). The combined result, [START_REF] Cummins | Highly scalable appearance-only SLAM-FAB-MAP 2.0[END_REF] and [START_REF] Davison | MonoSLAM: Real-time single-camera SLAM[END_REF], can be inserted into (8), with g given by

g = λI x T x λI x1 T λI x2 T I T 1 . ( 12
)
3) Implementation: Our implementation of the KLT tracker derives from the public library maintained by S. Birchfield at Clemson University. 7 We performed several modifications to the original code, but the most important among these modifications are contrast compensation warp extensions as described in Section III-B2 and evaluated in Section III-C. In addition, we provided codes for (re)starting the tracking of features at predicted locations. Finally, we also improved warp correction results for features at large scales by employing the pyramid level that most closely matches the current feature size.

4) Computational Considerations: The performance of differential tracking comes at a price of considerable computational complexity. In fact, code profiling 8 showed that the feature tracker is a major performance bottleneck of the navigation system presented in this paper. We are currently working on several opportunities to address this problem. Preliminary results indicate that the performance can be more than doubled by harnessing vector extensions of the x86 instruction set.

C. Performance Evaluation of the Correspondence Approaches

Evaluating the correspondence performance for real scenes is tricky, because ground-truth correspondences cannot typically be recovered in experiments with real 3-D scenes. Consequently, it is very difficult to assess the alignment accuracy of the correspondences. However, a correct correspondence alignment is very important in feature-based navigation, because the existing correspondences are employed to predict locations of previously unseen features. Bad predictions can exceptionally be troublesome, because they may give rise to association errors and subsequent degradation of the geometrical quality within a positive feedback spiral. Here, we estimate the correspondence alignment accuracy by looking at the reprojection error of the recovered two-view geometries. The smaller the reprojection error of the resulting two-view geometry, the better the correspondence approach becomes. The four evaluated correspondence approaches are listed as follows:

• isotropic scaling with contrast compensation (track5); • affine warp with contrast compensation (track8); • affine warp without contrast compensation (track6); • wide-baseline matching by employing Lowe's keypoints and SIFT descriptors (match). 8 We employed the GNU profiler gprof.

The experiment is designed as follows. For each of the 23 key images of the sequence referenced in Section IV-G, we look at correspondences between the key image (index i in the sequence), and the five subsequent images at indices i + 1, i + 2, i + 3, i + 4, and i + 5. For matching, we simply match the pairs (i, i + 1), (i, i + 2), and so on. For tracking, we initialize the tracker at index i, and then, track5 frames forward. In each case, we record five reprojection errors for geometries from (i, i + 1) to (i, i + 5). The results are summarized in Fig. 4 as means of the five reprojection errors.

The results illustrate that the correspondences obtained by tracking with contrast compensation yield the overall better and significantly more stable two-view geometries than the correspondences obtained by matching (track5 versus match). The figure also shows that contrast compensation provides a significant performance gain when tracking outdoors (track5 versus track6). Finally, the figure suggests that track5 is somewhat better than track8 (track5 versus track8). Our result that tracking performs better than matching is consistent with the findings in [START_REF] Scaramuzza | Real-time monocular visual odometry for on-road vehicles with 1-point RANSAC[END_REF], where a similar comparison was performed.

D. Decomposed Point Transfer in the Calibrated Context

The main shortcoming of tracking is that it requires an auxiliary technique for establishing initial correspondences and recovering from tracking failures. We address this problem by providing a module for predicting the locations of features that are currently not tracked. After an approximate feature location has been provided by the prediction module, the correct location can be recovered by differential tracking with warp correction with respect to the reference appearance acquired during the mapping stage. Feature prediction is therefore a critical task that enables the system to deal with large motions and local disturbances by providing means for a dynamic update of tracked features.

The adopted feature prediction approach exploits geometric constraints provided by currently tracked features and their mapped correspondences within the frame of a technique known as point transfer [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]. Point transfer locates an unknown 2-D point in the current image by employing the following elements: 1) the known projections of the same 3-D point in two other images and 2) some additional correspondences across the three images. This problem is illustrated in Fig. 5. The decomposed solution of that problem is given as follows: 1) Image correspondences are used to recover the two-view geometry (A, B); 2) the two known projections q A and q B are used to triangulate the 3-D point Q; 3) the two-view geometry (A, X) is recovered and put into the frame of the geometry (A, B); and 4) the desired point q X is obtained by projecting Q onto image X.

To perform the point transfer, we need to recover the three-view geometry between the current image and two key images from the map.

There are several ways of computing the three-view geometry, with different assumptions and performance requirements. The golden standard method described in [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF] involves bundle adjustment with respect to the reprojection error in all views, which may be costly for a real-time implementation. A more suitable solution would observe that several three-view geometries need to be recovered for the same key-image pair during navigation and therefore strive to reuse precomputed two-view geometries for such pairs. Such a decomposed solution has been proposed in [START_REF] Lourakis | Fast trifocal tensor estimation using virtual parallax[END_REF]. A similar approach has been employed in this paper but within the calibrated context, i.e., by assuming that all observed points have been expressed in normalized coordinates 9 that correspond to the case of unit focal distance [START_REF] Ma | An Invitation to 3-D Vision: From Images to Geometric Models[END_REF]. Some implementation details of our solution will be described as follows.

Each of the two geometries (A, B) and (A, X) (see Fig. 5) is independently recovered. The two essential matrices are estimated by the random sampling scheme MLESAC [START_REF] Torr | MLESAC: A new robust estimator with application to estimating image geometry[END_REF], using the recent five-point algorithm [START_REF] Nister | An efficient solution to the five-point relative-pose problem[END_REF] as the generator of motion hypotheses. The implementation employed has been provided within the library VW3410 maintained at the Imperial College, London, U.K.. The decomposition of the essential matrix into motion components is performed next, followed by the triangulation of 3-D points [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF].

Consequently, the geometries (A, B) and (A, X) (see Fig. 5) are expressed in the common frame. In the calibrated context, the adjustment involves the estimation of only one parameter (scale), whereas in the projective context, the ambiguity has 4 degrees of freedom (DOFs) [START_REF] Lourakis | Fast trifocal tensor estimation using virtual parallax[END_REF]. The scale factor between two metric frames is estimated by requiring that pairs of corresponding points visible in both frames have the same depth. In practice, different points vote for different scale factors due to noise, but a robust result is, in the end, obtained as the median of all individual factors.

Three-dimensional coordinates of the desired point Q are obtained by triangulating its projections onto the two key images A and B (see Fig. 5). This approach can be performed offline during mapping. The desired prediction q X of the triangulated point Q to the current image X is finally obtained by simple projection.

The aforementioned prediction procedure is very sensitive to the accuracy of the estimated two-view geometries. Thus, it makes sense to disregard the predictions when the estimates appear to be inaccurate with respect to the reprojection error [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]. The reprojection error may be determined either in a straightforward manner or, as calculated in this paper, by taking into account the probability that a bad geometry may produce a low reprojection error by chance (as proposed in [START_REF] Segvic | A mapping and localization framework for scalable appearance-based navigation[END_REF]).

IV. EXPERIMENTAL RESULTS

The goal of our experiments is to explore the possibilities and limits of the current implementation of the framework by navigating in different scenarios, environments with different proportions of vegetation to human made structures, and different illumination conditions. We also explore the limit in speed and in lateral deviation from the path. A practical application of online mapping and autonomous parking is also given. The results are quantitatively evaluated.

In all but the last experiment, a CyCab-a French-made fourwheel-drive four-wheel-steered intelligent vehicle designed to carry two passengers-was used. On our CyCab, all computations, except for the low-level control, were carried out on a laptop with a 2-GHz Pentium M processor. A 70 • field of view, forward looking, B&W Allied Vision Marlin (F-131B) camera was mounted on the robot at a 65-cm height. Except for experiment 3, the camera was used in the autoshutter mode, with the rest of the settings constant. During all experiments (except the last experiment), no software parameters were changed, except for the forward and turning speed. Mapping has been performed offline, except in experiment 6. The image resolution in the experiments was 320 × 240. Tracked feature patch sizes were 15 × 15 pixels.

A. Experiment 1: Basic Experiment

Experiment 1 (see Fig. 6) was conducted on an overcast day with a short time between mapping and navigation. Most views on the 158-m-long path contained buildings which provided stable image features. The main potential challenges in this experiment were given as follows: 1) motion blur in the teaching sequence caused by fast driving for the used exposure times; 2) driving under a building, which caused a quick illumination change; and 3) people (more than ten) and cars covering up features during navigation.

In the teaching phase, 958 logged images were reduced into 77 reference images in 257 s (3.7 fps). While the robot was moving at 50 cm/s in turns and at 90 cm/s otherwise during navigation, 934 images were processed at 4.1 fps on the average. Statistics with regard to mapping and navigation are shown in Table I. Reconstructed robot and reference image poses shown in Fig. 7 were used only for assessing the performance of the system.

The quick illumination change when driving under the building was easily handled due to the implemented illumination compensation in the tracker [START_REF] Jin | Real-time feature tracking and outlier rejection with changes in illumination[END_REF]. Motion blur in the teaching sequence did not impair the performance of the system. The moving objects and persons did not affect the navigation, because the tracking of features that reappear after occlusion was immediately restarted due to the feature reprojection scheme. Fig. 8 contains images processed at the end of the navigation. They describe an interesting situation where a moving car progressively occludes most features. It is shown that the tracking of reappearing features is restarted, because there were enough good features that were tracked for the camera pose estimation used in point reprojection.

B. Experiment 2: Robustness to Environment Changes

Experiment 2 was conducted on a narrow path along a small lake (see Figs. 6 and12). Mapping was carried out in June, under the strong summer sun (see Fig. 9). Navigation took place in October, when vegetation and illumination conditions were very different (see Fig. 10). Despite the large change in the environment, CyCab managed to navigate about 80% of the path, with only one human intervention. At one place, CyCab started to brush the rose plants on the left side of the path (the inside of the bend) in Fig. 10; therefore, we stopped the vehicle. Such a corner-cutting behavior naturally comes with wide separation between reference images and the chosen control strategy. Without stopping the vision system, CyCab was moved 50 cm to the right, and its automatic motion was resumed. CyCab's vision system gave up close to the end of the track when the change in the environment was very large (see Figs. 11 and12). Although CyCab did not complete the whole path (see the left side of Fig. 9, where it failed), this experiment still represents a success because of the difficult conditions that CyCab handled.

Shortly after CyCab got lost, we have repeated the experiment using a new map. As it shown in Fig. 9, right, CyCab completed the path without any problems and with smaller localization noise. Note that, because image-based visual servoing was used, localization noise had only an indirect effect on the motion of the robot, because it influenced only feature point reprojection and reference image switching.

This experiment indicates that seasonal vegetation changes may negatively affect the performance of the framework in environments where most features are provided by the vegetation. This experiment also suggests that, in the short term, under favorable conditions, vegetation may provide a large number of well-textured features, which can result in high-quality 3-D geometry estimation. However, unfavorable conditions such as wind or rain may easily degrade the quality of the created map.

The frame rates during navigation are lower in this experiment (see Table I) due to temporary implementation and processing platform limitations.

C. Experiment 3: Deterioration Due to Distant Features

In experiment 3, CyCab completed an approximately 304-m track, where in some places (see the right side of Fig. 13), the closest features were more than 100 m away. Because the width of the footpath matched the width of CyCab, it was easy to observe the lateral error during navigation. The mapping and navigation part of the experiment was conducted in succession, under very bright lighting conditions. Instead of the usual autoshutter mode, the camera was used in its high-dynamicrange mode. The start and end positions were identical.

As we can expect, the error in the estimated pose during navigation was the largest at places where there were no close features. Such large pose errors are represented by cluttered points in Fig. 14,e.g., at the right bottom part of the path. In this case, the 3-D pose error resulted in an early switching of a few reference images during turning and, subsequently, following the learned path with a 1-m lateral error for a short section of the path. Other than this case, CyCab performed excellently, even when the sun was shining into its camera, as shown in Fig. 15. With seamless motion over the first and final reference frames, CyCab demonstrated that the framework does not require global consistency in the 3-D reconstruction.

D. Experiment 4: Driving in a Loop

The aim of this experiment was to investigate navigation in a loop. The teaching was performed by driving CyCab in a full loop in a circular parking lot of an approximately 119-m circumference. The beginning and end of the loop were closed by matching the first and last images of the teaching sequence. If neighboring nodes were connected with line segments, then the first and the last light colored dots in Fig. 16 were connected.

CyCab managed to complete 1.25 loops, although the experiment was conducted at the end of day, where people were driving their cars away from the car park, and the sun was shining into the camera (see Fig. 17). The change in the scene worsened at the beginning of the second loop, where one of these cars provided the only close features. The lack of good features in conjunction with a lateral error resulted in very poor pose estimates, as shown in Fig. 16. Therefore, this experiment demonstrated that the lack of global consistency in pose does not preclude navigation, as long as local consistency is ensured (in Fig. 16, the path does not join up into a circle). This case is due to the ability of the image-based visual servoing scheme to handle situations where pose-based schemes may struggle when fed with poor pose estimates.

Eventually, CyCab was manually stopped when it no longer followed the curvature of the road (see the short straight section In experiment 5, the robot drove until the end of the road while maintaining a 1.8-m/s speed. In experiment 6, after online path learning, the robot parked itself into the garage close to the center of the image. of black dots in Fig. 16, where it happened); however, the experiment was a success, because it demonstrated that CyCab can connect the beginning and end of a loop and drive through the joint.

E. Experiment 5: Robustness to Speed

This experiment investigates how fast CyCab can navigate on a straight path. On the track shown in Figs. 18 and19, CyCab completed a 100-m straight path at a 1.8-m/s (6.5-km/h) speed. Raising the speed even higher caused oscillations in the robot's motion to appear. The oscillations were presumably caused by the delay between image measurements and control action and by the frame rate.

F. Experiment 6: Application to Automatic Parking With Online Mapping

In this experiment, online mapping (i.e., processing the images as they are grabbed) and a practical application are demonstrated. In the current state of the navigation system, i.e., without obstacle detection and avoidance, the practical applications are limited. However, even now, the framework can be used for automatic parking on private properties, which are under the control of the user.

During the experiment, a map was created online while driving CyCab from the entrance of the Institut de Recherche en Informatique et Systèmes Aléatoires (IRISA) to the CyCab garage approximately 50 m away (see Figs. 18 and20) at about 50 cm/s. Then, CyCab was manually driven to the entrance of IRISA, where the driver got out, and CyCab drove itself into the garage. During mapping, clouds covered the sun, whereas during navigation, the sky was clear. CyCab even handled the transition from strong sunshine to the darkness of the garage.

G. Experiment 7: Robustness to Lateral Deviation

A navigation system based on vision should also handle situations where the autonomous vehicle is required to deviate from the reference path to avoid an obstacle. Because obstacle detection and avoidance is out of the scope of this paper, in this experiment only the maximum possible lateral deviation from the reference path is investigated. Unlike in the previous experiments, a firewire color webcam, the Unibrain Fire-I, was mounted on the top of a 1995 right-hand-drive Renault Clio. During the experiment, images were logged at 30 Hz, whereas the vehicle was traveling at approximately 5 m/s.

The experiment was conducted on a single-direction doublelane L-shaped road of a small town at 7:30 on a sunny Saturday morning in June. The time of the experiment was chosen to minimize the effect of moving objects, because the goal was to test the sensitivity of localization to lateral deviation. The place of the experiment was chosen to emulate an unfavorable scenario where the houses are close to the road (see Fig. 21). Such situations where the lateral deviation is large compared to the distance from the scene are challenging, because the tracked points undergo a large amount of appearance and position change. The distance between the camera and the nearest house on the left was often only 2 m during the mapping of the approximately 100-m-long path. The right side of the road was occupied by parked cars. During mapping, one car drove past. Data were gathered for localization in the subsequent runs, at estimated lateral deviations of 0 m (left side), 2.5 m (middle), and 5 m (right side) from the reference path.

Offline localization during the 0-m deviation and the 2.5-m deviation was successful; however, the initial localization with wide-baseline matching at the 5-m lateral deviation failed for the first few reference images. After a later successful initial localization, the framework kept the camera localized until the pose tracking failed right after the turn. We can observe the increase of jitter with increasing lateral deviation in the localization results (see Fig. 22). A decline in the number of tracked points with increasing lateral deviation is shown in Fig. 23. It is also visible in the figure that the number of points in the map increased as the car approached the turn and decreased as it came out of the turn.

The results indicate that, with increasing lateral deviation, the localization accuracy and the number of tracked points decrease. The limit of the vision system for lateral deviation in the environment tested lies between 2.5 and 5 m.

V. DISCUSSION AND LESSONS LEARNED

A. Scalability and Performance

Statistics from experiments 1-6 are presented in Table I.

By performing simple image-based visual servoing instead of position-based control of the robot, we can have several advantages. Because there is no need for an accurate robot pose during navigation, we can allow a larger 3-D reconstruction error during mapping. Because of this case, there is no need to perform a computationally costly global bundle adjustment, and mapping can be done online. During the experiments, it was noticed that, after the baseline between reference images has increased beyond a certain distance, the 3-D reconstruction error also increased. Therefore, if a larger 3-D reconstruction error is allowed, we can have larger distances between reference images, and the memory requirement for storing the map is reduced. This condition is shown, for example, in experiment 3, where the average distance between reference images was 3.1 m. Sparse reference images improve not only scalability but performance as well, because the overhead associated with the loading of reference images and their switching is reduced.

The framework enables the learning and navigation of long paths, because the total memory and computational requirements for creating a map linearly grow with the length of the path. The computational cost during each navigation step is approximately constant. As for the memory requirements, when calculating with 3.1 m between reference images, a 1-km-long path can be represented using 25 MB of storage if we use 320 × 240 uncompressed images and neglect the stored feature point coordinates. Because we can store the reference images on a hard drive, a 1-TB drive may store an approximately 40 000-km worth of path.

There is a relationship between camera field of view and distance between reference images. In experiments not described in this paper due to the lack of space, we noticed that, when using only the center half of the images or when using a Logitech Quickcam Pro 4000 camera, the average distance between reference images increased up to 12 m. The detailed study of the effects of field of view constitutes future work.

B. Vision Techniques

The contrast compensation implemented in the tracker can handle large affine changes of illumination between the reference and current images, which was crucial, for example, during experiment 2 (see Fig. 10). Although the tracker was fairly resilient to illumination changes, the same is not true of the wide-baseline matching. Problems occurred from time to time when buildings that hold the majority of the features reflected the sunlight directly into the camera. The matching of overexposed features with well-exposed features using SIFT descriptors often failed, even when the tracker can track them. Because initial localization or relocalization is done on a stationary robot, the use of exposure bracketing (see [START_REF] Nourani-Vatani | Practical visual odometry for carlike vehicles[END_REF] for stereo vision) and the utilization of points resulting from all images in the matching process may alleviate this problem.

The use of 3-D information enables us to restart the tracking of features just becoming visible after occlusion, as shown in Fig. 8. This property is important in dynamic environments. In addition, having 3-D information enables the system to check the consistency of the tracked features. Tracked points that "jump" from the background onto a moving object in the foreground are discarded. Although having 3-D information may not be necessary for path following, as stated in the Introduction, it may extend the area of applicability of an outdoor path-following system.

Because only features that were reliably tracked are kept between two possibly distant reference images, the feature selection for 3-D geometry estimation did not pose a significant problem. We may intuitively think that maps built with an extended Kalman filter (EKF)-based monocular SLAM implementation are more accurate due to a larger amount of information integrated into the maps. However, the superiority of several EKF-based monocular SLAM implementations is not very clear, because unstable features or features located on slowly moving objects (e.g., clouds) may be tracked and incorporated into the map before being discarded. This incorporation of bad features may gradually compromise the integrity of the map. In contrast, errors in the 3-D reconstructions always stay local in our framework and do not affect other nodes of the map. A similar effect can also be achieved with local SLAM maps. Local SLAM maps may also alleviate the effects of linearization errors in EKF implementations.

The use of normalized image coordinates in the vision system together with tracked image patch scale estimation does not preclude performing mapping with one camera and navigating with a different but reasonably similar camera. Such capacity enables mapping by one vehicle and sharing the map by several vehicles.

C. Limitations

As shown in experiment 7, the framework has handled lateral deviations in excess of 2.5 m, even when used with a noisy camera with a high radial distortion. This condition indicates that the framework may enable obstacle avoidance, as long the scene is not totally covered up by the obstacle.

In the current implementation, the framework relies on a 3-D pose to switch reference images. In cases where the 3-D pose is less accurately recovered, it can happen that a reference image switch is not performed or is performed in the wrong direction. Such behavior occasionally happens when most of the observed points are located on a plane or on a tree. A wrong reference image switch more likely caused problems in turns where not turning in the right direction quickly reduced the number of visible feature points. With fewer points, the reconstructed geometry is often less accurate, which further worsens reference image switching and reduces the accuracy of points projected from the map into the image. When there is no replacement for lost feature points, the number of feature points declines. To address the issue of reference image switches, we plan to investigate a reference-image-switching strategy based on the more stable image information. Pose estimation based on homography for planar scenes is also an option.

One further limitation is that of illumination. Extreme illumination changes, e.g., the sun shining into the camera during mapping but not during navigation or the lack of light, may impair the performance of the framework, particularly the performance of the matcher.

The navigation at night remains an open question. Sensitive cameras and artificial illumination may help in some cases. Encouraging results in localization have been described in [START_REF] Bradley | Real-time image-based topological localization in large outdoor environments[END_REF], where image-based localization was demonstrated at night using headlights in sequences taken also at night. The localization in sequences taken during the day was not very successful.

Although the mapping and localization part of the system is 3-D, the control algorithm is 2-D. This case does not imply that the framework can only handle flat terrains. Several the tests were performed on moderately sloping terrains. The system also handled twists in the slopes.

Navigation frameworks for uncontrolled environments, as described in this paper, should detect and avoid obstacles. Because this case is not yet implemented in the framework, it constitutes part of the future work.

The choice of the speed of the robot should depend on the following factors: 1) exposure time as it influences motion blur; 2) frame rate and distance to features (as they influence how much features move between frames); and 3) safety considerations.

When considering navigation based on maps created a long time ago, we can expect vegetation to significantly change. This condition restricts the long-term application of such a system to places with a slower rate of change, e.g., to urban areas where buildings are 11 It seems reasonable to assume that the appearance of buildings slowly changes. However, old buildings are rebuilt, and new buildings are erected all the time. A wide-field-of-view camera or a panoramic camera may help capture parts of the scene that have not changed. To even more increase the robustness of the system, a mechanism should be added to the framework, through which new map points can be added to the map during navigation. Even then, snow may sufficiently change the facades of buildings to stop the system from working, which may restrict the use of the framework to climates without snow.

Experts may easily assess environments for vision-systemrelated risks of failures during navigation. However, commercial systems would benefit of such output as part of the mapping process.

D. Applications

Frameworks such as our approach may be used one day on arbitrary systems which have to move on a previously completed track. Such systems include people carriers, streetcleaning robots, and robots that transport goods between buildings of a factory. The framework is not limited to systems with wheeled or tracked locomotion. Because the only sensing modality is a single camera (no odometry), coupled with full 3-D geometry estimation, we could likely use the framework on hovercrafts, blimps, helicopters, and airplanes. However, for aircraft, the affine tracking of the tracker should be enabled (for the experiments in this paper, this property was disabled to obtain more accurate results) to handle rotated image patches, and the control algorithm should be changed to handle 3-D motion. We could also envisage the use of such a system on autonomous boats in places such as canals in some cities where several stationary features are visible.

Because it is reasonable to expect that the framework can handle teaching while moving forward and executing the path while moving backward, it could be used on transportation devices that drive themselves back to their base after use.

In safety-critical applications, the addition of the inertial measurement unit (IMU), Global Positioning System (GPS), odometry, or a motion model (predicting the motion of the vehicle) may be considered to ensure that eventual vision system failures are appropriately handled.

VI. CONCLUSION

An experimental evaluation of a framework for visual path following in outdoor urban environments using only monocular vision has been presented in this paper. In the framework, no sensor other than a camera was used. The path to follow was represented as a series of images with overlapping landmarks. It was shown that the use of local 3-D information, contrast compensation, and image-based visual servoing can lead to a system that can navigate in diverse outdoor environments with reasonable changes in lighting conditions and moving objects. Online learning was also demonstrated.

Because the framework does not rely on odometry, the range of applications may also include boats that navigate on urban canals or aircraft.

Fig. 1 .

 1 Fig. 1. Steps involved in building a representation of a path from a sequence of images, i.e., mapping.

Fig. 2 .

 2 Fig. 2. Map consists of reference images, point correspondences, and 2-D and 3-D information. During navigation, the point features from the map are projected into the current image and are tracked.

Fig. 3 .

 3 Fig. 3. Visual localization during navigation.

Fig. 4 .

 4 Fig. 4. Performance evaluation of four approaches for establishing correspondences between a given image and five subsequent images in the sequence. The horizontal axis holds the sequential number of the frame, whereas the vertical axis shows the mean reprojection error.

Fig. 5 .

 5 Fig. 5. Point transfer problem. Given two known projections of the same point Q onto key images A and B, find its projection in a current view X.The decomposed solution of that problem is given as follows: 1) Image correspondences are used to recover the two-view geometry (A, B); 2) the two known projections q A and q B are used to triangulate the 3-D point Q; 3) the two-view geometry (A, X) is recovered and put into the frame of the geometry (A, B); and 4) the desired point q X is obtained by projecting Q onto image X.

Fig. 6 .

 6 Fig. 6. Paths for experiments 1 and 2.

Fig. 7 .

 7 Fig. 7. Navigation results in experiment 1 shown as reconstructed robot poses (black) overlaid on 77 reconstructed reference image poses (lighter colored dots and barely visible sequence numbers). (Bottom left) First reference image pose.

Fig. 8 .Fig. 9 .

 89 Fig. 8. Every second frame of a sequence from experiment 1 demonstrates robust feature (light colored crosses) tracking resumption after occlusion by a passing car.

Fig. 10 .

 10 Fig. 10. Large difference in illumination and vegetation between a fourmonth-old reference image (left) and a current image used during navigation in experiment 2.

Fig. 11 .

 11 Fig. 11. Difference between the reference image (left) and current image (right) in experiment 2, which the vision system could no longer handle. Notice the missing flowers in the flowerbed.

Fig. 12 .

 12 Fig. 12. CyCab autonomously drives on the narrow path in experiment 2.

Fig. 13 .

 13 Fig. 13. Path for experiment 3.

Fig. 14 .Fig. 15 .

 1415 Fig. 14. Larger noise in the reconstructed robot poses, where all features are far away in experiment 3.

Fig. 16 .

 16 Fig. 16. Navigation results in the loop closing experiment (experiment 4).

Fig. 17 .

 17 Fig. 17. Sun shines into the camera in the reference image (left) of experiment 4 but not in the current image (right) during navigation.

Fig. 18 .

 18 Fig. 18. First images during navigation in experiments 5 (left) and 6 (right).In experiment 5, the robot drove until the end of the road while maintaining a 1.8-m/s speed. In experiment 6, after online path learning, the robot parked itself into the garage close to the center of the image.

Fig. 19 .

 19 Fig. 19. Navigation results in experiment 5.

Fig. 20 .

 20 Fig. 20. Navigation results in experiment 6.

Fig. 21 .

 21 Fig. 21. View difference example in experiment 7. (Left and right) Previous and next image from the map captured on the left side of the road, respectively. (Middle) Current image captured on the right side of the road. Notice the large separation between the reference images and the large lateral displacement of the current image from the reference images.

Fig. 22 .

 22 Fig. 22. Offline localization result in experiment 7 while driving on the reference path (left), in the middle (center), and on the right (right) of the path. Notice the increase of noise in the reconstructed robot poses with increasing lateral deviation. The point of getting lost in the rightmost track coincided with performing a right angle turn while being close to the tracked points.

Fig. 23 .

 23 Fig. 23. Number of points in the map and the average number of tracked points for each node in experiment 7. With increasing lateral deviation, the average number of tracked points decreases.

TABLE I SUMMARY

 I OF THE VISUAL-PATH-FOLLOWING EXPERIMENTS

Compared with[START_REF] Diosi | Outdoor visual path following experiments[END_REF], a gap in the experimental work has been filled, and more details of the vision system are given.

Videos that show results presented in this paper can be accessed at http://www.irisa.fr/lagadic/video/CycabNavigation.mov and http://www.zemris.fer.hr~ssegvic/pubs/diosi_et_al_07iros_0581_VI_i.mp4 [Accessed:February 22, 2011].

The source code of the KLT tracker maintained by S. Birchfield can be found at http://www.ces.clemson.edu/~stb/klt/ [Accessed:February 22, 2011].

An implementation is available in the VW Library downloadable from http://www.doc.ic.ac.uk/~ajd/Scene/index.html [Accessed:February 22, 2011].

We used two additional pyramid levels, which are iteratively obtained by subsampling each second pixel in a properly smoothed image.

During mapping, the reference is obtained by simply storing the first appearance of the feature. During localization, the reference is taken from the corresponding key image.

URL: http://www.ces.clemson.edu/~stb/klt/ [Accessed: February 22, 2011].

We employ the usual model for transforming pixels into normalized coordinates that comprise a 5-DOF linear transformation and the fourth-order radial distortion model[START_REF] Zhang | A flexible new technique for camera calibration[END_REF]. We recovered calibration parameters for our cameras by employing our own implementation of the procedure with a planar calibration target as described in[START_REF] Zhang | A flexible new technique for camera calibration[END_REF].

[START_REF] Cummins | Highly scalable appearance-only SLAM-FAB-MAP 2.0[END_REF] URL http://www.doc.ic.ac.uk/~ajd/Scene/Release/vw34.tar.gz [Accessed:February 22, 2011].

In a wider context, we could also consider space objects with slowly changing landscapes such as the moon.

ACKNOWLEDGMENT

The authors would like to thank F. Spindler, A. Cherubini, H. Tran, and F. Servant for their help during the experiments, M. Pupilli for lending the equipment, enabling the conduction of experiment 7, and for his comments on this paper, and the reviewers for their comments.


Olivier Kermorgant

Franc ¸ois Chaumette

Multi-sensor data fusion in sensor-based control: application to multi-camera visual servoing

Keywords: sensor fusion, multi-sensor, visual servoing, multi-camera

A low-level sensor fusion scheme is presented for the positioning of a multi-sensor robot. This non-hierarchical framework can be used for robot arms or other velocitycontrolled robots, and is part of the task function approach. A stability analysis is presented for the general case, then several control laws illustrate the versatility of the framework. This approach is applied to the multi-camera eye-in-hand/eyeto-hand configuration in visual servoing. Experimental results point out the feasibility and the effectiveness of the proposed control laws. Mono-camera and multi-camera schemes are compared, showing that the proposed sensor fusion scheme improves the behavior of a robot arm.

I. INTRODUCTION

Most actuated systems use sensors to obtain information about their environment. They can be a camera, ranging devices or temperature or force sensors. The concept of data fusion is the answer to how to combine features coming from the same environment, yet through different sensors.

A wide classification of sensor data fusion approaches is done in [START_REF] Luo | Multisensor fusion and integration: approaches, applications, and future research directions[END_REF]. The most popular approach is to use the sensor data to estimate the robot state [START_REF] Durrant-Whyte | Integration, Coordination and Control of Multi-Sensor Robot Systems[END_REF]. For instance, in [START_REF] Garcia | Sensor Fusion for Compliant Robot Motion Control[END_REF], inertial measurements are used to correct the force and wrist estimation of a force sensor. In [START_REF] Lippiello | Position-based visual servoing in industrial multirobot cells using a hybrid camera configuration[END_REF], several cameras are used together to estimate the 3D-position of an object. In this approach, sensor data are not directly used in the control loop and are part of the real-time estimation of the robot state. A model-based control law can then be applied.

The main alternative to state estimation is named sensorbased control. Here, one does not use the sensors to estimate the system state, but rather sticks to the sensor space. In this approach, each sensor is given a reference signal and considered as an independent subtask of the global task function. A classical scheme, often named hybrid control, is to draw a hierarchy between the different sensors and to build a control scheme that prevents lower subtasks to disturb higher ones [START_REF] Hosoda | Adaptive hybrid visual servoing/force control in unknown environment[END_REF]. This hierarchy can be made dynamic to prevent from reaching local minima [START_REF] Mansard | Task sequencing for sensor-based control[END_REF]. With another formulation, sensor-based control laws can be designed without imposing a strict hierarchy between the sensors. Here the data coming from different sensors is treated as a unique, higher-dimensional signal. This is the approach chosen in [START_REF] Malis | Robot control using disparate multiple sensors[END_REF] to fuse two cameras, and a force sensor and a camera, where the designed control law is equivalent to a weighted sum of the subtask control laws.

Our work is part of the non-hierarchical class of sensor data fusion schemes. Contrary to the previously presented approaches, there is no concept of priority between the different sensors: therefore only the global error is taken into account. The main contribution is the exhibition of a class of easy-tuned control laws that do not require any a priori hierarchy between the sensors and show nice properties in the sensor space and in the robot behavior.

Section II is dedicated to the modeling of multi-sensorbased control. Afterwards, a stability analysis is performed, from which several control laws are proposed. Finally, Section IV illustrates the control laws, with experiments in the case of multi-camera eye-in-hand/eye-to-hand cooperation. Results validate that non-hierarchical schemes can induce nice behaviors for a multi-sensor system.

II. MULTI-SENSOR MODELING

This section covers and adapts the modeling used in [START_REF] Malis | Robot control using disparate multiple sensors[END_REF] for the control from multiple sensors. We consider a robotic system equipped with k sensors providing data about the robot pose in its environment. Each sensor S i delivers a signal s i of dimension n i with k i=1 n i = n and we assume n ≥ 6. In the case of a motionless environment, the signal time derivative is directly related to the sensor velocity screw v i expressed in the sensor frame:

ṡi = L i v i (1)
where L i is named the interaction matrix of s i [START_REF] Chaumette | Visual servo control, part I: Basic approaches[END_REF], [START_REF] Samson | Robot Control : The task function approach[END_REF] and is of dimension (n i ×6). Its analytical form can be derived for many features coming from exteroceptive sensors. It depends mainly on the type of considered sensory data s and on the sensor intrinsic parameters. L i may also depend on other data: for instance the interaction matrix of an image point observed by a camera depends on the depth of that point, which is not actually measured in the image. Now, we consider a reference frame F e in which the robot velocity can be controlled. This frame can be the endeffector frame for a manipulator, or the vehicle body frame for a mobile robot. The screw transformation matrix allows expressing the sensor velocity v i wrt. the robot velocity v e :

v i = i W e v e (2)
i W e is given by:

i

W e = i R e i t e × i R e 0 3×3 i R e (3)
where i R e ∈ SO(3) and i t e ∈ R 3 are respectively the rotation and the translation between F e and F si . i t e × is the (3×3) skew-symmetric matrix related to i t e . From ( 1) and (2) we can express the time variation of a sensor signal wrt. the robot velocity screw:

ṡi = L i i W e v e (4)
Denoting s = (s 1 , . . . , s k ) the n-dimensional signal of the multi-sensor set, (4) allows linking the signal time variation with the robot velocity:

ṡ = L s v e (5)
with:

L s =    L 1 . . . 0 . . . . . . . . . 0 . . . L k       1 W e . . . k W e    = LW e (6)
where L ∈ R n×6k contains the interaction matrices of the sensors and W e ∈ R 6k×6 contains the transformation matrices, making L s ∈ R n×6 the global interaction matrix of the task. Note that this modeling can be expressed in any frame used for the robot control: indeed, for any frame

F f we have W e v e = W e e W f v f = W f v f .
The goal of multi-sensor servoing is to design a control law that makes the robot reach the desired sensor measurement s * . To do so, a simple proportional control law yields:

v e = -λC(s -s * ) ( 7
)
where λ is a positive scalar gain and C ∈ R 6×n is named the combination matrix and has to be defined. For instance, if we want to ensure an exponential decoupled decrease of the error (ss * ), then (5) leads to:

C = L + s (8)
L + s being the Moore-Penrose pseudo-inverse of L s , that is

L + s = (L ⊺ s L s ) -1 L ⊺ s when L s is of full rank 6.
In practice, an estimation L s + is used, as both (L i ) i and i W e i depend on potentially unknown parameters and are subject to noise or calibration errors: L i relies on the sensor model and the direct sensor measurements, while i W e depends on the pose between the sensor and the robot. Actually, the frame transformation matrix i W e is subject to change during the task when the sensor S i is not rigidly attached to the robot control frame, which is the case for instance for an eye-tohand system. The final control law thus yields:

v e = -λ L s + (s -s * ) (9)
where:

L s + = ( L Ŵe ) + (10)
The system behavior thus depends on the chosen estimation of L and W e . The pseudo-inverse inherits the anticommutativity from the classical inverse, that is ( L Ŵe ) + = Ŵe + L+ , when L (resp. Ŵe ) has orthonormal columns (resp.

rows) or when both matrices are of full rank equal to their common dimension [START_REF] Albert | Regression and the Moore-Penrose pseudoinverse[END_REF]. For the proposed configuration, this property is ensured in two cases: 1) Ŵe has orthonormal rows, which is equivalent to having only one sensor and no translation between this sensor and the control frame (see (3)) 2) L and Ŵe are both of rank 6k, which can be ensured again if there is only one sensor, using n ≥ 6 features that induce a full-rank interaction matrix Therefore as soon as one fuses several sensors, (10) must be used as such.

III. STABILITY ANALYSIS

From ( 5) and ( 9), the evolution of the sensor signal yields:

ṡ = L s v e = -λL s C(s -s * ) (11)
The proportional scheme [START_REF] Malis | Robot control using disparate multiple sensors[END_REF] has different stability properties depending on the number of features [START_REF] Chaumette | Visual servo control, part I: Basic approaches[END_REF]. a) n = 6: denoting the signal error e = (s-s * ), we can define the candidate Lyapunov function that is the squared error norm L = 1 2 e 2 , whose deritative yields:

L = e ⊺ ė = -λe ⊺ L s Ce (12)
Hence, using the combination matrix defined in (8) the system is globally asymptotically stable (GAS) if the following condition is ensured:

L s C = L s L s + = LW e ( L Ŵe ) + > 0 (13)
Therefore, if the features and sensor positions are chosen so that L s and L s + are of full rank, the system is stable as soon as the estimations of L and W e are close enough from their actual value, the ideal case being of course L Ŵe = LW e . b) n > 6: here condition ( 13) is never ensured as L s C ∈ R n×n is of rank 6 at most. This means local minima may exist, for configurations such that e ∈ KerC. Stability can thus be at most local. Defining the task error as e ′ = Ce yields:

ė′ = C ė + Ċe = (CL s + O)v e = -λ(CL s + O)e ′ (14)
where O ∈ R 6×6 = 0 when e = 0 [START_REF] Chaumette | Visual servo control, part I: Basic approaches[END_REF]. With the combination matrix from (8), this scheme is known to be locally asymptotically stable in a neighborhood of e = e * = 0 [START_REF] Isidori | Nonlinear control systems[END_REF] if:

CL s = L s + L s = ( L Ŵe ) + LW e > 0 (15)
If the features and sensor positions are chosen so that L s and L s + are of full rank 6, the system is locally stable as soon as the estimations of L and W e are not too coarse.

Finally, when the number of features is sufficient, that is n ≥ 6, the stability mainly depends on the chosen estimation for L s + . For this purpose, several control laws are proposed. The final choice depends of which information is available during the servoing:

1) C cur = (LW e ) + when both matrices can be measured (usually with noise) during the task.

2) C * = (L * W e ) + if W e is measured, while L * is computed at the desired position. 3) C 3 = (L * W * e ) + uses the values computed at the desired position. In this case, the combination matrix is constant. This choice is equal to the previous one when W e is constant, that is for instance for an eye-in-hand system. 4) C 4 = (LW e ) + where L is the mean interaction matrix allowing second-order minimization [START_REF] Tahri | On visual servoing based on efficient second order minimization[END_REF], that is

L =    1 2 (L 1 + L * 1 1 * W 1 ) . . . 0 . . . . . . . . . 0 . . . 1 2 (L k + L * k k * W k )   . 5) C 5 = ( 1 2 (LW e + L * W * e )
+ is known to have nice properties in the visual servoing case [START_REF] Chaumette | Visual servo control, part I: Basic approaches[END_REF] and is an approximation of the previous one. 6)

C 1 2 = ( 1 2 (L + L * )W e )
+ is another approximation of the second order minimization, by setting i * W i = I 6 . Again, this choice is equal to the previous one when W e is constant.

When a sensor is not rigidly attached to the effector, for instance in an eye-to-hand case, W e is not constant and the desired value W * e depends on the final 3D pose of the sensors wrt. the effector. This pose is generally unknown, making schemes C 3 and C 5 implausible choices. C 4 involves the knowledge of the pose error for each sensor i * W i , which is usually not known either. C 1 2 is thus an acceptable estimation for second order minimization. That is why only C cur ,C * and C 1 2 are considered in the next section. When reaching the final position all possibilities lead to the same behavior, hence local stability, as all combination matrices converge towards C * . However, the behaviors can be very different when s is far from s * .

Additionally, in [START_REF] Malis | Robot control using disparate multiple sensors[END_REF] the authors designed a combination matrix such that the resulting control law is a weighted sum of each subsystem task function (hence named subsystembased fusion):

C = C # = κ 1 1 W -1 e L + 1 . . . κ k k W -1 e L + k (16)
leading to the following control law:

v e = -λ k i=1 κ i i W -1 e L + i (s i -s * i ) = k i=1 κ i v i (17)
with κ i > 0 and

k i=1 κ i = 1.
This formulation is intuitive when each subsystem is of rank 6, and allows a very easy stability analysis: indeed, a sufficient condition for the whole system to be stable is that each subsystem is stable, the main drawback being that the non-stability of one subsystem prevents from ascertaining the stability of the whole system. However, not taking into account the coupling of the subsystems prevents from ensuring a decoupled decrease of the error near the convergence. This approach also makes it necessary to tune every subsystem gain in order to obtain a generic acceptable behavior: this is mainly due to the usually unbalanced velocity contribution from each sensor. In the next section, the combination matrix C # is also compared in order to point out these unbalanced velocity contributions Fig. 2. Eye-in-hand/eye-to-hand cameras. Pose between the two cameras is computed from robot kinematics and initial calibration. Pose between a camera and its landmark is computed during the task and the non-decoupling of the convergence.

Finally, the behavior is of course highly depending on which features are used by each sensor and the sensor poses wrt. the robot frame, that define the structure of L and W e .

IV. EXPERIMENTAL RESULTS

Experiments have been carried on a 6 degrees-of-freedom Gantry robot, the control laws are implemented using ViSP software [START_REF] Marchand | Visp for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. We have chosen to perform a visual servoing (VS) image-based task [START_REF] Chaumette | Visual servo control, part I: Basic approaches[END_REF] with two cameras. VS configurations can be divided into two categories: eye-in-hand, when the camera is attached to the end-effector, and eye-to-hand when the camera is fixed in the workspace. The combination of the two is called eye-in-hand/eye-to-hand cooperation and is a classical VS approach that leads to many applications [START_REF] Lippiello | Position-based visual servoing in industrial multirobot cells using a hybrid camera configuration[END_REF], [START_REF] Dune | One click focus with eye-inhand/eye-to-hand cooperation[END_REF], [START_REF] Flandin | Eye-in-hand / eye-tohand cooperation for visual servoing[END_REF]. This configuration allows a versatile comparison of the proposed sensor data fusion schemes: indeed, the two sensors are not rigidly attached, and known or unknown current interaction matrix represent common situations in visual servoing. We first describe the experimental conditions before presenting the behavior of the robot controlled by only one camera at a time. Afterwards, several multi-camera schemes are compared.

A. Experimental conditions

The eye-in-hand camera F c1 is observing a fixed landmark F o1 composed of four points. The end-effector carries a second landmark F o2 also composed of four points, which is observed by the eye-to-hand camera F c2 . s is composed of 2D-point cartesian coordinates, hence both subsystems are of dimension n i = 8, making s of dimension n = 16. A pose computation is performed from each image thanks to the knowledge of the landmark dimensions, to make it possible to estimate the depth of each point and thus use the current interaction matrix if needed. The objective of the task is to position the end-effector so that both cameras reach their respective reference image. To do so, the robot is controlled in the eye-in-hand camera frame where all velocities are expressed. As previously said in Section II, any other control frame would lead to the same results. To make it possible to express all velocities in F c1 , the pose between the fixed camera and the fixed robot frame, that is c2 M f , is calibrated. From a configuration making both cameras observe the fixed landmark, pose computation is performed from each image that gives c1 M o1 and c2 M o1 . We M f where c1 M f is available through the robot geometrical model and odometry.

In order to avoid any incompatibility between the two reference images, we initially choose a desired pose (0, 0, 0.5, 0, 0, 0) (m,deg) for the eye-in-hand camera, which means we want to have the camera 0.5 m from the landmark, the latter drawing a centered square in the image. From this pose, a reference image is computed and a single, eye-inhand camera VS task is performed. When the convergence is obtained, the corresponding eye-to-hand image is acquired and is then used as the desired eye-to-hand image in the multi-camera schemes. The initial eye-in-hand pose is (0.02, 0.02, 0.52, 47, 12, 28). An ideal behavior would be to follow a geodesic in 3D space, while having trajectories in the image without approaching the image borders.

B. Mono-camera behavior

The first mono-camera VS scheme is the eye-in-hand configuration with the desired interaction matrix, represented in Fig. 3a. Actually, even if only the eye-in-hand camera is used, the eye-to-hand features have a nice trajectory in the image. On a second step, we perform an eye-to-hand VS with the desired interaction matrix, the results of which are shown in Fig. 3b. This time, some of the eye-in-hand features quickly leave the image range, while the task is performed in the eye-to-hand image. A case of unbalanced velocity contribution can be seen on the velocity figures: indeed, v x is the opposite from one scheme to another. That is why the eye-in-hand points have a totally irrelevant x-motion in the eye-to-hand scheme. Hence, mono-camera schemes do not have the same behavior although they are built on coherent features. Of course, fusing several sensors does not systematically lead to unbalanced velocity contribution but this case allows illustrating the proposed fusion schemes.

C. Multi-camera behavior

At first, the subsystem-based fusion C = C # is performed. If equal weights are chosen in (17), that is κ 1 = κ 2 = 0.5, the behavior induced by the eye-to-hand view is predominant and the system acts like in the monocamera, eye-to-hand scheme: eye-in-hand features go out of the image range. This can be explained by the different conditionning of the interaction matrices: indeed, for a point the interaction matrix depends on the inverse depth, and here the desired depth is 0.5m for the eye-in-hand view and about 1.2m for the eye-to-hand view. Therefore, the scheme has to be tuned and we choose a lower weight for the eye-to-hand subsystem: κ 1 = 0.7 and κ 2 = 0.3. The results of the weighted task are shown in Fig. 4, where the eye-in-hand features are still close from leaving the image.

On the contrary, the three proposed low-level fusion schemes are performed without any tuning. Results are given in Fig. 5. The use of C cur leads to a very nice behavior in both images, but the robot trajectory is not satisfactory. As for the desired matrices configuration, results show that the eye-inhand behavior is improved compared to IV-B, by taking into account the eye-to-hand view. An even better behavior is obtained by using the mean value C 1 2 . Results show very regular velocities and error decrease, while trajectories in both images are quite smooth. Actually, this scheme also induces a smooth 3D trajectory of the end-effector.

First experiments show the robustness of low-level fusion when using the different proposed schemes. In order to illustrate the possible limitations, new runs are performed with another initial position (-0.12, 0.08, 0.8, -33, -63, -30), necessiting more complex motion and increasing the desired eye-to-hand depth to 1.4 m.

Mono-camera behaviors (not represented) still indicate that with the new initial position, eye-in-hand scheme performs as in Fig. 3a and eye-to-hand scheme as in Fig. 3b, making eye-in-hand features leave the image range. This is confirmed by the subsystem-based fusion scheme (not represented) that can be achieved again only if the subtask weights are adequately tuned.

As for the proposed low-level fusion schemes, this time they do not behave the same. Schemes using C cur or C 1 2 still converge to the desired position with nice behaviors in both images (Fig. 6a,c). Both schemes induce also satisfactory 3D-trajectories. On the opposite, the use of C * suffers from the large distance from the initial pose to the desired one, as eye-in-hand trajectory is not satisfactory (Fig. 6b).

V. CONCLUSIONS By using the global sensor signal, a multi-sensor fusion scheme can be designed without imposing any hierarchy between the sensors. The stability analysis concurs to the classical conditions on the system rank, with potential local minima in the case of redundancy. Several control laws have been compared in order to illustrate the proposed generic scheme, in the case of image-based visual servoing. Although unsatisfactory cases may be encountered for some control laws when the displacement to realize is very large, experiments highlight the advantages of these schemes and the nice behavior they induce wrt. mono-camera schemes or a weighted multi-camera control law. This framework does not depend on the sensor nature, making possible to extend the proposed scheme to other types of sensors or visual features.

Fig. 1 .

 1 Fig. 1. Multi-sensor model.

Fig. 3 .

 3 Fig. 3. Mono-camera, eye-in-hand (left) and eye-to-hand (right) VS. Velocities, feature errors and trajectories in eye-in-hand (top) and eye-tohand (bottom) images. deduce c2 M f = c2 M o1 c1 M -1 o1 c1

Fig. 4 .

 4 Fig. 4. Subsystem-based fusion C # . Unweighted velocity contributions and trajectories for eye-in-hand (left) and eye-to-hand (right) cameras. The eye-to-hand contribution is more important, the eye-in-hand camera nearly loses some features

Fig. 5 .

 5 Fig. 5. Fusion schemes: Velocities (top), feature error (middle), eye-in-hand and eye-to-hand images, and 3D trajectory of c1. All schemes converge without any subtask weighting. Image trajectories are satisfactory, but 3D behavior is far from geodesic for Ccur.

Fig. 6 .

 6 Fig. 6. Fusion schemes for new initial position. Nice behaviors are found with current (a) and mean (c) combination matrix. C * (b) draws unsatisfactory image behavior, making eye-in-hand view nearly lose some features.

ACKNOWLEDGMENT

The authors acknowledge support from the ANR Psirob SCUAV project of the French ministry of research.