Simon Lacroix

Gianluca Antonelli

Manu, Amaury, Stephanie Alessandro Barbara

Anne Lukas

Juan David

Keywords: Inspec>on, Environmental analysis, Cartography, Aerial photography, Live broadcas>ng, Inspec>on, Surveillance

1. Imagine a circle that contains all of human knowledge; 2. By the time you finish elementary school, you know a little; 3. By the time you finish high school, you know a bit more; 4. With a bachelor's degree, you gain a specialty; 5. A master's degree deepens that specialty; 6. Reading research papers takes you to the edge of human knowledge; 7. Once you're at the boundary, you focus; 8. You push at the boundary for a few years; 9. Until one day, the boundary gives way; 10. And, that dent you've made is called a PhD; 11. Of course, the world looks different to you now; 12. So, don't forget the bigger picture: 13. Keep pushing.

Going through a PhD is a long travel which passes over all those steps, each transition was an important moment, and in each of those moments I had the inestimable chance to be surrounded by people who supported me, who coached me, who trusted me, who helped me. Now, sorry if those couple of pages will not have a "formal shape", but this is for me the best moment of my three years of PhD: the moment in which I can finally thank all the people who made this possible.

I would like to start thanking my supervisor, Agostino Martinelli, for the support, the patience, the advices, the interesting discussions, his epic stories and for allowing me to defend my thesis even if I didn't go yet on the top of the "Mont Blanc". Thanks to my co-supervisor Davide Scaramuzza for hosting me 8 months in his lab in Zurich, for all the discussions and the feedbacks and thanks because the 8 months spent at the AI Lab gave me also the big gift of a dear friend. Thanks to my thesis director Christian Laugier, for his guidance and for being an example of

To my Mum and Dad, because without their love and support nothing would have been possible.

Abstract

Accurate egomotion estimation is of utmost importance for any navigation system. Nowadays different sensors are adopted to localize and navigate in unknown environments such as GPS, range sensors, cameras, magnetic field sensors, inertial sensors (IMU). In order to have a robust egomotion estimation, the information of multiple sensors is fused. Although the improvements of technology in providing more accurate sensors, and the efforts of the mobile robotics community in the development of more performant navigation algorithms, there are still open challenges. Furthermore, the growing interest of the robotics community in micro robots and swarm of robots pushes towards the employment of low weight, low cost sensors and low computational complexity algorithms. In this context inertial sensors and monocular cameras, thanks to their complementary characteristics, low weight, low cost and widespread use, represent an interesting sensor suite. This dissertation represents a contribution in the framework of vision-aided inertial navigation and tackles the problems of data association and pose estimation aiming for low computational complexity algorithms applied to MAVs. For what concerns the data association, a novel method to estimate the relative motion between two consecutive camera views is proposed. It only requires the observation of a single feature in the scene and the knowledge of the angular rates from an IMU, under the assumption that the local camera motion lies in a plane perpendicular to the gravity vector. Two very efficient algorithms to remove the outliers of the feature-matching process are provided under the abovementioned motion assumption. In order to generalize the approach to a 6DoF motion, two feature correspondences and gyroscopic data from IMU measurements are necessary. In this case, two algorithms are provided to remove wrong data associations in the feature-matching process. In the case of a monocular camera mounted on a quadrotor vehicle, motion priors from IMU are used to discard wrong estimations.

For what concerns the pose estimation problem, this thesis provides a closed form solution which gives the system pose from three natural features observed in a single camera image, once the roll and the pitch angles are obtained by v the inertial measurements under the planar ground assumption. Specifically, the system position and attitude can uniquely be determined by observing two point features but improved by exploiting the geometric constraints inherent to a virtual pattern formed by the three features.

In order to tackle the pose estimation problem in dark or featureless environments, a system equipped with a monocular camera, inertial sensors and a laser pointer is considered. The system moves in the surrounding of a planar surface and the laser pointer produces a laser spot on the abovementioned surface. The laser spot is observed by the monocular camera and represents the only point feature considered. Through an observability analysis it is demonstrated that the physical quantities which can be determined by exploiting the measurements provided by the aforementioned sensor suite during a short time interval are: the distance of the system from the planar surface; the component of the system speed that is orthogonal to the planar surface; the relative orientation of the system with respect to the planar surface; the orientation of the planar surface with respect to the gravity. A simple recursive method to perform the estimation of all the aforementioned observable quantities is provided. This method is based on a local decomposition of the original system, which separates the observable modes from the rest of the system.

All the contributions of this thesis are validated through experimental results using both simulated and real data. Thanks to their low computational complexity, the proposed algorithms are very suitable for real time implementation on systems with limited on-board computation resources. The considered sensor suite is mounted on a quadrotor vehicle but the contributions of this dissertations can be applied to any mobile device. vi Résumé L'estimation précise du mouvement 3D d'une caméra relativement à une scène rigide est essentielle pour tous les systèmes de navigation visuels. Aujourd'hui différents types de capteurs sont adoptés pour se localiser et naviguer dans des environnements inconnus : GPS, capteurs de distance, caméras, capteurs magnétiques, centrales inertielles (IMU, Inertial Measurement Unit). Afin d'avoir une estimation robuste, les mesures de plusieurs capteurs sont fusionnées. Même si le progrès technologique permet d'avoir des capteurs de plus en plus précis, et si la communauté de robotique mobile développe algorithmes de navigation de plus en plus performantes, il y a encore des défis ouverts. De plus, l'intérêt croissant des la communauté de robotique pour les micro robots et essaim de robots pousse vers l'emploi de capteurs à bas poids, bas coût et vers l'étude d'algorithmes à faible complexité. Dans ce contexte, capteurs inertiels et caméras monoculaires, grâce à leurs caractéristiques complémentaires, faible poids, bas coût et utilisation généralisée, représentent une combinaison de capteur intéressante.

Cette thèse présente une contribution dans le cadre de la navigation inertielle assistée par vision et aborde les problèmes de fusion de données et estimation de la pose, en visant des algorithmes à faible complexité appliqués à des microvéhicules aériens.

Pour ce qui concerne l'association de données, une nouvelle méthode pour estimer le mouvement relatif entre deux vues de caméra consécutifs est proposée. Celle-ci ne nécessite l'observation que d'un seul point caractéristique de la scène et la connaissance des vitesses angulaires fournies par la centrale inertielle, sous l'hypothèse que le mouvement de la caméra appartient localement à un plan perpendiculaire à la direction de la gravité. Deux algorithmes très efficaces pour éliminer les fausses associations de données (outliers) sont proposés sur la base de cette hypothèse de mouvement. Afin de généraliser l'approche pour des mouvements à six degrés de liberté, deux points caracteristiques et les données gyroscopiques correspondantes sont nécessaires. Dans ce cas, deux algorithmes sont proposés pour éliminer les outliers. Nous montrons que dans le cas d'une caméra monoculaire montée sur un quadrotor, les informations de mouvement fournies par l'IMU peuvent être utilisées pour éliminer de mauvaises estimations.

Pour ce qui concerne le problème d'estimation de la pose, cette thèse fournit une solution analytique pour exprimer la pose du système à partir de l'observation de trois points caractéristiques naturels dans une seule image, une fois que le roulis et le tangage sont obtenus par les données inertielles sous l'hypothèse de terrain plan. Plus spécifiquement, la position et l'attitude du système peuvent être uniquement déterminées à partir de l'observation de deux points caractéristiques, mais améliorées en exploitant les contraintes géométriques inhérentes à un pattern virtuel formé par les trois points caractéristiques.

Afin d'aborder le problème d'estimation de la pose dans des environnements sombres ou manquant de points caractéristiques, un système équipé d'une caméra monoculaire, d'une centrale inertielle et d'un pointeur laser est considéré. Le système se déplace dans l'entourant d'une surface plane et le pointeur laser produit un petit point sur la surface. Le point laser est observé par la caméra monoculaire et représente le seul point caractéristique considéré. Grâce à une analyse d'observabilité il est démontré que les grandeurs physiques qui peuvent être déterminées par l'exploitation des mesures fourni par ce systeme de capteurs pendant un court intervalle de temps sont : la distance entre le système et la surface plane ; la composante de la vitesse du systme qui est orthogonale à la surface ; l'orientation relative du système par rapport à la surface et l'orientation de la surface par rapport à la gravité. Une méthode récursive simple a été proposée pour l'estimation de toutes ces quantités observables. Cette méthode est basée sur une décomposition locale du système d'origine, qui sépare les modes observables du reste du système.

Toutes les contributions de cette thèse sont validées par des expérimentations à l'aide des données réelles et simulées. Grace à leur faible complexité de calcul, les algorithmes proposés sont très appropriés pour la mise en oeuvre en temps réel sur des systèmes ayant des ressources de calcul limitées. La suite de capteur considérée est monté sur un quadrotor, mais les contributions de cette thèse peuvent être appliquées à n'importe quel appareil mobile. Paul Corny machine (both from 1907), Bréguet Giant Gyroplane. 1924: Ohemichen's quadrotor; 2000: ETH Zurich's OS4 [START_REF] Bouabdallah | Pid vs lq control techniques applied to an indoor micro quadrotor[END_REF], [START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF], the CEA's X4-flyer [START_REF] Guenard | Dynamic modeling and intuitive control strategy for an "x4-flyer[END_REF] and the ANU's X-4 Flyer [START_REF] Pounds | Construction and Control of a Large Quadrotor micro air vehicle[END_REF]; Nowadays there exists a lot of quadrotors. We list here few exemplars: AscTec Pelican [1], KMelRobotics' NanoQuad [3], Mikrokopter's quadrotor [4], Flyduino, Arducopter, Parrot AR.Drone, DeltaDrone quadrotor [2]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Quadrotors in research projects and in companies. . . . . . . . . . 1. 4 Quadrotor notation. The four rotors are depicted in blue. w i is the angular velocity of the i-th rotor, F i and M i are the vertical force and the moment respectively produced by the i-th rotor. The body-vehicle's frame B is shown in black, it is attached to the vehicle, and its origin is coincident with the vehicle's center of mass. In gray it is depicted the world reference frame W. . . . . . 1. [START_REF]Skybotics UAV navigation solutions[END_REF] The quadrotor concept. The width of the arrows is proportional to the angular speed of the propellers. . . . . . . . . . . . . . . . 1. [START_REF]Vicon Motion Capture System[END_REF] Properties of some sensors commonly on-board a MAV. . . . . . . 1.7 Flight Assembled Architecture (a) at FRAC Centre in Orlans, France [START_REF] Raffaello | Flying machine enabled construction[END_REF]. Robot Quadrotors Perform James Bond Theme (b) [START_REF] Kumar | Robots that fly[END_REF].

Context

In recent years, flying robotics has received significant attention from the robotics community. The ability to fly allows easily avoiding obstacles and quickly having an excellent birds eye view. These navigation facilities make flying robots the ideal platform to solve many tasks like exploration, mapping, reconnaissance for search and rescue, environment monitoring, security surveillance, inspection etc. In the framework of flying robotics, micro aerial vehicles (MAV) have a further advantage. Due to the small size they can also be used in narrow out-and indoor environment and they represent only a limited risk for the environment and people living in it.

Why quadrotors?

Micro aerial vehicles can be classified into: Fixed-wing, Flapping-wing and rotorcrafts [START_REF] Kumar | Opportunities and challenges with autonomous micro aerial vehicles[END_REF] (Figure 1.1). Fixed-wing vehicles are not very agile in three-dimensional complex environment. Flapping-wing vehicles can be divided into avian-style and insect-style vehicles [START_REF] Mackenzie | A flapping of wings[END_REF]. The development of the former is strongly limited by the lack of knowledge about fluid-structure coupling and aeroelasticity. Insect-style flapping wing MAVs and rotor-craft can perform stationary flight and forward flight, which represents a significant advantage in terms of maneuverability. Nevertheless, insect-style flapping wing vehicles present an increasing complexity and it has not yet been demonstrated whether they can be considered more convenient than rotor crafts. The most common rotor craft configurations for MAVs are the Coaxial rotorcraft and the Quadrotor aircraft. The former is well represented by the Skybotix Coax [START_REF]Skybotics UAV navigation solutions[END_REF], a vehicle equipped with two co-axial, counter-rotating rotors and a stabilizer bar and the quadrotor vehicles. Quadrotors, thanks to their fast reaction to external disturbances, light weight and low crash impact, inherent safety, compactness, simple mechanical design, easier maneuverability and controllability and ability to carry small payloads, are nowadays the best option. , [START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF], the CEA's X4-flyer [START_REF] Guenard | Dynamic modeling and intuitive control strategy for an "x4-flyer[END_REF] and the ANU's X-4 Flyer [START_REF] Pounds | Construction and Control of a Large Quadrotor micro air vehicle[END_REF]; Nowadays there exists a lot of quadrotors. We list here few exemplars: AscTec Pelican [1], KMelRobotics' NanoQuad [3], Mikrokopter's quadrotor [4], Flyduino, Arducopter, Parrot AR.Drone, DeltaDrone quadrotor [2].

Brief quadrotor's history

At the beginning of the 20th century the French scientist and academician Charles Richet built a small, unmanned helicopter. Although the vehicle was not successful, it inspired two of his students, Louis and Jaques Bréguet, which in 1907, under his supervision, built the first quad-rotor (Figure 1.2) [START_REF] Leishman | The Breguet-Richet Quad-Rotor Helicopter of[END_REF]. At the same time, the French engineer Paul Corny, designed an other quadrotor vehicle (Figure 1.2). Both the machines were reported to have carried a pilot off the ground but both of them lacked in stability and didn't have a proper control architecture.

In 1920, Étienne Ohemichen, an employee of the French Peugeot car company, built a quadrotor machine, with eight additional rotors for control and propulsion. Nevertheless, his machine was underpowered and needed a hydrogen balloon for the stabilization. After various attempts, in 1924 Ohemichen demonstrated that a vertical flight machine could be stable and somehow maneuverable, and his vehicle was considered the earliest mention of a complete quadrotor hovering vehicle in history (Figure 1.2).

In 1956, Marc Adam Kaplan designed the most successful of the early designs of the rotor-craft (the Convertawings Model A quadrotor). With this machine Kaplan proved the quadrotor concept and realized the first four-rotor helicopter able to perform successful forward flight. This helicopter was intended to be the prototype for a line of much larger civil and military quadrotor helicopters, but due to a lack of orders for commercial or military versions, the project was terminated.

At the beginning of the third millennium, quadrotors drew the attention of academic researchers in order to address the problems faced by small-scale UAVs. The ETH Zurich's OS4 [START_REF] Bouabdallah | Pid vs lq control techniques applied to an indoor micro quadrotor[END_REF], [START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF], the CEA's X4-flyer [START_REF] Guenard | Dynamic modeling and intuitive control strategy for an "x4-flyer[END_REF] and the ANU's X-4 Flyer [START_REF] Pounds | Construction and Control of a Large Quadrotor micro air vehicle[END_REF] represents the first quadrotor research platforms.

Applications

Estimates from UAV Market Research (2011) reveals that the UAV market is estimated to exceed US$60 bilion in the next three years, and this forecast does not take into account for the thousands of MAVs already fielded.

As illustrated in Section 1.1.2, the development of quadrotors has been boosted by the military (Figure 1.2). Nowadays, considering the high number and variety of research projects involving quadrotors and of companies already selling readyto-flight products, their application field is widely extended (Figure 1.3). Quadrotor vehicles result suitable for search and rescue operations [START_REF] Michael | Collaborative mapping of an earthquake-damaged building via ground and aerial robots[END_REF], [START_REF] Scaramuzza | Vision-controlled micro flying robots: from system design to autonomous navigation and mapping in gps-denied environments[END_REF], powerline inspection [START_REF] Hrabar | Development of an autonomous helicopter for aerial powerline inspections[END_REF], [START_REF] Sa | Vertical infrastructure inspection using a quadcopter and shared autonomy control[END_REF], and building inspection, crop dusting, precision farming, remote sensing, security related tasks, aerial mapping, aerial photography, aerial delivery and cinematography tasks.

As stated by Vijay Kumar in [START_REF] Kumar | Opportunities and challenges with autonomous micro aerial vehicles[END_REF]: "While fixed-base industrial robots were the main focus in the first two decades of robotics, and mobile robots enabled most of the significant advances during the next two decades, it is likely that UAVs, and particularly micro-UAVs, will provide a major impetus for the next phase of education, research, and development".

The quadrotor concept

Quadrotors are VTOL (Vertical Take-Off and Landing) MAVs, lifted and propelled by four rotors in cross configuration (Figure 1.4). They present two pairs of identical fixed-pitch propellers, driven in opposite direction (two clockwise and two counter-clockwise). This allows to control lift and torque avoiding the need of a tail rotor. Control of vehicle motion is performed by varying the angular velocity of one or more propellers, thereby changing its torque load and thrust/lift characteristics (Figure 1.5). Taking-off and landing are performed by increasing or decreasing respectively the speed of the four rotors simultaneously. Rotation about the vertical-axis is obtained by augmenting the angular speed of two opposite propellers while decreasing the speed of the remaining two. Translational motion is strongly coupled to the vehicle attitude. In order to achieve a lateral movement, the quadrotor must adjust its pitch or roll by augmenting the angular speed of one rotor and decreasing the one of its diametrically opposite rotor. Due to its four actuators and its six degrees of freedom motion, the quadrotor is an under-actuated and highly dynamically unstable system.

Motivation and objectives

A crucial problem on an airborne vehicle is the stabilization and control in attitude and position, i.e. in six degrees of freedom. Most of the controlling approaches for MAVs present a cascade control structure [START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF]. The inner loop is devoted to the attitude control and the outer one to the position control. Due to the vehicle's high dynamics, the attitude controller must run at higher frequency than the position one.

With an attitude controller it is possible to perform a stationary flight at a fixed altitude, but it is not possible to compensate for drifts in the horizontal plane. The attitude controller relies on inertial measurement units, while the choice of the sensors related to the position controller is strictly task dependent and represents still an open problem for MAVs.

The most popular approaches to solve this problem are mainly based on the fusion of the data provided by an IMU and a GPS [START_REF] Abdelkrim | Robust nonlinear filtering for ins/gps uav localization[END_REF], [START_REF] Yun | Enhancement of gps signals for automatic control of a uav helicopter system[END_REF], or IMU, GPS and camera [START_REF] Vincenzo Angelino | High altitude uav navigation using imu, gps and camera[END_REF]. However, these approaches require a reliable GPS signal. Laser range finders have also been adopted in this framework [8], [START_REF] Bachrach | Autonomous flight in unstructured and unknown indoor environments[END_REF], [11]. Nevertheless, range finders sensors have two drawbacks: they have a restricted perception area (limited distance and field of view) and they are still too heavy for MAVs.

Figure 1.6 shows different sensors that can be mounted on a micro aerial vehicle with their advantages and drawbacks. Vijay Kumar's group at GRASP Lab (General Robotics, Automation, Sensing and Perception), University of Pennsylvania and Raffaello D'Andrea's group at IDSC (Institute for Dynamic Systems and Control), ETH Zurich, achieved impressive results with quadrotors catching the attention of not only all the robotics community but also of the media and the general public (Figure 1.7). Aggressive maneuvers [START_REF] Turpin | Trajectory design and control for aggressive formation flight with quadrotors[END_REF], dancing quadrotors [START_REF] Schoellig | So You Think You Can Dance? Rhythmic Flight Performances with Quadrocopters[END_REF], acrobatics [START_REF] Brescianini | Quadrocopter pole acrobatics[END_REF], quadrotors throwing and catching a ball [START_REF] Ritz | Cooperative quadrocopter ball throwing and catching[END_REF], constructions with quadrotor teams [START_REF] Willmann | Aerial Robotic Construction: Towards a New Field of Architectural Research[END_REF], [START_REF] Lindsey | Construction with quadrotor teams[END_REF] represent outstanding works on quadrotor control and trajectory tracking, but they are feasible thanks to the high frequency vehicle position feedback provided by a motion capture system [START_REF]Vicon Motion Capture System[END_REF]. This means that the vehicles' workspace must be equipped with high resolution (up to 16 Megapixels) and high frame rate (up to 1000 fps) cameras as shown in Figure 1.7c. Motion capture systems represent a perfect testbed to develop control algorithms, to test state estimation algorithms and to

