Agostino Martinelli
email: agostino.martinelli@inria.fr

Observabilty Properties and Deterministic Algorithms in Visual-Inertial Structure from Motion

This paper discusses the visual-inertial structure from motion problem (VI-SfM problem) with special focus on the following three fundamental issues: observability properties, resolvability in closed form and data association. Regarding the first issue, after a discussion about the current state of the art, the paper first investigates more complex scenarios. Specifically, with respect to the common formulation, which assumes three orthogonal accelerometers and three orthogonal gyroscopes, the analysis is extended to cope with the cases of a reduced number of inertial sensors and any number of point features observed by monocular vision. In particular, the minimal case of a single accelerometer, no gyroscope and a single point feature is addressed. Additionally, the analysis accounts for biased measurements and unknown extrinsic camera calibration. The results derived for these new and very challenging scenarios have interesting consequences both from a technological and neuroscientific perspective. Regarding the second issue, a simple closed form solution to the VI-SfM is presented. This solution expresses the structure of the scene and the motion only in terms of the visual and in-ertial measurements collected during a short time interval. This allows introducing deterministic algorithms able to simultaneously determine the structure of the scene together with the motion without the need for any initialization or prior knowledge. Additionally, the closed-form solution allows us to identify the conditions under which the VI-SfM has a finite number of solutions. Specifically, it is shown that the problem can have a unique solution, two distinct solutions or infinite solutions depending on the trajectory, on the number of point-features and on their arrangement in the 3D space and on the number of camera images. Finally, the paper discusses the third issue, i.e., the data association problem. Starting from basic results in computer vision, it is shown that, by exploiting the information provided by the inertial measurements, a single point correspondence (in the case of a planar motion) and two point correspondences (for a general 3D motion) are sufficient to characterize the motion between two camera poses. This allows us to use a 1-point RANSAC algorithm (in the planar case) or a 2-point RANSAC algorithm (in the general 3D case) to detect outliers. The paper concludes with some discussion about connections to related research fields both in the framework of computer science and neuroscience. Contents 1 Introduction 2 Observability properties in VI-SfM 2.1 State of the art 2.2 The System 2.3 Observability analysis 2.4 Observability Properties for the Standard Problem 2.5 Extension of the Theory of Herman and Krener 2.6 Observability Properties with unknown inputs 3 Resolvability in closed form 3.1 Solution of VI-SfM in closed form 3.2 Existence and number of distinct solutions 3.3 Performance Evaluation 3.4 Deterministic initialization of an EKF -based VI-SfM 4 Data Association i ii Contents 4.1 Outliers detection by using the closed-form solution 4.2 Essential matrix and epipolar constraint 4.3 Simplified model 5 Conclusion and discussion 5.1 Observability properties 5.2 Resolvability in closed-form and data association References

Introduction

The term Structure from Motion (SfM) was coined by the computer vision community to define the problem of estimating the three-dimensional structure of the scene and the motion from twodimensional image sequences. In this paper we consider the same estimation problem but the sensor suit is now composed by a monocular camera and inertial sensors (accelerometers and gyroscopes). We will refer to this problem as to the Visual-Inertial Structure from Motion problem (from now on the VI-SfM problem). The VI-SfM problem has particular interest and has been investigated in many disciplines, both in the framework of computer science (e.g., [START_REF] Bryson | Observability Analysis and Active Control for Airbone SLAM[END_REF][START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[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] Strelow | Motion estimation from image and inertial measurements[END_REF]) and in the framework of neuroscience (e.g., [START_REF] Berthoz | Perception of Linear Horizontal Self-Motion Induced by Peripheral Vision (Linearvection) Basic Characteristics and Visual-Vestibular Interactions[END_REF][START_REF] Dokka | Estimating distance during self-motion: a role for visual-vestibular interactions[END_REF][START_REF] Fetsch | Visual-vestibular cue integration for heading perception: Applications of optimal cue integration theory[END_REF]). These sensors require no external infrastructure and this is a key advantage for robots operating in unknown environments where GPS signals are shadowed. For this reason, vision and inertial sensing have received great attention by the mobile robotics community in the last years and many approaches have been introduced.

According to [START_REF] Corke | An Introduction to Inertial and Visual Sensing[END_REF], we distinguish between loosely coupled and tightly coupled approaches. In the former, the sensor data processing takes place in separate modules, which exchange information each other. The information delivered by the inertial can be used to speed up the tracking task of the features by predicting their locations within the next frame; in turn, data from the visual sensor allows updating the calibration parameters of inertial sensors. In the latter (tightly coupled approaches), all measurements, both visual and inertial, are combined and processed using a common filter-based approach.

A special issue of the International Journal of Robotics Research has recently been devoted to the visual and inertial sensor fusion [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 was 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. In [START_REF] Quian | Reduction of inherent ambiguities in structure from motion problem using inertial data[END_REF] the inertial measurements are used to reduce the ambiguities in the structure from motion problem.

The majority of the approaches so far introduced, perform the fusion of vision and inertial sensors by filter-based algorithms. In [START_REF] Armesto | Fast Ego-motion Estimation with Multi-rate Fusion of Inertial and Vision[END_REF], these 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 [START_REF] Veth | Fusing low-cost image and inertial sensors for passive navigation[END_REF] 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 SfM, an EKF has been adopted in [START_REF] Kim | Real-time implementation of airborne inertial-SLAM[END_REF] to perform VI-SfM . It was observed that any inconsistent attitude update severely affects any SfM 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. Very recently, in the frame work of micro aerial robotics, flight stabilization and fully autonomous navigation have been achieved by using monocular vision and inertial sensors as the only exteroceptive sensors. Also in this case, the sensor fusion was carried out by a filter based algorithm [START_REF] Weiss | Monocular-SLAM-Based Navigation for Autonomous Micro Helicopters in GPS-Denied Environments[END_REF][START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF].

Since most of the previous algorithms to fuse visual and inertial measurements are based on linear estimators, in the last years, the effect that the observability properties can have on the consistency of a linearized estimator, has been investigated [START_REF] Hesch | Towards consistent vision-aided inertial navigation[END_REF][START_REF] Kottas | On the consistency of vision-aided inertial navigation[END_REF][START_REF] Li | Improving the accuracy of EKF-based visual inertial odometry[END_REF]. More in general, recent works have investigated the observability properties of the VI-SfM problem for various scenarios (see section 2.1 for a detailed state of the art).

The first goal of this paper (chapter 2) is to provide the main results achieved in the last years about the observability properties of the VI-SfM problem and to provide an important step forward by analyzing new and very challenging scenarios. Specifically, in the second part of chapter 2, the observability analysis is extended to cope with the cases of a reduced number of inertial sensors and any number of point features observed by the monocular vision. In particular, the minimal case of a single accelerometer, no gyroscope and a single point feature is addressed. Additionally, the analysis accounts for biased measurements and unknown extrinsic camera calibration. The results derived for these new and very challenging scenarios have interesting consequences both from a technological and neuroscientific perspective.

There are very few methods able to perform the fusion of image and inertial measurements without a filter-based approach. One algorithm of this type has been suggested in [START_REF] Strelow | Motion estimation from image and inertial measurements[END_REF]. This algorithm is a batch method that performs SfM from image and inertial measurements. Specifically, it minimizes a cost function by using the Levenberg Marquardt algorithm [START_REF]Nonlinear models[END_REF]. This minimization process starts by initializing the velocities, the gravity and the biases to zero.

When using a recursive estimator (e.g. an EKF ), or an off-line optimization method to minimize a suitable cost function, an important issue that arises is the initialization problem. Indeed, because of the system non-linearities, an erroneous initialization can irreparably compromise the entire estimation process. This problem has firstly been considered in [START_REF] Lupton | Removing scale biases and ambiguity from 6DoF monocular SLAM using inertial[END_REF]. In particular, the proposed method is able to estimate the scale factor by using a square root information filter. Additionally, the same authors proposed an EKF that does not suffer from the initialization of the speed and of the orientation [START_REF] Lupton | Efficient Integration of Inertial Observations into Visual SLAM without Initialization[END_REF][START_REF] Lupton | Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions[END_REF]. An efficient solution to the initialization problem is obtained by introducing a deterministic algorithm able to determine the initial state by using the visual and inertial measurements acquired during a short time interval. This issue has been addressed only very recently [START_REF] Dong-Si | Estimator Initialization in Vision-aided Inertial Navigation with Unknown Camera-IMU 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 | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF]. Specifically, in [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF] a first closed-form solution to VI-SfM has been obtained. Then, new extensions of this solution have been derived to cope with the cases of biased accelerometer's measurements [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF][START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] and an unknown camera-IMU extrinsic calibration [START_REF] Dong-Si | Estimator Initialization in Vision-aided Inertial Navigation with Unknown Camera-IMU Calibration[END_REF]. In chapter 3, we provide a simple closed-form solution to the VI-SfM and, starting from this, we identify the conditions under which the VI-SfM has a finite number of solutions. Specifically, it is shown that the problem can have a unique solution, two distinct solutions or infinite solutions depending on the trajectory, on the number of point-features and on their arrangement in the 3D space and on the number of camera images.

Finally, a fundamental issue that arises in any visual motion estimation problem is data association. Any matching algorithm suffers from outliers, which must be detected and removed. To achieve this objective, the RANdom SAmple Consensus (RANSAC) introduced in [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF] has been extensively used in visual motion estimation. In the past, concerning the case of only visual measurements (i.e., in SfM), the 5-point RANSAC [START_REF] Nistér | Preemptive ransac for live structure and motion estimation[END_REF] has been adopted [START_REF] Lhuillier | Automatic structure and motion using a catadioptric camera[END_REF][START_REF] Tardif | Monocular visual odometry in urban environments using an omnidirectional camera[END_REF]. Indeed, five correspondences are in general necessary to identify the five parameters that characterize both the rotation (3 parameters) and the translation up to a scale (2 parameters), occurred between two camera frames. In the special case of a planar motion, the rotation can be characterized by a single parameter and the translation up to a scale by a further parameter. Hence, a 2-point RANSAC can be adopted to detect outliers for any planar motion. In the context of wheeled and indoor robotics, the motion not only is planar but also satisfies the non-holonomic constraint. This further information has been exploited in [START_REF] Scaramuzza | 1-Point-RANSAC Structure from Motion for Vehicle-Mounted Cameras by Exploiting Non-Holonomic Constraints[END_REF] to use a 1-point RANSAC for outliers detection.

The information provided by the inertial measurements, dramatically simplifies the data association task. Indeed, the rotation occurred between two camera frames can be efficiently obtained by integrating the inertial measurements. The translation up to a scale only depends on two parameters (for a general 3D motion) and one parameter (in the planar case). This makes possible the use of a 2-point RANSAC in the general 3D case and a 1-point RANSAC in the planar case [START_REF] Troiani | 1-Point-based Monocular Motion Estimation for Computationally-limited Micro Aerial Vehicles[END_REF]. We discuss this issue very briefly in chapter 4. This chapter discusses the observability properties in visual inertial structure from motion. The discussion starts by considering the standard formulation of the problem, where the system is a sensor assembling constituted by a monocular camera and an Inertial Measurement Unit (IMU), which consists of three orthogonal accelerometers and three orthogonal gyroscopes. In this case the considered state contains the vehicle speed and attitude, the biases on the inertial measurements, the position of the features observed by the camera, the parameters characterizing the extrinsic camera-inertial sensors calibration and the magnitude of the gravity. In the case of a single feature, this state consists of 24 components. The observability analysis is carried out by using the method of continuous symmetries, which has recently been introduced [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. Due to the system complexity, a direct derivation of the system continuous symmetries would require an excessive computational cost. For this reason, new theoretical results, which extend the theory developed in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], are here derived to significantly reduce the load of symbolic computation. Specifically, we introduce the quasi-projection operation, which allows us to reduce the space dimensionality when deriving the observability properties of any estimation problem. The first result provided in this chapter is the proof that, the information contained in the data provided by a monocular camera observing a single point-feature and by an IMU, allows estimating the absolute scale, the speed in the local frame, the absolute roll and pitch angles, the biases that affect the accelerometer's and the gyroscope's measurements, the magnitude of the gravitational acceleration and the extrinsic camera-IMU calibration.

Then, the chapter analyzes the observability properties when the number of inertial sensors is reduced. This analysis has never been provided before. The main result achieved in this context is that the observability properties of visual inertial structure from motion do not change by removing all the three gyroscopes and one of the accelerometers. In other words, exactly the same properties hold when the sensor assembling only consists of a monocular camera and two accelerometers. By removing a further accelerometer (i.e., by considering the case of a monocular camera and a single accelerometer) the system loses part of its observability properties. In particular, a new symmetry arises that corresponds to an internal rotation around the accelerometer axis. This means that some of the internal parameters that define the extrinsic camera calibration, are no longer observable. Although this symmetry does not affect the observability of the absolute scale and the magnitude of the velocity, it reflects in an indistinguishability of all the initial speeds that differ for a rotation around the accelerometer axis. On the other hand, as the camera is extrinsically calibrated (i.e., as the relative transformation between the camera frame and the accelerometer frame is known) this symmetry disappears and the system gains again full observability, as in the case of three orthogonal accelerometers and gyroscopes. This contribution is very important since allows us to reduce the number of inertial sensors. Additionally, it provides a new perspective in the framework of neuroscience to the process of vestibular and visual integration for depth perception and self motion perception.

To analyze these systems with a reduced number of inertial sensors, the chapter introduces a new methodology to derive the observability properties of a system when part of its input controls are unknown. To the best of our knowledge, this methodology is also an original contribution in the framework of control theory. It is an extension of the theory developed by Herman and Krener [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] to investigate the observability properties of a control system when all or part of its input controls are unknown. Indeed, the theory developed in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] always assumes that all the inputs of the system are known.

