Je Remercie

Hisham Abou-Kandil

Gabriel Peyré

Tarek Hamel

Jean-Jacques Slotine

Karine Beauchard

Pierre Rouchon

Hillion Mathieu

Je tiens à remercier tout particulièrement mon directeur de thèse Pierre Rouchon pour la confiance qu'il m'a faite en me confiant ces travaux de thèse, et pour son attention bienveillante au cours de ces trois années. Cela a été un honneur pour moi de pouvoir travailler à ses côtés et de bénéficier de son recul scientifique.

Je remercie Karine Beauchard pour son enthousiasme et sa réactivité pour répondre avec justesse et compétence à mes diverses questions. J'ai beaucoup apprécié nos séances de travail et nos nombreuses discussions, qu'elles soient d'ordre mathématique ou non.

Je remercie les membres du CAS, en particulier Philippe Martin et Laurent Praly, pour leurs conseils éclairés et leur gentillesse.

Je remercie Jean-Jacques Slotine, venu de loin pour participer au jury de ma thèse, pour m'avoir donné à Boston l'idée et l'envie de faire cette thèse au CAS.

Je remercie mon employeur la DGA pour m'avoir permis de réaliser cette formation par la recherche; je remercie en particulier ma future équipe qui s'est avant même mon arrivée intéressée à mon travail.

Je remercie Sysnav, qui a mis à ma disposition les moyens expérimentaux qui m'ont permis de valider mes travaux théoriques et bien plus encore. J'y ai trouvé une équipe soudée, passionnée par la technique et toujours prête à partager cet enthousiasme, dont les lumières m'ont été précieuses dans le domaine de la navigation inertielle. Je remercie en particulier Mathieu Hillion pour sa constante disponibilité pour répondre à mes questions, Pierre-Jean, Eric et Quentin pour avoir toujours été prêts à m'apporter de l'aide, et Georges pour ses anecdotes et sa longue expérience en navigation.

Je remercie les stagiaires que j'ai eu le plaisir d'encadrer pendant ma thèse, Mian, Rémi et surtout David pour sa gentillesse et sa persévérance.

Enfin, je remercie ma famille: mes parents qui m'ont appris à mener jusqu'au bout mes entreprises, mes soeurs qui m'ont aidée à ne pas les prendre trop au sérieux, et David qui m'a redonné la motivation et la confiance en moi qui parfois me manquaient pour surmonter les difficultés rencontrées.

Augmented reality: the fusion of vision and navigation.

Résumé

Cette thèse a pour objet l'étude d'algorithmes pour des applications de réalité visuellement augmentée. Plusieurs besoins existent pour de telles applications, qui sont traités en tenant compte de la contrainte d'indistinguabilité de la profondeur et du mouvement linéaire dans le cas de l'utilisation de systèmes monoculaires. Pour insérer en temps réel de manière réaliste des objets virtuels dans des images acquises dans un environnement arbitraire et inconnu, il est non seulement nécessaire d'avoir une perception 3D de cet environnement à chaque instant, mais également d'y localiser précisément la caméra. Pour le premier besoin, on fait l'hypothèse d'une dynamique de la caméra connue, pour le second on suppose que la profondeur est donnée en entrée: ces deux hypothèses sont réalisables en pratique. Les deux problèmes sont posés dans le contexte d'un modèle de caméra sphérique, ce qui permet d'obtenir des équations de mouvement invariantes par rotation pour l'intensité lumineuse comme pour la profondeur. L'observabilité théorique de ces problèmes est étudiée à l'aide d'outils de géométrie différentielle sur la sphère unité Riemanienne. Une implémentation pratique est présentée: les résultats expérimentaux montrent qu'il est possible de localiser une caméra dans un environnement inconnu tout en cartographiant précisément cet environnement.

Introduction

Augmented reality

The emergence of mobile phones since the years 2000, and smartphones in the last few years led to an increasing demand of the public to quick, easy and wide access to personal as well as general information in any situation of everyday's life. For example, it is nowadays a basic requirement to be able to precisely locate oneself wherever in the world, even although the Global Positioning System (GPS), based on radio-frequency signals, is well known to be highly sensitive (among others) to the relative positions of the satellite constellation with respect to the user ( [START_REF] Kaplan | Understanding GPS: principles and applications[END_REF]). Real-time localization is only one example of augmented reality, which can be defined as the addition of intangible information to our physical perception of the world: this can be achieved through sound, vision or even touch ( [Azuma et al., 1997]). Numerous applications have been reached by augmented reality: a few examples are urban navigation [START_REF] Graham | Augmented reality in urban places: Contested content and the duplicity of code[END_REF]), learning [START_REF] Gavish | Evaluating virtual reality and augmented reality training for industrial maintenance and assembly tasks[END_REF]), surgery or medical applications in general ( [START_REF] Speidel | Robust feature tracking for endoscopic pose estimation and structure recovery[END_REF] or [Mousavi Hondori et al., 2013]), 3D modeling ( [Brenner, 2005], [START_REF] Yang | Image-based 3D scene reconstruction and exploration in augmented reality[END_REF] or [START_REF] Newcombe | Live dense reconstruction with a single moving camera[END_REF]), robots teaching and learning ( [START_REF] Borrero | Interaction of real robots with virtual scenarios through augmented reality: Application to robotics teaching/learning by means of remote labs[END_REF]), manufacturing ( [START_REF] Ong | Augmented reality applications in manufacturing: A survey[END_REF]), archeology [START_REF] Girbacia | Virtual restoration of deteriorated religious heritage objects using augmented reality technologies[END_REF]), fine arts ( [START_REF] Bovcon | Atlas 2012" augmented reality: A case study in the domain of fine arts[END_REF]), gesture training ([de Sorbier et al., 2012]), astrophysics [START_REF] Vogt | Augmented reality in astrophysics[END_REF]); but also virtual interior design [START_REF] Han | Mixed reality system for virtual interior design[END_REF]), fashion [START_REF] Ruzanka | There's an app for that shirt! evaluation of augmented reality tracking methods on deformable surfaces for fashion design[END_REF]), tourism [START_REF] Lin | An android-based interactive museum exhibit system using wireless sensing and augmented reality technologies[END_REF]), advertisement [START_REF] Shiva | Augmented reality: The future tense of 3D advertisement[END_REF]), e-commerce ( [START_REF] Li | Research and application of online product display technology based on augmented reality[END_REF]) and, last but not least, gaming [START_REF] Santoso | ARobot: Development of 3rd person shooting game and handheld augmented reality[END_REF] or [START_REF] Woun | Deployment of augmented reality interactions in games[END_REF]).

The most natural and appealing way to augment reality is by the mean of visual addition, which yields some requirements, of software as well as of hardware types. The obvious hardware requirements are a capture (e.g a video camera, now available on most mobile phones [START_REF] Khan | Mobile phone sensing systems: A survey[END_REF]), and a display technology: the later has become the last fashion trend with the overwhelming (but still unavailable for the large public at the time of writing) Google Glass product ( [Edwards, 2013] or [Ackerman, 2013]). The hidden hardware technology is the processing capability, which is driven by the software complexity. As for the software part, the possibility to add virtual objects to a real image in a realistic way requires the ability to localize the camera with respect to the visible environment, to inlay a 3D model of the object in the physical world, and to project this 3D view in the image. The first part of these requirements is generally solved by Simultaneous Localization and Mapping (SLAM) algorithms: from the visual input (from one or multiple cameras), which can be aided by inertial inputs present in most portable technologies in the form of Micro-Electro-Mechanical Systems (MEMS) sensors, the camera is localized with respect to a 3D model of the environment. This model can be a complete input, for example a 3D model of the city ( [START_REF] Lothe | Real-time vehicle global localisation with a single camera in dense urban areas: Exploitation of coarse 3D city models[END_REF]); a partial input, e.g fiducial known markers ( [START_REF] Lim | Real-time single camera SLAM using fiducial markers[END_REF]); or simply a real-time reconstruction of the environment without any input, which is the domain of application of Structure from Motion (SfM) algorithms ( [START_REF] Wei | Applications of structure from motion: A survey[END_REF]). As for the second part of the software requirements, it leads to the necessity of a dense 3D modeling of the environment (at the very least a current camera-centered model) to be able to handle occlusions of the virtual object with the real world ( [Kahn, 2013] or [START_REF] Zollmann | Dense depth maps from sparse models and image coherence for augmented reality[END_REF]).

Dense 3D reconstruction of the environment

Environment reconstruction is tightly related to the SLAM problem [START_REF] Smith | Estimating uncertain spatial relationships in robotics[END_REF], which can be addressed by nonlinear filtering of observed key feature locations (e.g. [START_REF] Civera | Inverse depth parametrization for monocular SLAM[END_REF], [START_REF] Montemerlo | FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[END_REF]), or by bundle adjustment [START_REF] Strasdat | Real-time monocular SLAM: Why filter? In ICRA[END_REF], Strasdat et al., 2010a]). The problem of designing an observer to estimate the depth of single or isolated keypoints has raised a lot of interest, specifically in the case where the relative motion of the carrier is described by constant known ( [START_REF] Abdursul | Nonlinear observers for perspective time-invariant linear systems[END_REF], Dahl et al., 2010]), constant unknown [START_REF] Heyden | Provably convergent on-line structure and motion estimation for perspective systems[END_REF], or time varying known ( [START_REF] Dixon | Range identification for perspective vision systems[END_REF], Karagiannis and Astolfi, 2005, Luca et al., 2008, Sassano et al., 2010]) affine dynamics. From a different perspective, the seminal paper of [START_REF] Matthies | Kalman filter-based algorithms for estimating depth from image sequences[END_REF]] performs incremental depth field refining for the whole field of view via iconic (pixel-wise) linear Extended Kalman filtering. However, estimating a sparse point cloud is insufficient for augmented reality applications, and none of these methods provide an accurate dense depth estimation in a general setting concerning the environment and the camera dynamics.

Motion estimation

For the specific problem of motion estimation and localization, the nature and the quality of the available measurements are critical, and delimit the reachable performances. In the 1950s, expensive Inertial Measurement Units (IMUs) were developed, as missile guidance and control required extremely accurate navigation data [START_REF] Bezick | Inertial navigation for guided missile systems[END_REF], Chatfield, 1997, Faurre, 1971]. Tactical grade IMUs, less expensive, enable dead-reckoning techniques over short time periods, but require position fixes provided by GPS [START_REF] Abbott | Land-vehicle navigation using GPS[END_REF], or combination through data fusion of other sensors outputs [START_REF] Skog | [END_REF]Handel, 2009, Vissière et al., 2007]. As to recent low-cost IMUs using MEMS technologies, the cumulated error due to the bias of gyroscopes integrated over long time periods induces drift in orientation, which can be bounded. However, from biased accelerometers, only high frequency output can be relied on, since the double integration induces a position error quadratically growing in time; even more severe errors can be induced when accelerometers are used in the frame of attitude estimation. As odometers and velocimeters (e.g. Doppler radar [START_REF] Uliana | The navigation system of an autonomous underwater vehicle for antarctic exploration[END_REF], Pitot tube, Electromagnetic (EM) log sensor), are commonly available technologies in vehicles, mass market applications can combine their linear velocity outputs with angular velocity from low-quality IMUs. Unfortunately, Pitot probes and EM log sensors are known to only provide airspeed and Speed through the water (STW) instead of Speed over ground (SOG): real-time or beforehand bias estimation is necessary to make the measurements from these sensors exploitable. In a different category, the Kinect device has been a huge outbreak in the robotics and vision communities as it provides depth measurements registered at each pixel of a RGB image, at a relatively low cost: this output is named RGBD (Red Green Blue and Depth: refers to the aligned pair of an usual color image and a depth image (RGBD)). This sensor trivially solves the ambiguity issue in ego-motion estimation by a monocular system: while motion is generally estimated up to as scale factor (for example by homography estimation [START_REF] Hamel | Homography estimation on the special linear group based on direct point correspondence[END_REF]), RGBD data provide the input required to solve the SfM problem without resorting to stereo vision techniques [Pollefeys et al., 2004, Zhang andFaugeras, 1992].

Layout of the dissertation

Part I is dedicated to the study of real-time dense 3D reconstruction under the assumptions of known camera motion and known projection model for the on board monocular camera. These assumptions are motivated by the existence of a novel system developed by Sysnav company, providing linear and angular velocity of a system obtained by processing of measurements from gyrometers, accelerometers and magnetometers (see [Vissiere, 2008, Dorveaux, 2011] for a complete description and the historical development of this novel technique, and [START_REF] Vissière | Système fournissant la vitesse et la position d' un corps en utilisant les variations du champ magnétique évaluées grâce aux mesures de un ou des magnétomètres et de une ou des centrales inertielles[END_REF] for a technical patented description). 1 We propose in this part a new estimation method for dense depth data, from known camera dynamics, namely six degrees of freedom (6-DOF) inputs. This method is based on a formulation of the optical flow constraint deriving from the spherical camera model introduced in [START_REF] Bonnabel | Fusion of inertial and visual : a geometrical observer-based approach[END_REF], where the camera dynamics is made explicit. This model is described in Chapter 1 and highlighted by a comparison with other camera models. The principal assumptions and notations are also introduced in this chapter. Variational methods applied to this constraint enable dense depth field estimation (Chapter 2). In addition, a so-called geometrical flow constraint can be exploited to design a non-linear observer based on optical flow or depth measurements (Chapter 3). These results are based on [Zarrouati et al., 2012a[START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF][START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF], and meet the needs of augmented reality for a real-time dense 3D perception of the visible part of the environment.

The requirement of visual augmented reality concerning the localization of the carrier with respect to its environment is addressed in Part II. We intend in this part to exploit simultaneously image and dense depth data, as provided by the Kinect device, for motion estimation. To bypass the nonlinearities of the system dynamics which usually force to study specific geometrical formulations (e.g the essential space [START_REF] Soatto | Motion estimation via dynamic vision[END_REF], the Plücker coordinates [START_REF] Mahony | Image-based visual servo control of aerial robotic systems using linear image features[END_REF]), we lean on the spherical camera model and motion equations described in Chapter 1. In a first approach (Chapter 5), biased angular and linear velocity measurements are supposed to be available. Our work focuses on biases estimation: an important part of the study consists in determining the conditions of observability of such biases. These results appear in [START_REF] Zarrouati-Vissiere | Rotational and translational bias estimation based on depth and image measurements[END_REF] and have been completed by [START_REF] Zarrouati-Vissière | Inertial-sensor bias estimation from brightness/depth images and based on SO(3)-invariant integro/partial-differential equations on the unit sphere[END_REF]. Next in Chapter 6, a novel odometry method based on the processing of dense depth images like the ones provided by the Kinect device is described. This method is named geometrical flow velocity estimation. This work has been the subject of a filed patent application [START_REF] Zarrouati-Vissière | Mesure de mouvement par traitement de la profondeur[END_REF]. Finally, in Chapter 7, these geometrical flow velocity measurements are processed in addition to biased accelerometers, biased gyrometers and magnetometers to provide an estimation of acceleration and angular velocity biases, as well as a filtered estimate of the linear velocity.

In Part III, we describe a new implementation of a SLAM algorithm adapted for augmented reality. The proposed system includes magneto-inertial sensors (accelerometers, gyrometers and magnetometers), and a RGBD sensor. The algorithm is based on an implementation of Parallel Tracking and Mapping algorithm (PTAM), which was adapted to process RGBD and magnetoinertial data instead of the sole monocular RGB data. The vision algorithm, based on pose optimization and parallel bundle adjustment to optimize the map, is made more robust and scaled by the constraints of depth data. In addition, the fusion of visual position, inertial motion and geometrical flow velocity measurements enable a precise continuous localization. RGBD Red Green Blue and Depth: refers to the aligned pair of an usual color image and a depth image.

SBI Small Blurry Image.

SfM Structure from Motion.

SLAM Simultaneous Localization and Mapping.

SOG Speed over ground.

STW Speed through the water.

Part I

From motion to depth

Du mouvement à la profondeur

Chapter 1

Camera models and motion equations

Formation des images et influence du mouvement

La définition la plus générale d'une caméra pourrait être : un système par lequel le monde tridimensionnel est projeté sur une surface bidimensionnelle. La difficulté de la définition réside dans l'estimation de l'opérateur de projection : pendant des siècles, le principal objectif de l'Optique a été d'établir des modèles traduisant la formation d'images ; dans les dernières décades, les scientifiques se sont concentrés sur le problème de calibration de caméras par rapport à un modèle. Dans ce chapitre, différents modèles de caméras sont décrits, du plus simple et largement utilisé sténopé à des modèles plus élaborés. Le modèle sphérique, adopté dans les Parties I et II, peut être considéré comme un modèle unificateur. Les équations de mouvement associées à ce modèle sont explicitées.

The most general definition of a camera system could be: a system projecting the threedimensional world onto a two-dimensional surface. However, the estimation of the projection operator for any system is far from obvious: for many centuries, it has been the main purpose of optics to establish models representing image formation; in the past decades, scientists focused on the problem of calibration of systems according to a model. In this chapter, different camera models are described, from the simplest and most used pinhole camera model to more sophisticated ones. The spherical model, adopted in Parts I and II can be seen as a unifying model. The motion equations satisfied by such a spherical camera are then developed.

Camera models

Pinhole model

The pinhole camera model is the simplest as well as the oldest camera model, as it derives from the camera obscura (the latin for "darkened room"). Known by Aristotle and Kepler [Lefèvre, 2007] to observe stars and sun eclipses, its lightest implementation consists of a dark box with a small aperture (a pinhole) on one of the sides. It enabled the discovery that light travels across straight lines since an inverted image of the surrounding environment is projected onto the back side of the box. More complex devices including mirrors, lenses or translucent screens can rectify the image. Apart from its scientific interest, it used to be an entertainment object at a time where people were unfamiliar with two-dimensional views of the world. In addition, although there is no documentary evidence of this idea apart from the paintings themselves ( [Gaskell, 1998]), it is widely admitted that Dutch painters such as Vermeer used camerae obscurae as an aid to render unusual perspective views (Figure 1.1).

Figure 1.1: Woman with a pearl necklace, Johannes Vermeer, Gemäldegalerie, Berlin. The pinhole camera model is a schematic representation of the camera obscura: the image is the projection of the world onto a plane (the focal plane) located in front of the optical center of the system (the image in that case is virtual and not inverted). The projection is central, so that every light ray travels across the optical center (Figure 1.2). Let us define a direct frame of reference R cam attached to the camera and centered at the the optical center C: the optical axis (or principal axis), orthogonal to the image plane, is the z-axis, and the x and y axis have horizontal and vertical orientations, pointing in the right and down directions respectively. Let us consider a punctual object, with coordinates (x, y, z) with respect to the camera frame R cam . According to the pinhole camera model, the image of this object is located at the intersection of the image plane (the equation of this plane is z = f , where f is the focal length) and on the line (CX). The relation between the object coordinates and the image coordinates, both expressed in R cam , is:

(x, y, z) T ↦ f ( x z , y z ) T . (1.1)
From this relation, the perspective phenomenon can be seen in different ways: first, different punctual objects aligned on the same light ray are all projected on the same punctual image; secondly, the closer extended objects are from the camera center, the bigger they appear on the image; thirdly, the relation between points and image coordinates is non-linear. These considerations led to the introduction of the so-called homogeneous coordinates, consistent with the perspective projection: since all objects on a line that crosses the optical center are equivalent by perspective, their homogeneous coordinates are equal. The homogeneous coordinates of a point X in 3-D space is a vector with 4 components, denoted X = (x 1 , x 2 , x 3 , x 4 ) T . Consistency with perspective yields X ∼ λ X for any non-zero multiplicative factor λ if X is expressed in R cam . More precisely, X is the homogeneous representation of X as soon as x 4 ≠ 0 and

x = x 1 x 4 , y = x 2
x 4 and z = x 3 x 4 ;

the simplest homogeneous representation of X is (x, y, z, 1) T . Homogeneous coordinates are also possible for 2D-points. These coordinates enable to write the non-linear relation (1.1) as matrix multiplication:

X = ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠ ↦ x = ⎛ ⎜ ⎝ f x f y z ⎞ ⎟ ⎠ = ⎛ ⎜ ⎝ f 0 0 0 0 f 0 0 0 0 1 0 ⎞ ⎟ ⎠ ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠ = P X (1.2)
where P is the matrix projection. This basic projection model can be improved and augmented in numerous ways to better represent image formation in a real camera: this is the purpose of camera calibration. A brief summary of possible extensions is given in the following. Details can be found in the excellent textbook [START_REF] Hartley | Multiple view geometry in computer vision[END_REF] (chapters 4 and 6); a good (french) review is given in [Boutteau, 2010]; issues about (self-)calibration are addressed in [Pollefeys, 1999]; methods and ready-to-use algorithms for camera calibration were developped by Zhengyou Zhang [Zhang, 2000, Zhang, 2001].

Intrinsic calibration

The intrinsic calibration of a camera is the relation between the coordinates of a 3D-point X and its 2D projection x, both expressed in the camera reference frame R cam .

-Principal point offset In equation (1.2), it is assumed that R cam is centered at the optical center of the camera, so that the principal point (the intersection between the image plane and the principal axis) is the origin of coordinates in the image plane. In practice, pixels are labeled with positive integer values: the origin of coordinates is located in a corner of the frame (usually the upper-left corner to be consistent with the camera frame definition). Let us denote p x and p y the coordinates of the principal point: the projection relation is now

X = ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠ ↦ x = ⎛ ⎜ ⎝ f x + zp x f y + zp y z ⎞ ⎟ ⎠ = ⎛ ⎜ ⎝ f 0 p x 0 0 f p y 0 0 0 1 0 ⎞ ⎟ ⎠ ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠
.

(1.3) -Pixel coordinates To express the image coordinates in pixels, instead of meters, let us introduce m x and m y the number of pixels per meter in the horizontal and vertical directions, respectively: in particular cases, m x ≠ m y which results in rectangular pixels; but in most cases, pixels are square (m x = m y ). This yields

X = ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠ ↦ xp = ⎛ ⎜ ⎝ m x (f x + zp x ) m y (f y + zp y ) z ⎞ ⎟ ⎠ = ⎛ ⎜ ⎝ α x s x 0 0 0 α y y 0 0 0 0 1 0 ⎞ ⎟ ⎠ ⎛ ⎜ ⎜ ⎜ ⎝ x y z 1 ⎞ ⎟ ⎟ ⎟ ⎠ (1.4)
where α x ∶= m x f and α y ∶= m y f , are the focal lengths expressed in pixels, and x 0 ∶= m x p x and y 0 ∶= m y p y are the principal point coordinates expressed in pixels. s is the skew parameter: it describes the orthogonality default of horizontal and vertical axis. Since in practice, internal calibrations can only be performed using pixel coordinates, α x , α y , x 0 and y 0 are the values actually estimated, and then f , p x,y , m x,y are deduced from the previous relations. Other parameters are useful when performing an internal calibration: for example, resolution (the number of pixels actually seen in each direction, 640 by 480 for a VGA format), sensor size or field-of-view (denoted FOV, the angular section actually seen in each direction) are very meaningful in the choice of a camera objective. Simple relations exist between these parameters (for example, F OV = 2 tan ( resolution 2αx )

-1 ), and various combinations of these parameters can be used to express the projection matrix P . -Distortion In real cameras, perspective is not the only cause of non-linearity: low-cost lenses and optical devices can result in distortions. Distortions can be defined as an additional transformation applied to the "ideal" (undistorted) image coordinates xu as expressed in (1.2) (the principal point is the origin of coordinates). The radial distortion function R maps the ideal radius r u = x 2 u + y 2 u onto the real (distorted) radius r d of image coordinates xd : x 2 d + y 2 d = r d = R(r u ). Given the coordinates xu and the mapping R, the distorted image coordinates are

x d = x u R(r u ) r u , y d = y u R(r u ) r u .
(1.5)

Then, translation and conversion to pixels are applied similarly to (1.3) and (1.4).

Let us underline that the distortion center and the principal point may be different points. Details on distortion models and calibration procedures can be found in [START_REF] Devernay | Straight lines have to be straight[END_REF]. Let us cite two main radial distortion mappings:

1. Polynomial distortion model: the inverse mapping R -1 is a polynomial function of r d , whose degree is one of the parameters to estimate during calibration:

r u = R -1 (r d ) = r d (1 + κ 1 r 2 d + κ 2 r 4 d + . . . ).
The direct mapping R is computed solving a polynomial equation: as the closed-loop solution does not necessarily exist, numerical computations can be considered.

2. Fish-eye model: the direct mapping only depends on one parameter ω (corresponding to the field-of-view of the ideal lens):

r d = R(r u ) = 1 ω arctan(2r u tan( ω 2
)).

(1.6)

Extrinsic calibration

The extrinsic calibration of a camera is the relation between the coordinates of a 3D-point XW expressed in a fixed external reference frame (usually called the world and denoted R W ) and its 2D projection x expressed in the camera reference frame R cam . It depends on the (possibly varying) orientation and position of the camera with respect to the world. Let us introduce R(t) the rotation matrix representing the orientation of the camera frame R cam with respect to the world reference frame R W , and C(t) the position of the optical center of the camera with respect to R W . Then, the (inhomogeneous) coordinates X cam in R cam of a 3D-point with coordinates X W in R W are given by

X cam = R(t)(X W -C(t)).
This is linearly written in homogeneous coordinates as

Xcam = R(t) -R(t)C(t) 0 1×3 1 XW = K XW . (1.7)
Sometimes, the translation term -R(t)C(t) in the matrix K is simply replaced by T : one must always keep in mind that this corresponds indeed to the translation of the camera center with respect to a fixed reference frame, but expressed in the current camera frame. For a regular camera (pinhole model, no distortion considered) , the relation between the coordinates XW of a 3D point and its 2D projection x is simply the multiplication of the extrinsic and intrinsic calibration matrices (K and P respectively). The extrinsic matrix K is a member of the special Euclidean group SE(3) which is the set of rigid body transformations in three dimensions. More details on SE(3) and more generally on representation of 3D rigid-body kinematics can be found in Appendix C. When the acquisition system consists of a set of camera, as it happens for a stereo vision system (a stereo rig), extrinsic calibration can refer to the estimation of position and rotation of cameras with respect to each other. This is a crucial step in an algorithm that aims to reconstruct the geometry of a scene by triangulation of matched point of interests between stereoscopic views. In strict triangulation, the baseline (the distance between the optical centers of the cameras) is in a sense the unit of length of the entire map: it is applied as a scale factor in the 3D reconstruction process. Stereo Vision and its extension to SfM have been thoroughly studied as a rich field of Computer Vision: description and comparison of existing algorithms can be found in [Faugeras, 1993], [Szeliski, 2010], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF], and state-of-the-art applications are described in [Nistér et al., 2004, Zhang andFaugeras, 1992].

Other models

In the regular pinhole model, light rays travel across lenses: even when distortion is involved, only transmission is considered. Other optical devices, known as catadioptric systems, involve mirrors, which can be very useful to increase the field-of-view of the camera. Consequently, the effect of reflection on light rays must be included in the projection model of such a camera. Several classes of catadioptric systems exist, depending on the shape of the reflective systems: planar, conical, spherical, ellipsoidal, hyperboloidal and paraboloidal mirrors have been studied. For any of them, the parameters describing the overall geometry of the surface must satisfy the single effective viewpoint constraint: this constraint ensures that the camera system measures the intensity of light rays that all meet in a single point, also called the projection center. The expression of this constraint obviously depends on the class of the catadioptric system: it was first derived by Baker and Nayar in [START_REF] Baker | A theory of catadioptric image formation[END_REF]] and [START_REF] Baker | A theory of single-viewpoint catadioptric image formation[END_REF]. Then, Geyer showed that all classes of central catadioptric systems were equivalent in the sense that their projection mapping is isomorphic to projective mapping from the sphere to a plane with a projection center on the perpendicular to the plane ( [START_REF] Geyer | A unifying theory for central panoramic systems and practical implications[END_REF]). Recently, Barreto proposed a unifying geometric representation of central projection systems ( [Barreto, 2006]): it covers perspective, catadioptric camera models and includes distortion. In parallel, a general model for imaging systems (including multiple viewpoints systems) was studied ( [START_REF] Grossberg | A general imaging model and a method for finding its parameters[END_REF]). These developments suggest that image formation in a generic system is mainly governed by one (or many) viewpoint(s) and a surface of projection, which can be arbitrarily chosen. Once the generic projection model corresponding to these fixed parameters is known, the projection model (either linear or non-linear) of any specific camera, as long as this camera ensures uniqueness of an imaging point, is developed as a mapping between the generic and the specific model.

Spherical model: notations

The generic model chosen in the following (Parts I and II) is a spherical model: the projection surface is a unit three dimensional sphere (S 2 ) and the projection center its center, denoted by C(t) as previously. The coordinates of C(t) are expressed in the reference frame R W . Orientation of the camera is given by the quaternion q(t): any vector ς in the camera frame R cam corresponds to the vector qςq * in the reference frame R W using the identification of vectors as imaginary quaternions (see Appendix C.1.2). A pixel is labeled by the unit vector η in the camera frame: η belongs to the sphere S 2 and receives the brightness y(t, η). Thus at each time t, the image produced by the camera is described by the scalar field S 2 ∋ η ↦ y(t, η) ∈ R. The notations of this model are represented in Figure 1.3. In this model, no particular axis is preferred to another, as it is the case in the pinhole model where the optical axis, orthogonal to the image plane partially defines orientation and direction of the camera reference frame.

Motion equations

Spherical model: the camera in a fixed environment

The motion of the camera is given through the linear and angular velocities v(t) and ω(t) expressed in the camera reference frame R cam . Linear and angular velocities are related to the time derivatives of position and orientation through the motion equations (see Appendix C and [Radix, 1980])

q = 1 2 qω and Ċ(t) = qvq * . (1.8)
The model adopted for the scene is based on two main assumptions: -(H1): the scene is a closed, C 1 and convex surface Σ of R 3 , diffeomorphic to S 2 ; -(H2): the scene is a Lambertian surface; -(H3): the scene is static. Model and notations of a spherical camera in a static environment [Bonnabel andRouchon, 2009, Zarrouati et al., 2012c].

To be consistent with assumption (H1), the camera is inside the domain Ω ⊂ R 3 delimited by Σ = ∂Ω.

The first assumption implies that to a point M ∈ Σ corresponds one and only one camera pixel. At each time t, there is a bijection between the position of the pixel given by η ∈ S 2 and the point M ∈ Σ. Since the point M is labeled by s ∈ S 2 , this means that for each t, exist two mappings S 2 ∋ s ↦ η = φ(t, s) ∈ S 2 and S 2 ∋ η ↦ s = ψ(t, η) ∈ S 2 with φ(t, ψ(t, η)) ≡ η and ψ(t, φ(t, s)) ≡ s, for all η, s ∈ S 2 . To summarize we have: η = φ(t, s) and s = ψ(t, η)

(1.9)

where ψ(t, .) and φ(t, .) are diffeomorphisms of S 2 for every t > 0. Moreover, φ is implicitly defined by the relation

η = q(t) * → C(t)M (s) → C(t)M (s) q(t) (1.10)
The density of light emitted by a point M (s) ∈ Σ does not depend on the direction of emission: this results from the second assumption (H2) on the scene. In addition, it is independent of t (H3). This means that y(t, η) depends only on s: there exists a function y Σ (s) such that

y(t, η) = y Σ (ψ(t, η)).
(1.11)

In other words, ∂y ∂t s = 0.

(1.12)

The distance → C(t)M (s) between the optical center and the object seen in the direction η = φ(t, s) is denoted by D(t, η).

D(t, η) ∶= → C(t)M (ψ(t, η)) . (1.13)
Let us consider the time derivative of the distance D attached to a specific point M (s):

∂D ∂t s = ∂ → C(t)M (s) ∂t s = ∂ → C(t)M (s) ∂t s ⋅ → C(t)M (s) → C(t)M (s) = -qvq * ⋅ qηq * using (1.8) and (1.10) = -v ⋅ η.
(1.14)

The last relation holds since a dot product does not depend on the frame of reference in which it is computed. The meaning of equation (1.14) is simple: the distance between the camera and a specific point decreases as the camera moves towards that point.

On the other hand, the direction of observation η of a specific object M (s) in the camera frame evolves during the trajectory of the camera according to:

∂η ∂t s = ∂ ∂t ⎛ ⎝ q * → C(t)M (s) C(t)M (s) q ⎞ ⎠ s using (1.10) = q * ∂ ∂t ⎛ ⎝ → C(t)M (s) C(t)M (s) ⎞ ⎠ s q + η × ω using (1.8
) and properties of the quaternion product

= q * q - v D + η v ⋅ η D q * q + η × ω using (1.8) = η × (ω + 1 D η × v).
(1.15)

In a pure rotational motion (v = 0), this equation means that all objects move in the same way in the image: in other words, all pixels are relabeled with a different origin, which is consistent with the spherical geometry of the problem. If the trajectory of the camera is a translation (v ≠ 0), the objects move differently depending on their position with respect to the camera. Exactly as for a regular perspective camera, there is an intrinsic ambiguity in the projection of the trajectory of an object, due to the multiplicative term v D : a monocular camera system can not capture the scale factor of the perceived environment.

Tracking specific well-defined objects like we just did to obtain equations (1.12), (1.14) and (1.15), is somehow a Lagrangian approach. But these equations can be combined all together in a Eulerian approach, using chain rule differentiation: for any mapping

S 2 ∈ η ↦ h(t, η) ∈ R, ∂h ∂t s = ∂h ∂t η + ∂h ∂η t ⋅ ∂η ∂t s . ∂h ∂η t
is the gradient of h with respect to the Riemannian metric of S 2 (see Section A.2), denoted ∇h in the following. Applied to the intensity y and the depth D, it yields

∂y ∂t η = -∇y ⋅ (η × (ω + 1 D η × v)), (1.16) ∂D ∂t η = -∇D ⋅ (η × (ω + 1 D η × v)) -v ⋅ η, (1.17)
where ∂y ∂t η and ∂D ∂t η stand for partial derivatives of y and D with respect to t: they are denoted in the following ∂ t y and ∂ t D for sake of simplicity.

The first Partial Differential Equation (PDE) (1.16) is well-known in the literature, under a slightly different form, as the optical flow constraint (see [START_REF] Horn | Determining optical flow[END_REF] and Part I for more details on optical flow). The second PDE (1.17) was introduced in [START_REF] Bonnabel | Fusion of inertial and visual : a geometrical observer-based approach[END_REF]: the transport term is the same than in (1.16), but a second term breaks the ambiguity of the scale factor. This very interesting property enables to solve many issues encountered in image processing: this will be the main topic of Parts I and II. By analogy with the optical flow constraint (1.16), this equation (1.17) is named in the following geometrical flow constraint. Equations (1.16) and (1.17) are SO(3)-invariant: they remain unchanged by any rotation described by the quaternion σ and changing (η, ω, v) to (σησ * , σωσ * , σvσ * ).

Pinhole-adapted model

To use this model with camera data, one needs to write the invariant equations (1.16) and (1.17) with local coordinates on S 2 corresponding to a real camera model. We detail here the computations for a pinhole camera model, but as explained in Chapter 1, any single viewpoint model could be considered, as long as the mapping between a 3D point coordinates in the camera frame and its projection on the image plane is known. For the pinhole camera model, the pixel of coordinates (z 1 , z 2 ) corresponds to the unit vector η ∈ S 2 of coordinates in R 3 :

