Guillaume Bourmaud
email: guillaume.bourmaud@ims-bordeaux.fr

Rémi Mégret
email: remi.megret@ims-bordeaux.fr

Audrey Giremus
email: audrey.giremus@ims-bordeaux.fr

Yannick Berthoumieu
email: yannick.berthoumieu@ims-bordeaux.fr

From Intrinsic Optimization to Iterated Extended Kalman Filtering on Lie Groups

Keywords: Filtering, Optimization, Manifolds, Lie Groups, Matrix Transformations, Kalman, Gauss-Newton, Motion, Homography, Camera Pose, 3D Similarity

In this paper, we propose a new generic filter called Iterated Extended Kalman Filter on Lie Groups. It allows to perform parameter estimation when the state and the measurements evolve on matrix Lie groups. The contribution of this work is threefold: 1) the proposed filter generalizes the Euclidean Iterated Extended Kalman Filter to the case where both the state and the measurements evolve on Lie groups, 2) this novel filter bridges the gap between the minimization of intrinsic non linear least squares criteria and filtering on Lie groups, 3) in order to detect and remove outlier measurements, a statistical test on Lie groups is proposed.

In order to demonstrate the efficiency of the proposed generic filter, it is applied to the specific problem of relative motion averaging, both on synthetic and real data, for Lie groups SE(3) (rigid body motions), SL(3) (homographies) and Sim(3) (3D similarities). Typical applications of these problems are camera network calibration, image mosaicing and partial 3D reconstruction

Introduction

During the last decade, parameter estimation through optimization on matrix manifolds has been extensively studied [START_REF] Absil | Optimization algorithms on matrix manifolds[END_REF] and employed in a wide range of applications [START_REF] Lui | Advances in matrix manifolds for computer vision[END_REF]. This is due to the fact that taking intrinsically into account the geometry of the manifold increases the rate of convergence of algorithms [START_REF] Ring | Optimization methods on riemannian manifolds and their application to shape space[END_REF] and helps to avoid singularities.

When the parameters follow a dynamical system and/or when the measurements are acquired sequentially, it is important to be able to perform the estimation by filtering [START_REF] Haykin | Kalman filtering and neural networks[END_REF].

The link between optimization and filtering on Euclidean spaces, in the context of non linear least squares, has already been studied [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF][START_REF] Bertsekas | Incremental least squares methods and the extended Kalman filter[END_REF]. It led to the Iterated Extended Kalman Filter (IEKF) that produces better results in practice than the Extended Kalman Filter (EKF).

Even if several works successfully extended Euclidean filtering algorithms to manifolds (see Table 1 in [START_REF] Bourmaud | Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions[END_REF]), to the best of our knowledge, the link between optimization and filtering has not been established for parameter estimation on manifolds yet.

In this work, we focus on bridging the gap between the formulation of intrinsic non linear least squares criteria and Kalman filtering on matrix Lie groups [START_REF] Hall | Lie Groups, Lie Algebras, and Representations An Elementary Introduction[END_REF] that form an important kind of manifolds. Typical examples include 3D rotation matrices SO(3), unitary quaternions SU (2), rigid-body motion SE(3), 3D similarities Sim(3), homographies SL [START_REF] Bandeira | A Cheeger inequality for the graph connection laplacian[END_REF]. More specifically, we are interested in generalizing the IEKF to the case of a state and measurements evolving on Lie groups.

Related Work

On the one hand, a large amount of works proposes to minimize intrinsic non linear least squares to estimate parameters evolving on a Lie group. Most of them apply a modified Gauss-Newton algorithm (GN) [START_REF] Björck | Numerical methods for least squares problems[END_REF], or closely related algorithms such as Levenberg-Marquardt or Reweighted Non Linear Least Squares, to take into account the geometry of the Lie group. [START_REF] Taylor | Minimization on the Lie group SO(3) and related manifolds[END_REF] was the first to propose a modified GN to estimate a 3D orientation. Simultaneous Localization and Mapping approaches (SLAM) [START_REF] Grisetti | Robust optimization of factor graphs by using condensed measurements[END_REF][START_REF] Jeong | Pushing the envelope of modern methods for bundle adjustment[END_REF][START_REF] Klein | Parallel tracking and mapping for small ar workspaces[END_REF][START_REF] Konolige | Sparse sparse bundle adjustment[END_REF] are based on a GN like algorithm to estimate 3D points as well as camera poses from a video sequence and [START_REF] Li | Newton geodesic optimization on special linear group[END_REF] uses a GN to estimate homographies. In the more specific context of estimating global transformations from relative transformation measurements, several works derived specific GN like algorithms: [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] and [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF] tackle the synchronization of rotations problem, [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] considers the consistent pose registration problem, [START_REF] Strasdat | Double window optimisation for constant time visual SLAM[END_REF] covers the partial 3D reconstruction merging problem and [START_REF] Meidow | Efficient video mosaicking by multiple loop closing[END_REF] performs image mosaicking.

On the other hand, a large number of Kalman-like filters has been proposed to estimate a state evolving on a Lie group: [START_REF] Crassidis | Unscented filtering for spacecraft attitude estimation[END_REF][START_REF] Markley | Attitude error representation for Kalman filtering[END_REF][START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF][START_REF] Persson | Invariant momentum-tracking Kalman filter for attitude estimation[END_REF] deal with the estimation of a 3D orientation, [START_REF] Malis | Dynamic estimation of homography transformations on the special linear group of visual servo control[END_REF] dynamically estimates homographies and [START_REF] Davison | Monoslam: Real-time single camera slam[END_REF] performs SLAM. In [START_REF] Bonnabel | Invariant extended Kalman filter : theory and application to a velocity-aided attitude estimation problem[END_REF][START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF] an extended Kalman filter on Lie groups is derived, which is dedicated to continuous systems possessing symmetries. In our previous papers [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF][START_REF] Bourmaud | Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions[END_REF], we generalize the extended Kalman filter to Lie groups both for discrete and continuous prediction models. However, these approaches are not related to optimization on Lie groups.

To the best of our knowledge, only a few works, that are specifically devoted to SLAM, have related filtering to optimization on Lie groups. [START_REF] Strasdat | Real-time monocular slam: Why filter?[END_REF] proposes an information-filter like algorithm while [START_REF] Kaess | iSAM: Incremental smoothing and mapping[END_REF][START_REF] Kaess | isam2: Incremental smoothing and mapping using the bayes tree[END_REF] derive efficient square root information like filters. However, none of these approaches deal with the case of measurements evolving on a Lie group.

In preliminary works ( [START_REF] Bourmaud | Global motion estimation from relative measurements using iterated extended Kalman filter on matrix Lie groups[END_REF] and [START_REF] Bourmaud | Global motion estimation from relative measurements in the presence of outliers[END_REF]), we propose an Iterated Extended Kalman Filter on Lie groups specifically dedicated to relative motion averaging. In this paper, we are interested in proposing a generic Iterated Extended Kalman Filter on Lie groups. Thus, in the next section, we recall the (Euclidean) IEKF formalism.

1.2 Reminder: The Iterated Extended Kalman Filter

The IEKF [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF][START_REF] Bertsekas | Incremental least squares methods and the extended Kalman filter[END_REF] is a filter dedicated to non-linear systems. The objective of this filter is to recursively approximate the posterior distribution p (x k |z 1 , . . . , z k ) by a Gaussian distribution, where x k ∈ R p is the state we wish to estimate at time k and z l ∈ R q is a measurement available at time l. Then the state estimate is taken as the mean of this approximated posterior distribution. More specifically, the filter is composed of two steps, a prediction step and an update step.

Prediction

The prediction step consists in approximating the following distribution:

p (x k |z 1 , . . . , z k-1 ) = ˆp (x k |x k-1 ) p (x k-1 |z 1 , . . . , z k-1 ) d L x k-1 (1)
≈ N R p x k ; µ k|k-1 , P k|k-1 (2)
where d L x k-1 corresponds to the Lebesgue measure on R p , p (x k-1 |z 1 , . . . , z k-1 ) = N R p x k-1 ; µ k-1|k-1 , P k-1|k-1 and the state transition is defined as:

x k = f (x k-1 ) + n k (3)
where n k ∼ N R p (n k ; 0 p×1 , R k ) is a white Gaussian noise and f : R p → R p is a differentiable function.

