Maxime Lhuillier Lasmea

ENVIRONMENT MODELING FROM IMAGES TAKEN BY A LOW COST CAMERA

Keywords: catadioptric camera, multi-view geometry estimation, environment reconstruction, vision system

This paper describes a system to generate 3D model from image sequence taken in complex environment including variable ground surface, buildings and trajectory loops. Here we use a $1000 catadioptric camera and an approximate knowledge of its calibration. This contrasts to current systems which rely on more costly hardware such as the (calibrated) spherical vision camera Ladybug. All steps of the method are summarized. Experiments include a campus reconstruction from thousands of images.

INTRODUCTION

The automatic 3D modeling of environment from image sequence is a long-term and still active field of research. Wide view field camera is a natural choice for ground-based sequence. Current systems include multi-camera and accurate GPS and INS (Pollefeys et al., 2008) at high cost (>$100K), the Ladybug multicamera (www.ptgrey.com, 2010) at medium cost (≈$12K). Here we use a catadioptric camera at low cost (≈$1K): a mirror of revolution (www.0 -360.com, 2010) mounted on a perspective still camera (Nikon Coolpix 8700) thanks to adapter ring. The medium and high cost systems are more convenient since they provide video sequences and wide view field without sacrificing image resolution.

The first step is the estimation of successive camera poses using Structure-from-Motion (SfM). Recent work [START_REF] Micusik | Piecewise planar city 3d modeling from street view panoramic sequence[END_REF] suggests that bundle adjustment (BA) is not needed if several good experimental conditions are meet: large resolution, wide view field, and accurate knowledge of calibration. Here we do not require accurate calibration since it depends on both mirror profile (mirror manufacturer may not like to reveal it) and the pose between the perspective camera and the mirror. Furthermore, we would like to avoid calibration pattern handling for end-users. For these reasons, BA is needed to estimate simultaneously camera poses, reconstructed points and intrinsic parameters.

