Maxime Lhuillier

Incremental Fusion of Structure-from-Motion and GPS using Constrained Bundle Adjustments

Two problems occur when bundle adjustment (BA) is applied on long image sequences: the large calculation time and the drift (or error accumulation). In recent work, the calculation time is reduced by local BAs applied in an incremental scheme. The drift may be reduced by fusion of GPS and Structure-from-Motion. An existing fusion method is BA minimizing a weighted sum of image and GPS errors. This paper introduces two constrained BAs for fusion, which enforce an upper bound for the reprojection error. These BAs are alternatives to the existing fusion BA, which does not guarantee a small reprojection error and requires a weight as input. Then the three fusion BAs are integrated in an incremental Structure-from-Motion method based on local BA. Lastly, we will compare the fusion results on long monocular image sequences and low cost GPS.

Introduction

Bundle adjustment (BA) is an iterative method of estimating camera poses and 3d points detected in an image sequence [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF]. The resulting poses and points minimize a sum of squared reprojection errors.

Recent BA developments mainly concern accelerations for long sequences such as multicore BA [START_REF] Wu | Multicore Bundle Adjustment[END_REF], conjugated gradient [START_REF] Agarwal | Bundle Adjustment in the Large[END_REF], and local BA (LBA) [START_REF] Mouragnon | Generic and real-time structure from motion using local bundle adjustment[END_REF].

Another BA topic is fusion of data coming from several sensors. Fusion is useful for reducing the error accumulation of Structure-from-Motion (SfM), which is unavoidable for long image sequence (especially if the camera is monocular). Global BA is used in aerial Photogrammetry to combine image, inertial and GPS measures: the cost function minimized by BA is a sum of image, inertial and GPS terms weighted by measure covariances [START_REF] Mcglone | The Manual of Photogrammetry[END_REF]. There is also an attempt to include the GPS pseudo-ranges directly as measures in BA [START_REF] Ellum | Integration of raw gps measurements into a bundle adjustment[END_REF]. In a different context, the reprojection errors of 3d points involved in BA are modified such that points are constrained into vertical planes stored in a GIS database [START_REF] Lothe | Towards geographical referencing of monocular slam reconstruction using 3d city models: Application to real-time accurate vision-based localization[END_REF]. Recent work combines GPS and image measures [START_REF] Kume | Extrinsic camera parameter estimation using video images and gps considering gps position accuracy[END_REF] (or inertial and image measures [START_REF] Michot | Bi-objective bundle adjustment with application to multi-sensor slam[END_REF]) using LBA, which minimizes a weighted sum of GPS (or inertial) and image terms. In [START_REF] Michot | Bi-objective bundle adjustment with application to multi-sensor slam[END_REF], several weights are experimented. In [START_REF] Kume | Extrinsic camera parameter estimation using video images and gps considering gps position accuracy[END_REF], the experiments are limited to a small sequence (70 m) and the GPS term is defined by a high order polynomial.

In [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF], new constrained BAs are introduced for SfM-GPS fusion. These BAs enforce an upper bound for the reprojection error, while the other fusion BAs [START_REF] Mcglone | The Manual of Photogrammetry[END_REF][START_REF] Michot | Bi-objective bundle adjustment with application to multi-sensor slam[END_REF][START_REF] Kume | Extrinsic camera parameter estimation using video images and gps considering gps position accuracy[END_REF] do not guarantee a small reprojection error and requires a weight. This paper also compares the results of the fusion BAs in a context which is useful for applications: the incremental SfM based on LBA [START_REF] Mouragnon | Generic and real-time structure from motion using local bundle adjustment[END_REF]. In experiments, low cost GPS and monocular (calibrated) camera are mounted on a car moving in urban area. The trajectory length is larger (4 km) than in the previous works.

The current paper is an improved version of [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF]. The prerequisites detail our assumptions (Section 2.1) and studies our upper bound-based fusion scheme in a simple case (Section 2.2): if the sum of squared reprojection errors is approximated by one quadratic Taylor expansion. Section 3 provides a brief overview of BAs which solve the SfM-GPS fusion problem. Only sparse Levenberg-Marquardt [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] (second order) based methods are considered here. Section 4 introduces our two constrained BAs for fusion, which involve inequality constraint. Section 5 provides the detailed algorithms (useful for re-implementers). Lastly, Section 6 shows experiments in the same context as [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF].

The additional contributions over [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF] includes Section 2.2 and new experiments. Section 2.2 provides interesting properties and helps to convince the reader that SfM-GPS fusion is possible without calculation of SfM covariance. The properties detail the link between the ǫ-indifference region [START_REF] Bard | Non-Linear Parameter Estimation[END_REF] defined by our upper bound, the SfM covariance (that we do not estimate), and the GPS locations where fusion is possible.