The chapter is articulated as follows. Section 2.1 provides the state of the art. The system and its basic equations are provided in section 2.2. Section 2.3 reminds the reader some basic concepts in non linear observability. In particular, it provides the main results introduced in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] and [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. Then, section 2.4 provides an observability analysis for the standard formulation, i.e., when all the input controls are known. Section 2.5 contains the extension of the theory in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] to investigate the observability properties of a control system when all or part of its input controls are unknown. Finally, section 2.6 contains the new results about the observability properties when the number of inertial sensors is reduced. This includes the case of a single accelerometer.

State of the art

In [START_REF] Hesch | Towards consistent vision-aided inertial navigation[END_REF][START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF][START_REF] Kottas | On the consistency of vision-aided inertial navigation[END_REF][START_REF] Li | Improving the accuracy of EKF-based visual inertial odometry[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] Mirzaei | A Kalman filter-based algorithm for IMUcamera calibration: Observability analysis and performance evaluation[END_REF] and [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF] the observability properties of the VI-SfM have been investigated. In [START_REF] Hesch | Towards consistent vision-aided inertial navigation[END_REF][START_REF] Kottas | On the consistency of vision-aided inertial navigation[END_REF] and [START_REF] Li | Improving the accuracy of EKF-based visual inertial odometry[END_REF] the authors investigated the estimator inconsistency in the VI-SfM problem. In [START_REF] Hesch | Towards consistent vision-aided inertial navigation[END_REF] the authors found that standard EKF-based estimators lead to spurious information gain along unobservable directions. They also suggested a modification on the basic estimator to enforce the unobservable directions and thus to reduce inconsistency. In [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF][START_REF] Mirzaei | A Kalman filter-based algorithm for IMUcamera calibration: Observability analysis and performance evaluation[END_REF] and [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF] the observability properties have been derived by accounting an unknown transformation between the camera and the IMU frames and an unknown magnitude of the gravity. Additionally, in [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF] and [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF] also the case of biased inertial measurements has been considered. We remark that in [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF] and [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF] the observability properties have been derived starting from basic results in computer vision. Specifically, in [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF] and [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF], starting from the results derived in [START_REF] Chiusoa | Structure from Motion Causally Integrated Over Time[END_REF], a global frame has been fixed by constraining three directions determined by three points on the image plane. In [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF], the camera is considered as a localization sensor up to a scale. This is based on the assumption that the camera is observing a number of features (at least five [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF]), which guarantees that its motion can be reconstructed up to a scale. This significantly simplifies the observability analysis since, the expression of the observation provided by the camera consists of three components of the state which defines the system. In [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] the observability properties have been derived without using the previous mentioned results from computer vision and this allowed us to deal with the case when a single point feature is observed by the camera. The analysis was based on the concept of continuous symmetry introduced in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. Since under these conditions the camera observation has an expression much more complex, the analysis in [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] was limited to the case when the camera extrinsic calibration in the IMU frame is a priori known.

The System

We consider a system (from now on we call it the vehicle) consisting of a monocular camera and an Inertial Measurement Unit (IMU). The IMU consists of three orthogonal accelerometers and three orthogonal gyroscopes. We introduce a global frame to characterize the motion of the vehicle moving in a 3D environment. Its z-axis points vertically upwards. We will adopt lower-case letters to denote vectors in this frame (e.g., the gravity is g = [0, 0,g] T , where g is the magnitude of the gravitational acceleration). We define the vehicle local frame as the IMU frame. We will adopt upper-case letters to denote vectors in the vehicle frame. The camera frame differs from the local frame. We characterize the transformation between these two frames through R c and q c , where R c ≡ [X c , Y c , Z c ] T is the position of the camera optical center in the local frame and q c ≡ q c t + q c x i + q c y j + q c z k is the unit quaternion that characterizes the orientation of the camera frame in the local frame. We assume that both R c and q c are independent of time and are unknown.

The IM U provides the vehicle angular speed and acceleration. We will denote the measured quantities by Ω and A, respectively. These quantities differ from the true values, Ω true and A true . Regarding the angular speed, the one measured by the gyroscopes includes a bias and a zero-mean error, i.e.: Ω = Ω true + Ω bias + n Ω . Regarding the acceleration, the one measured by the accelerometers includes the inertial acceleration (A inertial ), the gravitational acceleration (G), a bias and a zero-mean error. In other words:

A = A inertial + A bias -G + n A .
Note that the gravity comes with a minus since, when the vehicle does not accelerate (i.e. A inertial is zero), the accelerometers perceive an acceleration that is the same of an object accelerated upward in absence of gravity.

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 in the global frame, v is its time derivative, i.e. the vehicle speed in the global frame (v ≡ dr dt ) and q ≡ q t + q x i + q y j + q z k is the unit quaternion that characterizes the vehicle orientation in the global frame.

In the following, we want to derive the analytical expression of the dynamics and the camera observations. For the sake of simplicity, we consider the case of noiseless measurements. The case with noise can be easily obtained with the substitution A → A + n A and Ω → Ω + n Ω . The dynamics of the previous state 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 + w x i + w y j + w z k. The dynamics of the state [r q , v q , q] T are:

         ṙq = v q vq = qA inertial q q * = qA q q * -qA bias q q * + g q q = 1 2 qΩ q - 1 2 qΩ bias q (2.1)
being q * the conjugate of q, q * = q tiq xjq ykq z . By considering the case of unknown biases, unknown magnitude of the gravity and unknown transformation between the IMU and the camera frames, the state that defines our system becomes the following 24-dimensional vector:

X ≡ r, v, q, A bias , Ω bias , R c , q c , g T (2.2)
whose dynamics are given in (2.1) with the following trivial additional equations:

Ȧbias = Ωbias = Ṙc = [0 0 0] T ġ = qc = 0 (2.3)
Note that this is the state that defines our system when a single point feature is observed by the camera. In this case the origin of the global frame can be chosen as coincident with the observed feature. In the case of multiple features, the state dimension becomes 24 + 3(N f -1) (N f being the number of observed features) and the coordinates of the further N f -1 features are included in the state (see [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] for more details). On the other hand, the state defined in (2.2) is not a suitable choice to characterize our system. Indeed, the expression of the camera observations in terms of it involves the product of five quaternions: (q c ) * q * r q qq c . This makes impossible to efficiently derive the observability properties. To have a simple expression of the camera observations it is much more convenient to adopt a new state. Let us refer to the case of a single feature. The new state is:

X n ≡ c F , V , q, A bias , Ω bias , R c , q c , g T (2.4)
where c F ≡ [ c F x , c F y , c F z ] T is the position of the feature in the camera frame and V is the vehicle speed in the local frame (i.e., in the IMU frame). By using the equations in (2.1) we obtain the following dynamics for the new state:

          c Ḟ = M ( c Ω) c F -R q c [V + (Ω -Ω bias ) ∧ R c ] V = M (Ω -Ω bias )V + A -A bias + G q = 1 2 qΩ q - 1 2 qΩ bias q Ȧbias = Ωbias = Ṙc = [0 0 0] T ġ = qc = 0 (2.5)
where:

• M (Ω) ≡   0 Ω z -Ω y -Ω z 0 Ω x Ω y -Ω x 0   ;
• c Ω is the angular speed in the camera frame, i.e., c Ω q = (q c ) * (Ω q -Ω bias q )q c ; • R q c is the rotation matrix associated with the quaternion q c (i.e., for a 3D vector w = [w x , w y , w z ] T , (R q c w) q = (q c ) * w q q c ); • the symbol "∧" denotes the vectorial product. The expression of the camera observations in terms of the new state is trivial. Indeed, the camera provides the direction of the observed feature in its own frame. Hence, it provides the vector c F up to a scale, or, equivalently, the two following ratios:

h cam (X n ) ≡ [h u , h v ] T = c F x c F z , c F y c F z T (2.6)
We have also to consider the two constraints q * q = 1 and (q c ) * q c = 1. These can be dealt as further observations:

h const (X n ) ≡ [h q , h q c ] T = [q * q, (q c ) * q c ] T (2.7)
Finally, the case of multiple features can be characterized by including in the state the position of each feature in the camera frame, i.e., c F →

c F i , i = 1, 2, • • • , N f .
The resulting state has dimension 24+3(N f -1).

Observability analysis

In this section we remind the reader some basic concepts in the theory of observability for non linear systems. For the sake of clarity, we will refer to a simple example. This will allow us to better illustrate these concepts and, in section 2.5.1, to introduce new concepts to deal with non linear systems when part (or even all) of the input controls are unknown.

A simple 2D localization problem

We consider a vehicle moving in a 2D-environment. The configuration of the vehicle in a global reference frame, can be characterized through the vector [x v , y v , θ v ] T where x v and y v are the cartesian vehicle coordinates, and θ v is the vehicle orientation. We assume that the dynamics of this vector satisfy the unicycle differential equations:

   ẋv = v cos θ v ẏv = v sin θ v θv = ω (2.8)
where v and ω are the linear and the rotational vehicle speed, respectively, and they are the system input controls. In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] we considered the same system. In that case, we assumed that the vehicle was equipped with a range sensor able to evaluate the distance from the origin (for instance, this can be obtained by fixing at the origin a marker that can be detected by the range sensor). Here, we assume that the vehicle is equipped with a GPS able to provide its position, i.e.:

z = [x v , y v ] T (2.9)
Our system is characterized by the previous two equations: the former describes its dynamics, the latter its observations. As the majority of real control systems, the dynamics given in (2.8) are affine in the controls, i.e. they can be written as follows:

Ṡ = f (S, u) = f 0 (S) + nc i=1 f i (S)u i (2.10) with S = [x v , y v , θ v ] T , n c = 2, u = [u 1 , u 2 ] T = [v, ω] T , f 0 (S) = [0, 0, 0] T , f 1 (S) = [cos θ v , sin θ v , 0] T , f 2 (S) = [0, 0, 1] T .
Additionally, our system is characterized by two observation functions (system outputs), which are h x (S) = x v and h y (S) = y v .

Observability rank criterion

This criterion was introduced in 1977 by Hermann and Krener [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF], to investigate the observability properties of a non linear system which satisfies (2.10) and with one or more outputs (observations). It requires to compute the Lie derivatives of all the observation functions with respect to all the vector fields f 0 (S), f 1 (S), • • • , f nc (S). The Lie derivatives are defined recursively. The zero order Lie derivative of a given observation function h(S) is the function itself, i.e., L 0 h ≡ h. Then, the (k+1)order Lie derivative of the observation function h(S) with respect to

f i 1 (S), • • • , f i k (S), f i k+1 (S) (with i 1 , i 2 , • • • , i k+1 = 0, 1, • • • , n c ) is L k+1 i 1 ,••• ,i k ,i k+1 h ≡ ∇ S L k i 1 ,••• ,i k h • f i k+1 (S).
Note that this operation is not commutative with respect to the indexes's order. Hence, for a given observation function, we have (n c + 1) k k-order Lie derivatives.

Let us denote with Ω, the space of all the Lie derivatives up to the k-order and with ∇Ω the vector space spanned by the gradients of these functions. In this notation, the observability rank criterion can be expressed in the following way: The dimension of the largest observable sub-system at a given S is equal to the dimension of ∇Ω1 . As a consequence, if for a given k-order the dimension of ∇Ω is equal to the dimension of S, the state S is observable and it is not necessary to compute higher order Lie derivatives.

Let us apply the observability rank criterion to our example. The system has two outputs: h x ≡ x v and h y ≡ y v . By definition, they coincide with their zero-order Lie derivatives. Their gradients with respect to the state S are, respectively: [1, 0, 0] T and [0, 1, 0] T . Hence, the space spanned by the zero-order Lie derivatives has dimension two. In particular, by considering only the zero-order Lie derivatives, we can only conclude that the first 2 state components are observable. We do not know whether the third component, i.e., the vehicle orientation, is observable or not. Let us compute the first order Lie derivatives. We obtain:

L 1 1 h x = cos θ, L 1 1 h y = sin θ, L 1 2 h x = L 1 2 h y = 0.
Hence, the space spanned by the Lie derivatives up to the first order span the entire configuration space and we conclude that also the vehicle orientation is observable.

Observability Properties for the Standard Problem

In [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] we investigated the observability properties of the VI-SfM problem in several contexts which include the case of a single and multiple features, the case of known and unknown magnitude of the gravity, the case of biased and unbiased inertial measurements. In all the considered cases, the extrinsic camera-IMU transformation was assumed known. In the case of a single feature, the state adopted to characterize the case of biased inertial measurements and unknown magnitude of the gravity was r, v, q, A bias , Ω bias , g T , whose dimension is 17. The results of the observability analysis carried out in [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] can be summarized as follows:

Theorem 1 (Known extrinsic calibration) Let us consider the VI-SfM problem with biased inertial measurements. Additionally, the magnitude of the gravity is unknown while the camera-IMU transformation is known. All the independent observable modes are: the positions in the local frame of all the observed features, the 3 components of the speed of the vehicle in the local frame, the two biases affecting the accelerometer and gyroscope measurements, the roll and the pitch angle and the magnitude of the gravity.

The derivation of this result required to analytically compute the Lie derivatives up to the third order. In general, the complexity of the computation of the Lie derivatives and the determination of their dependence or independence dramatically depends on the state dimension.

Let us consider now the case when the extrinsic camera-IMU transformation is unknown. To solve the structure from motion we also need to estimate the parameters that characterize this transformation. In other words, the state that defines our system, is the one given in (2.2) or in (2.4), for the case of a single feature. Let us use the state in (2.4). Our system is characterized by the equations (2.5), (2.6) and (2.7). Also this system is characterized by the general formulation given in (2.10) with S = X n , n c = 6 and u

= [u 1 , u 2 , • • • , u nc ] T = [A x , A y , A z , Ω x , Ω y , Ω z ] T . The expression of the vector fields f i , i = 0, 1, • • • , n c ,
which is necessary for the computation of the Lie derivatives, can be obtained by a direct computation. For instance, the analytical expression of f 0 is obtained by setting

A x = A y = A z = Ω x = Ω y = Ω z = 0 in (2.5).
In this case we found prohibitive to analytically deal with secondorder Lie derivatives. Specifically, by using the symbolic computation tool of Matlab running on a 2.7GHz dual-core Intel Core i7 processor with 4MB shared L3 cache, the time demanded to compute the rank of the matrix whose lines are the gradients of all the Lie derivatives up to the second order, is equal to 101734s and the analytical determination of its null space required 127683s. One of the goals of this chapter is to show how the theory developed in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] can be used to deal with such complex cases, namely, how the observability properties can be derived by avoiding the computation of high-order Lie derivatives and how the space dimensionality where the rank must be computed can be reduced.

Quasi-Projection Operation and Matrix

In this section we introduce new theoretical results that allow significantly reducing the load of symbolic computation requested to derive the observability properties of a given system starting from the analysis of its continuous symmetries. Throughout this section, we denote by n the dimension of the state. We start by reminding the reader the definition of continuous symmetry and the basic result derived in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], which characterizes the observable modes.

Definition 1 (Continuous Symmetry) The vector field w s (S) is a continuous symmetry in S for the system defined in (2.10) if and only if it is a non null vector belonging to the null space of the matrix whose lines are the gradients of all the Lie derivatives computed in S.

Property 1 g(S) is an observable mode if and only if its gradient is orthogonal to all the symmetries.

According to this result, the derivation of all the observable modes requires, first of all, to derive all the symmetries. The following remark provides an upper bound on the number of Lie derivatives that must be computed to detect the system symmetries: Remark 1 To detect the symmetries of the system in (2.10), only the first (n -1)-order Lie derivatives must be computed.

The reader is referred to [START_REF] Anguelova | Non linear Observability and Identifiability: General Theory and a Case Study of a Kinetic Model[END_REF] to verify the validity of this remark. On the other hand, this remark is not useful to deal with our case since it is prohibitive even to deal with the second-order Lie derivatives.

Let us denote with

φ 1 , • • • , φ n ′ (1 ≤ n ′ < n) a set
of independent column vectors of dimension n that are orthogonal to the gradients of nn ′ independent Lie derivatives. We have the following result: Property 2 A sufficient condition for g(S) to be an observable mode is that its gradient is orthogonal to all the vectors

φ 1 , • • • , φ n ′ . Proof. According to definition 1, if n -n ′ is the number of all the independent Lie derivatives, φ 1 , • • • , φ n ′
are all the symmetries of the system. In this case, as stated in property 1, the previous condition is also necessary. When nn ′ is smaller than the number of all the independent Lie derivatives, we only know that the vector space generated by the symmetries is a subspace of the space generated by φ 1 , • • • , φ n ′ . Hence, if the gradient of g(S) is orthogonal to this latter vector space it is also orthogonal to any of its subspace. From property 1 we conclude that g(S) is an observable mode This result is useful since it can be used by computing any number of independent Lie derivatives and not necessarily all of them. Additionally, this simple result allows us to reduce the system dimensionality in the special case when a component of the state verifies the sufficient condition stated in the previous property. In this case, since we know that this component is observable, to detect the other observable modes we can consider the reduced system that is obtained by removing this component from the state.

The second new result allows us to determine the dependence or independence of a given set of Lie derivatives by computing the rank of matrices whose dimension is smaller than the dimension of the original state. Let us suppose that we detected n-n ′ independent Lie derivatives

(Q 1 , • • • , Q n-n ′ ) and, as before, let us denote with φ 1 , • • • , φ n ′ (1 ≤ n ′ < n) a set
of independent column vectors orthogonal to their gradients. Let us suppose that we compute m additional Lie derivatives (P 1 , • • • , P m ). We need to determine if they are independent from the previous ones and also if they are independent one each other. In other words, we want to know the number of independent Lie derivatives in the set

Q 1 , • • • , Q n-n ′ , P 1 , • • • , P m .
The easiest way to determine this is to compute the gradient of all these functions and then the rank of the matrix that contains these gradients. The dimension of this matrix is (nn ′ + m) × n. We know that this rank is larger or equal to nn ′ since we assumed that Q 1 , • • • , Q n-n ′ are independent. Let us denote this rank by n-n ′ +p. We remark that determining p is very easy if the vectors φ 1 , • • • , φ n ′ are orthonormal. In this special case, it is immediate to project the gradients of the Lie derivatives

(P 1 , • • • , P m ) onto the vectors φ 1 , • • • , φ n ′ .
This allows us to reduce the space dimensionality: instead of working in the original n-dimensional space we can work in a n ′ -dimensional space. In other words, p is the rank of the m × n ′ matrix, whose lines are the projections of the gradients of the Lie derivatives (P 1 , • • • , P m ) onto the vectors φ 1 , • • • , φ n ′ . On the other hand, even in the case when the vectors φ 1 , • • • , φ n ′ are not orthonormal, p can be computed by computing a rank of a m × n ′ matrix. We have the following result: Property 3 p is the rank of the m × n ′ matrix, whose i, j entry is the scalar product ∇ S P i • φ j .

Proof. In this proof we will use the following two results from linear algebra about the matrix rank [START_REF] Mirsky | An Introduction to Linear Algebra[END_REF]:

(1) rank(AB) = rank(A) when rank(B) = k, where k is the num- ber of lines of B; (2) If A is a matrix over the real numbers, rank(AA T ) = rank(A T A) = rank(A) = rank(A T ).
Let us introduce the following notation:

• D is the (n -n ′ ) × n matrix whose lines are the gradients of the functions Q 1 , • • • , Q n-n ′ , i.e.: D ≡   ∇ S Q 1 • • • ∇ S Q n-n ′  
• E is the m × n matrix whose lines are the gradients of the functions P 1 , • • • , P m , i.e.:

E ≡   ∇ S P 1 • • • ∇ S P m  
• M is the (n-n ′ +m)×n matrix whose lines are all the previous gradients, i.e.:

M ≡ D E • F is the n × n ′ matrix whose columns are the vectors φ 1 , • • • , φ n ′ ; • N is the following n × n matrix: N ≡ D T F
We have: nn ′ + p = rank(M). Since rank(N ) = n, by using the first property mentioned at the beginning of this proof, we also have rank(M) = rank(MN ). We have:

MN = DD T 0 (n-n ′ )×n ′ ED T EF (2.11)
where 0 (n-n ′ )×n ′ is the zero (nn ′ ) × n ′ matrix. Because of the second property mentioned at the beginning, rank(DD

T ) = rank(D), which is n -n ′ , because of the independence of Q 1 , • • • , Q n-n ′ .
To compute the rank of the matrix in (2.11), we detect the largest number of independent columns. Let us consider any linear combination of its columns that includes all the first nn ′ columns. A necessary condition to be the null vector is that the first nn ′ coefficients are equal to zero. Hence, a necessary and sufficient condition such that this linear combination vanishes if and only if all the coefficients are equal to zero, is that the columns selected among the last m columns of the matrix MN are independent among them. This proves that rank(MN ) = nn ′ + rank(EF). But the i, j entry of EF is precisely the scalar product

∇ S P i • φ j
This result is very important since allows us to verify the independence of a set of Lie derivatives by computing the rank of matrices with low dimension. Specifically, the gradients of the Lie derivatives are n-dimensional vectors. Once we have detected nn ′ independent Lie derivatives and a basis for its null space (i.e., φ 1 , • • • , φ n ′ ), to detect further independent Lie derivatives, we can work with n ′ -dimensional vectors. Indeed, for a given Lie derivative P, instead of working with the n-dimensional vector ∇ S P, we can work with the n ′ -dimensional vector, which is defined as the vector whose j th entry is ∇ S P • φ j (j = 1, • • • , n ′ ). This operation would be a projection if the basis φ 1 , • • • , φ n ′ were orthonormal. For this reason we will call this operation a quasi-projection. This result is very powerful since it can be applied more consecutive times. Specifically, let us suppose that, by quasi-projecting the gradients of additional Lie derivatives in the n ′ -dimensional space we detect additional n ′n ′′ independent Lie derivatives (1 ≤ n ′′ < n ′ ). Let us denote with φ ′1 , • • • , φ ′n ′′ a basis of the null space of the matrix made by these gradients quasi-projected in the n ′ -dimensional space. Now, to work in the new n ′′ -dimensional space, we must quasi-project the gradient of a given Lie derivative, first on the n ′ -dimensional space and then on the n ′′ -dimensional space. By computing the matrix whose i, k entry is j φ j i φ ′k j we can simply multiply the gradient of the Lie derivative by this matrix. We we will call this matrix the quasi-projection matrix. Therefore, each time we apply the result in property 3, we need to compute the new quasiprojection matrix.

VI-SfM Observability

We illustrate the power of the previous two properties by referring to our case, where the original state has dimension n = 24. We remind the reader that the time demanded to compute the rank of the matrix whose lines are the gradients of all the Lie derivatives up to the second order, is equal to 101734s. We start by considering the matrix whose lines are the gradients of the following Lie derivatives:

L 0 h u , L 0 h v , L 0 h q , L 0 h p , L 1 5 h u , L 1 6 h u , L 1 6 h v .
The computation of its rank and its null space demands 0.37s and 0.24s, respectively. In particular, the rank is 7 proving that these functions are independent. In a second step we quasi-project the gradient of the following Lie derivatives: L 1 0 h u and L 1 4 h u . We build with them a 2 × 17 matrix. The computation of its rank and null space requires 0.027s and 3.11s, respectively. The rank is 2. The computation of the quasi-projection matrix requires 6.48s. In a third step we quasi-project the gradient of the following Lie derivatives: L 1 4 h v and L 1 5 h v . We build with them a 2 × 15 matrix. The computation of its rank and null space requires 64s and 81s, respectively. The rank is 2. The computation of the quasi-projection matrix requires 0.34s. Finally, we quasi-project the gradient of the Lie derivative L 1 0 h v . We build with it a 1 × 13 matrix. The computation of its rank and null space requires 0.041s and 0.089s, respectively. The computation of the quasi-projection matrix requires 0.44s. We can proceed further to detect all the independent Lie derivatives. However, it is more convenient to use property 2. Specifically, we can check if some of the components of the state are observable (property 2 is a sufficient condition). We compute the gradient of the components of the original state and we quasi-project them on the final 12-dimensional space. Regarding the four components of the quaternion q c we obtain the null vector, meaning that these components are four observable modes. Hence, we remove these coponents from the original state and we start again the observability analysis of a new system whose state has now dimension equal to 20. As previously mentioned, the complexity of the computation dramatically depends on the dimension of the state. Specifically, with the new state, it is possible to work with third-order Lie derivatives. In particular, we detect the following 19 independent Lie derivatives:

L 0 h u , L 0 h v , L 0 h q , L 1 0 h u , L 1 5 h u , L 1 6 h u , L 1 0 h v , L 1 6 h v , L 2 00 h u , L 2 01 h u , L 2 04 h u , L 2 05 h u , L 2 06 h u , L 2 00 h v , L 2 04 h v , L 3 400 h u , L 3 004 h u , L 3 005 h u , L 3 006 h v .
The computation of the null space of the matrix whose lines are their gradients requires 0.46s and provides the vector:

w Rotz s = [0 1×6 , -q z , -q y , q x , q t , 0 1×10 ] T
where 0 i×j denotes the i × j zero matrix. This vector expresses the system invariance under rotations around the vertical axis. Indeed, an infinitesimal rotation of magnitude ǫ about the vertical axis leaves the vectors c F , V , A bias , Ω bias and R c unchanged and the quaternion q changes as follows [START_REF] Goldstein | Classical Mechanics[END_REF]:

     q t q x q y q z      →      q t q x q y q z      + ǫ 2      -q z -q y q x q t      (2.12) i.e., the reduced 20-dimensional state X r ≡ c F , V , q, A bias , Ω bias , R c , g T changes as follows: X r → X r + ǫ 2 w Rotz s