η = 1 1 + z 2 1 + z 2 2 ⎛ ⎜ ⎝ z 1 z 2 1 ⎞ ⎟ ⎠ .
The optical axis of the camera(z-axis in the notations introduced in Subsection 1.1.1) corresponds here to the direction z 3 . Directions 1 and 2 correspond respectively to the horizontal axis from left to right and to the vertical axis from top to bottom on the image frame (x-axis and y-axis respectively).

The gradients ∇y and ∇Γ must be expressed with respect to z 1 and z 2 . Let us detail this derivation for y. We use two properties of the Riemannian gradient:

-∇y is tangent to S 2 , thus ∇y ⋅ η = 0; -the differential dy corresponds on one hand to ∇y ⋅ dη and on the other hand to ∂y ∂z 1 dz 1 + ∂y ∂z 2 dz 2 . The computation of dη yields

dη = 1 (1 + z 2 1 + z 2 2 ) 3 2 ⎛ ⎜ ⎝ 1 + z 2 2 -z 1 z 2 -z 1 ⎞ ⎟ ⎠ dz 1 + 1 (1 + z 2 1 + z 2 2 ) 3 2 ⎛ ⎜ ⎝ -z 1 z 2 1 + z 2 1 -z 2 ⎞ ⎟ ⎠ dz 2 . (1.18)
Then, by identification of ∇y with a vector of R 3 expressed in the camera frame, one writes

∇y = ⎛ ⎜ ⎝ ∇y 1 ∇y 2 ∇y 3 ⎞ ⎟ ⎠ .
This yields for dy

∇y ⋅ dη = 1 (1 + z 2 1 + z 2 2 ) 3 2 [(∇y 1 (1 + z 2 2 ) -∇y 2 (z 1 z 2 ) -∇y 3 (z 1 ))dz 1 + (-∇y 1 (z 1 z 2 ) + ∇y 2 (1 + z 2 1 ) -∇y 3 (z 2 ))dz 2 ]. (1.19)
Then, ∇y ⋅ η = 0 yields ∇y 3 = -∇y 1 z 1 -∇y 2 z 2 .

(1.20)

Equation (1.20) is plugged in (1.19), then by identification of dy with ∂y ∂z 1 dz 1 + ∂y ∂z 2 dz 2 , one gets:

∇y = 1 + z 2 1 + z 2 2 ⎛ ⎜ ⎜ ⎝ ∂y ∂z 1 ∂y ∂z 2 -z 1 ∂y ∂z 1 -z 2 ∂y ∂z 2 ⎞ ⎟ ⎟ ⎠ . (1.21)
Similarly we get the three coordinates of ∇Γ. Injecting these expressions in (1.16) and (1.17) yields the following PDEs corresponding to (1.16) and (1.17) in local pinhole coordinates:

∂ t y = - ∂y ∂z 1 z 1 z 2 ω 1 -(1 + z 1 2 )ω 2 + z 2 ω 3 + 1 D 1 + z 2 1 + z 2 2 (-v 1 + z 1 v 3 ) - ∂y ∂z 2 (1 + z 2 2 )ω 1 -z 1 z 2 ω 2 -z 1 ω 3 + 1 D 1 + z 2 1 + z 2 2 (-v 2 + z 2 v 3 ) (1.22) ∂ t D = - ∂D ∂z 1 z 1 z 2 ω 1 -(1 + z 1 2 )ω 2 + z 2 ω 3 + 1 D 1 + z 2 1 + z 2 2 (-v 1 + z 1 v 3 ) - ∂D ∂z 2 (1 + z 2 2 )ω 1 -z 1 z 2 ω 2 -z 1 ω 3 + 1 D 1 + z 2 1 + z 2 2 (-v 2 + z 2 v 3 ) -(z 1 v 1 + z 2 v 2 + v 3 ) (1.23)
where v 1 , v 2 , v 3 , ω 1 , ω 2 , ω 3 are the components of linear and angular velocities in the camera frame.

Chapter 2

Formulation as a minimization problem

La profondeur, solution d'un problème inverse