Classically, µ k|k-1 is obtained by propagating the previous mean µ k-1|k-1 through (3) without noise, while P k|k-1 is computed by linearizing (3) in x k-1 = µ k-1|k-1 and propagating P k-1|k-1 through this linearized version of the state transition model. The same prediction step can be seen alternatively as the fitting of a Gaussian distribution to the integrand in (1), using a Gauss-Laplace approximation (see Section 3), followed by the marginalization of x k-1 . In this case, µ k-1|k-1 is propagated by minimizing the negative log-likelihood of the integrand in (1):

{x k , xk-1 } = argmin x k ∈R p ,x k-1 ∈R p x k -f (x k-1 ) 2 R k + x k-1 -µ k-1|k-1 2 P k-1|k-1 (4)
where

• 2 Σ = (•) T Σ -1 (•) is the squared Mahalanobis distance. It is trivial to show that xk-1 = µ k-1|k-1 and xk = f µ k-1|k-1 .
The predicted mean µ k|k-1 , which coincides with the mode in the Gaussian case, is taken as xk . Finally, P k|k-1 is computed by applying the Gauss-Laplace covariance approximation equation (see Section 3), followed by a marginalization of x k-1 , which results in a closed form covariance prediction equation. This alternative view will help us in generalizing the IEKF prediction step to Lie groups.

Update

The update step consists in approximating the following distribution:

p (x k |z 1 , . . . , z k ) ∝ p (z k |x k ) p (x k |z 1 , . . . , z k-1 ) (5)
≈ N R p x k ; µ k|k , P k|k (6) assuming p (x k |z 1 , . . . , z k-1 ) = N R p x k ; µ k|k-1 , P k|k-1
and a measurement model defined as:

z k = h (x k ) + w k (7)
where

w k ∼ N R q (w k ; 0 q×1 , Q k ) is a white Gaussian noise, h (•) : R p → R q is a differentiable function.
w k and the state model noise n k are assumed to be independent.

Once again, this update step can be seen as the fitting of a Gaussian distribution to (5), using a Gauss-Laplace approximation (see Section 3).

In this case, the updated mean µ k|k = xk is defined as the minimizer of the negative log-likelihood of (5):

xk = argmin x k ∈R p z k -h (x k ) 2 Q k + x k -µ k|k-1 2 P k|k-1 (8)
Employing a GN algorithm to solve (8) allows one to compute µ k|k , while P k|k can be obtained by applying the Gauss-Laplace covariance approximation equation (see Section 3). Fortunately, by exploiting the specific structure of the GN applied to [START_REF] Bernstein | Matrix mathematics: theory, facts, and formulas[END_REF], it is possible to derive the IEKF update equations that allow to compute both µ k|k and P k|k very efficiently [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF].

Contribution and Outline of the paper

In this paper, we propose a generic Iterated Extended Kalman Filter on Lie Groups (LG-IEKF) that extends the link between the minimization of non linear least squares criteria and the Iterated Extended Kalman Filter [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF][START_REF] Bertsekas | Incremental least squares methods and the extended Kalman filter[END_REF] to the case where the state and the observations evolve on Lie groups.

For this purpose, we first present a fitting approach, called intrinsic Gauss-Laplace approximation, that allows to fit a concentrated Gaussian distribution on Lie groups to the probability density of a random variable evolving on a Lie group.

Since this fitting technique requires to find the minimizer of an intrinsic non linear least squares criterion, we present a generic intrinsic GN algorithm on Lie groups, called LG-GN, which has the advantage of taking intrinsically into account the geometry of the Lie group on which the parameters evolve.

Then, we show that a generalization of the IEKF, to the case where the state and the observations evolve on Lie groups, can be obtained by employing intrinsic Gauss-Laplace approximations both to derive the LG-IEKF prediction step and the LG-IEKF update step (which involves the LG-GN algorithm).

Finally, because of the Gaussian noise assumption, the LG-IEKF is sensitive to outlier measurements. Thus, a statistical test on Lie groups is derived to detect and remove them.

In order to demonstrate the efficiency of the proposed generic LG-IEKF, it is applied to the specific problem of relative motion averaging, both on synthetic and real data for Lie groups SE(3) (rigid body motions), SL(3) (homographies) and Sim(3) (3D similarities).

The rest of the paper is organized as follows: Section 2 introduces the formalism of Lie groups and the concentrated Gaussian distribution on Lie groups. Section 3 describes the intrinsic Gauss-Laplace approximation while Section 4 deals with the intrinsic GN on Lie groups algorithm. The theory behind the proposed filter is described in Section 5. In Section 6, our formalism is applied to the relative motion averaging problem, and evaluated experimentally. Finally, the conclusion is provided in Section 7.

Preliminaries

Introduction to Lie groups

Definitions

In this section we give the definitions and basic properties of (matrix) Lie groups and Lie algebra. For a detailed description of these notions the reader is referred to [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF]. A Lie group G is a group which has also the structure of a smooth manifold such that group composition and inversion are smooth operations. If G is a matrix Lie group, then X ∈ G ⊂ R n×n and its operations are matrix multiplication and inversion with the n × n identity matrix as identity element Id.

The matrix exponential exp G and matrix logarithm log G mappings establish a local diffeomorphism between an open neighborhood of 0 n×n in the tangent space at the identity T Id G, called the Lie Algebra g, and an open neighborhood of Id in G. The Lie Algebra g ⊂ R n×n associated to a p-dimensional Lie group is a p-dimensional vector space defined by a basis consisting of real matrices E i for i = 1 . . . p. Hence there is a linear isomorphism between g and R P that we denote as follows: T . The previous notions are summarized in Fig. 1.

[•] ∨ G : g → R P and [•]
In order to lighten the notations, we define

1 : exp ∧ G (•) = exp G [•] ∧ G and log ∨ G (•) = [log G (•)] ∨ G .
For the sake of brevity, in the rest of the paper, when we mention the Lie algebra g of a p-dimensional Lie group G, we implicitly refer to its isomorphic Euclidean space R p . Moreover, when we write A = exp ∧ G (a) we assume that log ∨ G (A) = a, i.e we work only on sets where exp ∧ G (•) and log ∨ G (•) are bijective functions. Let us introduce the linearized Baker-Campbell-Hausdorff formula which expresses the group product directly in R p :

log ∨ G (exp ∧ G (a) exp ∧ G (b)) = b + ϕ G (b) a + O a 2 (9)
where

ϕ G (b) = ∞ n=0 B n ad G (b) n n! = Id - 1 2 ad G (b) + • • • (10)
is the inverse of the left Jacobian2 of G, the B n are the Bernoulli numbers and

ad G (b) a = [b] ∧ G [a] ∧ G -[a] ∧ G [b] ∧ G ∨ G (11)

Product of Lie groups

Most of the time, we wish to estimate several parameters evolving on different Lie groups, at once. Since the product of Lie groups is a Lie group [START_REF] Rudkovskii | Twisted product of Lie groups[END_REF], the algorithms presented in this paper can be applied to "concatenations" of Lie groups. They can even be applied to Euclidean parameters since an Euclidean space R p is a trivial Lie group by taking the matrix embedding:

x ∈ R p → X = Id x 0 1×p 1 ⊂ R (p+1)×(p+1) (12)
Fig. 1: Illustration of the geometry of a (matrix) Lie group

The simplest way to "concatenate" several components is to consider the Lie group formed by their direct product (e.g: SO(3)×R3 ). Note that other ways to "concatenate" Lie groups exist [START_REF] Rudkovskii | Twisted product of Lie groups[END_REF] (e.g: semi-direct product, twisted product).

Additional notations

In the rest of the paper, G and G are Lie groups of intrinsic dimensions p and q respectively. d H X is the right invariant Haar measure of G.

Concentrated Gaussian distribution on Lie groups

Here, we introduce the concept of concentrated Gaussian distribution on Lie groups, which was initially proposed in [START_REF] Smith | Computing map trajectories by representing, propagating and combining pdfs over groups[END_REF] and then studied in [START_REF] Wang | Error propagation on the Euclidean group with applications to manipulators kinematics[END_REF][START_REF] Wolfe | Bayesian fusion on Lie groups[END_REF], as a generalization of the normal distribution in Euclidean space. The distribution of X ∈ G is called a right 3 concentrated Gaussian distribution on G of "mean" µ and "covariance" P , denoted X ∼ N R G (X; µ, P ) (the superscript R stands for "right"), if:

X = exp ∧ G ( ) µ (13)
where ∼ N R p ( ; 0 p×1 , P ) and P ⊂ R p×p is a symmetric positive-definite matrix. Note that (13) depends on the choice of the Lie algebra basis {E i } i=1...p . When the maximum of the eigenvalues of P is sufficiently "small", the probability mass is concentrated around µ and the probability density of X, w.r.t the right invariant Haar measure of G (denoted d H X) [START_REF] Faraut | Analysis on Lie groups: an introduction[END_REF], can be approximated as follows:

p (X) ≈ 1 (2π) p det (P ) e -1 2 log ∨ G (Xµ -1 ) 2 P ( 14
)
Such a distribution allows us to describe the covariance in R p and hence to use Euclidean tools while being invariant w.r.t the right action of the group on itself:

exp ∧ G ( ) = Xµ -1 = XX (µX ) -1 with X ∈ G.
In Fig. 2, we provide an example of a concentrated Gaussian distribution on the Lie group

SE (2) = X = R t 0 1×2 1 R ∈ SO (2) , t = u v ∈ R 2
with Lie algebra

se (2) =    x =   0 -θ x θ 0 y 0 0 0   θ, x, y ∈ R   
Note that the "banana" shape [START_REF] Long | The banana distribution is Gaussian: A localization study with exponential coordinates[END_REF] comes from the nonlinearity 4 of exp SE(2) and the correlations between θ, x and y. Such a distribution is a better representation of the uncertainty of a robot negotiating a bend compared to a Euclidean Gaussian distribution.

Intrinsic Gauss-Laplace approximation

A classical way to tackle a Bayesian filtering problem is to fit, at each time instant, a parametric distribution to the posterior distribution of the state. In this section, we propose a fitting method which will be employed, in the rest of the paper, to fit concentrated Gaussian distributions on Lie groups.

Problem

Let us consider the probability distribution of a random variable X ∈ G:

p (X) = αe -φ(X) 2 Σ ( 15
)
where φ : G → R m is a differentiable function and p < m.

The objective is to propose a method to fit a concentrated Gaussian distribution to p (X). 4 Of course, this shape is emphasized by the action of µ on exp ∧ G ( ).

Proposed solution

First of all, let us define the minimizer of the cost function φ (X)

2 Σ : X = argmin X∈G φ (X) 2 Σ (16)
A first order Taylor expansion of φ (•) around X gives:

φ (X) = φ exp ∧ G (δ) X ≈ φ X + Jδ (17)
where δ = log ∨ G X X-1 and

J = dφ exp ∧ G (s) X ds s=0 (18)
We choose to take q (X) as an approximation of p (X):

q (X) = βe -φ( X)+Jlog ∨ G (X X-1 ) 2 Σ (19)
Finally, it is possible to show that q (X) has the form of a concentrated Gaussian distribution:

q (X) = N R G X; X, P = J T Σ -1 J -1 (20)
We call this fitting method "intrinsic Gauss-Laplace approximation". This method is a generalization of the Euclidean Gauss-Laplace approximation which is used for instance in [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF] to derive the IEKF update equations.

In fact, if G is a Euclidean space, the method comes down to fitting a Gaussian distribution. This method will be employed in Section 5 to derive both the prediction and update steps of the novel filter we present in this paper.

Intrinsic Gauss-Newton on Lie groups

The intrinsic Gauss-Laplace approximation introduced in the previous section assumes that we are capable of finding the minimizer X of ( 16).

However, when the minimizer does not have a closedform expression, it is common to employ an iterative optimization algorithm.

In this section, we derive an intrinsic GN algorithm that allows one to estimate a matrix X of parameters evolving on G.

Introduction

When performing an iterative optimization on an Euclidean space, such as a gradient descent, the parameters x ∈ R n are iteratively updated as follows:

x l+1 = x l + δ l+1/l ( 21
)
where δ l+1/l ∈ R n is an increment that corrects the previous parameter vector x l to get x l+1 . In order to perform intrinsic optimization on a Lie group G, the update equation ( 21) cannot be applied since the operator "+" does not allow the parameters to remain on the manifold. The key ingredient to intrinsically take into account the geometry of the Lie group is to replace [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF] with:

X l+1 = exp ∧ G δ l+1/l X l (22)
where X l , X l+1 ∈ G and δ l+1/l ∈ R p . One can see that the update equation ( 22) is tightly related to the concept of concentrated Gaussian distribution on Lie groups (13).

Algorithm

A common iterative method for solving the problem ( 16), when G is a Euclidean space, is the Gauss-Newton method. In the following, we extend this formalism to Lie groups and call it intrinsic Gauss-Newton on Lie groups (LG-GN).

A Riemannian GN algorithm can be found in [START_REF] Absil | Optimization algorithms on matrix manifolds[END_REF]. As a consequence, the algorithm we present can fit into their formalism (since a Lie group is a Riemannian manifold) and the convergence proof proposed in [START_REF] Absil | Optimization algorithms on matrix manifolds[END_REF] applies. However, the LG-GN we introduce is dedicated to Lie groups (as the one already proposed in [63]) and leverages the "one parameter subgroup" [START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF] structure of the Lie group, i.e the Riemannian structure of the Lie group is not explicitly exploited.

As in the Euclidean case, the convergence of the algorithm depends on the starting point X 0 . X l+1 is obtained by linearizing φ (•) at the previous value X l and solving the following problem:

δ l+1/l = argmin δ∈R p φ X l -J l δ 2 Σ (23)
where J l is defined as:

J l = - dφ exp ∧ G (s) X l ds s=0 (24)
Assuming that J l has full column rank, the minimizer of ( 23) is:

δ l+1/l = J l T Σ -1 J l -1 J l T Σ -1 φ X l (25)
The current value of the parameters is finally updated as follows:

X l+1 = exp ∧ G α l δ l+1/l X l (26)
where 0 < α l ≤ 1 is a step size. At convergence, we take X = X l . The approach described here will be employed in the next section to derive the update step of the novel filter we present in this paper.

Iterated Extended Kalman Filter on Lie Groups

From the concept of concentrated Gaussian distribution on Lie groups, the intrinsic Gauss-Laplace approximation and the LG-GN algorithm introduced in the previous sections, we derive a new filter called Iterated Extended Kalman Filter on Lie Groups (LG-IEKF).

Proposed System Model

In order to derive our novel filter, we first introduce a prediction model that describes the dynamical behavior of the state X k ∈ G we wish to estimate, where k stands for the time. Then we propose a measurement model that relates X k to the measurement Z k ∈ G .

Prediction Model

Let the system state be modeled as satisfying the following equation:

X k = exp ∧ G (n k ) f (X k-1 ) (27)
where

X k ∈ G, X k-1 ∈ G and n k ∼ N R p (n k ; 0 p×1 , R k ) is a white Gaussian noise. f : G → G is a differen- tiable function.
The prediction model that we consider induces the following conditional distribution:

p (X k |X k-1 ) = N R G (X k ; f (X k-1 ) , R k ) ( 28
)
This model is more generic than the one studied in [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF] and thus allows us to deal with a larger class of problems (see section 6).

In order to simplify the notations, we have assumed w.l.o.g that the Lie groups G k and G k-1 , on which X k and X k-1 evolve respectively, are the same Lie group G.

In practice, we will allow them to be different in order to augment the size of the state during prediction 5 (see Section 6.2.2).

Measurement Model

We consider discrete measurements Z k on G related to X k as follows:

Z k = exp ∧ G (w k ) h (X k ) (29)
where w k ∼ N R q (w k ; 0 q×1 , Q k ) is a white Gaussian noise and h : G → G is a differentiable function. Moreover, n k and w k are assumed to be independent. The measurement model, that we consider, induces the following conditional distribution:

p (Z k |X k ) = N R G (Z k ; h (X k ) , Q k ) (30)
5 Augmenting the size of the state during the prediction step is sometimes called "smoothing" and not "filtering" in the literature.

Objective

We propose to approximate the state posterior distribution with a concentrated Gaussian distribution on Lie groups: p (X k |Z 1 , . . . , Z l ) ≈ N R G X k ; µ k|l , P k|l . We focus on l = k -1 (prediction) and l = k (update). Therefore, the aim of the LG-IEKF is to predict and update the distribution parameters µ k|k and P k|k . In our formalism, µ k|k is taken as the state estimate at time k.

LG-IEKF Prediction

We assume that the state posterior distribution at time k -1 is represented by:

p (X k-1 |Z 1 , . . . , Z k-1 ) = N R G X k-1 ; µ k-1|k-1 , P k-1|k-1 (31)
The aim of this section is to show how to fit a concentrated Gaussian distribution to the posterior distribution of the predicted state:

p (X k |Z 1 , . . . , Z k-1 ) = ˆp (X k |X k-1 ) p (X k-1 |Z 1 , . . . , Z k-1 ) d H X k-1 (32) ≈N R G X k ; µ k|k-1 , P k|k-1 (33)
In order to estimate µ k|k-1 and P k|k-1 , we propose to apply an intrinsic Gauss-Laplace approximation (see Section 3) to the integrand of (32) and then to marginalize X k-1 .

Mean Prediction

In order to predict the mean, we minimize the negative log-likelihood of (32):

Xk , Xk-1 = argmin X k ∈G,X k-1 ∈G    log ∨ G X k f (X k-1 ) -1 2 R k + log ∨ G X k-1 µ -1 k-1|k-1 2 P k-1|k-1    (34)
A trivial minimizer of this problem is Xk-1 = µ k-1|k-1 and Xk = f µ k-1|k-1 . We finally take:

µ k|k-1 = Xk = f µ k-1|k-1 (35)

Covariance Prediction

Concerning the covariance prediction, we apply the intrinsic Gauss-Laplace approximation formula [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF]. By linearizing the error function inside the squared Mahalanobis norm in [START_REF] Hartley | Rotation averaging[END_REF] around its minimizer, we obtain:

-log (p (X k |X k-1 ) p (X k-1 |Z 1 , . . . , Z k-1 )) ≈ Id -F k 0 Id δ k δ k-1 2 Σ (36)
where

X k-1 = exp ∧ G (δ k-1 ) µ k-1|k-1 and X k = exp ∧ G (δ k ) f µ k-1|k-1 . Moreover, Σ = R k P k-1|k-1 (37)
and

F k = - dlog ∨ G f µ k-1|k-1 f exp ∧ G (s) µ k-1|k-1 -1 ds s=0 (38)
Thus, according to equation ( 20), the covariance P can be approximated by:

P = Id -F k 0 Id T Σ -1 Id -F k 0 Id -1 (39) = R -1 k -R -1 k F k -F T k R -1 k F T k R -1 k F k + P -1 k-1|k-1 -1 (40) = F k P k-1|k-1 F T k + R k F k P k-1|k-1 P k-1|k-1 F T k P k-1|k-1 (41)
where we employed the blockwise matrix inversion [START_REF] Bernstein | Matrix mathematics: theory, facts, and formulas[END_REF].

Under the concentrated Gaussian approximation, the top left block of P in (41) corresponds to P k|k-1 . Thus, we obtain the following covariance prediction formula:

P k|k-1 = R k + F k P k-1|k-1 F T k (42)

Prediction step summary

The prediction step consists in propagating the mean µ k-1|k-1 and the covariance P k-1|k-1 by using the prediction model [START_REF] Fisher | Statistical tables for biological, agricultural and medical research[END_REF]. At the end of the prediction step, the concentrated Gaussian approximation of the posterior is:

p (X k |Z 1 , . . . , Z k-1 ) ≈ N R G X k ; µ k|k-1 , P k|k-1 (43)

LG-IEKF Update

The aim of this section is to demonstrate how to fit a concentrated Gaussian distribution to the posterior distribution of the state, after having received the measurement Z k , using an intrinsic Gauss-Laplace approximation:

p (X k |Z 1 , . . . , Z k ) ∝p (Z k |X k ) p (X k |Z 1 , . . . , Z k-1 ) (44)
≈N R G X k ; µ k|k , P k|k (45)

Mean Update

In order to update the mean, we minimize the negative log-likelihood of ( 44):

Xk = argmin X k ∈G    log ∨ G Z k h (X k ) -1 2
Q k + log ∨ G X k µ -1 k|k-1 2 P k|k-1    (46)
To minimize this function, we propose to apply the LG-GN algorithm described in Section 4. We introduce the following notations:

δ l+1/l = log ∨ G X l+1 X l -1 , δ l = log ∨ G X l µ -1 k|k-1 and δ l+1 = log ∨ G X l+1 µ -1
k|k-1 where X l denotes the parameters value at iteration l of the LG-GN.

At iteration l, we seek the minimizer of the following problem:

δ l+1/l = argmin δ∈R p   log ∨ G Z k h X l -1 -H l δ 2 Q k + δ l + ϕ G δ l δ 2 P k|k-1   = argmin δ∈R p ψ X l -Ψ l δ 2 Ξ k (47)
where we used [START_REF] Bertsekas | Incremental least squares methods and the extended Kalman filter[END_REF]. The matrix ϕ G δ l is defined in [START_REF] Björck | Numerical methods for least squares problems[END_REF],

H l = - dlog ∨ G Z k h exp ∧ G (s) X l -1 ds s=0 (48) ψ X l = log ∨ G Z k h X l -1 T δ l T T ( 49
)
Ψ l = H T l -ϕ G δ l T T (50)
and

Ξ k = Q k 0 0 P k|k-1 (51)
For the sake of brevity, the inverse of the left Jacobian of G is denoted ϕ G δ l ≡ ϕ l and Φ l = ϕ -1 l .

The minimizer of ( 47) is given by:

δ l+1/l = Ψ T l Ξ -1 k Ψ l -1 Ψ T l Ξ -1 k ψ X l = H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l -1 H T l Q -1 k log ∨ G Z k h X l -1 -ϕ T l P -1 k|k-1 δ l (52)
We now demonstrate (see Appendix A.1) that, neglecting second order terms in δ l+1/l , it is possible to obtain a generalization of the IEKF mean update equation [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF] to Lie groups:

δ l+1 = K l log ∨ G Z k h X l -1 + H l Φ l δ l (53)
where K l is a gain that we call Lie-Kalman gain (see Appendix A.2) and is defined as:

K l = P k|k-1 Φ T l H T l H l Φ l P k|k-1 Φ T l H T l + Q k -1 (54)
Note that this Lie-Kalman gain is a generalization of the Kalman gain [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF] to Lie groups. To the best of our knowledge, it is the first time that this expression is derived. Moreover ( 53) can be simplified:

Φ l δ l = Φ G δ l δ l = ∞ n=0 1 (n + 1)! ad G δ l n δ l = δ l (55) since ad G δ l δ l = 0 (56)
Finally, the current value of the parameters can be updated as follows:

X l+1 = exp ∧ G δ l+1/l X l (57)
≈ exp ∧ G K l log ∨ G Z k h X l -1 + H l δ l µ k|k-1 (58)
At convergence, we take µ k|k = Xk .

Covariance Update

In order to estimate the updated covariance, we apply the intrinsic Gauss-Laplace approximation formula [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF]. To do so, we linearize the error function inside the squared Mahalanobis norm in [START_REF] Lui | Advances in matrix manifolds for computer vision[END_REF] around its minimizer µ k|k :

-log (p (Z k |X k ) p (X k |Z 1 , . . . , Z k-1 )) ≈ log ∨ G Z k h µ k|k -1 -H l δ δ l + ϕ l δ 2 Σ ( 59
)
where

Σ = Q k 0 0 P k|k-1 , X k = exp ∧ G (δ)
µ k|k and the subscript l corresponds to the last iteration of the LG-GN employed to update the mean. Thus, according to equation [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF], the covariance P k|k can be approximated by:

P k|k = H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l -1 (60)
However, it is possible to show that (see Appendix A.3):

P k|k = Φ l (Id -K l H l Φ l ) P k|k-1 Φ T l ( 61
)
which is a generalization of the IEKF covariance update equation [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF] to Lie groups.

Update step summary

The update step consists in updating the mean µ k|k-1 and the covariance P k|k-1 by incorporating the information coming from the measurement Z k , by using the measurement model [START_REF] Govindu | Combining two-view constraints for motion estimation[END_REF]. At the end of the update step, the concentrated Gaussian approximation of the posterior is:

p (X k |Z 1 , . . . , Z k ) ≈ N R G X k ; µ k|k , P k|k (62)

Inlier test before update

The novel filter we propose is based on the classical assumption that the measurement noise w k (see Section 5.1) is a white Gaussian noise. This assumption is convenient and allowed us to derive an efficient algorithm. However, it also makes the LG-IEKF not resilient to outlier measurements (this is also a limitation of Kalman filtering on Euclidean spaces [START_REF] Ramachandra | Kalman filtering techniques for radar tracking[END_REF]). Consequently, we propose a statistical test on Lie groups to detect and remove outliers.