. Hence, w Rotz s is a continuous symmetry for our system and the only non-observable mode is the yaw angle. In other words, we extended the result in theorem 1 to the case of unknown camera-IMU calibration. We have the following new result: Theorem 2 (Unknown extrinsic calibration) Let us consider the VI-SfM problem with biased inertial measurements. Additionally, both the magnitude of the gravity and the camera-IMU transformation are unknown. All the independent observable modes are: the positions in the local frame of all the observed features, the three components of the speed of the vehicle in the local frame, the two biases affecting the accelerometer and gyroscope measurements, the roll and the pitch angle, the magnitude of the gravity and the transformation between the camera and IMU frames.

Note that this result holds even in the case when the camera observes a single point-feature.

Extension of the Theory of Herman and Krener

This section introduces a new methodology to derive the observability properties of a non linear system when all or part of its input controls are unknown. As it is common, we will refer to non linear systems whose dynamics are affine in the controls, i.e., they can be written as in the equation (2.10). Additionally, we will refer to the case when the observation (system output) is a scalar function of the state, i.e., z = h(S). The theory of the observability is based on the assumption that both the system inputs (i.e., u i in (2.10), i = 1, • • • , n c ) and the system output (i.e., z) are known during a given time interval. This is a basic assumption. Specifically, the observability rank criterion introduced in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] and used in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] to define the concept of continuous symmetry, is based on this assumption. To extend the observability rank criterion, let us focus on the main steps in the theory introduced in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]. Let us denote with [T in , T f in ] the interval of time where the functions u i (t)

(i = 1, • • • , n c ) and z(t) = h(S(t)) are known.
The observability rank criterion is obtained by proceeding with the following three steps:

(1) The Taylor's theorem allows us to state that the knowledge of the functions u i (t

) (i = 1, • • • , n c ) and z(t) for t ∈ [T in , T f in ]
is equivalent to the knowledge of any order time derivative for t = T in of the functions

u i (i = 1, • • • , n c ) and z; (2
) the values of all the Lie derivatives of the function h(S) in S(T in ) along all the directions f i (S), i = 0, 1, • • • , n c can be obtained by inverting a linear system whose coefficients and constant terms are given by the previous time derivatives; (3) the inverse function theorem allows us to identify whether the vector S(T in ) can be obtained starting from the knowledge of the Lie derivatives (which are non linear scalar functions of this vector).

The goal of this section is to extend the observability rank criterion to deal with the case when all or part of the n c input controls of our system are unknown. In this case, we do not know some of the functions

u i (t) (i = 1, • • • , n c ) t ∈ [T in , T f in ].
Let us denote with n k and n u the number of known and unknown input controls, respectively. We have n k + n u = n c . Additionally, we order the inputs such that the known input are the first n k . In other words, u i (t

) (i = 1, • • • , n k ) are known for any t ∈ [T in , T f in ], while u i (t) (i = n k + 1, • • • , n c ) are unknown.
Hence, the time derivatives of u i (t

) (i = n k + 1, • • • , n c
) are not available and the second step mentioned above cannot be used to obtain the Lie derivatives. Our basic idea consists in modifying the original state to be able to select some Lie derivatives, which can be obtained even without knowing all the time derivatives of u i (t

) (i = n k + 1, • • • , n c ) at t = T in .
This allows us to obtain a sufficient condition for the state observability and, by using the theoretical results derived in section 2.4, we obtain sufficient conditions to check whether a given mode is observable or not. We will call the new criterion the extended observability rank criterion. It will be introduced in 2.5.1. Then, to better illustrate the proposed methodology, we consider again the simple localization problem discussed in 2.3.1.

Extended observability rank criterion

Let us refer to the non linear system described by equation (2.10) and a given observation function z = h(S). It is possible to analytically derive the expression of the n th time derivative of the observation function in t = T in in terms of all the Lie derivatives of the function h along all the directions f i (S), i = 0, 1, • • • , n c computed in S 0 ≡ S(T in ) up to the n th order and all the time derivatives of the functions

u i (i = 1, • • • , n c ) computed in t = T in .
We have:

d n h(S(t)) dt n t=T in = (2.13) = n p=1 nc i 1 i 2 •••ip=0 L p i 1 i 2 •••ip h(S 0 ) n-p k 1 ,k 2 ,••• ,kp=0, | p j=1 k j =n-p C n, p k 1 ,k 2 ,••• ,kp u (k 1 ) i 1 • • • u (kp) ip
where:

• u (k) i ≡ d k u i dt k , k = 0, 1, • • • , n; i = 1, • • • , n c ; • u 0 ≡ 1 and u (k) 0 = 0, k > 1; • C n, p k 1 ,k 2 ,•
C n+1, p k 1 ,k 2 ,••• ,kp = C n, p-1 k 1 ,k 2 ,••• ,k p-1 + C n, p k 1 -1,k 2 ,••• ,kp + C n, p k 1 ,k 2 -1,••• ,kp + • • • + C n, p k 1 ,k 2 ,••• ,k p-1 -1,kp k p = 0 C n, p k 1 -1,k 2 ,••• ,kp + C n, p k 1 ,k 2 -1,••• ,kp + • • • + C n, p k 1 ,k 2 ,••• ,k p-1 -1,kp k p = 0 ,
which is obtained by directly differentiating the expression in (2.13) with respect to time.

The expression in (2.13) allows us to perform the second step mentioned above, i.e., it allows us to obtain the Lie derivatives of h starting from the knowledge of the time derivatives of the system inputs and output by inverting a linear system. When n u inputs are unknown, this step cannot be performed starting directly from (2.13). We split the expression in (2.13) as follows:

d n h(S(t)) dt n t=T in = n p=1 (2.14)      n k i 1 i 2 •••ip=0 L p i 1 i 2 •••ip h(S 0 ) n-p k 1 ,k 2 ,••• ,kp=0, | p j=1 k j =n-p C n, p k 1 ,k 2 ,••• ,kp u (k 1 ) i 1 • • • u (kp) ip + + i 1 i 2 •••ip=remaining L p i 1 i 2 •••ip h(S 0 ) n-p k 1 ,k 2 ,••• ,kp=0, | p j=1 k j =n-p C n, p k 1 ,k 2 ,••• ,kp u (k 1 ) i 1 • • • u (kp) ip     
The first sum only contains the know controls (i.e., i

1 , i 2 , • • • , i p = 0, 1, • • • n k )
while, for each addend in the second sum, i.e., the sum where the indexes i 1 , i 2 , • • • , i p take the remaining values, at least one control is unknown. In the special case when all the Lie derivatives

L p i 1 i 2 •••ip h(S 0
) vanish when at least one index i 1 , i 2 , • • • , i p is larger than n k , the second sum, which also contains unknown controls, vanishes as well. Hence, the expression in (2.14) can still be used to obtain all the Lie derivatives and the observability rank criterion can still be adopted. Obviously, this is a very special case. Our idea is to extend the original state to artificially reproduce such a situation. In particular, we include the unknown inputs in the state together with their time derivatives. By including the time derivatives up to the n th order, we will obtain

L p i 1 i 2 •••ip h(S 0 ) = 0 when at least one index i 1 , i 2 , • • • , i p is larger than n k and for all p = 0, 1, • • • , n.
Let us illustrate this by referring to the case when n k = n c -1 (and, consequently, n u = 1). Obviously, the zero order Lie derivative can be obtained without the necessity to know the inputs (it is trivially the output at t = T in , L 0 h(S 0 ) = h(S(T in ))). Let us consider the first order time derivative (n = 1). The expression in (2.14) becomes: where

dh(S(t)) dt t=T in = nc-1 i=0 L 1 i h(S 0 )u i + L 1 nc h(S 0 )u nc (2.
• f e 0 (S e ) ≡ [f 0 (S) T + f nc (S) T u nc , 0] T ; • f e i (S e ) ≡ [f i (S) T , 0] T , i = 1, • • • , n c -1; • f e nc (S e ) ≡ [0, 1]
T , with 0 the line vector whose entries are all zero and whose dimension is equal to the one of S T ;

• u e i ≡ u i , i = 1, • • • , n c -1; • u e nc ≡ u (1)
nc = unc .
It is immediate to realize that the first order Lie derivative along the direction f e nc (S e ) is identically zero. This allows us to obtain all the other first order Lie derivatives. By including in the state also the first time derivative of u nc (namely, u

nc ) we can obtain all the Lie derivatives, up to the second order, along the first n c -1 directions. By including higher order time derivatives of the unknown input control u nc we can obtain higher order Lie derivatives along the first n c -1 directions. At this point, the third step in the Herman & Krener theory previously mentioned can be performed by using the Lie derivatives that are available. By analyzing these Lie derivatives it is possible to detect potential symmetries according to the theory developed in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] and property 3 provides a sufficient condition for a given mode to be observable.

Observability of the system in 2.3.1 with unknown input controls

We illustrate the methodology introduced in the previous section to investigate the observability properties of the simple system introduced in 2.3.1 when part, or all, of the input controls are unknown. We already know that the state S = [x v , y v , θ v ] T is observable when all the input controls are known (i.e., when the functions v(t) and ω(t) are known for any t ∈ [T in , T f in ]). Intuitively, we know that the knowledge of both the inputs is unnecessary to have the full observability of the entire state. Indeed, the first two state components can be directly obtained from the GPS. By knowing these two components in a given time interval, we also know their time derivatives. In particular, we known ẋv (T in ) and ẏv (T in ). From (2.8) we easily obtain: θ v (T in ) = atan ẏv(Tin) ẋv(Tin) . Hence, also the initial orientation is observable, by only using the GPS measurements.

We want to show that, by applying the methodology introduced in the previous section, we obtain exactly the same result. We start by including in the original state the unknown v, i.e., S e = [x v , y v , θ v , v] T . We obtain:

      ẋv = v cos θ v ẏv = v sin θ v θv = ω v = v (1)
(2.17)

We have:

n c = 2, u = [u e 1 , u e 2 ] T = [ω, v (1) ] T , f e 0 (S e ) = [v cos θ v , v sin θ v , 0, 0] T , f e
1 (S e ) = [0, 0, 1, 0] T , f e 2 (S e ) = [0, 0, 0, 1] T . The first order Lie derivatives are:

L 1 0 h x (S e ) = v cos θ v , L 1 1 h x (S e ) = L 1 2 h
x (S e ) = 0. By chance, also L 1 1 h x (S e ) = 0 and we do not need to include also ω in the state. By using (2.14) up to the first order (i.e., n ≤ 1), we can determine L 1 0 h x (S e ). In other words, we can determine v cos θ v . By considering the second observation function (i.e., h y ) we find that we can also determine L 1 0 h y (S e ) = v sin θ v . The gradients of the functions L 0 h x (S e ), L 0 h y (S e ), L 1 0 h x (S e ) and L 1 0 h y (S e ) span the entire configuration space of the state S e meaning that this extended state is observable.

Observability Properties with unknown inputs

We use the methodology described in section 2.5 to analyze the VI-SfM problem when the number of accelerometers and gyroscopes is reduced. In other words, we consider the system defined in section 2.2 when the input vector u

= [u 1 , u 2 , • • • , u 6 ] T = [A x , A y , A z , Ω x , Ω y , Ω z ] T is
partially known, i.e., some of its components are known during a given time interval and the others are totally unknown. Specifically, we analyze three different cases:

(1) camera extrinsically calibrated and n k = 1 (i.e., the inertial sensors only consist of a single accelerometer); (2) camera extrinsically uncalibrated and n k = 2 (i.e., the inertial sensors only consist of two accelerometers); (3) camera extrinsically uncalibrated and n k = 1.

For the sake of simplicity, in the last two cases we do not consider the extreme case of a single feature. In particular, we assume that the camera is able to provide its position, orientation and speed up to a scale. This significantly reduces the computational load.

Single Accelerometer and Camera extrinsically calibrated

We directly analyze the extreme case of a single point feature. In the case of three orthogonal accelerometers and gyroscopes and unknown camera-IMU extrinsic calibration, the state that characterizes the vehicle was given in (2.4). Here, we assume that the camera is extrinsically calibrated. Without loss of generality, we assume that the camera and the IMU frame coincide (i.e., R c = [0 0 0] T and q c = 1) and that the single accelerometer points towards the z-direction of the camera frame. The state that characterizes this system is the following 12-dimensional vector:

X c 1 ≡ F , V , q, A bias z , g T (2.18)
where we adopt the subscript 1 to denote the case of a single accelerometer and the apex c to denote the case of a calibrated camera. F is the position of the feature in the local frame, V is the vehicle speed in the local frame, q the unit quaternion that describes the orientation of the local frame in the global frame, A bias z the bias of the accelerometer and g the magnitude of the gravity. The dynamics of this state are obtained by using (2.5):

        Ḟ = M (Ω)F -V V = M (Ω)V + A -A bias + G q = 1 2 qΩ q ġ = Ȧbias z = 0 (2.19)
where the matrix M is defined in section 2.2, Ω is the unknown angular speed of the camera, A bias = [0 0 A bias z ] T and A -A bias is the camera acceleration in the local frame, whose first two components are unknown and the third component is known up to the bias tanks to the accelerometer.

From (2.6) we easily obtain the expression of the camera observations:

h cam (X c 1 ) ≡ [h u , h v ] T = F x F z , F y F z T (2.20)
We have also to consider the constraint q * q = 1. This provides the further observation:

h const (X c 1 ) ≡ h q = q * q (2.21)
The dynamics in (2.19) provide seven independent directions along which the Lie derivatives can be computed. On the other hand, only two directions are available. They are the vector f 0 (X c 1 ), which is obtained by setting Ω = A = [0 0 0] T in (2.19) and the vector f 3 (X c 1 ), which is obtained by setting Ω = [0 0 0] T and A = [0 0 1] T in the dynamics in (2. [START_REF] Hartley | Multiple view Geometry in Computer Vision[END_REF]), once f 0 (X c 1 ) has been removed. Since the Lie derivatives along the other five directions are not null, we have to proceed as in section 2.5.2. We do this in several subsequent steps. In each step we check, first of all, which highest order of Lie derivatives of the observations can be used. This is obtained by checking that, for a given order, all the Lie derivatives up to this order, computed along at least one of the directions that are not available (i.e., f 1 , f 2 , f 4 , f 5 and f 6 ) are identically zero. Once this highest order is identified, we find the largest number of independent Lie derivatives up to this order. Then, we compute the set of all of vectors that are orthogonal to the gradients of these Lie derivatives. Finally, we apply the property 3 to detect which components of the vector in (2.18) are observable. We include new time derivatives of the unknown inputs (i.e., A x , A y , Ω x , Ω y , Ω z ) to make usable higher order Lie derivatives, as explained in section 2.5.

First step

We start with the 12-dimensional state given by the vector in (2.18). Since the first order Lie derivatives along f 4 , f 5 and f 6 are different from zero both for h u and h v , we can only use zero-order Lie derivatives. On the other hand, the first order Lie derivatives along f 1 and f 2 (and also f 3 ) vanish. Hence, it suffices to include Ω x , Ω y , Ω z in the state to use the Lie derivatives up to the first order. The zeroorder Lie derivatives are the three functions in (2.20, 2.21). They are independent. Hence the system has three observable modes. The set of vectors orthogonal to the gradients of these three functions can be determined. Property 3 does not allow us to prove the observability for any component of the state in (2.18).

Second step

We include Ω x , Ω y , Ω z in the state. The dimension of the new state is 15. We can use all the Lie derivatives up to the first order. We detect the additional independent functions from them: L 1 0 h u and L 1 0 h v . Hence the system has five independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 10 vectors. Again, property 3 does not allow us to prove the observability for any component of the state in (2.18).

Third step

To use the second order Lie derivatives we need to include A x , A y , Ω

x

≡ Ωx , Ω (1) y
≡ Ωy , Ω

≡ Ωz in the state. The dimension of the new state is 20. We can use all the Lie derivatives up to the second order. We detect the following additional independent functions: L 2 00 h u , L 2 03 h u and L 2 00 h v . Hence the system has eight independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 12 vectors. This time, property 3 allows us to conclude that the first three components of the vector in (2.18) (i.e., F ) are observable. On the other hand, it does not allow us to prove the observability for any other component of the state.

Forth step

To use the third order Lie derivatives we need to include also A (1)

x ≡ Ȧx , A (1)
y ≡ Ȧy , Ω (2)
x ≡ Ω(1)

x , Ω

y ≡ Ω(1) y , Ω (2)
z ≡ Ω(1) z in the state. The dimension of the new state is 25. We detect the following additional independent functions: L 3 000 h u , L 3 003 h u , L 3 030 h u , L 3 000 h v and L 3 003 h v . Hence the system has 13 independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 12 vectors. As in the previous step, property 3 allows us to conclude that the first three components of the vector in (2.18) (i.e., F ) are observable. On the other hand, it does not allow us to prove the observability for any other component of the state.

Fifth step

To use the fourth order Lie derivatives we need to include also A (2)

x ≡ Ȧ(1) x , A (2)
y ≡ Ȧ(1) y , Ω (3)
x ≡ Ω(2)

x , Ω

y ≡ Ω(2) y , Ω (3)
z ≡ Ω(2) z in the state. The dimension of the new state is 30. We detect the following additional independent functions: L 4 0000 h u , L 4 0003 h u , L 4 0030 h u , L 4 0300 h u , L 4 0000 h v and L 4 0003 h v . Hence the system has 19 independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 11 vectors. Property 3 allows us to conclude that the first six components of the vector in (2.18) (i.e., both the vector F and V ) are observable. On the other hand, it does not allow us to prove the observability for the other components of the state.

Sixth step

To use the fifth order Lie derivatives we need to include also A

(3) x ≡ Ȧ(2) x , A (3)
y ≡ Ȧ(2) y , Ω (4)
x ≡ Ω(3)

x , Ω

y ≡ Ω(3) y , Ω (4)
z ≡ Ω(3) z in the state. The dimension of the new state is 35. We detect the following additional independent functions: L 5 00000 h u , L 5 00003 h u , L 5 00030 h u , L 5 03000 h u , L 5 00000 h v and L 5 00003 h v . Hence the system has 25 independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 10 vectors. By using property 3 we find the same properties obtained in the previous step.

Seventh step

To use the sixth order Lie derivatives we need to include also A (4)

x ≡ Ȧ(3) x , A (4) y ≡ Ȧ(3) y , Ω (5) x
≡ Ω(4)

x , Ω

≡ Ω(4) y , Ω

≡ Ω(4) z in the state. The dimension of the new state is 40. We detect the following additional independent functions: L 6 000000 h u , L 6 000003 h u , L 6 000030 h u , L 6 030000 h u , L 6 000000 h v and L 6 000003 h v . Hence the system has 31 independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 9 vectors. Again, by using property 3, we find the same properties obtained in the previous step.

Eighth step

To use the seventh order Lie derivatives we need to include also A [START_REF] Bryson | Observability Analysis and Active Control for Airbone SLAM[END_REF] x ≡ Ȧ(4)

x , A

y ≡ Ȧ(4) y , Ω

x ≡ Ω(5)

x , Ω

y ≡ Ω(5) y , Ω (6)
z ≡ Ω(5) z in the state. The dimension of the new state is 45. We detect the following additional independent functions: L 7 0000000 h u , L 7 0000003 h u , L 7 0000030 h u , L 7 0300000 h u , L 7 0000000 h v and L 7 0000003 h v . Hence the system has 37 independent Lie derivatives. The set of vectors orthogonal to the gradients of these functions consists of 8 vectors. By using property 3, we find that the first six components of the vector in (2.18) (i.e., both the vector F and V ) and the last two components of this vector (i.e., the accelerometer bias A bias z and the magnitude of the gravity g) are observable. Regarding the other four components, namely the four components of the quaternion q that characterizes the camera orientation, we detect the same rotation symmetry described by equation (2.12). This means that the roll and the pitch angles are also observable and that the only unobservable mode is the yaw. Since this unobservable mode is a consequence of the system invariance with respect to rotations about the vertical axis, it is useless to include higher order Lie derivatives: the observability properties of the state in (2.18) do not change.

We conclude this section with the following important novel result: Theorem 3 (Single accelerometer, calibrated camera) Let us consider the VI-SfM problem when the inertial sensors only consist of a single accelerometer and no gyroscope. Additionally, the magnitude of the gravity is unknown and the camera-IMU transformation is known. All the independent observable modes are: the positions in the local frame of all the observed features, the three components of the speed of the vehicle in the local frame, the bias affecting the accelerometer measurements, the roll and the pitch angle, the magnitude of the gravity. This holds even in the extreme case of a single point feature.

This result is very important and basically means that, in the case when the camera is extrinsically calibrated, the observability properties of the VI-SfM problem do not change by removing two of the three accelerometers and all the gyroscopes. The information provided by these additional five inertial sensors is redundant.

Two Accelerometers and Uncalibrated Camera

In this section and in the next one we account an unknown camera-IMU calibration. For simplicity sake, we assume that the camera is observing at least five features. This allows us to consider the camera as a sensor able to provide its orientation, its angular speed and its position and speed up to a scale in a global reference frame attached to these features. Obviously, the gravity in this global frame is not necessarily along the vertical axis. We denote this vector by g ≡ [g x , g y , g z ] T , which is unknown (both in magnitude and in direction). Additionally, we denote by 1 µ the unknown absolute scale. Without loss of generality, we assume that the two available accelerometers are along the y and the z-axis in the IMU frame. The state that describes our system is the one given in (2.4), with only two bias components instead of six, since the inertial sensors only consist of two accelerometers. Additionally, we include in the state the position of all the observed N f features in the camera frame (N f ≥ 5). The state is:

X u 2 ≡ c F 1 , • • • , c F N f , V , q, A bias y , A bias z , R c , q c , g, µ T (2.22)
where the subscript 2 denotes two accelerometers and the apex u the fact that we are considering the case of a camera extrinsically uncalibrated. The dynamics of this state can be easily obtained from (2.5).

We have

(i = 1, • • • , N f ):           c Ḟ i = M ( c Ω) c F i -R q c [V + Ω ∧ R c ] V = M (Ω)V + A -A bias + G q = 1 2 qΩ q ġ = Ṙc = [0 0 0] T Ȧbias y = Ȧbias z = qc = μ = 0 (2.23)
where Ω is the unknown angular speed in the IMU frame, A is the acceleration in the IMU frame, whose first component is unknown and A bias = [0, A bias y , A bias z ] T . Since at least five point features are available, the angular speed is known in the camera frame, i.e., the vector c Ω (= R q c Ω) can be obtained from the visual measurements. Additionally, the visual measurements provide 3N f + 9 scalar functions (system outputs). The first 3N f are the components of the vectors

µ c F i , i = 1, • • • , N f ,
which are the positions of the features in the camera frame up to the scale. Hence, we have:

h F i x ≡ µ c F i x ; h F i y ≡ µ c F i y ; h F i z ≡ µ c F i z ; i = 1, • • • , N f (2.24)
Regarding the remaining nine outputs, three of them are the components of the speed in the camera frame up to a scale, i.e.:

h Vx ≡ µ c V x ; h Vy ≡ µ c V y ; h Vz ≡ µ c V z (2.25)
where c V ≡ [ c V x , c V y , c V z ] T = R q c [V + Ω ∧ R c ].
Since the camera provides its orientation in the global frame, also the component of the quaternion qq c can be considered system outputs. We have:

h t ≡ (qq c ) t ; h x ≡ (qq c ) x ; h y ≡ (qq c ) y ; h z ≡ (qq c ) z (2.26)
Finally, both the quaternions q and q c must be unit quaternions. Hence, we have the two outputs:

h q ≡ q 2 t + q 2
x + q2 y + q 2 z ; h q c ≡ (q c t ) 2 + (q c x ) 2 + (q c y ) 2 + (q c z ) 2 ; (2.27)

We start by investigating the observability properties of a simplified system that is obtained by referring to the state in (2.22) with N f = 1 and with the 3 + 9 = 12 outputs given by (2.24) for a single feature and (2. 25-2.27). Note that we are using the four observations in (2.26): this implicitly assumes that we are actually exploiting the camera observations related to at least five features, simultaneously. The dynamics in (2.23) provide seven independent directions along which the Lie derivatives can be computed. On the other hand, one of these directions is not available. This is the vector

f 1 (X u 2 ) that is obtained by setting c Ω = [0 0 0] T and A = [1 0 0] T in (2.23), once f 0 (X u
2 ) has been removed 2 . Since the Lie derivatives along this direction are not null, we have to proceed as in section 2.6.1. We must proceed in several subsequent steps. In each step we check, first of all, which highest order of Lie derivatives of the observations can be used. This is obtained by checking that, for a given order, all the Lie derivatives up to this order, computed at least once along f 1 , are identically zero. Once this highest order is identified, we find the largest number of independent Lie derivatives up to this order. We include new time derivatives of the unknown inputs (i.e., A x ) to make usable higher order Lie derivatives, as explained in section 2.5. We will show that, by including A x and its first time derivative, we can prove the observability of the entire state.

First step

We start with the 23-dimensional state given by the vector in (2.22), with a single feature. By chance, the first order Lie derivatives of the functions h Fx , h Fy and h Fz along f 1 are null. Regarding the other nine outputs, the first order Lie derivatives along this direction are different from zero. Among the usable Lie derivatives, we detect 14 independent functions that are: L 0 h Fx , L 0 h Fy , L 0 h Fz , L 0 h Vx , L 0 h Vy , L 0 h Vz , L 0 h t , L 0 h x , L 0 h y , L 0 h q , L 0 h q c , L 1 5 h Fx , L 1 6 h Fx and L 1 6 h Fy .

Second step

We include A x in the state. The new state has dimension 24. Now we can use the first order Lie derivatives of all the outputs and the second order Lie derivatives of the first three outputs. We detect seven additional independent Lie derivatives that are:

L 2 00 h Fx , L 2 02 h Fx , L 2 03 h Fx , L 2
00 h Fy , L 2 02 h Fy , L 2 03 h Fy and L 2 00 h Fz . Hence, the total number of independent Lie derivatives that are usable is 21.

Third step

We include A [START_REF] Anguelova | Non linear Observability and Identifiability: General Theory and a Case Study of a Kinetic Model[END_REF] x ≡ Ȧx in the state. The new state has dimension 25. Now we can use the second order Lie derivatives of all the outputs. We detect four additional independent Lie derivatives that are: L 2 00 h Vx , L 2 05 h Vx , L 2 06 h Vx and L 2 06 h Vy . Hence, the total number of independent Lie derivatives that are usable is 25, which coincides with the dimension of the state. This means that the entire state is observable.