Dans ce chapitre sont décrites les méthodes variationnelles et leurs applications à l'estimation du flot optique. Les plus connues de ces techniques d'estimation, la méthode d'Horn-Schunck et la méthode TV-L 1 , sont détaillées. De telles méthodes peuvent être adaptées à l'estimation de profondeur par la minimisation de notre contrainte de flot optique spécifique (1.16) plutôt que de la contrainte de flot optique classique : le mouvement apparaissant explicitement, le problème est bien posé au sens où, à chaque pixel de l'image, une équation scalaire est associée à l'unique inconnue du problème, la profondeur. Ces méthodes ont été présentées dans [Zarrouati et al., 2012a[START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF][START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

In this chapter, variational methods and their application to optical flow estimation are described. The most well-known, the Horn and Schunck and the TV-L 1 optical flow estimation methods are detailed. Such methods can be adapted to depth estimation by the minimization of our invariant optical flow constraint (1.16) where motion is made explicit, instead of the regular optical flow constraint: the problem is well-posed and geometrically constrained in the sense that at each pixel, one scalar equation is associated to the only unknown of the problem, the depth. These methods were presented in [Zarrouati et al., 2012a[START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF][START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

Variational methods applied to optical flow estimation

Optical flow

In [START_REF] Horn | Determining optical flow[END_REF]], Horn and Schunck described a method to compute the optical flow, defined as "the distribution of apparent velocities of movement of brightness patterns in an image". The entire method is based on the assumption that the light intensity emitted by a specific object does not depend on time or direction of observation (assumptions (H2) and (H3) of the Section 1.3.1 and the consequent equation (1.12)). Provided that the same physical point is observed in two different images y 1 and y 2 , the following equality holds:

y 1 (z 1 , z 2 ) = y 2 (z 1 + δz 1 , z 2 + δz 2 )
where z ∶= (z 1 , z 2 ) is the position of the point in the first image, and (δz 1 , δz 2 ) is the apparent displacement of the point between the two images. If the images are only separated by a short time interval δt, it is natural to introduce an apparent velocity vector (V 1 , V 2 ) ∶= 1 δt (δz 1 , δz 2 ). Extending this apparent velocity concept to the whole image yields a velocity field V (t, z) ∶= (V 1 , V 2 )(t, z 1 , z 2 ) named optical flow. First order differentiation in both time and space yields the so-called optical flow constraint:

∂y ∂t + V 1 ∂y ∂z 1 + V 2 ∂y ∂z 2 = 0. (2.1)
This equation can be identified with equation (1.22): an explicit expression of V 1 and V 2 with respect to velocity and depth is given in the following (see equations (2.10) and (2.11)).

Variational methods applied to inverse problems

An important field of image processing is devoted to image restoration: the object is to improve the quality of a given damaged image. The term "damage" covers a wide range of effects, from additional noise to missing pixel data or blur. The usual formulation of restoration is the following: one aims to reconstruct the ideal input y 0 given an output y (the image) related by y = Ky 0 + ν where ν designates noise and K is a linear operator that describes the "damaging" effect. For example, to restore -a noisy image: K is simply the identity (noise is already taken into account in ν); -a blurred image: K is the convolution with a Gaussian kernel; -an image where pixel data are missing: K is the characteristic function 1 S of a discrete set S. Variational methods (specifically variational minimization) apply to inverse problems as the solution is claimed to be the argument of the following minimized functional

y -Ky 0 + α 2 J(y)
where -J is a prior, in other words a weighting function of the image, -∫ y -Ky 0 is the data fidelity term, usually the sum over the whole rectangular frame occupied by the image of the residuals, and α 2 is a strictly positive parameter allowing to scale the influence of the prior term in the minimization task.

Many priors have been proposed in the recent years, and generally each one is very well adapted to a specific class of images. The most well known are the Sobolev prior and the total variation prior. The data fidelity term usually takes into play the Euclidean norm (L 2 ) but can be in some cases more sophisticated. For more details on variational minimization applied to inverse problems and the links between variational methods and PDEs methods, see [Peyré, 2013], [Grasmair, 2010],

or [START_REF] Guichard | Contrast invariant image analysis and PDEs[END_REF]. Variational methods have also been applied to other estimation problems, and in particular to optical flow estimation, for which two formulations are detailed in the following.

Variational method with Sobolev prior

For optical flow estimation, Horn and Schunck proposed the data fidelity term to be replaced by the optical flow constraint (2.1) in L 2 norm. The prior used for J is the Sobolev prior, defined as the L 2 norm of the gradient, applied to optical flow:

J(V 1 , V 2 ) = ∇V 1 2 L 2 + ∇V 2 2 L 2 .
This yields that the apparent velocity field V = (V 1 , V 2 ) is estimated for each time t by minimizing versus W ∶= (W 1 , W 2 ) the following cost (the image I is the rectangle of R 2 occupied by the image)

I(W ) = ∬ I ∂y ∂t + W 1 ∂y ∂z 1 + W 2 ∂y ∂z 2 2 + α 2 (∇W 1 2 + ∇W 2 2 ) dz 1 dz 2 (2.2)
where ∇ is the gradient operator in the Euclidean plane (z 1 , z 2 ) and the partial derivatives ∂y ∂t , ∂y ∂z 1 and ∂y ∂z 2 are assumed to be known. The Sobolev prior is a regularizing term, and α > 0 is a regularization parameter: when used for image restoration, it yields to a formulation where the solution of the minimization problem is the solution of the heat equation and can be explicitly deduced from the image y by convolution with a Gaussian kernel.

Notice that for optical flow estimation, the prior term is necessary to solve a fundamentally ill-posed problem where for each position in an image, two components of a vector are estimated from only one scalar equation. Here, the Sobolev prior comes from the assumption that the velocity field is only submitted to small spatial variations: this is a reasonable assumption in an environment where the depth D is continuous, regular and lower-bounded as shown by the following identification ((2.10) and (2.11)).

Such Horn-Schunk estimation of V at time t is denoted by

V HS (t, z) ∶= (V HS1 (t, z), V HS2 (t, z)).
For each time t, minimization of the cost I is computed using usual calculus of variation, as the velocity field solution V HS satisfies the system of Euler-Lagrange equations obtained as first-order stationary equations for I:

∂y ∂z 1 ∂y ∂t + ∂y ∂z 1 2 V HS1 + ∂y ∂z 1 ∂y ∂z 2 V HS2 -α 2 ∆V HS1 = 0 ∂y ∂z 2 ∂y ∂t + ∂y ∂z 1 ∂y ∂z 2 V HS1 + ∂y ∂z 2 2 V HS2 -α 2 ∆V HS2 = 0
with boundary conditions ∂V HS1 ∂n = ∂V HS2 ∂n = 0 (n the normal to ∂I). Here ∆ is the Laplacian operator in the Euclidean space (z 1 , z 2 ). The numerical resolution is usually based on 1. Computations of ∂y ∂z 1 , ∂y ∂z 2 and ∂y ∂t via differentiation filters (e.g. Sobel filtering) directly from the image data at different times around t. For example, ∂y ∂z 1 can be approximated using a central finite difference scheme (among others) by

∂y ∂z 1 (z 1 , z 2 , t) ≈ y(z 1 + δ, z 2 , t) -y(z 1 -δ, z 2 , t) 2δ ∶= Dy z 1 (z 1 , z 2 , t). (2.3)
To minimize the impact of noise on this differentiation scheme, a slight smoothing of the resulting derivative along the orthogonal direction (z 2 in this example) is required. Using the above definition (2.3) yields

∂y ∂z 1 (z 1 , z 2 , t) ≈ 1 4 (2Dy z 1 (z 1 , z 2 , t) + Dy z 1 (z 1 , z 2 -δ, t) + Dy z 1 (z 1 , z 2 -δ, t)) .
(2.4)

When δ is the length of the square pixels of the grid where image data are available, (2.4) can be expressed as a convolution operator: an approximation of ∂y ∂z 1 (z 1 , z 2 , t) is obtained by convolution of y with the following kernel:

1 8δ ⎛ ⎜ ⎝ 1, 0, -1 2, 0, -2 1, 0, -1 ⎞ ⎟ ⎠ .
2. Approximation of ∆V HS1 and ∆V HS2 by the difference between the weighted mean of V HS1 and V HS2 on the neighboring pixels and their values at the current pixel. The most common approximation of the discrete Laplacian ∆ D f of a function f (z 1 , z 2 ) is given by

∆ D f ≈ f (z 1 -δ, z 2 ) + f (z 1 + δ, z 2 ) + f (z 1 , z 2 -δ) + f (z 1 , z 2 + δ) -4f (z 1 , z 2 ) δ 2
in the case where f is known at every vertex of a square grid with length spacing δ. Thus, a fast computation of

∆ D f is fn-nf δ 2
(here n = 4) where fn is computed as the convolution of f with K n , here

K 4 = ⎛ ⎜ ⎝ 0, 1, 0 1, 0, 1 0, 1, 0 ⎞ ⎟ ⎠ .
This yields the following system of linear equations in (V HS1 , V HS2 ) at each pixel of the image:

⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ ∂y ∂z 1 2 + n α 2 δ 2 V HS1 + ∂y ∂z 1 ∂y ∂z 2 V HS2 = α 2 δ 2 (K n * V HS1 ) -∂y ∂z 1 ∂y ∂t ∂y ∂z 1 ∂y ∂z 2 V HS1 + ∂y ∂z 2 2 + n α 2 δ 2 V HS2 = α 2 δ 2 (K n * V HS2 ) -∂y ∂z 2 ∂y ∂t .
(2.5)

3. Iterative resolution (Jacobi scheme) of the linear system (2.5) in V HS1 and V HS2 .

Defining V HS1 = 1 n K n * V HS1 and V HS2 = 1 n K n * V HS2 , the inversion of (2.5) at each pixel writes ⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ ∂y ∂z 1 2 + ∂y ∂z 2 2 + n α 2 δ 2 V HS1 = n α 2 δ 2 + ∂y ∂z 2 2 V HS1 -∂y ∂z 1 ∂y ∂z 2 V HS2 -∂y ∂z 1 ∂y ∂t ∂y ∂z 1 2 + ∂y ∂z 2 2 + n α 2 δ 2 V HS2 = n α 2 δ 2 + ∂y ∂z 1 2 V HS2 -∂y ∂z 1 ∂y ∂z 2 V HS1 -∂y ∂z 2 ∂y ∂t .
(2.6) However, as V HS1 and V HS2 implicitly depend on V HS1 and V HS2 , the closed form solution of the problem (2.6) would require to invert a system of equations whose length is twice the size of an image, which is computationally not optimal. Alternatively, an iterative computation of the solution of this system appears, rewriting (2.6) as

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ V HS1 = V HS1 - ∂y ∂z 1 2 V HS1 + ∂y ∂z 1 ∂y ∂z 2 V HS2 + ∂y ∂z 1 ∂y ∂t ∂y ∂z 1 2 + ∂y ∂z 2 2 +n α 2 δ 2 = V 1 (V HS1 , V HS2 , y) V HS2 = V HS2 - ∂y ∂z 2 2 V HS2 + ∂y ∂z 1 ∂y ∂z 2 V HS1 + ∂y ∂z 2 ∂y ∂t ∂y ∂z 1 2 + ∂y ∂z 2 2 +n α 2 δ 2 = V 2 (V HS1 , V HS2 , y) . (2.7)
This form shows that V HS1 and V HS2 can be iteratively computed, applying the operators V 1 and V 2 to the result of the previous iteration. The convergence of this numerical method of resolution was proven in [Mitiche and reza Mansouri, 2004]. Three parameters have a direct impact on the speed of convergence and on the precision: the regularization parameter α, the number of iterations for the Jacobi scheme and the initial values of V HS1 and V HS2 at the beginning of this iteration step. To be specific, α should neither be too small in order to filter noise appearing in differentiation filters applied on y, nor too large in order to have V HS close to V when ∇V ≠ 0.

Variational method with total variation prior

In their seminal article [START_REF] Rudin | Nonlinear total variation based noise removal algorithms[END_REF], Rudin, Osher and Fatemi proposed for image denoising the total variation prior defined by the sum of the gradients in L 1 norm. Its main advantage resides in the fact that edges are better conserved: the smoothing/regularizing effect related to diffusion in the heat equation is attenuated. This prior enables the extension of variational methods to non-smooth functions, and is well-adapted to piecewise constant images (cartoon images). It can however sometimes lead to a staircase effect. The main issue with total variation prior comes from the non differentiability of this prior at point 0. A solution to this problem consists in the following approximation:

∇f ≈ ǫ 2 + ∇f 2
where ǫ is a small positive constant, which is differentiable. Another solution is based on dual formulation of the minimization task and was introduced by Chambolle in [Chambolle, 2004] along with a convergent iterative scheme summarized in the following. The solution of the minimization task min

y ∇y + β 2 y -y d 2
is given by

y = y d + 1 2β 2 ∇ ⋅ p (2.8)
where p = (p 1 , p 2 ) is the dual variable and ∇ ⋅ p denotes the Euclidean divergence of the vector field p. The dual variable is computed iteratively by

pn = p n-1 + 2β 2 τ ∇ y d + 1 2β 2 ∇ ⋅ p n-1
and then

p n = pn max(1, pn )
where p 0 = 0 and the time step τ ≤ 1 4.

When, in addition, the data fidelity term y -y d also involves L 1 norm, the method is known as TV-L 1 minimization, and a solution consists of dividing the problem in two minimization subtasks. An auxiliary variable y a is introduced, and the functional to minimize is approximated by

F β = ∇y + α 2 y a -y d + β 2 y -y a
where β is a small positive constant. Then, the minimization process of F β consists of alternating 1. min

y ∇y + β 2 y -y a 2
for which a solution is given by (2.8), and where ρ(y a ) designates the residuals associated to the auxiliary variable y a . The second step can be solved point-wise if the residual ρ(y a ) does not depend on spatial derivatives of y a . This method was applied to optical flow estimation in [START_REF] Chambolle | A first-order primal-dual algorithm for convex problems with applications to imaging[END_REF]] and [START_REF] Wedel | An improved algorithm for TV-L1 optical flow[END_REF]. In that case, the residual ρ(y) is simply the optical flow constraint, which is linear with respect to the optical flow components. This yields that the second step optimization is performed using regular optimization methods (e.g. Karush-Kuhn-Tucker conditions) combined with thresholding steps (see [START_REF] Wedel | Duality tv-l1 flow with fundamental matrix prior[END_REF]):

min

y a = y + ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ α 2 2β 2 ∇y if ρ(y) < -α 2 2β 2 ∇y 2 -α 2 2β 2 ∇y if ρ(y) > α 2 2β 2 ∇y 2 -ρ(y) ∇y ∇y 2 if ρ(y) ≤ α 2 2β 2 ∇y 2 .
(2.9)

Variational methods applied to depth estimation

We described in Chapter 1.3 a slightly different form of optical flow constraint, where linear and angular velocities appear explicitly and optical flow as a vector field is implicit. Expressing (1.16) in pinhole coordinates (see (1.22)) yields a precise identification of optical flow components with respect to velocities v and ω and depth D:

V 1 (t, z) = f 1 (z, ω(t)) + 1 D(t, z) g 1 (z, v(t)) V 2 (t, z) = f 2 (z, ω(t)) + 1 D(t, z) g 2 (z, v(t))
(2.10) with

f 1 (z, ω) ∶= z 1 z 2 ω 1 -(1 + z 1 2 )ω 2 + z 2 ω 3 g 1 (z, v) ∶= 1 + z 2 1 + z 2 2 (-v 1 + z 1 v 3 ) f 2 (z, ω) ∶= (1 + z 2 2 )ω 1 -z 1 z 2 ω 2 -z 1 ω 3 g 2 (z, v) ∶= 1 + z 2 1 + z 2 2 (-v 2 + z 2 v 3 ) (2.11)
where ω = (ω 1 , ω 2 , ω 3 ) T and v = (v 1 , v 2 , v 3 ) T are expressed in the camera reference frame R cam (see Section 1.3.2 for notations). Assuming that linear and angular velocities are known inputs, the structure of optical flow is well determined: in addition, it is linear with respect to Γ ∶= 1 D the inverse of depth. We will describe in the following how one can take advantage of this linear dependence and the knowledge of motion to adapt usual variational methods of optical flow estimation to depth estimation. Let us highlight the fact that depth only appears in the expressions (2.10) multiplied by the terms g 1 and g 2 which directly depend on the linear velocity: in others words, linear motion is necessary to estimate depth. In addition, the quality of depth estimation directly depends on the magnitude of linear velocity. This a well-known effect in stereo-vision where the choice of the baseline between the cameras is the result of a trade off between performance and practical considerations.

Estimation of depth with Sobolev priors

Instead of minimizing the cost I given by (2.2) with respect to the optical flow components W 1 and W 2 , let us define a new invariant cost J HS associated to our optical flow constraint (1.16),

J HS (Υ) = ∬ I ∂y ∂t + ∇y ⋅ (η × (ω + Υη × v)) 2 + α 2 ∇Υ 2 dσ η (2.12)
and minimize it with respect to any inverse depth profile I ∋ η ↦ Υ(t, η) ∈ R. The time t is fixed here and dσ η is the Riemannian infinitesimal surface element on S 2 . I ⊂ S 2 is the domain where y is measured and α > 0 is the regularization parameter.

The first order stationary condition of J HS with respect to any variation of Υ yields the following invariant PDE characterizing the resulting estimation Γ HS of Γ in pinhole cordinates:

α 2 ∆Γ HS = ∂y ∂t + ∇y ⋅ (η × (ω + Γ HS η × v)) (∇y ⋅ (η × (η × v))) on I (2.13) ∂Γ HS ∂n = 0 on ∂I (2.14)
where ∆Γ HS is the Laplacian of Γ HS on the Riemannian sphere S 2 and ∂I is the boundary of I, assumed to be piece-wise smooth and with unit normal vector n.

In pinhole coordinates, recall from Section 1.3.2 that

dσ η = 1 + z 2 1 + z 2 2 -3 2 dz 1 dz 2 ∇Υ 2 = 1 + z 2 1 + z 2 2 ∂Υ ∂z 1 2 + ∂Υ ∂z 2 2 + (z 1 ∂Υ ∂z 1 + z 2 ∂Υ ∂z 2 ) 2 ∂y ∂t + ∇y ⋅ (η × (ω + Υη × v)) 2 = (F + ΥG) 2
where

F ∶= ∂y ∂t + f 1 (z, ω) ∂y ∂z 1 + f 2 (z, ω) ∂y ∂z 2 G ∶= g 1 (z, v) ∂y ∂z 1 + g 2 (z, v) ∂y ∂z 2 .
(2.15)

Consequently, the first order stationary condition (2.13) reads in (z 1 , z 2 ) coordinates:

Γ HS G 2 + F G = α 2 (1 + z 2 1 + z 2 2 ) 3 2 ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ ∂ z 1 ⎛ ⎝ 1 + z 2 1 1 + z 2 1 + z 2 2 ∂Γ HS ∂z 1 ⎞ ⎠ + ∂ z 2 ⎛ ⎝ 1 + z 2 2 1 + z 2 1 + z 2 2 ∂Γ HS ∂z 2 ⎞ ⎠ + ∂ z 2 ⎛ ⎝ z 1 z 2 1 + z 2 1 + z 2 2 ∂Γ HS ∂z 1 ⎞ ⎠ + ∂ z 1 ⎛ ⎝ z 1 z 2 1 + z 2 1 + z 2 2 ∂Γ HS ∂z 2 ⎞ ⎠ ⎤ ⎥ ⎥ ⎥ ⎥ ⎦ (2.16) on the rectangular domain I ∶= [-z 1 , z1 ] × [-z 2 , z2 ] (z 1 , z2 > 0 with z2 1 + z2 2 < 1)
. The right term of (2.16) corresponds to the Laplacian operator on the Riemannian sphere S 2 multiplied by α 2 in pinhole coordinates by identification with (2.13). At the center of the image, it is equivalent to the Euclidean Laplacian, but as the direction of observation η moves towards the corners of the rectangular matrix of pixels, (the absolute value of pixel coordinates z 1 and z 2 increase), the correction due to the reprojection of the equation on S 2 back to a regular plane can not be neglected. As a numerical example, let us consider that the visible portion of the environment is a plane, orthogonal to the optical axis. The equation describing this plane is

Γ(z 1 , z 2 ) = Γ 0 1 + z 2 1 + z 2 2 .
The Euclidean Laplacian ∆ E Γ and the Riemannian Laplacian ∆ R Γ of this function write respectively

∆ E Γ(z 1 , z 2 ) = Γ 0 (z 2 1 + z 2 2 -2) (1 + z 2 1 + z 2 2 ) 5 2 , ∆ R Γ(z 1 , z 2 ) = - 2Γ 0 1 + z 2 1 + z 2 2 .
Notice that the Riemannian Laplacian ∆ R Γ can be computed by two different methods: first, using the identification of the Laplacian with the right term of the expression (2.16); secondly, using the differential calculus expression (A.4) applied to Γ(η) = Γ 0 η ⋅ (0, 0, 1) T . Figure 2.1 graphically represents the relative difference

∆ E Γ-∆ R Γ ∆ E Γ
over the whole image: the relative error reaches 200% in the corners in the image, for a camera with a total field of view of 60°by 50°(horizontally and vertically, respectively).

The numerical resolution of this scalar diffusion equation providing the estimation Γ HS of Γ is similar to the one used for the Horn-Schunck estimation V HS of V , except for the evaluation of the Laplacian, which is adapted to the spherical model.

Estimation of depth by total variation minimization

For total variation minimization as in subsection 2.1.4, both our optical flow constraint and the regularization term are taken into account in L 1 norm in the functional J T V -L 1 to minimize:

J T V -L 1 (Γ) = I α 2 ∂y ∂t + ∇y ⋅ (η × (ω + Γη × v)) + ∇Γ dσ η (2.17)
where dσ η is the Riemannian infinitesimal surface element on S 2 and α is a parameter that weights between the data fidelity and the regularization terms. As described for optical flow estimation, the minimization strategy used is divided in two tasks:

1. For Λ being fixed, find the argument Γ of the minimum of

J 1 (Γ) = I ∇Γ + β 2 (Γ -Λ) 2 dσ η (2.18)
2. For Γ being fixed, find the argument Λ of the minimum of

J 2 (Λ) = I α 2 ρ(Λ) + β 2 (Γ -Λ) 2 dσ η (2.19)
where ρ(Λ) = ∂y ∂t + ∇y ⋅ (η × (ω + Λη × v)) is the depth residual. The main algorithm steps are given by the following: -Initialization: Γ 0 = 0, Λ 0 = 0, Γ0 = 0 p 0 = 0, τ ≤ 1 4 Figure 2.1: Isolines of relative difference between the Riemannian and the Euclidean Laplacian of the depth field when the visible environment is a plane orthogonal to optical axis, for a total field of view of 60°by 50°(horizontally and vertically respectively).

-Iterations:

1.

p n+1 = p n + 2β 2 τ ∇ Γn 2. p n+1 = p n+1 max(1, p n+1 ) 3. Γ n+1 = Γ n + 1 2β 2 ∇ ⋅ p n+1 4. Λ n+1 = Λ n + ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ - ρ(Γ) G , if ρ(Γ) ≤ α 2 2β 2 G 2 α 2 2β 2 G, if ρ(Γ) < -α 2 2β 2 G 2 -α 2 2β 2 G, if ρ(Γ) > α 2 2β 2 G 2 5. Γn+1 = 2Λ n+1 -Λ n
where p = (p 1 , p 2 ) and G is defined as in (2.15). The first three steps of the iteration process result from the minimization of J 1 , the fourth step is the thresholding method for minimizing J 2 applied to our specific residual and the fifth step is a low-pass filter. More precisely, the fourth step is based on the first order stationary condition in the cases where ρ(Λ) = 0, ρ(Λ) < 0 and ρ(Λ) > 0, respectively. For example, when ρ(Λ) > 0, the minimum may only be reached if

Λ satisfies α 2 G -2β 2 (Γ -Λ) = 0 (2.20) which yields Λ = Γ -α 2 2β 2 G. Thus, ρ(Λ) > 0 reads ρ(Γ) > α 2 2β 2 G 2 .
Chapter 3

Reconstruction of the depth

Filtrage de la profondeur par la contrainte de flot géométrique

Dans ce chapitre, nous montrons comment notre contrainte de flot géométrique invariante (1.17) peut être mise à profit pour la création d'observateurs asymptotiques non-linéaires : les mesures de flot optique ou de profondeur ainsi que la dynamique de la caméra peuvent être utilisées comme entrées de l'observateur de manière à améliorer progressivement l'estimation de profondeur. La convergence de ces observateurs est étudiée, et repose sur des hypothèses géométriques sur l'environnement et la dynamique de la caméra. Ces observateurs et les preuves de convergence ont été présentés dans [Zarrouati et al., 2012a[START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

In this chapter, we show how our invariant geometrical flow constraint (1.17) can be of great benefit in the frame of an asymptotic non-linear observer: either optical flow or depth measurements can be used as inputs, along with camera dynamics, to incrementally refine depth estimation. Convergence of such observers, relying on geometric assumptions concerning the camera dynamics and the environment, is addressed. These observers and the proofs of convergence were presented in [Zarrouati et al., 2012a[START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

Asymptotic observer based on optical flow measures

From any optical flow estimation, such as V HS , it is reasonable to assume that we have access for each time t, to the components in pinhole coordinates of the vector field

̟ t ∶ S 2 ∋ η ↦ ̟ t (η) = η × (ω + 1 D η × v) ∈ T η
appearing in (1.17). T η is the tangent space of the unit sphere S 2 at point η (see Appendix A). This vector field can be considered as a measured output for (1.17), expressed as

̟ t (η) = f t (η) + 1 D(t,η) g t (η)
, where f t and g t are the vector fields

f t ∶ S 2 ∋ η ↦ f t (η) = η × ω ∈ T η (3.1)
g t ∶ S 2 ∋ η ↦ g t (η) = η × (η × v) ∈ T η . (3.2) Since D obeys to ∂ t D = -∇D ⋅ ̟ t -v ⋅ η,
we propose the following asymptotic observer for D, where D denotes the estimation of D:

∂ D ∂t = -∇ D ⋅ ̟ t -v ⋅ η + kg t ⋅ ( Df t + g t -D̟ t ) (3.3)
where ̟ t , f t and g t are known time-varying vector fields on S 2 and k > 0 is a tuning parameter. This observer is trivially SO(3) invariant and reads in pinhole coordinates:

∂ D ∂t = - ∂ D ∂z 1 V 1 - ∂ D ∂z 2 V 2 -(z 1 v 1 + z 2 v 2 + v 3 ) + k g 1 ( Df 1 + g 1 -DV 1 ) + g 2 ( Df 2 + g 2 -DV 2 ) (3.4)
where V is given by any optical flow estimation and (f 1 , f 2 , g 1 , g 2 ) are defined by (2.11).

As assumed in section 1.3.1, for each time t, there is a one to one smooth mapping between η ∈ S 2 attached to the camera pixel and the scene point M (s) corresponding to this pixel. This means that, for any t ≥ 0, the flow φ(t, s) defined by

∂φ ∂t (t,s) = ̟ t (φ(t, s)), φ(0, s) = s ∈ S 2
defines a time varying diffeomorphism on S 2 . As in 1.3.1, ψ denotes the inverse diffeomorphism: φ(t, ψ(t, η)) ≡ η. Assume that Γ(t, η) > 0, v(t) and ω(t) are uniformly bounded for t ≥ 0 and η ∈ S 2 . This means that the trajectory of the camera center C(t) remains strictly inside the convex surface Σ with minimal distance to Σ. These considerations motivate the assumptions used in the following theorem.

Theorem 1. Consider D(t, η) associated to the motion of the camera inside the domain Ω delimited by the scene Σ, a C 1 , convex and closed surface as explained in section 1.

3.1. Assume that exist v > 0, ω > 0, d m > 0 and D M > 0 such that ∀t ≥ 0, ∀η ∈ S 2 , v(t) ≤ v, ω(t) ≤ ω, d m ≤ D(t, η) ≤ D M .
Then, for t ≥ 0, D(t, η) is a C 1 solution of (1.17). Consider the observer (3.3) with a C 1 initial condition versus η, D(0, η). Then we have the following implications:

1. ∀t ≥ 0, the solution D(t, η) of (3.3) exists, is unique and remains C 1 versus η. Moreover

t ↦ D(t, ) -D(t, ) L ∞ = max η∈S 2 D(t, η) -D(t, η)
is decreasing (L ∞ stability);

2. if additionally for all s ∈ S 2 , ∫ +∞ 0 g τ (φ(τ, s)) 2 dτ = +∞, then we have for all p > 0,

lim t↦+∞ S 2 D(t, η) -D(t, η) p dσ η = 0
(convergence in any L p topology);

3. if additionally there is λ > 0 and T > 0 such that, for all t ≥ T and s ∈ S 2 , ∫ t 0 g τ (φ(τ, s)) 2 dτ ≥ λt, then we have, for all t ≥ T ,

D(t, ) -D(t, ) L ∞ ≤ e -k 1 D M λt D(0, ) -D(0, ) L ∞ (exponential convergence in L ∞ topology).
Remark 1. Assumptions on ∫ g t (φ(t, s)) 2 dt can be seen as a condition of persistent excitations. It should be satisfied for generic motions of the camera.

Proof. The facts that v, ω and D are bounded and that the scene surface Σ is C 1 , closed and convex, ensure that the mapping η = φ(t, s) and its inverse s = ψ(t, η) are C 1 diffeomorphism on S 2 with bounded derivatives versus s and η for all time t > 0. Therefore, D is also a function of (t, s). Set D(t, s) = D(t, φ(t, s)): in the (t, s) independent variables the PDE (1.17) becomes a set of ordinary differential equations indexed by s:

∂D ∂t s = -v(t) ⋅ φ(t, s).
Thus

D(t, s) -D(0, s) = - t 0 v(τ ) ⋅ φ(τ, s) dτ.
Consequently, 1 D is C 1 versus s and thus

1 D is C 1 versus η. Set D(t, s) = D(t, φ(t, s)). Then ∂ D ∂t s = -v(t) ⋅ φ(t, s) + k g t (φ(t, s)) 2 1 D(t, s) (D(t, s) -D(t, s)). (3.5) Set E = D -D and E = D -D. Then ∂E ∂t s = -k 1 D(t, s) g t (φ(t, s)) 2 E(t, s) (3.6)
Consequently, Ē is well defined for any t > 0 and C 1 versus s. Thus E and consequently D = E+D are also well defined for all t > 0 and are C 1 versus η. Since for any s and t 2 > t 1 ≥ 0 we have E(t 2 , s) ≤ E(t 1 , s) , we have also

E(t 2 , s) ≤ max σ E(t 1 , σ) = D(t 1 , ) -D(t 1 , ) L ∞ .
Thus, taking the max versus s, we get

D(t 2 , ) -D(t 2 , ) L ∞ ≤ D(t 1 , ) -D(t 1 , ) L ∞
This ends the proof of the first implication. Since

E(t, s) = E(0, s)e -k ∫ t 0 1 D(τ,s) gτ (φ(τ,s)) 2 dτ we have (φ(0, s) ≡ s). E(t, s) ≤ E(0, s) e -k 1 D M ∫ t 0 gτ (φ(τ,s)) 2 dτ .
Take p > 0. Then

S 2 E(t, η) p dσ η = S 2 E(t, s) p det ∂φ ∂s (t, s) dσ s .
By assumption ∂φ ∂s is bounded. Thus exists C > 0 such that

E(t, ) L p = S 2 E(t, η) p dσ η ≤ C S 2 E(t, s) p dσ s . When ∫ +∞ 0 g τ (φ(τ, s)) 2 dτ = +∞,
∀s ∈ S 2 , E(t, s) ≤ E(0, s) e -k 1 D M λt .
Thus, for all s ∈ S 2 we get

E(t, s) ≤ E(0, ) L ∞ . Since η ↦ ψ(t, η) is a diffeomorphism of S 2 , we get finally, ∀η ∈ S 2 , E(t, η) ≤ E(0, ) L ∞ e -k 1 D M λt .
This proves

E(t, ) L ∞ ≤ E(0, ) L ∞ e -k 1 D M λt
and concludes the proof of the theorem.

Asymptotic observer based on depth estimation

Instead of relying the observer on optical flow estimation, we can base it on depth estimations as outputs of variational methods described in subsection 2.2. For example, Γ HS is the output of the variational method with Sobolev prior. Then we propose the following observer for D (k > 0):

∂ D ∂t = -∇ D ⋅ (f t + Γ HS g t ) -v ⋅ η + k(1 -DΓ HS ) (3.7)
that reads in pinhole coordinates

∂ D ∂t = - ∂ D ∂z 1 (f 1 + Γ HS g 1 ) - ∂ D ∂z 2 (f 2 + Γ HS g 2 ) -(z 1 v 1 + z 2 v 2 + v 3 ) + k(1 -DΓ HS ). (3.8)
For this observer we have the following convergence result.

Theorem 2. Take assumptions of Theorem 1 concerning the scene surface Σ, D, v and ω.

Consider the observer (3.7) where Γ HS coincides with Γ = 1 D and where the initial condition is

C 1 versus η. Then ∀t ≥ 0, the solution D(t, η) of (3.7) exists, is unique, remains C 1 versus η and D(t, ) -D(t, ) L ∞ ≤ e -k 1 D t D(0, ) -D(0, ) L ∞ (exponential convergence in L ∞ topology).
The proof is similar to the one of Theorem 1.

Remark 2. Observers (3.3) and (3.7) are contracting for the metric M defined as

δ DT M δ D ∶= 1 2 S 2 δ D2 d ση .
(3.9) Details on contraction analysis can be found in [START_REF] Lohmiller | On metric observers for nonlinear systems[END_REF] or [START_REF] Lohmiller | On contraction analysis for non-linear systems[END_REF]]. This is proven noticing that δ D satisfies the following system (for observer

(3.3)) ∂δ D ∂t = -∇δ D ⋅ ̟ t + kg t ⋅ (-δ Dg t )
Thus using the same notations than in the proof of Theorem 1

∂δ D ∂t s = -k g t (φ(t, s)) 2 δ D(t, s) D(t, s) (3.10) which yields ∂ ∂t 1 2 S 2 δ D2 d ση = -k S 2 g t (φ(t, s)) 2 δ D(t, s) 2 D(t, s) det ∂φ ∂s (t, s) d σs ≤ 0.
This proves that the time-derivative of the variation δ D is decreasing for the metric M .

Chapter 4

Implementations, simulations and experiments

Implémentations

Dans ce chapitre, l'implémentation des méthodes précédentes sont décrites, et des simulations sont présentées. Une expérience préliminaire traitant des données réelles montre que les hypothèses de continuité sur l'environnement ne sont peut-être pas nécessaires : les résultats sont cohérents avec la géométrie réelle de l'environnement filmé. Ces résultats ont été présentés dans [Zarrouati et al., 2012a[START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF][START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

In this chapter, implementations are described, and simulation results are presented. A preliminary experiment on real data shows that continuity assumptions on the environment may not be necessary, given that the results are consistent with reality. These results were presented in [Zarrouati et al., 2012a[START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF][START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF].

Simulations

Sequence of synthetic images and method of comparison

The non-linear asymptotic observers described in Chapter 3 are tested on a sequence of synthetic images characterized by the following:

virtual camera: the size of each image is 640 by 480 pixels, the frame rate of the sequence is 60 Hz and the field of view is 50 deg by 40 deg; motion of the virtual camera: the trajectory of the camera is a combination of sinusoids; linear and angular velocities (plotted in Figures 4.1 and 4.2) are computed as the timederivatives of position and attitude. virtual scene: the scene is a room of dimensions 3 × 4 × 4.5 meters (width, length and height respectively); the walls are (virtually) painted with a gray pattern, whose intensity varies in horizontal and vertical directions as a sinusoid function; generation of the images: each pixel of an image has an integer value varying from 1 to 256, directly depending on the intensity of the observed surface in the direction indexed by the pixel, to which a normally distributed noise varying with mean 0 and standard deviation σ is added. To compare the performances of the various methods, we use the global relative error in the estimation of D, defined as

E = ∫ I D(t,η)-D(t,η) D(t,η) dσ η ∫ I dσ η
where D is the true value of the depth field, D is the estimation computed by any of the proposed methods and I is the image frame.

Implementation of the depth estimation based on optical flow measures

We test on the sequence described in 4.1.1 the depth estimation characterized by the PDE (3.3). The optical flow input V HS or V TVL1 (V 1 and V 2 components) is computed by a classical Horn-Schunck method or the TV-L 1 method described in Subsection 2.1.1. Note that convergence Theorem 1 assumes that the domain of definition of the image is the entire unit sphere S 2 . Here the field of view of our virtual camera limits this domain to a portion of the sphere I. However, the motion of our virtual camera ensures that most of the points of the scene appearing in the first image stay in the field of view of the camera during the whole sequence. The convergence of the method is only ensured for these points, and Neumann boundary conditions are chosen at the borders where optical flow points toward the inside of the image: To test the accuracy and the robustness of this method, the observer is successively implemented on the same sequence of images, on which noise is added. This perturbation is a centered normally distributed noise, characterized by a standard deviation σ. Recall from Subsection 4.1.1 that the image pixel values vary from 1 to 256. Figures 4.3 and 4.4 show the result of this implementation for perfect data (σ = 0); in Figures 4.5 and 4.6, the noise corrupting the images is small (σ = 1); in Figures 4.7 and 4.8, the noise is magnified by ten (σ = 10). The same observer gain k > 0 is chosen for the various levels of noise, according to scaling considerations: k = 500 s.m -1 . These results show that the method is valid for perfect data, but is highly dependent on the quality of estimation of the optical flow: the TV-L 1 method of optical flow estimation, more robust, yields a fast convergence rate (less than 2 seconds), a reasonable accuracy (less than 10% of relative error for σ = 10) and a good robustness to noise. On the contrary, although the method using the Horn-Schunck optical flow estimation has a reasonable accuracy for perfect data, robustness to noise is not here. Note that for both methods, peaks of error occur when the magnitude of linear velocity is close to zero: like in triangulation methods where the baseline must be large enough to accurately reconstruct a 3D position, the quality of the results is degraded when linear displacement is too small.

∂ D ∂n = 0 if n ⋅ V HS < 0 (4.1)

Implementation of the asymptotic observer based on rough depth estimation

Subsequently, the observer described by (3.7) was applied to the same sequence. The input depth fields Γ HS and Γ TVL1 are obtained as the outputs of (2.16) and the algorithm described in Subsection 2.2.2 respectively. The regularization parameter α is chosen accordingly to scaling considerations and taking into account the magnitude of noise: α = 300 m.s -1 provides a slow but clean convergence. The correction gain k = 30 m.s -1 enables a rate of convergence similar to the one of the previous observer (see Subsection 4.1.2).

As in the previous subsection , we first test the variational methods for different levels of noise corrupting the image data. For perfect and noisy data (σ = 0 and σ = 10 respectively), the error of estimation associated to the input depths Γ HS and Γ TVL1 are plotted in Figures 4. 9 and 4.10 and in Figures 4.13 and 4.14 (respectively). As expected, the variational method based on Sobolev prior is slow but shows a better robustness than the variational method based on TV-L 1 minimization.

Then, the observer (3.7) is tested, using the previous depth estimates as depth inputs. The results are plotted in Figures 4.11 and 4.12 and in Figures 4.15 and 4.16 respectively. For perfect data, the observer based on TV-L 1 estimations quickly provides almost perfect results (greater bound of 0.5% for the error) but is unusable for noisy data. The observer based on L 2 minimization shows more robustness but is still less accurate than the observer based on TV-L 1 optical flow estimation (the error is always greater than 5%).

Experiment on real data

Real data have been processed in preliminary experiments, to ensure that the regularity assumptions on the environment are not necessary to get consistent results. To realize the experiments, a camera was fixed on a motorized trolley traveling back and forth on a 2 meterlinear track in about 6 seconds. The resolution of the encoder of the motor enables to know the position and the speed of the trolley with a micrometric precision. This experimental benchmark was set up and provided by the CAS (The Systems and Control Centre of Mines ParisTech) and is shown in Figure 4.17. Synchronization of linear velocity measurements and RGB images acquired by the camera has been achieved after the acquisition by comparison of images and specific motion times (e.g. starting point, ending point, turn-around point). This might result in some imprecision that can affect the overall depth estimation.

The scene is a static work environment, with desks, tables, chairs, lamps, lit up by electric light plugged on the mains, with frequency 50 Hz. The acquisition frame rate of the cameras produces an aliasing phenomenon on the video data at 4.17 Hz. In other words, the light intensity in the room is variable, at a frequency that can not be easily ignored, which does not comply with the initial hypothesis (1.12). However, the impact of this temporal dependence in the equations can be reduced by a normalization of the intensity of the images such as y(x, y) = y(x,y) ȳ

where x and y are the horizontal and vertical indexes of the pixels in the image, y(x, y) is the intensity of this pixel and ȳ is the mean intensity on the entire image.

The depth field was estimated via the asymptotic observer (3.3) based on optical flow measures. The components of the optical flow were computed by the TV-L 1 variational method (see Subsection 2.1.4). The correction gain was tuned to k = 100 s.m -1 . An example of image data is shown in Figure 4.18, and the depth estimate associated to that image at the same time t is shown in Figure 4.19. At that specific time t ≈ 8 s, the trolley already traveled once along the track and is on its way back toward its starting point. Some specific estimates are extracted from the whole depth field (two tables, two chairs, a screen, two walls) and highlighted in black; they are compared to real measurements taken in the experimental room by a laser telemeter(in red): the estimate depth profile D( t, ⋅) exhibits a strong correlation with these seven punctual reference values of D( t, ⋅) since the relative error does not exceed 13%; the global appearance of the depth field looks very realistic.

Introduction

The Kinect device has been a huge outbreak in the robotics and vision communities as it provides depth measurements registered at each pixel of a RGB image, at a relatively low cost: this output is named RGBD. Since its first release in November 2010, it has been extensively evaluated, in terms of accuracy [Gonzalez-Jorge et al., 2013, Khoshelham andElberink, 2012], as well as calibration [Daniel Herrera et al., 2012] or possible applications [START_REF] Smisek | 3D with Kinect[END_REF], Zhang, 2012]. The most obvious applications in the field of robotics are indoor modeling [START_REF] Du | Interactive 3D modeling of indoor environments with a consumer depth camera[END_REF], Henry et al., 2012], control [START_REF] Stowers | Altitude control of a quadrotor helicopter using depth map from microsoft Kinect sensor[END_REF], obstacle avoidance [START_REF] Correa | Mobile robots navigation in indoor environments using Kinect sensor[END_REF]. But other applications have also raised interest: for example motion capture [START_REF] Asteriadis | Estimating human motion from multiple Kinect sensors[END_REF], Calderita et al., 2013], surgery [START_REF] Guo | A portable immersive surgery training system using RGB-D sensors[END_REF], training and learning [START_REF] Guerrero | Kinect-based posture tracking for correcting positions during exercise[END_REF], gesture detection [START_REF] Maisto | An accurate algorithm for the identification of fingertips using an RGB-D camera[END_REF], or human action recognition [START_REF] Munaro | 3D flow estimation for human action recognition from colored point clouds[END_REF]. Finally, the domain of 3D SLAM has literally exploded with new algorithms known as RGBD SLAM algorithms, such as [START_REF] Endres | An evaluation of the RGB-D SLAM system[END_REF], Audras et al., 2011, Huang et al., 2011] or Kinect Fusion [START_REF] Izadi | Kinectfusion: real-time 3D reconstruction and interaction using a moving depth camera[END_REF] and its Kintinuous extensions ( [START_REF] Whelan | Kintinuous: Spatially extended KinectFusion[END_REF], Whelan et al., 2013a[START_REF] Whelan | Deformation-based loop closure for large scale dense RGB-D SLAM[END_REF]).

The Kinect device made possible the work presented in the following part: our approach consist in the combined processing of dense intensity and depth fields (namely the RGBD data) for 6-DOF motion estimation. Chapter 5

Estimation of biases on linear and angular velocities

Estimation de biais sur les vitesses linéaire et angulaire Dans ce chapitre, nous étudions l'estimation de biais sur les vitesses linéaire et angulaire par l'exploitation combinée d'images RGBD et de données biaisées de vitesses linéaire et angulaire. Le système d'EDP lié au modèle de caméra sphérique décrit au Chapitre 1 est utilisé pour construire un observateur associé à une fonction de Lyapunov. On en déduit un système d'équations intégrales/aux dérivées partielles sur les champs et les biais estimés. La convergence de l'observateur est traitée dans le cadre d'hypothèses de régularité C 1 sur le mouvement de la caméra et la scène. Il est prouvé que les biais estimés convergent vers les biais réels si et seulement si la scène n'admet pas de symétrie de révolution. L'observateur peut être adapté à des capteurs plus réalistes pour lesquels les champs d'intensité lumineuse et de profondeur ne sont disponibles que sur une partie de la sphère S 2 . Ces résultats ont été présentés, partiellement dans [START_REF] Zarrouati-Vissiere | Rotational and translational bias estimation based on depth and image measurements[END_REF], et de manière plus complète dans [START_REF] Zarrouati-Vissière | Inertial-sensor bias estimation from brightness/depth images and based on SO(3)-invariant integro/partial-differential equations on the unit sphere[END_REF].

In this chapter, the estimation of angular and linear velocity biases from the combined exploitation of RGBD images and biased linear and angular velocity inputs is studied. The PDE system associated to the spherical camera model described in Chapter 1 is used to design an observer based on a Lyapunov functional. It yields an integro/partial differential system for the estimated fields and biases. The observer convergence is investigated under C 1 regularity assumptions on the object motion and its scene. It relies on Ascoli-Arzela theorem and precompactness of the observer trajectories. It is proved that the estimated biases converge towards the true ones, if and only if, the scene admits no cylindrical symmetry. The observer design can be adapted to realistic sensors where brightness and depth data are only available on a subset of S 2 . These results were presented partially in [START_REF] Zarrouati-Vissiere | Rotational and translational bias estimation based on depth and image measurements[END_REF] and fully in [START_REF] Zarrouati-Vissière | Inertial-sensor bias estimation from brightness/depth images and based on SO(3)-invariant integro/partial-differential equations on the unit sphere[END_REF].

Statement of the problem

We intend to study the situation, where linear and angular velocity are provided up to a slowly varying bias (the wind or an ocean current), which can be illustrated by [START_REF] Bristeau | The navigation and control technology inside the AR. drone micro UAV[END_REF]. Synchronized RGBD data are processed along with these measurements to estimate their biases, in order to improve the overall reconstructed trajectory. We recall from Chapter 1.3 that functions y and D (light intensity and depth respectively) obey to

∂ t y = -∇y ⋅ (η × (ω + 1 D η × v)), ∂ t D = -∇D ⋅ (η × (ω + 1 D η × v)) -v ⋅ η,
where ∂ t y and ∂ t D stand for partial derivatives of y and D with respect to t. The camera motion characterized by (v, ω) is not known precisely. We assume here that sensor data provide v m (t) and ω m (t) differing from true velocities by measurement biases, defined as constant errors:

v m (t) = v(t) + v b , ω m (t) = ω(t) + ω b , (5.1)
where v b , ω b ∈ R 3 are constant:

∂ t v b = 0, ∂ t ω b = 0. (5.2)
Thus, the state equations of the system are

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t y = -∇y ⋅ η × (ω m -ω b + 1 D η × (v m -v b )), ∂ t D = -∇D ⋅ η × (ω m -ω b + 1 D η × (v m -v b )) -η ⋅ (v m -v b ), ∂ t v b = 0, ∂ t ω b = 0.
(5.3) Here (v m , ω m ) are considered as known inputs and (y, D) as the measured outputs. The state is (y, D, v b , ω b ) T . The goal is to estimate in real-time the parameters (v b , ω b ) from the known signals corresponding to sensors measuring y, D, v m and ω m . We assume

1. v and ω are in C 1 b ([0, +∞), R 3 ); C(t) stays in a fixed compact subset K of Ω for all t > 0, 2. y Σ is a C 1 function of s ∈ S 2 ; the surface Σ is C 1 where C k b ([0, +∞), X)
is the set of functions whose time derivatives are bounded on X uniformly with respect to t ∈ [0, +∞) up to order k.

Thanks to the first assumption and (1.8), C(t) and q(t) belong to C 2 b ([0, +∞)). Thanks to the second assumption and (1.9), ψ(t, η) and φ(t, s) belong to C 1 b ([0, +∞) × S 2 , S 2 ). We deduce also from (1.11), (1.13) and the first assumption that

y, D ∈ C 1 b ([0, +∞) × S 2 , R) and D(t, η) ⩾ D * > 0 , ∀(t, η) ∈ [0, +∞) × S 2 .
(5.4)

5.2 Observability of the reference system

Characterization of the observability

To prove the observability, we consider two state-trajectories

t ↦ (y 1 (t, ⋅), D 1 (t, ⋅), v b 1 (t), ω b 1 (t)) T and t ↦ (y 2 (t, ⋅), D 2 (t, ⋅), v b 2 (t), ω b 2 (t)) T
and assume that they satisfy the same state equations (5.3) with the same inputs t ↦ (v m (t), ω m (t)) and outputs t ↦ (y(t, ⋅), D(t, ⋅)). It immediately follows that y 1 = y 2 = y,

D 1 = D 2 = D, v b 1 = v b 2 + v b and ω b 1 = ω b 2 + ω b with ∂ t v b = 0 and ∂ t ω b = 0. Then t ↦ (y(t, ⋅), D(t, ⋅), v b (t), ω b (t)) T satisfy: ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∇y ⋅ η × (ω b + 1 D η × v b ) = 0, ∇D ⋅ η × (ω b + 1 D η × v b ) + η ⋅ v b = 0, ∂ t v b = 0, ∂ t ω b = 0. (5.5) Observability means that (5.5) implies that (ω b , v b ) = 0.
Consider a solution (y, D, ω b , v b ) of (5.5). Take some fixed value t of t and set ȳ(⋅) = y( t, ⋅) with D(⋅) = D( t, ⋅). Then (ȳ, D) is a stationary solution of (1.16,1.17) when (ω, v) is constant and equal to (ω b , v b ). Characterizing a scene, i.e., the surface Σ and its painting y Σ , that admits a stationary solution of (1.16) and (1.17) with non zero constant values of (ω, v), i.e. that is compatible with an effective motion producing stationary images y and D, is not obvious.

These considerations motivate the following terminology: for a given scene (Σ, y Σ ), a constant motion (ω, v) = (ω b , v b ), such that a stationary solution (y, D) of (1.16,1.17) exists, i.e.,

∇D ⋅ η × (ω b + 1 D η × v b ) + η ⋅ v b = 0, ∇y ⋅ η × (ω b + 1 D η × v b ) = 0, (5.6)
is called a stationary motion inside (Σ, y Σ ). For any scene (Σ, y Σ ), the null motion v b = ω b = 0 is always stationary. Apart from this trivial stationary motion, only very specific scenes (see Theorem 3) admit non trivial stationary motions (v b , ω b ). Thus observability of (5.3) with output (y, D) is equivalent to the following statement: the stationary motions (v b , ω b ) inside the scene (Σ, y Σ ) are all reduced to the trivial one (0, 0).

Scenes with non trivial stationary motion

The following theorem characterizes the scenes admitting only trivial stationary motion, i.e., such that a stationary solution of (1.16,1.17) exists for some (ω, v) ≠ 0 constant. Theorem 3. Consider the assumptions given in Chapter 1.3 and Section 5.1. The scene (Σ, y Σ ) admits a non trivial stationary motion, if and only if, it admits a rotation axis.

Proof of Theorem 3: Let us consider a non-zero motion (v b , ω b ) T . Under the assumption that the equations (5.6) have a solution (y, D) ∈ C 1 (S 2 , R 2 ) we prove that (Σ, y Σ ) admits a rotation axis. Note that

Σ = D(η)η; η ∈ S 2 , y Σ (s) = y(t, η) with η = φ(t, s)(see Chapter 1.3).
Let us eliminate two trivial cases:

-

v b = 0, ω b ≠ 0: the two first equations of (5.6) reduce to ∇D ⋅ (η × ω b ) = 0, ∇y ⋅ (η × ω b ) = 0, (5.7)
which characterizes a cylindrical symmetry for y and D, with same rotation axis defined by the direction of ω b ; v b ≠ 0, ω b = 0: let us replace η by v b v b in the first equation of (5.6): this yields v b = 0, which is absurd. From now on, we consider only the case where v b and ω b are both non-zero. Our strategy consists in finding a fixed vector u such that a translation of the origin of the camera frame in the direction u transforms the functions D(η) and y(η) in new functions D and ỹ on the sphere S 2 and those functions are invariant by rotation about the axis Rω b . This requires several technical steps.

Step 1: Let us prove that v b and ω b are orthogonal. Starting from the first equation of (5.6), let us multiply it by D n (η ⋅ ω b ), where the choice of n ∈ N will follow, and integrate the result on S 2 :

I = S 2 D n ∇D ⋅ η × (ω b + 1 D η × v b ) + D n η ⋅ v b (η ⋅ ω b )dσ η = 0, (5.8)
We have

I = S 2 1 n + 1 ∇D n+1 ⋅ (η × ω b )(η ⋅ ω b ) + 1 n ∇D n ⋅ (η × (η × ω b ))(η ⋅ ω b ) + D n (η ⋅ v b )(η ⋅ ω b ) dσ η .
The two first terms are integrated by parts:

I = S 2 - 1 n + 1 D n+1 ∇ ⋅ ((η × ω b )(η ⋅ ω b )) - 1 n D n ∇ ⋅ ((η × (η × v b ))(η ⋅ ω b )) + D n (η ⋅ v b )(η ⋅ ω b ) dσ η .
The derivatives are developed:

I = S 2 - 1 n + 1 D n+1 (∇ ⋅ (η × ω b )(η ⋅ ω b ) + (η × ω b ) ⋅ ∇(η ⋅ ω b )) - 1 n D n ∇ ⋅ (η × (η × v b ))(η ⋅ ω b ) + (η × (η × v b )) ⋅ ∇(η ⋅ ω b ) + D n (η ⋅ v b )(η ⋅ ω b ) dσ η .
Now, let us recall the following basic formulae of differential geometry on S 2 (detailed and proven in Appendix A), where P is a constant vector: ∇(η ⋅ P) = -η × (η × P), (5.9)

∆(η ⋅ P) = -2η ⋅ P, (5.10) ∇ ⋅ (η × P) = 0.

(5.11)

Using the formulae (5.11) and (5.9) in the first term, and (5.10) and (5.9) in the second term yields:

I = S 2 1 n + 1 D n+1 (η × ω b ) ⋅ (η × (η × ω b )) - 1 n D n ((2η ⋅ v b )(η ⋅ ω b ) -(η × (η × v b )) ⋅ (η × (η × ω b ))) + D n (η ⋅ v b )(η ⋅ ω b ) dσ η .
The first term is obviously zero. Then

(η × (η × v b )) ⋅ (η × (η × ω b )) = -(η ⋅ v b )(η ⋅ ω b ) + v b ⋅ ω b .
Thus

I = S 2 - 1 n D n ((3η ⋅ v b )(η ⋅ ω b ) -v b ⋅ ω b ) + D n (η ⋅ v b )(η ⋅ ω b ) dσ η . Choosing n = 3 yields (v b ⋅ ω b ) S 2 D 3 dσ η = 0.
(5.12)

Since D > 0, we deduce that v b ⋅ ω b = 0. This ends the proof of Step 1.

As v b and ω b are orthogonal and both non-zeros, one can introduce a new vector, lu, where l > 0 is a length and u is a unit vector, by: v b = ω b × lu. Let us also define v and w the unit vectors such that v b = v b v and ω b = ω b w. Then, (u, v, w) is an orthonormal and direct frame of R 3 . From now on, (5.6), multiplied by lD v b writes:

∇D ⋅ ((Dη + lu) × w) + lDη ⋅ v = 0, ∇y ⋅ ((Dη + lu) × w) = 0.
(5.13)

Step 2: Let us prove that

D(η)η +lu ≠ 0, ∀η ∈ S 2 . It is sufficient to prove that D(-u) ≠ l. Let η ǫ ∶= -u+ǫv √ 1+ǫ 2 = -u + ǫv + o(ǫ). We have D(η ǫ )η ǫ = (D(-u) + ǫ ∇D -u ⋅ v + o(ǫ)) (-u + ǫv + o(ǫ)) = -D(-u)u + ǫ (D(-u)v -( ∇D -u ⋅ v) u) + o(ǫ)
Thus, (5.13) evaluated at η ǫ yields

∇D η ǫ ⋅ [[(l -D(-u))u + ǫ (D(-u)v -( ∇D -u ⋅ v) u)] × w] + ǫlD(-u) = o(ǫ), or ∇D η ǫ ⋅ [(D(-u) -l)v + ǫ (D(-u)u + ( ∇D -u ⋅ v) v)] + ǫlD(-u) = o(ǫ).
(5.14) Equation (5.13) evaluated at η = -u and substracted to (5.14) yields:

(D(-u) -l) ∇D η ǫ -∇D -u ⋅ v + ǫ D(-u) ∇D η ǫ ⋅ u + ( ∇D -u ⋅ v) ∇D η ǫ ⋅ v + lD(-u) = o(ǫ). (5.15)
Let us define

F ∶ R → R ǫ ↦ D(-u) ∇D η ǫ ⋅ u + ( ∇D -u ⋅ v) ∇D η ǫ ⋅ v + lD(-u). Assuming that (D(-u) -l) = 0 implies that F (ǫ) = o(1) as ǫ → 0. Since F is a C 0 function of ǫ, it implies F (0) = 0, i.e. ( ∇D -u ⋅ v) 2 + lD(-u) = 0
which is absurd since l > 0. This concludes the Step 2.

Step 3: Let us prove that f (η) ∶= D(η)η+lu (respectively y(η)) satisfies df (η)⋅δη = 0 (respectively dy(η)

⋅ δη = 0) where δη ∶= (D(η)η + lu) × w + l(η ⋅ v)η, ∀η ∈ S 2 . We have f (η) = g ○ k(η) where g(x) ∶= x and k(η) ∶= D(η)η + lu.
Thanks to the Step 2, f ∈ C 1 (S 2 ) and

df (η) ⋅ δη = dg[k(η)] ⋅ dk(η) ⋅ δη .
We have

dk(η) ⋅ δη = (∇D(η) ⋅ δη) η + D(η)δη = (∇D(η) ⋅ ([D(η)η + lu] × w + l(η ⋅ v)η)) η +D(η) ([D(η)η + lu] × w + l(η ⋅ v)η) = (∇D(η) ⋅ ([D(η)η + lu] × w)) η + D(η) ([D(η)η + lu] × w + l(η ⋅ v)η)
because ∇D(η) ⊥ η. Thanks to (5.13), we obtain

dk(η) ⋅ δη = D(η)[D(η)η + lu] × w. Thus df (η) ⋅ δη = ⟨ D(η)η + lu D(η)η + lu , D(η)[D(η)η + lu] × w⟩ = 0.
Similarly,

dy(η) ⋅ δη = ∇y ⋅ δη = ∇y ⋅ ((D(η)η + lu) × w + l(η ⋅ v)η) = ∇y ⋅ ((D(η)η + lu) × w) because ∇y(η) ⊥ η = 0 according to (5.13).
This ends the proof of Step 3.

Step 4: Let us prove that, when η = η(τ

) satisfies ∂ τ η = [D(η)η + lu] × w + l(η ⋅ v)η, then η ∶= [D(η)η + lu] [D(η)η + lu]
is a periodic trajectory on S 2 , describing a circle perpendicular to the axis Rw. Let η 0 ∈ S 2 . We consider the characteristic (see [LeVeque, 1992, Serre, 1999] for a description of the method of characteristics)

⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩ dη dτ (τ ) = D[η(τ )]η(τ ) + lu × w + l η(τ ) ⋅ v η(τ ), η(0) = η 0 .
The right hand side is defined by a C 1 function of η thus the Cauchy-Lipschitz theorem guarantees the existence and uniqueness of a maximal solution. Moreover, no explosion is possible in finite time (the solution η(τ ) lives on S 2 ) thus the maximal solution is defined for every τ ∈ R. From

Step 3, we deduce that the quantity τ ↦ D[η(τ )]η(τ )+lu is constant along the characteristics:

D[η(τ )]η(τ ) + lu ≡ D ∶= D[η 0 ]η 0 + lu , ∀τ ∈ R. Now let us consider η(τ ) ∶= D[η(τ )]η(τ ) + lu D[η(τ )]η(τ ) + lu = D[η(τ )]η(τ ) + lu D .
Then, η(τ ) is a C 1 -function and

D dη dτ = ∇D[η(τ )] ⋅ dη dτ η(τ ) + D[η(τ )] dη dτ = ∇D[η(τ )] ⋅ D[η(τ )]η(τ ) + lu × w η(τ ) +D[η(τ )] D[η(τ )]η(τ ) + lu × w + l η(τ ) ⋅ v η(τ ) .
Here, we have used the relation ∇D[η(τ )] ⋅ η(τ ) = 0 to get the last equality. Thanks to (5.13), we obtain

D dη dτ = D[η(τ )] D[η(τ )]η(τ ) + lu × w = D[η(τ )] Dη(τ ) × w.
Thus, η turns about the axis Rw as τ varies. Note that the speed D[η(τ )] is bounded from below by a positive constant, thus the trajectory of η is the (complete) circle on S 2 , perpendicular to the axis Rw and containing η0 ∶= [D(η 0 )η 0 + lu] D. Step 5: Let us prove that the map

S 2 → S 2 η 0 ↦ η0 ∶= D(η 0 )η 0 +lu [D(η 0 )η 0 +lu]
is surjective. This step leans on the statement that when the convex set defined by the surface Σ is translated by lu, the center of the camera frame stays inside the convex set (Σ + lu). This statement is ensured by the fact that D(-u) > l (or equivalently, that D(-u) > 0 , see Figure 5.2.2).

Thus, for any η0 ∈ S 2 , ∃M a point of the surface Σ + lu and ∃λ > 0 such that λη 0 = → CM = D(η 0 )η 0 + lu where η 0 ∈ S 2 (by definition of Σ + lu). Then λ = D(η 0 )η 0 + lu since η0 is a unit vector, and we have shown that ∃η 0 ∈ S 2 such that η0 = D(η 0 )η 0 +lu D(η 0 )η 0 +lu . Thus the map η 0 ↦ η0 is surjective. Now, let us prove that D(-u) > l. Let us assume that D(-u) ⩽ l. Then, the function η ↦ D(η) + lu ⋅ η is positive at η = u and non positive at η = -u, thus (intermediate values theorem) there exists η * such that η * ⋅w = 0 and D(η * )+lu⋅η * = 0. Then, the vector D(η * )η * +lu is orthogonal to w (because u and η * are) and orthogonal to η * , thus

D(η * )η * + lu = D(η * )η * + lu ⋅ (w × η * ) w × η * . Note that w × η * is unitary. Moreover, D(η * )η * + lu ⋅ (w × η * ) = lu.(w × η * ) because η * ⊥ w × η * = lη * ⋅ (u × w) = -lη * ⋅ v thus D(η * )η * + lu = l(η * ⋅ v)η * × w.
Incorporating this relation in (5.13), we get

l(η * ⋅ v)∇D(η * ) ⋅ (η * × w) × w + D(η * )lη * ⋅ v = 0.
Moreover,

η * × w × w = η * -(w ⋅ η * )w = η * is orthogonal to ∇D(η * ) thus lD(η * )η * ⋅ v = 0. Therefore, η * ⋅ w = η * ⋅ v = 0, i.e. η * = ±u, which is impossible because D(u) > 0 and D(-u) ≠ l (see
Step 2). This proves that D(-u) > l and ends the Step 5.

Step 6: Conclusion. The origin of the camera frame is translated by the vector -lu. We denote by η ∈ S 2 the space variable in this new system of coordinates. Then, to any pair (η, D(η)) in the first system of coordinates corresponds a unique pair (η, D) in the new system of coordinates:

η ∶= D(η)η + lu D(η)η + lu , D ∶= D(η)η + lu ,
and they describe the same point M (s) on Σ:

M (s) = D(η)η = -lu + Dη.
In addition, ỹ is defined so that for a given point M (s) on Σ, the light intensity is the same in both systems of coordinates:

y Σ (s) = y(t, η) where η = φ(t, s) in the first system of coordinates = ỹ(t, η) where η = D(η)η + lu D(η)η + lu in the new system of coordinates.
Steps 3 and 4 of this proof show that the quantities D and ỹ are constant when η covers circles on S 2 orthogonal to the axis Rw. Moreover, Step 5 ensures that η covers any such circle. Therefore, Σ = { D(η)η; η ∈ S 2 } (in the new system of coordinates) and y Σ (s) have rotation axis, with direction w. ◻

The asymptotic observer

A Lyapunov based observer

We propose the following observer for ω b and v b , inspired by the one proposed in [START_REF] Bonnabel | Fusion of inertial and visual : a geometrical observer-based approach[END_REF]: (5.16) where k y , k D , k ω , k v , λ y , λ D > 0 are constant gains. The choice of this observer is motivated by the following Lyapunov function candidate

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t ŷ = -∇ŷ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) + k y (y -ŷ), ∂ t D = -∇ D ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) -(v m -vb ) ⋅ η + k D (D -D), d ω b dt = -k ω ∫ S 2 λ y (ŷ -y)(∇ŷ × η) + λ D ( D -D)(∇ D × η) dσ η , d v b dt = -k v ∫ S 2 λ D ( D -D)η + λ y (ŷ -y)( 1 D η × (η × ∇ŷ)) +λ D ( D -D)( 1 D η × (η × ∇ D)) dσ η ,
V ∶= 1 2 S 2 λ y ỹ2 + λ D D2 dσ η + ω b 2 2k ω + ṽb 2 2k v (5.17) with ỹ ∶= ŷ -y, D ∶= D -D, ω b ∶= ω b -ω b and ṽb ∶= vb -v b .
(5.18)

Let us prove that V decreases along the trajectories of (5.16) for appropriate choices of k y and k D .. Let us consider any scalar field h(t, η) defined on S 2 and its integral H on the unit sphere:

H = S 2 h(t, η) 2 dσ η = S 2
h(t, q * ηq) 2 dσ η since η ↦ q * ηq is an isometry on S 2 . For η a constant vector of the earth-fixed frame, q * ηq is the same vector, expressed in the camera frame. Thus, h(t, q * ηq) is the value of h corresponding to a specific object M (s) of the scene. This yields

d dt H = S 2 d dt (h(t, q * ηq) 2 )dσ η = 2 S 2 h(t, q * ηq) d dt h(t, q * ηq) s dσ η = 2 S 2 h(t, q * ηq) (∂ t h + ∇h ⋅ (η × ω)) dσ η .
One can apply this calculation rule to the scalar fields ỹ and D. Then, equations (1.16) and (5.16) yield

∂ t ỹ + ∇ỹ ⋅ (η × ω) = ∇ŷ ⋅ (η × ( ω b + 1 D η × ṽb )) -∇ỹ ⋅ (η × ( 1 D η × v)) -k y ỹ . (5.19)
Equations (1.17) and (5.16) yield

∂ t D + ∇ D ⋅ (η × ω) = ∇ D ⋅ (η × ( ω b + 1 D η × ṽb )) + η ⋅ ṽb -∇ D ⋅ (η × ( 1 D η × v)) -k D D . (5.20) Now, using a ⋅ (b × c) = (a × b) ⋅ c and the expressions of d ω b dt and d v b dt , one gets V = - S 2 k y λ y ỹ2 + k D λ D D2 dσ η - S 2 λ y ∇ỹ 2 2 + λ D ∇ D2 2 ⋅ W (t, η)dσ η . (5.21) where (R, S 2 ) ∋ (t, η) ↦ W (t, η) = η × ( 1 D η × v) ∈ R 3 .
Integration by parts of the second term of this expression yields

V = - S 2 λ y k y - ∇ ⋅ W (t, η) 2 ỹ2 + λ D k D - ∇ ⋅ W (t, η) 2 D2 dσ η . (5.22)
Thus V is a Lyapunov function for trajectories of (5.16) when k y , k D are large enough (see Proposition 1 below).

The convergence analysis relies on an adaptation to this infinite dimensional setting of the Lasalle invariance principle. Precisely, we prove that any adherence value of (ỹ, D, ω b , ṽb ) as [t → +∞] satisfies geometric equations equivalent to (5.6) (see Theorem 4). This proof requires the following successive steps.

1. The Lyapunov function V decreases along the trajectories of (5.16), for appropriate choices of k y and k D (see Proposition 1). 2. The non-linear partial differential equations (5.16) define a well posed Cauchy problem for every t ≥ 0 (see Proposition 2). 3. The functions ∇ŷ and ∇ D are uniformly bounded with respect to (t, η) ∈ [0, +∞) × S 2 (see Proposition 3). 4. The estimated state (ŷ, D, ω b , vb ) depends continuously on (y, D, ω, v) (see Proposition 4).

Decrease of the Lyapunov function

In the next statement are given sufficient conditions for the choice of correction gains k y and k D to ensure the decrease of the Lyapunov function V .

Proposition 1. The Lyapunov function V decreases along the trajectories of (5.16) under the assumption that k y , k D > L 2 where L is defined as

L ∶= 1 D(t, η) 2η ⋅ v(t) - ∇D(t, η) D(t, η) ⋅ (η × (η × v(t))) L ∞ ((0,+∞)×S 2 )
.

Proof of Proposition 1: Note that L is well defined thanks to (5.4) and assumption v ∈ C 1 b ((0, +∞), R 3 ) (see Section 5.1). Moreover, L = ∇ ⋅ W (t, η) L ∞ ((0,+∞)×S 2 ) (see formulae (5.9) and (5.10)). From (5.22), we deduce that

V ≤ - S 2 λ y k y - L 2 ỹ2 + λ D k D - L 2 D2 dσ η ∶= -f (t) (5.23)
which ends the proof of Proposition 1. ◻

From now on, we assume that k y and k D satisfy the assumption of Proposition 1.

Existence and uniqueness

Existence and uniqueness of solutions of (5.16) is given by the next statement. For the simplicity of notations, from now on, we assume that

k v = k ω = 1. Proposition 2. Let R > 0. There exists a constant k * = k * R, (v m , ω m ) L ∞ (0,+∞) , D * , (y, D) C 1 ([0,+∞)×S 2 ) > 0 such that, for every ω b 0 , vb 0 ∈ R 3 with ( ω b 0 -ω b , vb 0 -v b ) ⩽ R, k y , k D > k * , there exists a unique solution (ŷ, D, ω b , vb ) ∈ C 1 ([0, +∞) × S 2 , R) 2 × C 1 ([0, +∞), R 3 ) 2 of the Cauchy-problem ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t ŷ = -∇ŷ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) + k y (y -ŷ), ∂ t D = -∇ D ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) -(v m -vb ) ⋅ η +k D (D -D), d ω b dt = -∫ S 2 λ y (ŷ -y)(∇ŷ × η) + λ D ( D -D)(∇ D × η) dσ η , d v b dt = -∫ S 2 λ D ( D -D)η + λ y (ŷ -y)( 1 D η × (η × ∇ŷ) +λ D ( D -D)( 1 D η × (η × ∇ D) dσ η , ŷ(0, η) = y(0, η), D(0, η) = D(0, η), ω b (0) = ω b 0 , vb (0) = vb 0 .
(5.24)

The proof of Proposition 2 is technical, thus we postpone it to Section 5.5.

Regularity and bounds

The following proposition states the uniform boundedness of the estimates and their partial derivatives.

Proposition 3. Let R and k * be as in Proposition 2. There exists

C = C R, (v m , ω m ) L ∞ (0,+∞) , D * , (y, D) C 1 ([0,+∞)×S 2 ) > 0 such that for every ( ω b 0 , vb 0 ) ∈ R 6 with ( ω b 0 -ω b , vb 0 -v b ) ⩽ R, k y , k D > k * , the solution of the Cauchy problem (5.24) satisfies ŷ(t, η) , D(t, η) , ∇ŷ(t, η) , ∇ D(t, η) ⩽ C, ∀(t, η) ∈ [0, +∞) × S 2 .
The proof of this result is a corollary of an intermediary result of the proof of Proposition 2 (see Proposition 6 in Section 5.5).

Continuity

The following proposition details the continuity of the estimated state with respect to (y, D, ω, v).

Proposition 4. Let T > 0. Let (v n , ω n ) n∈N , (v, ω) ∈ C 1 b ([0, +∞), R 3 ), (y n , D n ) n∈N , (y, D) ∈ C 1 b ([0, +∞) × S 2 , R) be associated solutions of (5.3) such that (v n , ω n ) → n→+∞ (v, ω) in C 0 ([0, T ], R 3 ) 2 , (5.25) (y n , D n ) n∈N is bounded in C 1 ([0, T ] × S 2 , R) 2 , (5.26) (y n , D n ) → n→+∞ (y, D) in C 0 ([0, T ] × S 2 , R) 2 .
(5.27)

Let R and k * be as in Proposition 2, (ŷ n , Dn , ω b n , vb n ) n∈N , (ŷ, D, ω b , vb ) be associated solutions of the observer (5.24) with k y , k D > k * and initial conditions such that

( ω b n , vb n )(0) → ( ω b , vb )(0) when n → +∞.
Then, for every T > 0,

( ω b n , vb n ) → n→+∞ ( ω b , vb ) in C 0 ([0, T ], R 3 ) 2 , (5.28) (ŷ n , Dn )(t) → n→+∞ (ŷ, D)(t) in C 0 ([0, T ] × S 2 , R) 2 , ∀t ∈ [0, T ].
(5.29)

The proof of Proposition 4 relies on tools introduced in the proof of Proposition 2, thus, we postpone it to Section 5.6.

Convergence

The following theorem states that the proposed observer is optimal, in the sense that the conditions for convergence of the observer and observability of the system are strictly equivalent.

Theorem 4. Let R and k * be as in Proposition 2. For every

( ω b 0 , vb 0 ) ∈ R 6 with ( ω b 0 - ω b , vb 0 -v b ) ⩽ R, k y , k D > k * , the solution of the Cauchy problem (5.24) satisfies 1. (ŷ -y)(t) L ∞ (S 2 ) + ( D -D)(t) L ∞ (S 2 ) → t→+∞ 0 , 2. for every adherence value (v b , ω b ) of ( vb , ω b ) as [t → +∞], there exists C ∈ K ⊂ Ω a camera position such that, ∀η ∈ S 2 ∇ȳ ⋅ η × ((ω b -ω b ) + 1 D η × (v b -v b )) = 0 (5.30) ∇ D ⋅ η × ((ω b -ω b ) + 1 D η × (v b -v b )) + η ⋅ (v b -v b ) = 0 (5.31)
where ȳ(t, η) and D(t, η) are the brightness and depth fields associated to the position C of the camera. According to Theorem 3, when the system is observable (i.e. when (Σ, y Σ ) does not admit a rotation axis), then

( ω b , vb )(t) → t→+∞ (ω b , v b ).
Proof of Theorem 4:

Step 1: Convergence of f (t), defined by (5.23), to zero when t → +∞. The function V (t) is nonincreasing and nonnegative, thus it converges when t → +∞. From (5.23), we deduce that the nonnegative fonction f satisfies

t 0 f (s)ds ⩽ V (0) -V (t) , ∀t > 0 .
Thus f ∈ L 1 (0, +∞). In order to conclude, it is sufficient to prove that f is uniformly continuous on [0, +∞) (Barbalat Lemma). We have

ḟ = 2 S 2 λ * y ỹ ∇ŷ ⋅ η × ( ω b + 1 D η × ṽb ) -∇ỹ ⋅ 1 D η × (η × v) -k y ỹ +λ * D D ∇ D ⋅ (η × ( ω b + 1 D η × ṽb )) -η ⋅ ṽb -∇ D ⋅ 1 D η × (η × v) -k D D dσ η (5.32) with λ * y ∶= λ y k y -L 2 and λ * D ∶= λ D k D -L 2 .
Thanks to Proposition 3, we get ḟ ∈ L ∞ (0, +∞), which ends the proof of Step 1.

Step 2: Proof of statement 1. The functions (ŷ -y, D -D) belong to C 1 b ([0, +∞)×S 2 , R) 2 thanks to Proposition 3 and (5.4). Thus Ascoli theorem guarantees the existence of adherence values in C 0 (S 2 , R) 2 of (ŷy, D -D)(t) when t → +∞. Thanks to Step 1, the only possible adherence value is (0, 0), thus the whole function converges to (0, 0) in C 0 (S 2 , R) 2 .

Step 3: Proof of statement 2. Let (v b 0 , ω b 0 ) be an adherence value of ( vb , ω b ) as t → +∞.

Let (t n ) n∈N be an increasing sequence of [0, +∞) such that ( vb , ω b )(t n ) → (v b 0 , ω b 0 ). Let T > 0. The functions v, ω (resp. y, D) belong to C 1 b ([0, +∞), R 3 ) 2 (resp. C 1 b ([0, +∞) × S 2 , R) 2 ), thus Ascoli's theorem guarantees the existence of v, ω ∈ C 0 ([0, T ], R 3 ) 2 (resp. D 0 , y 0 ∈ C 0 (S 2 , R) 2 ) such that, up to an extraction (v, ω)(t n + .) → (v, ω)(.) in C 0 ([0, T ], R 3 ) 2 , (y, D)(t n ) → (y 0 , D 0 ) in C 0 (S 2 , R) 2 .
The continuity of the flow for (5.3) justifies that

(y, D)(t n + .) → (y, D)(.) in C 0 ([0, T ] × S 2 , R) 2 where ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t y = -∇y ⋅ (η × (ω + 1 D η × v)), ∂ t D = -∇D ⋅ (η × (ω + Dη × v)) -v ⋅ η, y(0) = y 0 , D(0) = D 0 .
Thanks to Proposition 4, we know that

( ω b , vb )(t n + .) → ( ω b , vb ) in C 0 ([0, T ], R 3 ) 2 , (ŷ, D)(t n + .) → ( ŷ, D) in C 0 ([0, T ] × S 2 , R) 2 ,
where ( ŷ, D, ω b , vb ) is the solution of the observer associated to (y, D), The observer introduced in Section 5.3 can not be readily used since it brings into play the integral of y or D over the whole unit sphere. We will see that one can compensate this problem by considering virtual observations, equal to the real observations over the window defined by K.

ω m (t) = ω(t) + ω b , v m (t) = v(t) + v b and the initial conditions ( ω b , vb )(0) = (ω b 0 , v b 0 ). We know that V (t) converges to some limit V ∞ as t → +∞. Moreover, V (t n + t) → V (t) for every t ∈ [0, T ]. Therefore, V (t) = V ∞ for every t ∈ [0, T ]. In particular, 0 ⩽ f (t) ⩽ dV dt ≡ 0 i.e. ( ŷ -y)(t) = ( D -D)(t) = 0, ∀t ∈ [0, T ].
Let K 1 and K 2 be two compact sets s.t.

○ K 1 ⊂ ○ K 2 ⊂ ○ K. Let φ be a C ∞ scalar field S 2 ∋ η ↦ φ(η) ∈ R, s.t. φ = 1 on K 1 , φ = 0 and ∇φ = 0 on S 2 ∖ ○ K 2 .
Let us define X = φy and Λ = φD. Then,

∂ t X = -∇X ⋅ (η × (ω + 1 D η × v)) + y∇φ ⋅ (η × (ω + 1 D η × v))
and

∂ t Λ = -∇Λ ⋅ (η × (ω + 1 D η × v)) + D∇φ ⋅ (η × (ω + 1 D η × v)) -φη ⋅ v.
We propose the following adaptation of observer (5.16):

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t X = -∇ X ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) +y∇φ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) +k X (X -X), ∂ t Λ = -∇ Λ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) +D∇φ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) -φ(v m -vb ) ⋅ η + k Λ (Λ -Λ), ∂ t ω b = -k ω ∫ K λ X ( X -X)((∇ X -y∇φ) × η) +λ Λ ( Λ -Λ)((∇ Λ -D∇φ) × η)dσ η , ∂ t vb = -k v ∫ K λ Λ ( Λ -Λ)η +λ X ( X -X)( 1 D η × (η × (∇ X -y∇φ)) +λ Λ ( Λ -Λ)( 1 D η × (η × (∇ Λ -D∇φ))dσ η .
(5.33)

Notice that the estimates X and Λ of the virtual observations are computed only on the domain K, and not on the entire sphere S 2 , and only their values on K are used in the dynamics of the estimated biases. Neumann boundary conditions are imposed on the parts of ∂K where the direction of propagation points toward the inside of the domain K:

∂ X ∂n = 0, ∂ Λ ∂n = 0
where n is the outwards-pointing normal of ∂K such that

n ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) < 0.
Let us choose the candidate Lyapunov function

V = 1 2 K λ X X2 + λ Λ Λ2 dσ η + ω b 2 k ω + ṽb 2 k v .
One can prove that

V = K λ X X -∇ X ⋅ (η × (ω + 1 D (η × v))) -k X X dσ η + K λ Λ Λ -∇ Λ ⋅ (η × (ω + 1 D (η × v))) -k Λ Λ dσ η
and integration by parts yields

V = - K λ X k X - ∇ ⋅ W (t, η) 2 X2 + λ Λ k Λ - ∇ ⋅ W (t, η) 2 Λ2 dσ η -∮ ∂K λ X X2 + λ Λ Λ2 W (t, η) 2 ⋅ → n dl η where W is defined as (R, S 2 ) ∋ (t, η) ↦ W (t, η) = η ×(ω + 1 D η ×v) ∈ R 3 .
We guess that for given environment and trajectory, sufficiently large correction gains k X and k Λ can ensure the decrease of the candidate Lyapunov function V . Then convergence analysis done when K = S 2 should be extended to compact sub-domains K of S 2 . As the candidate Lyapunov function should decrease unless X = Λ = 0 on K, we guess that the necessary and sufficient condition for observability is the same as stated in Theorem 3, but restricted to the visible part of the environment. From a practical point of view, it is possible that specific views along the camera trajectory admit a cylindrical symmetry, although the environment does not: in that case, a residual error coupled between linear and angular velocity might subsist. However, as soon as a new view enables to raise the ambiguity (for example, a new wall appears in the field of view), this residual error vanishes. The observability condition in that situation does depend in a certain sense on the camera trajectory.

The observer in pinhole coordinates

The previous observer (5.33) can be finally adapted to a real model of camera: we choose here the widely spread pinhole camera model enabling a correspondence between the local coordinates on S 2 with a rectangular grid of pixels. The pixel of coordinates (z 1 , z 2 ) corresponds to the unit

vector η ∈ S 2 of coordinates in R 3 : 1 + z 2 1 + z 2 2 -1 2
(z 1 , z 2 , 1) T . The optical camera axis (pixel (z 1 , z 2 ) = (0, 0)) corresponds here to the direction z 3 . Directions 1 and 2 correspond respectively to the horizontal axis from left to right and to the vertical axis from top to bottom on the image frame.

The gradients ∇y and ∇D must be expressed with respect to z 1 and z 2 (see Subsection 1.3.2 for the detailed computations). Plugging these expressions in (5.33), we get a PDE system written in terms of (t, z 1 , z 2 ) as independent variables. Due to space limitation, this system is not given here, but its derivation is straightforward and a little bit tedious.

Simulations

The non-linear asymptotic observer (5.33) is tested on a sequence of synthetic images characterized by the following:

virtual camera providing images restricted to the spherical cap K ⊂ S 2 : the size of each image is 640 by 480 pixels, the frame rate of the sequence is 42 Hz and the field of view is 50 deg by 40 deg. motion of the virtual camera (ω, v): it consists of the motion of a real hand-held camera (filtered data), combining translations and rotations in each direction; the real linear and angular velocities expressed in the camera frame are plotted in Fig ures 5.2 and Fig. 5.3; zero-mean normally distributed noise (standard deviations σ v and σ ω ) is added to these velocities to test the robustness; virtual scene corresponding to (Σ, y Σ ): it consists of the walls, ceiling and floor of a virtual room; the observed walls are virtually painted with a gray pattern, whose intensity varies in horizontal and vertical directions as a sinusoid function; generation of the brightness images y: each pixel of an image has an integer value varying from 1 to 256, directly depending on the intensity of the observed surface in the direction indexed by the pixel, to which a zero-mean normally distributed noise with standard deviation σ y is added to test the robustness; generation of the depth images D: to each pixel of the rectangular grid of an image is attributed the depth of the corresponding element of the observed surface, computed with respect to position and orientation of the camera in the room, to which a zero-mean normally distributed noise with standard deviation σ D is added to test the robustness. The numerical resolution used to compute ŷ, D, ω b and vb according to (5.33) is based on a temporal Euler discretization scheme where ∇ŷ and ∇ D are computed via differentiation filters (Sobel filtering) directly from the image and depth previous estimates. The observer is then tested for reasonable biases: in rotation, a bias of 0.05 rad.s -1 (10, 000 deg/h, for a low-cost gyroscope) around the horizontal axis; in translation, a bias of 2.5 m.s -1 in the horizontal direction (9 km/h, for the windspeed). In other words, ω bx = 0.05 rad.s -1 and v bx = 2.5 m.s -1 . Biases in the other directions are set to 0. Initial conditions for ŷ and D are y(0, η) and D(0, η). Initial conditions for the estimated biases are set to zero. The chosen correction gains are: k y = k D = 2s -1 , k v = 10 -2 m 2 .s -2 and k ω = 10 -5 rad.m.s -2 . These correction gains are chosen in accordance with the expected values of biases and the scene averaged depth, to enable a reasonable convergence speed. The correction gains k y and k D are comparatively much larger than k v , which is itself larger than k ω , as large oscillations in the estimation of ω b can make the discretized observer to diverge. Finally, the ponderation coefficients are λ y = 1 and λ D = 5000m 2 , chosen to compensate the difference of magnitudes of y and D. First, when image and depth data contain no noise (σ y = σ D = 0), the results are plotted in Figure 5.4 and Figure 5.5 as the instantaneous errors of estimations ṽb and ω b expressed in the camera frame, respectively. In the first 6 s, errors slowly converge towards 0, and coupling between rotation and translation occurs: this reflects the fact that an horizontal translation can be interpreted as a rotation around the vertical axis to a certain extent. Oscillations decrease, and after convergence, errors stay bounded: for the bias in rotation, it does not exceed 0.002 rad.s -1 (4 % of the original bias); in translation, it is less than 0.01 m.s -1 (0.4 % of the original bias).

Then, to test the robustness of the method, noise is added to the image data (σ y = 30, about 12% of the full scale), to the depth data (σ D = 25 cm, which is three times as much that can be expected from a Kinect device), to linear velocity (σ v = 0.05m.s -1 ) and to angular velocity (σ ω = 0.005 rad.s -1 ). Results are plotted in Figure 5.6 and Figure 5.7. Convergence time is shorter for biases in translation estimation: after 3 s, error does not exceed 0.2 m.s -1 (8 % of the original bias). For the rotation, convergence is slower (as k ω is smaller), but in the last 3 sec of the simulation, biases are estimated up to 0.003 rad.s -1 (6 % of the original bias).

Experiments on real data

The observer described by the system of PDEs (5.33) is then tested on a sequence of real RGB and depth images. These images are acquired by a Kinect device, a pair of RBG and depth images is acquired at the same instant, at a frame rate of 25 Hz (see Section 9.1 for more details on the Kinect device). Gyrometers are embedded along with this device and provide angular velocity measurements at a frame rate of 125Hz. The system was provided by Sysnav company. Unfortunately, for this experiment, we did not have a biased linear velocity sensor synchronized with the system available. To overcome this issue, the system was only submitted to rotations, and only biases on angular velocity were estimated. More specifically, this is achieved by replacing (5.33) by the following system

⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ ∂ t X = -∇ X ⋅ (η × (ω m -ω b )) + y∇φ ⋅ (η × (ω m -ω b )) + k X (X -X), ∂ t Λ = -∇ Λ ⋅ (η × (ω m -ω b )) + D∇φ ⋅ (η × (ω m -ω b )) + k Λ (Λ -Λ), ∂ t ω b = -k ω ∫ K λ X ( X -X)((∇ X -y∇φ) × η) + λ Λ ( Λ -Λ)((∇ Λ -D∇φ) × η)dσ η .
(5.34)

The measured angular velocity is plotted in Figure 5.8: the motion is relatively slow, with an order of magnitude of 0.15 rad.s -1 (8.5 deg.s -1 ). In the first 40 sec, the system stands still, which reveals a bias on angular velocity: this bias is estimated by a rough mean calculation. This yields

ω b = ⎛ ⎜ ⎝ 1.0304 -1.0384 1.2129 ⎞ ⎟ ⎠ 10 -3 rad.s -1 .
The sequence of RGBD images is processed along with the measurements from gyrometers, which yields an estimation of angular velocity biases plotted in Figure 5.9. Biases in the x-axis and y-axis directions converge quickly (about 50 sec) and are submitted to perturbations, but bias in the z-axis direction still evolves at the end of the sequence. The same sequence is thus processed a second time, with the biases initialized at their final values at the end of the first processing. This operation is repeated a few times, and the resulting estimated biases are plotted in Figure 5.10. After several iterations, the tendency of evolution is stationary: biases are submitted to perturbations but have a constant mean. The perturbations can be the consequence of several factors, the main being that the hypothesis of null linear velocity may be unsatisfied. The mean of estimated biases is

ω bmean = ⎛ ⎜ ⎝ 1.050 -0.975 1.170 ⎞ ⎟ ⎠ 10 -3 rad.s -1 .
The magnitude of variations around this mean is about 10 -4 rad.s -1 . From this real data experiment, the method can be validated: a precision of 10 -4 rad.s -1 (5.7mdeg.s -1 ) can be expected on the angular velocity bias estimation.

Proof of the well posedness of the observer

The goal of this section is the proof of Proposition 2 in 3 steps:

-the first step consists in proving the existence of local (in time) solutions, -the second step consists in proving that solutions stays bounded in a appropriate way, so that -we can iterate the local argument to get global (in time) solutions, in a third step.

Local solutions

The goal of this section is the proof of the following result.

Proposition 5. Let R and M > 0. There exists (5.36) there exists a unique solution (ŷ, D, ω b , vb )

T = T (M, R, (v m , ω m ) L ∞ (0,+∞) , D * , (y, D) C 1 ([0,+∞)×S 2 ) , k y , k D , λ y , λ D ) > 0 such that, for every ω b 0 , vb 0 ∈ R 3 , ŷ0 , D0 ∈ C 1 (S 2 ) with ( ω b 0 -ω b , vb 0 -v b ) ⩽ R , (5.35) ŷ0 C 1 (S 2 ) , D0 C 1 (S 2 ) ⩽ M ,
∈ C 1 ([0, T ] × S 2 , R) 2 × C 1 ([0, T ], R 3 ) 2 , defined on the time interval [0, T ], of the Cauchy-problem ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t ŷ = -∇ŷ ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) + k y (y -ŷ) , ∂ t D = -∇ D ⋅ (η × (ω m -ω b + 1 D η × (v m -vb ))) -(v m -vb ) ⋅ η +k D (D -D) , d ω b dt = -∫ S 2 λ y (ŷ -y)(∇ŷ × η) + λ D ( D -D)(∇ D × η) dσ η , d v b dt = -∫ S 2 λ D ( D -D)η + λ y (ŷ -y)( 1 D η × (η × ∇ŷ) +λ D ( D -D)( 1 D η × (η × ∇ D) dσ η , ŷ(0, η) = ŷ0 (η), D(0, η) = D0 (η), ω b (0) = ω b 0 , vb (0) = vb 0 .
(5.37)

Proof of Proposition 5: We prove the existence and uniqueness of the non-linear and nonlocal system of PDEs (5.37) by a standard fixed-point approach. Our strategy consists in explicitly solving the two first PDEs of (5.37) with fixed functions t ↦ ( W b , V b )(t), instead of t ↦ ( ω b , vb )(t). Then, the explicit solutions ( Ŷ (t), d(t)) are plugged in the third and fourth ODEs of (5.37). We conclude by applying the Banach fixed point theorem (see [Brezis, 1983]) on the map

( W b , V b ) ↦ ( ω b , vb ). Let R, M > 0, ω b 0 , vb 0 ∈ R 3 , and ŷ0 , D0 ∈ C 1 (S 2
) such that (5.35) and (5.36) hold. For any T > 0, the functional space

E T ∶= {( W b , V b ) ∈ C 0 ([0, T ], R 6 ); ( W b , V b )(t) -(ω b , v b ) ⩽ 2R, ∀t ∈ [0, T ]} equipped with the L ∞ ((0, T ), R 6 )-norm is a Banach space. To any element ( W b , V b ) ∈ E T , we associate the solutions ( Ŷ , d, ω b , vb ) ∈ C 1 ([0, T ] × S 2 ) 2 × C 1 ([0, T ], R 3 ) 2 of the Cauchy problem ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t Ŷ = -∇ Ŷ ⋅ (η × (ω m -W b + 1 D η × (v m -V b ))) + k y (y -Ŷ ) , ∂ t d = -∇ d ⋅ (η × (ω m -W b + 1 D η × (v m -V b ))) -(v m -V b ) ⋅ η + k D (D -d) , d ω b dt = -∫ S λ y ( Ŷ -y)(∇ Ŷ × η) + λ D ( d -D)(∇ d × η) dσ η , d v b dt = -∫ S λ D ( d -D)η + λ y ( Ŷ -y)( 1 D η × (η × ∇ Ŷ ) +λ D ( d -D)( 1 D η × (η × ∇ d) dσ η , Ŷ (0, η) = ŷ0 (η) , d(0, η) = D0 (η) , ω b (0) = ω b 0 , vb (0) = vb 0 .
(5.38)

Let us emphasize that this Cauchy problem is well posed because Ŷ and d have explicit expressions along the characteristics (see [LeVeque, 1992, Serre, 1999] for a description of the method of characteristics).

Indeed, let us introduce the flow Φ t on the sphere S 2 associated to the ODE

⎧ ⎪ ⎪ ⎨ ⎪ ⎪ ⎩ dη dt = η × ω m (t) -W b (t) + 1 D(t,η) η × [v m (t) -V b (t)] , η(0) = η 0 .
(5.39)

This means that the solution of (5.39) is η(t) = Φ t (η 0 ), ∀t, η 0 . Note that the right hand side of this equation is continuous in t and C 1 in η, thus the flow is uniquely defined. This flow is defined for every t ∈ [0, T ] because no explosion is possible in finite time: it lives on the sphere S 2 . Moreover, for every t ∈ [0, T ], η ↦ Φ t (η) is a C 1 -diffeomorphism of the sphere S 2 , thus, there exists

C = C(R, (ω m , v m ) L ∞ (0,T ) , D * , D C 1 ([0,T ]×S 2 ) ) > 0 such that ∂Φ t ∂η (η) , ∂Φ -1 t ∂η (η) ⩽ C, ∀(t, η) ∈ [0, T ] × S 2 .
(5.40)

With the characteristics method, we get the following explicit expressions

Ŷ [t, Φ t (η)] = ŷ0 (η)e -kyt + t 0 k y y[τ, Φ τ (η)]e ky(τ -t) dτ, d[t, Φ t (η)] = D0 (η)e -k D t + ∫ t 0 k D D(τ, Φ τ (η)) -(v m -V b )(τ ).Φ τ (η) e k D (τ -t) dτ, or, equivalently Ŷ [t, η] = ŷ0 [0, Φ -1 t (η)]e -kyt + t 0 k y y[τ, Φ τ ○ Φ -1
t (η)]e ky(τ -t) dτ, (5.41) τ -t) dτ.

d[t, η] = D0 [Φ -1 t (η)]e -k D t + ∫ t 0 k D D[τ, Φ τ ○ Φ -1 t (η)] -(v m -V b )(τ ).[Φ τ ○ Φ -1 t ](η) e k D (
(5.42) Thus, ω b and vb are also explicit:

ω b (t) = ω b 0 - t 0 S 2 λ y ( Ŷ -y)(∇ Ŷ × η) + λ D ( d -D)(∇ d × η) dσ η dτ, (5.43) vb (t) = vb 0 -∫ t 0 ∫ S 2 λ D ( d -D)η + λ y ( Ŷ -y)( 1 D η × (η × ∇ Ŷ ) +λ D ( d -D)( 1 D η × (η × ∇ d) dσ η dτ.
(5.44)

Now, let us introduce the map

F T ∶ E T → C 0 ([0, T ], R 6 ) ( W b , V b ) ↦ ( ω b , vb ).
Step 1: Let us prove the existence of

T * = T * (M, R, (ω m , v m ) L ∞ (0,1) , D * , (D, y) C 1 ([0,1]×S 2 ) , k y , k D , λ y , λ D ) ∈ (0, 1)
such that F T maps E T into E T for every T < T * . From now on, we assume that T ∈ (0, 1). Let ( W b , V b ) ∈ E T . Thanks to the explicit expressions (5.41), (5.42) and the bounds (5.40), (5.4) there exists a constant

C 1 = C 1 (M, R, (ω m , v m ) L ∞ (0,1) , D * , (y, D) C 1 ([0,1]×S 2 ) , k y , k D ) > 0 such that Ŷ (t, η) , ∇ Ŷ (t, η) , d(t, η) , ∇ d(t, η) ⩽ C 1 , ∀(t, η) ∈ [0, T ] × S 2 .
(5.45)

Then, we deduce from (5.43), (5.44) and (5.35) that

ω b (t) -ω b , vb (t) -v b ⩽ R + C 2 T, ∀t ∈ [0, T ],
where

C 2 = C 2 (M, R, (ω m , v m ) L ∞ (0,1) , D * , (D, y) C 1 ([0,1]×S 2 ) , k y , k D , λ y , λ D ) > 0. Thus if T < R C 2 , then ( ω b , vb ) belongs to E T . This ends Step 1 with T * ∶= min{1, R C 2 }.
Step 2: Let us prove the existence of

T * * = T * * (M, R, (ω m , v m ) L ∞ (0,1) , D * , (y, D) C 1 ([0,1]×S 2 ) , k y , k D , λ y , λ D ) ∈ (0, 1) such that F T is (1 2)-contractant on E T for every T < T * * . Let ( W b 1 , V b 1 ), ( W b 2 , V b 2 ) ∈ E T , Φ 1 t , Φ 2
t be the associated flows on S 2 and ( Ŷ 1 , d1 , ω b 1 , vb 1 ), ( Ŷ 2 , d2 , ω b 2 , vb 2 ) be the associated solutions of (5.38). The regularity of the flow of (5.39) with respect to the function ( W b , V b ) justifies the existence of a constant

C 3 = C 3 (R, (ω m , v m ) L ∞ (0,T ) , D * , D C 1 ([0,T ]×S 2 ) ) > 0 such that, for every t ∈ [0, T ], Φ 1 t -Φ 2 t C 1 (S 2 ) , (Φ 1 t ) -1 -(Φ 2 t ) -1 C 1 (S 2 ) ⩽ C 3 ( W b 1 -W b 2 , V b 1 -V b 2 ) L ∞ (0,T ) .
Using the regularity of y, ŷ0 , D, D0 and bound (5.36), we deduce the existence of a constant

C 4 = C 4 (M, R, (ω m , v m ) L ∞ (0,T ) , D * , (y, D) C 1 ([0,T ]×S 2 ) , k y , k D ) > 0 such that ( Ŷ 1 -Ŷ 2 , d1 -d2 )(t) C 1 (S 2 ) ⩽ C 4 ( W b 1 -W b 2 , V b 1 -V b 2 ) L ∞ (0,T ) , ∀t ∈ [0, T ] .
Thus, we deduce from (5.43) and (5.44) that

( ω b 1 -ω b 2 , vb 1 -vb 2 ) L ∞ (0,T ) ⩽ C 5 T ( W b 1 -W b 2 , V b 1 -V b 2 ) L ∞ (0,T )
for some constant C 5 , that depends on the same quantitites as C 4 . This gives the conclusion with T * * = 1 (2C 5 ). ◻

Bounds on solutions

The goal of this section is the proof of the following result.

Proposition 6. Let R > 0. There exists

k * = k * (R, (ω m , v m ) L ∞ (0,1) , D * , (y, D) C 1 ([0,1]×S 2 ) ) > 0 such that for every ω b 0 , vb 0 ∈ R 3 with (5.35), k y , k D > k * , T * > 0, if (ŷ, D, ω b , vb ) ∈ C 1 ([0, T * ] × S 2 ) 2 × C 1 ([0, T * ], R 3 ) 2 is a solution of (5.24) on the time intervall [0, T * ], then (ŷ, D)(t) C 1 (S 2 ) ⩽ (y, D) C 1 (S 2 ) + 1 , ∀t ∈ [0, T * ].
The proof of this proposition relies on the following technical Lemma.

Lemma 5. Let M 0 > 0. There exists

k * = k * (M 0 ) > 0 such that -for every a ∈ C 1 ([0, +∞) × S 2 , R 3 ), b ∈ C 1 ([0, +∞) × S 2 , R) such that a(t, η) ⋅ η = 0 , ∀(t, η) ∈ [0, +∞) × S 2 , (5.46) a(t, η) , b(t, η) , ∂a ∂η (t, η) , ∂b ∂η (t, η) ≤ M 0 , ∀(t, η) ∈ [0, +∞) × S 2 , (5.47) -for every k > k * , h 0 ∈ C 1 (S 2 , R), T * > 0 and h ∈ C 1 ([0, T * ] × S 2 , R) solution on [0, T * ] of ∂ t h(t, η) = ∂h ∂η (t, η) ⋅ a(t, η) + b(t, η) -kh(t, η) , h(0, η) = h 0 (η) , then, h(t, .) C 1 (S 2 ) ⩽ h 0 C 1 (S 2 ) + 1 , ∀t ∈ [0, T * ].
Proof of Lemma 5: Let us consider the flow ϕ(t, η) associated to the following equation

∂ t ϕ = -a(t, ϕ) ϕ(0, η) = η.
(5.48)

The ODE has a local (in time) solution for any η ∈ S 2 thanks to Cauchy-Lipschitz theorem because a is continuous in t and C 1 in η. This solution lives on S 2 thanks to assumption (5.46), thus no explosion is possible and ϕ(t, η) is defined for every (t, η) ∈ [0, +∞) × S 2 . The new function: -t) dτ .

z(t, η) ∶= h(t, ϕ(t, η)) (5.49) solves ∂ t z(t, η) = b(t, ϕ(t, η)) -kz. Thus z(t, η) = e -kt h 0 (η) + t 0 b(τ, ϕ(τ, η))e k(τ
(5.50)

Step 1: Bound on h. For every t ∈ [0, T * ], ϕ(t, .) is a bijection of S 2 thus h(t, .) t-τ ) dτ by (5.50) and (5.47)

L ∞ (S 2 ) = z(t, .) L ∞ (S 2 ) ⩽ h 0 L ∞ (S 2 ) e -kt + ∫ t 0 M 0 e -k(
⩽ h 0 L ∞ (S 2 ) + M 0 k ⩽ h 0 L ∞ (S 2 ) + 1 when k > M 0 .
Step 2: Bound on ∂h ∂η. From (5.49) and (5.50), we deduce that

∂h ∂η (t, ϕ(t, η)) = ∂z ∂η (t, η) ∂ϕ ∂η (t, η) -1 = e -kt ∂h 0 ∂η (η) ∂ϕ ∂η (t, η) -1 + ∫ t 0 e -k(t-τ ) ∂b ∂η (τ, ϕ(τ, η))Σ(τ, t, η)dτ (5.51)
where

Σ(τ, t, η) ∶= ∂ϕ ∂η (τ, η) ⋅ ∂ϕ ∂η (t, η) -1 . Note that ⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ ∂ ∂t ∂ϕ ∂η (t, η) -1 = ∂ϕ ∂η (t, η) -1 ∂a ∂η (t, ϕ(t, η)) , ∂ϕ ∂η (0, η) -1 = Id and ∂Σ ∂τ (τ, t, η) = -∂a ∂η (τ, ϕ(τ, η))Σ(τ, t, η) , Σ(t, t, η) = Id .
Thus, by Gronwall Lemma,

∂ϕ ∂η (t, η) -1 ⩽ e M 0 t , ∀(t, η) ∈ [0, T * ] × S 2 , Σ(τ, t, η) ⩽ e M 0 t-τ , ∀(τ, t, η) ∈ [0, T * ] 2 × S 2 .
(5.52)

We deduce from (5.51) and (5.52) that

∂h ∂η (t, ϕ(t, η)) ⩽ e (M 0 -k)t ∂h 0 ∂η L ∞ (S 2 ) + ∫ t 0 M 0 e (M 0 -k)(t-τ ) dτ ⩽ ∂h 0 ∂η L ∞ (S 2 ) + M 0 k-M 0 when k > M 0 ⩽ ∂h 0 ∂η L ∞ (S 2 ) + 1 when k > 2M 0 .
This ends the proof of Lemma 5 with k * = 2M 0 . ◻ Proof of Proposition 6: We apply Lemma 5 to ỹ = ŷy and D = D -D with (see equations (5.19) and (5.20))

a(t, η) ∶= -η × (ω m -ω b )(t) + 1 D(t, η) η × (v m -vb )(t) b 1 (t, η) ∶= ∇y(t, η) ⋅ η × ( ω b (t) -ω b ) + 1 D(t, η) η × ( vb (t) -v b ) b 2 (t, η) ∶= ∇D(t, η) ⋅ η × ( ω b (t) -ω b ) + 1 D(t, η) η × ( vb (t) -v b ) + vb (t) ⋅ η -v b ⋅ η .
Note that ( vb , ω b )(t) is bounded uniformly with respect to t ∈ [0, T * ] by a constant that depends only on R thanks to the decrease of the Lyapunov function V . Thus, the assumptions of Lemma 5 are satisfied with

M 0 = M 0 (R, (ω m , v m ) L ∞ (0,1) , D * , (y, D) C 1 ([0,1]×S 2 ) ) > 0 .
Therefore, there exists a constant k * > 0 (that depends on the same quantities as M 0 ) such that, for every k y , k D > k * then

(ŷ -y, D -D)(t) C 1 (S 2 ) ⩽ 1 , ∀t ∈ [0, T * ] ,
which gives the conclusion. ◻

Global solutions

The goal of this section is the proof of Proposition 2 thanks to Propositions 5 and 6.

Let

ω b 0 , vb 0 ∈ R 3 and R ∶= ( ω b 0 -ω b , vb 0 -v b ) . We define M ∶= (y, D) C 1 (S 2 ) + 1 .
By Proposition 5, there exists a time T = T (M, R) > 0 and a unique local solution of (5.24) defined on [0, T ]. By Proposition 6, we have

(ŷ, D)(T ) C 1 (S 2 ) ⩽ M .
By decreasing of the Lyapunov function, we have

( ω b -ω b , vb -v b )(T ) ⩽ 2V (T ) ⩽ 2V (0) = ( ω b 0 -ω b , vb 0 -v b ) = R
Thus we can apply Proposition 5 with initial condition at t = T and we get a solution defined on [0, 2T ]. By iterating this argument, we get a solution defined for every t ∈ [0, +∞). ◻

Continuity of the flow

The goal of this section is to prove Proposition 4. We use the same notations as in the proof of Proposition 5.

Let R ⩾ ( ω b n (0)ω b , vb n (0)v b ) for every n ∈ N. Thanks to the convergences in assumption, there exists T 1 > 0 such that the maps (F n T 1 ) n∈N and F T 1 associated to (v n , ω n , y n , D n ) n∈N and (v, ω, y, D) are (1 2)-contractions of the same space E T 1 .

Chapter 6

The geometrical flow method for velocity estimation Estimation de vitesses linéaire et angulaire à partir d'images de profondeur Dans ce chapitre est décrite une nouvelle méthode d'odométrie, basée sur le traitement d'images de profondeur seules, considérées comme un champ scalaire dense. L'adaptation de cette méthode à des images de profondeur réelles associées à un modèle de caméra pinhole est détaillée. Des résultats préliminaires d'implémentation de notre méthode sur des données réelles est prometteuse : un indice de confiance peut être associé à la vitesse calculée, ce qui est très utile en fusion de données lorsque la redondance de l'information est exploitée. Ces résultats ont été présentés dans [Zarrouati-Vissière et al., 2013a] et ont été l'objet d'une demande de brevet [START_REF] Zarrouati-Vissière | Mesure de mouvement par traitement de la profondeur[END_REF].

In this chapter, a novel odometry method based on the sole processing of depth images as a dense scalar field is described. Adaptation of this method to real depth images associated to a pinhole camera model is detailed. Preliminary results of implementation of our method on real data seem promising: a confidence index can be assigned to the resulting velocity, which is of great help in data fusion where (partial) redundancy of information is involved. These results were presented in [Zarrouati-Vissière et al., 2013a] and are the subject of a filed patent application [START_REF] Zarrouati-Vissière | Mesure de mouvement par traitement de la profondeur[END_REF]].

The geometrical flow as a conservation law

Recall from chapter 1.3 the PDE satisfied by the depth

D ∂ t D = -∇D ⋅ (η × (ω + 1 D η × v)) -v ⋅ η, see equation (1.17)
The equation is called in this dissertation the geometrical flow equation by analogy with the optical flow equation (see equation (1.16)). We will work on this PDE, with the help of two differential calculus relations, recalled in the following. These relations hold on the unit sphere S 2 , for η ∈ S 2 and P ∈ R 3 any vector independent of η: ∇ ⋅ (η × P) = 0, see equation (A.7)

∇ ⋅ (η × (η × P)) = -2η ⋅ P, see equation (A.4) where ∇⋅ denotes the divergence operator on S 2 as a Riemaniann manifold.

From (1.17), the partial time-derivative of D 3 is computed:

∂ t D 3 = -∇D 3 ⋅ (η × ω) - 3 2 ∇D 2 ⋅ (η × (η × v))) -3D 2 v ⋅ η (6.1)
Then, using (A.7):

∇D 3 ⋅ (η × ω) = ∇ ⋅ (D 3 η × ω)
and using (A.4):

∇D 2 ⋅ (η × (η × v))) = ∇ ⋅ (D 2 η × (η × v))) -D 2 ∇ ⋅ (η × (η × v))) = ∇ ⋅ (D 2 η × (η × v))) + 2D 2 v ⋅ η
Thus, (6.1) can be reformulated as