GPS

• Absolute posi*on; (outdoor).

• Bad or no recep*on in indoor or urban environments.

Magne2c field sensors

• Earth's magne*c field direc*on (outdoor).

• Disturbed by electronic devices nearby

Barometric / Pressure sensors

• Absolute al*tude.

• Not reliable indoor;

• Affected by weather condi*ons.

Airspeed sensors

• Vehicle's airspeed. • Not suitable for rotorcraFs.

Cameras

• Vast informa*on;

• Visual feedback.

• Affected by light changes; • Textured environments required.

Laser rangefinders

• Distance to objects. A viable option for GPS denied environments is obtained by fusing visual and inertial data. This option has become very popular for Micro Aerial Vehicle (MAV) navigation due to the low cost, power consumption and weight.

Minimalist perception

The trend of robotics miniaturization boosts researchers towards minimalist design, investigation of the least complex solutions to a given class of tasks and selection of the simplest set of sensors. According to this perspective, throughout this dissertation we decided to tackle the problems of data association and pose estimation in the framework of MAVs navigation.

The considered sensor suite is essentially composed of a monocular camera rigidly attached to an inertial measurement unit and mounted on a micro quadrotor. The choice of the sensors is related to their complementary properties (see Figure 2.1), their low power consumption, low cost and low weight.

Once chosen our minimal set of sensors, we want to establish the minimal amount of information necessary to perform the data association and pose esti-

Contributions

The topics addressed by this thesis are the data association and the pose estimation of a camera-IMU system in the framework of MAVs navigation. The previous section highlighted the importance of on-board perception and the limitations inherent to small flying platforms such as limited payload and resources. In order to find a compromise between the aforementioned specifics, we propose minimal complexity algorithms which exploits system properties of typical navigation constraints in indoor or city-like environments. The contributions of this dissertation are summarized below:

1. Two low computational complexity methods to perform the outlier detection task.

1.1 1-point algorithm. Relying on the assumption that the local vehicle motion lies in a plane perpendicular to the gravity vector, we provide an efficient algorithm which only requires the observation of a single feature in the scene and the knowledge of the angular rates from an IMU. It's efficiency and low computational complexity make it suitable for real-time implementations.

1.2 2-point algorithm. Algorithm which requires the observation of one more feature with respect to the previous one, but it relaxes the hypothesis on the vehicle motion, being therefore suitable to tackle the outlier detection problem in the case of a 6DoF motion. Additionally, we show that if the monocular camera is rigidly attached to a quadrotor vehicle, motion priors from IMU can be exploited to discard wrong estimations in the framework of a 2-point-RANSAC based approach.

2. Two low computational complexity algorithms to face the pose estimation problem.

2.1 Virtual patterns. Many localization algorithms utilize artificial landmarks, such as for example ultrasonic beacons, bar-code reflectors, visual pattern, but they are not reliable in a landmark-free environment. Nevertheless, the geometry of a known visual pattern encodes useful information. Starting from this consideration, and taking advantage of the so called "planar ground assumption" (common scenario in indoor or city-like environments), we propose an algorithm which exploits the geometric information encoded in the angles of a virtual triangle made by three natural features belonging to the ground plane. The algorithm is based on a closed solution which provides the vehicle pose from a single camera image, once the roll and pitch angles are obtained by the inertial measurements.

Virtual features.

The feature extraction and matching task is computationally very expensive and fails in dark or featureless environments.

In order to significantly reduce the computational burden and to make the feature matching task more robust with respect to outliers, we introduce a virtual feature by equipping the vehicle with a laser pointer. This problem differs from the classical vision and IMU data fusion one, because the feature is moving jointly with the vehicle. To the best of our knowledge, this problem has never been considered so far.

-Observability analysis. An observability analysis is performed to identify the physical quantities (called "observable modes") that can be estimated by using the information provided by the aforementioned sensor suite. -Local decomposition. To estimate the observable modes, we perform a local decomposition of the original system and we apply a simple recursive method (Extended Kalman Filter) to the observable sub-system.

Thesis outline

The rest of the thesis is structured as follows.

Chapter 2 -Vision and Inertial sensors -provides a brief overview of visual and inertial sensing from both a biological and technological point of view. The calibration techniques used during the experiments are here described.

Chapter 3 -Data association -introduces the first two contributions of this dissertation. In the first section we provide an overview of the data association problem for the feature matching process. The feature detection, tracking and matching problems are introduced. In the second section we describe the data association problem and we provide an overview about the state of the art. Two low computational complexity methods to perform the outlier detection task between two different views of a monocular camera rigidly attached to an inertial measurement unit are presented.

Chapter 4 -Pose estimation -introduces the last two contributions of this dissertation. In the first section we provide an overview of the visual-aided inertial pose estimation problem, with an emphasis on aerial navigation. Two low computational complexity algorithms to face the pose estimation problem are presented.

Chapter 5 -Conclusions -summarizes the contributions of this dissertation and provides perspectives for future developments. In this chapter we give a brief overview of visual and inertial sensing from both a biological and technological point of view. The calibration techniques used during the experiments are here described.

In recent years, the fusion of vision and inertial sensing has received great attention by the mobile robotics community. These sensors require no external infrastructure and this is a key advantage for robots operating in unknown environments where GPS signals are shadowed. Additionally, these sensors have very interesting complementarities and together provide rich information to build a system capable of vision-aided inertial navigation and mapping.

CAMERA IMU

Slow mo'on

Impossible to dis'nguish

• A near object with low rela've speed from a far object with higher rela've speed.

• A change in inclina'on from body accelera'on.

A biological overview

A special issue of the International Journal of Robotics Research was recently been devoted to the problem of fusing vision and inertial data [START_REF] Dias | Editorial : Special issue : 2nd workshop on integration of vision and inertial sensors[END_REF]. In [START_REF] Corke | An introduction to inertial and visual sensing[END_REF], a tutorial introduction to the vision and inertial sensing is presented. This work provides a biological point of view and it illustrates how vision and inertial sensors have useful complementarities allowing them to cover the respective limitations and deficiencies.

The sense of sight

Vision is one of the most important senses and is common to almost all living creatures. In an essay on the differences between human and animal vision, [START_REF] Erichsen | Human and Animal Vision[END_REF], the authors stated that "we must never make the mistake of thinking that only we see the world as it really is." The evolution of the organs dedicated to visual perception followed the different needs of animals and adapted to different circumstances. Eyesight helps fulfilling the most basic living activities: hunt, exploration, protection. Nature teaches us that the way how to perceive the environment, to interpret it, and therefore the technology behind the development of new sensing sensors, must be application dependent.

The organs devoted to visual perception are the eyes. They perform the conversion of light into electro-chemical impulses. There are more than 40 different types of eye in nature. The simplest eye, the one belonging to microorganisms, only detects if the environment is dark or bright.

The human eye (Figure 2.2) has a more complex structure which allows us to collect the light in the environment, regulate its intensity through a diaphragm Insects have compound eyes (Figure 2.3), consisting of thousands of photoreceptor units (ommatidium) located on a convex surface. The perceived image derives from the combination of the inputs of each individual eye unit. One of the benefits of compound eyes with respect to simple eyes is the large field of view. In some cases they are also able to detect the polarization of light.

The perception of gravity

All living organisms must have a perception of the environment in which they live, of the forces that are acting on them and of their motion. An important role is assumed by the perception of gravity and of the orientation of the body with respect to it. In most mammals, the sensing system devoted to the perception of gravity (therefore movement and sense of balance) is called vestibular system (Figure 2.4a) and it presents a similar structure of an Inertial Measure- ment Unit. The human vestibular system is located within the inner ear and it is composed of three main parts: the vestibule, the cochlea, the circular canals. In the vestibule there are the utricle and the saccule, two otoliths organs devoted to the measurement of inertial and gravitational forces (Figure 2.4b). Both of them contain otoliths which acts as proof mass. The otolithic membrane acts as the spring and the damper and hair celles, embedded in the membrane are responsible for the displacement detection. They provide therefore information about the linear motion and the angular position of the head. In order to distinguish the gravity from body motions, animals uses other cues such as vision. It seems also that they can divide the acceleration signal by frequency. Lower frequency components are associated to pose and high frequency ones to acceleration.

The angular velocity of the head is measured by the three semicircular canals which are oriented in three orthogonal planes [START_REF] Gillingham | Spatial orientation in flight[END_REF]. Each channel is filled with a viscous fluid. As a result of any rotation, the fluid pushes against one of the extremities of the channel, where is located the ampulla. The latter senses the corresponding force.

The brain processes all the signals provided by the three channels and provides an estimation of the head instantaneous angular velocity, in order to ensure gaze stability (process known as vestibulo-ocular reflex ).

Insects have similar ways to detect a change in speed and direction. Flies and other insects are equipped with mechanoreceptors (antennas) which contain Johnston's organ responsible of the antenna displacement detection due to pressure, gravity, or sound. Flies have also a device very similar to a gryroscope (halteres) (Figure 2.5) very important in gaze stabilization.

Inertial Measurement Unit

An Inertial Measurement Unit (IMU) is an electronic device usually composed by a three-axis accelerometer and a three-axis gyroscope. They are called Inertial

Accelerometers

An accelerometer senses the acceleration of a mass at rest in the frame of reference of the accelerometer device. An accelerometer at rest on the earth surface measures an acceleration of 9.81m/s 2 corresponding to the gravity acceleration. On the contrary, an accelerometer in free fall measures zero acceleration.

The physical principle exploited by an accelerometer is quite simple (Figure 2.7). A mass, m, usually called seismic mass or proof mass is supported by a spring c. A viscous damper, b, provides damping proportional to the relative velocity of the test mass and the sensor body and it is necessary for a quick stabilization of the system. When the sensor is subjected to acceleration, the mass is displaced and the displacement is measured in order to obtain the acceleration. Equation 2.1 expresses the dynamics of the system.

ẍ(t) + 2ξω n ẋ(t) + ω 2 n x(t) = -ÿ(t) (2.1)
The acceleration of the sensor body, ÿ(t), is converted to spatial displacement with a natural frequency ω n and a damping ratio ξ. The mechanical motion is then converted into an electrical signal.

It is not possible to distinguish if the sensor is accelerating or if it is subject to some components of gravity acceleration. To resolve this ambiguity it is necessary to make strong assumptions or to fuse the sensor information with measurements from other sensors.

Nowadays there are three main types of MEMS accelerometers: capacitive, piezo-electric and piezo-resistive. The piezo-electric devices have a large dynamic range but they are not suitable for inertial navigation systems due to the the lack of DC response. In piezo-resistive accelerometers, the test mass displacement is measured by a piezo-resistor which changes its value. They are preferred in high shock applications. In capacitive accelerometers, the mass displacement is measured by a changing capacitor. They are more performant in low frequency range and they can be used in servo mode to achieve high stability and linearity. A detailed overview about MEMS has been given in [START_REF] Yazdi | Micromachined inertial sensors[END_REF] and [START_REF] Lobo | Inertial Sensor Data Integration in Computer Vision Systems[END_REF].

Gyroscopes

A gyroscope is an heading sensor, used to measure or maintain orientation and it has benn invented by Léon Foucault in 1852. Gyroscopes can be essentially Vibrating structure gyroscopes (VSG), called also Coriolis Vibratory Gyroscopes (CVG), do not present any rotating disk. The basic physical principle is that a vibrating object tends to continue vibrating in the same plane as its support rotates. The Coriolis effect induced by rotation is measured.

Optical gyroscopes appeared for the first time at the beginning of 1980 and their primary application was related to aircraft. Instead of moving mechanical parts, optical gyroscopes rely on two monochromatic light beams, or lasers, emitted from the same source and they are based on the Sagnac effect, named after French physicist Georges Sagnac in 1913. Two laser beams are sent travelling through an optical fiber, one in clockwise direction and the other one in counterclockwise direction. According to the Sagnac effect, the beam travelling in the direction of rotation has a higher frequency. The difference in frequency of the two beams is proportional to the angular velocity of the cylinder. Optical gyroscopes are not sensible to vibrations, accelerations, shocks, and provide very precise rotational rate information.

Camera

Vision is a very powerful sense and this explains the great attention devoted in the last decades to the design and development of technological devices able to convert light into digital images and image processing algorithm able to extract task-dependent useful information from camera measurements. Vision sensors are nowadays very light weight, low cost, low power devices and they become A CMOS sensor presents advantages with respect to a CCD one. The CMOS chip is simpler, requires less power (one-hundredth less than a CCD chip). This is a very important characteristic if the camera is mounted on board to systems with limited autonomy.

For our experiments we choose therefore a monocular camera based on CMOS technology.

Pinhole Camera Model

A camera model, in order to characterize the transformation between 3D scene point coordinates and 2D image pixel coordinates, must take into account the transformation between the camera and the world, and the size and position with Following the convention of the pinhole camera, the image plane Π is located between the optical center and the scene (Figure 2.11). The axis perpendicular to the image plane and passing through the optical center is named optical axis and the intersection of it with the image plane is called principal point p p . The distance f between the image plane and the optical center is called focal length.

Let {C} be the camera reference frame, with the origin in the optical center o c (or center of projection) and with the z-axis parallel to the optical axis. Let u and v be the two axes that identify a 2D reference frame belonging to the image plane, with the center coincident with the principal point p p . Let P = [x, y, z] and p = [u, v] be the 3D coordinate of a scene point in the camera reference frame and its corresponding 2D pixel coordinate on the image plane respectively. By using homogeneous coordinates for P and p, p = [u, v, 1] and P = [x, y, z, 1] the projection equation becomes:

  λu λv λ   =   f k u 0 u 0 0 0 f k v v 0 0 0 0 1 0       x y z 1     . (2.2)
where u 0 and v 0 are the coordinates of the principal point, k u and k v are the inverse of the pixel size along u and v respectively, measured in pixel • m -1 . λ is equal to the third coordinate of P in the camera reference frame. A monocular camera provides therefore the position of a point in the scene, up to a scale factor λ. In order to recover λ we need a stereo pair, multiple images, or the fusion of monocular and inertial information.

If we want to express the coordinates of a point P in the world reference frame, we have to take into account the transformation between the last one and the camera reference frame. In this case, being Pw = [x w , y w , z w , 1] the homogeneous coordinate of a scene point P in the world reference frame, the projection equation becomes:

  λu λv λ   =   α u 0 u 0 0 α v v 0 0 0 1   [R|T ]     x w y w z w 1     . (2.3)
Where α u = f k u and α v = f k v represent the focal length in pixels. The matrices R and T encode the relative rotation and translation between the camera and the world reference frame and are called camera extrinsic parameters.

The matrix

A =   α u 0 u 0 0 α v v 0 0 0 1   (2.4)
is called intrinsic parameter matrix and its elements are called camera intrinsic parameters.

Camera Calibration

Camera calibration consists in the estimation of the intrinsic and extrinsic parameters of the camera model in order to map the image points into the corresponding scene points with the highest precision possible. The idea behind the camera calibration process is that by knowing the 3D coordinates P of some particular scene points and their corresponding pixel coordinates in the image plane p, it is possible to estimate the unknown parameters that characterize the camera model. One of the first camera calibration algorithm was proposed in 1987 by [START_REF] Tsai | A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses[END_REF] and it consists in a two-stage technique to compute the position and the orientation first and the internal parameters later, by knowing corresponding 2D pixel coordinates and 3D scene point coordinates. In 1998 [START_REF] Zhang | A flexible new technique for camera calibration[END_REF] introduced the use of a planar grid as calibration object. In order to facilitate the corner extraction process, the most used planar grid is a chessboard-like pattern (Figure 2.13).

This method requires several images of the chessboard taken from different position and orientation (Figure 2.12). It is important that the pattern covers the bigger portion possible of the camera field of view and the accuracy of the result increases with the number of images used.

The intrinsic and extrinsic camera parameters (including radial and tangential distortion) are identified by knowing the coordinates of the corners on the pattern and their corresponding pixel coordinates in each image and solving a least square minimization plus a nonlinear refinement. This method has been implemented in an open source Matlab toolbox [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF] and it is the method we used for our experiments.

Camera-IMU Calibration

For a robust fusion of the information provided from a monocular camera and an Inertial Measurement Unit, we need to know the relative orientation and translation between the reference frames associated to the two sensors (intersensor calibration).

There exist many methods to calibrate a camera-IMU system [START_REF] Kelly | Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration[END_REF] [71] offline or online [START_REF] Weiss | Vision based navigation for micro helicopters[END_REF]. To perform the Camera-IMU calibration we used the InerVis Toolbox for Matlab from [57].

In order to estimate the rotation between the two reference frames, the two sensors must observe the vertical direction (Figure 2

.14).

Once the camera is calibrated, it is possible to recover the extrinsic parameters (rotation and translation) with respect to a reference frame attached on the chessboard. When the system is static, the accelerometers senses only the gravity vector and they can therefore provide the gravity direction in the IMU reference frame. By using a chessboard, positioned in a way that the vertical lines of the chessboard are parallel to the gravity vector, it is possible to determine the vertical direction in the camera reference frame (orthogonal Procrustes method for 3D attitude estimation). The relative accelerometer and camera measurement are recorded from different static poses of the system. Being v I i and v C i the estimation of the vertical direction extracted from IMU and camera measurements respectively, the rotation between the two sensor corresponds to the quaternion q that maximises: This chapter introduces the first two contributions of this dissertation. In the first section we provide an overview of the data association problem related to the feature matching process. The feature detection, tracking and matching problems are introduced. In the second section we describe the data association problem and we give an overview about the state of the art. Two low computational complexity methods to perform the outlier detection task between two different views of a monocular camera rigidly attached to an inertial measurement unit are presented. The first one only requires the observation of a single feature in the scene and the knowledge of the angular rates provided by an inertial measurement unit, under the assumption that the local camera motion lies on a plane perpendicular to the gravity vector. In the second proposed algorithm we relax the hypothesis on the camera motion. The observation consists of two features in the scene (instead of only one) and of angular rates from inertial measurements. We show that if the camera is on-board a quadrotor vehicle, motion priors from 3. Data association inertial measurements can be used to discard wrong data association. Both the methods are evaluated on synthetic and real data.