A measurement Z k is an inlier if and only if:

log ∨ G Z k h (X k ) -1 2
Q k < t ( 63
)
where t is a threshold to define. However, the true value X k is unknown. We only have an approximation of the posterior distribution of the state:

p (X k |Z 1 , . . . , Z k-1 ) ≈ N R G X k ; µ k|k-1 , P k|k-1 (64)
which can also be expressed as follows:

X k = exp ∧ G k|k-1 µ k|k-1 (65)
where k|k-1 ∼ N R p ; 0 p×1 , P k|k-1 . Thus, we propose a statistical inlier test on Lie groups w.r.t the current estimate of X k :

log ∨ G Z k h µ k|k-1 -1 2
Qerr < t (66)
where

Q err = E H k|k-1 k|k-1 + w k H k|k-1 k|k-1 + w k T = H k|k-1 P k|k-1 H T k|k-1 + Q k (67)
Indeed, from [START_REF] Govindu | Combining two-view constraints for motion estimation[END_REF] and neglecting second order terms in w k and k|k-1 , we have:

0 = log ∨ G Z k h (X k ) -1 exp ∧ G (-w k ) log ∨ G Z k h exp ∧ G k|k-1 µ k|k-1 -1 exp ∧ G (-w k ) log ∨ G Z k h µ k|k-1 -1 -H k|k-1 k|k-1 -w k (68)
where

H k|k-1 = - dlog ∨ G Z k h exp ∧ G (s) µ k|k-1 -1 ds s=0 (69)
Thus, under the concentrated Gaussian assumption, the LHS of ( 66) is distributed according to the chi-squared distribution with q degrees of freedom, since G is a qdimensional Lie group. Consequently, one way to decide whether Z k is an inlier is to define a threshold based on the p-value of χ 2 (q) [START_REF] Fisher | Statistical tables for biological, agricultural and medical research[END_REF]. Note that since we neglected second order terms, this theoretical threshold is possibly restrictive.

Summary of the LG-IEKF and remarks

The LG-IEKF algorithm is summarized in Alg.1.

Remark 1Employing the LG-GN allowed us to iteratively refine the linearization point during the update step contrary to the Extended Kalman Filter on Lie Groups (LG-EKF), proposed in [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF], that performs only one linearization. In this sense, the LG-EKF is a special case of the LG-IEKF proposed in this paper. Note that the prediction model we use in this paper is more generic than the one employed in [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF].

Remark 2The models ( 27) and ( 29) as well as the algorithm Alg.1 reduce to the traditional models and algorithm of the IEKF in the case of a state and measurements evolving on Euclidean spaces. In this sense, the LG-IEKF generalizes the IEKF to Lie groups.

Algorithm 1 LG-IEKF Algorithm

Inputs :

µ k-1|k-1 , P k-1|k-1 , Z k , Q k , R k , t (optional)
Outputs : µ k|k , P k|k 1) Prediction

µ k|k-1 = f µ k-1|k-1 P k|k-1 = F k P k-1|k-1 F T k + R k where F k = - dlog ∨ G f (µk-1|k-1)f(exp ∧ G (s)µ k-1|k-1 ) -1 ds s=0
2) Inlier Test (optional)

log ∨ G Z k h µ k|k-1 -1 2
Q err < t
where

Q err = H k|k-1 P k|k-1 H T k|k-1 + Q k and H k|k-1 = - dlog ∨ G Z k h(exp ∧ G (s)µ k|k-1 ) -1 ds s=0
3) Update (if 2) is satisfied)

Set X 0 = µ k|k-1 and δ 0 = 0 p×1
Iterate until convergence:

K l = P k|k-1 Φ T l H T l H l Φ l P k|k-1 Φ T l H T l + Q k -1 δ l+1 = K l log ∨ G Z k h X l -1 + H l δ l X l+1 = exp ∧ G δ l+1 µ k|k-1
where

H l = - dlog ∨ G Z k h(exp ∧ G (s)X l ) -1
ds s=0

ϕ l ≡ ϕ G δ l is the inverse of the left Jacobian of G and Φ l = ϕ -1 l . At convergence:

µ k|k = X l P k|k = Φ l (Id -K l H l Φ l ) P k|k-1 Φ T l F k , H k|k-1
and H l are Jacobian matrices which expressions depend on the application.

Application To Relative Motion Averaging

Here we apply the generic LG-IEKF algorithm to the problem of estimating global transformations from noisy relative transformation measurements6 , a.k.a relative motion averaging.

Such a problem occurs for instance in the context of consistent pose registration [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] encountered in 3D localization, structure from motion or camera network calibration. In this case, a motion or transformation is a rigid body transformation matrix (SE(3)). Thus, the relative measurements correspond to the rigid transformations between two camera poses and the global motions we wish to estimate are the rigid transformation matrices between a reference camera pose and all the other camera poses.

In our context, a transformation is defined as an element of a Lie group. Consequently, the solution we propose can be applied to several other problems such as the synchronization of rotations problem (SO(3)) [START_REF] Boumal | Robust estimation of rotations from relative measurements by maximum likelihood[END_REF], image mosaicing (SL(3)) [START_REF] Meidow | Efficient video mosaicking by multiple loop closing[END_REF] or the partial 3D reconstruction merging problem (Sim(3)) [START_REF] Wachinger | Registration strategies and similarity measures for three-dimensional ultrasound mosaicing[END_REF].

Objective

We aim at estimating global transformations {T iR } i=1:N , where each global transformation T iR ∈ G is defined as the transformation between a main reference frame (RF) R and a RF i, and G is a q-dimensional Lie group.

We consider the case where the noises on the relative transformation measurements {Y ij } 1≤i<j≤N are mutually independent. Each Y ij ∈ G denotes a noisy relative transformation between a RF j and a RF i expressed as follows:

Y ij = exp ∧ G b i ij T iR T -1 jR (70)
where b i ij ∼ N R q b i ij ; 0 q×1 , Σ i ij is a white Gaussian noise. The problem is illustrated in Fig. 3a. Note that (70) is invariant w.r.t the right action of G . Indeed,

T iR M (T jR M ) -1 = T iR T -1
jR for any M ∈ G . In the context of consistent pose registration, it simply means that rotating and translating all the camera poses does not affect the relative measurements.

Case 1: Outlier Free Estimation

In this section, we consider the case where the measurements are outlier free.

State of the Art

A large amount of works addressing the problem tackled in this section has been previously published. However, they usually do not intrinsically take into account the Lie group structure and are tailored to specific applications such as: -Relative orientation averaging (Lie group SO(3), or SU (2) for unitary quaternion), a.k.a multiple rotation averaging [START_REF] Hartley | Rotation averaging[END_REF], a.k.a synchronization of rotations [START_REF] Boumal | Cramer-Rao bounds for synchronization of rotations[END_REF]. In [START_REF] Govindu | Combining two-view constraints for motion estimation[END_REF] and [START_REF] Martinec | Robust rotation and translation estimation in multiview reconstruction[END_REF], in order to obtain a closed-form solution that only requires solving a large linear system of equations, the Lie group geometry of SU (2) and SO(3) respectively are overlooked. [START_REF] Fredriksson | Simultaneous multiple rotation averaging using Lagrangian duality[END_REF] formulates the problem as [START_REF] Govindu | Combining two-view constraints for motion estimation[END_REF], but the Lie group constraints of SU (2) are extrinsically taken into account using Lagrangian duality.

-Relative Euclidean motion (Lie group SE(3)) averaging, a.k.a camera pose registration problem. In [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF], a Euclidean motion is parametrized as an element of the Lie algebra se (3) of SE(3), which is a vector space, in order to formulate the problem into a classical nonlinear unconstrained Euclidean minimization problem. However, this parametrization causes problems at the boundary of the Lie algebra. [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF] proposes an iterative algorithm that intrinsically takes into account the Lie group structure of SE(3). Nevertheless, the minimized criterion is not invariant (w.r.t the left and right action of the Lie group on itself) while the measurement model is invariant to the action of SE(3).