∂ t D 3 = -∇ ⋅ (D 3 η × ω) - 3 2 ∇ ⋅ (D 2 η × (η × v)))
Which means that the partial time-derivative of D 3 can be completely expressed as the divergence of a vector field. Physically, the integral formulation of this point-wise equation is equivalent to the basic assumption that the volume of the environment in which the camera is moving is constant, which is a conservation law. This does not seem like a huge discovery. However, this point-wise formulation can be expressed as a local balanced equation, which is, to our knowledge, new.

Integrating this equation on a fixed portion Ω of S 2 with boundary ∂Ω, Stokes's theorem yields

∂ t Ω D 3 dσ η = -∮ ∂Ω (D 3 η × ω)⋅ → n dl η - 3 2 ∮ ∂Ω (D 2 η × (η × v)))⋅ → n dl η
where dσ η is an infinitesimal surface element of S 2 , dl η is an infinitesimal length element, and → n is the outwards-pointing normal vector of ∂Ω.

This equation is rearranged using the double product properties

∂ t Ω D 3 dσ η = ∮ ∂Ω (D 3 η× → n)dl η ⋅ ω + 3 2 ∮ ∂Ω (D 2 η × ( → n ×η))dl η ⋅ v (6.2)
Which is simplified, noticing that → n, as the normal vector of boundary ∂Ω is an element of T η (the tangent set of S 2 at point η), which implies η × (

→ n ×η) = → n. Finally, ∂ t Ω D 3 dσ η = ∮ ∂Ω (D 3 η× → n)dl η ⋅ ω + 3 2 ∮ ∂Ω D 2 → n dl η ⋅ v (6.3)
Formally, given two consecutive depth images and a fixed domain Ω of S 2 , the left term of (6.3) can be computed by discrete approximation of the time-derivative. Computation of ∮ ∂Ω (D 3 η× → n)dl η and 3 2 ∮ ∂Ω D 2 → n dl η yields a scalar equation, linear with respect to v and ω. To each domain Ω corresponds such an equation. Six linearly independent equations yield the reconstruction of the linear and angular displacements v and ω that occurred between the two depth images.

Implementation of the method for a pinhole camera model

To adapt this method to real images, it is necessary to express the integral terms of (6.3) with respect to the real coordinates of pixels in the image. As already mentioned in Chapter 1, this requires to know the calibration model and parameters of the camera. In this section, we consider a regular pinhole camera model, where distortion is neglected. The image is a rectangular matrix, whose size defines the resolution of the image: we will denote it (r x , r y ). A pixel's position is denoted (i, j) ∈ (1, r x ) × (1, r y ). Using the notations of Chapter 1, the unitary vector η corresponding to the pixel's position (i, j) is given by

η = 1 ( i-x 0 f rx ) 2 + ( j-y 0 f ry ) 2 + 1 ⎛ ⎜ ⎜ ⎝ i-x 0 f rx j-y 0 f ry 1 ⎞ ⎟ ⎟ ⎠
For sake of simplicity, we define z 1 ∶= i-x 0 f rx and z 2 ∶= j-y 0 f ry , so that

η = 1 z 2 1 + z 2 2 + 1 (z 1 , z 2 , 1) T .
The implementation of the proposed method for a real image requires the choice of domains Ω on S 2 : a natural choice is to ensure that the domains, when reprojected on the plane image, are identified with rectangles (the smallest possible rectangle is a single pixel). In particular, the infinitesimal surface element dσ η is, reprojected on S 2 , the area corresponding to a pixel: it naturally depends on the position of this pixel in the image. The value of this area is established by the following consideration: it is simply the Euclidean norm of the vector dη 1 × dη 2 , where vectors dη 1 and dη 2 are infinitesimal displacements along each side of the pixel:

dη 1 ∶= ∂η ∂z 1 dz 1 , and dη 2 ∶= ∂η ∂z 2 dz 2
where dz 1 et dz 2 are the length and the width of a pixel. Computation yields

∂η ∂z 1 = 1 (1 + z 2 1 + z 2 2 ) 3 2 ⎛ ⎜ ⎝ 1 + z 2 2 -z 1 z 2 -z 1 ⎞ ⎟ ⎠ , ∂η ∂z 2 = 1 (1 + z 2 1 + z 2 2 ) 3 2 ⎛ ⎜ ⎝ -z 1 z 2 1 + z 2 1 -z 2 ⎞ ⎟ ⎠ hence dσ η = dz 1 dz 2 (1 + z 2 1 + z 2 2 ) 3 2 .
Similarly, an infinitesimal length element dl η corresponds to the norm of dη 1 or dη 2 , depending on the direction of displacement along ∂Ω: horizontally corresponds to dl η 1 and vertically to dl η 2 . This yields

dl η 1 = 1 + z 2 2 1 + z 2 1 + z 2 2 dz 1 , dl η 2 = 1 + z 2 1 1 + z 2 1 + z 2 2 dz 2 .
The outwards-pointing normal → n along ∂Ω corresponds to the unit vector of T η whose direction is given by ±dη i × η (i = 1, 2): the sign depends on the direction in which the path is run, and dη i is only the orientation. Thus

→ n +1 = 1 1 + z 2 2 ⎛ ⎜ ⎝ 0 -1 z 2 ⎞ ⎟ ⎠ , → n -1 = 1 1 + z 2 2 ⎛ ⎜ ⎝ 0 1 -z 2 ⎞ ⎟ ⎠ , → n +2 = 1 1 + z 2 1 ⎛ ⎜ ⎝ 1 0 -z 1 ⎞ ⎟ ⎠ , → n -2 = 1 1 + z 2 1 ⎛ ⎜ ⎝ -1 0 z 1 ⎞ ⎟ ⎠
The previous computations are aggregated to finally obtain the expressions of η× → n dl η and → n dl η on any of the sides of a given rectangle, which is parameterized by the side coordinates (z 1 , z 1 , z 2 , z 2 ). These expressions are gathered in the following table:

z 1 z 2 (η× → n)dl η → n dl η ∈ (z 1 , z 1 ) z 2 dz 1 (1+z 2 1 +z 2 2 ) 3 2 ⎛ ⎜ ⎝ 1 + z 2 2 -z 1 z 2 -z 1 ⎞ ⎟ ⎠ dz 1 1+z 2 1 +z 2 2 ⎛ ⎜ ⎝ 0 -1 z 2 ⎞ ⎟ ⎠ z 1 ∈ (z 2 , z 2 ) dz 2 (1+z 1 2 +z 2 2 ) 3 2 ⎛ ⎜ ⎝ -z 1 z 2 (1 + z 1 2 ) -z 2 ⎞ ⎟ ⎠ dz 2 1+z 1 2 +z 2 2 ⎛ ⎜ ⎝ 1 0 -z 1 ⎞ ⎟ ⎠ ∈ (z 1 , z 1 ) z 2 dz 1 (1+z 2 1 +z 2 2 ) 3 2 ⎛ ⎜ ⎝ -(1 + z 2 2 ) z 1 z 2 z 1 ⎞ ⎟ ⎠ dz 1 1+z 2 1 +z 2 2 ⎛ ⎜ ⎝ 0 1 -z 2 ⎞ ⎟ ⎠ z 1 ∈ (z 2 , z 2 ) dz 2 (1+z 1 2 +z 2 2 ) 3 2 ⎛ ⎜ ⎝ z 1 z 2 -(1 + z 1 2 ) z 2 ⎞ ⎟ ⎠ dz 2 1+z 1 2 +z 2 2 ⎛ ⎜ ⎝ -1 0 z 1 ⎞ ⎟ ⎠

Implementation of the method for a pinhole camera model including radial distortion

In this section, we use the notations of Chapter 1. Similarly to the previous implementation, the first step is to express the unitary vector η with respect to pixel coordinates (i, j). The fish-eye distortion model described by (1.6) yields

η = 1 tan(rw) 2 (2 tan(w 2)) 2 + 1 ⎛ ⎜ ⎜ ⎝ z 1 tan(rw) 2r tan(w 2) z 2 tan(rw) 2r tan(w 2) 1 ⎞ ⎟ ⎟ ⎠
where r ∶= z 2 1 + z 2 2 (same definitions for z 1 and z 2 than in the previous section) and w is the distortion coefficient.

Then dσ η is computed using the same method than in the previous section:

dσ η = 2w tan(rw) tan(w 2) 2 (1 + tan(rw) 2 ) r(4 + tan(rw) 2 tan(w 2) 2 ) 3 2 dz 1 dz 2 .
Finally, the expressions of η× → n dl η and → n dl η on the four sides of a given rectangle are computed and gathered in the following table. For sake of concision of the relations, we use the following notations: C w = cos(w 2), S w = sin(w 2), C r = cos(wr), S r = sin(wr). .3: Angular velocities: measurements using the geometrical flow method (blue) and gyrometers measurements (red). Both measurements are perfectly consistent, with the sole exception that the gyrometers measurements are biased, and the geometrical flow measurements are noisy. The magnitude of the difference does not exceed 10 -3 rad.s -1 .

z 1 ∈ (z 1 , z 1 ) z 2 = z 2 (η× → n)dl η = dz 1 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) 3 2 ⎛ ⎜ ⎝ (z 2 2 S r (4S 2 w C 2 r + C 2 w S 2 r ) + 4wrz 2 1 S 2 w C r ) -z 1 z 2 (C 2 w S 3 r + 4S 2 w C r (C r S r -wr)) -2z 1 r 2 wC w S r S w ⎞ ⎟ ⎠ z 1 ∈ (z 1 , z 1 ) z 2 = z 2 → n dl η = dz 1 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) ⎛ ⎜ ⎝ -2z 1 z 2 S w (S r C r -wr) -2S w (z 2 2 S r C r + wz 2 1 r) z 2 rC w S 2 r ⎞ ⎟ ⎠ z 1 = z 1 z ∈ (z 2 , z 2 ) (η× → n)dl η = dz 2 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) 3 2 ⎛ ⎜ ⎝ -z 1 z 2 (C 2 w S 3 r + 4S 2 w C r (C r S r -wr)) (z 1 2 S r (C 2 w S 2 r + 4S 2 w C 2 r ) + 4wrz 2 2 S 2 w C r ) -2r 2 wz 2 C w S r S w ⎞ ⎟ ⎠ z 1 = z 1 z ∈ (z 2 , z 2 ) → n dl η = dz 2 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) ⎛ ⎜ ⎝ 2S w (z 1 2 S r C r + wz 2 2 r) 2z 1 z 2 S w (S r C r -wr) -z 1 rC w S 2 r ⎞ ⎟ ⎠ z 1 ∈ (z 1 , z 1 ) z 2 = z 2 (η× → n)dl η = dz 1 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) 3 2 ⎛ ⎜ ⎝ -(z 2 2 S r (4S 2 w C 2 r + C 2 w S 2 r ) + 4wrz1 2 S 2 w C r ) z 1 z 2 (C 2 w S 3 r + 4S 2 w C r (C r S r -wr)) 2z 1 r 2 wC w S r S w ⎞ ⎟ ⎠ z 1 ∈ (z 1 , z 1 ) z 2 = z 2 → n dl η = dz 1 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) ⎛ ⎜ ⎝ 2z 1 z 2 S w (S r C r -wr) 2S w (z 2 2 S r C r + wz 2 1 r) -z 2 rC w S 2 r ⎞ ⎟ ⎠ z 1 = z 1 z ∈ (z 2 , z 2 ) (η× → n)dl η = dz 2 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) 3 2 ⎛ ⎜ ⎝ z 1 z 2 (C 2 w S 3 r + 4S 2 w C r (C r S r -wr)) -(z 1 2 S r (C 2 w S 2 r + 4S 2 w C 2 r ) + 4wrz 2 2 S 2 w C r ) 2r 2 wz 2 C w S r S w ⎞ ⎟ ⎠ z 1 = z 1 z ∈ (z 2 , z 2 ) → n dl η = dz 2 Cw r 3 (4S 2 w C 2 r +C 2 w S 2 r ) ⎛ ⎜ ⎝ -2S w (z 1 2 S r C r + wz 2 2 r) -2z 1 z 2 S w (S r C r -wr) z 1 rC w S 2
To the questions concerning the influence of the environment, one can immediately answer that the most favorable environment is the one that satisfies the hypothesis that enabled to formulate the geometrical flow equation in Section 1.3.1, especially hypothesis (H1). However, this is not sufficient to ensure that the 6-DOF motion can be fully reconstructed. In fact, the possibility to reconstruct the motion of the camera is ensured by the same condition that was pointed in Chapter 5 for observability of biases on velocity (see Theorem 3): the environment should not admit a rotation axis. This is proved working by contradiction and using usual linear algebra tools and Stokes formula.

This yields an issue for implementation of this method on real depth images: given that the actual field of view of the camera is much narrower than the entire field of view of the unit sphere in the ideal spherical model, a non-cylindrical symmetry should be visible even from a small visible part of the scene. For a given environment, ensuring that, for any point of view of the camera, the visible part of the scene won't admit a rotation axis is a difficult task, which requires a precise work on the geometry of the environment: in our opinion, this can not be achieved in real-time.

Similarly, the choice of the most efficient domains is a critical task: ideally, we would like to be able to choose only six domains that would enable to invert the linear system of six equations written in the form of equation ( 6.3). Unfortunately, even if a precise characterization of the environment ensures that 6 such domains exist, their choice require a thorough analysis of the geometry of the scene.

Fortunately, it is possible, once the domains are chosen and the (rectangular) linear system is (pseudo) inverted, to know the quality of the measurements: the covariance of the result is directly encrypted in the pseudo-inverse of the linear system weighted by the residuals of the linear system. This a precious information for data fusion such as Kalman filtering.

It is rather difficult to compare this novel method to other known techniques. The closest algorithm that can perform 6-DOF motion estimation from depth data is (to our knowledge) the Iterative Closest Point (ICP) algorithm, which is used in the seminal work known as Kinect fusion [START_REF] Izadi | Kinectfusion: real-time 3D reconstruction and interaction using a moving depth camera[END_REF]. ICP performs an estimation of displacement by iterative alignment of two point clouds, without any explicit point correspondence ( [START_REF] Rusinkiewicz | Efficient variants of the ICP algorithm[END_REF]). Variants have been studied to take into account surface normals or elastic deformations of point clouds (e.g for medical applications). It is mostly used for depth data merging. This algorithm has two main limitations: its domain of convergence is very local, and it requires an important processing time. By comparison, our proposed method runs at 140 Hz on a commercial laptop purchased in 2011 including an Intel Core i5-2520M CPU @ 2.50 GHz processor. To be specific, depending on the number of domains used to form the rectangular linear system to be inverted, the processing time can vary (see Table 6.1 for a comparison).

See for example [START_REF] Gold | New algorithms for 2D and 3D point matching: pose estimation and correspondence[END_REF]], [Zhang, 1994], [START_REF] Li | The 3D-3D registration problem revisited[END_REF] for more details on ICP variants. Dense depth data can also be used as an aid for SLAM techniques: for example, it is well adapted to plane extraction [Georgiev et al., 2011, Dube and[START_REF] Dube | [END_REF], which can then be used in SLAM algorithms [START_REF] Taguchi | SLAM using both points and planes for hand-held 3D sensors[END_REF]]. An interesting approach using RGBD images and presented in [START_REF] Steinbrucker | Real-time visual odometry from dense RGB-D images[END_REF] sensitive to noise due to the explicit use of image gradients. It is used conjointly with ICP in [Whelan et al., 2013a].

Chapter 7

Estimation of biases on angular velocity and acceleration

Estimation de biais pour le filtrage de la vitesse linéaire

Etant données des mesures de vitesses linéaire et angulaire par la méthode du flot géométrique telle que décrite en Chapitre 6, l'estimation de biais sur des données issues de gyromètres et d'accéléromètres est possible : on utilise pour cela des gyromètres et des accéléromètres biaisés et des magnétomètres. L'observabilité est garantie pour des mouvements non-planaires, et l'observateur est associé à une fonction de Lyapunov. On en déduit un système d'équations différentielles non-linéaires, dont la convergence asymptotique est étudiée dans le cadre d'hypothèses de bornitude sur les vitesses et de périodicité sur la vitesse angulaire. En outre, la périodicité des trajectoires garantit une convergence exponentielle locale, qui rend l'observateur robuste au bruit ou aux perturbations. L'implémentation sur des données réelles montre une estimation correcte des biais angulaires ; quant aux biais sur les accéléromètres, il n'est pas possible de conclure à la précision des résultats faute de référence, mais l'estimation réalisée permet une amélioration notable de la reconstruction de trajectoire. Ces résultats ont été présentés dans [Zarrouati-Vissière et al., 2013a].

Given the linear and angular velocity measurements provided by the geometrical flow method described in Chapter 6, estimation of biases on accelerometers and gyrometers is made possible, relying on biased accelerometers and gyrometers, and magnetometers measurements. Observability is guaranteed for non planar motion, and the observer design is based on a Lyapunov functional. It yields a system of non linear differential equations, whose asymptotic convergence towards equilibrium is studied in the framework of bounded velocities and periodic angular velocity. In addition, the periodicity of the trajectory guarantees local exponential convergence, which makes this observer robust to noise or perturbations. Implementation on real data shows that biases on angular velocity are correctly estimated; as for accelerometers biases, no reference enables to conclude on the accuracy of our results, but it is possible to infer that trajectory estimation is indeed improved. These results were presented in [Zarrouati-Vissière et al., 2013a].

Problem formulation

The equations of motion of a 3D rigid body described in Appendix C, and more specifically equation C.5, can be rearranged in the following way:

∂ t v = v × ω + g + a ∂ t g = g × ω
where v and ω are the linear and angular velocity, g is the gravitational acceleration and a is the specific acceleration of the body, all of them expressed in the body frame. Embedded sensors make some measurements available: as real equipment, data contain errors and in particular constant errors, called biases. These biases should be estimated on line: this is the goal of this study, as well as trajectory reconstruction. To be specific, the measures are:

a m = a + a b , ω m = ω + ω b
where a b and ω b are constant biases. Thus, the state equations of the system are

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t v = v × (ω m -ω b ) + g + a m -a b , ∂ t g = g × (ω m -ω b ), ∂ t ω b = 0, ∂ t a b = 0. (7.1)
In addition, measures of the linear and angular velocities are extracted from depth images processing (see Chapter 6): v m and ω m ′ . Comparison of ω m ′ and ω m gives a so-called measure of the angular bias: ω bm . Finally, g, as the measure in a moving frame of the constant vector A, has a constant norm. A last class of embedded sensors, the magnetometers, gives a measure b in the body frame of the magnetic field B. A and B are known vectors. This yields the output equation

b ⋅ g = A ⋅ B.
The inputs of this system are ω m and a m , the outputs are v m , ω bm and b.

Observability

To study the observability of the system, one looks for the assumptions on the trajectory under which two states

(v i , g i , ω b i , a b i ) (i = 1, 2)
,verifying the same state equations with same inputs and outputs are necessarily equal. Since v m is measured,

v 1 = v 2 . Similarly, ω b 1 = ω b 2 .

98

Defining δg = g 1g 2 and δa b = a b 1a b 2 , the subtraction of the two systems of state equations yields

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ 0 = δg -δa b , ∂ t δg = δg × ω, 0 = 0, ∂ t δa b = 0,
and the output equation becomes

δg ⋅ b = 0. This is equivalent to ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ δa b = δg, ∂ t δa b = 0, δa b × ω = 0, δa b ⋅ b = 0. (7.2)
which has to be verified for any time t. Thus, a necessary condition on the trajectory for this system to have a non trivial solution δa b is that ω(t) remains collinear to a fixed direction (δa b ). In addition, this direction has to be orthogonal to b.

Proposition 7. The system is observable if and only if the angular velocity does not remain collinear to a fixed direction orthogonal to the magnetic field expressed in the body frame.

Remark 3. The following condition is sufficient for observability: ∃t 1 such that at least one of the following conditions is true ω(t 1 ) × ω(t 1 ) ≠ 0, b(t 1 ) ⋅ ω(t 1 ) ≠ 0.

(7.3)

Observer design

The observer is designed as follows:

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t v = v × (ω m -ω b ) + ĝ + a m -a b -k v (v -v m ), ∂ t ĝ = ĝ × (ω m -ω bm ) -k g1 (v -v m ) -k g2 (ĝ ⋅ b -A ⋅ B)b, ∂ t ω b = -k w1 ( ω b -ω bm ) + k w2 (v -v m ) × v, ∂ t a b = k a (v -v m ).
(7.4)

From systems (7.1) and (7.4), and assuming that the measures v m and ω bm are close enough to the real states v and ω b so that in particular, their dynamics are the same as the real dynamics, the error dynamics is

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t ṽ = ṽ × (ω m -ω b ) -v × ω b + g -a b -k v ṽ, ∂ t g = g × (ω m -ω b ) -k g1 ṽ -k g2 (g ⋅ b)b, ∂ t ω b = -k w1 ω b + k w2 ṽ × v, ∂ t a b = k a ṽ. (7.5) where ṽ = v -v m , g = ĝ -g, ω b = ω b -ω bm and a b = a b -a b .
Linear velocity measurements (v m ) are taken into account to correct the estimation of all states: although it seems quite natural for the linear velocity and the bias on acceleration estimates, that might be surprising for the specific acceleration and especially for the rotational bias estimates. Indeed, measurements of this bias being available, its estimation is straightforward. Actually, the correction terms are chosen so that the following Lyapunov functional is positive and decreases along the trajectories:

V = λ v ṽ 2 + λ g g 2 + λ w ω b 2 + λ a a b 2 .
Indeed,

V = -λ v k v ṽ 2 -λ w k w1 ω b 2 -λ g k g2 (g ⋅ b) 2 + (λ v -λ g k g1 )ṽ ⋅ g + (λ a k a -λ v )ṽ ⋅ a b + (λ w k w2 -λ v )ṽ ⋅ ω b
Coupled terms are brought into play, that are canceled by the choice of specific correction gains. Choosing k v , k w1 , k g2 > 0, k g1 ∶= λv λg , k a ∶= λv λa and k w2 ∶= λv λw yields

V = -λ v k v ṽ 2 -λ w k w1 ω b 2 -λ g k g2 (g ⋅ b) 2 ≤ 0.

Asymptotic convergence

Proposition 8. Assuming that linear and angular velocities are bounded and angular velocity is T -periodic, the error dynamics of the observer (defined in (7.5)) converge towards zero for any initial condition.

Proof.

Step 1: Let us prove that ṽ, ω b and g ⋅ b tend to 0 as t tends to ∞.

The Lyapunov functional V decreases along the trajectories of the system and is positive, thus it tends to a limit V ∞ ≥ 0. Using the previous expression of the time derivative of V ,

V (t) = V (0) - t 0 λ v k v ṽ(s) 2 + λ w k w1 ω b (s) 2 + λ g k g2 (g(s) ⋅ b(s)) 2 ds
and we deduce that t ↦ ṽ(t), t ↦ ω b (t) and t ↦ g(t) ⋅ b(t) ∈ L 2 (0, ∞).

Then, V (t) ∈ L ∞ (0, ∞) implies that ṽ(t), ω b (t), g(t) and a b (t) ∈ L ∞ (0, ∞) themselves. Assuming that t ↦ v(t) and t ↦ ω(t) ∈ L ∞ (0, ∞), the time derivatives of ṽ, ω b and g ⋅ b are bounded. Thus ṽ, ω b and g ⋅ b are uniformly continuous. Using Barbalat's lemma, we conclude that (ṽ, ω b , g ⋅ b)(t) → t→+∞ (0, 0, 0).

Step 2: The periodic system Since the angular velocity is T -periodic, let us define (t n ) n∈N an increasing sequence such that t n →

0 ∞ , ṽ0 ∞ , g0 ∞ , ω b 0 ∞ , a b 0 ∞ ) ∈ (R 3 ) 5 such that (b, ṽ, g, ω b , a b )(t n ) → n→+∞ (b 0 ∞ , ṽ0 ∞ , g0 ∞ , ω b 0 ∞ , a b 0 ∞ ).
Let us define the maps (α, +∞) ∋ t ↦ (b ∞ , ṽ∞ , g∞ , ω b∞ , a b∞ )(t) ∈ (R 3 ) 5 solutions of the following Cauchy problem:

Σ ∞ ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t b ∞ = b ∞ × ω, ∂ t ṽ∞ = ṽ∞ × ω + g∞ -a b∞ -k v ṽ∞ , ∂ t g∞ = g∞ × ω -k g1 ṽ∞ , ∂ t ω b∞ = -k w1 ω b∞ , ∂ t a b∞ = k a ṽ∞ , (7.6) with initial conditions ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ b ∞ (α) = b 0 ∞ , ṽ∞ (α) = ṽ0 ∞ , g∞ (α) = g0 ∞ , ω b∞ (α) = ω b 0 ∞ , a b∞ (α) = a b 0 ∞ . (7.7) Thanks to Step 1, we know that ṽ0 ∞ = 0, ω b 0 ∞ = 0, g0 ∞ ⋅ b 0 ∞ = 0.
Let us define X ∶= (b, ṽ, g, ω b , a b ) and X ∞ = (b ∞ , ṽ∞ , g∞ , ω b∞ , a b∞ ).

Step 3: Let us prove that ∀τ > 0,

X(t n + τ ) → n→+∞ X ∞ (α + τ ). Let τ > 0.
On one hand, X ∞ is the solution of the periodic linear system of dynamical equations Σ ∞ : it writes ∂ t X ∞ = A ∞ (t)X ∞ . On the other hand, X is the solution of almost the same system, differing by an additive term that is proven to tend to 0 as t tends to ∞:

∂ t X = A ∞ (t)X + c(t) with c(t) = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ 0 -v × ω b -k g2 b(g ⋅ b) k w2 ṽ × v 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ (7.8) Let us introduce R ∞ (t, s) the resolvant asssociated to A ∞ (t): X ∞ (t) = R ∞ (t, s) ⋅ X ∞ (s) ∀s, t > 0.
In particular,