n i=1 (q v I i q * ) • v C i . ( 2

Feature extraction and matching

Image features represent the informative content of a raw image. Feature extraction plays therefore an important role in the creation of compact and robust environmental models for map building and localization.

Features are repeatable and salient structures extracted from images and mathematically formalized, that characterize the environment. They are classified into low-level feature and high-level feature. The former are geometrical primitives like points, lines, corners, blobs, polygons while the latter are objects. Raw images contain an high amount of data, but a low level of distinctiveness. The feature extraction process performs an abstraction of the raw image reducing the volume of data, but augmenting the level of distinctiveness.

Once that the features are extracted in the first image, their relative correspondences must be identified in the consecutive images. There are two main approaches to select features and their correspondences:

• Extracting features in one image and using local search techniques to track them in the following images;

• Extracting features in all the images independently and match them according to the similarity of their descriptors.

Feature Detection

A local feature is an image pattern that can be distinguished from the neighbor pixels thanks to the difference in intensity, texture and color. In the framework of visual odometry, great importance is given to corners and blobs because of the precision with which they can be localized. A corner is defined as the intersection between two or more edges, while blobs are image patterns with distinctive intensity, texture and color. A good feature detector must have the following properties:

• Repetability: a large number of features should be detected in consecutive images;

• Localization accuracy: both in scale and position;

• Computational efficiency: it must be suitable for real time applications; • Robustness: to noise, blur, artifacts;

• Distinctiveness: in order to reduce wrong data associations;

• Invariance: to photometric changes (affine intensity) and geometric (2D rotation, scale, affine transformation) changes.

The choice of a feature detector is not only function of its properties but also of the environment, of the task, of the computational constraints and of the motion baseline. Corners are faster to compute than blobs, easier to localize in position but less distinctive, difficult to localize in scale, more difficult to redetect after large changes in viewpoint and scale, but they can be a good choice in urban and indoor environments.

A performance evaluation of feature detectors and descriptors can be found in [START_REF] Siegwart | Introduction to Autonomous Mobile Robots[END_REF], [70].

Feature Tracking

Detecting features in one image and tracking them by using local search techniques is suitable for small scale environments in which the motion baseline and the appearance deformation is small. The local search techniques mainly used for tracking features are [START_REF] Siegwart | Introduction to Autonomous Mobile Robots[END_REF]:

• Sum of Absolute Differences (SAD)
• Sum of Squared Differences (SSD)

• Normalized Cross Correlation (NCC)

Feature Matching

The feature matching task consists of searching the correspondences for features in different images.

The easiest way to search for correspondences is to compare the feature descriptors of all the features detected in the first image with the descriptors of all the features detected in the next image by using a similarity measurement. The features in the second image with the closer descriptors are selected as the correspondences. However it can happen that one feature in the first image is associated with more than one feature in the second image. To disambiguate the solution, a mutual consistency check is performed. Every feature in the second image is paired with features in the second image. The pairs showing consistent preferred match are chosen as image correspondences.

The main disadvantage of this approach is that its computational complexity is quadratic in the number of features. It is therefore not suitable for real-time applications if the number of features is high. A faster image matching technique consists in searching the correspondences not in the whole image, but in regions where the features are expected to be. Those regions can be computed by using a motion model (assuming constant velocity [START_REF] Andrew | Real-time simultaneous localisation and mapping with a single camera[END_REF]) or by using additional sensors like IMU, lasers, GPS or wheel odometry [62].

Outlier detection

The sets of feature correspondences are usually contaminated by outliers, i.e. wrong data associations. It is of outmost importance to remove them because they can negatively affect the accuracy of the esimated motion.

The standard method for model estimation from a set of data affected by outliers is RANSAC (RANdom SAmple Consensus) [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF]. It consists of randomly selecting a set of data points, computing the corresponding model hypothesis, and verifying this hypothesis on all the other data points. The solution is the hypothesis with the highest consensus. The number of iterations (N ) necessary to guarantee a robust outlier removal is [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF]:

N = log(1 -p) log(1 -(1 -ε) s ) (3.1)
where s is the number of data points from which the model can be computed, ε is the percentage of outliers in the dataset, p is the probability of success requested. Figure 3.3 shows the number of iterations (N ) with respect to the number of points necessary to estimate the model (s). The values are computed for p = 0.99 and = 0.5. Note that N is exponential in the number of data points s; this means that it is extremely important to look for minimal parametrizations of the model, in order to reduce the number of iterations, which is of utmost importance for vehicles equipped with a computationally-limited embedded computer.

Related works

When the camera is calibrated, its six degrees of freedom (DoF) motion can be inferred from a minimum of five-point correspondences, and the first solution to this problem was given in 1913 by Kruppa [START_REF] Kruppa | Zur ermittlung eines objektes aus zwei perspektiven mit inner orientierung[END_REF]. Several five-point minimal solvers were proposed later in [START_REF] Faugeras | Motion from point matches: multiplicity of solutions[END_REF], [START_REF] Philip | A non-iterative algorithm for determining all essential matrices corresponding to five point pairs[END_REF], [START_REF] Triggs | Routines for relative pose of two calibrated cameras from 5 points[END_REF], but an efficient implementation, based on [START_REF] Philip | A non-iterative algorithm for determining all essential matrices corresponding to five point pairs[END_REF], was found only in 2003 by Nister [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] and later revised in [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF]. Before that, the six- [START_REF] Pizarro | Relative pose estimation for instrumented, calibrated imaging platforms[END_REF], seven-or eight-solvers were commonly used. However, the five-point solver has the advantage that it works also for planar scenes. A more detailed analysis of the state of the art can be found in [START_REF] Scaramuzza | Visual odometry [tutorial[END_REF]. Despite the five-point algorithm represents the minimal solver for 5DoF motion of calibrated cameras, in the last few decades there have been several attempts to exploit different cues to reduce the number of motion parameters. In [START_REF] Tanskanen | A minimal case solution to the calibrated relative pose problem for the case of two unknown orientation angles[END_REF], the authors proposed a three-point minimal solver for the case of two known camera-orientation angles. For instance, this can be used when the camera is rigidly attached to a gravity sensor (in fact, the gravity vector fixes two cameraorientation angles). Later, the work in [START_REF] Naroditsky | Two efficient solutions for visual odometry using directional correspondence[END_REF] improved on [START_REF] Tanskanen | A minimal case solution to the calibrated relative pose problem for the case of two unknown orientation angles[END_REF] by showing that the three-point minimal solver can be used in a four-point (three-plus-one) RANSAC scheme. The three-plus-one stands for the fact that an additional far scene point (ideally, a point at infinity) is used to fix the two orientation angles. Using their four-point RANSAC, they also showed a successful 6 DoF VO. A two-point minimal solver for 6-DoF Visual Odometry was proposed in [START_REF] Kneip | Robust real-time visual odometry with a single camera and an imu[END_REF], which uses the full rotation matrix from an IMU rigidly attached to the camera. In the case of planar motion, the motion model complexity is reduced to 3 DoF and can be parameterized with two points as described in [START_REF] Ortin | Indoor robot motion based on monocular images[END_REF]. For wheeled vehicles, the work in [START_REF] Scaramuzza | Real-time monocular visual odometry for on-road vehicles with 1-point ransac[END_REF]82] showed that the motion can be locally described as planar and circular, and, therefore, the motion model complexity is reduced to 2 DoF, leading to a one-point minimal solver. Additionally, it was shown that, by using a simple histogram voting technique, outliers can be found in as little as a single iteration. A performance evaluation of five-, two-, and one-point RANSAC algorithms for Visual Odometry was finally presented in [START_REF] Scaramuzza | Performance evaluation of 1-point-ransac visual odometry[END_REF].

Epipolar Geometry

Before going on, we would like to recall some definitions about epipolar geometry. When a camera is calibrated, it is always possible to project the feature coordinates onto a unit sphere. This allows us to make our approach independent of the camera model.

Let p 1 = (x 1 , y 1 , z 1 ) and p 2 = (x 2 , y 2 , z 2 ) be the image coordinates of a point feature seen from two camera positions and back projected onto the unit sphere (i.e., p 1 = p 2 = 1) (Figure 3.4).

The image coordinates of point features relative to two different unknown camera positions must satisfy the epipolar constraint (Figure 3.4) [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

p 2 T Ep 1 = 0 (3.2)
where E is the essential matrix, defined as

E = [T] × R. R and T = [T x , T y , T z ] T
describe the relative rotation and translation between the two camera positions, and [T] X is the skew symmetric matrix:

[T] × =   0 -T z T y T z 0 -T x -T y T x 0   (3.3)
According to equation (3.2), the essential matrix can be computed given a set of image coordinate points. E can then be decomposed into R and T [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

The minimum number of feature correspondences needed to estimate the essential matrix is function of the degrees of freedom of the camera's motion. In the case of a monocular camera performing a 6DoF motion (three for the rotation and three for the translation), considered the impossibility to recover the scale factor, a minimum of five correspondences is needed.

1-point algorithm

In this section we propose a novel method to estimate the relative motion between two consecutive camera views, which only requires the observation of a single feature in the scene and the knowledge of the angular rates from an inertial measurement unit, under the assumption that the local camera motion lies in a plane perpendicular to the gravity vector [START_REF] Troiani | 1-point-based monocularmotion estimation for computationally-limited micro aerial vehicles[END_REF]. Using this 1-point motion parametrization, we provide two very efficient algorithms to remove the outliers of the feature-matching process. Thanks to their inherent efficiency, the proposed algorithms are very suitable for computationally-limited robots. We test the proposed approaches on both synthetic and real data, using video footage from a small flying quadrotor. We show that our methods outperform standard RANSAC-based implementations by up to two orders of magnitude in speed, while being able to identify the majority of the inliers.

Parametrization of the camera motion

We consider a micro aerial vehicle equipped with a monocular camera and an IMU. The transformation between the camera reference frame {C} and the vehicle's body frame {B} (that for aerial vehicles is coincident with the IMU frame) can be computed using [57]. Without loss of generality, we can assume that these two frames are coincident.

According to aerospace conventions [24], the X B -axis of an aerial vehicle commonly defines the forward direction, the Z B -axis is downward, and the Y B -axis follows the right-hand rule. We assume the same convention for our vehicle (Figure 3.5). We use the Z -Y -X Euler angles to model the rotation of the vehicle in the World frame. To go from the World frame to the Body frame, we first rotate about z W axis by the angle Y aw, then rotate about the intermediate y-axis by the angle P itch, and finally rotate about the X B -axis by the angle Roll.

We define as well a coordinate frame {C p } attached to the vehicle, with the same origin as the vehicle's Body Frame but with its z-axis aligned to the gravity vector (g). The Roll and P itch angles and the relative rotation about Z Cp -axis (dY aw) of the vehicle are provided by the IMU fusing the integration of the high frequency gyroscopic measurements with the gravity direction obtained by the accelerometers. If the system is in motion, the resulting estimation allows us to safely recover the short term relative orientation of the vehicle, that is only affected by a slowly changing drift term.

X Cp Z Cp X B Y B Z B {C p } {B} g Z W Y W X W {W} Y Cp
Considering that the camera is rigidly attached to the vehicle, two camera orientation angles are known (they correspond to the Roll and P itch angles provided by the IMU).

If R x (γ), R y (γ), R z (γ) are the orthonormal rotation matrices for rotation of γ about the x-, y-and z-axes, the matrices

Cp1 R B 1 = (R x (Roll 1 ) • R y (P itch 1 )) T Cp2 R B 2 = (R x (Roll 2 ) • R y (P itch 2 )) T (3.4)
allow us to virtually rotate the two camera frames into two new frames {C p 1 } and {C p 2 } (Figure 3.6). P itch i and Roll i , (i = 1, 2) are the angles provided by the IMU relative to two consecutive camera frames.

The two new image planes are parallel to the ground (z Cp 1 z Cp 2 g).

If the vehicle undergoes perfect planar motion, the essential matrix depends only on 2 parameters. Integrating the gyroscopic data within the time interval relative to two consecutive camera frames (i.e. the camera framerate), we can obtain the relative rotation of the two frames about Z Cp -axis. We define a third reference frame C p 0 , that corresponds to the reference frame C p 1 rotated according to dY aw, in order to have the same orientation of C p2 (Figure 3.6)). The matrix that describes this rotation is the following: To recap we can express the image coordinates into the new reference frames according to:

Cp0 R Cp1 = (R z (dY aw)) T (3.5)
p Cp 0 = Cp0 R Cp1 • Cp1 R B 1 • p 1 p Cp 2 = Cp2 R B 2 • p 2 (3.6)
At this point the transformation between {C p0 } and {C p2 } is a pure translation:

T = ρ[cos(α) -sin(α) 0] T R = I 3 (3.7)
and it depends only on α and on ρ (the scale factor). The essential matrix results therefore notably simplified:

E = [T] × R = ρ   0 0 -sin(α) 0 0 -cos(α) sin(α) cos(α) 0   (3.8)
At this point, being p Cp 0 = [x 0 y 0 z 0 ] T and p Cp 2 = [x 2 y 2 z 2 ] T , we impose the epipolar constraint according to (3.2) and we obtain the homogeneous equation that must be satisfied by all the point correspondences.

(x 0 z 2 -z 0 x 2 )sin(α) + (y 0 z 2 -z 0 y 2 )cos(α) = 0 (3.9)

where p 0 = [x 0 y 0 z 0 ] T and p 2 = [x 2 y 2 z 2 ] T are the directions (or unitsphere coordinates) of a matched feature in {C p0 } and {C p2 } respectively. Equation 3.9 depends only on one parameter (α). This means that the relative vehicle motion can be estimated using only a single image feature correspondence.

At this point we can recover the angle α from 3.9:

α = tan -1 z 0 y 2 -y 0 z 2 x 0 z 2 -z 0 x 2 (3.10)

1-point Ransac

One feature correspondence is randomly selected from the set of all the matched features. The motion hypothesis is computed according to (3.7). Without loss of generality we can set ρ = 1. Inliers are, by definition, the correspondences which satisfy the model hypothesis within a defined threshold. The number of inliers in each iteration is computed using the reprojection error. We used an error threshold of 0.5 pixels. The minimum number of iterations to guarantee a good outlier detection, considering p = 0.99 and ε = 0.5 is 7 (according to (3.1)).

Me-RE (Median + Reprojection Error)

The angle α is computed from all the feature correspondences according to (3.10).

A distribution {α i } with i = 1, 2, . . . , N f is obtained, where N f is the number of correspondences between the two consecutive camera images. The best angle α * is computed as the median of the afore-mentioned distribution α * = median{α i }.

The inliers are then detected by using the reprojection error. Unlike the 1point RANSAC, this algorithm is not iterative. Its computational complexity is linear in N f .

Performance evaluation

We evaluated the performance of the proposed approaches on both synthetic and real data. We compare our 1-point RANSAC and Me-RE methods with the 5point RANSAC [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] in simulations, and with the 5-point RANSAC [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] and the 8-point RANSAC [START_REF] Hc Longuet-Higgins | A computer algorithm for reconstructing a scene from two projections[END_REF] in experiments on real data.

Data association

Experiments on synthetic data We simulated different trajectories of a quadrotor moving in indoor scenarios (Figure 3.7). The simulations have been performed using the Robotics and Machine Vision Toolbox for Matlab [24].

To make our simulations as close as possible to the experiments, we simulated a quadrotor vehicle moving in indoor environment, equipped with a downlooking monocular camera. We randomly generated 1600 features on the ground plane (Figure 3.7). Note that no assumptions are made on the feature's depth.

We simulated a perspective camera with the same parameters of the one we used for the experiments and added a Gaussian noise with zero mean and standard deviation of 0.5 pixels to each image point. The vehicle was flying at the fix height of 2m above the ground. We generated a circular trajectory (easily repeatable in our flying arena) with a diameter of 1.5m. The period for one rotation is 10s. The camera framerate is 15Hz, its resolution is 752 x 480. For the 1-point RANSAC and the Me-RE, we set a threshold of 0.5 pixels. For the 5-point ransac we set a minimum number of trials of 145 iterations, and a threshold of 0.5 pixels as well.

In Figure 3.8 we present the results obtained along the aforementioned trajectory in the case of perfect planar motion (the helicopter is flying always at the same height above the ground, and the Roll and P itch angles are not affected by noise).

Figure 3.9 represents the results when the Roll and P itch angles are affected by a Gaussian Noise with standard deviation of 0.3 degrees.

We evaluated as well the case in which the measure of the angle dY aw is affected by a Gaussian Noise with standard deviation of 0.3 degrees. The relative results are shown in Figure 3. [START_REF] Armesto | Fast ego-motion estimation with multi-rate fusion of inertial and vision[END_REF] We finally evaluated the case of non perfect planar motion introducing a sinusoidal noise (frequency 4 rad/s and with amplitude of 0.02m) on the z Wcomponent of motion of the vehicle. Figure 3.11 represents the relative results.

We can observe that the Median + Reprojection Error (Me-RE) performs always better than the 1-point RANSAC, and requires no iterations (its computational complexity is linear in the number of features).

In the case of perfect planar motion (Figure 3.8), the Me-RE algorithm finds more inliers than the 5-point RANSAC. The latter algorithm requires at least 145 iterations according to Figure 3.3 to insure a good performance.

When the variables Roll, P itch and dY aw are affected by errors (Figures 3.9 and 3.10), the performance of our algorithms drops, but they can still find almost the 50% of inliers.

As expected, if the vehicle's motion is not perfectly planar (Figure 3.11), the performances of the 1-point RANSAC and the Me-RE get worse. The oscillations that we can see in the plots are related to the fact that when the vehicle is approaching the ground, less features are in the field of view of its on-board camera. Experiments on real data We tested our method on a nano quadrotor (Figure 3.12) [3] equipped with a MicroStrain 3DM-GX3 IMU (250 Hz) and a Matrix Vision mvBlueFOX-MLC200w camera (FOV: 112 deg).

The monocular camera has been calibrated using the Camera Calibration Toolbox for Matlab [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF]. The extrinsic calibration between the IMU and the camera has been performed using the Inertial Measurement Unit and Camera Calibration Toolbox [57]. The dataset was recorded in our flying arena and ground truth data have been recorded using an Optitrack motion capture system with submillimeter accuracy.

The trajectory has been generated using the TeleKyb Framework [START_REF] Grabe | The telekyb framework for a modular and extendible ros-based quadrotor control[END_REF] (Figure 3.13). The vehicle followed a circular trajectory (1.5m of diameter, period of 10s) with fixed height above the ground of 1.5m. We computed SURF features (Speeded Up Robust Feature). The feature detection and matching tasks has been performed using the Machine Vision Toolbox from [24].

To evaluate the performance of our methods, we compared the number of inliers found by the 1-point RANSAC and Me-RE methods with the number of inliers found by the 5-point RANSAC and the 8-point RANSAC methods. Figure 3.14 presents the result of this comparison.

We observe that in the interval [380 : 490] the Me-RE algorithm has a very good performance (it finds even more inliers than the 5-points RANSAC). On the contrary the performance drops in the intervals [350 : 380] and [490 : 540]. The last plot in Figure 3.15 shows the height of the vehicle above the ground during the trajectory. We can notice that in the interval [380 : 490] the motion of the vehicle along the z-World axis is smoother than in the other intervals, therefore it affects less the performance of the 1-point and of the Me-RE methods.

Figure 3.16 shows the computation time of the compared algorithms, implemented in Matlab and run on an Intel Core i7-3740QM Processor. According to our experiments, the 5-point RANSAC takes about 67 times longer than the 8-point. The reason of this is that for each candidate point set, the 5-point RANSAC returns up to ten motion solutions and this involves both Singular Value Decomposition (SVD) and Groebner-basis decompositions. Instead, the 8point RANSAC only returns 1 solution and has only one SVD, no Groebner-basis decomposition.

The Me-RE algorithm is not considered as a complete alternative to the 5point RANSAC. However, thanks to its negligible computation time (Figure 3.16), it can be run at each frame. If the resulting number of inliers will be below a defined threshold, it will be more suitable to switch to the 5-point algorithm.

Conclusions

In this section we presented two algorithms (1-point RANSAC and Median + Reprojection Error ) to perform outlier detection on computationally constrained micro aerial vehicles. The algorithms operate with the aid of an on-board IMU and assume that the vehicle's motion is locally planar. Both the algorithms rely on the reprojection error to look for inliers once that the essential matrix has been estimated, but the 1-point RANSAC needs at least 7 iterations to provide a satisfying solution, whereas the Me-RE's computational complexity is linear in the number of features and the performance is better. The Me-RE algorithm can therefore be a good replacement of the 5-point RANSAC when the motion of the vehicle is smooth and the camera framerate is high. The motion can then be refined applying standard methods [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF] to the remaining inliers. Considering that α * is estimated as the median of the distribution of the α computed from all the feature correspondences (3.10) , the standard deviation of this distribution can be considered as an index of reliability of the Me-RE algorithm.

2-point algorithm

In this section we present a novel method to perform the outlier rejection task between two different views of a camera rigidly attached to an Inertial Measurement Unit (IMU). Only two feature correspondences and gyroscopic data from IMU measurerments are used to compute the motion hypothesis [START_REF] Troiani | 2-point-based outlier rejection for camera-imu systems with applications to micro aerial vehicles[END_REF]. By exploiting this 2-point motion parametrization, we propose two algorithms to remove wrong data associations in the feature-matching process for case of a 6DoF motion. We show that in the case of a monocular camera mounted on a quadrotor vehicle, motion priors from IMU can be used to discard wrong estimations in the framework of a 2-point-RANSAC based approach. The proposed methods are evaluated on both synthetic and real data.

Parametrization of the camera motion

Let us consider a camera rigidly attached to an Inertial Measurement Unit (IMU) consisting of three orthogonal accelerometers and three orthogonal gyroscopes. The transformation between the camera reference frame {C} and the IMU frame {I} can be computed using [57]. Without loss of generality, we can therefore assume that these two frames are coincident ({I} ≡ {C}). The ∆φ, ∆θ and ∆ψ angles characterizing the relative rotation between two consecutive camera frames can be calculated by integrating the high frequency gyroscopic measurements, provided by the IMU. This measurement is affected only by a slowly-changing drift term and can safely be recovered if the system is in motion. If R x (∆), R y (∆), R z (∆) are the orthonormal rotation matrices for rotations of ∆ about the x-, y-and z-axes, the matrix

C 0 R C 1 = (R x (∆φ) • R y (∆θ) • R z (∆ψ)) T (3.11)
p 0 = C 0 R C 1 • p 1 .
(3.12)

At this point, the transformation between {C 0 } and {C 2 } is a pure translation

T = ρ[s(β) • c(α) -s(β) • s(α) c(β)] T R = I 3 , (3.13)
which depends only on the angles α and β and on the scale factor ρ. The essential matrix results therefore simplified:

E = [T] × R = ρ   0 -c(β) -s(β) • s(α) c(β) 0 -s(β) • c(α) s(β) • s(α) s(β) • c(α) 0   . (3.14)
With s(•) and c(•) we denote the sin(•) and cos(•) respectively. At this point, being p 0 = [x 0 y 0 z 0 ] T and p 2 = [x 2 y 2 z 2 ] T , the coordinates of a feature matched between two different camera frames and backprojected onto the unit sphere, we impose the epipolar constraint according to (3.2) and we obtain the homogeneous equation that must be satisfied by all the point correspondences.

x 2 (y 0 c(β)

+ z 0 s(α)s(β)) -y 2 (x 0 c(β) -z 0 c(α)s(β))+ -z 2 (y 0 c(α)s(β) + x 0 s(α)s(β)) = 0. (3.15)
Equation (3.15) depends on two parameters (α and β). This means that the relative vehicle motion can be estimated using only two image feature correspondences that we will identify as p A and p B , where p i j = [x i j y i j z i j ] T with i = A, B and j = 0, 2 indicate the direction of the feature i in the reference frame j.

At this point, we can recover the angles α and β solving (3.15) for the features p A and p B :

α = -tan -1 c 4 c 2 -c 1 c 5 c 4 c 3 -c 1 c 6 , β = -tan -1 c 1 c 2 c(α)+c 3 s(α) , (3.16)
where

c 1 = x A 2 y A 0 -x A 0 y A 2 , c 2 = -y A 0 z A 2 + y A 2 z A 0 , c 3 = -x A 0 z A 2 + x A 2 z A 0 , c 4 = x B 2 y B 0 -x B 0 y B 2 , c 5 = -y B 0 z B 2 + y B 2 z B 0 , c 6 = -x B 0 z B 2 + x B 2 z B 0 .
(3.17)

Finally, without loss of generality, we can set the scale factor ρ to 1 and estimate the essential matrix according to (3.14).

Hough

The angles α and β are computed according to (3.16) from all the feature pairs matched between two consecutive frames and distant from each other more than a defined threshold (see Section 3.2.4.5). A distribution {α i , β i } with i = 1, 2, . . . , N is obtained, where N is a function of the position of the features in the environment.

To estimate the best angles α * and β * , we build a Hough Space (Figure 3.18) which bins the values of {α i , β i } into a grid of equally spaced containers. Considering that the angle β is defined in the interval [0, π] and that the angle α is defined in the interval [0, 2π], we set 360 bins for the variable α and 180 bins for the variable β. The number of bins of the Hough Space encodes the resolution The angles α * and β * are therefore computed as

< α * , β * >= argmax{H},
where H is the Hough Space. The factors that influence the distribution are the error on the estimation of the relative rotation, the image noise, and the percentage of outliers in the data. The closer we are to ideal conditions (no noise on the IMU measurements), the narrower will be the distribution. The wider is the distribution, the more uncertain is the motion estimate.

To detect the outliers, we calculate the reprojection error relative to the estimated motion model.

The camera motion estimation can be then refined processing the remaining subset of inliers with standard algorithms [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

2-point Ransac

Using (3.13) we compute the motion hypothesis that consists of the translation vector T and the rotation matrix R = I 3 by randomly selecting two features from the correspondence set. To have a good estimation, we check that the distance between the selected features is below a defined threshold (see Section 3.2.4.5). If it is not the case, we randomly select another pair of features. Constraints on the motion of the camera can be exploited to discard wrong estimations. The inliers are then computed using the reprojection error. The hypothesis that shows the highest consensus is considered to be the solution.

Quadrotor motion model

We consider a quadrotor equipped with a monocular camera and an IMU.

The vehicle body-fixed coordinate frame {B} has its Z B -axis pointing downward (following aerospace conventions [24]). The X B -axis defines the forward direction and the Y B -axis follows the right-hand rule.

Without loss of generality we can consider the IMU reference frame {I} coinciding with the vehicle body frame {B}.

The modelization of the vehicle rotation in the World frame {W } follows the Z -Y -X Euler angles convention: being φ, θ, ψ respectively the Roll, P itch and Y aw angles of the vehicle, to go from the World frame to the Body frame, we first rotate about z W axis by the angle ψ, then rotate about the intermediate y-axis by the angle θ, and finally rotate about the X B -axis by the angle φ.

The transformation between the camera reference frame {C} and the IMU frame {I} can be computed using [57]. Without loss of generality, we can therefore assume that also these two frames are coincident ({I} ≡ {C} ≡ {B}).

A quadrotor has 6DoF, but its translational and angular velocity are strongly coupled to its attitude due to dynamic constraints. If we consider a coordinate frame {B 0 } with the origin coincident with the one of the vehicle's body frame {B} and the X B 0 and Y B 0 axes parallel to the ground, we observe that, in order to move in the X B 0 direction, the vehicle must rotate about the y-axes axis (P itch angle), while, in order to move in the Y B 0 direction, it must rotate about the x-axis (Roll angle) (Figure 3.20).

These motion constraints allow us to discard wrong estimations in a RANSAC based outlier detection approach. By looking at the relation between the x and y component of the estimated translation vector and the ∆φ, ∆θ angles provided by the IM U measurements (the same used in (3.11)), we are able to check the consistency of the motion hypothesis. If the estimated motion satisfies the condition ((|∆φ|

> )&(∆φ • T y > 0)) ((|∆θ| > )&(∆θ • T x < 0)) ((|∆φ| < )&(|∆θ| < )), (3.18)
we count the number of inliers (the number of correspondences that satisfy the motion hypothesis according to a predefined threshold) by using the reprojection error, otherwise we select another feature pair. The condition in (3.18) is satisfied if the x and y components of the motion hypothesis are coherent with the orientation of the vehicle. If both the angles ∆φ and ∆θ are below the threshold , we cannot infer nothing about the motion and we proceed in the evaluation of the model hypothesis using the reprojection error. The value of the threshold (see 3.2.4.5) is a function of the vehicle dynamics and of the controller used. Using (3.1) and considering p = 0.99 and ε = 0.5, we calculate the minimum number of iterations necessary to guarantee a good performance to our algorithm and we set it to 16.

Performance evaluation

To evaluate the performance of our algorithms, we run simulations and experiments on real data. We compared the proposed approaches with the 5-point RANSAC [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] on synthetic data, and with the 5-point RANSAC [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] and the 8-point RANSAC [START_REF] Hc Longuet-Higgins | A computer algorithm for reconstructing a scene from two projections[END_REF] on real data.

Experiments on synthetic data We built a synthetic scenario for our simulation by using the Robotics and Machine Vision Toolbox for Matlab [24]. We simulated a quadrotor equipped with a downlooking monocular camera and an IMU, moving in an indoor environment (Figure 3.21). Random features were generated without any assumption on the structure of the environment.

The on-board downlooking monocular camera was simulated as a perspective camera with the same intrinsic parameters of the camera that we used in the experiments. A white gaussian noise with a standard deviation of 0.5 pixels was added to each extracted feature.

We generated a trajectory consisting of a take-off and of a constant-height maneuver. The camera framerate is 15Hz, its resolution is 752 x 480. For the reprojection error in the 2-point RANSAC and in the Hough algorithm, we set a threshold of 0.5 pixels. For the 5-point RANSAC, we set the minimum number to trials to 145 iterations, and the threshold to 0.5 pixels for the reprojection error.

Figure 3.22 shows the results of a simulation run along the trajectory depicted in Figure 3.21, in the ideal case of no noisy IMU measurements. The helicopter takes off and performs a constant height maneuver.

In Figure 3.23, we present the results related to simulations where the quantities ∆φ, ∆θ and ∆ψ are affected by a Gaussian noise with standard deviation of 0.3 degrees. Those errors do not affect the performance of the 5-point algorithm (that does not use IMU readings to compute the motion hypothesis). In this case, the Hough and the 2-point RANSAC approaches can still detect more than half of the inliers. The motion hypothesis can then be computed on the obtained set of correspondences by using standard approaches [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

In Figure 3.24, we present the results related to simulations where the quantities ∆φ and ∆θ are affected by a Gaussian noise with standard deviation of 0.3 degrees and in Figure 3.25 only the angle ∆ψ is affected by a Gaussian noise with standard deviation of 0.3 degrees. These two plots show that errors on rotations about the camera optical axis (that in our case coincides with rotations about the vehicle Z B axis, i.e. errors on ∆ψ) affects more the performances of both the algorithms than errors on ∆φ and ∆θ.

Experiments on real data

The proposed approaches are tested on our nano quadrotor (Figure 3.12) [3] equipped with a MicroStrain 3DM-GX3 IMU (250 Hz) and a Matrix Vision mvBlueFOX-MLC200w camera (FOV: 112 deg and a resolution of 752 x 480).

The monocular camera calibration has been performed using the Camera Calibration Toolbox for Matlab [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF].

To extrinsically calibrate the IMU and the camera, we used the Inertial Measurement Unit and Camera Calibration Toolbox [57].

To validate the performance of our methods, we flew the quadrotor in our flying arena, equipped with an Optitrack motion capture system with submillimeter accuracy. The trajectory consisted of a take-off and a constant-height maneuver above the ground, as shown in Figure 3.26 and was generated using the TeleKyb Framework [START_REF] Grabe | The telekyb framework for a modular and extendible ros-based quadrotor control[END_REF]. We recorded a dataset composed of camera images, IMU measurements and ground truth data provided by the Optitrack.

We processed our dataset with SURF features, matching them in consecutive camera frames. We run the 8-point RANSAC method on each correspondences set to have an additional term of comparison. Figure 3.29 shows the error characterizing the estimated relative rotation be-tween two consecutive camera frames obtained by IMU measurements and the ground truth values. Looking at both Figure 3.27 and Figure 3.29, we can notice that the smaller are the errors on the angles estimations, the higher is the number of inliers detected by the Hough and the 2-point RANSAC method.

Our algorithms and the algorithms that we used for the comparison, are implemented in Matlab and run on an Intel Core i7-3740QM Processor. We summarize their computation time in Figure 3.28. We can notice that the computation time of the 5-point RANSAC is almost 67 times the computation time of the 8-point RANSAC. This is due to the fact that the 5-points returns up to 10 motion solutions for each candidate set. Singular Value Decomposition (SVD) and Groebner-basis decompositions are involved and this explains the high computation time.

The computation time of the Hough algorithm is function of the number of feature pairs used to compute the distribution in Figure (3.18). In our experiments, we choose all the feature pairs distant more than a defined threshold one to each other. We experimentally set this threshold to 30 degrees on the unit sphere.

Conclusions

In this section, we proposed two algorithms (Hough and 2-point RANSAC) to address the outlier rejection task systems equipped with a monocular camera rigidly attached to an IMU. We used a quadorotor micro aerial vehicle as platform to demonstrate the validity of our results. We show that the relations between the vehicle's translational and angular velocity and its attitude can be exploited in order to discard wrong estimations in the framework of a RANSAC-based approach.

Both methods rely on on-board IMU measurements to calculate the relative rotation between two consecutive camera frames and to the reprojection error to detect the inliers. The two algorithms differ in the way to compute the motion hypothesis.

The computation time of the Hough algorithm (Figure 3.28) is function of the number of feature pairs used to compute the distribution in Figure (3.18). Smart policies for the choice of the pairs of features to use (based for example on the feature positions in the image plane and not only on their relative position) can be used in order to reduce the computational complexity of the approach.

Experimental results show that the 2-point RANSAC algorithm can be a good replacement of the 5-point RANSAC. The motion hypothesis can always be refined by processing the found inliers with classic methods [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF].

Algorithm

Hough 2--points 5--points 8--points This chapter introduces the last two contributions of this dissertation. In the first section we provide an overview of the visual-aided inertial pose estimation problem, with an emphasis on aerial navigation. Two low computational complexity algorithms to face the pose estimation problem are presented. The first method requires inertial measurements from an IMU, and the observation of three features in the scene, under the planar ground assumption. It does not require any known pattern, but it exploits the geometrical constraints of a virtual one: three features form a triangle. The latter contribution faces the problem of featureless or dark environments. An aerial vechicle, moving in the surrounding of a planar surface is considered. In order to reduce the computational burden required to perform the feature extraction and matching task, a virtual feature is introduced by equipping the vehicle with a laser pointer (in addition to a monocular camera and an IMU). The laser spot produced by the laser pointer on the planar surface is the unique point feature observed. We identified the physical quantities that can be determined with this setup, we analytically derived the link between those quantities and the sensor data, and we estimated them with an Extended Kalman Filter.

Filtering approaches and closed form solutions

The problem of fusing vision and inertial data has been extensively investigated in the past. In [START_REF] Corke | An introduction to inertial and visual sensing[END_REF], a tutorial introduction to the vision and inertial sensing is presented. This work provides a biological point of view and it illustrates how vision and inertial sensors have useful complementarities allowing them to cover the respective limitations and deficiencies. Specifically, as it has been derived very recently in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF], the fusion of these sensors allows us to obtain the speed and the scale factor in closed form, allowing real time applications and robustness with respect to kidnapping. In [START_REF] Armesto | Fast ego-motion estimation with multi-rate fusion of inertial and vision[END_REF], inertial and visual sensors are used to perform egomotion estimation. The sensor fusion is obtained by an Extended Kalman Filter (EKF ) and by an Unscented Kalman Filter (U KF ). The approach proposed in [START_REF] Gemeiner | Simultaneous motion and structure estimation by fusion of inertial and vision data[END_REF] extends the previous one by also estimating the structure of the environment where the motion occurs. In particular, new landmarks are inserted on line into the estimated map. This approach has been validated by conducting experiments in a known environment where a ground truth was available. Also, in [97] an EKF has been adopted. In this case, the proposed algorithm estimates a state containing the robot speed, position and attitude, together with the inertial sensor biases and the location of the features of interest. In the framework of airbone SLAM, an EKF has been adopted in [START_REF] Kim | Real-time implementation of airborne inertialslam[END_REF] to perform 3D-SLAM by fusing inertial and vision measurements. It was observed that any inconsistent attitude update severely affects any SLAM solution. The authors proposed to separate attitude update from position and velocity update. Alternatively, they proposed to use additional velocity observations, such as air velocity observation. More recently, a vision based navigation approach in unknown and unstructured environments has been suggested [START_REF] Bloesch | Vision based mav navigation in unknown and unstructured environments[END_REF].

Recent works investigate the observability properties of the vision-aided inertial navigation system [START_REF] Jones | Inertial structure from motion with autocalibration[END_REF], [START_REF] Kelly | Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration[END_REF], [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF], [START_REF] Martinelli | Closed-form solution for attitude and speed determination by fusing monocular vision and inertial sensor measurements[END_REF], [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF], [START_REF] Martinelli | Observability properties and deterministic algorithms in visual-inertial structure from motion[END_REF] and [START_REF] Mourikis | A multi-state constraint Kalman filter for vision-aided inertial navigation[END_REF]. In particular, in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF], the observable modes are expressed in closed-form in terms of the sensor measurements acquired during a short time-interval.

Visual UAV pose estimation in GPS-denied environments is still challenging. Many implementations rely on visual markers, such as patterns or blobs, located in known positions [START_REF] Zhang | Autonomous hovering of a vision/imu guided quadrotor[END_REF], [START_REF] Eberli | Vision based position control for mavs using one single artificial landmark[END_REF], [START_REF] Cheviron | Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of uav[END_REF]. Those approaches have the drawback that can work only in structured environment. In [START_REF] Hwangbo | Visual-inertial uav attitude estimation using urban scene regularities[END_REF] Visual-Inertial Attitude Estimation is performed using image line segments for the correction of accumulated errors in integrated gyro rates when an unmanned aerial vehicle operates in urban areas. The approach will not work in environments that do not present a strong regularity in structure.

In [START_REF] Weiss | Vision based navigation for micro helicopters[END_REF], [START_REF] Weiss | Real-time onboard visual-inertial state estimation and self-calibration of mavs in unknown environments[END_REF] the authors developed a very robust Vision Based Navigation System for micro helicopters. Their pose estimator is based on a monocular VS-LAM framework (PTAM, Parallel Tracking and Mapping [START_REF] Klein | Parallel tracking and mapping for small AR workspaces[END_REF]). This software was originally developed for augmented reality and improved with respect to robustness and computational complexity. The resulting algorithm can be used in order to make a monocular camera a real-time on-board sensor for pose estimates. This allowed the first aerial vehicle that uses on-board monocular vision as a main sensor to navigate through an unknown GPS-denied environment and independently of any external artificial aids [START_REF] Weiss | Monocular-slam–based navigation for autonomous micro helicopters in gps-denied environments[END_REF], [START_REF] Weiss | Real-time onboard visual-inertial state estimation and self-calibration of mavs in unknown environments[END_REF].

Natraj et al. [START_REF] Natraj | Vision based attitude and altitude estimation for uavs in dark environments[END_REF] proposed a vision based approach, close to structured light, for roll, pitch and altitude estimation of UAV. They use a fisheye camera and a laser circle projector, assuming that the projected circle belongs to a planar surface. The latter must be orthogonal to the gravity vector in order to allow the estimation of the aforementioned quantities. The attitude estimation of the planar surface becomes crucial in order to extend the operational environment of UAVs. Shipboard operations, search and rescue cooperation between ground and aerial robots, low altitude manoeuvres, require to attenuate the position error and to track the platform attitude.

Virtual patterns

In this section we propose a new approach to perform MAV localization by only using the data provided by an Inertial Measurement Unit (IMU) and a monocular camera [START_REF] Troiani | 1-point-based monocularmotion estimation for computationally-limited micro aerial vehicles[END_REF]. The goal of our investigation is to find a new pose estimator which minimizes the computational complexity. We focus our attention on the problem of relative localization, which makes possible the accomplishment of many important tasks (e.g. hovering, autonomous take off and landing). In this sense, we minimize the number of point features which are necessary to perform localization. While 2 point features is the minimum number which provides full observability, by adding an additional feature, the precision is significantly im-proved, provided that the so-called planar ground assumption is honoured. This assumption has recently been exploited on visual odometry with a bundle adjustment based method [START_REF] Kitt | Monocular visual odometry using a planar road model to solve scale ambiguity[END_REF]. The proposed method does not use any known pattern but only relies on three natural point features belonging to the same horizontal plane, which form therefore a virtual pattern (a triangle). It is based on a closed solution which provides the vehicle pose from a single camera image, once the roll and the pitch angles are obtained by the inertial measurements. The first step of the approach provides a first estimate of the roll and pitch (through the IMU data) and then the vehicle heading by only using two of the three point features and a single camera image. In particular, the heading is defined as the angle between the MAV and the segment made by the two considered point features. Then, the same procedure is repeated two additional times, i.e., by using the other two pairs of the three point features. In this way, three different heading angles are evaluated. On the other hand, these heading angles must satisfy two geometrical constraints, which are fixed by the angles characterizing the triangle made by the three point features. These angles are estimated in parallel by an independent Kalman Filter. The information contained in the geometrical constraints is then exploited by minimizing a suitable cost function. This minimization provides a new and very precise estimate of the roll and pitch and consequently of the yaw and the vehicle position.

The System

Let us consider an aerial vehicle equipped with a monocular camera and IMU sensors. We assume that the transformation among the camera frame and the IMU frame is known (we can assume that the vehicle frame coincides with the camera frame).

We assume that three reliable point-features are detected on the ground (i.e. they belong to the same horizontal plane). As we will see, two is the minimum number of features necessary to perform localization. Figure 4.1 shows our global frame G, which is defined by only using two features, P 1 and P 2 . First, we define P 1 as the origin of the frame. The z G -axis coincides with the gravity vector but with opposite direction. Finally, P 2 defines the x G -axis1 .

Then, by applying the method in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF], the distance between these point features can be roughly determined by only using visual and inertial data (specifically, at least three consecutive images containing these points must be acquired).

The method

The first step of the method consists in estimating the Roll and the Pitch angles. This is performed by an Extended Kalman Filter (EKF) which estimates the gravity in the local frame by only using inertial data. Once the direction of the gravity vector in the local frame is estimated, the Roll and Pitch angles are obtained. The second step returns the Yaw angle and the position of the vehicle taking as input the Roll and Pitch angles and a single camera image (3palgorithm, Section 4.2.2.2). The core of the 3p-algorithm is the 2p-algorithm, which is described in the next section.

2p-Algorithm

This algorithm needs only two point features in a single camera image, and the Roll and Pitch angles estimated from IMU measurements. Figure 4.2 represents a schematic of the algorithm.

For each feature, the camera provides its position in the local frame up to a scale factor. The knowledge of the absolute Roll and Pitch, allows us to express the position of the features in a new vehicle frame N , which Z N -axis is parallel to the gravity vector. Let us denote with [x 1 , y 1 , z 1 ] T and [x 2 , y 2 , z 2 ] T the coordinates of P 1 and P 2 in the new local frame. The camera provides

µ 1 = x 1 z 1 , ν 1 = y 1 z 1 , µ 2 = x 2 z 2 and ν 2 = y 2 z 2 .
Additionally, the camera also provides the sign of z 1 and z 2 1 . Since the Z N -axis has the same orientation as the z G -axis, and the two features P 1 and P 2 belongs to a plane perpendicular to the gravity vector, z 1 = z 2 = -z, where z is the position of the origin of the vehicle frame in the global frame. We Note that the position x, y, z is obtained in function of the distance D. Specifically, the position scales linearly with D. As previously said, a rough knowledge of this distance is provided by using the method in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF] and described in section 4.2.2.3. We remark that a precise knowledge of this distance is not required to accomplish tasks like hovering on a stable position.