-Relative homography (Lie group SL(3)) averaging, a.k.a image mosaicing. In [START_REF] Caballero | Homography based Kalman filter for mosaic building. applications to UAV position estimation[END_REF], an Euclidean Extended Kalman Filter is derived for which the Lie group constraints are extrinsically taken into account using a reprojection after each iteration. [START_REF] Meidow | Efficient video mosaicking by multiple loop closing[END_REF] derives an iterative algorithm based on a matrix exponential update rule. However they do not completely take advantage of the Lie group structure of SL [START_REF] Bandeira | A Cheeger inequality for the graph connection laplacian[END_REF]. In order to obtain a closed-form solution to the problem, [START_REF] Khurd | Global error minimization in image mosaicing using graph connectivity and its applications in microscopy[END_REF] performs a relaxation where the Lie group constraints of SL(3) are not considered.

Implementation of the LG-IEKF

In order to apply the LG-IEKF to the relative motion averaging problem, we need to specify the prediction and update models.

However, before detailing these models, let us describe the way the observations {Y ij } 1≤i<j≤N are used.

We distinguish the temporally consecutive observations (Y i (i+1) for i = 1...N -1) from the other observations (Y ij with j > i + 1). Indeed, in order to guide the estimation process, the temporally consecutive observations are used as control inputs in the prediction steps (see Section 6.2.3). The other observations are used as measurements in the update steps (see Section 6.2.4). A graphical example of the prediction and update steps is proposed in Fig. 3.

Prediction model

As explained in Section 5.1.1, during the LG-IEKF derivation, we have assumed w.l.o.g that the Lie groups G k and G k-1 , on which X k and X k-1 evolve respectively, are the same Lie group G. For this application, we allow them to be different in order to augment the size of the state during prediction. Thus, at time instant k -1, the state X k-1 contains the global transformations T 1R , T 2R , ..., T (k-1)R , T kR , while at time k, the state

X k contains the global transformations T 1R , T 2R , ..., T (k-1)R , T kR , T (k+1)R . Consequently, G k = G × G × • • • × G , i.e k + 1 direct products of G .
More precisely, the prediction step that we propose augments the size of the state by duplicating the global motion T kR and propagating it using the control input

Y (k+1) k = Y -1
k (k+1) in order to predict T (k+1)R . Thus we consider the following prediction model:

X k = exp ∧ G b k+1 (k+1)k Y (k+1) k (X k-1 ) kR 0 0 X k-1 (71) =exp ∧ G k b k+1 (k+1)k 0 Y (k+1) k 0 0 Id (X k-1 ) kR 0 0 X k-1 (72)
where the notation (X k-1 ) kR corresponds to extracting the global motion component T kR from the state

X k-1 . b k+1 (k+1)k ∼ N R q b k+1 (k+1)k ; 0 q×1 , Σ k+1 (k+1)k is a white Gaus- sian noise.
As can be seen, eq.( 72) has the form of [START_REF] Fisher | Statistical tables for biological, agricultural and medical research[END_REF] where

f (X k-1 ) = Y (k+1) k 0 0 Id (X k-1 ) kR 0 0 X k-1 and n k = b k+1 (k+1)k 0 .
As a consequence, the LG-IEKF prediction equations can be applied.

Update model

The measurement model we consider corresponds to.

Z k = Y i (k+1) = exp ∧ G b i i(k+1) (X k ) iR (X k ) -1 (k+1)R ( 73
)
where i < k and b

i i(k+1) ∼ N R q b i i(k+1) ; 0 q×1 , Σ i i(k+1)
is a white Gaussian noise.

As can be seen, eq.( 73) has the form of ( 29), where

w k = b i i(k+1) and h (X k ) = (X k ) iR (X k ) -1
(k+1)R . As a consequence, the LG-IEKF update equations can be applied.

Note that if several measurements Y i (k+1) , for all i < k, are available, then we concatenate them in order to have a single measurement equation.

Application of the LG-IEKF prediction step

In order to apply the LG-IEKF prediction step to (72), we need to compute F k (see eq.( 38)). We obtain (see Appendix B.1): LG-IEKF : This paper LG-EKF (Bourmaud et al.) LG-GN : This paper Fig. 4: Computational time and Residual errors for different number of global motions (N ). The number of relative motions is fixed to N + n (in our experiments n = 30).

Results on Simulated Data for the Camera Pose Registration Problem

We compare the proposed approach to state of the art algorithms on a camera pose registration problem, i.e global Euclidean motion (Lie group SE(3)) estimation from relative measurements.

The explicit definitions of exp ∧ SE(3) , log ∨ SE(3) and Ad SE(3) can be found in [START_REF] Selig | Lie groups and Lie algebras in robotics[END_REF].

The data are simulated using the generative model proposed in (70).

We consider 6 different algorithms:

-"Chain": it composes the temporally consecutive relative measurements to obtain the global motions -"Euc-GN Lie Algebra": algorithm proposed in [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] (Agrawal et al.)

-"GN Non Invariant": algorithm proposed in [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF]

-"LG-GN": LG-GN derived in Section 4 minimizing

i,j log ∨ SE(3) Y ij T jR T -1 iR 2 Σ i ij (77)
-"LG-IEKF": LG-IEKF derived in Section 5

-"LG-EKF": modified version of the algorithm proposed in [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF] (Bourmaud et al.) which actually corresponds to applying the LG-IEKF with only one iteration at each update step

In order to fairly compare the algorithms [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF], [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] and the LG-GN to the LG-IEKF, they are applied incrementally since it guides the estimation process and reduces the chance of falling in a poor local minimum. All the algorithms are coded in Matlab and tested with the following configuration: core I5 4x2.27GHz, 4GB, Linux 64 bits.

We simulate circular camera trajectories (see Fig. 6) with N cameras. In order to compare the results of each approach to the true global motions, we need to add a step to align the estimated global motions with the true global motions. For that purpose, we choose to employ another LG-GN to minimize the sum of the following er-

ror function: log ∨ SE(3) µ iR T RR T rue T -1 iR T rue 2
. The final error of this LG-GN obtained for each approach as well as the computational time are presented in Fig. 4.

Results show that the LG-IEKF, which considers invariant errors, performs significantly better than the state of the art algorithms [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] and [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF].

Indeed, the parametrization used in [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] causes issues at the boundary of the Lie algebra, while [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF] does not consider an invariant error function and thus does not fully exploit the geometry of the problem. For these reasons, [START_REF] Agrawal | A Lie algebraic approach for consistent pose registration for general Euclidean motion[END_REF] and [START_REF] Govindu | Lie-algebraic averaging for globally consistent motion estimation[END_REF] frequently fall into poor local minima.

As expected, the LG-GN, which is a batch optimization, performs slightly better than the LG-IEKF. However, the LG-IEKF has a much lower computational time. Finally, one can see that the LG-IEKF, that iteratively refines its linearization point, outperforms the LG-EKF.

Case 2: Estimation in the presence of outliers

In this section, we consider the case where the measurements are corrupted with outliers and compare the performances of the proposed LG-IEKF against the state of the art algorithms able to deal with outliers. As we will see in the experimentations on real data, the outlier measurements are usually due to duplicated structures in the environment [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF].

State of the Art

A large amount of works has been recently devoted to specifically dealing with multiple rotation averaging in the presence of outliers. This problem is also known as synchronization of rotations in the mathematics community and is usually tackled by minimizing a given criterion. In [START_REF] Bandeira | A Cheeger inequality for the graph connection laplacian[END_REF] and [START_REF] Singer | Three-dimensional structure determination from common lines in cryo-em by eigenvectors and semidefinite programming[END_REF], spectral relaxations of the problem are proposed while [START_REF] Boumal | Robust estimation of rotations from relative measurements by maximum likelihood[END_REF] uses their results as initialization for a second order Riemannian trust-region algorithm to compute a local maximizer. [START_REF] Wang | Exact and stable recovery of rotations for robust synchronization[END_REF] derives LG-IEKF : This paper Fig. 5: Estimation error of the global motions for our approach, Chatterjee et al. [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] and Roberts et al. [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] on a camera pose estimation problem (λ = 1 10 and number of relative motions fixed to 5N + n with n = 60).

an algorithm that exactly estimates the global rotations when a subset of the measurements are perfect and outperforms [START_REF] Singer | Three-dimensional structure determination from common lines in cryo-em by eigenvectors and semidefinite programming[END_REF]. In [START_REF] Hartley | L1 rotation averaging using the Weiszfeld algorithm[END_REF] and [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF], two robust iterative algorithms, based on L1 and L1-L2 minimization criterion, respectively, are devised. However, the considered error functions are not convex and consequently need a good initialization such as [START_REF] Wang | Exact and stable recovery of rotations for robust synchronization[END_REF] to avoid poor local minima. Finally, [START_REF] Crandall | Discrete-continuous optimization for large-scale structure from motion[END_REF] proposes a discretization of SO(3) to apply a loopy belief propagation algorithm on the resulting Markov random field.