X ∞ (α + τ ) = R ∞ (α + τ, α) ⋅ X 0 ∞ ∀τ > 0. (7.9) Since the system Σ ∞ is T-periodic, R ∞ (t + T, s + T ) = R ∞ (t, s). Let us define (k n ) n∈N such that t n = k n T + α n . From Step 2, α n → n→+∞ α. Then, ∀n ∈ N, R ∞ (t n + τ, t n ) = R ∞ (α n + τ, α n ) and R ∞ (α n + τ, α n ) → n→+∞ R ∞ (α + τ, α) (7.10)
by continuity of the resolvant. Now, from the variation of constants method,

X(t n + τ ) = R ∞ (t n + τ, t n ) ⋅ X(t n ) + tn+τ tn R ∞ (t n + τ, s) ⋅ c(s)ds.
(7.11) Substracting (7.9) and(7.11) yields

X(t n + τ ) -X ∞ (α + τ ) = R ∞ (t n + τ, t n ) ⋅ X(t n ) -R ∞ (α + τ, α) ⋅ X 0 ∞ + tn+τ tn R ∞ (t n + τ, s) ⋅ c(s)ds.
Thus,

X(t n + τ ) -X ∞ (α + τ ) ≤ A n (τ ) + B n (τ ) with A n (τ ) = R ∞ (t n + τ, t n ) ⋅ X(t n ) -R ∞ (α + τ, α) ⋅ X 0 ∞ and B n (τ ) = tn+τ tn R ∞ (t n + τ, s) ⋅ c(s)ds.
First, let us prove that A n (τ ) → n→+∞ 0 ∀τ > 0:

A n = (R ∞ (t n + τ, t n ) -R ∞ (α + τ, α)) ⋅ X(t n ) + R ∞ (α + τ, α) ⋅ (X(t n ) -X 0 ∞ ) = (R ∞ (α n + τ, α n ) -R ∞ (α + τ, α)) ⋅ X(t n ) + R ∞ (α + τ, α) ⋅ (X(t n ) -X 0 ∞ ) ≤ (R ∞ (α n + τ, α n ) -R ∞ (α + τ, α)) sup n∈N X(t n ) + R ∞ (α + τ, α) sup n∈N X(t n ) -X 0 ∞ From (7.10), (R ∞ (α n + τ, α) -R ∞ (α + τ, α)) → n→+∞ 0. From Step 1, sup n∈N X(t n ) < +∞ By definition of X 0 ∞ , X(t n ) -X 0 ∞ → n→+∞ 0. This proves that A n → n→+∞ 0. Let us prove that B n → n→+∞ 0. Let us define the maps f n ∶ s ∈ (0, τ ) ↦ c(t n + s). By definition of c(t), ∀s ∈ (0, τ ), f n (s) → n→+∞ 0. As f n (s) is bounded on (0, τ ), according to the dominated convergence theorem, f n L 1 (0,τ ) → n→+∞ 0. Thus c L 1 (tn,tn+τ ) → n→+∞ 0. Since B n ≤ τ R ∞ c L 1 (tn,tn+τ ) , we proved that B n → n→+∞ 0. Thus, X(t n + τ ) → n→+∞ X ∞ (α + τ ).
Step 4: Let us prove that ∀τ > 0, X ∞ (α + τ ) = 0 From Step 1 and Step 3, we know that

∀τ > 0, ⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ ṽ∞ (α + τ ) = 0 ω b∞ (α + τ ) = 0 g∞ (α + τ ) ⋅ b ∞ (α + τ ) = 0 (7.12)
Since X ∞ is the solution of Σ ∞ and ω ∈ C 1 (α, +∞), we deduce that X ∞ ∈ C 1 (α, +∞) and thus, from the two first equalities of (7.12) that

∂ t ṽ∞ (α + τ ) = 0, ∂ t ω b∞ (α + τ ) = 0 ∀τ > 0.
Injecting all of this in Σ ∞ yields (7.13) which, in addition to the third equality of (7.12), is equivalent to

∀τ > 0, ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t b ∞ (α + τ ) = b ∞ (α + τ ) × ω(α + τ ), 0 = g∞ (α + τ ) -a b∞ (α + τ ), ∂ t g∞ (α + τ ) = g∞ (α + τ ) × ω(α + τ ), ∂ t a b∞ (α + τ ) = 0,
∀τ > 0, ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ a b∞ (α + τ ) = g∞ (α + τ ), ∂ t a b∞ (α + τ ) = 0, a b∞ (α + τ ) × ω(α + τ ) = 0, a b∞ (α + τ ) ⋅ b ∞ (α + τ ) = 0.
Finally, using the Proposition 7 and the following Remark, we deduce that if ∃t 1 such that ω(t 1 ) × ω(t 1 ) ≠ 0 or b(t 1 ) ⋅ ω(t 1 ) ≠ 0, then a b∞ (α + τ ) = a b∞ (α + τ ) = 0∀τ > 0, which concludes the proof of Step 4.

Exponential convergence

Proposition 9. If in addition to the previous assumptions, we assume that the trajectory is T -periodic, the observer exponentially locally converges towards the real trajectory.

Proof. Since the trajectory is T-periodic, the time functions b(t), v(t) and ω(t) are T-periodic.

To prove the exponential convergence, let us underline how the steps of the proof of asymptotic convergence are modified. The step 1 remains unchanged. In step 2, the Σ ∞ system is defined as in the following: (7.14) with same initial conditions. In the step 3, the term c(t) is now defined as

Σ ∞ ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t b ∞ = b ∞ × ω, ∂ t ṽ∞ = ṽ∞ × ω -v × ω b∞ + g∞ -a b∞ -k v ṽ∞ , ∂ t g∞ = g∞ × ω -k g1 ṽ∞ -k g2 (g ∞ ⋅ b ∞ )b ∞ , ∂ t ω b∞ = -k w1 ω b∞ + k w2 ṽ∞ × v, ∂ t a b∞ = k a ṽ∞ ,
c(t) = ⎛ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ 0 -ṽ × ω b 0 0 0 ⎞ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ . (7.15)
Same conditions remain true on the time function c and on the resolvant of Σ ∞ , thus the conclusion holds:

∀τ > 0, X(t n + τ ) → n→+∞ X ∞ (α + τ ).
In the Step 4, we deduce as previously from the observability assumption on the system that ∀τ > 0, X ∞ (α + τ ) = 0. Then, since Σ ∞ is periodic, a usual result on periodic systems states that ∃ Ū , λ > 0 such that R ∞ (t, s) ≤ Ū exp -λ(t-s) . Then

X(t) = R ∞ (t) ⋅ X(0) + t 0 R ∞ (t, s) ⋅ c(s)ds
where c is quadratic in X: ∃µ > 0 such that c(s) ≤ µ X(s) 2 . Thus

X(t) ≤ Ū exp -λt X(0) + t 0 Ū exp -λ(t-s) µ X(s) 2 ds or X(t) exp λt ≤ Ū X(0) + t 0 Ū exp λs µ X(s) 2 ds Let us define δ 0 ∶= X(0) , δ 1 > max(δ 0 , Ū δ 0 ), t 1 ∶= sup{t > 0; X(s) ≤ δ 1 , ∀s ∈ (0, t)}.
Working by contradiction, we assume that t 1 < +∞. We apply Gronwall's lemma to the inequality

X(t) exp λt ≤ Ū δ 0 + t 0 Ū µδ 1 exp λs X(s) ds, t ≤ t 1 . It yields X(t) exp λt ≤ Ū δ 0 exp Ū µδ 1 t X(t) ≤ δ 1 exp ( Ū µδ 1 -λ)t
Assuming δ 1 ≤ λ (2µ Ū ) yields X(t) ≤ δ 1 exp -λ 2t : in particular, this implies that X(t 1 ) < δ 1 , which contradicts the definition of t 1 . To conclude, under the assumption that X(0) ≤ min(λ (2µ Ū ), λ (2µ Ū 2 )), X(t) has an exponential decay:

X(t) ≤ λ (2µ Ū ) exp -λ 2t .
The asymptotic convergence of the periodic system yields the exponential convergence of the complete system for initial conditions close enough to the real trajectory.

Robustness to noise

As for any real system equipped with sensors, measurements are not ideal: the inputs and outputs of the system are noisy representations of the real variables that they represent. Let us define

(ξ a , ξ w , ξ wb , ξ v , ξ b ) such that ⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ a m = a + a b + ξ a , ω m = ω + ω b + ξ w , ω bm = ω b + ξ wb , v m = v + ξ v , b m = b + ξ b . (7.16)
Using the observer defined by (7.4), the error dynamics is now given by

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t ṽ = ṽ × ω -v × ω b + g -a b -k v ṽ + v × ξ w + ξ a + k v ξ v + ṽ × ξ w , ∂ t g = g × ω -k g1 ṽ -k g2 (g ⋅ b)b + g × (ξ w -ξ wb ) + k g1 ξ v -k g2 (g ⋅ b)ξ b -k g2 (ĝ ⋅ ξ b )(b + ξ b ) +g × (ξ w -ξ wb ), ∂ t ω b = -k w1 ω b + k w2 ṽ × v + k w1 ξ wb -k w2 ξ v × v -k w2 ξ v × ṽ, ∂ t a b = k a ṽ -k a ξ v .
(7.17) In compact form, and using the notations of Section 7.5 studying exponential convergence, this writes:

∂ t Y = A ∞ (t)Y + c(t) + f 1 (t) + f 2 (t)Y
where

f 1 (t) ∶= ⎛ ⎜ ⎜ ⎜ ⎝ v × ξ w + ξ a + k v ξ v g × (ξ w -ξ wb ) + k g1 ξ v -k g2 (g ⋅ b)ξ b -k g2 (ĝ ⋅ ξ b )(b + ξ b ) k w1 ξ wb -k w2 ξ v × v -k a ξ v ⎞ ⎟ ⎟ ⎟ ⎠ , (7.18)
and

f 2 (t)Y ∶= ⎛ ⎜ ⎜ ⎜ ⎝ ṽ × ξ w g × (ξ w -ξ wb ) -k w2 ξ v × ṽ 0 ⎞ ⎟ ⎟ ⎟ ⎠ . (7.19)
Recall also that -the resolvent R ∞ associated to the linear T-periodic matrix

A ∞ satisfies R ∞ (t, s) ≤ Ū exp -λ(t-s) ; -c(t) is actually quadratic in Y (t): c(t) = Q(Y (t)).
Let us recall the result of Section 7.5 on the following Cauchy problem:

∂ t X = A ∞ (t)X + c(t) X(0) = X 0 . (7.20) ∃δ, K, λ > 0 such that X 0 < δ implies X(t) ≤ K X 0 exp -λt ∀t > 0. (7.21)
The following proposition states that our observer is locally robust to deterministic noise on measurements:

Proposition 10. In addition to the previous assumptions, let us assume that the perturbations are characterized by some constants F 1 and

F 2 such that f 1 (t) ≤ F 1 and f 2 (t)Y ≤ F 2 Y . X(t)
is the solution of (7.20) with X 0 < δ so that (7.21) is satisfied. Then the solution Y to the Cauchy problem

∂ t Y = A ∞ (t)Y + Q(Y (t)) + f 1 (t) + f 2 (t)Y Y (0) = X 0 (7.22) satisfies Y (t) -X(t) ≤ K L (F 1 + F 2 X 0 ) c L + o(F 1 + F 2 )
for some positive constants K L and c L depending on the initial condition X 0 and the level of perturbation F 2 .

Proof.

Step 1: Linearization of the perturbed system Let us define Y 1 the solution of the following Cauchy problem:

∂ t Y 1 = (A ∞ (t) + ∂ X Q(X(t)) + f 2 (t)) Y 1 + f 1 (t) + f 2 (t)X(t) Y 1 (0) = 0 (7.23) Since Q is quadratic in X, ∂ X Q(X(t)) is linear in X: in particular, (7.21) implies that ∂ X Q(X(t)) ≤ Q 1 for some positive constant Q 1 ( X 0 ). Similarly, (7.21) implies that f 2 (t)X(t) ≤ F 2 X 0 .
Step 2: Let us prove that the resolvent associated to the linearized system has an exponential decay. Let us consider the following Cauchy problem

∂ t Y 2 = (A ∞ (t) + ∂ X Q(X(t)) + f 2 (t)) Y 2 Y 2 (0) = Y 0 (7.24)
The explicit solution of this problem is given by the method of variation of constants, where R ∞ is the resolvent associated to A ∞ :

Y 2 (t) = R ∞ (t, 0)Y 0 + t 0 R ∞ (t, s)(∂ X Q(X(s)) + f 2 (s))Y 2 (s)ds.
This yields

Y 2 (t) ≤ R ∞ (t, 0) Y 0 + t 0 R ∞ (t, s) Q 1 ( X 0 + F 2 Y 2 (s) ds, Y 2 (t) ≤ Ū Y 0 exp -λt + t 0 Ū exp -λ(t-s) Q 1 ( X 0 ) + F 2 Y 2 (s) ds.
Then, by a similar reasoning than used in Section 7.5,

∃δ y , K L ( X 0 , σ 2 ) and c L ( X 0 , F 2 ) such that Y 0 < δ y implies Y 2 (t) ≤ K L Y 0 exp -c L t ∀t > 0. (7.25)
On the other hand, the solution of the Cauchy problem

Y 2 writes Y 2 (t) = R L (t, 0)Y 0 , where R L is the resolvent associated to A ∞ (t) + ∂ X Q(X(t)) + f 2 (t). This proves that R L (t, s) ≤ K L exp -c L (t-s) .
Step 3: Let us prove that Y 1 (t) ≤

K L (F 1 +F 2 X 0 ) c L
. Using the definition of R L and the variation of constants to the linearized Cauchy problem (7.23) yields

Y 1 (t) = t 0 R L (t, s)(f 1 (s) + f 2 (s)X(s))ds, thus Y 1 (t) ≤ t 0 K L exp -c L (t-s) (F 1 + F 2 X 0 )ds,
and finally

Y 1 (t) ≤ K L (F 1 + F 2 X 0 ) c L .
Step 4:

Conclusion We proved that Y 1 (t) = O(F 1 + F 2 ). Let us introduce the function Y 3 = Y -X -Y 1 .
We write Y 3 = (F 1 + F 2 )y. We will prove that y = O(F 1 + F 2 ). Y 3 is the solution of the following Cauchy problem:

∂ t Y 3 = (A ∞ (t)) Y 3 + Q(Y 3 + X + Y 1 ) -Q(X) -∂ X Q(X(t)) ⋅ Y 1 + f 2 (t)(X(t) + Y 3 (t)) Y 3 (0) = 0 (7.26) Since Q is quadratic, Q(Y 3 + X + Y 1 ) can be written in the form Q(Y 3 ) + Q(X + Y 1 ) + LY 3 ⋅ (X + Y 1 ) where L is a constant. Since Y 1 = O(F 1 + F 2 ), Q(X + Y 1 ) can be written in the form Q(X) + ∂ X Q(X(t)) ⋅ Y 1 + (F 1 + F 2 )o(1).
Thus, (7.26) writes

∂ t Y 3 = (A ∞ (t) + L(X + Y 1 ) + f 2 (t)) Y 3 + Q(Y 3 ) + (F 1 + F 2 )o(1) + f 2 (t)X(t) Y 3 (0) = 0 (7.27)
From the linear part of this system, we conclude as in Step 2 that the resolvent has an exponential decay since the terms L(X + Y 1 ) and f 2 are bounded and small. We write this resolvent R 3 : R 3 (t, s) ≤ K 3 exp -c 3 (t-s) .

Then, by variation of constants,

Y 3 (t) = t 0 R 3 (t, s)(Q(Y 3 )(s) + (F 1 + F 2 )o(1) + f 2 (s)X(s))ds, thus (F 1 + F 2 ) y(t) ≤ t 0 K 3 exp -c 3 (t-s) ((F 1 + F 2 ) 2 Q(y)(s) + (F 1 + F 2 )o(1) + F 2 K X 0 exp -λs )ds.
Finally,

y(t) ≤ K X 0 c 3 exp -λt + K 3 c 3 o(1) + (F 1 + F 2 ) t 0 K 3 exp -c 3 (t-s) (Q(y)(s))ds.
This proves that y(t) = O(F 1 + F 2 ) and then, Y 3 = o(F 1 + F 2 ), which concludes the proof of Proposition 10.

Implementation on real data

To test this method on real data, the same sequence of depth images that what used to illustrate the geometrical flow velocity estimation method in Section 6.4 is processed. Embedded gyrometers , accelerometers and magnetometers provide angular velocity, specific acceleration and magnetic field expressed in the rigid-body reference frame R cam . The asymptotic observer (7.4) is implemented on this data, and provides estimates for ω b , a b and v plotted in Figures 7.1, 7.2 and 7.3 respectively. The noisy velocity measurements obtained by the geometrical flow estimation method are clearly filtered by this observer. As for the biases, it seems that the sequence is not long enough to enable asymptotic convergence towards constant values.

Loop processing of the sequence is thus performed as in Subsection 5.4.4: the results of the estimated biases values are plotted in Figures 7.4 and 7.5. The estimated biases on angular velocity are submitted to perturbations (the magnitude of these perturbations does not exceed 5.10 -5 rad.s -1 ) but converge towards a mean value:

ω bmean = ⎛ ⎜ ⎝ 1.025 0.075 -0.50 ⎞ ⎟ ⎠ 10 -3 rad.s -1 .
This value is compared with a rough mean computation of the angular velocity measured by the gyrometers during the 40 first seconds, when the system is known to stand still. This value is

ω b = ⎛ ⎜ ⎝ 1.04 -0.21 -0.43 ⎞ ⎟ ⎠ 10 -3 rad.s -1 .
We conclude that the proposed observer can estimate biases up to a precision of about 3.10 -4 rad.s -1 (about 60deg.h -1 ). The estimated biases on acceleration converge slowly towards a constant value (the magnitude of the perturbations does not exceed 5.10 -4 m.s -2 )

a b = ⎛ ⎜ ⎝ 0.072 -0.023 -0.12 ⎞ ⎟ ⎠ m.s -2
which is consistent with the usual bias values observed on sensors. The only possibility to validate the accuracy of this result consists of showing that it actually improves the linear velocity estimation: Figure 7.6 is a comparison of the initial filtered velocity (obtained by the regular processing of the sequence of data),and the filtered velocity obtained after a complete bias estimation(obtained by multiple loop processing of the same sequence). This figure shows that the velocity estimation is clearly improved, especially for the component along the y-direction. This is explained by the fact that velocity measurements along this direction are particularly noisy: the observer relies mostly on the accelerometers to predict the state, which is rarely updated by valid velocity measurements. Thus, a bias on the specific acceleration is more visible than for the components where valid velocity fixes are available.

Part III

Simultaneous tracking and mapping for augmented reality

Localisation et cartographie simultanés pour la réalité augmentée

Chapter 8

Architecture of the original algorithm PTAM L'algorithme présenté dans cette dissertation est une modification de l'algorithme PTAM [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF] décrit dans ce chapitre. La plupart des algorithmes de SLAM réalisent le suivi de trajectoire et la cartographie de l'environnement à chaque image [Davison et al., 2007, Eade andDrummond, 2006], ce qui implique qu'un suivi et une cartographie de mauvaise qualité vont se produire au même moment, et donc se dégrader mutuellement en un cercle vicieux. Au contraire, PTAM réalise un suivi et une cartographie parallèles : la carte est optimisée dans un processus séparé, et seuls les bons points d'intérêt associés à un suivi de bonne qualité sont ajoutés à la carte. Une autre spécificité de cet algorithme est la méthode d'optimisation de la cartographie : tandis que la plupart des algorithmes de SLAM utilisent des méthodes de filtrage telles que Extended Kalman Filter (EKF) [START_REF] Civera | Inverse depth parametrization for monocular SLAM[END_REF] ou filtrage particulaire [START_REF] Montemerlo | FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[END_REF], qui sont toutes deux connues pour leur lourdeur en terme de temps de calcul, PTAM utilise l'ajustement de faisceaux, dont la complexité est en O(m 3 + mn) où m est le nombre de poses de caméra et n est le nombre de points de la carte [START_REF] Engels | Bundle adjustment rules[END_REF]. Cela permet de traiter en temps réel une très grande quantité de points d'intérêt visuels de basse qualité et faciles à détecter (plus précisément des coins Features from Accelerated Segment Test (FAST)), sans recourir à des descripteurs de points complexes. Cependant, la carte produite par cet algorithme n'est connue qu'à un facteur d'échelle près, ce qui est inévitable lorsque l'on traite des images produites par un système monoculaire. En outre, elle est uniquement composée de points d'intérêt discrets, ce qui ne permet pas de gérer en réalité augmentée les occultations qui peuvent avoir lieu entre un objet virtuel et l'environnement réel. Pour finir, cet algorithme a été conçu pour fonctionner sur de petits espaces de travail, alors qu'il serait nécessaire de cartographier une pièce entière.

The algorithm presented in this dissertation is based on the original PTAM [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF] described in this chapter. Most of the SLAM algorithms perform tracking and mapping refining at each frame [Davison et al., 2007, Eade andDrummond, 2006] which implies that low-quality tracking and mapping would occur at the same time and will consequently degrade each other in a vicious circle. On the contrary, PTAM performs parallel tracking and mapping: the map is refined in a separate thread, and only good features associated to good tracking quality are added. Another specificity of this algorithm is the refinement method of the cartography: while most SLAM algorithms are based on EKF [START_REF] Civera | Inverse depth parametrization for monocular SLAM[END_REF] or particle filtering [START_REF] Montemerlo | FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges[END_REF], which are both known to be a great computational burden, PTAM is based on bundle adjustment, whose complexity is O(m 3 + mn) where m is the number of camera poses and n is the number of map points [START_REF] Engels | Bundle adjustment rules[END_REF]. This enables to deal in real-time with a large amount of low-quality and easy-to-detect visual features (namely FAST corners), without the requirement of complex point descriptors. However, the map produced by this original algorithm intrinsically lacks a scale factor, which is unavoidable with monocular image processing. In addition, it is only composed of sparse features, which prevents the handling of occlusion of virtual objects in real images as required by augmented reality. Finally, it was first designed to map small workspaces, while we would like to be able to map at least a whole room.

Overview of the algorithm

PTAM is the acronym for Parallel Tracking and Mapping, an application initially proposed in [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF] and later improved in [START_REF] Klein | Parallel tracking and mapping on a camera phone[END_REF], created by a research group of Oxford University. This open source system is one of the first to enable realtime processing: every step of tracking and mapping algorithms has been carefully chosen and optimized to ensure real-time capabilities of the system. In addition, trade off between performance and quality is adjustable, depending on the available resources. Only one camera is necessary (monocular system). As highlighted in Chapter 1, this yields an overall unknown scale factor on the produced map and in the localization within that map. Three distinct stages may exist:

-Map initialization: at the very beginning of the algorithm, this phase consists of the creation of a map. This phase is necessary since monocular system can not strictly speaking simultaneously estimate pose and structure without any external a priori. The map will be augmented all along the motion of the camera. -Tracking and Mapping generic algorithm: once the map has been created, two threads run in parallel. Tracking consists of localizing (estimating the pose of ) the camera with respect to the map. When new features of the environment appear along the motion of the camera, the tracking module notifies the mapping module to reinforce the cartography. If this is not the case, resources are available for the mapping module to optimize the map. -Relocalization: sometimes, the tracking algorithm may not return a valid camera pose, generally because of the poor quality of the images (and consequently of the detected interest points). A specific method is implemented to enable the relocalization of the camera in the map. These three stages are more precisely described in the following sections, to better underline the modifications implemented in the frame of this thesis. But first are introduced some general considerations.

The map consists of a set of camera poses and 3D-points, linked by measurements, or in other words by the position in the RGB images (when existing) of these 3D points at the camera poses. Feature tracking and SLAM as a general method consists of 1. detection of Two dimensional (2D) point features in the current view; 2. matching of point features with 2D features detected in other views or with 3D features of a map; 3. depending on the external inputs, estimation of camera pose (pose tracking), of 3D position of the 2D features (mapping), or of a combination of both (for example, homography or essential matrix estimation, as for map initialization). PTAM uses FAST corner detector in the first step [START_REF] Rosten | Machine learning for high-speed corner detection[END_REF]), which does not require as much processing time as more robust detectors such as Harris detector ( [START_REF] Harris | A combined corner and edge detector[END_REF]) or SIFT extractor ( [Lowe, 2004]). On the other hand, FAST does not provide a descriptor that could be used to match two 2D features (second step): matching is thus performed by patch comparison.

Map initialization

In order to start the SLAM loop, the algorithm must be initialized with some 3D features positioned in a fixed reference frame and a camera pose with respect to the same frame. Since the system is monocular, 3D positioning of features is performed by stereo vision. The procedure is the following:

-the user selects an initial pose, -performs a slow motion (preferably a lateral translation) that allows for robust feature tracking, -then selects a final pose. Features are tracked from frame to frame all along the motion, and a relative pose estimation of the second pose with respect to the first one is computed, up to a scale factor. Two different methods are possible:

-Essential matrix estimation: if x and x ′ are the measurements of the same 3D point in two different views, their coordinates are related by

x ′ T Ex = 0, with E = R[T ] ×
where E is the essential matrix, R and T are the difference of orientation and position (with the SE(3) notation convention) between the two views, and [T ] × is the matrix representation of the cross product with T . E, and then R and T (up to a scale factor) can be estimated by the five-point algorithm ( [Nister, 2004], [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF]) combined with RANdom SAmple Consensus (RANSAC) algorithm [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF]]. homography estimation: if x i and x ′ i are the measurements of the same 3D points X i in two different views, where the points X i all belong to the same plane, their coordinates are related by

x ′ i = Hx i , with H = R + T ⋅ n d
where R and T have already been defined, and n and d are the normal and the distance to the plane, respectively. This requires for the user to start map initialization in front of a plane where features are present. The plane and its normal are estimated by RANSAC algorithm, then homography and R and T d are estimated according to [START_REF] Faugeras | Motion and structure from motion in a piecewise planar environment[END_REF]. Then, an external (arbitrary) scale factor is provided, and the robustly tracked features are positioned in the map by triangulation.

Pose estimation (tracking)

Once a map has been created, current pose estimation with respect to the map is possible, and is an essential step in the SLAM loop. Pose estimation is not given in a closed-loop form, but rather as the optimization of an initial pose. This requires -a pose initialization (or pose prediction), -a function, or cost, to optimize, (the cost must depend on the pose), and -an optimization method. The pose initialization is computed using a motion model. Since the system does not include velocity sensors or accelerometers, the motion model is arbitrarily chosen under the assumption of constant velocity with stability constraints (decaying alpha-beta model). This yields an estimate of linear and angular velocity v and ω, which are applied to the previous camera pose to compute a current pose prediction:

K pred = exp ∑ i α i G i dt K prev
where the α i are the components of v and ω and the G i are the infinitesimal generators of SE(3) (see Appendices B and C for more details). K prev and K pred denote the previous and the predicted camera poses (using the notations introduced in chapter 1). Finally, dt is the time interval between previous and current image acquisition.

From this predicted pose and the 3D points of the map, a set of potentially visible points is computed, and these points are projected on the image plane. In parallel, FAST detector is run on the current image, then the algorithm searches for matches between the projected points of the Potentially Visible Set (PVS) and the actual features of the image. The search is limited to a fixed region around the position of the projected features of the PVS. Figure 8.1 illustrates the PVS computation and the consequent match search. Details about FAST detector and patch scoring including warping and pyramidal search won't be given here, but can be found in the original publication [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF]). Once the match has been performed, the cost to minimize is defined as the projection error between the projected PVS and the detected features. Considering n pairs of matched features, x j d is the vector of coordinates in the image of the detected features and x j p is the vector of coordinates in the image of the matched projected point of the map, with j ∈ (1, n). As described in Chapter 1, x j p is given in homogeneous coordinates by where P is the intrinsic calibration matrix (under the assumption of a linear projection model, which can be adapted to non-linear models including distortions), and Xj W is the position of the corresponding 3D point of the map. The cost to minimize is given by

C(K) = j W j xj p -xj d 2
where the W j are weights iteratively computed to diminish the influence of outliers (defined as mismatched features) in the pose estimation (more details on these weights below). This method is known as iteratively re-weighted least squares minimization. Gauss-Newton iterations were employed for this non linear least-square minimization ( [START_REF] Hartley | Multiple view geometry in computer vision[END_REF]). Only error, weights and Jacobian are required for this method.

-Error is as previously mentioned the reprojection error xj pxj d . -Weights are computed using any robust M-estimator function: for example, the Tukey's biweight function might be used , where the influence function parameters directly depend on the median of the errors distribution. -To compute the Jacobian of the error with respect to the camera pose, the problem is formulated in terms of pose update, expressed as an element of the Lie algebra of SE(3) and more specifically as the 6 components α i of the pose update expressed with respect to the 6 infinitesimal generators G i :

K current = K update K pred , with K update = exp ∑ i α i G i , and α i = argmin C(α i ).
With this formulation, the derivative of the error with respect to the pose update is simply

i j ∂(x j p -xj d ) ∂α i = i j P G i K pred Xj W . (8.1)
To summarize, the tracking process consists of 1. applying the motion model to the previous camera pose;

2. determining the PVS of map points;

3. finding currently visible matches with the projection of the PVS;

4. updating camera pose to minimize reprojection errors;

5. assessing the quality of the updated camera pose.

The last step of this process is essential for relocalization and mapping purposes. The number of map points projected and actually matched with FAST corners in the image is compared with the total number of points of the PVS. The ratio value is a measure of the quality of tracking. Below a certain threshold, current measurements can not be used to augment the map. Below an even lower threshold, the tracking is considered to be lost and the relocalization procedure is initiated.

Mapping

After initialization (described in Section 8.2), the map contains two keyframes, defined as specific and carefully chosen views, which contain the measurements (the 2D detected features) that link camera poses and 3D map points. Insertion of a new keyframe in the map enables the addition of new 3D points in the cartography by triangulation (Subsection 8.4.1): the precision of 3D positioning of these new points directly depend on the quality of pose estimation of the camera. The position of 3D points can be optimized when features have been detected in multiple views, along with camera poses associated to these views (Subsection 8.4.2).

Keyframe insertion

As the camera explores new parts of its environment, the map must be augmented to enable pose estimation: this is performed by keyframe insertion. A keyframe must satisfy the following criteria:

-the camera pose estimation associated to this view was assessed to be good; -the time elapsed since previous keyframe insertion exceeds a given threshold; -the linear distance to the closest keyframe's camera pose is sufficient to provide a good stereo baseline for triangulation (this distance depends on the mean depth of detected 3D map points). If these conditions are met (verified by the tracking module), the mapping module aborts any operation other than keyframe insertion or places the keyframe in a waiting queue. Then keyframe is processed, which consists in thorough features detection (new features are considered now that real-time tracking constraints do not prevail). Another measurement of these features is necessary to compute their 3D position: these measurements and correspondences are searched in the closest keyframe by epipolar search, which consists of comparing the patch of a feature of the inserted keyframe to patches located along the epipolar line in the target view. Comparison is achieved through zero-mean sum of squared differences. When match is found, 3D position is computed by triangulation using the target keyframe camera pose and the inserted keyframe tracked camera pose.

Bundle adjustment

When no keyframe waits to be inserted in the map, the mapping module can achieve map optimization, which consists, as in pose update in the tracking module, of minimizing the difference between the projection of 3D map points in keyframes and their actual measurements. But, on contrary with pose updates, the minimization is performed by variations of both camera poses and 3D map points positions: this method, based on Levenberg-Marquadt algorithm [Marquardt, 1963], is called bundle adjustment and has been extensively studied (see [START_REF] Hartley | Multiple view geometry in computer vision[END_REF] for textbook implementation and [START_REF] Snavely | Modeling the world from internet photo collections[END_REF] for an example of application).

The main steps of bundle adjustment are recalled in the following, but first is summarized Levenberg-Marquadt iterative parameters estimation method.

123

Consider a functional relation that links parameters Π and measurements X, such that f (Π) = m. The goal is to give an estimate of Π that best fits the model given the measurements, starting from a initialization parameter Π 0 . The residual error is defined as ǫ(Π) = f (Π)m. The goal is to minimize the (possibly weighted) squared error ǫ(Π) 2 . When f is nonlinear, the iterative method seeks for a small update δ Π of the parameter, so that f (Π + δ Π ) can be approximated by f (Π) + Jδ Π where J is the Jacobian of f with respect to Π. The goal is to find the minimal argument δ Π of ǫ(Π + δ Π ) 2 = ǫ(Π) + Jδ Π 2 . First-order stationary equation yields the result as the solution of the following normal equation

J T Jδ Π = -J T ǫ(Π) (Gauss-Newton iterative method).
Levenberg-Marquadt's method augments the normal equation by

(J T J + λI)δ Π = -J T ǫ(Π) or even (J T J) λ δ Π ∶= (J T J + λdiag(J T J))δ Π = -J T ǫ(Π)
where the parameter λ varies depending on the achieved iteration. This method is applied to bundle adjustment using the relation

xj i = P K i Xj W = f 2D (K i , Xj W ). (8.2)
The measurements formally consists of the concatenation of any measurement xj i that could exist in a given set of keyframes. The vector of parameters Π is the concatenation of camera poses K i associated to the keyframes (and more specifically poses updates expressed as the components of the Lie algebra element with respect to infinitesimal generators like in pose estimation 8.3) and 3D map points positions Xj W associated to measurements. Let us define -J K as the Jacobian of f 2D with respect to camera pose (see (8.1)), -J X as the Jacobian of f 2D with respect to 3D points position (computed by chain rule differentiation, taking into account the pinhole projection first, and then the distortion) δ K as the update of camera poses, δ X as the update of 3D points positions, ǫ is the reprojection difference, in other words the difference between the 2D position of the measurement and the 2D position of the projection of the associated 3D point in the image plane of the associated view. The usual normal equations are given in the form

J T K J K J T K J X J T X J K J T X J X δ K δ X = J T K ǫ J T X ǫ
and transformed by Levenberg-Marquadt augmentation by

(J T K J K ) λ J T K J X J T X J K (J T X J X ) λ δ K δ X = J T K ǫ J T X ǫ .
The solution is given in a triangular form. First, δ K is given as the solution of

((J T K J K ) λ -J T K J X (J T X J X ) -1 λ J T X J K )δ K = J T K ǫ -J T K J X (J T X J X ) -1 λ J T X ǫ
then δ X is given as the solution of

(J T X J X ) λ δ X = J T X ǫ -J T X J K δ K .
The minimization does not equally take into account all the measurements: weights are iteratively computed using a M-estimator influence function such as the Tukey's biweight objective function to reject the outliers influence in the optimization process (as for pose update, see section 8.3). The error to minimize is ǫ T W ǫ , where W is a diagonal weighting matrix. The optimization procedure is modified accordingly.

Relocalisation

In case where the tracking was considered of poor quality for three successive frames, the system is declared "lost". A recovery procedure is initiated, described in [START_REF] Williams | Real-time SLAM relocalisation[END_REF]. The goal is to give a pose estimate that can be used as tracking initialization. The fastest solution is to compare a light version of the current image to similar versions of the views associated to keyframes of the map. Comparison is achieved through zero mean sum of squared differences. The "light" version is obtained by down sampling and blurring of the image and are named Small Blurry Image (SBI).

Chapter 9

Fusion of magneto-inertial, visual and depth measurements Fusion de données RGBD, de mesures magnéto-inertielles et de mesures de vitesse par la méthode du flot géométrique.