The new experiments show the robustness of the fusion methods against several important factors: upper bounds for image error and track lengths, time shift between GPS and video recorders, frequency of GPS perturbations, number of iterations, incomplete GPS data. Lastly, a 5 km long sequence is experimented with GPS providing altitude (the GPS in [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF] does not).

Prerequisites

Section 2.1 introduces notations and assumptions. Section 2.2 details our fusion scheme when the sum of squared reprojection errors is approximated by one quadratic Taylor expansion.

Main notations and assumptions

The Euclidean norm is ||.||. Different fonts are used for vectors (e.g. x), matrices (e.g. H) and function/real (e.g. e). Vector x concatenates the 3d parameters (camera poses and 3d points) and e(x) is the sum of squares of reprojection errors of x.

In this paper, we assume that the starting/input x of fusion BA is the minimizer x * of e, i.e. ∀x, e(x * ) ≤ e(x). Standard BA (not fusion BA) provides x * . Let x 1 be location(s) of the camera. The variable ordering is such that

x T = x T 1 x T
2 . Let P = I 0 be such that x 1 = Px. Let x gps 1 be the location(s) of the camera provided by GPS at the same time(s) as x 1 . Assuming that the GPS drift (or accumulation error) is bounded and that of SfM is not, the ideal output x of fusion BA meet x 1 ≈ x gps 1 . Vector x 2 concatenates all 3d points, all rotations of the camera, and the camera locations without GPS data.

Let e t be a threshold which is slightly greater than the minimum e(x * ) of e. In our context, the final/output x of fusion BA is assumed to be acceptable if its reprojection error is similar to the minimum of e, i.e. e(x) < e t .

(1)

Last, we assume that H > 0, i.e. the hessian of e is positive definite in a neighborhood of x * .

Quadratic Taylor approximation

Let q and H * be the quadratic Taylor expansion and Hessian of e at x * . Since e gradient is zero at x * , e(x * + ∆) ≈ q(∆) = e(x * ) + 0.5∆ T H * ∆.

(

) 2
In the paper we use block-wise notations

x * 1 x 1 ∆ 1 x * 2 x 2 ∆ 2 = x * x ∆ , H 1 H T 21 H 21 H 2 = H * . (3)
Assume that ∆ 1 is a step of x * 1 to remove (or to reduce) the SfM drift, i.e. x * 1 + ∆ 1 = x gps 1 . Now we should find x such that both Eq. 1 and

x 1 = x * 1 + ∆ 1 are meet.
The set of values x defined by Eq. 1 is called ǫ-indifference region ( [START_REF] Bard | Non-Linear Parameter Estimation[END_REF], p.171) where ǫ = e t -e(x * ). Since ǫ is small and H * > 0, Eq. 2 is used to approximate the ǫ-indifference region by ellipsoïd ( [START_REF] Bard | Non-Linear Parameter Estimation[END_REF], p.172)

E ǫ x * = {x * + ∆, q(∆) ≤ e(x * ) + ǫ}. (4)
Now we should find x ∈ E ǫ x * such that x 1 = x * 1 + ∆ 1 . In other words, x gps 1 should be in the ellipsoïd projection

E ǫ x * 1 = {x 1 , ∃x 2 , x T 1 x T 2 T ∈ E ǫ x * } = {x * 1 + ∆ 1 , ∃∆ 2 , q( ∆ 1 ∆ 2 ) ≤ e(x * ) + ǫ}. ( 5
) Lemma 1 is useful to explicit E ǫ x * 1 . Lemma 1: Function ∆ 2 → q( ∆ T 1 ∆ T 2
T ) has minimum q( ∆ 1 -H -1 2 H 21 ∆ 1 ) = e(x * ) + 0.5∆ T 1 C -1 1 ∆ 1 , (6)
where

C 1 = (H 1 -H T 21 H -1 2 H 21 ) -1 is the top-left block of H * -1 .
Proof: Thanks to H * > 0 and Section 6.1 of [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF],

H 1 -H T 21 H -1 2 H 21 is the Schur complement of H 2 in H * and C 1 is the top-left block of H * -1 . Furthermore, H * > 0 implies H 2 > 0. Thus, quadratic function ∆ 2 → q( ∆ T 1 ∆ T 2 T ) has minimizer -H -1 2 H 21 ∆ 1 and minimum e(x * ) + 0.5∆ T 1 I -H -1 2 H 21 T H * I -H -1 2 H 21 ∆ 1 . (7)
Thanks to Lemma 1, we can use ∆ 2 = -H -1 2 H 21 ∆ 1 in Eq. 5 and obtain

E ǫ x * 1 = {x * 1 + ∆ 1 , ∆ T 1 C -1 1 ∆ 1 ≤ 2ǫ}. ( 8
)
= (H 1 -H T 21 H -1 2 H 21 ) -1 .
In this case, e(x) is minimized by choosing