3p-Algorithm

The three features form a triangle in the (x G , y G )-plane. For the sake of clarity, we start our analysis supposing that we know the angles characterizing the triangle (γ 1 and γ 2 in Figure 4.5). Then, we will show how we estimate on line these angles (Section 4.2.2.4). We run the 2p-algorithm three times, respectively with the sets of features (P 1 , P 2 ), (P 1 , P 3 ) and (P 2 , P 3 ) as input. We obtain three different angles. Y aw 12 is the Yaw of the vehicle in the global frame given in (4. The three above-mentioned angles must satisfy the following constraints:

γ 1 = Y aw 13 -Y aw 12 γ 2 = Y aw 23 -Y aw 12 (4.6)
Let us denote the known values of these angles with γ 0 1 and γ 0 2 . We correct the estimation of the roll and pitch angles by exploiting these constraints. We solved a nonlinear least-squares problem minimizing the following cost function:

c(ζ) = [(Y aw 13 -Y aw 12 -γ 0 1 ) 2 + (Y aw 23 -Y aw 12 -γ 0 2 ) 2 ] (4.

IMU EKF

Roll, Pitch

Scale factor initialization

Recent works on visual-inertial structure from motion have demonstrated its observability properties [START_REF] Jones | Inertial structure from motion with autocalibration[END_REF], [START_REF] Kelly | Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration[END_REF], [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF], [START_REF] Martinelli | Closed-form solution for attitude and speed determination by fusing monocular vision and inertial sensor measurements[END_REF], [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF], [68], [START_REF] Martinelli | Observability properties and deterministic algorithms in visual-inertial structure from motion[END_REF], and [START_REF] Mourikis | A multi-state constraint Kalman filter for vision-aided inertial navigation[END_REF]. It has been proved that the states that can be determined by fusing inertial and visual information are: the system velocity, the absolute scale, the gravity vector in the local frame, and the biases that affects the inertial measurements. The work in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale and bias determination[END_REF] expresses all the observable modes at a given time T in in closed-form and only in function of the visual and inertial measurements registered during the time interval [T in , T f in ].

In the following, we will adopt lower-case letters to denote vectors in the global frame (e.g. the gravity is g = [0, 0, -g] T , where g 9.8ms -1 ). Lower-case letters will be used to denote vectors in the camera frame. Since this local frame is time dependent, we adopt the following notation: W t (τ ) will be the vector with global coordinates w(τ ) in the local frame at time t. Additionally, we will denote with C t 2 t 1 the matrix which characterizes the rotation occurred during the time interval (t 1 , t 2 ). We have:

W t 1 (τ ) = C t 2 t 1 W t 2 (τ ) and (C t 2 t 1 ) -1 = C t 1 t 2 .
Finally, C t will denote the rotation matrix between the global frame and the local frame at time t, i.e., w(τ

) = C t W t (τ ).
The position r of the system is:

r(t) = r(T in ) + v(T in )∆t + t T in τ T in a(ξ)dξdτ (4.8) where t ∈ [T in , T f in ].
Integrating by part we obtain:

r(t) = r(T in ) + v(T in )∆t + t T in (t -τ )a(τ )dτ (4.9)
where v ≡ dr dt , a ≡ dv dt and ∆t ≡ t -T in . The accelerometers provide the acceleration in the local frame and they also perceive the gravitational acceleration. Their measurements are also corrupted by a constant term (B) usually called bias. We can therefore write the accelerometer measurement like this:

A τ (τ ) ≡ A i τ (τ ) -G τ + B (4.10)
where A i τ (τ ) is the inertial acceleration and G τ is the gravity acceleration in the local frame (depending on time because the local frame can rotate). Rewriting equation (4.9) by highlighting the vector A τ (τ ) provided by the accelerometer and neglecting the bias term B:

r(t) = r(T in ) + v(T in )∆t + g ∆t 2 2 + C T in [S T in (t)] (4.11)
where:

S T in (t) ≡ t T in (t -τ )C τ T in A τ (τ )dτ ;
The matrix C τ T in can be obtained from the angular speed during the interval [T in , τ ] provided by the gyroscopes [START_REF] Farrell | Aided navigation: GPS with high rate sensors[END_REF].The vector S T in (t) can be obtained by integrating the data provided by the gyroscopes and the accelerometers delivered during the interval [T in , t].

The visual measurements related to the observation of N point-features are recorded simultaneously with the inertial measurements. Let us denote the feature position in the physical world with p i , i = 1, ..., N . P i t (t) denotes their position at time t in the local frame at time t. We have:

p i = r(t) + C T in C t T in P i t (t) (4.12)
Writing this equation for t = T in we obtain:

p i -r(T in ) = C T in P i T in (T in ) (4.13)
By inserting the expression of r(t) provided in (4.11) into equation (4.12), by using (4.13) and by pre multiplying by the rotation matrix (C T in ) -1 , we obtain:

C t T in P i t (t) = P i T in (T in ) -V T in (T in )∆t -G T in ∆t 2 2 -S T in (t) (4.14) i = 1, 2, ..., N
A single image processed at time t, provides the position of the N features up to a scale factor, which correspond to the the vectors P i t (t). The data provided by the gyroscopes during the interval (T in , T f in ) allow us to build the matrix C t T in . At this point, having the vectors P i t (t) up to a scale, allows us to also know the vectors C t T in P i t (t) up to a scale. We assume that the camera provides n i images of the same N point-features at consecutive image stamps: t 1 = T in < t 2 < ... < t n i = T f in . For the sake of simplicity, we adopt the following notation:

• P i j ≡ C t j
T in P i t j (t j ), i = 1, 2, ..., N ; j = 1, 2, ..., n i

• P i ≡ P i T in (T in ), i = 1, 2, ..., N • V ≡ V T in (T in ) • G ≡ G T in • S j ≡ S T in (t j ), j = 1, 2, ..., n i
The vectors P i j can be written as P i j == λ i j µ i j . Without loss of generality we can set T in = 0. Equation (4.14) can be written as follows:

P i -V t j -G t 2 j 2 -λ i j µ i j = S j (4.15)
The corresponding linear system is:

-G t 2 j 2 -V t j + λ 1 1 µ 1 1 -λ 1 j µ 1 j = S j λ 1 1 µ 1 1 -λ 1 j µ 1 j -λ i 1 µ i 1 + λ i j µ i j = 0 3 (4.16)
where j = 2, ..., n i , i = 2, ..., N and 0 3 is the 3 × 1 zero vector. This linear system consists of 3(n i -1)N equations in N n i + 6 unknowns. The two column vectors X and S and the matrix Ξ are defined as follows:

X ≡ [G T , V T , λ 1 1 , ..., λ N 1 , ..., λ 1 n i , ..., λ N n i ] T S ≡ [S T 2 , 0 3 , ..., 0 3 , S T 3 , 0 3 , ..., 0 3 , ..., S T n i , 0 3 , ..., 0 3 ] T Ξ ≡ (4.17)                 T 2 S 2 µ 1 1 0 3 0 3 -µ 1 2 0 3 0 3 0 3 0 3 0 3 0 33 0 33 µ 1 1 -µ 2 1 0 3 -µ 1 2 µ 2 2 0 3 0 3 0 3 0 3 ... ... ... ... ... ... ... ... ... ... ... 0 33 0 33 µ 1 1 0 3 -µ N 1 -µ 1 2 0 3 µ N 2 0 3 0 3 0 3 ... .
0 3 0 3 0 3 0 3 0 3 -µ 1 n i 0 3 0 3 0 33 0 33 µ 1 1 -µ 2 1 0 3 0 3 0 3 0 3 -µ 1 n i µ 2 n i 0 3 ... ... ... ... ... ... ... ... ... ... ... 0 33 0 33 µ 1 1 0 3 -µ N 1 0 3 0 3 0 3 -µ 1 n i 0 3 µ N n i                
where T j ≡ -t 2 j 2 I 3 , S j ≡ -t j I 3 and I 3 is the identity 3 × 3 matrix; 0 33 is the 3×3 zero matrix. The linear system in (4.16) can be written in a compact format:

ΞX = S (4.18)
The linear system in 4.18 contains completely the sensor information. By adding the following equation to the system:

|ΠX| 2 = g 2 (4.19)
where Π ≡ [I 3 , 0 3 ... 0 3 ], it is possible to exploit the information related to the fact that the magnitude of the gravitational acceleration is known. The Visual-Inertial Structure from Motion problem consists in the determination of the vectors: P i , (i = 1, 2, ..., N ), V , G and it can be solved by finding the vector X, which satisfies (4.18) and (4.19). The scale factors are the quantities λ i j for i = 1, 2, ..., N , j = 1, 2, ..., n i contained in the state vector X.

In our case to initialize the scale factor we need at least three consecutive images containing the two points P 1 and P 2 . This is enough considering that we know the gravity magnitude and that we know in advance we will not occur in degenerative cases (none of the camera poses will be aligned along with the two features, and the three camera poses and the two features will not belong to the same plane) [START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF].

Estimation of γ 1 and γ 2

In order to estimate the angles characterizing the triangle γ 1 and γ 2 (Figure 4.5), we run a Kalman filter. The state that we want to estimate is Γ = [γ 1 , γ 2 ] T . During the prediction step the filter does not update neither the state Γ nor its covariance matrix because the angles are constant in time. For the observation step we need the estimated Roll and Pitch (which allow us to virtually rotate the vehicle frame V into the the new frame N ) and the observations of the three features in the current camera image [x i , y i , z] T = z[µ i , ν i , 1] T for i = 1, 2, 3. At this point the sides of the triangle can be computed according to: a = z ∆µ 2 12 + ∆ν 2 12 , b = z ∆µ 2 13 + ∆ν 2 13 , c = z ∆µ 2 23 + ∆ν 2 23 . Applying the law of cosine we can easily compute the two required angles:

γ 1 = acos a 2 +b 2 -c 2 2ab γ 2 = π -acos a 2 +c 2 -b 2 2ac
Note that these angles are independent from z. γ 1 and γ 2 represent the observation of the state Γ of the Kalman Filter.

Performance Evaluation

Simulations

In order to evaluate the performance of the presented method, we simulated different 3D trajectories and scenarios.

The considered scenarios to test the 2p-Algorithm is shown in Figure 4.1. The features are P 1 = [0, 0, 0], P 2 = D * [1, 0, 0], where D = 0.1m. To compare the 2p-Algorithm with the 3p-Algorithm, we added a third feature P 3 = D * [0.5, √ 3/2, 0] (Figure 4.5). The angles γ 1 and γ 2 are respectively 60deg and 120deg.

The trajectories are generated with a quadrotor simulator that, given the initial conditions, the desired position and desired Yaw, performs a hovering task [START_REF] Castillo | Modelling and Control of Mini-Flying Machines[END_REF]. The initial vehicle position is x = y = z = 0 m, the initial vehicle speed is v x = v y = v z = 0 ms -1 in the global frame.

Starting from the performed trajectory, the true angular speed and the linear acceleration are computed each 0.01s We denote with Ω true i and A true v i the true value of the body rates and linear accelerations at time stamp i. The IMU readings are generated as following:

Ω i = N Ω true i -Ω bias , P Ω i and A i = N A true v i -A g -A bias , P A i where:
• N indicates the Normal distribution whose first entry is the mean value and the second one is the covariance matrix;

• P Ω i and P A i are the covariance matrices characterizing the accuracy of the IM U ;

• A g is the gravitational acceleration in the local frame and A bias is the bias affecting the accelerometer's data;

• Ω bias is the bias affecting the gyroscope's data.

In all the simulations we set both the matrices P Ω i and P A i diagonal and in particular:

P Ω i = σ 2
gyro I 3 and P A i = σ 2 acc I 3 , where I 3 is the identity 3 × 3 matrix. We considered several values for σ gyro and σ acc , in particular: σ gyro = 1 deg s -1 and σ acc = 0.01 ms -2 .

The camera is simulated as follows. Knowing the true trajectory of the vehicle, and the position of the features in the global frame, the true bearing angles of the features in the camera frame are computed each 0.3s. Then, the camera readings are generated by adding zero-mean Gaussian errors (whose variance is set to (1 deg) 2 ) to the true values. Figure 4.8 summarizes these results by providing the mean error on the estimated position and attitude.

Experimental Results

This section describes our experimental results. The robot platform is a Pelican from Ascending Technologies [1] equipped with an Intel Atom processor board (1.6 GHz, 1 GB RAM ) (Figure 4.9).

Our sensor suite consists of an Inertial Measurement Unit (3-Axis Gyro, 3-Axis Accelerometer ) belonging to the Flight Control Unit (FCU) AscTec Autopilot , and a monocular camera (Matrix Vision mvBlueFOX, F OV : 130 deg). The camera is calibrated using the Camera Calibration Toolbox for Matlab [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF]. The calibration between the IMU and the camera has been performed using the Inertial Measurement Unit and Camera Calibration Toolbox in [57]. The IMU provides measurements update at a rate of 100Hz, while the camera framerate is 10Hz.

The Low Level Processor (LLP) of our Pelican is flashed with the 2012 LLP Firmware [1] and performs attitude data fusion and attitude control. We flashed the High Level Processor (HLP) with the asctec hl firmware [START_REF] Achtelik | Onboard imu and monocular vision based control for mavs in unknown in-and outdoor environments[END_REF]. The on-board computer runs linux 10.04 and ROS (Robot Operating System). We implemented our method using ROS as a middleware for communication and monitoring . The HLP communicates with the on-board computer through a FCU-ROS node. The communication between the camera and the on-board computer is achieved by a ROS node as well. The presented algorithms are running online and on-board at 10Hz. The scenario setup is shown in Figure 4.11. We used an ARToolKit Marker with the only aim of having a ground truth to evaluate the performance of our approach. The estimation of the camera pose provided by the marker is not used to perform the estimation. The marker is positioned such that it's reference frame is coincident with the configuration shown in Figure 4.5. The three features considered are the center of the three little balls in Figure 4.11. The use of three blob markers instead of natural features is only related to the need to get a ground truth. The information related to the pattern composed by the 3 features (D = 0.25m, γ 1 = 60deg, γ2 = 120deg) is only used to evaluate the performance of our approach. The algorithm does not require any information about the Figures 4.12.a and 4.12.b show respectively the position and the attitude estimated by using the proposed approach and compared with the ground truth obtained with the ARToolkit marker. From Figure 4.12.a we see that the difference between our estimates and the ground truth values is of the order of 2cm for x and y and less than 0.5cm for z. From Figure 4.12.b we see that the difference between our estimates and the ground truth values is of the order of 2deg for P itch and less than 0.5deg for Roll and Y aw.

We believe that the main source of error is due to the distortion of the lens, which is not fully compensated by the calibration. Note that this distortion also affects our ground truth. We plan to test our approach in an environment equipped with a Motion Capture System.

Conclusion

In this section we proposed a new approach to perform MAV localization by only using the data provided by an Inertial Measurement Unit and a monocular camera. The approach exploits the so-called planar ground assumption and only needs three natural point features. It is based on a closed solution which provides the vehicle pose from a single camera image, once the roll and the pitch angles are obtained by the inertial measurements. This makes the approach very simple in terms of computational complexity and robust since the closed form solution makes unnecessary any initialization. We evaluated the performance of the proposed approach by using both synthetic and real data. We also described the results obtained by implementing the approach on our quadrotor in real-time and on-board. The very low computational cost of the proposed approach makes it suitable for pose control in tasks like hovering, autonomous take off and landing.

Virtual features

In this section we consider a vehicle which accomplishes a 3D-trajectory in the surrounding of a planar surface. The vehicle is equipped with a monocular camera and inertial sensors. This is the typical navigation problem in an indoor environment or in a city-like environment. All the approaches previously mentioned, require to extract natural features from the images provided by the camera and in particular to detect the same features in different images. The feature matching task becomes critical in outdoor environment because of possible illumination changes. In order to significantly reduce the computational burden required to perform these tasks and to make the feature matching more robust, we introduce a virtual feature by equipping our vehicle with a laser pointer [START_REF] Troiani | Vision-aided inertial navigation using virtual features[END_REF]. The laser beam produces a laser spot on the planar surface. This laser spot is observed by the monocular camera and it is the unique point feature used by the proposed approach.

To the best of our knowledge, this problem has never been considered so far. Compared to classical vision and IMU data fusion problems, the feature is moving in the environment but we exploit the hypothesis that it moves on a planar surface. The first question which arises is to understand which are the observable modes, i.e. the physical quantities that can be determined by only using the inertial data and the camera observation of the laser spot during a short time-interval. The results provided in Section 4.3.3 address precisely this issue. Then, the second step we consider is to analytically determine the link between the observable modes and the sensor data. This is obtained by performing a local decomposition of the original system (Section 4.3.4). This decomposition separates the observable modes from the rest of the system and will allow us to introduce a simple recursive method to perform the estimation of all the observable modes (Section 4.3.4). The method is validated by using synthetic data (Section 4.3.5). Preliminary tests with real data are also provided and more complete experiments are in progress.

The System

Let us consider an aerial vehicle equipped with a monocular camera and IM U sensors. The vehicle is also equipped with a laser pointer. The configuration of the laser pointer in the camera reference frame is known. The vehicle moves in the surrounding of a planar surface and we assume that the laser spot produced by the laser beam belongs to this planar surface (see fig. 4.13). The position and the orientation of this planar surface are unknown. The camera observations consist in the position of the laser spot in the camera frame up to a scale factor. The IMU consists of three orthogonal accelerometers and three orthogonal gyroscopes. We assume that the monocular camera is intrinsically calibrated and that the transformations among the camera frame and the IM U frames are known (we can assume that the vehicle frame coincides with the camera frame). The IM U provides the vehicle angular speed and acceleration. Actually, regarding the acceleration, the one perceived by the accelerometer (A) is not simply the vehicle acceleration (A v ). It also contains the gravity acceleration (G). In particular, we have A = A v -G since, when the camera does not accelerate (i.e. A v is zero) the accelerometer perceives an acceleration which is the same of an object accelerated upward in the absence of gravity.

We will use uppercase letters when the vectors are expressed in the local frame and lowercase letters when they are expressed in the global frame. Hence, regarding the gravity we have: g = [0, 0, -g] T , being g 9.8 ms -2 .

Finally, we will adopt a quaternion to represent the vehicle orientation. Indeed, even if this representation is redundant, it is very powerful since the dynamics can be expressed in a very easy and compact notation [START_REF] Jack | Quaternions and rotation sequences : a primer with applications to orbits, aerospace, and virtual reality[END_REF].

Our system is characterized by the state [r, v, q] T where r = [r x , r y , r z ] T is the 3D vehicle position, v is its time derivative, i.e. the vehicle speed in the global frame (v ≡ dr dt ), q = q t + iq x + jq y + kq z is a unitary quaternion (i.e. satisfying q 2 t + q 2 x + q 2 y + q 2 z = 1) and characterizes the vehicle orientation. The analytical expression of the dynamics and the camera observations can be easily provided by expressing all the 3D vectors as imaginary quaternions. In practice, given a 3D vector w = [w x , w y , w z ] T we associate with it the imaginary quaternion w q ≡ 0 + iw x + jw y + kw z . The dynamics of the state [r q , v q , q] T are:

       ṙq = v q vq = qA vq q * = qA q q * + g q q = 1 2 qΩ q (4.20)
being q * the conjugate of q, q * = q t -iq x -jq y -kq z and Ω the angular velocity.

We derive the expression of the camera observation consisting in the position of the laser spot in the camera frame up to a scale factor. The laser spot is on a planar surface whose configuration is unknown. Without loss of generality, we choose the camera frame with the z-axis parallel to the laser pointer (see figure 4.14). In addition, the camera frame is such that the laser beam intercept the xy-plane in [L, 0, 0]. In 4.3.2 we introduce a simple and efficient method to determine the parameter L together with the rotation to transform vectors from the original camera frame into the chosen camera frame.

Finally, we characterize the planar surface in the global frame with the equation z = ky, where k is an unknown parameter.

In these settings, by carrying out analytical computation (which uses the basic quaternion rules) we obtain the analytical expression of the position [X s , Y s , Z s ] of the laser spot in the camera reference frame. We have:

         X s = L Y s = 0 Z s =
r z + 2q z q x L -2q y kq x L -2q y q t L -2q t Lkq z -kr y 2kq z q y -2kq t q x -q 2 z -q 2 t + q 2 y + q 2 x (4.21)

The camera provides the vector [X s , Y s , Z s ] up to a scale factor. This is equivalent to the two ratios Xs Zs and Ys Zs . Hence, since the latter is identically zero, the camera observation is given by h cam = Xs Zs , which is:

h cam =
L(2kq z q y -2kq t q x -q 2 z -q 2 t + q 2 y + q 2 x ) r z + 2q z q x L -2q y kq x L -2q y q t L -2q t Lkq z -kr y (4.22)

Camera-laser module calibration

In figure 4.14, we display the position and the direction of the laser pointer in the original camera frame. The calibration consists in estimating the four parameters L x , L y , θ, φ. In other words, it consists in estimating the line made by the laser beam in the original camera frame. This line is determined starting from the position of the laser spot in the original camera frame for at least two spots.

To have an accurate estimate, the two spots must be as far as possible one each other. The precision can be further improved by considering more than two spots (Figure 4.15b) and by finding the best line fit. In order to have the Cartesian coordinates of a single spot in the original camera frame, it suffices to project the spot on a checkerboard (Figure 4.15a). By using the Camera Calibration Toolbox for Matlab [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF], it is possible to get the equation of the plane containing the checkerboard in the original camera frame and, known the direction of the spot from the camera measurement, the 3D position is finally obtained. The chosen camera frame is obtained by rotating the original frame such that in the new frame the z-axis has the same orientation of the laser beam. Additionally, we also require that the laser beam intersects the new x -axis. In other words, we require that the laser beam intersects the new xy-plane in the point [L, 0, 0] T , for a given L. We want to obtain the quaternion q which characterizes this rotation. This will allow us to express the vectors provided by the camera in the chosen frame. Note that, since the two frames only differ by a rotation (i.e., they share the same origin), we are allowed to express the vectors provided by the camera in the new frame, even if these vectors are defined up to a scale. Finally, in this section we want to determine the value of L. As we will see, both the quaternion q and the parameter L only depend on the calibration parameters: L x , L y , θ, φ.

We start by rotating the original frame of φ about its z-axis. The quaternion characterizing this rotation is q z-axis (φ) = cos φ 2 + k sin φ 2 . Then, we rotate the frame obtained with this rotation of θ about its y-axis. The quaternion characterizing this rotation is q y-axis (θ) = cos θ 2 +j sin θ 2 . Hence, the previous two rotations are characterized by the quaternion q zy ≡ q z-axis (φ)q y-axis (θ). The obtained frame has the z-axis aligned with the laser beam. On the other hand, the laser beam does not intersect necessarily the x-axis. To obtain this, we have to rotate again the frame about its current z-axis. Let us compute the intersection of the laser beam with the xy-plane. We compute this point in the original frame. By a direct computation we obtain:

r inters = [L x -τ 2 x L x - τ x τ y L y , L y -τ x τ y L x -τ 2
y L y , -τ x τ z L x -τ y τ z L y ] T , where τ x = sin (θ) cos (φ), τ y = sin (θ) sin (φ), τ z = cos (θ). We then compute this vector in the rotated frame by doing the quaternion product: R inters q = q * zy r inters q q zy . Let us denote R inters with [L x , L y , 0] T . We finally have: L = L 2

x + L 2 y and q = q zy q z-axis (α), where q z-axis (α) = cos α 2 + k sin α 2 and α = arctan

L y L x .

Observability Properties

We investigate the observability properties of the system whose dynamics are given in (4.20) and whose observation function is given in (4.22). We have also to consider the constraint q * q = 1. This can be dealt as a further observation (system output):

h const (r q , v q , q) = q * q (4.23)

Finally, we want to investigate whether the parameter k is identifiable or not. This is done by performing an observability analysis on the extended state S = [r q , v q , q, k] T , whose dynamics are given in (4.20) and by the additional equation k = 0. We apply the method introduced in [START_REF] Martinelli | State estimation based on the concept of continuous symmetry and observability analysis: The case of calibration[END_REF]. This will allow us to detect all the observable modes, i.e. all the physical quantities that can be determined by only using the information contained in the data provided by the IMU and the camera in a given time-interval.

The system is characterized by the state: S = [r q v q q, k] T , whose dimension is 11 (r q and v q are imaginary quaternions, i.e. they are characterized by 3 real numbers; q contains 4 real numbers; k is a real number). The dynamics are given in (4.20) together with the equation k = 0 and the observations are given in (4.22) and (4.23). To compute the Lie derivatives, we need to express the dynamics as follows:

Ṡ = f (S, u) = f 0 (S) + L i=1 f i (S)u i (4.24)
We have L = 6 and the six inputs are the three components of the acceleration, A, and the three components of the angular speed, Ω. Namely:

u 1 = A x , u 2 = A y , u 3 = A z , u 4 = Ω x , u 5 = Ω y , u 6 = Ω z .
The seven vector functions f 0 , f 1 , ..., f 6 are:

f 0 = [v x , v y , v z , 0, 0, -g, 0 5 ] T f 1 = [0 3 , q 2 t + q 2
x -q 2 y -q 2 z , 2q t q z + 2q y q x , -2q t q y + 2q z q x , 0 5 ] T f 2 = [0 3 , -2q t q z + 2q y q x , q 2 t + q 2 y -q 2 z -q 2 x , 2q t q x + 2q z q y , 0 5 ] T f 3 = [0 3 , 2q t q y + 2q z q x , -2q t q x + 2q z q y , q 2 t + q 2 z -q 2 x -q 2 y , 0 5 ] T

f 4 = [0 6 , -1/2q x , 1/2q t , 1/2q z , -1/2q y , 0] T f 5 = [0 6 , -1/2q y , -1/2q z , 1/2q t , 1/2q x , 0] T f 6 = [0 6 , -1/2q z , 1/2q y , -1/2q x , 1/2q t , 0] T
where we denoted with 0 n the vector line whose dimension is n and whose entries are all zeros. We must compute the Lie derivatives of the two observation functions given in (4.22) and (4.23) with respect to all the vector fields. By a direct computation, performed by using the symbolic Matlab computational tool, we were able to find not more than 6 independent Lie derivatives1 .

In particular, according to the notation introduced in [64], the system has 5 continuous symmetries which are:

w 1 s = [0, 1, k, 0 8 ] T w 2 s = [1, 0 10 ] T w 3 s = [0 4 , 1, k, 0 5 ] T w 4 s = [0 3 , 1, 0 7 ] T w 5 s = [0 6
, q z -kq y , q y + kq z , -q x + kq t , -q t -kq x , 0] T The observable modes are all the solutions of the system of partial differential equations associated with the five symmetries. For instance, the equation associated with w 1 s is ∂ ∂ry + k ∂ ∂rz = 0 (see [START_REF] Martinelli | State estimation based on the concept of continuous symmetry and observability analysis: The case of calibration[END_REF] for more details). Since this system of partial differential equations consists of 5 equations on a manifold whose dimension is 11, the number of independent solutions is 6 = 11 -5 [START_REF] John | Partial Differential Equations[END_REF]. A possible choice of these solutions is:

r z -kr y v z -kv y 2[c k s k (-q 2 t + q 2 x -q 2 y + q 2 z ) + (2c 2
k -1)(q t q x + q y q z )] 4c k s k (q t q z + q x q y ) + 2(2c2 k -1)(q t q y -q x q z ) k q * q where c k ≡ cos arctan(k) 2 and s k ≡ sin atan(k)

2

. By knowing the value of the first solution, r z -kr y and the value of the fifth solution, k, we can determine the quantity |rz-kry| √ 1+k 2 , which is the distance of the vehicle from the planar surface1 . Hence, to better visualize the physical meaning, it is convenient to select the following 6 observable modes:

m 1 = r z -kr y √ 1 + k 2 m 2 = v z -kv y √ 1 + k 2 m 3 = 2[c k s k (-q 2 t + q 2
x -q2 y + q 2 z ) + (2c 2 k -1)(q t q x + q y q z )] m 4 = 4c k s k (q t q z + q x q y ) + 2(2c 2 k -1)(q t q y -q x q z ) m 5 = k m 6 = q * q Finally, the physical meaning of also m 3 and m 4 becomes clear by referring to a new global frame x, ỹ, z. This frame has the xỹ-plane coincident with the planar surface. In other words, this new global frame has the vertical axis coincident with the axis orthogonal to the planar surface. In this new frame, m 1 is the z-coordinate of the vehicle, m 2 is the component of the vehicle speed along the z-axis. m 3 and m 4 are related to the roll and pitch angles of the vehicle in the new frame. In particular, the roll angle is arctan

m 3 √ 1-m 2 3 -m 2 4
and the pitch is arcsin(m 4 ). m 5 is related to the orientation of the xỹ-plane with respect to the gravity. In particular, the z-axis makes an angle arctan(k) = arctan(m 5 ) with the gravity. m 6 is trivially the magnitude of the quaternion, which is 1 since it describes a rotation. From now on, we will adopt the new frame to characterize the vehicle configuration. The state in this frame is S = [r q ṽq q, k] T . The x, ỹ, z-frame is obtained by rotating the x, y, z-frame about the x-axis of the angle arctan(k). Hence, it is characterized by the quaternion:

p = cos arctan(k) 2 + i sin arctan(k) 2 (4.25)
Therefore, q = pq or: q = p * q (4.26)

By using the quaternion p it is also possible to obtain: rq = p * r q p ṽq = p * v q p (4.27)

By using (4.26) and (4.27), we obtain the expressions of the observable modes in the new coordinates. We have:

m 1 = rz m 2 = ṽz m 3 = 2(q t qx + qy qz ) m 4 = 2(q t qy -qx qz ) m 5 = k m 6 = q * q (4.28)
In particular m 1 is the z-coordinate of the vehicle, m 2 the component of the vehicle speed along the z-axis, m 3 and m 4 are related to the roll and pitch angles of the vehicle in this frame. In particular, the roll is arctan

m 3 √ 1-m 2 3 -m 2 4
and the pitch is arcsin(m 4 ). m 5 is related to the orientation of the xỹ-plane with respect to the gravity. In particular, the z-axis makes an angle arctan(k) = arctan(m 5 ) with the gravity. m 6 is trivially the magnitude of the quaternion q, which is 1 since it describes a rotation. By summarizing the results of the observability analysis performed in this section we say that the information contained in the data provided by the IM U and the camera during a given time-interval, allows us to determine the six modes m 1 , ..., m 6 . For this reason, in the rest of the section, we will focus our attention only on these six quantities (actually, on the first five, since m 6 trivially expresses the constraint of having a unitary quaternion).

Local Decomposition and Recursive Estimation

The goal of this section is to provide a method able to estimate the observable modes in (Section 4.3.3). To achieve this goal, the first step is to determine the link between the observable modes and the sensor data. By adopting the terminology introduced in [START_REF] Isidori | Nonlinear Control Systems[END_REF], we have to perform a local decomposition of our system. We remind the reader that the local decomposition is the extension of the Kalman canonical decomposition [START_REF] William | Modern Control Theory[END_REF] to the case of a non linear system. It consists in writing the equations characterizing the dynamics and the observation only in terms of the observable modes. We also remind the reader that in the non linear case it is often impossible to characterize the system with a unique decomposition. The decomposition only holds in a local region of the space of states. This is the reason why it is called local decomposition. To cover the entire space of states more than one decomposition is required (see [START_REF] Isidori | Nonlinear Control Systems[END_REF]). In the following, we will show that for our system the number of decompositions is two.

We first provide the dynamics of the state S = [r q ṽq q, k] T . We obtain:

             ṙq = ṽq vq = qA q q * + p * g q p q = 1 2 qΩ q k = 0 (4.29)
A local decomposition for the dynamics is:

               ṁ1 = m 2 ṁ2 = -m 4 A x + m 3 A y + ξ(m 3 , m 4 )A z + g z ṁ3 = Ω x ξ(m 3 , m 4 ) + Ω z m 4 ṁ4 = Ω y ξ(m 3 , m 4 ) -Ω z m 3 ġz = 0 (4.30)
where g z is the component of the gravity along the z-axis, i.e.

g z = -g cos(arctan(k)) = -g √ 1+k 2 = -g √ 1+m 2 5
; the function ξ(m 3 , m 4 ) depends on the original state and in particular changes its sign depending on the sign of q2

x + q2 y -1 2 :

ξ(m 3 , m 4 ) ≡      1 -m 2 3 -m 2 4 if q2 x + q2 y < 1 2 -1 -m 2 3 -m 2 4 if q2 x + q2 y > 1 2 (4.31)
Hence, as previously said, we have two local decompositions for our original system. The validity of (4.30) can be checked by using (4.28) and (4.29). Note that deriving (4.30) is troublesome. In contrast, checking its validity is very simple since it only demands to perform differentiation.

To complete the local decomposition we need to express the camera observation function in (4.22) in terms of the observable modes. We obtain:

h cam = Lξ(m 3 , m 4 ) m 4 L -m 1 (4.32)
The validity of (4.32) can be checked by using (4.22), (4.25), (4.26), (4.28) and (4.31).

The equations (4.30) and (4.32) represent a local decomposition for our system. They provide the analytical expression of the link between the observable modes and the sensor data. Specifically, equation (4.30) provides the link between the observable modes and the IM U data. Equation (4.32) provides the link between the observable modes and the data delivered by the monocular camera. Having these equations allows us to perform the estimation of the state [m 1 , m 2 , m 3 , m 4 , g z ]. An efficient and simple approach is obtained by using an Extended Kalman Filter, EKF . To implement this filter it suffices to compute the Jacobian of the dynamics in (4.30) and the Jacobian of the observation function in (4.32) ( [START_REF] Bar-Shalom | Tracking and data association[END_REF]).

Let us consider the state m = [m 1 , m 2 , m 3 , m 4 , g z ]. The basic ingredients to implement an EKF , which estimates m, are the analytical expression of the Jacobians of the dynamics, and the observation [START_REF] Bar-Shalom | Tracking and data association[END_REF].

The Jacobian of the observation is obtained by differentiating the expression of h cam given in (4.32) with respect to m, i.e.

H ≡ ∂h cam ∂m = Lξ(m 3 , m 4 ) (m 4 L -m 1 ) 2 × × 1 0 -m 3 (m 4 L-m 1 ) 1-m 2 3 -m 2 4 Lm 2 3 -L+m 1 m 4 1-m 2 3 -m 2 4 0
where the function ξ(m 3 , m 4 ) is defined in (4.31). Regarding the Jacobian of the dynamics, we need first of all to discretize the equations in (4.30). Let us denote with δt the discretization time step. The Jacobian of the dynamics with respect to the state m is:

F m =       1 δt 0 0 0 0 0 δt (A y -A z m 3 ) -δt (A x + A z m 4 ) 0 0 0 -δtΩ x m 3 δt(Ω z -Ω x m 4 ) 0 0 0 -δt(Ω z + Ω y m 3 ) -δtΩ y m 4 0 0 0 0 0 1       where m 3 = m 3 ξ(m 3 , m 4 ) and m 4 = m 4
ξ(m 3 , m 4 ) . The Jacobian of the dynamics with respect to the input u = [A x , A y , A z , Ω x , Ω y , Ω z ] T is:

F u =       0 0 0 0 0 0 -m 4 m 3 ξ 0 0 0 0 0 0 ξ 0 m 4 0 0 0 0 ξ -m 3 0 0 0 0 0 0      
where ξ ≡ ξ(m 3 , m 4 ).

Performance Evaluation

We evaluate the performance of the proposed strategy by using both synthetic and real data. The advantage of simulations is that the ground truth is perfectly known and this allows us a quantitative evaluation of the proposed strategy.

Simulations

We simulate many different trajectories in 3D and many different scenarios corresponding to different orientation of the planar surface. For all the simulations we use the proposed strategy to estimate the observable modes, i.e.:

• the distance of the vehicle from the planar surface (d = |m 1 |);

• the component of the vehicle speed orthogonal to the planar surface (v o = m 2 );

• the roll (R) and the pitch (P ) angles in the x, ỹ, z-frame (i.e. the frame where the x, ỹ-plane coincides with the planar surface);

• the orientation of the plane with respect to the gravity (α). . . . Specifically, in all the simulations the values of the estimated d, v o , R, P and α will be compared with the ground truth values.

Simulated Trajectories

The trajectories are generated by randomly generating the linear and angular acceleration of the vehicle at 100 Hz. In particular, at each time step, the three components of the linear acceleration and the angular speed are generated as Gaussian independent variables whose mean values will be denoted respectively with µ a and µ ω and whose variances will be denoted respectively with σ 2 a and σ 2 ω . We set the parameters to be close to a real case: µ a = 0 ms -2 , σ a = 1 ms -2 , µ ω = 0 deg s -1 and σ ω = 10 deg s -1 . The initial vehicle position is at x = 0, ỹ = 0, z = 1m. The initial vehicle speed is [1, 0, 0]ms -1 in the x, ỹ, z-frame. Simulated Sensors Starting from the performed trajectory, the true angular speed and the linear acceleration are computed at each time step of 0.01s (respectively, at the time step i, we denote them with Ω true i and A true v i ). Starting from them, the IMU sensors are simulated by randomly generating the angular speed and the linear acceleration at each step according to the following:

Ω i = N Ω true i -Ω bias , P Ω i and A i = N A true v i -A gi -A bias , P A i where:
• N indicates the Normal distribution whose first entry is the mean value and the second its covariance matrix;

• P Ω i and P A i are the covariance matrices characterizing the accuracy of the IM U ;

• A gi is the gravitational acceleration in the local frame and A bias is the bias affecting the data from the accelerometer; • Ω bias is the bias affecting the data from the gyroscope.

In all the simulations we set both the matrices P Ω i and P A i diagonal and in particular: P Ω i = σ 2 gyro I 3 and P A i = σ 2 acc I 3 , where I 3 is the identity 3 × 3 matrix. We considered several values for σ gyro and σ acc , in particular: σ gyro = 1 deg s -1 and σ acc = 0.01 ms -2 .

Regarding the camera, the provided readings are generated in the following way. By knowing the true trajectory, and the position and the orientation of the planar surface, the true bearing angles of the laser spot in the camera frame are computed 1 . They are computed each 0.1s. The parameter L is set equal to 0.3m. Then, the camera readings are generated by adding to the true values zero-mean Gaussian errors whose variance is equal to (1 deg) 2 for all the readings. ). As expected, the estimation in presence of bias becomes worse. However, the error on the estimated α in presence of bias is smaller than 1 deg. measurements are displayed. We initialized the filter by using a value of the initial observable state which differs from the ground truth by a relative error in the range [START_REF] Armesto | Fast ego-motion estimation with multi-rate fusion of inertial and vision[END_REF][START_REF] Castillo | Modelling and Control of Mini-Flying Machines[END_REF]%. We also evaluated the robustness of the filter with respect to systematic errors on the imu-camera calibration and laser-camera calibration. Specifically, we performed simulations by introducing errors of one cm and one deg on the calibration parameters. These systematic errors affect the estimated α (the difference with respect to the ground truth is in the range [4,[START_REF]Vicon Motion Capture System[END_REF]deg) while for all the other observable modes the effect is negligible (less than one deg for R and P and less than 1cms -1 and 1cm respectively for v o and d).

Simulation Results

Preliminary experiments

In this section we provide preliminary results obtained by using a data set provided by the autonomous system laboratory at ETHZ in Zurich. The data are provided together with a reliable ground-truth, which has been obtained by performing the experiments at the ETH Zurich Flying Machine Arena [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF], which is equipped with a Vicon motion capture system. As previously said, the observations of the laser spot are simulated. This was possible thanks to the fact that a reliable ground truth was provided together with the inertial data. In particular, given the true trajectory, we simulated the same planar surface described in the previous section. By having the true vehicle configuration it was possible to create the observations performed by the camera on the laser spot produced by a laser pointer as in the simulations (see the last paragraph in 4. The Laser module and the monocular camera are mounted on a fixed baseline, and the latter is calibrated using the Camera Calibration Toolbox for Matlab [START_REF] Bouguet | Camera calibration toolbox for matlab[END_REF]. The calibration between IMU and camera has been performed using the Inertial Measurement Unit and Camera Calibration Toolbox by Lobo [57].

The results of the Camera-laser module calibration described in Section4.

Conclusion

In this section we considered an aerial vehicle equipped with a monocular camera and inertial sensors. Additionally, a laser pointer is mounted on the vehicle and it produces a laser spot. The laser spot is observed by the monocular camera and it is the unique point feature used in the proposed approach. We focused our attention to the case when the vehicle moves in proximity of a planar surface and in particular when the laser spot belongs to this surface. The work provided two main contributions. The former is the analytical derivation of all the observable modes, i.e. all the physical quantities that can be determined by only using the inertial data and the camera observation of the laser spot during a short timeinterval. Specifically, it is shown that the observable modes are: the distance of the vehicle from the planar surface; the component of the vehicle speed, which is orthogonal to the planar surface; the relative orientation of the vehicle with respect to the planar surface; the orientation of the planar surface with respect to the gravity. Once the observed modes have been derived, a local decomposition of the original system has also been provided. This decomposition separates the observable modes from the rest of the system and allowed us to introduce a simple recursive method to perform the estimation of all the observable modes (second contribution). The use of a virtual laser spot feature overcomes the limits of feature tracking algorithms and makes our approach suitable to work even in dark or featureless environment. The method is validated by using synthetic data. The validation with real data is in progress. We presented a low-cost low-weight sensor suite and a low computational complexity approach in the framework of aerial navigation. It can be integrated in the framework of autonomous takeoff and landing, safe touch-down and low altitude manoeuvres. However, we want to emphasize that both the contributions are very general and can be applied in other frameworks. In particular, in all the environment where GPS is denied and where the most of objects have planar surfaces (e.g. in an indoor or city-like environment). For instance, these contributions could be used in the framework of humanoid robotics (where visual and inertial sensing are often adopted and the navigation usually occurs in an indoor environment).

Chapter 5 Conclusions

This dissertation tackles the problems of data association and pose estimation of a camera-IMU system, with a focus on MAVs navigation. Due to the limited computation resources of MAVs, a particular attention is devoted to the study of low computational complexity techniques.

Two methods to perform outlier detection on computationally-constrained micro aerial vehicles are presented. The algorithms rely on on-board IMU measurements to calculate the relative rotation between two consecutive camera frames and the reprojection error to detect the inliers. The first method assumes that vehicle's motion is locally planar, while the second method includes the general case of a 6DoF motion. Even if the 5-point RANSAC is the "golden standard method" for 6DoF motion estimation, experimental results shows that the proposed Me-RE and 2-point RANSAC algorithms can be used as a first choice before committing to the 5-point RANSAC due to their very low computational complexity. Considering that the Me-RE algorithm relies on the local planar motion assumption, we remark that it can replace the 5-point algorithm if the motion of the vehicle is smooth and the camera framerate is high. We show that the Me-RE algorithm outperforms standard RANSAC-based methods by up to two orders of magnitude in speed, while being able to identify the majority of the inliers. The motion can then be refined applying standard methods [START_REF] Stewénius | Recent developments on direct relative orientation[END_REF], [START_REF] Hartley | Multiple view geometry in computer vision[END_REF] to the remaining inliers. We show that in the case of a monocular camera mounted on a quadrotor vehicle, motion priors from IMU can be used to discard wrong estimations in the framework of a 2-point-RANSAC based approach.

For what concerns the pose estimation problem, two low computational complexity algorithms are provided.

The first is a new approach to perform MAV localization by only using the data provided by an Inertial Measurement Unit and a monocular camera. The approach exploits the so-called "planar ground assumption" and the geometric constraints encoded in a virtual pattern made by three natural point features belonging to the ground plane. It is based on a closed solution which provides the vehicle pose from a single camera image, once the roll and the pitch angles are obtained by the inertial measurements. This makes the approach very simple in terms of computational complexity and robust since the closed form solution makes unnecessary any initialization. The very low computational cost of the proposed approach makes it suitable for pose control in tasks like hovering, autonomous take off and landing.

In order to overcome the limits of feature tracking algorithms and to provide a solution for featureless or dark environments, we introduce the concept of virtual features. We consider an aerial vehicle equipped with a monocular camera, inertial sensors and a laser pointer. We suppose that the vehicle is moving in the surrounding of a planar surface whose orientation is unknown (assumption that holds in indoor or city-like environments and on landing platforms) and that the laser spot produced by the laser pointer belongs to this surface. The laser spot is then observed by the monocular camera and it represents the unique point feature used in the proposed approach. The difference with respect to classical vision and IMU data fusion problems is that in this case the feature is moving jointly with the vehicle. We analytically derive all the physical quantities (called "observable modes") that can be determined by only using the information contained in the inertial data and the camera observation of the laser spot during a short timeinterval. Specifically, it is shown that the physical quantities we can estimate are: the distance of the vehicle from the planar surface; the component of the vehicle speed which is orthogonal to the planar surface; the relative orientation of the vehicle with respect to the planar surface; the orientation of the planar surface with respect to the gravity. Once having derived the observable modes, a local decomposition of the original system is provided. This decomposition separates the observable modes from the rest of the system and allowed us to introduce a simple recursive method to perform the estimation of all the observable modes. Additionally this decomposition provides the link between them and the sensor data. This corresponds to write the equations characterizing the dynamics and the observations only in terms of the observable modes and it is performed by using an extension of the Kalman Canonical decomposition for nonlinear systems [START_REF] Martinelli | State estimation based on the concept of continuous symmetry and observability analysis: The case of calibration[END_REF]. Once obtained the local decomposition, we estimate the observable subspace by using an Extended Kalman Filter.

All the algorithms described in this dissertation have been developed in the framework of MAVs navigation. However, they can be used wherever is required low computational complexity and low payload budget.

Research Outlook

Since the performed experiments provided encouraging results, future work will include supplementary performance evaluations.

Moreover, the contributions presented in this dissertation can be extended in the following directions:

1. The first one aims to the data association issue.

1.1 1-point algorithm -Real-time on-board implementation, considering the standard deviation of the distribution of the α computed from all the feature correspondences (3.10) as an index of reliability of the Me-RE algorithm. The feature matching set will be preprocessed at each timestamp with the Me-RE algorithm. If the variance of the aforementioned distribution will be higher than a predefined threshold, the resultant motion hypothesis will be discarded and a 5-point algorithm will be run on the whole matching set. On the contrary, the 5-point algorithm will be run only on the resultant inlier set.

1.2 2-point algorithm -Real-time on-board implementation, considering smart policies for the choice of the pairs of features to use (based for example on the feature positions in the image plane and not only on their relative position) in order to reduce the computational complexity of the approach.

2. The second one addresses the pose estimation problem.

2.1 Virtual patterns -Implementation with natural features with smart policies for the choice of the triplet of features to use. Additionally it would be interesting to analyse the computational complexity versus the robustness of the algorithm while considering virtual patterns made by more than three features.

2.2 Virtual features -Real-time on-board implementation to perform autonomous landing and safe touchdown tasks in dark or featureless environments. It is therefore important to consider laser patterns (instead of a single spot) in order to improve the precision. Additionally, we remarked that the performance depends on the laser-module configuration in the camera frame. Hence, the performance can be improved by using a laser module with a servomotor which changes the configuration according to the vehicle trajectory.

viii 5 .

 5 1 Research Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . Bibliography List of publications xi List of Figures 1.1 MAVs classification. The vehicles in the pictures are: Perching Glider, MIT (a); Festo's Smartbird (b); AeroVironments Nano Hummingbird (c); Skybotix's CoaX (d); Parrot's AR.Drone (e). . 1.2 History of quadrotors with respect to their application fields. 1907:

  Figure (c) represents a motion capture system room. Figures (a),(c) courtesy of http://www.

25 2 . 40 xiii 3 . 4 1. 2 6 1. 2 . 1 9 1. 3

 24034262193 14 Camera and IMU observing the vertical direction. Redrawn from [57]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 2.15 Camera poses relative to the calibration chessboard (a). Results of rotation estimation (b). . . . . . . . . . . . . . . . . . . . . . . 26 3.1 Comparison of feature detectors: properties and performances [88]. 30 3.2 Surf features matched across multiple frames overlaid on the first image. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.3 Number of RANSAC iterations. . . . . . . . . . . . . . . . . . . . 32 3.4 Epipolar constraint. p 1 , p 2 , T and P lie on the same plane (the epipolar plane). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.5 Notation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 3.6 C p1 and C p2 are the reference frames attached to the vehicle's body frame but which z-axis is parallel to the gravity vector. They correspond to two consecutive camera views. C p0 corresponds to the reference frame C p1 rotated according to dY aw. . . . . . . . . 36 3.7 Synthetic scenario. The green line represents the trajectory and the red dots represent the simulated features. . . . . . . . . . . . 39 3.8 Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5-point RANSAC (black), true number of inliers(blue) for a perfect planar motion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.9 Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5-point RANSAC (black), true number of inliers(blue) in presence of perturbations on the Roll and P itch angles. . . . . . . . . . . . 10 Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5-point RANSAC (black), true number of inliers(blue) in presence of perturbations on the dY aw angle. . . . . . . . . . . . . . . . . 3.11 Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5-point RANSAC (black), true number of inliers(blue) for a nonperfect planar motion (s 1 = 0.02 * sin(8 * w c • t)). . . . . . . . . . 3.12 Nano quadrotor from KMelRobotics: a 150g and 18cm sized platform equipped with an integrated Gumstix Overo board and Ma-trixVision VGA camera. . . . . . . . . . . . . . . . . . . . . . . . 3.13 Plot of the real trajectory. The vehicle's body frame is depicted in black and the green line is the trajectory followed. . . . . . . . . . 3.14 Number of found inliers by Me-RE (red), 1-point RANSAC (green),5-point RANSAC (black), 8-point RANSAC (blue) along the trajectory depicted in Figure 3.13. . . . . . . . . . . . . . . . . . . . 3.15 From the top to the bottom: Roll, P itch and dY aw angles [deg] estimated with the IMU (red) versus Roll, P itch and dY aw angles [deg] estimated with the Optitrack system (blue). The last plot shows the height of the vehicle above the ground (non perfect planarity of motion). . . . . . . . . . . . . . . . . . . . . . . . . . 3.16 Computation time. . . . . . . . . . . . . . . . . . . . . . . . . . . 3.17 The reference frame C 0 and C 2 differ only for the translation vector T . ρ = |T | and the angles α and β allow us to express the origin of the reference frame C 2 in the reference frame C 0 . . . . . . . . . 3.18 Hough Space in α and β computed with real data. . . . . . . . . 3.19 Notation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.20 Motion constraints on a quadrotor relative to its orientation. ∆φ > 0 implies a movement along Y B 0 positive direction, ∆θ < 0 implies a movement along Y B 0 positive direction. . . . . . . . . . . . . . . 3.21 Synthetic scenario. The red line represents the trajectory and the blue dots represent the simulated features. The green dots are the features in the current camera view. . . . . . . . . . . . . . . . . . 3.22 The IMU measurements are not affected by noise (ideal conditions). 3.23 The angles ∆φ, ∆θ and ∆ψ are affected by noise. . . . . . . . . . 3.24 Only the angles ∆φ and ∆θ are affected by noise. . . . . . . . . . 3.25 Only the angle ∆ψ is affected by noise. . . . . . . . . . . . . . . . 3.26 Real scenario. The vehicle body frame is represented in blue, while the red line represents the followed trajectory. . . . . . . . . . . . 3.27 Number of inliers detected with the Hough approach (red), the 2-point RANSAC (cyan), the 5-point RANSAC (black) and the 8-point RANSAC (blue) along the trajectory depicted in Figure 3.26. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiv 3.28 Computation time. . . . . . . . . . . . . . . . . . . . . . . . . . . 3.29 Errors between the relative rotations ∆φ (err R ), ∆θ (err P ), ∆ψ (err Y ) estimated with the IMU and estimated with the Optitrack. 4.1 Global frame. Two is the minimum number of point features which allows us to uniquely define a global reference frame. P 1 is the origin, the x G -axis is parallel to the gravity and P 2 defines the x G -axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 The 2p-algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 The three reference frames adopted in our derivation. . . . . . . . 4.4 The yaw angle (-α) is the orientation of the X N -axis in the global frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5 The triangle made by the 3 point features. . . . . . . . . . . . . . 4.6 Flow chart of the proposed pose estimator . . . . . . . . . . . . . 4.7 Estimated x, y, z (a), and Roll, P itch, Y aw (b). The blue line indicate the ground truth, the green one the estimation with the 2p-Algorithm and the red one the estimation with the 3p-Algorithm 4.8 Mean error on the estimated states in our simulations. For the position the error is given in %. . . . . . . . . . . . . . . . . . . . 4.9 AscTec Pelican quadcopter [1] equipped with a monocular camera. 4.10 Our Pelican quadcopter: a system overview . . . . . . . . . . . . . 4.11 Scenario: The AR Marker and the 3 balls are used only with the aim to get a rough ground truth. The AR Marker provides the camera 6DOF pose in a global reference frame according to our conventions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.12 Estimated position (a), respectively x, y, z and estimated attitude (b), respectively Roll, P itch, Y aw. The red lines represent the estimated values with the 3p-Algorithm, the blue ones represent a rough ground truth (from ARToolkit Marker). . . . . . . . . . . . 4.13 Quadrotor equipped with a monocular camera, IM U and a laser pointer. The laser spot is on a planar surface and its position in the camera frame is obtained by the camera up to a scale factor. . 4.14 The original camera frame XY Z, the chosen camera frame X Y Z and the laser module at the position [L x , L y , 0] and the direction (θ, φ) (in the original frame) and position [L, 0, 0] and the direction (0, 0) (in the chosen camera frame). . . . . . . . . . . . . . . 4.15 Camera-Laser module calibration steps. Figure (a) is the camera image containing the laser spot projected onto a checkerboard. In green the reference frame attached to the checkerboard. Figure (b) represents three different camera positions used during the calibration process, the green grid represents the checkerboard and the red lines represent the reference frame attached to the checkerboard. 4.16 The Pelican quadcopter equipped with a monocular camera and a laser module. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.17 A typical vehicle trajectory in our simulations. . . . . . . . . . . . 4.18 Estimated α in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . 4.19 Estimated P in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . 4.20 Estimated R in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . 4.21 Estimated v 0 in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . 4.22 Estimated d in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . . . . . . 4.23 Estimated α in the experiment. Blue dots indicate ground true values while red discs indicate the estimated values. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.1.1 Why quadrotors? . . . . . . . . . . . . . . . . . . . . . 2 1.1.2 Brief quadrotor's history . . . . . . . . . . . . . . . . . 3 1.1.3 Applications . . . . . . . . . . . . . . . . . . . . . . . 4 1.1.4 The quadrotor concept . . . . . . . . . . . . . . . . . . Motivation and objectives . . . . . . . . . . . . . . . . Minimalist perception . . . . . . . . . . . . . . . . . . Contributions . . . . . . . . . . . . . . . . . . . . . . . . 10 1.4 Thesis outline . . . . . . . . . . . . . . . . . . . . . . . . 12

Figure 1

 1 Figure 1.1: MAVs classification. The vehicles in the pictures are: Perching Glider, MIT (a); Festo's Smartbird (b); AeroVironments Nano Hummingbird (c); Skybotix's CoaX (d); Parrot's AR.Drone (e).

Figure 1 . 2 :

 12 Figure 1.2: History of quadrotors with respect to their application fields. 1907: Paul Corny machine (both from 1907), Bréguet Giant Gyroplane. 1924: Ohemichen's quadrotor; 2000: ETH Zurich's OS4[START_REF] Bouabdallah | Pid vs lq control techniques applied to an indoor micro quadrotor[END_REF],[START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF], the CEA's X4-flyer[START_REF] Guenard | Dynamic modeling and intuitive control strategy for an "x4-flyer[END_REF] and the ANU's X-4 Flyer[START_REF] Pounds | Construction and Control of a Large Quadrotor micro air vehicle[END_REF]; Nowadays there exists a lot of quadrotors. We list here few exemplars: AscTec Pelican [1], KMelRobotics' NanoQuad[3], Mikrokopter's quadrotor[4], Flyduino, Arducopter, Parrot AR.Drone, DeltaDrone quadrotor[2].

Figure 1 . 3 :

 13 Figure 1.3: Quadrotors in research projects and in companies.

Figure 1 . 4 :

 14 Figure 1.4: Quadrotor notation. The four rotors are depicted in blue. w i is the angular velocity of the i-th rotor, F i and M i are the vertical force and the moment respectively produced by the i-th rotor. The body-vehicle's frame B is shown in black, it is attached to the vehicle, and its origin is coincident with the vehicle's center of mass. In gray it is depicted the world reference frame W.

Figure 1 . 5 :

 15 Figure 1.5: The quadrotor concept. The width of the arrows is proportional to the angular speed of the propellers.

•

  Biased and noisy measurements;• Large uncertain for slow mo*ons.

Figure 1 . 6 :Figure 1 . 7 :

 1617 Figure 1.6: Properties of some sensors commonly on-board a MAV.

Figure 2 . 1 :

 21 Figure 2.1: Complementary properties of cameras and IMU sensors.

Figure 2 . 2 :

 22 Figure 2.2: Human eye. Image courtesy of http://www.biographixmedia.com.

Figure 2 . 3 :

 23 Figure 2.3: Insect eye. Image courtesy of http://www.wikipedia.org.

Figure 2 . 4 :

 24 Figure 2.4: Vestibular system. Image (a) courtesy of http://www.chrcentre. com.au, image (b) courtesy of http://biology.nicerweb.com.

Figure 2 . 5 :

 25 Figure 2.5: Halteres: small knobbed structures in some two-winged insects. They are flapped rapidly and function as gyroscopes, providing informations to the insect about his body rotation during flights.

Figure 2 . 6 :

 26 Figure 2.6: IMU applications Image courtesy of http://www. unmannedsystemstechnology.com.

Figure 2 . 7 :

 27 Figure 2.7: Physical principle of a single-axis accelerometer.

Figure 2 . 8 :

 28 Figure 2.8: Scheme of a 3D mechanical Gyroscope. Image courtesy of http: //www.wikipedia.org.

  to electron conversion (photosite) Electron to voltage conversion (transistors) (b)

Figure 2 . 9 :

 29 Figure 2.9: Diagram of a CCD (a) and CMOS (b) sensor.

Figure 2 . 10 :

 210 Figure 2.10: Matrix Vision mvBlueFOX-MLC usb camera.

Figure 2 .

 2 Figure 2.11: Pinhole camera model for standard perspective cameras.

Figure 2 .

 2 Figure 2.12: Chessboard images used for calibration.

Figure 2 . 13 :

 213 Figure 2.13: Extracted corners on a chessboard.

Figure 2 . 14 :

 214 Figure 2.14: Camera and IMU observing the vertical direction. Redrawn from [57].

Figure 2 . 15 :

 215 Figure 2.15: Camera poses relative to the calibration chessboard (a). Results of rotation estimation (b).

  (a) Harris corners. (b) Surf features.

Figure 3 .

 3 Figure 3.1: Comparison of feature detectors: properties and performances [88].

Figure 3 . 2 :

 32 Figure 3.2: Surf features matched across multiple frames overlaid on the first image.

Figure 3 . 3 :

 33 Figure 3.3: Number of RANSAC iterations.

Figure 3 . 4 :

 34 Figure 3.4: Epipolar constraint. p 1 , p 2 , T and P lie on the same plane (the epipolar plane).

Figure 3 .

 3 Figure 3.5: Notation.

Figure 3 . 6 :

 36 Figure 3.6: C p1 and C p2 are the reference frames attached to the vehicle's body frame but which z-axis is parallel to the gravity vector. They correspond to two consecutive camera views. C p0 corresponds to the reference frame C p1 rotated according to dY aw.

Figure 3 . 7 :

 37 Figure 3.7: Synthetic scenario. The green line represents the trajectory and the red dots represent the simulated features.

Figure 3 . 8 :

 38 Figure 3.8: Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5point RANSAC (black), true number of inliers(blue) for a perfect planar motion.

Figure 3 . 9 :Figure 3 . 10 :

 39310 Figure 3.9: Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5point RANSAC (black), true number of inliers(blue) in presence of perturbations on the Roll and P itch angles.

Figure 3 .

 3 Figure 3.11: Number of found inliers by Me-RE (red), 1-point RANSAC (cyan), 5-point RANSAC (black), true number of inliers(blue) for a non-perfect planar motion (s 1 = 0.02 * sin(8 * w c • t)).

Figure 3 . 12 :

 312 Figure 3.12: Nano quadrotor from KMelRobotics: a 150g and 18cm sized platform equipped with an integrated Gumstix Overo board and MatrixVision VGA camera.

Figure 3 . 13 :

 313 Figure 3.13: Plot of the real trajectory. The vehicle's body frame is depicted in black and the green line is the trajectory followed.

Figure 3 . 14 :

 314 Figure 3.14: Number of found inliers by Me-RE (red), 1-point RANSAC (green), 5-point RANSAC (black), 8-point RANSAC (blue) along the trajectory depicted in Figure 3.13.

Figure 3 . 15 :Figure 3 . 16 :

 315316 Figure 3.15: From the top to the bottom: Roll, P itch and dY aw angles [deg] estimated with the IMU (red) versus Roll, P itch and dY aw angles [deg] estimated with the Optitrack system (blue). The last plot shows the height of the vehicle above the ground (non perfect planarity of motion).

Figure 3 . 17 :

 317 Figure 3.17: The reference frame C 0 and C 2 differ only for the translation vector T . ρ = |T | and the angles α and β allow us to express the origin of the reference frame C 2 in the reference frame C 0 .

Figure 3 . 18 :

 318 Figure 3.18: Hough Space in α and β computed with real data.

Figure 3 .

 3 Figure 3.19: Notation.

Figure 3 . 20 :

 320 Figure 3.20: Motion constraints on a quadrotor relative to its orientation. ∆φ > 0 implies a movement along Y B 0 positive direction, ∆θ < 0 implies a movement along Y B 0 positive direction.

Figure 3 . 21 :

 321 Figure 3.21: Synthetic scenario. The red line represents the trajectory and the blue dots represent the simulated features. The green dots are the features in the current camera view.

Figure 3 . 22 :

 322 Figure 3.22: The IMU measurements are not affected by noise (ideal conditions).

Figure 3 . 23 :

 323 Figure 3.23: The angles ∆φ, ∆θ and ∆ψ are affected by noise.

Figure 3 . 24 :

 324 Figure 3.24: Only the angles ∆φ and ∆θ are affected by noise.

Figure 3 . 25 :

 325 Figure 3.25: Only the angle ∆ψ is affected by noise.

Figure 3 . 26 :

 326 Figure 3.26: Real scenario. The vehicle body frame is represented in blue, while the red line represents the followed trajectory.

Figure 3 . 27 :

 327 Figure 3.27: Number of inliers detected with the Hough approach (red), the 2point RANSAC (cyan), the 5-point RANSAC (black) and the 8-point RANSAC (blue) along the trajectory depicted in Figure 3.26.

Figure 4 .

 4 Figure 4.1: Global frame. Two is the minimum number of point features which allows us to uniquely define a global reference frame. P 1 is the origin, the x G -axis is parallel to the gravity and P 2 defines the x G -axis

Figure 4 .

 4 3 displays all the reference frames: the global frame G, the vehicle frame (represented by V ) and the new vehicle frame N . Our goal is to determine the coordinates of the origin of the vehicle frame in the global frame and the orientation of the X N -axis with respect to the x G -axis (which corresponds

Figure 4 . 2 :

 42 Figure 4.2: The 2p-algorithm.

Figure 4 . 3 :

 43 Figure 4.3: The three reference frames adopted in our derivation.

Figure 4 . 5 :

 45 Figure 4.5: The triangle made by the 3 point features.

  3) while the other expressions are: Y aw 12 = -atan(∆ν 12 /∆µ 12 ) Y aw 13 = -atan(∆ν 13 /∆µ 13 ) Y aw 23 = -atan(∆ν 23 /∆µ 23 ) (4.5)

7 )

 7 in which the variables Y aw ij are nonlinear functions of ζ = [Roll, P itch] T .Once the least-squares algorithm finds the Roll and Pitch angles that minimize the cost function, we can estimate the Yaw angle and the coordinates x, y and z as described in 2p-algorithm (Figure4.6).

Figure 4 . 6 :

 46 Figure 4.6: Flow chart of the proposed pose estimator

Figures 4 . 7 .

 47 a show the results regarding the estimated x, y and z. Figures4.7.b show the results regarding the estimated Roll, P itch and Y aw. In each figure we represent the ground truth values in blue, the values estimated with the 2p-Algorithm in green and the values estimated with the 3p-algorithm in red.

Figure 4 . 7 :

 47 Figure 4.7: Estimated x, y, z (a), and Roll, P itch, Y aw (b). The blue line indicate the ground truth, the green one the estimation with the 2p-Algorithm and the red one the estimation with the 3p-Algorithm

Figure 4 . 8 :

 48 Figure 4.8: Mean error on the estimated states in our simulations. For the position the error is given in %.

Figure 4 . 9 :

 49 Figure 4.9: AscTec Pelican quadcopter [1] equipped with a monocular camera.

Figure 4 . 10 :

 410 Figure 4.10: Our Pelican quadcopter: a system overview

Figure 4 .Figure 4 . 12 :

 4412 Figure 4.11: Scenario: The AR Marker and the 3 balls are used only with the aim to get a rough ground truth. The AR Marker provides the camera 6DOF pose in a global reference frame according to our conventions.

Figure 4 . 13 :

 413 Figure 4.13: Quadrotor equipped with a monocular camera, IM U and a laser pointer. The laser spot is on a planar surface and its position in the camera frame is obtained by the camera up to a scale factor.

Figure 4 . 14 :

 414 Figure 4.14: The original camera frame XY Z, the chosen camera frame X Y Z and the laser module at the position [L x , L y , 0] and the direction (θ, φ) (in the original frame) and position [L, 0, 0] and the direction (0, 0) (in the chosen camera frame).

Figure 4 . 15 :

 415 Figure 4.15: Camera-Laser module calibration steps. Figure (a) is the camera image containing the laser spot projected onto a checkerboard. In green the reference frame attached to the checkerboard. Figure (b) represents three different camera positions used during the calibration process, the green grid represents the checkerboard and the red lines represent the reference frame attached to the checkerboard.

Figure 4 . 16 :

 416 Figure 4.16: The Pelican quadcopter equipped with a monocular camera and a laser module.

Figure 4 .

 4 Figure 4.17: A typical vehicle trajectory in our simulations.

Figure 4 .

 4 17 displays a typical 3D trajectory obtained in our simulations. The figure also displays the planar surface, consisting of a plane, which makes an angle of α = π 8 rad = 22.5 deg with the gravity. Figures 4.18

  a and b display the estimated α respectively in the case without and with bias. The values of the biases adopted in our simulations are: Ω bias = [0.03 0.03 0.03] T (deg s -1 ) and A bias = [0.03 0.03 0.03] T (ms -2

Figures 4. 19

 19

Figure 4 . 18 :

 418 Figure 4.18: Estimated α in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values.

Figure 4 . 19 :Figure 4 . 20 :Figure 4 . 21 :Figure 4 . 22 :

 419420421422 Figure 4.19: Estimated P in absence (a) and in presence of bias (b) on the inertial data. Blue dots indicate ground true values while red discs indicate the estimated values.

Figure 4 .

 4 Figure 4.23 displays the estimated α. Figures 4.24 a and b display the estimated P and R and figures 4.25 a and b display the estimated v o and d. All the observable modes are estimated with very good accuracy. Additionally, we remark that the convergence of the filter occurs in less than half second for all the observable modes.

Figure 4 . 23 :Figure 4 . 24 :Figure 4 . 25 :

 423424425 Figure 4.23: Estimated α in the experiment. Blue dots indicate ground true values while red discs indicate the estimated values.

Figure 4 . 26 :

 426 Figure 4.26: The Pelican quadcopter equipped with a monocular camera and a laser module and passive markers.

4. 3 . 5 . 3

 353 Camera-laser module calibrationIn order to test the Camera-laser module calibration techinique, we equipped our robot platform (Pelican quadrotor from Ascending Technologies) with a monocular camera (Matrix Vision mvBlueFOX, F OV : 130 deg) and a Laser Module (SparkFun TTL, wavelength: 650nm, poweroutput: 0.45-0.8mW ).

  3.2 are the following: θ = 47.1 deg, φ = -3.1 deg and L x = -0.146 m, L y = -0.005 m. The resulting L is 0.100 m.

Figure 4 .

 4 [START_REF] Bouabdallah | Design and control of quadrotors with application to autonomous flying[END_REF] represents some steps of the calibration.

  flyingmachinearena.org. . . . . 1.8 This table summarises the objective of this dissertations in terms of minimalist perception. . . . . . . . . . . . . . . . . . . . . . . . 2.1 Complementary properties of cameras and IMU sensors. . . . . . Vestibular system. Image (a) courtesy of http://www.chrcentre. com.au, image (b) courtesy of http://biology.nicerweb.com. . 17 2.5 Halteres: small knobbed structures in some two-winged insects. They are flapped rapidly and function as gyroscopes, providing informations to the insect about his body rotation during flights. . 17 2.6 IMU applications Image courtesy of http://www.unmannedsystemstechnology. com. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.7 Physical principle of a single-axis accelerometer. . . . . . . . . . . 19 2.8 Scheme of a 3D mechanical Gyroscope. Image courtesy of http: //www.wikipedia.org. . . . . . . . . . . . . . . . . . . . . . . . . 20 2.9 Diagram of a CCD (a) and CMOS (b) sensor. . . . . . . . . . . . 21 2.10 Matrix Vision mvBlueFOX-MLC usb camera. . . . . . . . . . . . 22 2.11 Pinhole camera model for standard perspective cameras. . . . . . 22 2.12 Chessboard images used for calibration. . . . . . . . . . . . . . . . 24 2.13 Extracted corners on a chessboard. . . . . . . . . . . . . . . . . .

2.2 Human eye. Image courtesy of http://www.biographixmedia.com. 2.3 Insect eye. Image courtesy of http://www.wikipedia.org. . . . . xii 2.4

• Heavy for MAVs • Expensive • 2D informa*on

Task Minimal sensor suite Minimal amount of informa1on Hypothesis Algorithm Outlier rejec1on

		• Camera • IMU	• 1 feature • Angular rates (IMU)	Local planar mo9on	1--point algorithm
		• Camera • IMU	• 2 features (IMU) • Angular rates	None (6 DoF)	2--point algorithm
	Pose es1ma1on	• Camera • IMU • Camera • IMU	• 3 features • IMU • 1 feature (laser spot)	Planar ground assump9on Planar surface with unknown	Virtual paFerns Virtual features
		• Laser module	• IMU	orienta9on.
	Figure 1.8: This table summarises the objective of this dissertations in terms of
	minimalist perception.
	mation tasks, and to derive minimal complexity algorithms (Figure 1.8).

  The sense of sight . . . . . . . . . . . . . . . . . . . . 14 2.1.2 The perception of gravity . . . . . . . . . . . . . . . . 15 2.2 Inertial Measurement Unit . . . . . . . . . . . . . . . . 17

	Chapter 2
	Visual and Inertial sensors
	Contents
	2.1 A biological overview . . . . . . . . . . . . . . . . . . . 14
	2.1.1 2.2.1 Accelerometers . . . . . . . . . . . . . . . . . . . . . . 18
	2.2.2 Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . 19
	2.3 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
	2.3.1 Pinhole Camera Model . . . . . . . . . . . . . . . . . . 21
	2.4 Camera Calibration . . . . . . . . . . . . . . . . . . . . 23
	2.5 Camera-IMU Calibration . . . . . . . . . . . . . . . . . 24

  .5)

	Chapter 3
	Data association

Contents 3.1 Feature extraction and matching . . . . . . . . . . . . 28 3.1.1 Feature Detection . . . . . . . . . . . . . . . . . . . . 28 3.1.2 Feature Tracking . . . . . . . . . . . . . . . . . . . . . 29 3.1.3 Feature Matching . . . . . . . . . . . . . . . . . . . . . 30 3.2 Outlier detection . . . . . . . . . . . . . . . . . . . . . . 31 3.2.1 Related works . . . . . . . . . . . . . . . . . . . . . . . 32 3.2.2 Epipolar Geometry . . . . . . . . . . . . . . . . . . . . 33 3.2.3 1-point algorithm . . . . . . . . . . . . . . . . . . . . . 34 3.2.4 2-point algorithm . . . . . . . . . . . . . . . . . . . . . 45

  .. ... ...

		...	... ... ...	...	... ...
	... ... ... ...	...	... ... ...	...	... ...
	T n i S n i µ 1 1

Note that the planar assumption is not necessary to define a global frame. It is sufficient that P 1 and P

do not lie on the same vertical axis (defined by the gravity). The X G -axis can be defined assuming that P 2 belongs to the x G -z G -plane. In other words, P 2 has zero y G coordinate.

For a camera with a field of view smaller than 180deg the z-component is always positive in the original camera frame.

A possible choice of 6 independent Lie derivatives is:L 0 h const , L 0 h cam , L 1 f 0 h cam , L 1 f 4 h cam , L 1 f 5 h cam , L

f 0 , f 0 h cam .

In other words, also rz-kry √ 1+k

is a solution of the system of partial differential equations.

This is obtained also by knowing that the laser pointer has the same orientation as the camera and that it is located at the position [L, 0, 0]

[11] A. Bachrach, R. He, and N. Roy. Autonomous flight in unknown indoor environments. International Journal of Micro Air Vehicles, 1(4):217-228, December 2009. 7

Acknowledgements

Matt Might, a Professor of Computer Science at University of Utah, described what a doctorate is in the following way:

obtain:

Let us denote by D the distance between P 1 and P 2 . We have:

with ∆µ 12 ≡ µ 2 -µ 1 and ∆ν 12 ≡ ν 2 -ν 1 . In other words, z can be easily obtained in terms of D. The previous equation provides z up to a sign. This ambiguity is solved considering that the camera provides the sign of z 1 and z 2 . Then, we obtain Finally we obtain the coordinates of the origin of the vehicle frame in the global frame,

List of publications Journals