Dans ce chapitre, nous décrivons comment PTAM a été amélioré par l'apport de données magnéto-inertielles et de profondeur : l'ambiguité du facteur d'échelle devient obsolète, tout comme le problème de l'occultation. En outre, les mesures de vitesse linéaire apportées par la méthode du flot géométrique (voir le Chapitre 6) peuvent être utilisées en même temps que les accéléromètres pour filtrer les mesures de position très bruitées fournies par l'algorithme de vision, et ainsi fournir une estimation de la position même quand les mesures visuelles ne sont pas disponibles. Une partie de ces améliorations ont été réalisées directement à Sysnav : toutes ont été possibles grâce à leur aide. L'algorithme initial PTAM a été implémenté et adapté pour être complété et rendu plus robuste par des capteurs supplémentaires, qui peuvent être classés en deux catégories : les capteurs magnéto-inertiels, plus précisément les accéléromètres, les gyromètres et les magnétomètres ; le capteur de profondeur, qui est une sous-partie du capteur RGBD. Nous décrivons ici les capteurs et leur synchronisation, puis les améliorations apportées par la fusion d'un algorithme de vision et de données complémentaires.

In this chapter, we describe how PTAM is improved when aided by magneto-inertial and depth data: the scale factor ambiguity becomes obsolete, as well as the occlusion issue. In addition, linear velocity measurements from our geometrical flow method (see Chapter 6) can be used along with accelerometers to filter the noisy position measurements provided by the vision algorithm, and to provide a position estimate even when vision measurements are not available. Part of these improvements were performed by Sysnav company: all of them were made possible thanks to their help. The original algorithm of PTAM was implemented and adapted to be completed and made more robust by additional sensors, that can be separated in two classes: magneto-inertial sensors, namely accelerometers, gyrometers and magnetometers; depth sensor, as part of the RGBD sensor. We describe the sensors and their synchronization, then the improvements provided by fusion of a vision algorithm with complementary informations.

Sensors and synchronization

Depth sensor

We make use of the RGBD sensor integrated in the Kinect device developed by Microsoft for the Xbox 360 video game console and first released in November 2010. Although the technology on which this sensor is based has been known for years, this device is a technological outbreak in the way data is provided, and because of its relatively low cost. At 24.8 Hz, two images of a resolution of 640 by 480 pixels are jointly provided. The first image is a regular RGB image, but the second image is a dense depth image aligned with the RGB image. This yields that to any object in the field of view of the camera, a color and a 3D position (with respect to the camera frame R cam ) is provided: on one hand this is a direct solution to the scale factor issue of monocular systems and on the other hand this theoretically allows to bypass any triangulation limitation. The image and depth sensors are actually regular CMOS sensor, the depth sensor being equipped with an infrared filter. Along with them, an infrared emitter sends a given pattern of infrared "dots". This pattern is projected onto the environment: depending on the depth of the objects on which the dots are projected, the pattern is more or less distorted. The distorted pattern is then acquired by the infrared sensible CMOS sensor, and disparity is computed from the distortion. Depth is then computed from this disparity, and stereo rectification enables to align RGB and depth images. Concerning the depth sensor, one can neither decide of the date of data acquisition, nor precisely know this date : one can only assume that acquisition rate is constant, that no data is lost during transmission by the USB or even the sensor's driver, and that the depth image and the RGB image of a specific frame were acquired at the same time. An example of RGBD data is given in Figure 9.1, in which areas where depth data is not valid can be seen (black areas, the depth data associated to these pixels is zero). Depth data is typically corrupted in presence of sun light or reflective surfaces, and of course if the environment is out of the range of the sensor (0.5-7 meters).

Magneto-inertial sensors

Over the past decades, MEMS have become more popular and more accurate, because they are small, they are cheap and they can be sufficient when high precision data is not required. For example, they are present in any current smartphone. The 3 accelerometers and 3 gyrometers used in our system are MEMS, gathered in a 6-DOF IMU distributed by Epson. It measures a few millimeters, consumes only 6 mA, and is operated at 125Hz. In addition to these sensors, magnetometers are Anistropic Magnetoresistances (AMR), and also operate at 125Hz. Synchronization between accelerometers, gyrometers and magnetometers is achieved independently of the depth sensor through a global numerical trigger, which ensures that data was acquired for all magneto-inertial sensors at the same date. RGBD data and magneto-inertial data can be seen as two separate devices (RGBD sensor and magneto-inertial sensor) in which various data types are internally synchronized. The magnetoinertial sensor sends to the RGBD sensor a signal synchronized with magneto-inertial data that encodes a value varying from one frame to the next. The depth sensor's hardware has been modified to acquire this signal and to associate it with the current RGBD frame. When a RGBD frame sent by the RGBD sensor is received by the master system, synchronization with a magneto-inertial data frame is achieved by comparison of the signals.

Map initialization

Map initialization becomes easier when scale factor ambiguity is removed. The first 3D positions of map points can be computed from one single view: the position of a 2D point in a view gives the perspective line on which the 3D point is located, and the depth image gives the position on this line (to be precise, the position along the optical axis). Since accuracy of localization crucially depends on the quality of the map, only very distinctive map points are initialized, which is achieved through robust detection and description of features in several frames, and consistency check between several frames. In addition, map points insertion is restricted to features for which depth data is valid.

Mapping

Using data sources other than visual data can be of great profit for the mapping module. First, insertion of new map points is greatly simplified and made more robust when triangulation is not necessary (like in map initialization). Secondly, depth measurements can be added in the overall optimization by bundle adjustment.

Keyframe insertion

When the user explores large areas and does not want to be restricted to small workspaces for which the original algorithm was first designed, the principal differences arise in -the number of keyframes and map points of the cartography; -the range of camera poses admissible: in particular, orientation is submitted to larger variations when the user intends to map a whole room or building. The problem of map size is critical in bundle adjustment, for which processing time evolves in O(m 3 + m 2 n) where m is the number of keyframes and n the number of map points of the cartography: it must be addressed as soon as the decision on keyframe insertion is taken. This would tend to increase the value of the threshold of the linear distance from which a new keyframe is considered as necessary (see Subsection 8.4.1). In parallel, camera orientation is submitted to large variations: it is natural when exploring a large environment to turn the head to see as most new areas as possible. But precisely, orientation variation induces large view variations, which means that the number of features present in two consecutive views is likely to significantly decrease, possibly up to the point where no map points are visible in the current view: this leads then to the loss of tracking. In pure visual SLAM algorithms, a trade off is necessary to decide when a new keyframe should be inserted: the linear distance between the closest keyframe of the cartography must be sufficient to enable good triangulation quality, unless large variation of orientation occurs: in this later case, map points must be added to the map as soon as possible to ensure tracking continuity.

When depth data is available, the constraints on minimal linear distance between keyframes is looser, since triangulation is not anymore necessary. Criteria used to decide whether a keyframe should be inserted are changed according to these considerations:

-linear distance between keyframes must be over a minimal threshold to limit the map size growth, and below a maximal threshold to ensure good correspondence between detected features and sufficient view superposition; -difference of orientation between keyframes must be as small as possible to ensure good correspondence between detected features and sufficient view superposition; -difference of orientation between keyframes must be below a maximal threshold; -if difference of orientation between the current view and all existing keyframes is over a minimal threshold, the current view should be inserted as soon as possible, since tracking is likely to be lost soon. Map points insertion process depends on depth data availability. Considering a given 2D detected feature of the current view: like in the original algorithm, a feature match is searched on the epipolar line of the target view (the closest from the standpoint of linear distance), then the 3D position of the corresponding map point is computed:

-if depth data is available for both 2D features position and corresponding 3D positions are close, the consequent 3D position is the mean of both positions (expressed in the world frame) -if depth data is available for neither of 2D features position, 3D position is computed by triangulation;

-if depth data is available for only one 2D feature position, the corresponding 3D point is reprojected in the other view; if this reprojection is close to the associated detected feature, the 3D point is added to the map.

Depth data in bundle adjustment

Bundle adjustment as described in Section 8.4.2 was only adapted to 2D measurements. But with the use of depth sensor, 3D measurements are available. One of the main weaknesses of bundle adjustment lies in the continuous drift of the scale factor: as camera poses and map points positions are simultaneously optimized, both can slowly corrupt each other if nothing stops that vicious circle. 3D measurements can put an end to this drift since it intrinsically contains a scale factor. To our knowledge, this method has first been published in [START_REF] Scherer | Using depth in visual simultaneous localisation and mapping[END_REF]. We describe here how it is adapted to our application. When depth data is available for a 2D feature detected in several different keyframes, a 3D measurement is automatically created, which is related to camera pose and 3D map point world position by

Xj i = K i Xj W = f 3D (K i , Xj W ). (9.1)
which is similar to the 2D equation of projection (8.2). To use bundle adjustment in a similar way, one has -to define the error ǫ 2 to be minimized, -to explicit the Jacobian of f 3D with respect to the camera pose K i , -to explicit the Jacobian of f 3D with respect to the map point position Xj W . Then bundle adjustment as described in Section 8.4.2 can be readily used.

The error to be minimized is simply the Euclidean norm of the difference ǫ between the 3D measurement and the 3D estimate computed from the 3D world position and the camera pose.

The Jacobian J K of f 3D with respect to the camera pose is computed as in (8.1) by use of the infinitesimal generators of the Lie algebra of SE(3), omitting the projection matrix P .

Finally, the Jacobian J X of f 3D with respect to the 3D map point world position is extremely easy to compute, as it simply is the rotation term of the element of SE(3) representing the camera pose K i .

2D measurements and 3D measurements are used together in the optimization procedure: the size of the Jacobian matrices relative to f 2D and f 3D are different (sizes 2×6 and 3×6 respectively for J K , and sizes 2×3 and 3×3 respectively for J X ), but they are in the algorithm always combined together to compute the pose updates δ K (vectors of length 6) and map points position updates δ X (vectors of length 3). However, the weights given to measurements of different types must be normalized so that their influence in the optimization is the same.

Pose estimation

The pose estimation is in its principle the same than in the original algorithm: prediction of the current camera pose from the previous camera pose, projection of the PVS on the image plane, pose update by minimization of the reprojection error. It has been modified to integrate magneto-inertial and depth data in the pose prediction and in the computation of the PVS steps. The pose prediction consists of two distinct processes: linear velocity estimation (v the linear velocity expressed in camera frame R cam ) and attitude estimation (quaternion q that expresses the passage from the world reference frame R W to the current camera frame R cam ).

Attitude estimation is achieved through the processing of measurements of magnetometers, accelerometers and gyrometers, as described in [START_REF] Martin | Design and implementation of a low-cost observer-based attitude and heading reference system[END_REF], along with the estimation of gyrometers biases. Biases and scale factors of accelerometers are independently previously estimated. The principal assumption of this work consists of neglecting the linear acceleration of the moving system: ∂ t V(t) = 0. This yields (see Appendix C.2 for notations and details on the usual motion equations under flat Earth assumption, e.g. (C.5)) that the specific acceleration a is simply a measurement of the gravity acceleration expressed in the camera frame (R cam ) q * Aq. In parallel, magnetometers provide the measurements of the magnetic field expressed in the camera frame q * Bq and gyrometers enable to express the attitude dynamics (C.4). An invariant non-linear observer has been designed to provide attitude and gyrometers biases estimation, for which local convergence is ensured. The difference of orientation between previous and current RGBD frames is then estimated (strictly from magneto-inertial measurements) and applied to the previous camera pose estimated by visual measurements to provide current attitude prediction.

Linear velocity estimation is achieved through depth images processing, as described in Chapter 6. This yields a linear displacement estimation, applied to the previous camera pose (with the help of attitude estimation to express the linear displacement in the world camera frame R W ) to provide a prediction a camera pose position. The pose prediction algorithm is summarized in Figure 9.2: data, state variables and algorithms steps are respectively depicted in blue, green and red to highlight the data flow.

Computation of the PVS is improved using the magnetic field measurement. To each map point is attached the value of the magnetic field measured by he system at the time when this map point was first observed. At a given time, a map point can only be added to the PVS if this magnetic field vector and the current magnetic field measurement are close enough. This criterion enables to reject in a simple way the map points that belong to a non-visible surface. Consider for example a map point on a wall of a room; if the camera user goes to the next room and turns around to look at the other face of that wall, the map point is strictly speaking in its field of view since occlusion is not taken into account in the PVS construction. Yet, the u-turn operated ensures that magnetic field measurements associated to the two views are opposite: thanks to this selection criterion the map point will not be added to the PVS.

Tracking quality and relocalization

Sources of localization other than images are of great help when the quality of RGB data is too poor to enable robust feature detection and matching, which often occurs when the camera is submitted to high dynamics, or simply when the texture of the environment is too uniform. For this purpose, magneto-inertial and depth data can "take over" to fill data in when algorithm based on pure vision is not sufficient. This issue is addressed in the next section, dealing with localization for augmented reality. This section describes how magneto-inertial data, as redundant data, can assess of the tracking quality in one hand, and help in the recovery process in the other hand.

Assessing the visual tracking quality

Recall from Section 8.3 that in the original algorithm, tracking quality was estimated from the ratio of features of the PVS actually detected in the current frame. Along with this criterion, the absolute number of detected features is used: the minimum number of matched features necessary for pose estimation is theoretically 5. To these two purely visual criteria, a comparison of vision and magneto-inertial data is added.

Formally, the quality of visual tracking is judged from the standpoint of attitude accuracy. As we already mentioned in Section 9.4, processing of magneto-inertial data enables attitude estimation of the camera. This attitude is compared to the attitude estimated by visual tracking on a relative basis. Indeed, accelerometers enable the estimation of gyrometers biases, apart from the bias about the vertical axis. Magnetometers eliminate this ambiguity under an assumption of homogeneous magnetic field, which is generally not satisfied indoor. This yields a slow (bounded time-derivative) drift of the pitch angle: thus, pitch estimation lies in a cone of uncertainty, linearly growing in time.

The difference of attitude between the current time t and a given anterior time t 0 is computed by magneto-inertial attitude estimation (R in ) and by visual attitude estimation (R vis ). These attitude variations are then compared to each other. If they are close and stable in time during the time interval (t 0 , t), the visual quality is considered to be good and the current frame will be used for the next frames for relative attitude computation (t will replace the anterior time t 0 ). Since vision is not anymore the only localization data, visual tracking loss is not as bad as in the original algorithm: criteria for good tracking quality are purposely difficult to meet to make sure that "bad" views are not chosen as keyframes, which would corrupt the entire map. As a consequence, tracking loss is more frequent and the relocalization procedure is optimized to be as robust and fast as possible.

Relocalization procedure

In the original algorithm, tracking can only be recovered if the camera is close enough to the camera pose of a keyframe of the cartography, which is a large limitation. On the other hand, the multiplication of the number of keyframes must be limited to avoid memory overload and large processing time devoted to optimization. This is why a new structure has been introduced, named Fast Small Blurry Image (FSBI). A FSBI is much lighter than a keyframe, as it only contains a SBI, a camera pose and a magnetic field measurement. A new FSBI is created when tracking is not lost and provided that a minimal linear distance of a few millimeters exist with respect to the already existing FSBIs. These small structures are kept in memory all along the camera motion and organized with respect to camera pose orientation. When tracking is lost, an attitude is estimated using magneto-inertial measurements: a difference of magneto-inertial attitudes since the last time when quality was considered to be good is computed and applied to Like for pose prediction, the camera pose estimation is decoupled between attitude estimation and position estimation.

Figure 9.6 shows an implementation of the augmented reality system, which is intended to be worn on the head: the RGBD sensor is placed right on top of the eyes, and augmented reality glasses are fixed in front of the eyes, to project the augmented images. The magneto-inertial sensors lie on top of the head: all components are linked to an external master system. Figure 9.3: Implementation of the augmented reality system, which is intended to be worn on the head: the RGBD sensor and augmented reality glasses are fixed respectively on top and in front of the eyes, to acquire real images and project the augmented images. The magneto-inertial sensors lie on top of the head: all components are linked to an external master system.

Attitude estimation

Attitude estimation is achieved using magneto-inertial and visual measurements. Magnetoinertial measurements (namely gyrometers, magnetometers and accelerometers) provide debiased measurements of angular velocity ω m . Vision provide attitude measurements expressed by quaternions q V . The proposed non linear observer was inspired by invariance theory (see [START_REF] Bonnabel | Symmetry-preserving observers[END_REF] for details on invariance theory, see [START_REF] Mahony | Nonlinear complementary filters on the special orthogonal group[END_REF] for a SO(3)invariant attitude observer):

∂ t q = 1 2 qω m + qK(q * q V -1) q(0) = q V (0) (9.2)
where 1 designates the quaternion (1, 0, 0, 0) T and K is a correction gain. Local convergence is ensured by the following. Let us define ξ ∶= q * q V -1 the attitude error, and ζ ∶= qξq * the attitude error expressed in the inertial frame R W . The dynamics of this latter is given by

∂ t ζ = (∂ t q)ξq * + q(∂ t ξ)q * + qξ(∂ t q * ) = q(ω m × ξ)q * + q(∂ t ξ)q * (9.3)
On the other hand, assuming that the attitude measurements q V have same dynamics that the true attitude q (∂ t q V = 1 2 q V ω m ), the dynamics of the error ξ is

∂ t ξ = -q * (∂ t q)q * q V + q * ∂ t q V = -1 2 (ω m + Kξ)(ξ + 1) + 1 2 (ξ + 1)ω m = ξ × ω m -1 2 Kξ(ξ + 1) which in linearized around equilibrium (ξ = 0) by ∂ t ξ = ξ × ω m - 1 2 Kξ (9.4)
Plugging (9.4) into (9.3) yields

∂ t ζ = - 1 2 Kζ.
Thus, a positive correction gain K ensures local exponential convergence of ζ towards zero. This means that our attitude estimate stays close to the visual attitude measurements when such measurements are available, is continuous and differentiable by construction, and is always available. This characteristics satisfy the conditions required for realistic augmented reality.

Position estimation

For position estimation, magneto-inertial and RGBD data are combined: first, to estimate the linear velocity v, and then to estimate the position X C itself. The proposed observer for velocity and position is the following:

⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ∂ t v = v × ω m + a m -q * Aq + k v (v m -v) + k vx q * (X m -X C )q ∂ t X C = qvq * + k X (X m -X C ) v(0) = v m (0) X C (0) = X m (0) (9.5)
where a m and ω m are the measurements from accelerometers and gyrometers expressed in camera frame R cam , -q is the output of the previous attitude estimation (see Subsection 9.6.1), v m is the output of the linear velocity estimation algorithm from depth data expressed in camera frame R cam (see Chapter 6), -X m is the position measurement with respect to the inertial frame R W (the output of the tracking algorithm). To establish convergence results, let us define -V m ∶= qv m q * the linear velocity measurement expressed in R W , -V ∶= qvq * the linear velocity estimate expressed in R W , -Ṽ ∶= V -V the error of linear velocity, -X ∶= X C -X the error of position, ξ ∶= qq * -1 the absolute error of attitude.

Then, as in the previous section, the dynamics of V is given by

∂ t V = q(ω m × v)q * + q(∂ t v)q * = qa m q * -(ξ + 1)A(ξ * + 1) + k v (V m -V) + k vx (ξ + 1)(X m -X C )(ξ * + 1). (9.6)
Assuming that attitude error ξ is small, ξ * = -ξ (ξ can be identified with a vector of R 3 ). The terms of equation (9.6) involving ξ can be linearized, yielding

∂ t V = qa m q * -A + 2A × ξ + k v (V m -V) + k vx (X m -X C ) -2k vx (X m -X C ) × ξ (9.7) and similarly ∂ t X C = V + 2 V × ξ -k X (X m -X C ). (9.8)
Finally, assuming that the measurements X m and v m can be identified with true position and velocity X and v, this yields the following dynamics for the errors Ṽ and X:

∂ t X = Ṽ -k X X + 2 V × ξ ∂ t Ṽ = -k vx X -k v Ṽ + 2A × ξ + 2k vx X × ξ.
(9.9)

This system can be decomposed in a linear globally exponentially stable system (for positive correction gains k X , k v and k vx ), and perturbation terms which directly depend on the attitude error ξ. Provided that the correction gains are large enough and the attitude error ξ is small enough, the perturbations terms should be dominated by the exponential convergence of the errors of position and linear velocity towards 0.

by ASUS ), and behind are fixed the inertial sensors (accelerometers, magnetometers and gyrometers). Figure 10.2 shows the room equipped with cameras of the Vicon motion capture system. Note that only position and orientations are provided: velocities are postprocessed in the following for results comparison.

Note also that the motion capture system provides the attitude and position of the rigid body formed by the reflective markers attached to the system, with respect to an inertial fixed reference frame: both differ from the the reference frame R cam that defines our system and the world R W . As a result, thorough evaluation of all the different reference frames must be achieved in order to enable trajectory comparison. This work, which might be fascinating as well as time-consuming, is not presented in this dissertation. Imprecisions might persist, yielding slight repeatable trajectory differences. In a first test, the system is placed from its starting point onto a trolley (shown in Figure 10.3), then the trolley moves back and forth along the lateral direction of the system (the x-axis): the system is only submitted to translations. Linear velocity estimation is the result of the fusion Figure 10.3: The trolley used to ensure that our system is only submitted to pure lateral translation.

of linear velocity measurements (computed by geometrical flow method described in Chapter 6), filtered by accelerometers using inertial attitude estimation (see Subsection 9.6.1 for attitude estimation and system (9.5) for linear velocity filtering). An important part of the algorithm is the detection of inaccurate linear velocity measurements (caused by depth images of poor quality). Figures 10.4 and 10.5 show the results of linear velocity estimation, and more precisely the linear velocity measurements, detected as accurate (blue points) and inaccurate (red points). The filtered linear velocity is plotted in cyan and the reference (computed from motion capture positions) in black. A zoom on the linear velocity in the x-axis direction shows that most of the inaccurate linear velocity measurements are actually detected: for example between the times t = 160 sec and t = 240 sec, the high velocity phases are subject to inaccurate measurements: nevertheless, the accelerometers during these short periods are sufficient to provide open-loop integration, before the next accurate measurement provides the next velocity fix. The same phenomenon occurs between the times t = 260 sec and t = 330 sec. Filtered linear velocity is also compared to the velocity that was used in the original algorithm, computed by a motion model described in Section 8.3. Recall that this velocity is used to predict the current pose of the camera, which enables the computation of the PVS. The velocity is plotted (in blue) along with filtered velocity (in red) and velocity reference (in black) computed from motion capture positions in Figure 10.6. These results shows that the velocity of the motion model is noisier and less accurate than the filtered velocity, which should lead to better tracking results. Unfortunately, we were unable to prove that the impact of this improved prediction step actually improved the tracking accuracy.

Finally, the position is filtered as described by (9.5), using attitude estimation, filtered linear velocity (expressed in R cam ) and position measurements (the output of the tracking algorithm). The result is shown in Figure 10.7. Recall that the position is expressed in the world reference frame R W , which explains that the larger variation of position occurs along the z-axis. These results show that the strong position outliers are canceled: the filter provides a continuous position even when the tracking algorithm based on RGB data processing is lost, which occurs in this test only once, at t ≈ 170 sec. The position estimation in the x-axis direction does not seem accurate in this test: at some times, the error exceeds 10 cm. However, it is repeatable. This error might actually be the artifact of an imprecision in the harmonization of the different reference frames, as explained in Subsection 10.1.

Exploration of an entire room

In a second test, the system is handheld and the goal is to explore the entire room, with different dynamics: first, a slow loop is performed around the room, then motion with higher dynamics occur to test the robustness of the algorithm. The motion mostly occurs in a horizontal plane.

Just as for the previous test, linear velocities are estimated, and plotted along with the motion model and the reference linear velocities in Figure 10.8. Again, the filtered velocity is cleaner, except at very specific times where inaccurate velocity measurements were not successfully detected (for example at times t ≈ 130 sec or t ≈ 190 sec in the x-axis direction).

Then the attitude estimation is performed and results are plotted in Figure 10.9: recall that attitude estimation as described in Subsection 9.6.1 is based on gyrometers measurements and attitude measurements (outputs of the tracking algorithm). Along with these is plotted the attitude estimate based on inertial data only as described in Section 9.4. All of them are represented as Euler angles (see the Subsection C.1.3 in appendix for definition of these angles). The goal of this filter is completely achieved as it filters the high frequency dynamics of the tracking output as well as provides continuous attitude estimate when this measurement is not available: for example, at times t ≈ 100 sec or t ≈ 130.

Finally, position estimation is performed as described by (9.5) and explained for the previous test. The result is plotted in Figure 10.10, and a top view of the trajectory is shown in Figure 10.11. These results show that position estimation is generally accurately achieved, except at certain times, which are the combination of the loss of tracking and inaccurate and undetected linear velocity measurements.

Cartography

A cartography can be reconstructed as a point cloud: from each depth image, a new point cloud can be generated, whose points positions in the world reference frame R W are computed using current filtered camera position and attitude. To each of these points, a color is attributed using the RGB image, and the different point clouds are then aggregated to form the cartography. The result of such a reconstruction is shown in Figures 10.12 and 10.13. In the first figures, different views of the room are shown, along with the corresponding views of the point cloud formed by the map points of the Vision algorithm. In the second figure, an overview of the entire reconstruction is shown. Notice that the points corresponding to large depth measurements (e.g the distant walls of the room) form noisier point clouds: indeed, the noise distribution of the depth image directly depends on the depth value itself. To assess the accuracy of the reconstructed cartography, distances were measured in the room using a laser telemeter. These various lengths, corresponding to physical distances such as space between windows or distance from a wall to another, are shown in Figure 10.14. The

Conclusion

In this thesis, we have presented different algorithms that meet the needs of visual augmented reality for a dense 3D perception of the surrounding environment on one hand and for a precise localization of the camera in this environment on the other hand.

The theoretical framework is based on a spherical camera model, which allows to express the dynamics of the light intensity and the depth by a system of SO(3)-invariant PDEs. The camera dynamics are made explicit in this system, as well as the depth of the environment surrounding the camera. It appears that linear velocity and depth are indistinguishable in the first PDE (the optical flow constraint), as expected for a monocular system; but the second PDE (the geometrical flow constraint) is not submitted to this ambiguity.

In the first part of this manuscript, the assumption of known camera dynamics is made to estimate the current dense depth field. Like for optical flow estimation, variational methods can be implemented to estimate the depth as the solution of the optical flow constraint where motion dynamics is made explicit. This formulation is advantageous because the fundamentally ill-posed problem of optical flow is now geometrically constrained by the knowledge of the camera dynamics. Accuracy of the result is highly dependent of the quality of the processed RGB images and of the value of the linear velocity. The indistinguishability of depth and velocity translates here as unobservability of the depth for a purely rotational motion. The geometrical flow constraint is thus exploited to design a non-linear asymptotic observer that refines the depth estimation based on optical flow or depth measurements.

The release of the Kinect device in November 2010 provided a low-cost and easy solution to the problem of dense depth estimation. In addition, it enabled to completely pose the converse problem, namely motion estimation from RGBD data as dense scalar fields. In a first time, the possibility to embed linear velocity sensors such as Pitot tubes or EM log sensors on the system as well as MEMS gyrometers led to the problem of bias estimation, unavoidable as soon as relatively low-cost sensors are involved. Interestingly, the observability of this question is strongly related to geometric properties of the environment, but not to the motion of the camera itself. Moreover, while these properties are quite intuitive, their meticulous demonstration requires a deep understanding of the Riemannian geometry of the unit sphere. The combined processing of regular images and depth images increase the probability that observability conditions may be satisfied for a given environment, but only depth images are necessary for the convergence of the estimation method. In a second time, a novel estimation method of linear and angular velocity by depth images processing was proposed. This technique is fast and does not lean on features correspondence nor on explicit computation of spatial derivatives of the dense depth field. Exactly like for the previous observability problem of biases estimation, the quality of the results depend on properties of the environment. In addition, a confidence index is assigned to any measurement, which is a key component of good data fusion. These measurements are finally used to estimate biases on accelerometers and gyrometers in a third part, as well as to filter the linear velocity.

A practical implementation of a SLAM solution was tested for augmented reality: visual position measurements are issued from an adaptation of PTAM, where the missing scale factor is provided by the depth component of the processed RGBD data. Velocity measurements are obtained from our novel geometrical flow estimation method, and combined with these visual position measurements and inertial data to reconstruct a continuous and smooth trajectory that meet the augmented reality requirements. Cartography of the environment is still subject to long-time drifts or distortions when loop closure occurs, which is one of the biggest limitations of the actual system. This might be fixed by the addition of better features for detection and description: for example, the ORB detector and descriptor [START_REF] Rublee | ORB: an efficient alternative to sift or surf[END_REF] is comparable to SIFT in terms of accuracy, but runs about 100 times faster. The pose prediction of the tracking module is highly improved by our trajectory estimation, which means that the following pose estimation should not suffer from the reduce in the number of "low-quality" features and the addition of "good-quality" ones. In addition, better descriptors could be used in the relocalization module instead of the actual FSBI, and could be of great help for loop-closure detection [START_REF] Angeli | Real-time visual loop-closure detection[END_REF]. After loop-closure, the map should be globally corrected, by a distribution of error in the loop either uniformly or weighted by the path length, or by more sophisticated relaxation methods [START_REF] Sprickerhof | A heuristic loop closing technique for large-scale 6D SLAM[END_REF]. If the map can be efficiently corrected by loop-closure detection, the algorithm could benefit of two advantages. First, in a situation where pose estimation is submitted to large drifts, the local nature of pose optimization in the tracking module of PTAM may lead to the duplication of the environment in the cartography, and consequently to oscillations between two positions in the camera pose estimation, one for each representative of the cartography. On the contrary, if loop-closure is detected, this harmful effect would be avoided. Secondly, drifts of the cartography yield drifts in the visual attitude measurements: even if the cartography is initially aligned with the inertial reference frame, the visual measurements used in the attitude observer (9.2) eventually lead to erroneous attitude estimation. This has a direct consequence in the position observer (9.5), where the gravity acceleration is expressed in the camera frame thanks to this attitude estimate, and makes impossible the sole use of accelerometers when neither velocity nor position measurements are available.

To our knowledge, the work the most similar to our system in terms of results was achieved in the Kintinuous project, which is an extension of the Kinect Fusion work. Descriptions and videos are available online [START_REF] Whelan | Kintinuous: Spatially extended KinectFusion[END_REF], Whelan et al., 2013a[START_REF] Whelan | Deformation-based loop closure for large scale dense RGB-D SLAM[END_REF]). The team first dealt with the possibility to work on large areas, then improved the odometry part (originally only based on ICP, now augmented by the work of [START_REF] Steinbrucker | Real-time visual odometry from dense RGB-D images[END_REF]), and finally (at the time of writing) added a place recognition module for loop-closure detection and post-correction of the map. This system, and especially the mapping module, almost entirely The differential df in the direction δη writes P ⋅ δη, which means that the projections of P and ∇f on T η are the same.

∇(η ⋅ P) ⋅ δη = δη ⋅ P (A.3)

On the other hand, ∇f ⋅ η = 0 because ∇f at point η lives in the tangent space T η , which means that ∇f equals its projection on T η , and thus the projection of P on that space. This projection is written in the form P -(η ⋅ P)η, or equivalently -η × (η × P), which is the relation (A.1). Now, we prove that ∆f = -2η ⋅ P (A.4)

By definition, the Laplacian of a function is the divergence of its gradient. Thus ∆f = ∇ ⋅ (-η × (η × P)).

Let us define S 2 → T η η ↦ g(η) ∶= -η × (η × P).

The divergence of a vector field is the derivative of the area of a surface element as it evolves along the flow defined by the vector field. More precisely, as δη 1 and δη 2 are two infinitesimal vectors of T η for η ∈ S 2 , they define a surface element whose area is (δη 1 × δη 2 ) ⋅ η. As η, δη 1 and δη 2 evolve according to Rearranging the terms, and since δη 1 and δη 2 ∈ T η yields:

∂(δη 1 × δη 2 ) ⋅ η ∂s = -2(η ⋅ P)(δη 1 × δη 2 ) ⋅ η
By identification with (A.6), this proves that ∇ ⋅ g(η) = -2(η ⋅ P)

Consider a vector expressed in a specific fixed coordinate frame R W by x w , and in a moving coordinate frame R c by x c . Let us denote R c w the rotation matrix from R W to R c :

x c = R c w x w .
As R c is moving, the effect of a small variation Ω at the position (orientation) R c w transforms x c into x ′ c such that x ′ c = exp Ωdt x c (see equation (B.1)). Equivalently

x ′ c -x c = m=∞ m=1 (Ωdt) m m! x c .
At first order in Ωdt, this yields

x ′ cx c = Ωx c dt, which is usually written

∂ t x c = ⎛ ⎜ ⎝ ω x ω y ω z ⎞ ⎟ ⎠ × x c
where a × b stand for the usual cross product in R 3 . The vector ω = (ω x , ω y , ω z ) T is the so-called angular velocity. Similarly, a first order approximation of (B.2) yields

∂ t R c w = ΩR c w . (C.2)
This relation is extended to higher orders using the Hausdorff formula and the adjoint representation of Lie groups and Lie algebras (see [Hall, 2003] and [Selig, 2005]). The matrix R c w that describes the relative attitude between two coordinate frames is often called the direction cosine matrix. It is mainly used in the robotics communities. Since the angular rates or angular velocities are provided by gyroscopes or gyrometers, it is often desired in inertial navigation to integrate relation (C.2) from a given initial attitude and given embedded sensor measurements to reconstruct the trajectory of a moving object. One must be cautious as the "orthogonal" property of the rotation matrix is easily lost by numerical integration . The proper integration method as described in [Radix, 1980] consists in right-multiplying the current orientation by a so-called micro-rotator defined by

δR = I + sin θ θ Θ + 1 -cos θ θ 2 Θ 2
where Θ is the skew-symmetric matrix associated to the vector θ, θ = θ and θ is obtained by integration of

∂ t θ = ω + 1 2 θ × ω, θ(0) = 0, t ∈ [0, dt]. (C.3)
This holds for short time intervals dt.

C.1.2 Quaternions

Quaternions were discovered by Hamilton in 1843: this fact was set in stone by the mathematician himself by carving the multiplication of the three basic imaginary units on the Broome Bridge in Dublin (although it appeared later that Gauss had made the same discovery a few years before). Any quaternion q ∈ H can be written in the form q = q 0 + q 1 e 1 + q 2 e 2 + q 3 e 3 where (q 0 , q 1 , q 2 , q 3 ) are real numbers and the three imaginary units satisfy e 2 1 = e 2 2 = e 2 3 = -1, e 1 e 2 = e 3 , e 2 e 3 = e 1 , e 3 e 1 = e 2 and the commutation of any of these products yields an opposite result. This is an extension of the theory of complex numbers. Equivalently, a quaternion can be identified with a vector of R 4 , itself divided into a scalar (q 0 ) and a vector (q) of R 3 . It follows that the product of two quaternions p and q is pq = p 0 q 0p ⋅ q p 0 q + q 0 p + p × q .

The norm q of a quaternion is the Euclidean norm of its representation as a vector of R 4 . The conjugate of a quaternion q = (q 0 , q) is q * = (q 0 , -q) (like for the complex numbers, the imaginary part is the opposite). Since q and q * have same norm q , the inverse q -1 of q is q * q 2 . A vector x of R 3 can be seen as an imaginary quaternion where the scalar part is simply zero. Dot product and cross product are defined in H to coincide with the same operators in R 3 :