x 2 = x * 2 -H -1 2 H 21 (x gps 1 -x * 1 ). ( 10
) If x gps 1 / ∈ E ǫ x * 1
, Theorem 1 can still be used to fuse SfM and GPS incompletely: we replace x gps . Remind that x * is the minimizer of sum of squared reprojection errors. Under the assumption that the image noise follows the zero-mean normalized Gaussian vector, the x * covariance is approximated by H * -1 [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]. Then we see that C 1 is the covariance matrix of x * 1 and E ǫ

x * 1 is an uncertainty ellipsoïd of x * 1 .

BA candidates for SfM-GPS fusion

Here we review three BAs which fuse SfM and GPS. They meet the requirements of Section 2.1.

UBA: BA without explicit constraint

Such a BA was used to combine measurements from different sensors [START_REF] Mcglone | The Manual of Photogrammetry[END_REF]. We refer to it as UBA or "unconstrained BA". A sum of weighted terms is minimized:

e U (x) = e(x) + β||Px -x gps 1 || 2 . ( 11
)
Here the problems are the adequate choice of weight β and the risk of inlier loss due to the term β||Pxx gps 1 || 2 . The inliers are the detected points involved in e such that the reprojection error is less than a threshold. These problems are similar if we generalize β||.|| 2 by a quadratic form defined by a covariance matrix. In our framework, the UBA output is ignored if e(x) > e t

IBA: BA with inequality constraint

Another method uses penalty function ( [START_REF] Bard | Non-Linear Parameter Estimation[END_REF], p.141). In our context, the iterations of this constrained BA enforce the inequality constraint in Eq. 1, i.e. c I (x) > 0 where c I (x) = e t -e(x). Here we minimize

e I (x) = γ/c I (x) + ||Px -x gps 1 || 2 (12)
where γ > 0. Function x → ||Px-x gps 1 || is minimized while the penalty function γ/c I (x) enforces the inequality constraint. Penalty is the main (positive infinite) term in the neighborhood of c I (x) = 0, and it does not change the minimizers too much of x → ||Pxx gps 1 || 2 elsewhere. Although the principle is simple, such an IBA was not used before for fusion of SfM and another sensor.

EBA: BA derived from equality constraints

BAs in [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] minimize e(x) subject to equality constraint c(x) = 0. At first glance, we could try c(x) = Pxx gps 1 since we would like x 1 ≈ x gps 1 . One iteration improves x by adding step ∆ subject to the linearized constraint c(x + ∆) ≈ c(x) + ∂c ∂x ∆ = 0. Like unconstrained BA, damping is used to define ∆ between the Gauss-Newton step, which minimizes the quadratic Taylor expansion of e, and a gradient descent step. The Taylor expansions require a small enough ∆, which in turn requires a small enough value of ||c(x)|| = || ∂c ∂x ∆||. Now we see that c(x) = Pxx gps 1 can not be used: on the one hand the constrained BAs in [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] require small ||c(x)||, while on the other c(x * ) = x * 1x gps 1 may have large modulus since it is the drift between SfM and GPS. Therefore we introduce EBA, which is derived from a constrained BA in [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF]. EBA is a different method and c is replaced by another function

c α (x) = Px -((1 -α)x gps 1 + αx * 1 ) where α ∈ [0, 1]. (13)
Note that Eq. 13 is the same as Eq. 14 in [START_REF] Lhuillier | Fusion of GPS and Structure-from-Motion using constrained Bundle Adjustments[END_REF], i.e. c α (x) = c(x) -αc(x * ). Eq. 13 makes easier the understanding of c α (x) = 0: x 1 is a linear interpolation of x gps 1 and x * 1 . Eq. 13 implies c 1 (x * ) = 0 and c 0 (x) = c(x). EBA decreases α progressively from 1 (no constraint before all iterations) to 0 (full constraint). The final value of α may be different to 0 and this measures the success of fusion between GPS and image data from α = 1 (failure) to α = 0 (100% success). A decrease of α may produce an increase of e(x), but this increase is moderated since we integrate in EBA the reduction method (a constrained BA in Section 4.4 of [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF]). This is useful to meet Eq. 1.

Note that EBA minimizes α and the (integrated) reduction method minimizes e(x). For the paper clarity, the reduction method and EBA are described in two different Sections 4.3 and 4.4.

Iteration of BAs

Section 4 describes the iterations of Levenberg-Marquardt (LM), IBA and EBA (the former is useful to explain the latters). The supplementary material shows that successful iteration is possible in all cases.

The quadratic Taylor expansion of e at x is

e(x + ∆) ≈ e(x) + g T ∆ + 0.5∆ T H∆ (14)
where g and H are the gradient and hessian of e. The projection function

E : R n → R m meets e(x) = ||E(x)|| 2 .
Let J be the jacobian of E at x. We have g = 2J T E(x) and use the Gauss-Newton approximation H ≈ 2J T J. We assume J T J > 0 since H > 0 (Section 2.1).

Levenberg-Marquardt without constraint

