VISUAL-INERTIAL (VI) sensor fusion is an active research

Transcription

1 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED APRIL, On the Comarison of Gauge dom Handling in Otimization-based Visual-Inertial State Estimation Zichao Zhang, Guillermo Gallego, Daide Scaramuzza Abstract It is well known that isual-inertial state estimation is ossible u to a four degrees-of-freedom (DoF) transformation (rotation around graity and translation), and the extra DoFs ( gauge freedom ) hae to be handled roerly. While different aroaches for handling the gauge freedom hae been used in ractice, no reious study has been carried out to systematically analyze their differences. In this aer, we resent the first comaratie analysis of different methods for handling the gauge freedom in otimization-based isual-inertial state estimation. We exerimentally comare three commonly used aroaches: fixing the unobserable states to some gien alues, setting a rior on such states, or letting the states eole freely during otimization. Secifically, we show that (i) the accuracy and comutational time of the three methods are similar, with the free gauge aroach being slightly faster; (ii) the coariance estimation from the free gauge aroach aears dramatically different, but is actually tightly related to the other aroaches. Our findings are alidated both in simulation and on real-world datasets and can be useful for designing otimization-based isual-inertial state estimation algorithms. Index Terms Sensor Fusion, SLAM, Otimization and Otimal Control I. INTRODUCTION VISUAL-INERTIAL (VI) sensor fusion is an actie research field in robotics. Cameras and inertial sensors are comlementary [1], and a combination of both roides reliable and accurate state estimation. While the majority of the research on VI fusion focuses on filter-based methods [2], [3], [4], nonlinear otimization has become increasingly oular within the last few years. Comared with filter-based methods, nonlinear otimization based methods suffer less from the accumulation of linearization errors. Their main drawback, high comutational cost, has been mitigated by the adance of both hardware and theory [5], [6]. Recent work [5], [7], [8], [9] has shown imressie real-time VI state estimation results in challenging enironments using nonlinear otimization. Although these works share the same underlying rincile, i.e., soling the state estimation as a nonlinear least squares otimization roblem, they use different methods to handle Manuscrit receied: February, 24, 2018; reised Aril, 16, 2018; acceted Aril, 16, This aer was recommended for ublication by Editor Francois Chaumette uon ealuation of the Associate Editor and Reiewers comments. This work was suorted by the DARPA FLA rogram, the Swiss National Center of Cometence Research Robotics, through the Swiss National Science Foundation, and by the SNSF-ERC starting grant. The authors are with the Robotics and Percetion Grou, Det. of Informatics, Uniersity of Zurich, and Det. of Neuroinformatics, Uniersity of Zurich and ETH Zurich, Switzerland htt://rg.ifi.uzh.ch. Digital Object Identifier (DOI): see to of this age. y (m) x (m) (a) gauge aroach y (m) Transformed free Fixed x (m) (b) Gauge fixation aroach Fig. 1: Different ose uncertainties of the keyframes on the Machine Hall sequence of the EuRoC MAV Dataset [15] (MAV moing toward the negatie x direction). The left lot shows the uncertainties from the free gauge aroach, where no reference frame is selected. On the right we set the reference frame to be the first frame, and, consequently, the uncertainties grow as the VI system moes. For isualization uroses, the uncertainties hae been enlarged. We can clearly identify the difference in the arameter uncertainties from free gauge and gauge fixation aroaches. Howeer, by using the coariance transformation in Section VI-B, we show that the free gauge coariance can be transformed to satisfy the gauge fixation condition. The transformed uncertainties agree well with the gauge fixation ones. the unobserable DoF in VI systems. It is well known that for a VI system, global osition and yaw are not obserable [3], [10], which in this aer we call gauge freedom following the conention from the field of bundle adjustment [11]. Gien this gauge freedom, a natural way to get a unique solution is to fix the corresonding states (i.e., arameters) in the otimization [12]. Another ossibility is to set a rior on the unobserable states, and the rior essentially acts as a irtual measurement in the otimization [5], [8], [13], [7]. Finally, one may instead allow the otimization algorithm to change the unobserable states freely during the iterations. While these three methods all roe to work in the existing literature, there is no comarison study of their differences in VI state estimation: they are often resented as imlementation details and therefore not well studied and understood. Moreoer, although the similar roblem for ision-only bundle adjustment has already been studied (e.g., [11], [14] with 7 unobserable DoFs in the monocular case), to the best of our knowledge, such a study has not been done for VI systems (which hae 4 unobserable DoFs). In this work, we resent the first comaratie analysis of the different aroaches for handling the gauge freedom in otimization-based isual-inertial state estimation. We comare these aroaches, namely the gauge fixation aroach, the gauge rior aroach and the free gauge aroach on simulated and real-world data in terms of their accuracy, comutational cost and estimated coariance (which is of