The works [START_REF] Enqvist | Non-sequential structure from motion[END_REF], [START_REF] Jiang | A global linear method for camera pose registration[END_REF] and [START_REF] Moulon | Global fusion of relative motions for robust, accurate and scalable structure from motion[END_REF] are also relevant for the multiple rotation averaging problem. However, they are specifically tailored for SO(3) and it is not straightforward to apply them to other Lie groups.

To the best of our knowledge, only one approach [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] was proposed to deal with the generic problem of global motion estimation from relative measurements in the presence of outliers. An Expectation Maximization (EM) algorithm is proposed, introducing latent variables to classify the measurements as inliers or outliers.

Implementation of the LG-IEKF

In order to apply the LG-IEKF, we assume that the temporally consecutive observations (Y i (i+1) for i = 1...N -1) are not corrupted with outliers. This assumption might appear restrictive, however, for image sequences for example, it is usually satisfied (see Sections 6.3.4 and 6.3.5). Indeed, relative transformations computed between consecutive images usually do not produce outlier measurements.

The LG-IEKF we apply is the same than the one derived in Section 6.2.2. Nevertheless, this time the in-

Results on Simulated Data for the Camera Pose Registration Problem

We compare the performance of the LG-IEKF to two state of the art algorithms [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] and [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] on a camera pose registration problem (Lie group SE(3)) with outliers. [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] was developed to deal with SO(3) but its extension to SE(3) is straightforward. We simulate circular camera trajectories (see Fig. 6) with N cameras where each camera T iR T rue has a timestamp t i and we generate noisy relative motions as follows: first of all, a measurement can be either an inlier or an outlier (except temporally consecutive relative measurements that are always inliers). We model the probability of a measurement to be an inlier as P (Z ij is inlier) = exp (-λ |t i -t j |) where λ is a userchosen parameter, i.e a larger time difference increases the chance to produce an outlier. After having drawn the label of a measurement (inlier or outlier), we sample the measurement. The distribution of the independent outliers is modeled as a centered Gaussian distribution on Lie groups with a large covariance matrix (the large covariance is not a problem in this case since log ∨ SE [START_REF] Bandeira | A Cheeger inequality for the graph connection laplacian[END_REF] is defined on the whole group) while an inlier can be sampled using (70).

In Fig. 5, we compare the proposed method (LG-IEKF with inlier test) against the robust approach proposed in [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] and the Expectation Maximization algorithm (EM) of [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF]. [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] and [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] are initialized by composing the temporally consecutive relative measurements as it is proposed by the authors of those papers. As in Section 6.2.7, in order to compare the results of each approach to the true global motions, we align the estimated global motions with the true global motions using an LG-GN. The error obtained, for each approach, at convergence of the LG-GN is presented in Fig. 5. We show that our method outperforms both [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] and [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF]. Indeed, [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] is based on a robust convex L2-L1 norm to mitigate the influence of the outliers. However, because of the Lie group curvature, the complete functional is not convex. Therefore, the algorithm usually gets stuck in a poor local minimum. [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] introduces latent variables to classify the relative motions as inliers or outliers. However, the labels obtained at the initialization of the global motions are very difficult to modify. Indeed, the E-step does not take into account the estimation errors of the current global motion estimates which is negligible only when N is small. Therefore, a lot of inliers remain classified as outliers. In comparison, our approach incrementally rejects outliers, taking into account the current uncertainty of the global motions, and refines its estimate with the inliers. Consequently, the global motions are correctly recovered. An example of recovered global motions with the three different approaches is presented in Fig. 6.

Results on Real Data for the Partial 3D reconstruction merging problem

In this section, the LG-IEKF is applied to a partial 3D reconstruction merging problem (Lie group Sim(3)). The experimental setup is the following: the camera is hand-held and evolves around two duplicated objects (the position of the camera is the same at the beginning and at the end of the video). We split the video in several (half-overlapping) parts. For each part, we applied a SLAM algorithm similar to [START_REF] Klein | Parallel tracking and mapping for small ar workspaces[END_REF] and obtained a 3D point cloud of the scene as well as the camera poses. We estimated the 3D similarities (Lie group Sim(3)) between every pair of point clouds using a RANSAC algorithm followed by a LG-GN algorithm on Sim(3). The covariance matrix of each relative motion is obtained by applying an intrinsic Gauss-Laplace approximation.

In Fig. 7, we compare the results of our approach to the EM algorithm of [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] and the method of [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF]. For each algorithm, we provide the camera trajectory obtained by aligning the camera poses of each reconstruction with the estimated global 3D similarities. Both [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] and [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] are initialized by composing the control inputs relative similarities.

In order to qualitatively compare the results, we manually annotated the 3D similarity measurements either as inlier or as outlier. Additionally, the position of the camera is the same at the beginning and at the end of the video, thus the first and the last estimated camera pose should be perfectly superimposed.

On the one hand, both the EM algorithm of [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] and the method of [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] remains stuck in a poor local minimum. Indeed, the first and the last camera poses are far from being superimposed and a lot of inliers are classified as outliers by [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] (see Fig. 7d). On the other hand, our approach perfectly infers the set of inliers (see Fig. 7e) while the first camera pose and the last one are almost perfectly superimposed.

Results on Real Data for the Automatic planar image mosaicking problem

In this section, the LG-IEKF presented in this paper is applied to an automatic planar image mosaicking problem. We took 53 photos of a planar scene (see Fig. 8a) with a smartphone, detected points of interest and estimated the homographies (Lie group SL(3)) between every pair of images using a RANSAC algorithm followed by a LG-GN algorithm on SL(3). The covariance matrix of each relative motion is obtained by applying an intrinsic Gauss-Laplace approximation. In our implementation, we use the Lie algebra basis of sl (3) given in [START_REF] Benhimane | Homography-based 2d visual tracking and servoing[END_REF]. In this dataset, there are 65% of outliers (see Fig. 8b) due to the ambiguity of the scene (some paper sheets are almost identical). Note that the relative homography between image 43 and image 44 is not available. Instead, we employ the relative homography between image 43 and image 45 as control input.

In Fig. 8, we compare the results of our approach against the EM algorithm of [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] which is initialized by composing the control inputs relative homographies. On the one hand, once again, the proposed EM of [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] classifies a lot of inliers as outliers since the estimation errors of the global motions estimates is not taken into account during the E-step. Consequently, [START_REF] Roberts | Structure from motion for scenes with large duplicate structures[END_REF] is not able to correctly recover the global motions (see Fig. 8c). On the other hand, our approach perfectly infers the set of inliers and produces a mosaic visually close to the ground truth (see Fig. 8d).

We could not apply [START_REF] Govindu | Efficient and robust large-scale rotation averaging[END_REF] because log ∨ SL(3) is not defined on the whole group. Fig. 8: Image mosaicking results: in the labeling matrices a white pixel corresponds to an inlier, a black pixel corresponds to an unavailable measurement, a gray pixel corresponds to an outlier.

Conclusion

In this paper, we proposed a generic Iterated Extended Kalman Filter on Lie Groups (LG-IEKF) that allows to perform parameter estimation when the state and the measurements evolve on Lie groups. This novel filter extends the link between the minimization of non linear least squares criteria and the Iterated Extended Kalman Filter [START_REF] Bell | The iterated Kalman filter update as a Gauss-Newton method[END_REF][START_REF] Bertsekas | Incremental least squares methods and the extended Kalman filter[END_REF] to Lie groups.

For this purpose, we first presented a fitting approach, called intrinsic Gauss-Laplace approximation, that allows to fit a concentrated Gaussian distribution on Lie groups to the probability density of a random variable evolving on a Lie group.

Since this fitting technique requires to find the minimizer of an intrinsic non linear least squares criterion, we presented a generic intrinsic GN algorithm on Lie groups, called LG-GN. This optimization algorithm has the advantage of taking intrinsically into account the geometry of the Lie group on which the parameters evolve.

Then, we showed that a generalization of the IEKF, to the case where the state and the observations evolve on Lie groups, can be obtained by employing intrinsic Gauss-Laplace approximations both to derive the LG-IEKF prediction step and the LG-IEKF update step (which involves the LG-GN algorithm). For each of these two steps, we were able to obtain a computationally efficient algorithm, by exploiting the specific structure of the problem.