The LM iteration to minimize e(x) without constraint is the following [START_REF] Press | Numerical Recipies in C[END_REF] (UBA minimizes a different function using LM). Efficient sparse methods are used to solve (H + λdiag(H))∆ = -g for the current value of x and a damping coefficient λ > 0. If e(x + ∆) < e(x), the iteration is successful: x is replaced by x + ∆ and λ is replaced by λ/10. Otherwise, λ is replaced by 10λ.

IBA

The method is the same as in Section 4.1, except for the calculation of ∆. Let x i be a coefficient of x and f (x) = γ/(e t -e(x)). We have

∂f ∂x i = γ (e t -e) 2 ∂e ∂x i ∂ 2 f ∂x i ∂x j = γ (e t -e) 3 ((e t -e) ∂ 2 e ∂x i ∂x j + 2 ∂e ∂x i ∂e ∂x j ). ( 15
)
Then, we use the Gauss-Newton approximation H ≈ 2J T J and obtain the gradient and hessian of e I :

g I = γ (e t -e) 2 g + 2P T (Px -x gps 1 )
H I ≈ 2γ (e t -e) 3 ((e t -e)J T J + gg T ) + 2P T P.

(16)

Now, the linear system (H I + λdiag(H I ))∆ = -g I is solved. This can not be solved as in Section 4.1 since H I is not sparse due to the dense term gg T . Section 5.1 provides an efficient method to solve this linear system.

Reduction Method (BA with equality constraint)

Now the LM iteration to minimize e(x) subject to constraint c(x) = 0 is described [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF]. We use notations

x 1 ∆ 1 g 1 x 2 ∆ 2 g 2 = x ∆ g , H 1 H T 21 H 21 H 2 = H (17)
and jacobian C 1 C 2 of c at x. In our case, C 1 = I, C 2 = 0 and step ∆ is such that

c(x + ∆) ≈ c(x) + C 1 ∆ 1 + C 2 ∆ 2 = c(x) + ∆ 1 = 0. (18) Then, ∆ is ∆(∆ 2 ) = -c(x) T ∆ T 2 T . (19)
Thanks to Eq. 14 and ∆ = ∆(∆ 2 ), we obtain

e(x + ∆(∆ 2 )) ≈ ē2 + ∆ T 2 ḡ2 + 0.5∆ T 2 H 2 ∆ 2 (20)
where

ē2 = e(x) -g T 1 c(x) + 0.5c(x) T H 1 c(x) ḡ2 = g 2 -H 21 c(x) (21)
Step ∆ 2 meets (H 2 + λdiag(H 2 ))∆ 2 = -ḡ 2 . Now the iteration is the same as in Section 4.1 using ∆ = ∆(∆ 2 ).

From Reduction Method to EBA

Assume that EBA is the reduction method using the constraint in Eq. 13. A problem is the descending condition e(x + ∆(∆ 2 )) < e(x) to test step ∆ = ∆(∆ 2 ). In our fusion context, the initial value of x is x * , which minimizes e. So the descending condition can not be meet at the very beginning of EBA. However, we remind that our condition for fusion is Eq. 1. We solve this problem, substituting the descending condition by

e(x + ∆(∆ 2 )) < e t . ( 22
)
Now the ∆ 2 calculation in a successful EBA iteration is concisely written as: find positive δ and λ such that

   ḡ2 = g 2 -H 21 c α-δ (x) (H 2 + λdiag(H 2 ))∆ 2 = -ḡ 2 e(x + -c α-δ (x) T ∆ T 2 T ) < e t . (23)
Then we add ∆ = -c α-δ (x) T ∆ T

2

T to x, and subtract δ from α. The detailed algorithm is in Section 5.2. According to the supplementary material, Eqs. 23 have a solution thanks to small enough δ and large enough λ.

Implementation

Now we will explain how to implement efficiently IBA (Section 5.1) and EBA (Section 5.2).

IBA

In Section 4.2, (H I + λdiag(H I ))∆ = -g I should be solved efficiently. Let H and g be such that

H I + λdiag(H I ) = H + gg T , g = 2γ (e t -e) 3 g. (24)
Basic computation shows that

( H + gg T ) -1 = (I - H-1 gg T 1 + gT H-1 g ) H-1 . ( 25
)
We introduce a = -H-1 g I , b = H-1 g, and obtain

∆ = -( H + gg T ) -1 g I = a - gT a 1 + gT b b. (26)
Now we explain how to estimate a and b. According to Eqs. 16 and 24, H has the sparse structure of J T J. More precisely [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF], we have H = U W W T V and H > 0 where U is a 6 × 6 block-wise matrix, V is a 3 × 3 block-wise invertible diagonal matrix, and W is a 6 × 3 block-wise matrix such that the (i, j) block is zero if the j-th 3d point is not seen in the i-th image. So linear systems Ha = -g I and Hb = g are solved using the same efficient sparse method [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF] as the linear system (H + λdiag(H))∆ = -g. The algorithm in C style is the following. The inputs are reprojection error e(x) = ||E(x)|| 2 , GPS location(s) x gps 1 , initial x which minimizes e (i.e. x = x * ), maximum number of iterations It max , and threshold e t . The output is x such that e(x) < e t and e I (x) has the smallest possible value. have observation(s) in these images, e(x) is the sum of squared reprojection errors of these 3d points in the N most recent images. There is no gauge freedom and H > 0. Step 4 uses n = 3 and N -n = 7 [START_REF] Mouragnon | Generic and real-time structure from motion using local bundle adjustment[END_REF].