2 2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED APRIL, 2018 interest for, e.g., actie SLAM [16]). While all these methods hae similar erformance in terms of estimation error, the free gauge aroach is slightly faster, due to the fewer iterations required for conergence. We also find that, as mentioned by [7], in the free gauge aroach, the resulting coariance from the otimization is not associated to any articular reference frame (as oosed to the one from the gauge fixation aroach), which makes it difficult to interret the uncertainties in a meaningful way. Howeer, in this work we further show that by alying a coariance transformation, the free gauge coariance is actually closely related to other aroaches (see Fig. 1). The rest of the aer is organized as follows. In Section II, we introduce the otimization-based VI state estimation roblem and its non-unique solution. In Section III we resent different aroaches for handling gauge freedom. Then we describe the simulation setu for our comarison study in Section IV. The detailed comarison in terms of accuracy/timing and coariance is resented in Sections V and VI, resectiely. Finally, we show exerimental results on real-world datasets in Section VII. II. PROBLEM FORMULATION AND INDETERMINACIES The roblem of isual-inertial state estimation consists of inferring the motion of a combined camera-inertial (IMU) sensor and the locations of the 3D landmarks seen by the camera as the sensor moes through the scene. By collecting the equations of the isual measurements (image oints) and the inertial measurements (accelerometer and gyroscoe), the roblem can be written as a non-linear least squares (NLLS) otimization one, where the goal is to minimize the objectie function (e.g., assuming Gaussian errors) J(θ) =. r V (θ) 2 Σ }{{ V + r I (θ) 2 Σ } I, (1) }{{} Visual Inertial where r 2 Σ = r Σ 1 r is the squared Mahalanobis norm of the residual ector r, weighted using the coariance matrix Σ of the measurements. The cost (1) can be used in full smoothing [5] or fixed-lag smoothing [7] aroaches. The isual term in (1) consists of the rerojection error between the measured image oints x ij and the redicted ones ˆx ij by a metric reconstruction. Assuming a inhole camera model, ˆx ij (θ) K i (R i R i i)(x j, 1), where (R i, i ) are the extrinsic arameters of the i-th camera (i = 0,..., N 1) and X j are the 3D Euclidean coordinates of the j-th landmark oint (j = 0,..., K 1). We assume that the intrinsic calibrations K i are noise-free. The inertial term in (1) consists of the error between the inertial measurements and the redicted ones by a model of the trajectory of the IMU. For examle, [17] considers the error in the raw acceleration and angular elocity measurements, whereas [5] considers errors in equialent, lower rate measurements (inertial reintegration terms at the rate of the isual data). In this work, we consider the latter formulation, although most of the results do not deend on the choice of formulation. The arameters of the roblem (also known as state), θ. = { i, R i, i, X j }, (2) comrise the camera motion arameters 1 (extrinsics and linear elocity) and the 3D scene (landmarks). The accelerometer and gyroscoe biases are usually exressed in the IMU frame and thus not affected by a fixation of the coordinate frame. Therefore, we exclude the biases from the state and assume that the IMU measurements are already corrected. A full descrition of the inertial and isual measurement models is out of the scoe of this work, and we refer the reader to [5] for details. A. Solution Ambiguities and Geometrical Equialence When addressing the VI state estimation roblem, it is essential to note that the objectie function (1) is inariant to certain transformations of the arameters θ = g(θ), i.e., J(θ) = J(g(θ)). (3) Secifically, g, defined by homogeneous matrices of the form g =. ( ) Rz t, (4) 0 1 is a 4-DoF transformation consisting of an arbitrary translation t R 3 and a rotation R z = Ex(αe z ) by an arbitrary angle (yaw) α ( π, π) around the graity axis e z = (0, 0, 1).. For notation simlicity, we define the maing Ex(θ) = ex(θ ), where ex is the exonential ma of the Secial Orthogonal grou SO(3), and θ is the skew-symmetric matrix associated with the cross-roduct, i.e., a b = a b, b. This is the well-known Rodrigues formula. Alying a transformation (4) to the reconstruction (2) gies another reconstruction g(θ) = θ { i, R i, i, X j }, i = R z i + t i = R z i R i = R zr i X j = R zx j + t Both arameters θ and θ reresent the same underlying scene geometry (camera trajectory and 3D oints), i.e., they are geometrically equialent. They generate the same redicted measurements; and, therefore, the same error (1). As a consequence of the inariance (3), the arameter sace M can be artitioned into disjoint sets of geometrically equialent reconstructions. Each of these sets is called an orbit [11] or a leaf [14]. Formally, the orbit associated to θ is the 4D manifold (5) M θ. = {g(θ) g G}, (6) where G is the grou of transformations of the form (4). Note that the objectie function (1) is constant on each orbit. The main consequence of the inariance (3) is that (1) does not hae a unique minimizer because there are infinitely many reconstructions that achiee the same minimum error: all the reconstructions on the orbit (6) of minimal cost (see Fig. 2), differing only by 4-DoF transformations (4). Hence, the VI estimation roblem has some indeterminacies or unobserable states: there are not enough equations to comletely secify a unique solution. 1 For simlicity, we assume that the coordinate frames of the camera and the IMU coincide, e.g., by comensating the camera-imu calibration [18].

3 ZHANG et al.: ON THE COMPARISON OF GAUGE FREEDOM HANDLING IN OPTIMIZATION-BASED VISUAL-INERTIAL STATE ESTIMATION 3 TABLE I: Three gauge handling aroaches considered. (n = 9N + 3K is the number of arameters in (2)) Size of arameter ec. Hessian (Normal eqs) Fixed gauge n 4 inerse, (n 4) (n 4) Gauge rior n inerse, n n gauge n seudoinerse, n n B. Additional Constraints: Secifying a Gauge The rocess of comleting (1) with additional constraints c(θ) = 0 (7) that yield a unique solution is called secifying a gauge C [14], [11]. In other words, equations (7) select a reresentatie of the orbit (6), i.e., to remoe the indeterminacy within the equialence class. In VI, this is achieed by secifying a reference coordinate frame for the 3D reconstruction. For examle, the standard gauge in camera-motion estimation consists of selecting the reconstruction that has the reference coordinate frame located at the first (i = 0) camera osition and with zero yaw. These constraints secify a unique transformation (4), and therefore, a unique solution θ C = C M θ among all equialent ones. By construction, gauges C are transersal to orbits M θ, so that θ C [14]. III. OPTIMIZATION AND GAUGE HANDLING From an otimization oint of iew, the minimization of the NLLS function (1) using the Gauss-Newton algorithm resents some difficulties. Een if we use a minimal arametrization for all elements of the state (arameter ector) θ, the Hessian matrix of (1), which dries the arameter udates, is singular due to the unobserable DoFs. More secifically, it has a rank deficiency of four, corresonding to the 4-DoFs in (4). There are seeral ways to mitigate this issue, as summarized in Table I. One of them is to otimize in a smaller arameter sace where there are no unobserable states, and therefore the Hessian is inertible. This essentially enforces hard constraints on the solution (gauge fixation aroach). Another one is to augment the objectie function with an additional enalty (which yields an inertible Hessian) to faor that the solution satisfies certain constraints, in a soft manner (gauge rior aroach). Lastly, one can use the seudoinerse of the singular Hessian to imlicitly roide additional constraints (arameter udates with smallest norm) for a unique solution (free gauge aroach). The first two strategies require VI roblem-secific knowledge (which state to constrain), whereas the last one is generic. A. Rotation Parametrization for Gauge Fixation or Prior One roblem with the gauge fixation and gauge rior aroaches is that fixing the 1-DoF yaw rotation angle of a camera ose is not straightforward, as we discuss next. The standard method to udate orientation ariables (i.e., rotations) during the iterations of the NLLS soler (Gauss- Newton or Leenberg-Marquardt LM) of (1) is to use local coordinates, where, at the q-th iteration, the udate is R q+1 = Ex(δ q )R q. (8) Gauge C Start gauge Gauge rior Gauge fixation Orbit of minimum cost M θ Fig. 2: Illustration of the otimization aths taken by different gauge handling aroaches. The gauge fixation aroach always moes on the gauge C, thus satisfying the gauge constraints. The free gauge aroach uses the seudoinerse to select arameter stes of minimal size for a gien cost decrease, and therefore, moes erendicular to the isocontours of the cost (1). The gauge rior aroach follows a ath in between the gauge fixation and free gauge aroaches. It minimizes a cost augmented by (11), so it may not exactly end u on the orbit of minimum isual-inertial cost (1). Setting the z comonent of δ q to 0 allows fixating the yaw with resect to R q. Howeer, concatenating seeral such udates (Q iterations), R Q = Q 1 q=0 Ex(δq )R 0, does not fixate the yaw with resect to the initial rotation R 0, and therefore, this arametrization cannot be used to fix the yawalue of R Q to that of the initial alue R 0. Although yaw fixation or rior can be alied to any camera ose, it is a common ractice to use the first camera. Thus, for the rotations of the other camera oses, we use the standard iteratie udate (8), and, for the first camera, R 0, we use a more conenient arametrization. Instead of directly using R 0, we use a left-multilicatie increment: R 0 = Ex( 0 )R 0 0, (9) where the rotation ector 0 is initialized to zero and udated. Indeed, the rotation ector formulation has a singularity at 0 = π, but it is alicable when the initial rotation is close to the otimal alue ( 0 < π), which is often the case in real systems (e.g., initial alues are roided by a front-end, such as [5]). B. Different Aroaches for Handling Gauge dom Based on the reious discussion, gauge fixation consists of fixing the osition and yaw angle of the first camera ose throughout the otimization. This is achieed by setting 0 = 0 0, 0z. = e z 0 = 0, (10) where 0 0 is the initial osition of the first camera. Fixing these alues of the arameter ector is equialent to setting the corresonding columns of the Jacobian of the residual ector in (1) to zero, namely J 0 = 0, J 0z = 0. The gauge rior aroach adds to (1) a enalty r P 0 2 Σ, where r P P 0 (θ) =. ( 0 0 0, 0 0z ). (11) The choice of Σ P 0 in (11) will be discussed in Section V.

4 4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED APRIL, 2018 Finally, the free gauge aroach lets the arameter ector eole freely during the otimization. To deal with the singular Hessian, we may use the seudoinerse or add some daming (Leenberg-Marquardt algorithm) so that the NLLS roblem has a well-defined arameter udate. A comarison of the aths followed in arameter sace during the otimization iterations of the three aroaches is illustrated in Fig. 2. Next, we show an exerimental comarison of the three gauge handling aroaches. IV. COMPARISON STUDY: SIMULATION SETUP A. Data Generation We use three 6-DoF trajectories for our exeriments, namely a sine-like shae one, an arc-like one and a rectangular one. We denote them as sine, arc and rec resectiely. We consider two landmark configurations: lane, where the 3D oints are roughly distributed on seeral lanes and random, where the 3D oints are generated randomly along the trajectory. Fig. 3 shows some simulation setu examles. To generate the inertial measurements, we fit the trajectories using B-slines and then samle the accelerations and angular elocities. The samled alues are corruted with biases and additie Gaussian noise, and then are used as inertial measurements. For the isual measurements, we roject the 3D oints through a inhole camera model to get the corresonding image coordinates and then corrut them with additie Gaussian noise. B. Otimization Soler To sole the VI state estimation roblem (1), we use the LM algorithm in the Ceres soler [19]. We imlement the different aroaches for handling the gauge freedom described in Section III. For each trajectory, we samle seeral keyframes along the trajectory. Our arameter sace contains the states (i.e., osition, rotation and elocity) at these keyframes and the ositions of the 3D oints. The initial states are disturbed randomly from the groundtruth. C. Ealuation 1) Accuracy: To ealuate the accuracy of an estimated state, we first calculate a transformation to align the estimation and the groundtruth. The transformation is calculated from the first oses of both trajectories. Note that the transformation has four DoFs, i.e., a translation and a rotation around the graity ector. After alignment, we calculate the root mean squared error (RMSE) of all the keyframes. Secifically, we use the Euclidean distance for osition and elocity errors. For rotation estimation, we first calculate the relatie rotation (in angle-axis reresentation) between the aligned rotation and the groundtruth, and then use the angle of the relatie rotation as the rotation error. 2) Comutational Efficiency: To ealuate the comutational cost, we record the conergence time and number of iterations of the soler. We run each configuration (i.e., the combination of trajectory and oints) for 50 trials and calculate the aerage time and accuracy metrics. Fig. 3: Samle simulation scenarios. The left one shows a sine trajectory with randomly generated 3D oints, and the right one shows an arc trajectory with the 3D oints distributed on two lanes. 3) Coariance: We also comare the coariances roduced by the otimization algorithm, which are of interest for alications such as actie SLAM [20]. The coariance matrix of the estimated arameters is gien by the inerse of the Hessian. For the free gauge aroach, the Moore-Penrose seudoinerse is used, since the Hessian is singular [11]. V. COMPARISON STUDY: TIMING AND ACCURACY A. Gauge Prior: Choosing the Aroriate Prior Weight Before comaring the three aroaches from Section III, we need to choose the rior coariance Σ P 0 in the gauge rior aroach. A common choice is Σ P 0 = σ0 2 I, for which the rior (11) becomes r P 0 2 = w P r P Σ P 0 2, with w P = 1/σ We tested a wide range of the rior weight w P on different configurations and the results were similar. Therefore, we will look at one configuration in detail. Note that w P = 0 is essentially the free gauge aroach, whereas w P is the gauge fixation aroach. 1) Accuracy: Fig. 4 shows how the RMSE changes with the rior weight. It can be seen that the estimation errors of different rior weights are ery similar (note the numbers on the ertical axis). While there is no clear otimal rior weight for different configurations of trajectories and 3D oints, the RMSE stabilizes at one alue after the weight increases aboe a certain threshold (e.g., 500 in Fig. 4). 2) Comutational Cost: Fig. 5 illustrates the comutational cost for different rior weights. Similarly to Fig. 4, the number of iterations and the conergence time stabilize when the rior weight is aboe a certain alue. Interestingly, there is a eak in the comutational time when the rior weight increases from zero to the threshold where it stabilizes. The same behaior is obsered for all configurations. To inestigate this behaior in detail, we lot in Fig. 6 the rior error with resect to the aerage rerojection error at each iteration for seeral rior weight alues. The osition rior error is the Euclidean distance between the current estimate of the first osition and its initial alue, the yaw rior error is the z- comonent of the relatie rotation of the current estimate of the first rotation with resect to its initial alue, and the aerage rerojection error is the total isual residual aeraged by the number of obsered 3D oints in all keyframes. For ery large rior weights (10 8 in the lot), the algorithm decreases the rerojection error while keeing the rior error almost equal to