Drift or error accumulation occurs in SfM of long image sequences. It should be detected between images taken at similar locations in the scene [START_REF] Anan | Visual localization and loopback detection with a high resolution omnidirectional camera[END_REF]Hartley, 2005, Havlena et al., 2009) and removed using BA [START_REF] Cornelis | Drift removal for sequential structure from motion algorithms[END_REF]). Here we remove drift using constrained bundle adjustment (CBA) based on [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF], instead of a re-weighted version of the standard BA [START_REF] Cornelis | Drift removal for sequential structure from motion algorithms[END_REF] which relies much on heuristic initialization. The next step is the estimation of the 3D scene. Like (Pollefeys et al., 2008, Micusik and[START_REF] Micusik | Piecewise planar city 3d modeling from street view panoramic sequence[END_REF], we apply dense stereo method on a small number of M consecutive images, iterate this process several times along the sequence, and merge the obtained view-centered 3D models into the global and final 3D model. Furthermore, we use an over-segmentation in the reference image of view-centered model in conjunction with dense stereo (Zitnick andKang, 2007, Micusik and[START_REF] Micusik | Piecewise planar city 3d modeling from street view panoramic sequence[END_REF]. Super-pixels (small regions) are useful to reduce stereo ambiguity, to constrain depth discontinuities at super-pixel borders selected among image contours, to reduce computational complexity.

Our over-segmentation is defined by triangle mesh in image such that (1) super-pixel is a list of connected triangles and (2) triangles of the view-centered model are back-projected triangles of the image mesh. In work (Zitnick andKang, 2007, Micusik and[START_REF] Micusik | Piecewise planar city 3d modeling from street view panoramic sequence[END_REF], super-pixel is pixel list and is unused by surface meshing. Our choice has several advantages. Mesh regularizes super-pixel shape and defines the resolution of final reconstruction by view field sampling. This is useful to compress large scene. Besides, we obtain triangles in 3D such that the consistency with image contours can not be degraded by depth error unlike [START_REF] Chai | Depth map compression for real-time view-based rendering[END_REF]. This is not a luxury because depth estimation is difficult in uncontrolled environment. Our super-pixels are not restricted to be planar in 3D contrary to those in (Zitnick andKang, 2007, Micusik and[START_REF] Micusik | Piecewise planar city 3d modeling from street view panoramic sequence[END_REF].

The last step is filtering of triangles in view-centered models. It reduces the redundancy and removes the most inaccurate and unexpected triangles. Here we accept non-incremental method with complexity greater than linear in the number of camera poses, since the main calculations are done in the previous step (which has linear complexity and is parallelizable using multi-cores).

This paper improves work (Lhuillier, 2008a[START_REF] Lhuillier | Toward automatic 3d modeling of scenes using a generic camera model[END_REF] thanks to polygons for super-pixels, drift removal using CBA, accelerations for larger sequence (feature selection in the SfM step, complexity handling in the triangle filtering step), redundancy reduction, experiments on more challenging sequence.

OVERVIEW OF THE RECONSTRUCTION METHOD

This Section has six parts describing camera model, Structurefrom-Motion, drift removal, over-segmentation mesh, view-cen--tered model and triangle filtering.

Camera Model

A single view-point camera model with a general radial distortion function and a symmetry axis is used (Lhuillier, 2008a). It simplifies the reconstruction process for non-single view-point camera (if any), assuming that depth is large enough.

We assume that the projection of the whole view field is delimited by two concentric circles which can be detected in images. Furthermore, the mirror manufacturer provides the lower an d upper bounds of the "ray angle" between observation ray and the symmetry axis. The initial calibration is that of equiangular camera: the mapping from the ray angle of 3D point to the distance between the point projection and the circle center is linear.

Structure-from-Motion

Structure-from-Motion (Lhuillier, 2008a) is applied to estimate geometry (camera poses and a sparse cloud of 3D points) using the calibration initialization of Section 2.1: estimate the 2-view and 3-view geometries of consecutive images from matched Harris points (step 1) and then estimate the whole sequence geometry using bundle adjustment (BA) applied in a hierarchical framework (step 2). Another BA is applied to refine simultaneously radial distortion parameters and the 3D assuming that the radial distortion is constant in the whole sequence (step 3).

Although it is important to get a maximum number of reconstructed features for 3D scene modeling, we noticed that there are many more 3D points than needed to initialize the geometry in our wide view field context. Indeed, this is not uncommon to have more than 2000 features per outdoor image involved in BA, and this implies a waste of computation time. So the number n f of features per image is limited to 500 in all BAs of steps 1-2-3: 3D points are randomly selected and removed while n f is larger than 500 in all images. In practice, this simple scheme holds a good point distribution in the view field. The 4 th step is the following: step 3 is applied a second time without n f limit to get a maximum number of reconstructed features consistent with the poses and calibration.

Our BA is the sparse Levenberg-Marquardt method assuming that there are more structure parameters than camera ones: it includes profile Choleski decomposition [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] of the reduced camera system.

Drift Removal

Drift or error accumulation is unavoidable in the geometry estimation of long sequence. Methods [START_REF] Havlena | Randomized structure from motion based on atomic 3d models from camera triplets[END_REF][START_REF] Anan | Visual localization and loopback detection with a high resolution omnidirectional camera[END_REF][START_REF] Cornelis | Drift removal for sequential structure from motion algorithms[END_REF] detect the drift between two reconstructed images i and j if these images are taken at similar locations. These methods also provide list Li,j of point matches between i and j, which is used to remove drift. Without drift removal, scene part visible in i and j is reconstructed twice.

Adequate BA and its initialization are applied to remove reconstruction duplicates while maintaining low re-projection errors in the whole sequence of images {0, 1 • • • n -1}. Once the 2D feature match list Li,j is given for pair {i, j}, we remove the drift between i and j as follows. First, we choose integer k such that the neighborhood

N (i) of i is the list {i -k • • • i • • • i + k}. Sec- ond, N (i)
and its data (3D geometry and image features) are duplicated in images N (n + k) = {n • • • n + 2k} such that images n + k and i are the same. Third, we use RANSAC to fit the similarity transformation s of 3D points matched by Li,j and apply s to obtain N (n + k) geometry in the same basis as

{0 • • • n -1} geometry. Fourth, {0 • • • n + 2k}
geometry is refined by BA taking into account L n+k,j (L n+k,j is a copy of Li,j with image index changes). Now N (n + k) geometry is the drift correction of N (i) geometry. Fifth, constrained bundle adjustment (CBA) is applied to minimize the global re-projection error subject to constraint c(x) = 0, where x concatenates 3D parameters of {0 • • • n + 2k} and c(x) concatenates the drifts between poses of N (i) and N (n + k) (more details in Appendix B). At this point, the drift between i and j is removed but N (n + k) is redundant. Last, we remove data involving N (n + k) and apply BA to {0 • • • n -1} geometry by taking into account Li,j. This scheme is applied using a limit of n f = 500 features per image to avoid waste of time as in Section 2.2, with the only difference that Li,j and L n+k,j are not counted by this limit.

Over-Segmentation Mesh

This mesh has the following purposes. It makes image-based simplification of the scene such that the view field is uniformly sampled. This is useful for time and space complexities of further processing and more adequate than storing depth maps for all images. Furthermore, it segments the image into polygons such that depth discontinuities are constrained to be at polygon borders. These borders are selected among image contours. If contours are lacking, borders are preferred on concentric circles or radial segments of the donut image. This roughly corresponds to horizontal and vertical depth discontinuities for standard orientation of the catadioptric camera (if its symmetry axis is vertical).

In short, the image mesh is build in four steps: initialization checkerboard (rows are concentric rings, columns have radial directions, cells are two Delaunay triangles), gradient edge integration (perturb vertices to approximate the most prominent image contours), optimization (perturb all vertices to minimize the sum, for all triangles, of color variances, plus the sum, for all vertices, of squared moduluses of umbrella operators), and polygon segmentation (triangles are regrouped in small and convex polygons). In practice, lots of polygons are quadrilaterals similar to those of the initialization checkerboard.

View-Centered 3D Model

View-centered 3D model is build from image mesh (Section 2.4) assuming that the geometry is known (Sections 2.1, 2.2 and 2.3).

Depth Map in the Reference Image

We reproject catadioptric image onto the 6 faces of a virtual cube and apply match propagation [START_REF] Lhuillier | Match propagation for imagebased modeling and rendering[END_REF] to two parallel faces of two cubes. The depth map in the i th image is obtained by chaining matches between consecutive images of N (i). In the next steps, the over-segmentation mesh in the i th image is back-projected to approximate the depth map.

Mesh Initialization For all polygons in image i, a plane in 3D (or nil if failure) is estimated by a RANSAC procedure applied on depths available inside the polygon. A depth is inlier of tentative plane if the corresponding 3D point is in this plane up to thresholding (Appendix A). The best plane π defines 3D points which are the intersections between π and the observation rays of the polygon vertices in the i th image. These 3D points are called "3D vertices of polygon" although the polygon is 2D.

For all edges e in image i, we define boolean be which will determine the connection of triangles in both edge sides. Since depth discontinuity is prohibited inside polygons, we initialize be = 1 if both triangles are in the same polygon (other cases: be = 0).

Connection Connections between polygons are needed to obtain a more realistic 3D model. Thus edge booleans are forced to 1 if neighboring polygons satisfy coplanarity constraint. For all polygons p with a plane in 3D, we collect in list Lp the polygons q in p-neighborhood (including p) such that all 3D vertices of q are in the plane of p up to thresholding (Appendix A). If the sum of solid angles of polygons in Lp is greater than a threshold, we have confidence in coplanarity between all polygons in Lp and we set be = 1 for all edges e between two polygons of Lp.

Hole Filling

We fill hole H if its neighborhood N is coplanar. Both H and N are polygon lists. The former is a connected component of polygons without plane in 3D and the latter contains polygons with plane in 3D. Neighborhood N is coplanar if there is a plane π (generated by random samples of vertices of N ) such that all 3D vertices in N are in π up to thresholding (Appendix A). If N is coplanar, all polygons of H get plane π and we set be = 1 for all edges between two triangles of H ∪ N .

View-Centered Mesh in 3D Now, 3D triangles are generated by back-projection of triangles in the image mesh using polygon planes and edge booleans. Triangle t inside a polygon p with plane in 3D is reconstructed as follows. Let Cv be the circularlylinked list of polygons which have vertex v of t. We obtain sublist(s) of Cv by removing the Cv-links between consecutive polygons which share edges e such that be = 0. A Cv-link is also removed if one of its two polygons has not plane in 3D. Let S p v be the sub-list which contains p. The 3D triangle of t is defined by its 3 vertices: the 3D vertex reconstructed for v is the mean of 3D vertices of the polygons in S p v which correspond to v. Refinement Here we provide a brief overview of the refinement, which is detailed in [START_REF] Lhuillier | Toward automatic 3d modeling of scenes using a generic camera model[END_REF]. The view-centered mesh (3D triangles with edge connections) is parametrized by the depths of its vertices and is optimized by minimizing a weighted sum of discrepancy and smoothing terms. The discrepancy term is the sum, for all pixels in a triangle with plane in 3D, of the squared distance between the plane and 3D point defined by pixel depth (Appendix A). The smoothing term is the sum, for all edges which are not at image contour, of the squared difference between normals of 3D triangles in both edge sides. This minimization is applied several times by alternating with mesh operations "Triangle Connection" and "Hole Filling" [START_REF] Lhuillier | Toward automatic 3d modeling of scenes using a generic camera model[END_REF].

Triangle Filtering

For all i, the method in Section 2.5 provides a 3D model centered at image i using images N (i). Now, several filters are applied on the resulting list of triangles to remove the most inaccurate and unexpected triangles.

Notations

We need additional notations in this Section. Here t is a 3D (not 2D) triangle of the i th view-centered model. The angle between two vectors u and v is angle(u, v) ∈ [0, π]. Let di, ci be the camera symmetry direction and center at the i th pose in world coordinates (di points toward the sky). Let Uj(v) be the length of major axis of covariance matrix Cv of v ∈ R 3 as if v is reconstructed by ray intersection from v projections in images N (j) using Levenberg-Marquardt.

Uncertainty Parts of the scene are reconstructed in several viewcentered models with different accuracies. This is especially true in our wide view field context where a large part of the scene is visible in a single image. Thus, the final 3D model can not be defined by a simple union of the triangle lists of all view-centered models. A selection on the triangles should be done. We reject t if the i th model does not provide one of the best available uncertainties from all models: if all vertices v of t have ratio Ui(v)/ minj Uj(v) greater than threshold u0.

Prior Knowledge Here we assume that the catadioptric camera is hand-held by a pedestrian walking on the ground such that (1) the camera symmetry axis is (roughly) vertical (2) the ground slope is moderated (3) the step length between consecutive images and the height between ground and camera center do not change too much. This knowledge is used to reject unexpected triangles which are not in a "neighborhood of the ground".

A step length estimate is s = mediani||ci -ci+1||. We choose angles αt, α b between di and observation rays such that 0

< αt < π 2 < α b < π. Triangle t is rejected if it is below the ground: if it has vertex v such that angle(di, v -ci) > α b and height 1 s d T i (v -ci)
is less than a threshold. The sky rejection does not depend on scale s. We robustly estimate the mean m and standard deviation σ of height d T i (vci) for all vertex v of the i th model such that angle(di, vci) < αt. Triangle t is rejected if it has vertex v such that angle(di, vci) < αt and

1 σ (d T i (v -ci) -m) is greater than a threshold.
Reliability 3D modeling application requires additional filtering to reject "unreliable" triangles that filters above miss. These triangles includes those which are in the neighborhood of the line supporting the cj , j ∈ N (i) (if any). Inspired by a two-view reliability method [START_REF] Doubek | Reliable 3d reconstruction from a few catadioptric images[END_REF], we reject t if it has vertex v such that max j,k∈N (i) angle(vcj, vc k ) is less than threshold α0. This method is intuitive: t is rejected if ray directions vcj , j ∈ N (i) are parallel.

Redundancy Previous filters provide a redundant 3D model insofar as scene parts may be reconstructed by several mesh parts selected in several view-centered models. Redundancy increases with threshold u0 of the uncertainty-based filter and the inverse of threshold α0 of the reliability-based filter. Our final filter decreases redundancy as follows: 3D triangles at mesh borders are progressively rejected in the decreasing uncertainty order if they are redundant with other mesh parts. Triangle t is redundant if its neighborhood intersects triangle of the j th view-centered model (j = i). The neighborhood of t is the truncated pyramid with base t and three edges. These edges are the main axes of the 90% uncertainty ellipsoids of the t vertices v defined by Cv.

Complexity Handling

We apply the filters above in the increasing complexity order to deal with large number of triangles (tens of millions in our case). Filters based on prior knowledge and reliability are applied first. Thanks to cj and reliability angle α0, we estimate radius ri and center bi of a ball which encloses the selected part of the i th view-centered model: bi

= 1 2 (ci-1 + ci+1) and tan(α0/2) = ||ci+1 -ci-1||/(2ri) if N (i) = {i -1, i, i + 1}. Let N (i) = {j, ||bi -bj|| ≤ ri + rj}
be the list of view-centered models j which may have intersection with the i th view-centered model. Then the uncertaintybased filter is accelerated thanks to N (i): triangle t is rejected if Ui(v)/ min j∈N(i) Uj (v) ≥ u0 for all vertices v of t. Last, the redundancy-based filter is applied. Its complexity due to uncertainty sort is O(p log(p)), where p is the number of triangles. Its complexity due to redundancy triangle tests is O(p 2 ), but this is accelerated using test eliminations and hierarchical bounding boxes.

EXPERIMENTS

The image sequence is taken in the university campus on august 15-16th afternoons without people. There are several trajectory loops, variable ground surface (road, foot path, unmown grass), buildings, corridor and vegetation (bushes, trees). This scene accumulates several difficulties: not 100% rigid scene (due to breath of wind), illuminations changes between day 1-2 subsequences (Fig 2 ), low-textured areas, camera gain changes (uncorrected), aperture problem and non-uniform sky at building-sky edges. The sequence has 2260 3264 × 2448 JPEG images, which are reduced by 2 in both dimensions to accelerate all calculations.

The perspective camera points toward the sky, it is hand-held and mounted on a monopod. The mirror (www.0 -360.com, 2010) provides large view field: 360 degrees in the horizontal plane, about 52 degrees above and 62 degrees below. The view field is projected between concentric circles of radii 572 and 103 pixels. We use a core 2 duo 2.5Ghz laptop with 4Go 667MHz DDR2.

First, the geometry is estimated thanks to the methods in Sections 2.1, 2.2 and 2.3. The user provides the list of image pairs {i, j} such that drift between i and j should be removed (drift detection method is not integrated in the current version of the system). Once the geometry of days 1 and 2 sub-sequences are estimated using the initial calibration, points are matched between images i and j using correlation (Fig. 2) and CBA is applied to (424,451,1434,216) images and are obtained by (16,62,39,9) CBA iterations in (190,2400(16,62,39,9) CBA iterations in (190, ,1460,370) ,370) seconds, respectively. We think that a large part of the drift in case (d) is due to the single view point approximation, which is inaccurate in the outdoor corridor (top right corner of Fig. 4) with small scene depth. A last BA is applied to refine the geometry (3D and intrinsic parameters) and to increase the list of reconstructed points. The final geometry (Fig. 1.g) has 699410 points reconstructed from 3.9M Harris features; the means of track lengths and 3D points visible in one view are 5.5 and 1721, respectively.

Then, 2256 view-centered models are reconstructed thanks to the methods in Section 2.4 and 2.5 using k = 1. This is the most time consuming part of the method since one view-centered model is computed in about 3 min 30s. The first step of view-centered model computation is the over-segmentation mesh in the reference image. It samples the view field such that the super-pixels at the neighborhood of horizontal plane projection are initialized by squares of size 8 × 8 pixels in the images. The mean of number of 3D triangles is 17547. Fig. 3 shows super-pixels of a reference image and the resulting view-centered model. Last, the methods in Section 2.6 are applied to filter the 39.6M triangles stored in hard disk. A first filtering is done using reliability (α0 = 5 degrees), prior knowledge and uncertainty filters (u0 = 1.1): we obtain 6.5M triangles in 40 min and store them in RAM. Redundancy removal is the last filtering and selects 4.5M triangles in 44 min. Texture packing and VRML file saving take 9 min. Fig. 4 shows views of the final model. We note that the scene is curved as if it lie on a sphere surface whose diameter has several kilometers: a vertical component of drift is left.

An other experiment is the quantitative evaluation of scene accuracy (discrepancy between scene reconstruction and ground truth) for a view-centered model using k = 1. A representative range of baselines is obtained with the following ground truth: the [0, 5] 3 cube and camera locations defined by ci = `1 1 + i/5 1 ´T , i ∈ {0, 1, 2} (numbers in meters). First, synthetic images are generated using ray-tracing and the knowledge of mirror/perspective camera/textured cube. Second, methods in Sections 2.1, 2.2, 2.4 and 2.5 are applied. Third, a camerabased registration is applied to put the scene estimation in the coordinate frame of ground truth. Last, the scene accuracy a0.9 is estimated using the distance e between vertex v of the model and the ground truth surface: inequality |e(v)| ≤ a0.9||v -c1|| is true for 90% of vertices. We obtain a0.9 = 0.015.

CONCLUSION

We present an environment reconstruction system from images acquired by a $1000 camera. Several items are described: camera model, structure-from-motion, drift removal, view field sampling by super-pixels, view-centered model and triangle filtering. Unlike previous work, image meshes define both super-pixels (convex polygons) and triangles of 3D models. The current system is fully automatic up to the loop detection step (that previous methods could solve). Last it is experimented on a challenging sequence.

Future work includes loop detection integration, better use of visibility and prior knowledge for scene reconstruction, joining meshes of view-centered models to form a continuous surface, and accelerations using GPU.

Figure 1 :

 1 Figure 1: Geometry estimation steps: (a) day 1 sequence, (b) remove drift, (c) merge day 1-2 sequences, (d-f) remove drifts, (g) use all features. All results are registered in rectangle [0, 1] × [0, 0.8] by enforcing constant coordinates on the two poses surrounded by gray disks in (a). Gray disks in (b,d,e,f) show poses where drift is corrected. Day 1-2 sequences are merged on gray disk in (c). remove drifts using k = 1. Cases (b,d,e,f) of Fig. 1 are trajectory loops with(424,451,1434,216) images and are obtained by(16,62,39,9) CBA iterations in(190,2400,1460,370) seconds, respectively. We think that a large part of the drift in case (d) is due to the single view point approximation, which is inaccurate in the outdoor corridor (top right corner of Fig.4) with small scene depth. A last BA is applied to refine the geometry (3D and intrinsic parameters) and to increase the list of reconstructed points. The final geometry (Fig.1.g) has 699410 points reconstructed from 3.9M Harris features; the means of track lengths and 3D points visible in one view are 5.5 and 1721, respectively.

Figure 2 :

 2 Figure 2: From top to bottom: 722 and 535 matches of Li,j used to remove drift in cases (d) and (e) of Fig. 1. Images of days 1 and 2 are on the left and right, respectively.

Figure 4 :

 4 Figure 4: Top view (top left), local views (top right) and oblique view (bottom) of the final 3D model of the campus. The top view can be matched with Fig. 1.g. The transformation between top and oblique views is a rotation around horizontal axis.

www. 0 -360.com, 2010. www.ptgrey.com, 2010. Zitnick, C. and Kang, S., 2007. Stereo for image-based rendering using image over-segmentation. IJCV 75(1), pp. 49-65.

APPENDIX A: POINT AND PLANE THRESHOLDING

Let p be a 3D point. The covariance matrix Cp of p is provided by ray intersection from p projections in images

In this paper, ray intersection and covariance Cp result from the angle error in [START_REF] Lhuillier | Toward automatic 3d modeling of scenes using a generic camera model[END_REF]. The Mahalanobis distance Dp between points p and p ′ is Dp(p

We define points p = ci + zu and p ′ = ci + z ′ u using camera location ci, ray direction u and depths z, z ′ . If z is large enough, u is a good approximation of the main axis of Cp: we have Cp ≈ σ 2 p uu T and u T C -1 p u ≈ σ -2 p where σ 2 p is the largest singular value of Cp. In this context, we obtain Dp(p ′ ) ≈ |z-z ′ | σp .

If x has the Gaussian distribution with mean p and covariance Cp, D 2 p (x) has the X 2 distribution with 3 d.o.f. We decide that points p and p ′ are the same (up to error) if both D 2 p (p ′ ) and D 2 p ′ (p) are less than the 90% quantile of this distribution: we decide that p and p ′ are the same point if D(p, p ′ ) ≤ 2.5 where [START_REF] Schindler | On robust regression in photogrammetric point clouds[END_REF]. Thus Cp ≈ σ 2 p uu T and p ′ ∈ π imply

Last, we obtain the point-to-plane thresholding and distance used in Section 2.5. We decide that p is in plane π if D(p, p ′ ) ≤ 2.5 where p ′ ∈ π. The robust distance between p and π is

APPENDIX B: CONSTRAINED BUNDLE ADJUSTMENT

In Section 2.3, we would like to apply CBA (constrained bundle adjustment) summarized in [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] to remove the drift. This method minimizes the re-projection error function x → f (x) subject to the drift removal constraint c(x) = 0, where x concatenates poses and 3D points. Here we have c(x) = x1x g 1 where x1 and x g 1 concatenate 3D locations of images N (i) and their duplicates of images N (n + k), respectively. All 3D parameters of sequence {0 • • • n + 2k} are in x except the 3D locations of N (j) and N (n + k). During CBA, x g 1 is fixed and x1 evolves towards x g 1 . However, there is a difficulty with this scheme. CBA iteration [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] improves x by adding step ∆ which minimizes quadratic Taylor expansion of f subject to 0 ≈ c(x + ∆) and linear Taylor expansion c(x + ∆) ≈ c(x) + C∆. We use notations x T = `xT

´, C = `C1 C2 ánd obtain C1 = I, C2 = 0. Thus, we have ∆1 = -c(x) at the first CBA iteration. On the one hand, ∆1 = -c(x) is the drift and may be very large. On the other hand, ∆ should be small enough for quadratic Taylor approximation of f .

The "reduced problem" in [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] is used: BA iteration minimizes the quadratic Taylor expansion of ∆2 → g(∆2) where g(∆2) = f (∆(∆2)) and ∆(∆2) T = `-c(x) T ∆ T

2

´.

Step ∆2 meets H2(λ)∆2 = -g2, where (λ, g2, H2(λ)) are damping parameter, gradient and damped hessian of g. Update x ← x + ∆(∆2) holds if g(∆2) < min{1.1f0, g(0)}, where f0 is the value of f (x) before CBA. It can be shown that this inequality is true if c(x) is small enough and λ is large enough.

Here we reset c by cn at the n th iteration of CBA to have a small enough c(x). Let x 0 1 be the value of x1 before CBA. We use cn

, where γn increases progressively from 0 (no constraint at CBA start) to 1 (full constraint at CBA end). One CBA iteration is summarized as follows. First, estimate ∆2(γn) for the current value of (λ, x) (a single linear system H2(λ)X = Y is solved for all γn ∈ [0, 1]). Second, try to increase γn such that g(∆2(γn)) < min{1.1f0, g(0)}. If the iteration succeeds, apply x ← x + ∆(∆2). Furthermore, apply λ ← λ/10 if γn = γn-1. If the iteration fails, apply λ ← 100λ. If γn > γn-1 or γn = 1, choose γn+1 = γn at the (n + 1) th iteration to obtain ∆(∆2) T = `0T ∆ T 2 ´and to decrease f as soon (or much) as possible.


Alexandre Eudes
email: alexandre.eudes@lasmea.univ-bpclermont.fr

Sylvie Naudet-Collette

Maxime Lhuillier
email: maxime.lhuillier@lasmea.univ-bpclermont.fr

Michel Dhome
email: michel.dhome@lasmea.univ-bpclermont.fr

Weighted Local Bundle Adjustment and Application to Odometry and Visual SLAM Fusion

Local Bundle Adjustments were recently introduced for visual SLAM (Simultaneous Localization and Mapping). In Monocular Visual SLAM, the scale factor is not observable and the reconstruction scale drifts as time goes by. On long trajectory, this problem makes absolute localisation not usable. To overcome this major problem, data fusion is a possible solution. In this paper, we describe Weighted Local Bundle Adjustment(W-LBA) for monocular visual SLAM purposes. We show that W-LBA used with local covariance gives better results than Local Bundle Adjustment especially on the scale propagation. Moreover W-LBA is well designed for sensor fusion. Since odometer is a common sensor and is reliable to obtain a scale information, we apply W-LBA to fuse visual SLAM with odometry data. The method performance is shown on a large scale sequence.

Introduction

Monocular Visual SLAM is a still active topic in computer vision. A first approach comes from Davison [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF] in 2003. He adapts SLAM community techniques based on Kalman filter to input images in SLAM algorithms.

Next, Nister et al. [START_REF] Nister | Visual odometry[END_REF] take the problem of visual SLAM with the photogrammetric community point of view and adapts Structure from Motion algorithms to be used in real time. There is no optimisation to improve accuracy in the first work of Nister et al. Later, in 2006, Mouragnon et al. [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF] introduce local bundle adjustment (LBA) to solve this problem. Even if bundle adjustment based SLAM is more accurate than Kalman based one [START_REF] Strasdat | Real-Time Monocular SLAM: Why Filter?[END_REF], no covariance is provided in [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF]. That makes data fusion not optimal.

Recently, we introduce in [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF] uncertainty on the localisation and open the possibility to make fusion from LBA-based visual SLAM. We use weighted local bundle adjustment c 2010. The copyright of this document resides with its authors. It may be distributed unchanged freely in print or electronic forms.

(W-LBA) only for uncertainty propagation but keep (non weighted) LBA for geometry optimisation.

In monocular SLAM, the scene structure and camera motion are theoretically reconstructed up to an unknown scale factor. This reconstruction can be updated to the real world scale by rescaling with only one extra measurement. Unfortunately, experiments show that the scale factor drifts during long trajectories. This problem is a major limitation in monocular visual SLAM.

One possible solution of this problem is to rely on another sensor to estimate the scale. It involves the fusion of monocular visual information with an other sensor. Odometer seems to be the most adapted sensor: it's a common sensor in robotics and automotive industry and it provides reliable scale information. Karlsson et al. (fusion algorithm [START_REF] Karlsson | The vSLAM algorithm for robust localization and mapping[END_REF]) and Goncalves et al. (vision algorithm [START_REF] Goncalves | A visual front-end for simultaneous localization and mapping[END_REF]) use a particle filter to fuse odometer with distance between robot and visual landmark. Cumani et al. [START_REF] Cumani | Integrating monocular vision and odometry for SLAM[END_REF] reconstruct the scene and camera position by chaining triples of cameras reconstructed by standard SfM techniques. The odometer is then used to correct the scale propagation and the 3D point positions. Their method is similar to the method described in Section 5.1 but additional optimisations are performed in our work.

In this paper, we propose different applications of W-LBA to reduce drift in scale. In Section 2 and 3, we recall visual SLAM and local bundle adjustment process. In Section 4, we show that W-LBA can also be used for geometry optimisation and propose another weighting scheme in order to improve efficiency. In Section 5, we use the W-LBA to fuse visual SLAM with odometer measurements. Finally in Section 6, we provide experimental results and compare the proposed solution with previous work. The aim of this algorithm is to determine the camera localisation at each image in the reference coordinate frame. In addition, a sparse 3D point cloud is reconstructed to support the iterative localisation process. We assume that the initialisation of the algorithm is done (the first three camera poses and their 3D point cloud are estimated).

LBA based SLAM

Matching 2D. The first step of the method is a 2D tracking of interest points which provides 2D/2D correspondences between consecutive images. Interest points are extracted from each image using Harris detector [START_REF] Harris | A combined corner and edge detector[END_REF]. Those points are then matched between the two images using SURF [START_REF] Bay | Surf: Speeded up robust features[END_REF] descriptor.

Camera pose estimation. With this 2D/2D correspondence, we can associate a previously triangulated 3D point to an observation in the current image. Thanks to those 3D/2D correspondences, the current camera pose is estimated using Grunert's algorithm [START_REF] Haralick | Review and analysis of solutions of the three point perspective pose estimation problem[END_REF] applied in the RANSAC [START_REF] Fischler | Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography[END_REF] framework. Since the camera intrinsic parameters are known, only 6 parameters (3 for translation, 3 for rotation) need to be estimated. Key-frames. They are a sub-sampling of the input video. Indeed, we don't treat all images to ensure sufficient baseline between two frames for triangulation and reduce computational time. A frame is declared as key-frame, if a condition based on the number of 2D/2D correspondences is met. In those special frames, new 3D points are triangulated from the two 2D/2D correspondences between the last 3 key-frames. Last, the result of the previous steps is refined by LBA(Section 3) to optimise the geometry.

3 Bundle Adjustment

Notation

We denote C (v) the covariance matrix of the random vector v, C (u,v) the covariance matrix of the random vector (u v ) and C (v/u) the covariance between vectors v and u:

C (u,v) = C (u) C (u/v) C (v/u) C (v) (1)
Notation r ∼ N (r, C (r) ) means that the Gaussian vector r has mean r and covariance C (r) .

Camera Poses

The camera pose at key-frame t is refined by several LBA over time, therefore to distinguish the pose between refinements, we define for each camera pose a double timing index: C t t represents the pose of key-frame t refined at time t . We define c t t as the 3D position of the camera C t t .

Global Bundle Adjustment

Bundle adjustment is a well known iterative method [START_REF] Triggs | Bundle adjustment-a modern synthesis[END_REF] designed to solve non-linear least square problems in Structure-from-Motion. It refines simultaneously the scene (3D points cloud) and the camera poses by minimising the reprojection errors:

x → ||y -P(x)|| 2 (2)
where vector x usually concatenates 6 parameters (3 for rotation,3 for translation) for each camera pose and 3 parameters for each 3D point. y concatenates interest points detected in images, P concatenates projection functions, and ||.|| is the Euclidean norm.

Unfortunately, this method becomes very time consuming even with reduction and variable ordering methods [START_REF] Triggs | Bundle adjustment-a modern synthesis[END_REF]. To overcome this limitation, iterative methods based on Bundle Adjustment, called Local Bundle Adjustment, were introduced. Those methods optimise only a limited number of cameras and 3D points. Two versions are detailed below.

Local Bundle Adjustment (LBA)

In this Section, we recall the Local Bundle Adjustment (LBA) presented in [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF].

Local Bundle Adjustment is a bundle adjustment such that (1) the estimated parameters x are the poses of the n most recent key-frames and the list of 3D points which have at least one observation in these frames, (2) the reprojection errors are only considered in the N most recent key-frames (n < N).

This LBA B is concisely written as

x = B(p, y) = argmin x||y -P(p, x)|| 2 . ( 3
)
p contains the poses of the Nn cameras which are not estimated, x concatenates the n poses and the 3D points which are estimated, and y is the vector of detected 2D points. Even in large sequence, LBA estimates only 6n + 3s parameters (s is the number of 3D points in the n last key-frames). In this way the geometry of long sequence can be iteratively refined in real-time (n = 3 and N = 10 are commonly used values).

Weighted Local Bundle Adjustment (W-LBA)

Here we recall the definition of an other LBA, the Weighted Local Bundle Adjustment (W-LBA) presented in [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF]. W-LBA provides a Maximum Likelihood Estimator (MLE) to estimate the structure and camera poses simultaneously. Contrary to the LBA, this estimator takes care of the uncertainty on previous estimated cameras.

As in the LBA formulation, the W-LBA estimates the structure and n camera poses contained in x. The only difference is that N poses are now optimised. The N cameras are split in two, n cameras are freely optimised and contained in x and the Nn old cameras are in p. The old cameras are considered to be already well optimised, so they are less allowed to move than new one but not fully fixed as in LBA. The constraint on old poses localisation p is build according to the localisation at the previous step and weighted by a covariance matrix.

This local bundle assumes that p ∼ N ( p, C p ) and y ∼ N (ȳ, σ 2 I) are independent, where ȳ = P( p, x). According to this Hypothesis, the MLE B w (p, y) of p and x is:

x = B w (p, y) = argmin ( p,x) 1 σ 2 ||y -P( p, x)|| 2 + (p -p) C -1 p (p -p), ( 4
)
where σ is the noise on 2D points. In the W-LBA, C p is the "weight" on camera poses. This weighting acts on how the W-LBA takes into account of the old estimated poses p. In previous work [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF], we propagate C p through iterations and call it global covariance. Since the old camera poses are re-evaluated accordingly to the a priori constraints, this formulation allows easier integration of external information. In Section 4, we introduce the notion of local covariance.

Local and Global Covariance

In this Section, we explain the difference between the "global" covariance we defined in [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF] and the "local" one we propose here. In the W-LBA, we defined the C p in Eq.4 as a global covariance. The global covariance has the property to increase during time and is similar to one computed with global bundle adjustment whereas the local one does not grow and represents uncertainty of camera respectively to the oldest camera in the local bundle. This covariance is computed with the standard covariance estimation technique [START_REF] Hartley | Multiple view geometry in computer vision[END_REF], by inverting the approximation of the Hessian (J J) -1 of the projection function (P in Eq.2) minimised by Bundle Adjustment on the N last cameras.

To inverse the Hessian, we need to fix the gauge on the BA [START_REF] Triggs | Bundle adjustment-a modern synthesis[END_REF]. This is done by removing lines of the Jacobian matrix corresponding to fixed parameters. Here, we chose the first frame pose (R t-N , t t-N ) and the largest coordinate of the 3D location of the (tn)-th pose C t-n t-1 . To integrate it in the W-LBA, the local covariance is evaluated on the result of the optimisation done at the previous key-frame. By keeping only the block of (J J) -1 corresponding to the camera parameters, we evaluate :

C (C t-N t-1 ,...,C t-1 t-1 ) . (5)
Then, we remove the first and the (n -1) last poses from Eq.5 to get the C p :

C p = C (C t-N+1 t-1 ,...,C t-n t-1 ) . (6)
This new covariance could now be used in the bundle (B w ) to optimise the next step (at time t) using Eq.4. Since the C p in W-LBA is a local covariance, we optimise parameters with local constraints.

Odometer Integration

In this Section, we present two methods of fusion between odometer and visual SLAM. The first one is a simple modification of the standard visual SLAM process. The second uses the W-LBA with local covariance to integrate odometer constraints in the optimisation process.

We defined d l the odometer distance between time l and l + 1. Since in this Section all cameras are evaluated at time t, we omit the second index for improving notation readability.

LBA Fusion

In this method presented in [START_REF] Eudes | Fast odometry integration in local bundle adjustment-based visual slam[END_REF], the idea is to change LBA initialisation by substituting the estimated pose of the current key-frame t by a corrected one.

The estimated 3D location c t is replaced by ct thanks to the scale correction provided by

ct = c t-1 + d t-1 c t -c t-1 ||c t -c t-1 || 2 , ( 7
)
where ||.|| 2 is the Euclidean norm. The camera orientation is kept unchanged.

We integrate this correction after the camera pose estimation in t and before the triangulation of new 3D points. So the new 3D points are triangulated at a corrected scale. Then this new value of pose at time t and the new 3D points are optimised by LBA as in the visual SLAM original process (figure 1).

W-LBA fusion

W-LBA with local covariance allows to estimate a more accurate position, since all poses in the sliding window are refined. Moreover, a part of the estimated camera (old cameras p) are constrained by previous estimation weighted by covariance C p . Using this property, this LBA can be used to impose constraint on old cameras (p in Section 3.4). In our case, we will add constraints coming from the odometer measurement by correcting the constraint on old poses. Thus we replace all the camera c by c in p. We introduce the following notations:

norm l = ||c l+1 -cl || 2 :
Euclidean distance between cl and c l+1 .

dir l = c l+1 -c l norm l
: Direction defined by the vector cl c l+1 . We do iteratively the same treatment on each pose in p. We initialise the process by: ct-N+2 = c t-N+2 and C ct-N+2 = C c t-N+2 . Then we update the position and the covariance like described below.

Position update First, we correct the position c l thanks to the odometer measurement between l and l -1 times. c l is replaced by cl thanks to the scale correction provided by

cl = cl-1 + d l-1 dir l-1 . (8)
The camera orientation is not modified.

Then we need to compute the covariance of cameras (c l ) to take into account the above modification.

Covariance update The covariance matrix of the modified camera is determined thanks to the covariance of the previous pose, the odometer covariance and the transformation previously defined (Eq.8). We define J cl-1 ,J c l ,J d l-1 the Jacobian of Eq.8 for cl-1 ,c l ,d l-1 respectively. They are given by:

J cl-1 = I - d l-1 norm l-1 (I -dir l-1 dir l-1 ) (9)
J c l = d l-1 norm l-1 (I -dir l-1 dir l-1 ) (10)
J d l-1 = dir l-1 (11)
We compute the covariance C cl :

C cl = J (c l-1 ,c l ,d l-1 ) C (c l-1 ,c l ,d l-1 ) J (c l-1 ,c l ,d l-1 ) (12)
The camera c l is not independent of other camera in p. Thus we also need to evaluate the correlation between cl with the i-th camera in p. The correlation is computed as follow:

i

∈ [t -N + 1...l -1], C (c l /c i ) = C (c i /c l ) = J (c l-1 ,c l ,d l-1 ) C (c l-1 ,c l ,d l-1 /c i ) (13)
i When the pose (p) and covariance (C p ) of all the (Nn) old cameras are updated, we just apply the W-LBA with local covariance instead of LBA in the visual SLAM (figure 1).

∈ [l + 1...t -n], C (c l /c i ) = C (c i /c l ) = J (c l-1 ,c l ,d l-1 ) C (c l-1 ,c l ,d l-1 /c i ) (14)

Experiments

First we demonstrate that the W-LBA is more accurate than LBA and more particularly when local covariance is used. Then, we show that data fusion with W-LBA is possible and that W-LBA fusion outperforms LBA fusion.

The LBA (Section 3.3) is the method used by Mouragnon et al. for poses optimisation. Two W-LBA have been presented here: The global W-LBA, W-LBA with global covariance (Section 3.4) is our method ( [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF]) used to propagate error but not for geometry optimisation. The local W-LBA (Section 4) is our proposal to integrate local covariance in W-LBA.

The LBA fusion (Section 5.1) is the modification of the bundle initialisation( [START_REF] Eudes | Fast odometry integration in local bundle adjustment-based visual slam[END_REF]). The W-LBA fusion (Section 5.2) is our proposal to fuse odometer with visual SLAM using local W-LBA.

First, trajectories are registered to ground truth, then different errors are computed to compare trajectories (c) to ground truth (GT ). We refer to position error as ||c t -GT t || 2 , angular error as angle(c t-1 -c t , GT t-1 -GT t ) and inter-camera ratio as

||c t-1 -c t || 2 ||GT t-1 -GT t || 2 .
The latter indicates the scale accuracy of the reconstruction. For each error, we give the mean, standard deviation and minimum or maximum values.

Compare W-LBA and LBA

In this Section, we make a comparison between LBA, global W-LBA and local W-LBA.

Experiments are made on a synthetic sequence of a corridor with textured walls generated by 3D Studio Max. This sequence is composed of 2900 frames and is 365m long with a 1 m/s camera speed. Some images of the sequence images are provided in figure 2.

In figure 3, we compare the trajectories given by LBA (in purple) , local (in blue) and global (in black) W-LBA to the ground truth (in red). The table 1 presents the statistical results of the different methods.

In [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF], there is no performance evaluation of the W-LBA bundle, we only give the mathematical formulation and derive the propagation of covariance. We observe that the global W-LBA gives quite similar results than LBA on the reconstruction on figure 3. However, the local W-LBA is more appropriate and gives more accurate results than global W-LBA. It is more accurate in position and has less scale propagation error but we note a minor angular error increase.

Odometry fusion

The odometry fusion has been tested in a real 4 km long sequence taken in urban area by a car in normal driving conditions (50 km/h maximum, in standard traffic). The camera provides 640 × 480 grey-scale images at 30 fps. The ground truth is given by a trajectometer(IXSEA landins) and we use the standard odometer of the car. Fig. 5 shows some images of the sequence. Fig. 4 provides a top view of the four trajectories (ground truth, LBA, LBA fusion,W-LBA fusion).

We observe that the scale of the trajectory is not constant all over the sequence and the scale factor drift is important at the end of the sequence. Thus fusion is a necessity to provide a correct trajectory reconstruction. Indeed, the two fusion methods, contrary to the standard monocular SLAM (LBA), achieved to reconstruct the 4 km sequences with less that 40m error. Statistics (in table 2) show that our method is more compliant with ground truth scale and has a lower error in position with a mean error of only 18 meters (about 50% error reduction compared to LBA fusion).

Conclusion

In this paper, we highlight the use of Weighted Local Bundle Adjustment with local covariance to increase the accuracy of reconstruction and reduce the drift of scale factor. We demonstrate and prove with experiments, that W-LBA used with local covariance is more appropriate for data fusion. The fusion with odometer data allows to reduce the scale factor drift due to lack of constraint in the monocular visual SLAM. Future works include the fusion between visual SLAM and GPS, loop closing using W-LBA and check global uncertainty propagation with fusion as in [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF].

Figure 1 :

 1 Figure 1: Visual SLAM overview and additional modules

Figure 2 :

 2 Figure 2: Three images of the synthetic sequence.

Figure 3 :

 3 Figure 3: Top view of the trajectories reconstructed by LBA , global and local W-LBA and the ground truth of the synthetic sequence.

Figure 4 :

 4 Figure 4: Top view of the reconstruction of the different methods of fusion with odometer (LBA fusion and W-LBA fusion) and the ground truth of the real sequence.

Figure 5 :

 5 Figure 5: Three images of the real sequence.

Table 1 :

 1 Statistics on each reconstruction of figure 3 compared with ground truth

		Inter-camera ratio	Position error(m)	Angular error(deg)
		mean	std	min	mean	std	max	mean	std	max
	LBA	0.793	0.108	0.797	10.558	5.148	21.248	1.104	1.013	6.801
	global W-LBA	0.799	0.099	0.656	11.324	5.777	20.332	1.868	1.276	9.687
	local W-LBA	0.848	0.076	0.848	8.288	3.431	12.394	1.906	1.220	9.074

Table 2 :

 2 Statistics on each reconstruction of figure 4 compared with ground truth

		Intercamera ratio	Position error(m)	Angular error(deg)
		mean	std	max	mean	std	max	mean	std	max
	LBA	0.177	0.256	2.071	348.980	271.22	894.79	15.382	8.243	161.47
	LBA fusion	0.985	0.074	2.117	33.690	23.144	85.471	5.086	5.797	17.650
	W-LBA fusion	1.016	0.151	5.350	18.012	11.286	43.235	3.397	12.018	17.938


Alexandre Eudes
email: alexandre.eudes@cea.fr

Maxime Lhuillier

Sylvie Naudet

Michel Dhome

Fast Odometry Integration in Local Bundle Adjustment-based Visual SLAM

The Simultaneous Localisation And Mapping (SLAM) for a camera moving in a scene is a long term research problem. Here we improve a recent visual SLAM which applies Local Bundle Adjustments (LBA) on selected key-frames of a video: we show how to correct the scale drift observed in long monocular video sequence using an additional odometry sensor. Our method and results are interesting for several reasons: (1) the pose accuracy is improved on real examples (2) we do not sacrifice the consistency between the reconstructed 3D points and image features to fit odometry data (3) the modification of the original visual SLAM method is not difficult.

Introduction

Monocular visual SLAM has been introduced in the last decade by using different techniques. At least two kinds of mechanics are proposed: techniques from data fusion field (using Kalman filter [START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF]) and techniques from Structure-from-Motion (using bundle adjustment [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF]). In theory, the scene structure and camera motion are reconstructed up to an unknown scale factor and this result may be used in applications. Unfortunately, experiments show that the scale factor drift during the camera motion in the scene. This problem makes the use of pure monocular visual SLAM very difficult in practice for long trajectories.

To reduce this problem, Lothe et al. [START_REF] Lothe | Real-Time Vehicle Global Localisation with a Single Camera in Dense Urban Areas[END_REF] use the known distance between the road and the camera or Scaramuzza et al. [START_REF] Scaramuzza | Absolute scale in structure from motion from a single vehicle mounted camera by exploiting nonholonomic constraints[END_REF] use the nonholonomic constraint of the vehicle to evaluate the scale factor. But this evaluation could be done only in specific situation and did not provided a reliable scale estimation at all time.

Like Cumani et al. [START_REF] Cumani | Integrating monocular vision and odometry for SLAM[END_REF], we use the information provided by the vehicle odometer to have a correct scale estimation. But we integrate it in the monocular SLAM proposed by Mouragnon et al. [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF] to take advantage of bundle adjustment optimisation.

LBA-based Monocular SLAM

In this section, the different steps of our monocular visual SLAM is summarized. For more details, please refer to [START_REF] Mouragnon | Real time localization and 3d reconstruction[END_REF]. We assume that the initialization of the algorithm is done (the first three camera poses and their 3D point cloud are estimated).

Tracking 2D The first step of the method is a 2D tracking of interest points which provides 2D/2D correspondences between consecutive images. The tracking step is based on the matching of Harris interest points [START_REF] Harris | A combined corner and edge detector[END_REF] using a GPU accelerated version of SURF descriptor [START_REF] Bay | Surf: Speeded up robust features[END_REF].

Camera pose estimation Thanks to the 2D/2D matching, we determine where the already triangulated 3D points are observed in the new frame and obtain 3D/2D correspondences. Then we estimate the new camera pose (6 d.o.f.) using Grunert's algorithm [START_REF] Haralick | Review and analysis of solutions of the three point perspective pose estimation problem[END_REF] applied in the RANSAC [START_REF] Fischler | Random sample consensus[END_REF] scheme.

Key-frame they make a sub-sampling of the input image sequence. We decide that the new frame is a key-frame thanks to a criterion based on the number of 2D/2D correspondences. In those special images, the scene (3D point cloud) is updated by triangulation and the geometry (last camera poses and their 3D points) is optimized by local bundle adjustment.

Local Bundle Adjustment Bundle adjustment is a well known iterative method [START_REF] Triggs | Bundle adjustment-a modern synthesis[END_REF] designed to solve nonlinear least square problems for Structure-from-Motion. It refines the scene (3D points clouds) and the camera poses all together by minimizing the reprojection errors. Unfortunately, this method becomes rapidly intractable for real-time performance.

For this reason we use a Local Bundle Adjustment (LBA).This is a bundle adjustment such that (1) the estimated parameters are the poses of the n most recent key-frames and the list of 3D points which have at least one detected projection in these n frames (2) the reprojection errors are only considered in the N most recent key-frames. We use n = 3 and N = 10.

In this case, there are only 6n + 3p parameters to estimate (p is the number of 3D points in the n last keyframes) and the geometry of long sequence can be iteratively refined in real-time.

Odometry Integration

In this section, we present our improvement of the visual SLAM previously described.

Odometer Odometer is a distance sensor only. Note that several odometers (one for each vehicle wheel) are theoretically sufficient to estimate a planar trajectory, but in practice, the result exhibits large drift due to error on the vehicle orientation estimation. Fortunately, one odometer on standard driving condition provides us a reliable enough distance to be used in our visual SLAM.

Improved method Here we would like to cancel the problem of scale factor unobservability in the monocular visual SLAM without increasing the computation time.

The idea is to change LBA initialization by replacing the estimated pose of the new key-frame t by a corrected one. After the camera pose estimation of t and before the triangulation of new 3D points, the estimated 3D location C t is replaced by Ct thanks to the scale correction provided by

Ct = C t-1 + Odo(t -1, t) ||C t -C t-1 || 2 (C t -C t-1 ) (1)
using notations t -1 : previous key-frame Odo(t -1, t) : odometer distance between t -1 and t ||.|| 2 : Euclidean norm. The camera orientation is kept unchanged. This is important to note that this modification not only impacts the pose but also the new 3D points. Then this new value of pose t and the new 3D points are optimized by LBA as in the original method.5

Experiments

Experiments include synthetic and real sequences. The trajectories of original and improved methods (respectively without and with odometry) are compared to the ground truth.

Two kinds of figures are provided: top views of reconstructed trajectories and error graphs depending on time. To simplified visual comparison by the reader, some interest points marked by blue mark in top views can be match with black vertical lines in error graphs.

How to Check Trajectory with Ground Truth ?

The estimated trajectories C t are registered to the ground truth GT . The trajectory provided by the original method is defined up to the scale; this scale is set by the ground truth distance between the 3D locations of the two first key-frames. We do not change the scale of the trajectory provided by the modified method.

We refer to position error as ||C t -GT t || 2 (2). The inter-camera ratio is computed as ||Ct-1-Ct||2 ||GTt-1-GTt||2 (3). This is the local value of the estimated scale in the reconstruction. This ratio is useful to visualize the scale error. However it does not inform on the real error since we can make a large error in percentage on a small quantity but the real difference could stay small. For this reason, we add a third information: the difference of norms

||C t-1 -C t || 2 -||GT t-1 -GT t || 2 (4).

Synthetic results

The synthetic sequence is a corridor with textured walls generated by 3D Studio Max. This sequence of 2900 frames is 365m long with a 1m/s camera speed. A virtual odometer is generated from the ground truth to test the method; the camera internal parameters and the ground truth are exactly known; the odometer is noise free. Fig. 3 shows the ground truth, the trajectory of the original method (pure visual SLAM) and the trajectory of the improved method (using odometry). We see that the improved method closes the loop with near no error (the original method does not). Furthermore, the localization error of the improved method is at most 4m against about 20m for the original one (Fig. 2). Fig. 1 shows that our method corrects the existing scale factor drift and that the local error is greatly reduced.

Real results Fig. 4 shows images(640*480) of the sequence taken in standard urban traffic and driving condition (50 km/h) with a calibrated camera, mounted on the car. The ground truth is given by a trajectometer and we use the standard odometer of the car. Fig. 7 provides a top view of the three trajectories (ground truth, original method, improved method) in the same coordinate frame. This 4 km trajectory confirms the scale drift of the original method.

Fig. 6 and7 show that the scale factor is close to 1 at any time and the local error remains low thanks to the improved method. The loop closure error on this 4 km sequences is 30m and the maximum localization error is 80m (Fig. 5).

We see that the improved method corrects the scale drift and improves localization and reconstruction, even with standard odometer.

Conclusion and future work

In this article, we demonstrate the usage of an odometer with the LBA-based monocular visual SLAM. Odometry re-scales the pose of a new keyframe, this improves the list of new 3D points involved by this key-frame and the LBA result. Experiments on both synthetic and real sequences show that our method greatly improves the original method by stabilizing the scale factor and providing more accurate result. Since the final result is the output of successive LBAs only applied on image features, the pose covariance may be estimated in real-time with the method [START_REF] Eudes | Error propagations for local bundle adjustment[END_REF]. The use of odometry covariance for both visual SLAM and pose covariance in the context of LBA-based monocular visual SLAM is still the topic of future work.

Figure 1 .

 1 Figure 1. Intercamera comparison with Ground Truth on synthetic sequence: original (top), improved (bottom).

Figure 2 .

 2 Figure 2. Localization error (Eq.2) on the synthetic sequence.

Figure 3 .

 3 Figure 3. Reconstructions of the synthetic sequence in the same coordinate system.

Figure 4 .

 4 Figure 4. images of the real sequence.

Figure 5 .

 5 Figure 5. Localization Error (Eq.2) on the real sequence.

Figure 6 .

 6 Figure 6. Intercamera comparison with Ground Truth on real sequence: original (top), improved (bottom).

Figure 7 .

 7 Figure 7. Top reconstructions of the real sequence.