Our paper adds step (5), a fusion step which is the local version of UBA, IBA or EBA: e is the reprojection error of the LBA which refines the geometry of the k-most recent keyframes. The minimizer x * of e is estimated before each fusion LBA using a single iteration of standard LBA. Vector x 1 is the 3d location of the most recent key-frame. Fusion LBA does not involve point outliers since they are rejected by steps [START_REF] Ellum | Integration of raw gps measurements into a bundle adjustment[END_REF][START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF] as in [START_REF] Mouragnon | Generic and real-time structure from motion using local bundle adjustment[END_REF].

UBA, IBA and EBA run under the same conditions: same keyframes, same matches, same maximum number of iterations (It max = 4), same k and e t . Our default values are k = 40 and e t = 1.05 2 e(x * ), i.e. a RMS increase of 5% is accepted for fusion. The other (default) parameters of UBA and IBA are

β = e(x * ) ||Px * -x gps 1 || 2 , γ = e t -e(x * ) 10 ||Px * -x gps 1 || 2 . ( 28
)
These weights are such that the ratio between image term and GPS term in e U (e I , respectively) is 1 (0.1, respectively) before the fusion optimization.

Step ( 5) is used in the main loop once the SfM result is registered in the GPS coordinate system. The registration method is the following. First we select times t 0 = 0 and t 1 such that the distance between the two GPS positions is greater than 10 meters. Then we define the vertical direction in the SfM result assuming that both x-axis and motion of the camera are horizontal between t 0 and t 1 . Now three points are defined in both coordinate systems (SfM and GPS) and a similarity transformation is estimated from these points. Finally, the SfM result is mapped in the GPS coordinate system using the similarity transformation.

Notations

The 3d location of keyframes are provided by six methods: SfM, GPS, GT (ground truth), UBA, IBA and EBA.

Let a and b be two different methods that we would like to compare. Let l i a and e i a be the 3d location and the reprojection error (RMS) provided by method a at the i-th keyframe. We study the distribution of ∀i, ||l i al i b ||, where a ∈ {SfM, GPS, UBA, IBA, EBA}, b ∈ {GPS, GT}. Its mean, standard deviation and maximum are m b a , σ b a and ∞ b a in meters. We also study the distribution of ∀i, e i a /e i SfM , where a ∈ {UBA, IBA, EBA}. Its mean, standard deviation and maximum are m 2d a , σ 2d a and ∞ 2d a . We refer to these distributions as location errors and image errors, respectively. Here l i a and e i a are estimated after the calculation of the entire sequence by method a.

Experimental conditions for sequence 1

Our GPS and camera are mounted on a car. Its trajectory has straight lines, sharp curves, traffic circles, stop and go due to traffic lights. It is 4 km long. The scene includes low and high buildings, trees and moving vehicles.

The GPS is low cost (Ublox Antaris 4). It provides one 2D location (longitude, latitude) at 1Hz and the altitude is set to 0. Once the GPS coordinates are converted to euclidean coordinates in meters, linear interpolation is used to obtain a 3d GPS location at all times. The ground truth is provided at 10Hz by IXSEA LandINS and RTK (not low cost) GPS. We have m gt gps = 4.28, σ gt gps = 2.34 and ∞ gt gps = 12.2, so the name "low cost GPS" is confirmed. The camera is monocular and calibrated; it points forward and provides 640× 352 images (Fig. 1) at 25 Hz. 2480 keyframes are selected from 14850 images, such that there are about 400 Harris point matches between three consecutive keyframes. We assume that the distance between camera and GPS antenna is small in comparison to GPS accuracy: the GPS coordinates of the camera (x gps 1 ) are approximated by those of the GPS antenna.

Comparison of UBA, IBA and EBA

Here we compare the methods using the default parameters in Section 6.1.

Tab. 1 shows the location errors. The three fusions (UBA, IBA, EBA) greatly reduce the errors relative to GPS to about 2 meters. The errors relative to ground truth are also greatly reduced to about 5 meters, which is the magnitude order of the GPS accuracy. However, the fusion methods are not able to improve the mean of GPS accuracy since the fusion errors are slightly larger than the pure GPS errors. According to values of m GP S a and m GT a , the best results are obtained by IBA, followed by EBA and UBA. Tab. 1 also shows the image errors. We check that they are acceptable for all fusion BAs since they show Local view (right) of GPS (black crosses), GT (black dots), UBA (red dots), IBA (green dots), EBA (blue dots). One dot is one keyframe.