5 ZHANG et al.: ON THE COMPARISON OF GAUGE FREEDOM HANDLING IN OPTIMIZATION-BASED VISUAL-INERTIAL STATE ESTIMATION 5 Position (m) Rotation (rad) Velocity (m/s) e e e e e e e e e e e e e e e e e e e e e+06 Prior Weight Fig. 4: RMSE in osition, orientation and elocity for different rior weights Conergence time (sec) Number of iterations (#) e e e e e e e e e e e e e e e e e e e e+03 Prior Weight Fig. 5: Number of iterations and comuting time for different rior weights. zero. In contrast, for smaller rior weights (e.g., ), the otimization algorithm reduces the rerojection error during the first two iterations at the exense of increasing the rior error. Then the otimization algorithm sends many iterations fine-tuning the rior error while keeing the rerojection error small (moing along the orbit), hence the comutational time increases. 3) Discussion: While the accuracy of the solution does not significantly change for different rior weights (Fig. 4), a roer choice of the rior weight is required in the gauge rior aroach to kee the comutational cost small (Fig. 5). Extremely large weights are discarded since they sometimes make the otimization unstable. We obsere similar behaior for different configurations (trajectory and oints combination). Therefore, in the rest of the section we use a roer rior weight (e.g., 10 5 ) for the gauge rior aroach. B. Accuracy and Comutational Effort We comare the erformance of the three aroaches on the six combinations of simulated trajectories (sine, arc and rec) and 3D oints (lane and random). We otimize the objectie function for differently erturbed initializations and obsere that the results are similar. For the results resented in this section, we erturb the groundtruth ositions by a random ector of 5 cm (with resect to a trajectory of 5 m), the orientations by a random rotation of 6 degrees, the elocities by a uniformly distributed ariable in [ 0.05, 0.05] m/s (with resect to a mean elocity of 2 m/s) and the 3D oint ositions by a uniform random ariable in [ 7.5, 7.5] cm. Yaw Prior Error (rad) e e e e end start Aerage Rerojection Error (x) Position Prior Error (m) e e e e end start Aerage Rerojection Error (x) Fig. 6: Prior error s. aerage rerojection error for some reresentatie rior weights. Each dot in the lot stands for an iteration with the corresonding rior weight. The otimization starts from the bottom-right corner, where the rerojection errors are the same and the rior errors are zero. As the otimization roceeds, the rerojection error decreases and there are different behaiors for different rior weights regarding the rior error. Note that the free gauge case behaes as the zero rior weight. The aerage RMSEs of 50 trials are listed in Table II. We omit the results for the gauge rior aroach because they are identical to the ones from the gauge fixation aroach u to around 8 digits after the decimal. It can be seen that there are only small differences between the free gauge aroach and the gauge fixation aroach, and neither of them has a better accuracy in all simulated configurations. The conergence time and number of iterations are lotted in Fig. 7. The comutational cost of the gauge rior aroach and the gauge fixation aroach are almost identical. The free gauge aroach is slightly faster than the other two. Secifically, excet for the sine trajectory with random 3D oints, the free gauge aroach takes fewer iterations and less time to conerge. Note that the gauge fixation aroach takes the least time er iteration due to the smaller number of ariables in the otimization (see Table I). C. Discussion Based on the results in this section, we conclude that: The three aroaches hae almost the same accuracy. In the gauge rior aroach, one needs to select the roer rior weight to aoid increasing the comutational cost. With a roer weight, the gauge rior aroach has almost the same erformance (accuracy and comutational cost) as the gauge fixation aroach. The free gauge aroach is slightly faster than the others, because it takes fewer iterations to conerge (cf. [14]). While it may be ossible to fix the unobserable DoFs (recall that we use a tailored arametrization (9) to fix the yaw DoF), the free gauge aroach has the additional adantage that is generic, i.e., not secific of VI, and therefore it does not require any secial treatment on rotation arametrization. VI. COMPARISON STUDY: COVARIANCE A. Coariance Comarison Gien a high rior weight, as discussed in the reious section, the coariance matrix from the gauge rior aroach

6 6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED APRIL, 2018 TABLE II: RMSE on different trajectories and 3D oints configurations. The smallest errors (e.g., gauge fixation s. free gauge) are highlighted. Time (sec) Iterations (#) Time er iter. (ratio) Configuration Gauge fixation gauge sine lane arc lane rec lane sine random arc random rec random Position, rotation and elocity RMSE are measured in m, deg and m/s, resectiely Fix Prior random sine random arc random rec lane sine lane arc lane rec random sine random arc random rec lane sine lane arc lane rec 1.00 random sine random arc random rec lane sine lane arc lane rec Fig. 7: Number of iterations, total conergence time and time er iteration for all configurations. The time er iteration is the ratio with resect to the gauge fixation aroach (in blue), which takes least time er iteration. is similar to the gauge fixation aroach and therefore omitted here. We only comare the coariances of the free gauge aroach and the gauge fixation aroach in this section. An examle of the coariance matrices of the free gauge and gauge fixation aroaches is isualized in Fig. 9. If we look at the to-left block of the coariance matrix, which corresonds to the osition comonents of the states: (i) for the gauge fixation aroach (Fig. 9c), the uncertainty of the first osition is zero due to the fixation, and the osition uncertainty increases afterwards (cf. Fig. 1b); (ii) in contrast, the uncertainty in the free gauge case (Fig..9a) is distributed oer all the ositions (cf. Fig. 1a). This is due to the fact that the free gauge aroach is not fixed to any reference frame. Therefore, the uncertainties directly read from the free gauge coariance matrix are not interretable in a geometricallymeaningful way. Howeer, this does not mean the coariance estimation from the free gauge aroach is useless: it can be transformed to a geometrically-meaningful form by enforcing a gauge fixation condition, as we show next. B. Coariance Transformation Coariances are aerages of squared erturbations of the estimated arameter. A erturbation θ of a reconstruction θ can be decomosed into two comonents: one arallel to the orbit M θ (6) and one arallel to the gauge C (7). The comonent of θ arallel to the orbit M θ is not geometrically meaningful since the erturbed reconstruction is also in the orbit (thus, arbitrarily large erturbations roduce no change θ θ T θ (M θ ) θ C θ g Q C θ C θ C T θc (C) θ C = Q C θ C θ C θ θ C T θc (M θ ) M θ Fig. 8: Illustration of the coariance transformation in the arameter sace. M θ is the subsace that contains all the arameters that are equialent to free gauge estimation θ (i.e., different by a 4-DoF transformation). C is that subsace that contains all the arameters that satisfy the gauge fixation condition (10). We first transform θ to the gauge fixation estimation θ C along M θ, together with the erturbation θ ( θ C / θ) θ. Then we roject the erturbation onto the tangent sace to the gauge T θc (C), arallel to the M θ, using the rojector Q C θ C. The aerage of the outer roduct of these transformed erturbations is the coariance Co(θ C ). of the scene geometry). Therefore, only erturbations along the gauge C, θ C, reresent changes of the reconstructed geometry and are therefore meaningful. Such erturbations lie on the tangent sace T θc (C). Hence, geometricallymeaningful erturbations are gauge-deendent [14], [11]. The coariance from the free gauge aroach Co (θ) at an estimate θ can be transformed into the coariance of a gien gauge fixation C (10) by the following formula [14]: Co(θ C ) ( ) ( ) Q C θ C θ C Co(θ) Q C θ C θ θ C, (12) θ where θ C = C M θ = g(θ) is the equialent arameter that satisfies the gauge. Secifically, g {R z, t} (4) is obtained by ushing θ along M θ (Fig. 8) until it meets C, satisfying C 0 = R z 0 + t, 0 = e z Log(R z Ex( 0 )), (13) where { 0, 0 } θ and C 0 θ C. Recall that the rotation of the first camera ose is arameterized differently (9), and therefore should be transformed as C 0 = Log(R z Ex( 0 )), where Log is the inerse oerator of Ex, defined in Section II-A. The transformation rule (12) consists of two oerations (also illustrated in Fig. 8): (i) transferring erturbations along the orbit M θ (oerator θ C / θ), and (ii) rojecting the erturbations on the tangent sace to the gauge T θc (C) (oerator Q C θ C ). These oerators are secified in Aendix A. In Fig. 9, we show an examle of coariance transformation on simulated data. Because VI systems are mostly used for motion estimation, we only show the coariance of the motion arameters. To better areciate the entries of the coariance in site of their magnitude difference, we use a logarithmic scale for isualization. Secifically, we lot log 10 ( σ ij + ε), where Co Σ = (σ ij ) is the coariance matrix, and ε = 10 7 defines the alue corresonding to the white color. We transform the free gauge coariance to the reference frame secified by the gauge fixation constraint (10). It can be seen that the transformed coariance agrees well with the coariance from the the gauge fixation, with a ery small relatie error in Frobenius norm (0.11 %).

7 ZHANG et al.: ON THE COMPARISON OF GAUGE FREEDOM HANDLING IN OPTIMIZATION-BASED VISUAL-INERTIAL STATE ESTIMATION (a) gauge coariance (b) Transformed free gauge coariance (c) Gauge fixation coariance Fig. 9: Coariance of free gauge (Fig. 9a) and gauge fixation (Fig. 9c) aroaches using N = 10 keyframes. In the middle (Fig. 9b), the free gauge coariance transformed using (12) shows ery good agreement with the gauge fixation coariance: the relatie difference between them is Σ b Σ c F / Σ c F 0.11% ( F denotes Frobenius norm). For better isualization, the magnitude of the coariance entries is dislayed in logarithmic scale. The yellow bands of the gauge fixation and transformed coariances indicate zero entries due to the fixed 4-DoFs (the osition and the yaw angle of the first camera) (a) gauge coariance (b) Transformed free gauge coariance (c) Gauge fixation coariance Fig. 10: Coariance comarison and transformation using N = 30 keyframes of the EuRoC Vicon 1 sequence (VI1). Same color scheme as in Fig. 9. The relatie difference between (b) and (c) is Σ b Σ c F / Σ c F 0.02%. Obsere that, in the gauge fixation coariance, the uncertainty of the first osition and yaw is zero, and it grows for the rest of the camera oses (darker color), as illustrated in Fig. 1b. C. Discussion In this section, we hae seen that the arameter coariance from the free gauge aroach is different from the other aroaches and cannot be directly interreted in a meaningful way. Howeer, we can actually transform the free gauge coariance into the gauge fixation one by a linear transformation (12). The coariance transformation method in Section VI-B, which is a secial case of the general theory in [14], not only roides insights into the differences and connections of the comared methods, but it can also be useful for coariance calculation if the otimization method is used as a black box (i.e., cannot directly calculate the coariance inerse of the Hessian matrix from the Jacobians of the measurement model). VII. EXPERIMENTS ON REAL-WORLD DATASETS We erformed the same exerimental comarison as in the simulation on two sequences from the EuRoC MAV Dataset [15]: Machine Hall 1 (MH1) and Vicon Room 1 (VI1). We used a semi-direct isual odometry algorithm (SVO [21]) to roide the initialization of the arameters in the otimization roblem (1). We used the stereo setu of SVO to remoe scale ambiguity. As for the biases, we used the groundtruth alues in the dataset. The ealuation method described in Section IV was used. Note that we did not run the otimization oer the full trajectories but on shorter segments, which is enough to demonstrate the differences of the three methods. The comutational cost of the three different aroaches is lotted in Fig. 11. The results are consistent with our simulation exeriments: the free gauge aroach, which requires fewer iterations to conerge, is faster than the other two, The accuracies are reorted in Table III, and all three methods hae similar estimation error. In Fig. 10, we obsere, as in Fig. 9, the aarent difference between the coariances and further show that, by alying (12), we can calculate the coariance in a certain reference frame using the free gauge coariance, and the result agrees well with the coariance from actually fixating the gauge (cf. Fig. 10b and Fig. 10c). VIII. CONCLUSION In this work, we resented the first comarison study of different aroaches, namely the gauge fixation aroach, the gauge rior aroach and the free gauge aroach, for handling the gauge freedom in otimization-based isual-inertial state estimation. We showed in simulation as well as on realworld datasets that all these methods hae similar accuracy and efficiency, with the free gauge aroach being slightly faster due to fewer iterations in the otimization. Howeer, one major

8 8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED APRIL, 2018 Time (sec) EuRoC MH EuRoC VI Iterations (#) EuRoC MH EuRoC VI Time er iter. (ratio) Fix Prior EuRoC MH EuRoC VI Fig. 11: Comutational cost of the three different methods for handling gauge freedom on two sequences from the EuRoC dataset. The time er iteration is the ratio with resect to the gauge fixation aroach. TABLE III: RMSE on EuRoC datasets. Same notation as in Table II. Sequence Gauge fixation gauge EuRoC MH EuRoC VI difference we identified is the estimated coariance from the otimization algorithms are different, esecially for the free gauge aroach. To better understand the connection between the different aroaches, we showed how to transform the free gauge coariance to satisfy the gauge fixation condition, which indicates the coariances from different aroaches are actually closely related. APPENDIX A OPERATORS FOR COVARIANCE TRANSFORMATION The Jacobian θ C θ in (12) is comuted from g according to the relations (5) and the chosen arametrization of θ, θ C. It is a block-diagonal, full-rank square matrix of size 9N + 3K. Differentiating on (5), we obtain the matrices in the diagonal, C i / i = i C/ i = X C j / X j = R z. Differentiating the rotation arameters, we hae, for the first camera ose (arametrization (9)), C 0 / 0 = J 1 r ( C 0 ) J r ( 0 ), where J r is the right Jacobian of SO(3) [22,. 40], and for the remaining oses (arametrization (8)), δ C i / δ i = R z. The oblique rojector Q C θ C in (12) is gien by Q C θ C. = I UθC (V θ C U θc ) 1 V θ C, (14) where I is the identity matrix, U θc is a basis for the tangent sace to the orbit at θ C, T θc (M θ ), and V θc is a basis for the orthogonal comlement of the tangent sace to the gauge C at θ C, (T θc (C)) (Fig. 8). Both U θc and V θc are (9N +3K) 4 matrices and their secific form deend on the choice of arametrization and gauge constraints. Matrix U θc can be obtained by alying to the arameter θ C an infinitesimal transformation (4), δg =. { R z, t}. The resulting arameter can be written as δg(θ C ) θ C +D(θ C ), where the generators of the infinitesimal gauge [14] D(θ C ) =. U θc ( α, t ) are linearly-related with ( α, t ), the local coordinates describing δg. The rows of U θc are U C i = [ e z C i, I] U C i = [ e z i C, 0] U C 0 = [J 1 l ( C 0 )e z, 0] U δ C i = [e z, 0], i 0 U X C j = [ e z X C j, I], (15) where J l is the left Jacobian of SO(3) [22,. 40]. Matrix V θc is gien by the deriatie of the constraints (7), V. θ C = c θ (θ C). In case of the gauge fixation (10), only two deriaties are non-anishing: ( 0 0 0)/ 0 = I and (e z 0 )/ 0 = e z. REFERENCES [1] P. Corke, J. Lobo, and J. Dias, An introduction to inertial and isual sensing, Int. J. Robot. Research, ol. 26, no. 6, , [2] A. I. Mourikis and S. I. Roumeliotis, A multi-state constraint Kalman filter for ision-aided inertial naigation, in IEEE Int. Conf. Robot. Autom. (ICRA), Ar. 2007, [3] E. Jones and S. Soatto, Visual-inertial naigation, maing and localization: A scalable real-time causal aroach, Int. J. Robot. Research, ol. 30, no. 4, Ar [4] M. Li and A. I. Mourikis, High-recision, consistent EKF-based isualinertial odometry, Int. J. Robot. Research, ol. 32, no. 6, , [5] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, On-manifold reintegration for real-time isual-inertial odometry, IEEE Trans. Robot., ol. PP, no. 99,. 1 21, Feb , 2, 3 [6] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, isam2: Incremental smoothing and maing using the Bayes tree, Int. J. Robot. Research, ol. 31, , Feb [7] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, Keyframe-based isual-inertial SLAM using nonlinear otimization, Int. J. Robot. Research, , 2 [8] Z. Yang and S. Shen, Monocular isual-inertial state estimation with online initialization and camera-imu extrinsic calibration, IEEE Trans. Autom. Sci. Eng., ol. 14, no. 1, , Jan [9] H. Rebecq, T. Horstschaefer, and D. Scaramuzza, Real-time isualinertial odometry for eent cameras using keyframe-based nonlinear otimization, in British Machine Vis. Conf. (BMVC), Set [10] J. Kelly and G. S. Sukhatme, Visual-inertial sensor fusion: Localization, maing and sensor-to-sensor self-calibration, Int. J. Robot. Research, ol. 30, no. 1, , [11] B. Triggs, P. McLauchlan, R. Hartley, and A. Fitzgibbon, Bundle adjustment a modern synthesis, in Vision Algorithms: Theory and Practice, ser. LNCS, W. Triggs, A. Zisserman, and R. Szeliski, Eds., ol Sringer Verlag, 2000, , 2, 3, 4, 6 [12] S. Leutenegger, P. Furgale, V. Rabaud, M. Chli, K. Konolige, and R. Siegwart, Keyframe-based isual-inertial SLAM using nonlinear otimization, in Robotics: Science and Systems (RSS), [13] S. Shen, N. Michael, and V. Kumar, Tightly-couled monocular isualinertial fusion for autonomous flight of rotorcraft MAVs, in IEEE Int. Conf. Robot. Autom. (ICRA), May 2015, [14] K. Kanatani and D. D. Morris, Gauges and gauge transformations for uncertainty descrition of geometric structure with indeterminacy, IEEE Trans. Inf. Theory, ol. 47, no. 5, , Jul , 2, 3, 5, 6, 7, 8 [15] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. Achtelik, and R. Siegwart, The EuRoC MAV datasets, Int. J. Robot. Research, , 7 [16] A. J. Daison and R. M. Murray, Simultaneous localization and mabuilding using actie ision, IEEE Trans. Pattern Anal. Machine Intell., ol. 24, no. 7, [17] A. Patron-Perez, S. Loegroe, and G. Sibley, A sline-based trajectory reresentation for sensor fusion and rolling shutter cameras, Int. J. Comut. Vis., ol. 113, no. 3, , [18] P. Furgale, J. Rehder, and R. Siegwart, Unified temoral and satial calibration for multi-sensor systems, in IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), [19] A. Agarwal, K. Mierle, and Others, Ceres soler, htt://ceres-soler. org. 4 [20] H. Carrillo, I. Reid, and J. A. Castellanos, On the comarison of uncertainty criteria for actie slam, in 2012 IEEE International Conference on Robotics and Automation, May 2012, [21] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, SVO: Semidirect isual odometry for monocular and multicamera systems, IEEE Trans. Robot., ol. PP, no. 99,. 1 17, [22] G. S. Chirikjian, Stochastic Models, Information Theory, and Lie Grous, Volume 2: Analytic Methods and Modern Alications (Alied and Numerical Harmonic Analysis). Birkhauser,