If we include in the state all the observed features, we obtain that all of them are observable. Indeed, the camera provides for each feature its position up to a scale. On the other hand, we have shown that µ is observable, meaning that it is possible to get the scale. We conclude this section with the following important novel result: Theorem 4 (Two accelerometers, uncalibrated camera) Let us consider the VI-SfM problem when the inertial sensors only consist of two accelerometers and no gyroscope. Additionally, the magnitude of the gravity and the camera-IMU transformation are unknown and at least five features are available. All the independent observable modes are: the positions in the local frame of all the observed features, the three components of the speed of the vehicle in the local frame, the bias affecting the accelerometer measurements, the vehicle orientation, the magnitude of the gravity and the camera-IMU transformation.

Note that, the fact that the entire orientation (i.e., also the yaw angle) is observable, is a consequence of the fact that we fixed the global frame on the point features that are available from the beginning. Theorem 4 is very important and basically states that, even in the case when the camera is not extrinsically calibrated, the observability properties of the VI-SfM problem do not change by removing one of the three accelerometers and all the gyroscopes. The information provided by these additional four inertial sensors is redundant.

Single Accelerometer and Uncalibrated Camera

We want to know how the observability properties change by removing a further accelerometer, namely, we want to investigate the case when only a single accelerometer is available. We make the same assumptions as in the previous section, i.e., we consider the case when at least five features are available. Our system is now described by the following state:

X u 1 ≡ c F 1 , • • • , c F N f , V , q, A bias z , R c , q c , g, µ T (2.28)
where N f is the number of observed features and we assumed, without loss of generality, that the available accelerometer is along the z-axis in the IMU frame.

Before computing the Lie derivatives to apply the extended observability rank criterion introduced in section 2.5, we derive a continuous symmetry by using an intuitive procedure. Let us suppose to collect the data from the camera and the accelerometer during a given time interval for a generic vehicle motion, starting from a given initial state. We remark that, independently of the motion, by rotating the initial state around the accelerometer axis (i.e., around the z-axis of the IMU frame) we obtain exactly the same measurements. Let us derive how this rotation changes the initial state by referring to an infinitesimal rotation of an angle ǫ. We rotate all the features, the camera frame (namely its position and orientation in the IMU frame) the initial vehicle speed and orientation, simultaneously, around the z-axis of the IMU frame, by the angle ǫ. The camera configuration in the IMU frame changes as follows [START_REF] Goldstein | Classical Mechanics[END_REF]:

R c → R ′c = R c + ǫ   Y c -X c 0   ; q c → q ′c = q c + ǫ 2      q c z q c y -q c x -q c t     
The initial speed in the IMU frame (V ≡ [V x , V y , V z ] T ) changes as follows:

V → V ′ = V + ǫ   V y -V x 0  
Let us derive how the initial orientation changes. The state in (2.28) contains the quaternion q, which describes the IMU orientation in the global frame and not the orientation of the global frame in the IMU frame. This last orientation is described by the quaternion p ≡ p t + ip x + jp y + kp z ≡ q * = qtiq xjq ykq z . The quaternion p changes as q c , namely:

p → p ′ = p + ǫ 2      p z p y -p x -p t     
Hence, we have:

q * t → q * t + ǫ 2 q * z , q * x → q * x + ǫ 2 q * y , q * y → q * y -ǫ 2 q *
x and q * z → q * z -ǫ 2 q * t . By using q * t = q t , q * x = -q x , q * y = -q y and q * z = -q z we obtain:

q → q ′ = q + ǫ 2      -q z q y -q x q t     
The rotation does not affect all the remaining quantities in the state in (2.28). Indeed, µ and A bias z are scalar quantities. The vectors c F 1 , • • • , c F N f are the relative positions of the features in the camera frame. Since, by definition, we are both rotating the features and the camera frame, these relative positions are unvaried. Finally, the vector g remains unvaried since we are rotating the global frame and the gravity, simultaneously.

The rotation described above, is characterized by the following symmetry:

w int ≡ (2.29) ≡ 0 3N f , V y , -V x , 0, - q z 2 , q y 2 , - q x 2 , q t 2 , 0, Y c , -X c , 0, q c z 2 , q c y 2 , - q c x 2 , - q c t 2
, 0, 0, 0, 0 T where 0 3N f is the zero 1 × 3N f vector. Namely, the transformation:

X u 1 → X u 1 +
ǫw int on the initial state, cannot be detected by analyzing the measurements delivered by the camera and the accelerometer independently of the trajectory. Now we proceed as in the previous section, in several subsequent steps. As in the previous section, we refer to the case of a single feature and we show that w int is the only continuous symmetry. The dynamics of (2.28) provide seven independent directions along which the Lie derivatives can be computed. On the other hand, two of these directions are not available. These are the vectors f 1 (X u 1 ) and f 2 (X u 1 ), which are obtained by setting c Ω = [0 0 0] T , A = [1 0 0] T and c Ω = [0 0 0] T , A = [0 1 0] T in the dynamics, once f 0 (X u 1 ) has been removed. Since the Lie derivatives along these directions are not null, we have to proceed as in section 2.6.2. We must proceed in several subsequent steps. In each step we check, first of all, which highest order of Lie derivatives of the observations can be used. This is obtained by checking that, for a given order, all the Lie derivatives up to this order, computed at least once along f 1 or f 2 , are identically zero. Once this highest order is identified, we find the largest number of independent Lie derivatives up to this order. We include new time derivatives of the unknown inputs (i.e., A x and A y ) to make usable higher order Lie derivatives, as explained in section 2.5. We will show that, by including A x and A y and their first time derivative, we can collect a number of independent Lie derivatives that is equal to the dimension of the state minus one. Hence, the system has a unique symmetry, which is the vector w int in (2.29).

First step

We start with the 22-dimensional state given by the vector in (2.28), with a single feature. By chance, the first order Lie derivatives of the functions h Fx , h Fy and h Fz along f 1 and f 2 are null. Regarding the other nine outputs, the first order Lie derivatives along these directions are different from zero. Among the usable Lie derivatives, we detect 14 independent functions, which are:

L 0 h Fx , L 0 h Fy , L 0 h Fz , L 0 h Vx , L 0 h Vy , L 0 h Vz , L 0 h t , L 0 h x , L 0 h y , L 0 h q , L 0 h q c , L 1 5 h Fx , L 1 6 h Fx and L 1 6 h Fy .

Second step

We include A x and A y in the state. The new state has dimension 24. Now we can use the first order Lie derivatives of all the outputs and the second order Lie derivatives of the first three outputs. We detect six ad-ditional independent Lie derivatives that are: L 2 00 h Fx , L 2 03 h Fx , L 2 00 h Fy , L 2 03 h Fy , L 2 00 h Fz and L 2 03 h Fz . Hence, the total number of independent Lie derivatives that are usable is 20.

Third step

We include A ≡ Ȧy in the state. The new state has dimension 26. Now we can use the second order Lie derivatives of all the outputs. We detect five additional independent Lie derivatives which are: L 2 00 h Vx , L 2 05 h Vx , L 2 06 h Vx , L 2 00 h Vy and L 2 06 h Vy . Hence, the total number of independent Lie derivatives that are usable is 25, which is the dimension of the state minus one. This means that the system has a unique symmetry, which is the vector w int in (2.29).

If we include in the state all the observed features, we obtain that all of them are observable. Indeed, the camera provides for each feature its position up to a scale. On the other hand, it is immediate to check that the gradient of µ computed with respect to the state is orthogonal to the symmetry w int . From property 1 we obtain that µ is observable, meaning that it is possible to get the scale. We conclude this section with the following important novel result: Theorem 5 (Single accelerometer, uncalibrated camera) Let us consider the VI-SfM problem when the inertial sensors only consist of a single accelerometer and no gyroscope. Additionally, the magnitude of the gravity and the camera-IMU transformation are unknown and at least five features are available. The system loses some of the observability properties since it arises a continuous internal symmetry. As a consequence, the initial speed and orientation and the camera configuration in the IMU frame are not fully observable: all these quantities cannot be distinguished from the same quantities rotated around the accelerometer axis. All the remaining states are observable as in the case of two or more accelerometers.

This chapter investigates the possibility of solving the visual-inertial structure from motion problem in closed form. This makes possible to introduce deterministic algorithms that are very important to efficiently and robustly initialize any filter-based or optimization approach to the VI-SfM problem. The chapter starts by deriving a simple closedform solution to this problem (section 3.1). This solution could be extended to cope with the case of biased accelerometer measurements [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] and the case of unknown camera-IMU extrinsic calibration [START_REF] Dong-Si | Estimator Initialization in Vision-aided Inertial Navigation with Unknown Camera-IMU Calibration[END_REF]. For the sake of simplicity and brevity, in this chapter we do not consider these extensions. Special attention is devoted to identify the conditions under which the problem has a finite number of solutions. Specifically, it is shown that the problem can have a unique solution, two distinct solutions or infinite solutions depending on the trajectory, on the number of point-features and on their arrangement in the 3D space and on the number of camera images (section 3.2). In the case of biased measurements, more images and additional restrictive conditions arise [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF]. Finally, section 3.3 provides some results obtained by performing Monte Carlo simulations. Specifically, the closed-form solution is used in conjunction with a filtering approach to show its benefit in the initialization task.

Solution of VI-SfM in closed form

In the following we assume that the camera and the IMU frames coincide and that the IMU biases and the magnitude of the gravity are known. These assumptions will be relaxed in section 3.3. Since the local frame is time dependent, we will 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 that characterizes the rotation occurred during the time interval (t 1 , t 2 ) and with C t 1 t 2 its inverse (i.e., (C t 2 t 1 ) -1 = C t 1 t 2 ). Let us refer to vectors that are independent of the origin of the reference frame (e.g., speed, acceleration, etc.). For these vectors we have:

W t 1 (τ ) = C t 2 t 1 W 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 (τ ). We assume that the camera is observing one or more point-features during the time interval [T in , T f in ]. Our goal is to express in closed-form the observable modes at a given time T in only in terms of the visual and inertial measurements obtained in the time interval [T in , T f in ].

The position of the vehicle r at any time t ∈ [T in , T f in ] satisfies the equation r(t) = r(T in ) + v(T in )∆t + t T in τ T in a(ξ)dξdτ . The last term contains a double integral over time that can be simplified in a single integral by integrating by parts. We obtain:

r(t) = r(T in ) + v(T in )∆t + t T in (t -τ )a(τ )dτ (3.1)
where a ≡ dv dt and ∆t ≡ t-T in . We write equation (3.1) by highlighting the vector A τ (τ ) provided by the accelerometer:

r(t) = r(T in ) + v(T in )∆t + g ∆t 2 2 + C T in S T in (t) (3.2)
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 and High Rate Sensors[END_REF]. Hence, 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].

Let us suppose that N f point-features are observed, simultaneously. Their position in the local frame are F i t (t) and, in the global frame f i .

f i = r(t) + C T in C t T in F i t (t) (3.3)
We write this equation at time t = T in obtaining:

f i -r(T in ) = C T in F i T in (T in ) (3.4)
By inserting the expression of r(t) provided in (3.2) into equation (3.3), by using (3.4) and by pre multiplying by the rotation matrix (C T in ) -1 (we remind the reader that, according to our notation,

v(T in ) = C T in V T in (T in ) and g = C T in G T in )
we finally obtain the following equation:

C t T in F i t (t) = F i T in (T in ) -V T in (T in )∆t -G T in ∆t 2 2 + (3.5) -S T in (t); i = 1, 2, • • • , N f
A single image provides the bearing angles of all the point-features in the local frame. In other words, an image taken at time t provides all the vectors F i t (t) up to a scale. Since the data provided by the gyroscopes during the interval (T in , T f in ) allow us to build the matrix C t T in , having the vectors F i t (t) up to a scale, allows us to also know the vectors C t T in F i t (t) up to a scale. We assume that the camera provides n i images of the same N f point-features at the consecutive times:

t 1 = T in < t 2 < • • • < t n i = T f in .
From now on, for the sake of simplicity, we adopt the following notation:

• F i j ≡ C t j T in F i t j (t j ), i = 1, 2, • • • , N f ; j = 1, 2, • • • , n i • F i ≡ F i T in (T in ), i = 1, 2, • • • , N f • V ≡ V T in (T in ) • G ≡ G T in Ξ ≡                   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 f 1 -µ 1 2 0 3 µ N f 2 0 3 0 3 0 3 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • T n i S n i µ 1 1 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 f 1 0 3 0 3 0 3 -µ 1 n i 0 3 µ N f n i                   (3.8)
of bias). The linear system in (3.7) can be written in the following compact format:

ΞX = S (3.9)
The sensor information is completely contained in the above linear system. Additionally, we assume that the magnitude of the gravitational acceleration is a priori known. This extra information is obtained by adding to our linear system the following quadratic equation: |G| = g. By introducing the following 3×(N

f n i +6) matrix, Π ≡ [I 3 , 0 3 • • • 0 3 ],
this quadratic constraint can be written in terms of X as follows:

|ΠX| 2 = g 2 (3.10)
The VI-SfM problem can be solved by finding the vector X, which satisfies (3.9) and (3.10).

Existence and number of distinct solutions