that the increase of RMS reprojection errors per keyframe due to fusion is about 5%. RMS e i Sf M ranges from 0.37 to 0.54 pixel (the mean is 0.44), which implies that e i f ≤ 0.702 pixel ∀f, ∀i. Fig. 2 shows top views of GPS, SfM and IBA trajectories. We see the drift of SfM compared to GPS (top left). At this scale, it is difficult to see a difference between GPS and IBA (bottom left). The same observation can be done for UBA and EBA. Fig. 2 also shows a local view of the 3d locations provided by the fusion BAs (right), in the case where there are high buildings at the road border. The car moves from right to left. We see that the trajectory shapes of the fusion BAs are better than that of the GPS: fusion trajectories are smooth like GT trajectory, GPS trajectory (using linear interpolation) is not smooth at a point on the left. We can also see that the GPS does not provide a good local scale factor to the trajectory.

The mean times of U-/I-/EBA are 0.25, 0.27 and 0.28 seconds for each keyframe, respectively. Here we use a core 2 duo 2.5Ghz laptop, sparse implementation of hessians, and Cholesky factorization of reduced camera system to solve the LM linear systems [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF].

Weight changes for UBA and IBA

Remember that UBA and IBA require choosing weights β and γ, respectively. So we re-do the UBA and IBA fusions of Section 6.4 using different weights around the default values in Eq. 28. The results are given in Tab. 2. We can see that the fusion results are similar if we divide or multiply the weights by 2. We can also see that large changes of weight (division or multiplication by 10) provide bad fusion results. These experiments suggest that the tuning of the weights is important, although it is not difficult to get weight which provides acceptable fusion results. Furthermore, they confirm that IBA has the best 3d location results.

f w. m gps f ∞ gps f m gt f ∞ gt f m 2d f ∞ 2d f UBA β

Changes of sliding window size

We redo the experiments of Section 6.4 for different values of k. Tab. 3 shows that fusion is more difficult for small k. UBA, IBA and EBA provides acceptable results if k ≥ 40: the mean of location errors is less than 6.07 and that of image errors is less than 1.05. Fusion is more difficult if k < 40, especially for UBA and IBA whose locations errors increases dramatically. For EBA, the location errors are acceptable (except ∞ gt eba = 35.1 if k = 25), but the mean of image errors increase up to 1.098. Since small k is better for computation time, we found k = 40 is a good compromise for the 3 fusion BAs.

Such results are suggested by Section 2.2. Remind that the gauge is fixed at the beginning and x 1 is at the end of the k most recent keyframes. In this context, we can assume that covariance C 1 of x * 1 (Section 2.2) increases if the optimized sequence length k increases. Then Theorem 1 implies that E ǫ x * 1 , the region of the GPS locations where fusion is possible, increases if k increases. Thus, small k makes fusion more difficult.

Changes of image upper bound

We redo the experiments of Section 6.4 for different values of e t . Here the notation µ = e t /e(x * ) is more convenient. According to Section 6.1, the default value is µ = 1.05. Tab. 3 shows that fusion is more difficult for small µ (or e t ): the location errors increase if µ decreases. This confirms the following intuition: the smaller e t , the stronger constraint enforced by SfM, the less tolerance for inaccurate GPS. Furthermore, the means of image errors are less than 1.053. This might be surprising since they are cases where the image errors are greater than µ. We should remember that e t is an upper bound for a sum of reprojec- tion errors over the k most recent keyframes; it does not enforce upper bound for individual keyframe. Last, we see that EBA is the most robust to small e t .

m

Time shift between GPS and video recorders

We redo the experiments of Section 6.4 for different values of δt, which is the time shift between GPS and video recorders (in seconds). The previous experiences have δt = 0 and now we try δt = 0 as if the experimenter synchronizes the two recorders manually. Tab. 3 shows that the results are not dramatically corrupted by a bad synchronization: δt ∈ {-.5, 0, .5, 1} provides acceptable results, δt > 2 does not. Such a conclusion depends on the camera speed: the larger speed, the more corrupted GPS location for camera (x gps 1 ) due to δt = 0, the more difficult fusion. Here the speed is less than 60 km/h.

Upper bound for track lengths

Tab. 3 shows that the result of SfM-GPS fusion depends on L ∞ , the upper bound of the track lengths of image points. The default value is L ∞ = 5, e.g. if a point is tracked over 13 consecutive frames, then we split this track into 2 tracks with length 5 and 1 track with length 3. Large L ∞ makes fusion more difficult (except EBA).

Number of iterations

GPS as a periodic perturbation of SfM

We redo the experiments of Section 6.4 with GPS data such that l i gps = l i sf m + 10 cos(2πi/p) sin(2πi/p) 0

T , where p is the period of a circular perturbation around the SfM result and 0 0 1 T is the vertical direction. Tab. 4