p ⋅ q = 1 2 (pq * + qp * ), p × q = 1 2 (pq *qp * ).

It appears then that the operator R q ∶ x ↦ q * xq is a rotation as soon as q = 1, since it preserves lengths and angles. The corresponding rotation matrix R q can be written in the form R q = ⎛ ⎜ ⎝ q 2 0 + q 2 1q 2 2q 2 3 2(q 1 q 2q 0 q 3 ) 2(q 1 q 3 + q 0 q 2 ) 2(q 1 q 2 + q 0 q 3 ) q 2 0q 2 1 + q 2 2q 2 3 2(q 2 q 3q 0 q 1 ) 2(q 1 q 3q 0 q 2 ) 2(q 2 q 3 + q 0 q 1 ) q 2 0q 2 1q 2 2 + q 2 3 ⎞ ⎟ ⎠ .

Conversely, any rotation can be represented by two unitary opposite quaternions. Identification is made through the axis-angle representation of a rotation: any element R of SO(3) can be represented by a scalar θ (the magnitude of rotation) and a unit vector u (the axis of rotation) (and equivalently by their opposite). Then the quaternion q representing the same rotation is (cos θ 2 , sin θ 2 u). More details on this representation and identification with SO(3) can be found in [Radix, 1980] and [Savage, 2000].

It can be proven that the time derivative of the quaternion representing the attitude of a moving object with respect to a fixed frame is related to the angular velocity by

∂ t q = 1 2 qω (C.4)
where ω is the vector of angular velocity in R 3 . Numerical integration of quaternions is faster than for direct cosine matrices and quaternions are "lighter" in terms of memory space: they are therefore favored for applications where real-time computation is necessary. But like for rotation matrices, in case of imprudent numerical computations, quaternions may no longer be unitary. A proper integration scheme is

q t+dt = q t δq
where δq is defined as

δq = (cos θ 2 , sin θ 2 θ θ)
where θ is computed by integration as in (C.3).

C.1.3 Euler angles

It is sometimes useful to see a direct cosine matrix as a succession of rotations about predefined axis. They are as many representations (sequences) as possibilities to choose the axis: in addition, the order of the sequence is also decisive since the group of rotations is not commutative. An element of SO( 3) is then uniquely defined by three angles of magnitude, three predefined axis, and a sequence. One possibility is to choose three fixed axis, but in the most popular choice, the axis of rotations are defined successively by the previous rotations: this yields the Euler angles representation. We detail here one of these representations for a coordinate frame where the axis are defined by the computer vision convention: the z axis is the optical axis, the y axis point downwards and the x axis points towards the right of the image (see figure C.1). Then, the convention adopted in this thesis is to define Euler angles (ψ, θ, φ) as the successive rotations about axis y 0 , then about the newly defined axis x 1 , and finally about the newly defined axis z 2 . This yields for (ψ, θ, φ) the usual names of yaw, pitch and roll. It is often more meaningful to represent an orientation with angles which are easy to figure than with quaternions or matrices. However, computations with Euler angles are particularly tedious and highly non-linear since they involve trigonometric functions. Moreover, a singularity is encountered at θ = ± π 2 . In this thesis, Euler angles are only used to represent an attitude with respect to another as an external output of navigation filters: we will never use them in the observers themselves. This is why relations between angular velocities and time derivatives of Euler angles are not given. They can be found in numerous textbooks including [Radix, 1980], [Savage, 2000].

C.2 Position, linear velocity and acceleration

The position of a rigid-body with respect to another reference frame R W is simply the coordinates of the origin C(t) of a reference frame R cam attached to the moving body expressed in R W . In this thesis, R W always refers to a reference frame attached to a physical point at the earth's surface. Earth's rotation is neglected and R W is an inertial reference frame. In addition, Earth's curvature is also neglected. More details on reference frames commonly used in inertial navigation or in radio navigation can be found in [Zarrouati, 1987]. The linear velocity of the moving rigid-body relative to the inertial frame R W can be expressed in both frames of reference:

-in R W , the linear velocity, denoted V(t) is simply ∂ t C(t); -in R cam , the linear velocity is denoted v(t) and is expressed as v(t) = q * V(t)q, where q is the quaternion representing the attitude of the rigid-body at time t (see paragraph C.1.2). The acceleration γ of the moving rigid-body relative to R W expressed in R cam satisfies

∂ t v = v × ω + γ

C.3. THE SPECIAL EUCLIDEAN GROUP

The Lie algebra of SE(n) is the set of all matrices X of M n+1 (R) of the form Ω V 0 1×n 0 such that Ω T = -Ω.

In R 3 , the infinitesimal generators at identity can be deduced from the Lie algebra of SO(3) ((C.1)) and the three unit vectors

G vx = ⎛ ⎜ ⎝ 1 0 0 ⎞ ⎟ ⎠ , G vy = ⎛ ⎜ ⎝ 0 1 0 ⎞ ⎟ ⎠ , G vz = ⎛ ⎜ ⎝ 0 0 1 ⎞ ⎟ ⎠ .
They can be written in the form

G 1 = G ωx 0 3×1 0 1×3 0 , G 2 = G ωy 0 3×1 0 1×3 0 , G 3 = G ωz 0 3×1 0 1×3 0 , G 4 = 0 3×3 G vx 0 1×3 0 , G 5 = 0 3×3 G vy 0 1×3 0 , G 6 = 0 3×3 G vz 0 1×3 0 .
Consider a vector expressed in a reference frame R cam attached to a moving object with respect to its coordinates in another fixed reference frame R W by

x c = Kx w
where K is a member of the Special Euclidean Group SE(n). A small variation of K, in other words of the position and/or orientation of the moving object, is represented by an element (Ω, V) of the Lie algebra, with can be written in the form

(Ω, V) ≡ i α i G i
This variation has the following effect on x c (see (B.1)):

x ′ c = exp ∑ i α i G i x c . (C.6)
Similarly (see equation (B.2)), a small variation of orientation and position applied to an element K of SE(n) yields an new element K ′ which is given by

K ′ = exp ∑ i α i G i K.
(C.7)

INSTITUT DES SCIENCES ET TECHNOLOGIES La réalité augmentée: fusion de vision et navigation.

Résumé: Cette thèse a pour objet l'étude d'algorithmes pour des applications de réalité visuellement augmentée. Plusieurs besoins existent pour de telles applications, qui sont traités en tenant compte de la contrainte d'indistinguabilité de la profondeur et du mouvement linéaire dans le cas de l'utilisation de systèmes monoculaires. Pour insérer en temps réel de manière réaliste des objets virtuels dans des images acquises dans un environnement arbitraire et inconnu, il est non seulement nécessaire d'avoir une perception 3D de cet environnement à chaque instant, mais également d'y localiser précisément la caméra. Pour le premier besoin, on fait l'hypothèse d'une dynamique de la caméra connue, pour le second on suppose que la profondeur est donnée en entrée: ces deux hypothèses sont réalisables en pratique. Les deux problèmes sont posés dans le contexte d'un modèle de caméra sphérique, ce qui permet d'obtenir des équations de mouvement invariantes par rotation pour l'intensité lumineuse comme pour la profondeur. L'observabilité théorique de ces problèmes est étudiée à l'aide d'outils de géométrie différentielle sur la sphère unité Riemanienne. Une implémentation pratique est présentée: les résultats expérimentaux montrent qu'il est possible de localiser une caméra dans un environnement inconnu tout en cartographiant précisément cet environnement.

Mots clés: Réalité augmentée, systèmes de navigation, vision, algorithmes SLAM, estimation de biais, profondeur, observabilité, géométrie différentielle, capteurs MEMS, données RGBD Augmented reality: the fusion of vision and navigation.

Abstract: The purpose of this thesis is to study algorithms for visual augmented reality. Different requirements of such an application are addressed, with the constraint that the use of a monocular system makes depth and linear motion indistinguishable. The real-time realistic insertion of virtual objects in images of a real arbitrary environment yields the need for a dense 3D perception of this environment on one hand, and a precise localization of the camera on the other hand. The first requirement is studied under an assumption of known dynamics, and the second under the assumption of known depth: both assumptions are practically realizable. Both problems are posed in the context of a spherical camera model, which yields SO(3)-invariant dynamical equations for light intensity and depth. The study of theoretical observability requires differential geometry tools for the Riemannian unit sphere. Practical implementation on a system is presented and experimental results demonstrate the ability to localize a camera in a unknown environment while precisely mapping this environment.

Keywords: Augmented reality, navigation systems, vision, SLAM algorithms, bias estimation, depth, observability, differential geometry, MEMS sensors, RGBD data

Acronyms2D

  Two dimensional. 3D Three dimensional. 6-DOF six degrees of freedom. EKF Extended Kalman Filter. EM Electromagnetic. FAST Features from Accelerated Segment Test. FSBI Fast Small Blurry Image. GPS Global Positioning System. GPU Graphical Processing Unit. ICP Iterative Closest Point. IMU Inertial Measurement Unit. MEMS Micro-Electro-Mechanical Systems. PDE Partial Differential Equation. PTAM Parallel Tracking and Mapping algorithm. PVS Potentially Visible Set. RANSAC RANdom SAmple Consensus.

  Figure 1.2: Pinhole camera model

Figure

  Figure 1.3:

ya α 2

 2 ρ(y a ) + β 2 yy a 2

  Figure 4.1: Angular velocity of the camera for the virtual sequence.

  Figure 4.2: Linear velocity of the camera for the virtual sequence.

Figure 4 Figure 4 Figure 4

 444 Figure 4.3: Overview of estimation errors of D using the asymptotic observer (3.4) with optical flow input V HS and V TVL1 . Images are perfect.

Figure 4 . 6 :Figure 4

 464 Figure 4.6: Zoom on estimation errors of D using the asymptotic observer (3.4) with optical flow input V HS and V TVL1 . Images are corrupted by Gaussian noise (σ = 1).

Figure 4

 4 Figure 4.8: Zoom on estimation errors of D using the asymptotic observer (3.4) with optical flow input V HS and V TVL1 . Images are corrupted by Gaussian noise (σ = 10).

Figure 4 Figure 4

 44 Figure4.9: Overview of estimation errors of D using the variational methods described in Subsections 2.2.1 and 2.2.2. Images are perfect.

Figure 4 .

 4 Figure 4.11: Overview of estimation errors of D using the asymptotic observer (3.7) with depth inputs Γ HS and Γ TVL1 . Images are perfect.

Figure 4 .Figure 4

 44 Figure 4.12: Zoom on estimation errors of D using the asymptotic observer (3.7) with depth inputs Γ HS and Γ TVL1 . Images are perfect.

Figure 4

 4 Figure 4.14: Zoom on estimation errors of D using the variational methods described in Subsections 2.2.1 and 2.2.2. Images are corrupted by Gaussian noise (σ = 10).

Figure 4 .

 4 Figure 4.15: Overview of estimation errors of D using the asymptotic observer (3.7) with depth inputs Γ HS and Γ TVL1 . Images are corrupted by Gaussian noise (σ = 10).

Figure 4 .

 4 Figure 4.16: Zoom on estimation errors of D using the asymptotic observer (3.7) with depth inputs Γ HS and Γ TVL1 . Images are corrupted by Gaussian noise (σ = 10).

Figure 4 .

 4 Figure 4.17: The linear track and the motorized trolley used to measure the linear velocity of the system. This experimental benchmark was set up and provided by the CAS (The Systems and Control Centre of Mines ParisTech).

Figure 4

 4 Figure 4.18: The static scene as seen by the camera

Figure 4 .

 4 Figure 4.20: The Kinect device, released by Microsoft in November 2010. It provides RGBD data, which enables motion estimation techniques based on combined processing of image and depth, as presented in this part.

Figure 5

 5 Figure 5.1: The vectors D(η)η taken from the camera center C and Dη taken from the camera center translated by -lu arrive at the same point M (s). It is equivalent to see this as a translation of the domain Σ by lu.

  Substracting the equations on ŷ and y (resp. on D and D), we get the relation (5.30) (resp. (5.31)). ◻ 5.4 Practical implementation, simulations and experimental results 5.4.1 Adaptation to a spherical cap Concretely, a spherical camera is only a model, and the image perceived by real cameras only occupy a part of S 2 . Let us call K this portion: y(t, η) and D(t, η) are known only for η ∈ K.

Figure 5 Figure 5

 55 Figure 5.2: Components of the linear velocity v used to test the observer: translations in the horizontal, vertical and optical axis directions, respectively.

Figure 5 Figure 5 Figure 5

 555 Figure 5.4: Error of estimation of the biases in translation for noiseless image and depth data. Real biases are ω bx = 0.05 rad.s -1 and v bx = 2.5 m.s -1

Figure 5 . 8 :

 58 Figure 5.8: Angular velocity as measured by gyrometers embedded on the system.

Figure 5 . 9 :Figure 5

 595 Figure5.9: Estimated biases resulting from the processing of RGBD images and biased angular velocity. Bias in the z-axis direction (black) is still converging at the end of the sequence.

  Figure6.2: Linear velocities measured using the geometrical flow method. In the third subplot, linear velocity along the optical axis is plotted along with linear velocity computed from the laser telemeter measurements (red).

  Figure6.3: Angular velocities: measurements using the geometrical flow method (blue) and gyrometers measurements (red). Both measurements are perfectly consistent, with the sole exception that the gyrometers measurements are biased, and the geometrical flow measurements are noisy. The magnitude of the difference does not exceed 10 -3 rad.s -1 .

n→+∞∞:

  up to an extraction, the bounded sequence (t n [T ]) n∈N has an adherence value, called α. Since the Lyapunov function V is bounded, the sequences (b, ṽ, g, ω b , a b )(t n ) are bounded, thus up to an extraction, ∃(b

Figure 7 . 1 :Figure 7 . 2 :

 7172 Figure 7.1: Bias estimation on angular velocity by the observer 7.4 processing RGBD data along with accelerometers, magnetometers, gyrometers and velocity measurements.

Figure 7 . 3 :

 73 Figure 7.3: Filtering of the linear velocity measurements by the observer 7.4 processing RGBD data along with accelerometers, magnetometers, gyrometers and velocity measurements.

Figure 7 . 4 :

 74 Figure 7.4: Bias estimation on angular velocity by the observer 7.4 applied to loop-processing of RGBD data along with accelerometers, magnetometers, gyrometers and velocity measurements.

120 Figure 8 . 1 :

 12081 Figure 8.1: Projection of the Potentially Visible Set on the image plane and match search with dectected features.

Figure 9

 9 Figure 9.1: An example of aligned RGB and depth images acquired by the Kinect device

Figure 10

 10 Figure 10.1: Our system equipped with reflective markers tracked by the Vicon system.

Figure 10

 10 Figure10.2: Views of the room equipped with a motion capture system (system Vicon©)

Figure 10

 10 Figure 10.4: Linear velocity measurements are evaluated as accurate or inaccurate, depending on which they are used or not as velocity fixes in the linear velocity filter (9.5). When measurements are unavailable (red), accelerometers are integrated in open-loop. Overview (top) and zoom (bottom) on linear velocity along the lateral direction. Comparison with the reference trajectory (black) shows an excellent accuracy.

Figure 10

 10 Figure 10.5: Linear velocity measurements are evaluated as accurate or inaccurate, depending on which they are used or not as velocity fixes in the linear velocity filter (9.5). When measurements are unavailable, accelerometers are integrated in open-loop. Linear velocity along the vertical direction (top) and optical axis (bottom) directions.

  Figure 10.6: Comparison of the filtered linear velocity and the motion model velocity used in the prediction step of the tracking algorithm.

Figure 10 . 9 :Figure 10

 10910 Figure 10.8: Comparison of the filtered linear velocity and the motion model velocity used in the prediction step of the tracking algorithm.

Figure 10 .

 10 Figure 10.11: Top view of the trajectory of the camera during entire room exploration.

Figure 10 .

 10 Figure 10.12: Details of the reconstructed cartography, computed using filtered position and depth measurements (left) and map points of the Vision algorithm (right).

Figure 10 .

 10 Figure 10.13: Entire view of the reconstructed cartography, computed using filtered position and depth measurements. The reconstructed point cloud is very accurate, since relative errors on direct physical measurements do not exceed 1%. The overall geometry of the reconstructed scene is also very convincing: walls are flat and objects are not duplicated.

Figure 10

 10 Figure 10.14: Definition of the meter-readings measured in the room by a laser telemeter. same physical lengths are then estimated in the point cloud and compared to their actual values: comparison of both is shown in Table10.1. Due to the noise in certain parts of the point clouds, especially the walls, the measurements can only be given with an uncertainty margin, which depends on the physical object being measured. The reconstructed point cloud is very accurate, since relative errors on the measurements do not exceed 1%. The overall geometry of the reconstructed scene is also very convincing: walls are flat and objects are not duplicated.

  1 × δη 2 ) ⋅ η ∂s = ∇ ⋅ g(η)(δη 1 × δη 2 ) ⋅ η (A.6)It is developped as∂(δη 1 × δη 2 ) ⋅ η ∂s =( ∂δη 1 ∂s × δη 2 ) ⋅ η + (δη 1 × ∂δη 2 ∂s ) ⋅ η + (δη 1 × δη 2 ) ⋅ ∂η ∂sUsing (A.5) and the definition of g(η) applied to δη 1 , δη 2 and η:∂(δη 1 × δη 2 ) ⋅ η ∂s =((-δη 1 × (η × P)η × (δη 1 × P)) × δη 2 ) ⋅ η + (δη 1 × (-δη 2 × (η × P)η × (δη 2 × P))) ⋅ η + (δη 1 × δη 2 ) ⋅ (-η × (η × P))

  Figure C.1: Definition of the axis used to define the Euler angles of this dissertation

  Figure C.2: Euler angles definition

  for each s we have

	lim t↦+∞ Moreover E(t, s) is uniformly bounded by the L ∞ function E(0, s). By Lebesgue dominate E(t, s) = 0. convergence theorem lim t↦+∞ E(t, ) L p = 0. Previous inequality leads to lim t↦+∞ E(t, ) L p = 0
	which ends the proof of the second implication of Theorem 1. When, for t > T , ∫ T 0 g τ (φ(τ, s)) 2 dτ ≥ λt, we have,

  is the minimization of optical flow constraint where 6-DOF motion and depth are made explicit. It does require RGB images and is highly

	Number of domains considered Processing time (ms)
	40	6.5
	100	8.8
	200	15.3
	500	38.0
	1000	76.6
	Table 6.1: Comparison of different processing times depending on the number of domains
	considered for the geometrical flow velocity estimation.

  Table10.1. Due to the noise in certain parts of the point clouds, especially the walls, the measurements can only be given with an uncertainty margin, which depends on the physical object being measured. The reconstructed point cloud is very accurate, since relative errors on the measurements do not exceed 1%. The overall geometry of the reconstructed scene is also very convincing: walls are flat and objects are not duplicated. Table 10.1: Comparison of some specific physical distances measured in reality and in the point cloud of the cartography.

	B 1 B 2 B 3 M 1 M 2 F HF	Real (m) Cartography (m) 5.92 5.94 ± 0.02 5.91 5.90 ± 0.01 6.45 6.42 ± 0.10 2.41 2.39 ± 0.05 4.21 4.12 ± 0.15 1.31 1.28 ± 0.03 2.44 2.39 ± 0.06

Remerciements

Part II

From depth to motion

De la profondeur au mouvement

Step 1: We prove that, for every

where, for every t ∈ [0, T 1 ] (integrations by part)

For every t ∈ [0, T 1 ], we have

(5.53)

Thanks to (5.25), we also have Φ n t → Φ t in C 0 ([0, T 1 ] × S 2 , S 2 ) (consequence of Gronwall lemma). From the explicit expressions of Ŷ n , Ŷ , dn , d (see (5.41) and (5.42)) and the convergence (5.27), we deduce that

(5.54)

Moreover, thanks to (5.26), the quantities ∇ dn L ∞ ((0,T 1 )×S 2 ) , ∇ Ŷ n L ∞ ((0,T 1 )×S 2 ) are uniformly bounded. Thus, the right hand side of (5.53) converges to zero as n → +∞. We may work similarly to prove that B n (t) and (F n T 1 -F T 1 )( W b , V b ) (2) (t) converge to zero uniformly with respect to t ∈ [0, T 1 ]. This ends the proof of the first step.

Step 2: Conclusion. We have

(5.55)

and

→ 0 thanks to the first step. Iterating this argument on a finite number of intervals

, etc, we conclude that (5.28) holds. Let ϕ t be the flow associated to the equation

and ϕ n t similarly associated to ( ω b n , vb n ). Then, by (5.28), ϕ n t → ϕ t in C 0 ([0, T ] × S 2 , S 2 ). Thanks to the explicit expressions of (ŷ n Dn ) and (ŷ, D) in terms of ϕ n t and ϕ t (obtained by the characteristic method, as (5.41) and (5.42)), we deduce that (5.29) holds. This ends the proof of Proposition 4. ◻

Experiments on real data

This method is tested on real depth images. The system of acquisition is the same than in Section 5.4.4. For the sequence of images tested, the system lies in the beginning on a table, in front of a laser telemeter, which measures the distance towards a plane surface of the system. The system is submitted to rotations and translations, in the following order:

-rotation about the vertical axis (y-axis in the camera reference frame R cam ), -rotation about the optical axis (z-axis in the camera reference frame R cam ), -rotation about the lateral axis (x-axis in the camera reference frame R cam ), -translation along the optical axis (the laser telemeter measurements provide a reference for this linear velocity) -rapid rotation about the vertical axis (y-axis in the camera reference frame R cam ). Figure 6.1 illustrates the setup used for this experiment. The method presented in the previous

Figure 6.1: Setup of this experiment. The system lies on a table, in front of a laser telemeter, which measures the distance towards a plane surface of the system. The Kinect device acquires depth images, and angular velocities of the system are provided by embedded gyrometers. sections is implemented, and the results are plotted in the following figures. The linear velocities expressed in the reference camera frame R cam are represented in Figure 6.2. The linear velocity along the optical axis is plotted along with the velocity computed by time-differentiation and low-pass filtering of distance measurements from the laser telemeter in the third subplot. The magnitude of the difference does not exceed 0.1m.s -1 . This enables to validate the functioning of this method, although the results are noisy, especially in the x and y directions.

The angular velocities about the x, y and z axis are plotted along with gyrometers measurements in Figure 6.3. Again, both measurements are perfectly consistent, with the sole exception that the gyrometers measurements are biased, and the geometrical flow measurements are noisy. The magnitude of the difference does not exceed 10 -3 rad.s -1 .

From these preliminary results arise several questions: are any environments more favorable than others to this method? Can we describe them? Can we make sure that the choice of the domains Ω i is the most efficient? Can we assign a confidence index to the measured velocities?

Inertial data ( Hz) 1. the current and FSBI's magnetic field measurements are compared ;

2. if they are close enough, the SBIs corresponding to the current and to the selected FSBI are compared;

3. if they are close enough, a difference of camera poses between the FSBI and the current view is estimated by warping of the current SBI;

4. step 3 provides a camera pose estimate used as the initialization of tracking procedure (PVS projection and pose update);

5. the resulting camera pose attitude is compared to magneto-inertial attitude;

6. if they are close enough, tracking is recovered; otherwise, the FSBI selected in step 3 is removed from the list of FSBI to compare the current view with, and the procedure starts over from step 1.

Localization and augmented reality

The objective of augmented reality is to inlay the image of a virtual object in a real image, in a realistic way. Realistic can be defined using the following criteria:

occlusion: the virtual 3D object (or part of it) must be hidden in the image when it is placed behind real objects; relative displacement: real image and virtual object must evolve in a consistent way along the camera motion; stability: if the camera user is immobile, the virtual object must appear immobile in the image; persistence: the image of the virtual object must continuously appear in the image, independently of the tracking state. The first criterion involves a complete current knowledge of the geometry of the visible scene, which can be given by the complete and dense cartography of the environment. However, the computational burden associated to such a goal would prevent any real-time processing. The depth image actually gives a complete and easy solution: for a given position of the virtual object in the current camera frame R cam , occultation is immediate by comparison of the current real depth (measured) and the potential virtual depth (computed).

The three last criteria are associated with the problem of localization: indeed, for a given position of the virtual object in an external reference frame (the world R W ), its position in the current view only depends on the current camera pose in the world R W . This means that localization must be accurate, stable and continuous (respectively, following the three last criteria), which can not be satisfied using visual data only since it implies ambiguity of the scale factor and does not prevent a tracking loss. We will describe how fusion between visual, magneto-inertial and depth measurements may solve this issue. Camera pose is described by a unitary quaternion q and a position X C in the inertial reference frame R W . Its state is completed by a linear velocity v expressed in the camera frame R cam .

Chapter 10

Experimental results

Validation quantitative de l'algorithme

Les résultats de ce nouvel algorithme sont présentés dans ce chapitre : la qualité de la reconstruction de trajectoire est estimée par comparaison avec une trajectoire de référence acquise par un système d'acquisition de mouvement ; la qualité de la cartographie peut uniquement être évaluée par des mesures manuelles. Les résultats sont suffisamment bons pour permettre l'addition de données virtuelles à l'image réelle, et un petit jeu a été conçu pour démontrer les capacités du système.

The results of this new algorithm and system are presented in this chapter: the tracking quality is evaluated by comparison with a reference trajectory acquired by a motion capture system; the quality of the cartography can only be evaluated by manual measurements. The results are good enough to perform visual augmentation of a real image with a virtual object, and a small game has been designed to demonstrate the systems capabilities.

Experimental conditions

To test our system and the associated algorithm, experiments have been performed in a room equipped with a motion capture system (distributed by Vicon). This system, widely used in research as well as in the entertainment industry, consists of a set of cameras pointing towards the center of the room, equipped with infrared optical filters and infrared LEDs. The positions and orientations of these cameras have to be carefully calibrated before any experiment. Reflecting dots (markers) are embedded on the system whose trajectory is to be estimated: the set of markers forms a rigid object, tracked by the cameras (infrared filtering ensures that only these dots are detected). Provided that these dots are detected by enough cameras, the position and orientation of the object can be estimated, with a millimeter resolution. The actual resolution of reconstruction depends on the number of detecting cameras. See [START_REF] Aghajan | Multi-camera networks: principles and applications[END_REF] for more details on motion capture systems. Figure 10.1 shows our system equipped with the reflective dots: at the forefront of the system is the RGBD sensor (an Xtion Pro distributed runs on the Graphical Processing Unit (GPU) of the computer, which enables Massively Parallel Computation. The successive modifications applied to the Kintinuous project confirm our intuition that loop-closure detection is necessary, and suggest that significant improvements in real-time performances and increase of the map size should be inquired by parallel implementation on GPU of our algorithms.

Appendix A

Differential geometry on the Riemannian unit sphere

To deal with vector fields, gradients and algebraic computations on the unit sphere S 2 , two different approaches can be adopted:

-the sphere S 2 can be thought of as a subset of R 3 , where any point or vector has regular coordinates (e.g. Cartesian or spherical), and special constraints must be taken into account to make sure that vectors and points lie on that surface. For example, a point X has Cartesian coordinates (x, y, z) T where x 2 + y 2 + z 2 = 1. -the sphere S 2 can be thought of as a two-dimensional manifold, where vectors and metrics are defined with respect to curves living on S 2 . Although the first approach is the most natural to begin with, computations can easily become tedious. The second approach, adopted in this thesis, requires first to understand the basics of Riemannian metrics and the differential geometry associated. In the following, we first recall these basics in a simplified way (more details can be found in [Stoker, 1969] or [START_REF] Doubrovine | Géométrie contemporaine: méthodes et applications[END_REF]), then some useful relations of differential geometry are established.

A.1 Riemannian manifolds

A n-dimensional r-differentiable manifold M can be thought as a topological space which is locally equivalent to the Euclidean vector space R n . A precise definition involves a countable collection of patches covering M. Any of these patches can be mapped into the unit ball of R n . Compositions of these mappings are consistent with respect to each other. The mappings are differentiable of order r with respect to the Euclidean coordinates of R n . Any composition of the mappings has non-vanishing Jacobian. Finally, M is connected.

A Riemannian manifold is a manifold for which a Riemannian metrics is defined and enables to define the concepts of lengths and vectors, necessary to work on intrinsic differential geometry. First, let us define a differentiable curve: it is the locus of points p(t) in the manifold that results when the local coordinates u i of the points of the locus are differentiable functions u i (t) of a parameter t. To define the length of such a curve, let us a define the length of a line element dl, independent (by assumption) of the choice of local coordinates:

where the functions g ik are defined with respect to the coordinates, are differentiable and such that the quadratic form in the differentials is positive definite. To make sure that such a definition does not depend on the choice of local coordinates, it is required that the functions g ′ ik associated to new coordinates u ′ i must satisfy

Then, the length of of a differentiable curve associated to u i (t) is defined by

dt and, more generally, a metric is defined using only the coefficients g ik , so that the geometrical metric properties are invariant under change of local coordinates. Then, vectors in the manifold M are identified with the tangent vectors of differentiable curves. For example, let us consider a curve u i (t) through a point p that corresponds to t = 0; its tangent vector at p can be written in the form du i dt t=0 . Every differentiable curve through p defines such a vector v. Conversely, every vector v is the tangent vector of a curve. These vectors form a linear vector space, called the tangent space of the manifold M at the point p.

These general matters are used in this thesis under the following form:

-a point on the unit sphere S 2 is associated to a unit vector η; -considering a mapping S 2 ∋ η ↦ h(η) ∈ R, the gradient of h at point η, denoted ∇h, lives in the tangent space at η denoted T η ; -any differential geometry manipulation on h only has meaning in the tangent space T η .

A.2 Differential calculus

In this section, we will establish some useful relations. Unless otherwise specified, P is a constant vector of R 3 . Let us define the mapping

By definition of the differential of a mapping f defined on S 2 ,

which concludes the proof of the relation (A.4).

Finally, we prove that ∇ ⋅ (η × P) = 0 (A.7)

The method used to prove the relation (A.4) is applied to the vector field f (η) = η × P. This yields

Rearranging the terms yields the Jacobi identity:

which by identification with (A.6) yields

Appendix B

Matrix Lie groups and Lie algebra

We will focus in this short description on matrix Lie groups on R.

A matrix Lie group is any subgroup G of the group GL(n, R) of invertible real matrices with the following property: if A m is any sequence of matrices in G and A m converges to some matrix A, then either A ∈ G, or A is not invertible. Thus a matrix Lie group is a closed subgroup of GL(n, R). GL(n, R) itself is a matrix Lie group. Other matrix Lie groups of interest will be studied in the following sections.

A Lie group is a differentiable manifold G which is also a group and such that the group product G × G ↦ G and the inverse map g ↦ g -1 are differentiable. A matrix Lie group is a Lie group (as its name indicates, but not obviously from its definition).

Matrix Lie groups are in fact transformations that act on a vector space (e.g. R n ), and composition of such transformations are simply matrix products.

The exponential of a square matrix X, denoted exp X is defined as

Such a definition is consistent since it is proven that the series converges. The matrix exponential is a continuous function of X.

The main properties of the matrix exponential are the following:

-

The logarithm of a square matrix X, denoted log X, is defined as

whenever the series converge. The logarithm of a matrix X is defined and continuous on the set of square matrices with X -I < 1.

Let G be a matrix Lie group. The Lie algebra of G, denoted g, is the set of all matrices X such that exp tX is in G for all real numbers t.

Appendix B. Matrix Lie groups and Lie algebra

If G is a connected matrix Lie group, then every element A of G can be written in the form

Suppose G is a matrix Lie group with Lie algebra g. Then g is the tangent space at the identity to G.

Any element X I of the tangent space at the identity (and thus of g) can be written in the form

where the G i are the infinitesimal generators of the Lie algebra and the α i are real numbers. Consider an element A of G; a small variation around A acts as exp X I : A ′ = exp X I A where A ′ is the new transformation. From the point of view of the vector space on which the Lie group G acts, consider two vectors x and y such that

y ′ is the result of a variation X I at A acting on y:

We can also formally write δA ∶= A ′ -A. Then Lie groups and Lie algebras have bee extensively studied. See for example [Hall, 2003] for a good introduction on these topics, and [Olver, 1999] for infinitesimal invariance theory applied to Lie groups.

Appendix C

3D rigid-body kinematics and quaternions

C.1 Attitude and angular velocity

The attitude (or orientation) of a rigid body can be represented in many ways: we will start with the most general one and give some other examples that are of great interest for specific applications.

C.1.1 The special orthogonal group

The orthogonal group O(n) is the group of orthogonal matrices, that is, the column vectors that make up such a matrix are orthonormal. An equivalent definition is that A ∈ O(n) if and only if A T A = I n (or A -1 = A T ). Consequently, the determinant of orthogonal matrices is ±1. The Special Orthogonal group SO(n) is the subgroup of orthogonal matrices with determinant one. The elements of SO(n) are the rotations of R n , and thus describe the orientation of a rigid body with respect to fixed external frame of reference. For example, every element of SO(2) has the form cos θsin θ sin θ cos θ , with θ ∈ R.

The Special Orthogonal group is a matrix Lie group. The Lie algebra of SO(n) is the space of all matrices X of M n (R) such that X T = -X: they are denoted dtΩ where Ω is written in the form

The infinitesimal generators at the identity (for n = 3) are

where ω is the angular velocity introduced in Section C.1. The reason why acceleration is only introduced in body coordinates is that the sensors used in this thesis are "strap-down" sensors, rigidly attached to the moving body frame. Body coordinates is the natural expression of these sensors. This is also the case for angular velocity. To be precise, embedded accelerometers provide measurements of the specific acceleration a γ = aq * Aq where A = ⎛ ⎜ ⎝ 0 9.81 0

q * Aq is the gravity acceleration expressed in the body frame R cam . Finally, the linear velocity expressed in R cam satisfies ∂ t v = v × ω + aq * Aq.

(C.5)

The main issue of inertial navigation with strap-down systems is encrypted in this equation: a precise estimation of attitude is required to properly reconstruct linear velocity. In practice, this also means that errors in acceleration measurements are indistinguishable of imprecise estimation of attitude.

C.3 The Special Euclidean group

Another matrix Lie group of interest is the Special Euclidean group SE(n). The Euclidean group E(n) by definition consists of all one-to-one, onto, distance-preserving maps of R n to itself. In fact, one can prove that every element of E(n) can be written uniquely as an orthogonal linear transformation R followed by a translation T: it maps a vector x ∈ R n into Rx + T ∈ R n . It is denoted (R, T). The product operation for E(n) is the following:

The inverse of an element of E(n) is given by (R, T) -1 = (R -1 , -R -1 T).

Since translations are not linear, E(n) is not a subgroup of GL(n, R), but it is isomorphic to a subgroup of GL(n + 1, R): to any element (R, T) of E(n), one associates the matrix R T 0 1×n 1 .

Product operator and inversion are then simply expressed as matrix multiplication and inversion. SE(n) is the subgroup of E(n) where the element R is a member of SO(n). It actually describes all rigid-body transformations of R n . To be more specific, it enables to linearly express the coordinates of a given fixed vector in a moving reference frame with respect to the coordinates of the same vector in a fixed reference frame: it is an extrinsic calibration matrix (see Section 1.1.1).