Finally, we derived a statistical test on Lie groups to detect and remove outlier measurements.

In order to demonstrate the efficiency of the LG-IEKF, it has been applied to relative motion averaging problems for three Lie groups of interest. In each of these applications, the proposed approach significantly outperforms the state of the art algorithms.

Since this novel filter was derived generic prediction and measurement models on Lie groups, it can be applied to a large class of problems.

Future work will consider its use in localization from a wearable camera in a known 3D environment as well as in visual odometry [START_REF] Berger | Second-order recursive filtering on the rigid-motion lie group se (3) based on nonlinear observations[END_REF].

A Derivation of the LG-IEKF

A.1 Derivation of δ l+1

Here, we derive the expression of the δ l+1 in (53):

δ l+1 = log ∨ G X l+1 X l -1 X l µ -1 k|k-1 = log ∨ G exp ∧ G δ l+1/l exp ∧ G δ l ϕ l δ l+1/l + δ l = ϕ l H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l -1 H T l Q -1 k log ∨ G Z k h X l -1 -ϕ T l P -1 k|k-1 δ l + δ l = ϕ l H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l -1 H T l Q -1 k log ∨ G Z k h X l -1 -ϕ T l P -1 k|k-1 δ l + H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l Φ l δ l = ϕ l H T l Q -1 k H l + ϕ T l P -1 k|k-1 ϕ l -1 H T l Q -1 k log ∨ G Z k h X l -1 + H l Φ l δ l = K l log ∨ G Z k h X l -1 + H l Φ l δ l (78)
where K l is the Lie-Kalman gain derived in Appendix A.2.

A.2 Lie-Kalman Gain Derivation

Here, we derive the expression of the Lie-Kalman gain (54) (the superscripts and underscripts are omitted):

K = ϕ H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 = ϕ H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 HΦP Φ T H T + Q HΦP Φ T H T + Q -1 = ϕ H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 HΦP Φ T H T + H T HΦP Φ T H T + Q -1 = ϕ H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 H + ϕ T P -1 ϕ ΦP Φ T H T HΦP Φ T H T + Q -1 = P Φ T H T HΦP Φ T H T + Q -1 (79)

A.3 Covariance Update Derivation

Here, we derive the expression of the P k|k in (61) (the superscripts and underscripts are omitted):

P k|k = H T Q -1 H + ϕ T P -1 ϕ -1 = H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 H + ϕ T P -1 ϕ ΦP Φ T -H T Q -1 HΦP Φ T = ΦP Φ T -H T Q -1 H + ϕ T P -1 ϕ -1 H T Q -1 HΦP Φ T = ΦP Φ T -ΦKHΦP Φ T = Φ (Id -KHΦ) P Φ T ( 80
)
where K is defined in (79).

B Relative Motion Averaging

B.1 Derivation of F k

From ( 38) and (72), we have:

log ∨ G k f µ k-1|k-1 f exp ∧ G k-1 (δ) µ k-1|k-1 -1 =log ∨ G k Y (k+1) k 0 0 Id µ k-1|k-1 kR 0 0 µ k-1|k-1 Y (k+1) k 0 0 Id exp ∧ G (δ kR ) µ k-1|k-1 kR 0 0 exp ∧ G k-1 (δ) µ k-1|k-1 -1 =log ∨ G k Y (k+1) k 0 0 Id µ k-1|k-1 kR 0 0 µ k-1|k-1 Y (k+1) k 0 0 Id exp ∧ G k δ kR δ µ k-1|k-1 kR 0 0 µ k-1|k-1 -1 =log ∨ G k Y (k+1) k 0 0 Id exp ∧ G k - δ kR δ Y -1 (k+1) k 0 0 Id =log ∨ G k Y (k+1) k 0 0 Id exp ∧ G k - 0 Id 0 Id δ Y -1 (k+1) k 0 0 Id = - Ad G Y (k+1) k 0 0 Id 0 0 Id δ (81)
where we introduced the adjoint representation Ad G (•) ⊂ R q×q of G on R q that enables us to transform an increment i ij ∈ R q , that acts onto an element Y ij through left multiplication, into an increment j ij ∈ R q , that acts through right multiplication:

exp ∧ G i ij Y ij = Y ij exp ∧ G Ad G Y -1 ij i ij (82)
Consequently, from (81), we obtain: From ( 48) and (73), we have:

F k = - dlog ∨ G k f µ k-1|k-1 f exp ∧ G k-1 (
log ∨ G Z k h exp ∧ G (δ) X l -1 =log ∨ G Y i (k+1) exp ∧ G δ (k+1)R X l (k+1)R X l -1 iR exp ∧ G (-δ iR ) =log ∨ G Y i (k+1) X l (k+1)R X l -1 iR exp ∧ G Ad G X l iR X l -1 (k+1)R δ (k+1)R exp ∧ G (-δ iR ) log ∨ G Y i (k+1) X l (k+1)R X l -1 iR -δ iR + Ad G X l iR X l -1 (k+1)R δ (k+1)R (84)
where we approximated ϕ (•) (see [START_REF] Björck | Numerical methods for least squares problems[END_REF]) by Id.

Consequently, from (84), we obtain:

H l = - dlog ∨ G Z k h exp ∧ G (δ) X l -1 dδ δ=0 0 Id 0 -Ad G X l iR X l -1 (k+1)R 0 (85)

∧G

  : R P → g. For example let a ∈ g ⊂ R n×n , then we have [a] ∨ G = a ∈ R P . Thus we can define a basis [E i ] ∨ G = e i such that {e i } i=1...p is the natural basis of R P and a = p i=1 a i E i with a = (a 1 . . . a p )

Fig. 2 :

 2 Fig. 2: Illustration of a concentrated Gaussian distribution on SE (2). An arrow represents the position and the orientation of a robot in a 2D plane.

Fig. 3 :

 3 Fig. 3: Illustration of the relative motion averaging problem: dashed observations (Y ij with j > i + 1) are used as measurements during the LG-IEKF update step. Temporally consecutive observations (Y i (i+1) for i = 1...N -1) are used as control inputs during the LG-IEKF prediction step in order to guide the estimation process.

  Lie Algebra (Agrawal et al.) GN Non Invariant(Govindu)

Fig. 6 :

 6 Fig.6: Camera pose registration problem results, a cone represents a camera pose, a black line is an inlier measurement and a gray dashed line is an outlier

Fig. 7 :

 7 Fig.7: Wearable camera experiment: in the labeling matrices a white pixel corresponds to an inlier, a black pixel corresponds to an unavailable measurement, a gray pixel corresponds to an outlier. In the results, a cone represents the pose of a camera. The first camera position of the video sequence is dark red and the last camera position is bright red.

  Ground Truth : (left) labeling matrix manually annotated, (right) scene overview [55] : (left) labeling matrix, (right) mosaic of input images This paper : (left) labeling matrix, (right) mosaic of input images

B. 2

 2 δ) µ k-1|k-1 Derivation of H l

For several Lie groups of interest, such as SO(3), SE(3), Sim(3), analytical expressions of exp ∧ G (•) and log ∨ G (•) exist[START_REF] Selig | Lie groups and Lie algebras in robotics[END_REF]. However, for SL(3) for example, matrix exponential and logarithm have to be computed.

A closed form expression of ϕ G (b) was recently derived for SE(3) in[START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF].

In this paper, we consider quantities that are invariant to the right action of the Lie group on itself. Similar results could be obtained by considering the left action, leading to a left concentrated Gaussian distribution on G, which is the modelization used for instance in[START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF].

The Matlab code is available at https://sites.google.com/site/guillaumebourmaud/

The research leading to these results has received funding from the European Community's Seventh Framework Programme (FP7/2007-2013) under grant agreement 288199 -Dem@Care

where we introduced the adjoint representation Ad G (•) ⊂ R q×q of G on R q that enables us to transform an increment i ij ∈ R q , that acts onto an element Y ij through left multiplication, into an increment j ij ∈ R q , that acts through right multiplication:

(75)

Application of the LG-IEKF update step

In order to apply the LG-IEKF update step to (73), we need to compute H l (see eq.( 48)). We obtain (see Appendix B.2):

In our implementation, ϕ l and Φ l are approximated by the identity matrix. Also, since we consider the case where the measurements are outlier free, the inlier test (Step 2 in Alg.1) is skipped.