We are interested in understanding how the existence and the number of solutions of the VI-SfM problem depend on the motion, on the number of observed point-features, on the point-features arrangement in the 3D space and on the number of camera images. The resolvabil-6×9 matrix. Hence, the dimension of N (Ξ) is at least 3. Let us consider the case when N f = 2. In this case Ξ is a 12 × 12 matrix. In [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] we prove the following property:

Property 5 (n i = 3, N f = 2)
The dimension of N (Ξ) is 1 if and only if the following two conditions are met:

(1) for a given j (e.g., for j = 2), the three vectors F 1 1 , F 2 1 and χ j span the entire 3D-space;

(2) for the other value of j (e.g., for j = 3)

F i j is not proportional to F k j , ∀i, k = 1, 2, • • • , N f .
From now on, we will say that a condition is satisfied in general when the probability that it is not satisfied is zero. We remark that both conditions (i) and (ii) are met in general. For N f ≥ 2 we have:

Property 6 (n i = 3, N f ≥ 2)
When n i = 3 and N f ≥ 2 the VI-SfM problem has in general two distinct solutions. In some special cases it has infinite solutions.

n i ≥ 4

When n i ≥ 4 the number of equations is larger than the number of unknowns, except when n i = 4 and N f = 1. In this case the matrix Ξ is 9 × 10 and the dimension of its null space is at least 1. We have the following property (see [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] for its proof):

Property 7 (n i = 4, N f = 1)
The dimension of N (Ξ) is 1 if and only if the four vectors F 1 1 , χ 2 , χ 3 and χ 4 span the entire 3D-space.

We do not provide here necessary and sufficient conditions for any value of n i and N f . The following property holds (see [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] for its proof):

Property 8 (n i ≥ 4) When n i = 4 and N f = 1 the VI-SfM problem has in general two distinct solutions. If n i = 4, N f ≥ 2 or if n i ≥ 5, ∀N f it has in general a unique solution.

Constant acceleration and constant speed

From property 4 we know that when the vehicle moves with constant acceleration, the dimension of N (Ξ) is at least 1. In [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] we also provide a sufficient condition that guarantees that the dimension of N (Ξ) is exactly 1. Hence, the VI-SfM has two distinct solutions. A special case of constant acceleration occurs when the vector α 0 vanishes, i.e., when the vehicle moves with constant speed. Since |Πn 0 | = |α 0 | = 0, according to theorem 6, the VI-SfM has infinite solutions. In [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] we also prove that in this case the roll and pitch angles can be uniquely determined. Hence, we have:

Property 9 (Constant speed) Let us suppose that the vehicle moves with constant speed. The VI-SfM has infinite solutions. Additionally, the orientation of the vehicle with respect to the horizontal plane can be uniquely determined.

Table 3.1 summarizes the results of this section by providing the number of solutions case by case. Note that this table does not account the point-features arrangement in the 3D space. Specifically, the motion and the point-features are not supposed to be either coplanar or collinear. Regarding these cases, necessary conditions are provided in [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] where the previous analysis is also extended to cope with the case of biased accelerometer's measurements.

Performance Evaluation

In this section we evaluate the performance of the closed-form solution in solving the VI-SfM starting from noisy and biased synthetic data generated through Monte Carlo simulations. Additionally, we consider the case when the transformation between the visual and inertial sensors is not perfectly known.

Simulated Trajectories

All the trajectories are randomly generated starting from the following initial true state:

Results

We considered three distinct scenarios that correspond to a different number of point features in the environment. Specifically, we considered the following three values: N f = 1, 2, 5. For each scenario we performed 1000 trials. In each trial the position of the features is generated randomly pursuant to a uniform distribution on a box of 1m side centred on the starting position r(T in ). For each trial, we computed the following errors that characterize the performance of the closed-form solution in solving the VI-SfM problem:

• E v , defined as the norm of the vector that is the difference between the true initial speed and the one determined by the closed-form solution; • E µ , defined as the error on the scale in %; • E roll , defined as the difference between the true initial roll and the one determined by the closed-form solution; • E pitch , defined as E roll but by considering the initial pitch instead of the initial roll.

These errors are plotted in fig. 3.1. Table 3.2 displays the mean values of these errors, averaged on the 1000 trials that have been performed. Regarding E roll and E pitch the mean value is computed starting from their absolute values. We remark that there is a huge improvement (nearly one order of magnitude) by passing from N f = 1 to N f = 2 while the improvement obtained by further increasing N f is almost negligible.