shows the fusion results for several p. For small periods (p ≤ 100), the fusion is mainly SfM since m sf m f < m gps f . For large periods (p ≥ 800), the fusion is mainly GPS since m sf m f > m gps f . This suggests that the fusion only uses the low frequencies of i → l i gpsl i sf m .

Incomplete GPS data

The experiments of Section 6.4 are redone with incomplete GPS data, as if GPS satellites are occluded due to high buildings. l i gps is available and fusion is done if and only if (i modulo p) ∈ [0, p/2], where p is a period. Tab. 5 shows the fusion results for several p. EBA has the best results, but fusion is more difficult for large values of p.

Experiment for sequence 2

Sequence 2 has the following differences with sequence 1: 3d locations provided by Flytec GPS at 1 Hz, H.264 compressed video at 30 Hz by Gopro camera, 19515 images reduced to 640 × 480, 4000 keyframes, 5 km long trajectory loop (max speed 77 km/h), the altitude variation is 51 m. The ground truth is unknown. The camera and GPS recorders are manually synchronized. Fig. 3 shows the recorders, three images of the sequence, and a top view of the GPS and IBA trajectories obtained using the default parameters (Section 6.1). Tab. 6 provides the location and image errors. Now EBA has the best mean of location errors and the image errors are larger than those of sequence 1. RMS e i Sf M ranges from 0.35 to 0.51 pixel (the mean is 0.45), which implies that fusions always have e i f ≤ 0.77 pixel.

Conclusion

Two constrained bundle adjustments IBA and EBA were introduced to fuse GPS and Structure-from-Motion data. They enforce an upper bound for the reprojection errors and are described in details. The experiments compare our two BAs with the existing UBA (which minimizes a weighted sum of image and GPS errors) in the difficult context of incremental Structure-from-Motion applied on long urban image sequences and low cost GPS. We also study the fusion results for several parameter settings. Such experiments were not done before.

The three fusion BAs greatly improve the poses of the Structure-from-Motion; the resulting increases of reprojection errors are small. According to ground truth, the resulting pose accuracies are similar to that of the GPS. The GPS accuracy is slightly better (it is the only sensor which provides absolute data, our monocular camera can not). EBA has two advantages: it does not require weight choice and it is the most robust method to bad parameter settings and bad experimental conditions. IBA may provide the best results for good parameter settings. UBA is ranked #3.

Future work includes experiments with other fusion BAs, improvement to initialize the visual reconstruction in the GPS coordinate system, parameter setting from knowledge of the GPS performance, fusion with other sensors, application to georeferenced 3d modeling.

1 is close as possible to x gps 1 .

 11 Lastly, we provide a probabilistic interpretation of C 1 and E ǫ x * 1

Figure 1 :

 1 Figure 1: Images of sequence 1.

Figure 2 :

 2 Figure 2: Top views of trajectories: GPS+SfM (top left), GPS+IBA (bottom left).

Figure 3 :

 3 Figure 3: Video (Gopro) and GPS (Flytec) recorders, 3 images of sequence 2, top view of the GPS and IBA trajectories.

  62 3.09 15.1 1.06 0.06 1.43 IBA 2.32 2.84 14.6 1.09 0.05 1.51 EBA 2.12 1.58 10.4 1.13 0.09 1.48

Table 1 :

 1 Location errors and image errors using the default parameters. If f ∈

	f

{UBA, IBA, EBA}, ∞ 2d f ∈ [1.28, 1.3].

Table 2 :

 2 Location and image errors for weight changes.

	10 β 2 UBA 2β UBA	135 2.66 2.55	409 11.3 5.62 14.0 1.04 1.31 133 409 1.00 1.11 10.8 5.55 14.9 1.04 1.29
	UBA 10β	405	1.3k	405 1.3k 1.02 1.19
	IBA IBA IBA	γ 10 γ 2 2γ	22.4 1.88 1.64	80.7 22.9 80.9 1.06 1.43 9.84 4.88 12.3 1.06 1.32 12.5 4.78 12.1 1.05 1.26
	IBA 10γ	195	690	193 691 1.00 1.10