N f E v (ms -1 ) E µ (%) E roll (deg) E pitch (

Deterministic initialization of an EKF -based VI-SfM

In this section we show the benefit of using the closed-form solution for initializing a filter based approach to solve the VI-SfM problem. In section 3.1 we formulated the VI-SfM problem as the problem of determining the vectors:

F i , (i = 1, 2, • • • , N f ) and V , G.
For the sake of clarity, in this section we choose to display the results in a global frame. For this reason, we need to consider at least two point-features. Indeed, two is the minimum number of point-features to uniquely define a global frame, provided that they do not lie on the same vertical axis (defined by the gravity). We define the global frame as follows: first, we define one of the point-feature as the origin of the frame. The z-axis coincides with the gravity axis but with opposite direction. Finally, the x-axis is defined by requiring that the second point-feature belongs to the xz-plane. In other words, the second point-feature has zero y-coordinate. In these settings, the VI-SfM can be defined as the estimation of the vehicle configuration and the estimation of the x and the z coordinate of the second point-feature (from now on, p x and p z , respectively). By adding more point-features, the state to be estimated also includes all the three coordinates of each point-feature. We adopt an Extended Kalman Filter (EKF ) to perform this estimation. The state to be estimated is:

x e ≡ [r, v, q, p x , p z , A bias , Ω bias , f 3 , • • • , f N f ] T
By collecting the sensor measurements during the time-interval [T in , T f in ], the closed-form solution discussed in the previous sections allows us to determine the vectors F i , (i = 1, 2, • • • , N f ), V and G at the time T in . Note that, when N f ≥ 2, having the vectors F i , V and G at the time T in , allows us to build the state x e at time T in (with the exception of A bias and Ω bias ). Since our simulated measurements are corrupted by noise and also include a bias on the IMU and an error on the extrinsic camera-IMU calibration, the values obtained with the closed-form solution will differ from the true values.

In this section, we investigate how the performance of the EKF depends on its initialization and how this performance can be improved by using the closed-form solution to initialize the state. Since the closedform solution does not provide the initial A bias and Ω bias , their initial values will be set to zero.

Results

We adopted the same parameters that characterize the simulations described in the previous section. As previously explained, we need at least two point features. The first one is the origin and the second one is characterized by p x = 2m and p z = 1m. We also considered the case of more than two point-features (N f ≥ 3), obtaining similar results in terms of performance and, for the sake of brevity, in the following we only refer to the case of N f = 2.

We first investigate the convergence of the EKF vs the initialization of the state. In all the considered initializations we set the initial accelerometer and gyroscope biases to zero. In general, the EKF diverges when: (a) the initial scale factor error exceeds 20%; (b) the initial attitude error exceeds 4 deg. These conclusions on the EKF convergence have been obtained by running many simulations with the settings specified in 3.3.1 and 3.3.2. As an illustration, we display here the results obtained with a particular trial. Figures 3.2ad display the trajectories estimated by the EKF when the initial state differs from the true state because of an error on the absolute scale and on the attitude (as said, the initial state is also affected by an error on the inertial sensors' biases since they are always initialized to zero). Figure 3.2a displays the true trajectory (blue) together with the one estimated by only using inertial measurements (black) and the one estimated by the EKF with an initial absolute scale set to 1.1 times the true value and an error of 1 deg on the roll, pitch and yaw angles. running many simulations, we found that the initial state determined through the closed-form solution is never affected by an error larger than 8% regarding the absolute scale and than 0.7 deg regarding the attitude.

All the results illustrated so far assume a perfect data association.

In autonomous robotics, any feature-based approach to SfM, must be able, first of all, to autonomously remove all the outliers. In this chapter we briefly address this important problem. In section 4.1 we present a method able to detect outliers by using the closed-form solution described in the previous chapter with a single point feature. In sections 4.2 and 4.3 we describe a simpler approach obtained by combining some concepts from epipolar geometry with basic concepts in inertial navigation.

Outliers detection by using the closed-form solution

In this section we show the use of the closed-form solution to detect outliers in the features matching process. We start by remarking that the closed-form solution can work even in the extreme case of a single feature correspondence in consecutive images. Specifically, with a single point feature, it is possible to determine the following physical quantities that regard the vehicle motion: the absolute roll, the absolute pitch and the three components of the speed in the local frame. This suggests to find inliers by a simple histogram voting. In particular, we can build an independent histogram for each of the previous physical quantities. According to table 3.1, we need at least 5 consecutive images to have a unique solution for these quantities. Let us suppose to have n i ≥ 5 images and that the same N f features have been detected and correctly matched in the first n i -1 images. Now, we consider the last image, i.e. the n th i . We start from a given matching of the previous N f features with N f image points detected on this n th i image. We will use the closed-form solution to detect the inliers and outliers in this last matching by proceeding as follows.

We consider each feature separately. For each one we compute the three components of the speed and the roll and pitch angles. Hence, we obtain for each of these 5 quantities N f values that correspond to the N f features. Starting from these values we build a histogram voting for each of these 5 physical quantities. We use each histogram to remove outliers. By combining two or more histograms we can remove more outliers.

We tested this procedure on synthetic data. We adopted the same parameters used in the simulations described in the previous chapter, with the exception of the number of features that are now N f = 50. We considered n i = 8 consecutive images and we correctly matched these 50 features in the first 7 images, while in the matching with the last image we introduced a given percentage (P o ) of outliers. Specifically, we considered the following two cases, P o = 10%, 40%. We ran 1000 trials for each case. Table 4.1 displays the results. We provide the percentage of the inliers that have been missed by our procedure (the Missed Inliers (M.I.) in the second, third and fourth column) and the percentage of outliers classified as inliers (the False Inliers (F.I.) in the fifth, sixth and seventh column). In particular, the results reported in the second and fifth column are obtained by only using the histograms obtained with the three components of the speed (V x V y V z ); the results in the third and in the sixth column are obtained by only using the histograms obtained with the roll and pitch angles (r p); finally, the results in the fourth and in the seventh column are obtained by using all the previous histograms.

V x V y V z r p V x V y V z r p V x V y V z r p V x V y V z r

Essential matrix and epipolar constraint

The previous procedure requires to already have a correct data association in the first n i -1 images. We now want to refer to two consecutive images, without assuming any prior knowledge. We will refer to the RANdom SAmple Consensus (RANSAC) [START_REF] Fischler | Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[END_REF], which is the most popular method to estimate a given model from a set of observed data that contains outliers. In the case of two camera images, the model consists of the rotation and the translation up to a scale occurred between the two camera poses, i.e., the transformation up to a scale between the two reference frames attached to the camera at the two times when the two images are taken. This model can be obtained from matched points starting from the epipolar constraint.

Let us consider two camera images and let us denote with m ≡ [u, v, 1] T and m ′ ≡ [u ′ , v ′ , 1] T the coordinates (up to a scale) of the same scene point from the two camera configurations, respectively. Additionally, we denote with s ≡ [s x , s y , s z ] T the translation between the two camera frames and with R the rotation between them. The epipolar constraint [START_REF] Hartley | Multiple view Geometry in Computer Vision[END_REF] is the following homogeneous equation: The epipolar constraint can be considered an equation in the unknown rotation matrix R and the unknown translation s. Since it is a homogeneous equation, it is independent of the magnitude of the translation. For this reason, it is convenient to adopt the following parametrizations:

(1) s = ρ[cos φ, sin φ, 0] T for a general planar motion; (2) s = ρ[cos θ cos φ, cos θ sin φ, sin θ] T for a general 3D motion.

In both cases, the magnitude of the translation can be neglected. For a general 3D motion, the model that must be inferred from matched points consists of five parameters. Three of them characterize the rotation, i.e., define the matrix R. The remaining two parameters are the previous θ and φ. Hence, at least five correspondences must be used to estimate the model [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF]. In the planar case, the model consists of two parameters: the former characterizes the rotation, the latter (the previous φ) the translation up to a scale. Therefore, to detect and remove outliers, a five-point and a two-point RANSAC can be adopted in the 3D and planar case, respectively.

Simplified model

When monocular vision is coupled with an IMU, the rotation occurred between two camera configurations can be easily inferred from the inertial measurements. Hence, we can assume that the matrix R is known. This fact, significantly simplifies the epipolar constraint, by reducing the number of unknowns in the essential matrix. In the following, we denote with m ′′ ≡ [u ′′ , v ′′ , 1] T , the vector obtained by pre multiplying m by R and then by dividing all the components by the third component. The epipolar constraint (i.e., the equation (4.1)) reads: in the 3D case. We discuss these two cases separately.

Conclusion and discussion

In this paper we provided a series of new results about the visualinertial structure from motion problem. In particular, these new results significantly improve the current state of the art by providing new properties related to three fundamental issues: observability properties, resolvability in closed-form and data association. These results are important from a technological point of view. Additionally, they can provide a new insight for the comprehension of the process of vestibular and visual integration, which has been investigated in the framework of neuroscience.

Observability properties

The analysis started by considering the common formulation, which assumes three orthogonal accelerometers and three orthogonal gyroscopes. Theorem 2 provides the most general result in this sensor settings, by accounting biased inertial measurements, unknown magnitude of the gravity, unknown camera-IMU extrinsic calibration and the case of any number of point-features. This theorem states that all the independent observable modes are: the positions in the local frame of all the observed features, the three components of the speed in the local frame, the biases affecting the inertial measurements, the roll and the pitch angle, the magnitude of the gravity and the transformation between the camera and IMU frames. The fact that the yaw angle is not observable is an obvious consequence of the system invariance under rotation about the gravity vector.

Then, the analysis is extended to derive the observability properties when the number of inertial sensors is reduced. The case of a single accelerometer and no gyroscope is investigated. Theorem 3 basically states that, if the camera is extrinsically calibrated, the observability properties do not change. On the other hand, as the camera is not extrinsically calibrated, an internal symmetry arises (see theorem 5). Specifically, from the sensor measurements collected during any time interval and by accomplishing any motion, it is not possible to distinguish all the physical quantities rotated around the accelerometer axis. This means that, in this setting, it is not possible to fully perceive selfmotion. As an additional accelerometer is introduced, the system gains again the full observability (theorem 4). These results clearly show that the information provided by an IMU is redundant. Additionally, these results are consistent with our knowledge about the vestibular system, which provides balance in most mammals. Indeed, the otoliths, which indicate linear accelerations, consists of two organs (the utricle and the saccule) able to sense the acceleration only along two independent axes.

Resolvability in closed-form and data association

In the paper, we discussed the resolvability of the VI-SfM problem, in closed form. In the case of visual measurements only, the SfM has been solved up to a scale [START_REF] Chiusoa | Structure from Motion Causally Integrated Over Time[END_REF][START_REF] Davison | MonoSLAM: Real-Time Single Camera SLAM[END_REF][START_REF] Hartley | In Defense of the Eight-Point Algorithm[END_REF][START_REF] Longuet-Higgins | A computer algorithm for reconstructing a scene from two projections[END_REF][START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] and a closed-form solution has also been derived [START_REF] Hartley | In Defense of the Eight-Point Algorithm[END_REF][START_REF] Longuet-Higgins | A computer algorithm for reconstructing a scene from two projections[END_REF][START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF], allowing the determination of the three-dimensional structure of the scene, without the need for any prior knowledge. In chapter 3 we extend this deterministic solution to the case of the VI-SfM problem. This makes possible to introduce deterministic algorithms, which are very important to efficiently and robustly initialize any filter-based or optimization approach.

The most useful applications of this closed-form solution will be in all the applicative domains that need to solve the SfM problem with low-cost sensors and that do not demand any infrastructure (e.g., in GPS denied environment). In these contexts, there is often the need to perform the estimation without any prior knowledge. Typical examples of applicative domains are the emergent fields of space robotics [START_REF] Mourikis | Vision-Aided Inertial Navigation for Spacecraft Entry, Descent, and Landing[END_REF], humanoid robotics and unmanned aerial navigation in urban-like environments [START_REF] Weiss | Vision Based Navigation for Micro Helicopters[END_REF], where the use of the GPS is often forbidden. The closed-form solution could also play an important role in the framework of neuroscience by providing a new insight on the process of vestibular and visual integration for depth perception and self-motion perception. The influence of extra retinal cues in depth perception has extensively been investigated in the last decades [START_REF] Cornilleau-Peres | The visual perception of three-dimensional shape from self-motion and object-motion[END_REF][START_REF] Longuet-Higgins | The interpretation of a moving retinal image[END_REF][START_REF] Macneilage | Vestibular heading discrimination and sensitivity to linear acceleration in head and world coordinates[END_REF][START_REF] Wexler | Self-motion and perception of stationary objects[END_REF]. In particular, a very recent study investigates this problem by performing trials with passive head movements [START_REF] Dokka | Estimating distance during self-motion: a role for visual-vestibular interactions[END_REF]. The conclusion of this study is that the combination of retinal image with vestibular signals can provide rudimentary ability to depth perception. Our findings could provide a new insight to this integration mechanism for depth perception since, according to the closed-solution here provided, by combining retinal image with vestibular signals it is possible to determine the scale factor even without any knowledge about the initial speed. Our findings also show that it is possible to easily distinguish linear acceleration from gravity. Specifically, the closed-form solution performs this determination by a very simple matrix inversion. This problem has also been investigated in neuroscience [START_REF] Angelaki | Neurons compute internal models of the physical laws of motion[END_REF][START_REF] Merfeld | Humans use internal models to estimate gravity and linear acceleration[END_REF]. Our results could provide a new insight to this mechanism since they clearly characterize the conditions (type of motion, features arrangement in the 3D space) under which this determination can be performed.

Finally, in the last chapter of the paper, we discussed very briefly the data association problem. We showed that, since the rotation occurred between two camera frames can be efficiently obtained by integrating the inertial measurements, the parameters necessary to characterize the model reduce to two for a general 3D motion and to one in the planar case. This makes possible the use of a 2-point RANSAC in the general 3D case and a 1-point RANSAC in the planar case.

Figure 2 .

 2 Figure 2.1 displayes the three reference frames together with some of the previous vectors.

Fig. 2 . 1

 21 Fig. 2.1 Global frame, local (IMU) frame and camera frame with the feature position ( c F ) in the camera frame and the vehicle speed (V ) and the camera position (R c ) in the local frame.

15 )

 15 Let us include the unknown u nc in the state, i.e., S → S e ≡ [S, u nc ]

  r(T in ) = [0.5, 0.5, 0.5]m; v(T in ) = [0.1, 0.1, 0.1]ms -1 ; the true trajectory and the true camera-IMU transformation, the true bearing angles of the point-features in the camera frame are computed. They are computed each 0.1s. 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.

Fig. 3 . 1

 31 Fig. 3.1 The value of the four errors computed to evaluate the performance of the closedform solution in solving the VI-SfM problem. Each figure contains three plots: they correspond to the three scenarios defined by N f = 1, 2, 3 respectively from the top to the bottom.

Fig. 3 . 2

 32 Fig. 3.2 Fig a displays the true trajectory (blue), the trajectory estimated by the EKF (red) with initial errors (10% on the scale and 1 deg on the attitude) and the trajectory estimated by only using inertial measurements (black). Figs b, c and d display the trajectories estimated by the EKF when the initial state is affected by an error on the attitude (b) and on the absolute scale (larger and smaller than the true one, in c and d, respectively).

m

  ′ T Em = 0 (4.1) where E = [s] × R is the essential matrix and [s]

sin θ cos θ sin φ sin θ 0 -

 0 cos θ cos φ cos θ sin φ cos θ cos φ 0

Table 3 .

 3 [START_REF] Armesto | Fast Ego-motion Estimation with Multi-rate Fusion of Inertial and Vision[END_REF] The mean values of the four errors for the three scenarios.

Table 4 .

 4 1 Simulation results on outliers and inliers detection.

Actually, this condition guarantees that the system is locally weakly observable. The reader is referred to[START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] for a detailed description of weak and local observability.

2.6. Observability Properties with unknown inputs[START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF]

Note that in this caseu = [u 1 , u 2 , • • • , un c ] T = [Ax, Ay, Az, c Ωx, c Ωy, cΩz] T , i.e., the last three inputs are the components of the angular speed in the camera frame, which is known.

• S j ≡ S T in (t j ), j = 1, 2, • • • , n i

We remark that the difference

is independent of i (see equation (3.5), where, by definition, C t j T in F i t j (t j ) = F i j ). Hence, we will set χ j ≡ F i j -F i 1 . This quantity characterizes the motion of the vehicle. We will denote with µ i j the unit vector with the same direction of F i j and we introduce the unknowns λ i j such that F i j = λ i j µ i j . Finally, without loss of generality, we can set T in = 0, i.e., ∆t = t. Our sensors provide µ i j and S j for i = 1, 2, • • • , N f ; j = 1, 2, • • • , n i . Equation (3.5) can be written as follows:

The VI-SfM problem is the determination of the vectors:

We can use the equations in (3.6) to determine these vectors. On the other hand, the use of (3.6) requires to also determine the quantities λ i j . By considering j = 1 in (3.6), i.e. t j = t 1 = T in = 0, we easily obtain:

Then, we write the linear system in (3.6) as follows:

and 0 3 is the 3 × 1 zero vector. This linear system consists of 3(n i -1)N f equations in N f n i + 6 unknowns.

Let us define the two column vectors X and S:

and the matrix Ξ (see equation (3.8) at the next page), 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 (note that the third set of columns disappear in absence ity of the VI-SfM problem can be investigated by computing the null space of the matrix Ξ in (3.8). Let us denote with N (Ξ) this space. In [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] we prove the following theorem that allows us to obtain all the properties of the VI-SfM problem by investigating the null space of Ξ: In [START_REF] Martinelli | Resolvability of Visual-Inertial Structure from Motion in Closedform[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] we also prove the following important property:

Finally, when n i ≥ 4 and the vehicle moves with constant acceleration the dimension of N (Ξ) is at least 1.

Regarding the cases n i = 3 and n i ≥ 4 with constant acceleration, the proof is obtained by showing that there is at least one 3D-vector α 0 and one 3D-vector ν 0 such that the following vector always belongs to N (Ξ):

where n1

In the case of constant acceleration, α 0 is precisely its value. In the following, we discuss the number of solutions of the VI-SfM problem depending on the number of camera images (n i ).

n i ≤ 2

From property 4 we know that the dimension of N (Ξ) is at least 3 and, consequently, by using theorem 6, we conclude that the VI-SfM has always infinite solutions.

n i = 3

From property 4 we know that the dimension of N (Ξ) is at least 1, independently of the number of point-features. When N f = 1, Ξ is a

Cases

Number of Solutions

Varying Acceleration Unique Solution

Constant and non null Acceleration Two Solutions q(T in ) = 1, which corresponds to the vehicle attitude roll = pitch = yaw = 0 deg. Then, 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 zero-mean Gaussian independent variables whose covariance matrices are equal to (1ms -2 ) 2 I 3 and (10 deg s -1 ) 2 I 3 , respectively.

Simulated Sensors

We simulate an Xsens Mtx device, according to the signal noise analysis provided in [START_REF] Woodman | An introduction to inertial navigation[END_REF], which is based on the Allan Variance (see [START_REF] Woodman | An introduction to inertial navigation[END_REF] for more details). The Mtx is a MEMS IMU characterized by low weight (about 30 g) and low cost. Hence it is suitable for many applications.

Starting from the accomplished trajectory, the true angular speed and the linear acceleration are computed at each time step of 0.01s (respectively, at the j th time step, we denote them with Ω true j and A true j ). 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:

where:

• N (., .) indicates the Normal distribution whose first entry is the mean value and the second its covariance matrix; • P Ω and P A are the covariance matrices characterizing the thermo-mechanical white noise that affects both the gyroscopes and the accelerometers; • Ω bias and A bias are the gyroscope and accelerometer's biases, respectively.

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

) 2 is obtained from the experimental results provided in [START_REF] Woodman | An introduction to inertial navigation[END_REF] on the Angular Random Walk (ARW). Specifically, this value slightly exceeds the one that can be inferred from the ARW provided for the examined Xsens Mtx device. Similarly, the value (1 cm s -2 ) 2 is obtained from the experimental results provided in [START_REF] Woodman | An introduction to inertial navigation[END_REF] on the Velocity Random Walk (VRW). Also this value slightly exceeds the one that can be inferred from the VRW provided for the examined Xsens Mtx device.

Finally, the two bias vectors are set as follows: A bias = 0.001 μ m s -2 , Ω bias = 0.01 μ deg s -1 where μ is the unit vector pointing in the direction [START_REF] Anguelova | Non linear Observability and Identifiability: General Theory and a Case Study of a Kinetic Model[END_REF][START_REF] Anguelova | Non linear Observability and Identifiability: General Theory and a Case Study of a Kinetic Model[END_REF][START_REF] Anguelova | Non linear Observability and Identifiability: General Theory and a Case Study of a Kinetic Model[END_REF]; Also in this case these values are obtained starting from the experimental results provided in [START_REF] Woodman | An introduction to inertial navigation[END_REF] about the bias instability. Specifically, we are assuming that the bias are the ones that arise after about 100 s from the last calibration.

Regarding the camera, first of all we assume that its frame coincides with the IMU frame (i.e., the two frames have the same origin and the same orientation). We characterize an error in the extrinsic calibration by setting the actual position of the origin of the camera frame in the IMU frame to [0.002, -0.003, 0.004]m and the actual orientation q cam = 1 -2. A single correspondence is sufficient to get the model (i.e., the parameter φ). From equation (4.2) we have:

Hence, 1-point RANSAC can be easily adopted to detect and remove outliers [START_REF] Troiani | 1-Point-based Monocular Motion Estimation for Computationally-limited Micro Aerial Vehicles[END_REF].

3D motion

In this case the model consists of two parameters, φ and θ. We need to use the epipolar equation in (4.3), for two distinct points. Let us denote them by the subscript 1 and 2. By a direct computation it is possible to obtain the following expression for φ: φ = (4.5)

Then, one of the two epipolar equations (e.g., the first one) can be used to obtain θ:

Hence, 2-point RANSAC can be easily adopted to detect and remove outliers.