Table 3 :

 3 Location and image errors for parameter changes.

		gt uba	m gt iba	m gt eba	m 2d uba	m 2d iba	m 2d eba	∞ 2d *
	default	5.59 4.57 5.49 1.037 1.049 1.038 1.301
	k=25	54.0	195	6.62 1.013 1.009 1.098 1.439
	k=30	65.2 43.3 4.99 1.018 1.021 1.070 1.501
	k=35	34.6 33.5 5.15 1.035 1.032 1.051 1.392
	k=45	5.84 4.56 5.74 1.029 1.042 1.032 1.291
	k=50	6.07 4.59 5.86 1.027 1.035 1.028 1.263
	µ=1.01	112	76.3 6.16 1.003 1.004 1.052 1.288
	µ=1.03	94.9 5.01 5.40 1.022 1.049 1.039 1.314
	µ=1.07	5.58 4.55 5.51 1.037 1.053 1.038 1.299
	δt=-2	89.9	229	396 1.025 1.021 1.054 1.394
	δt=-1	8.68 5.49 5.84 1.063 1.081 1.074 1.501
	δt=-.5	5.39 4.87 5.59 1.058 1.061 1.052 1.456
	δt=.5	5.91 4.71 5.79 1.032 1.048 1.035 1.283
	δt=1	6.40 5.39 6.50 1.030 1.054 1.039 1.337
	δt=2	10.3 7.64 7.99 1.060 1.071 1.073 1.444
	δt=3	20.9 16.1 9.56 1.048 1.056 1.100 1.569
	L ∞ =6	5.39 4.57 5.28 1.041 1.059 1.043 1.320
	L ∞ =7	5.37 4.67 5.10 1.045 1.067 1.047 1.343
	L ∞ =8	11.9 9.47 4.99 1.035 1.045 1.048 1.360
	L ∞ =9	54.4 48.5 5.09 1.015 1.020 1.055 1.368
	L ∞ =∞ 54.5 54.4 4.45 1.005 1.007 1.036 1.277
	Itmx=1 85.1	120	4.61 1.015 1.027 1.057 1.312
	Itmx=2 4.81 4.61 4.61 1.041 1.059 1.050 1.366
	Itmx=3 5.49 4.56 4.83 1.038 1.051 1.042 1.306

Table 4 :

 4 Location and image errors if i → l i gpsl i sf m has period p.

Table 5 :

 5 Tab. 3 shows the fusion results for values of It max . UBA and IBA fail if It max = 1. If It max ∈ {2, 3, 4}, we see that (1) the location and image errors are acceptable (2) the greater It max , the smaller image error. Furthermore, the greater It max , the smaller α returned by EBA. If It max = 4, the mean value is ᾱ = 0.0017. If It max = 1, ᾱ = 0.13. Remind that α > 0 means that the EBA fusion is incomplete (Section 3.3). Other examples of incomplete EBA fusion are ᾱ(µ = 1.01) = 0.76 and ᾱ(k = 25) = 0.56. Location and image errors for incomplete GPS data.

	p m gps uba	m gps iba	m gps eba	m 2d uba	m 2d iba	m 2d eba
	20 36.8	31.9	1.80	1.04 1.04 1.06
	30 98.4	33.3	1.96	1.03 1.04 1.07
	40 32.3	7.84	9.87	1.03 1.04 1.12
	50	153	421	175	1.02 1.03 1.08

Table 6 :

 6 Location errors and image errors for sequence 2.

if (0.9999err < err ′ ) break; // convergence is too slow err = err ′ ; U pdateD = 1; λ = λ/10; } else λ = 10λ; }

EBA

Solving the linear system of Eqs. 23 is the main calculation. At first glance, this should be done for each tried (λ, δ) since ḡ2 depends on c α-δ (x). Fortunately, we can reduce the number of these calculations. We solve ∆ a 2 and ∆ b 2 such that

and obtain

. Now we see the improvement: once the linear system in Eq. 27 is solved, ∆ 2 is obtained very efficiently for all tried δ.

We try δ ∈ {α, α/2, • • • α/2 10 } in the decreasing order. If all δ above fail, we change the EBA iteration using ∆ T = 0 T (∆ a 2 ) T . Then we find λ such that e(x + ∆) < e(x) as in unconstrained BA (U-iteration).

Remind that EBA minimizes α, but it is interesting to obtain the smallest e(x) for a given α. Thus, we alternate successful iteration with δ > 0 (Eiteration) and successful U-iteration to decrease e as much as possible. The U-iterations do not update α. If α = 0, only U-iterations are applied until convergence.

The following algorithm in C style provides the remaining details. The inputs are reprojection error e(x) = ||E(x)|| 2 , constraint c, initial x which minimizes e, maximum number of iterations It max , and threshold e t . The output is (x, α) such that e(x) < e t , x 1 = (1 -α)x gps

if (α == 0 && 0.9999err < err ′ ) break; α old = α, err = err ′ ; U pdateD = 1; λ = λ/10; } else λ = 10λ; } 6 Experiments

Integrating fusion to LBA-based SfM

SfM [START_REF] Mouragnon | Generic and real-time structure from motion using local bundle adjustment[END_REF] reconstructs the very beginning of the sequence using standard methods and then alternates the following steps: (1) a new keyframe is selected from the input video and interest points are matched with the previous keyframe using correlation [START_REF] Bard | Non-Linear Parameter Estimation[END_REF] the new pose is estimated using Grunert's method and RANSAC (3) new 3d points are reconstructed from the new matches and (4) LBA refines the geometry of the n-most recent keyframes. In the LBA context, x concatenates the 6D poses of the n-most recent images and the 3d points which