Kalman Kf

Filter

Kanade Klt

Lucas

IF Information Filter. iff if and only if. IMU Inertial Measurement Unit

Keywords: APS Active Pixel Sensor, CCD Charge-Coupled Device, CMOS Complementary Metal-Oxide-Semiconductor, DOF IBVS Image Based Visual Servoing Based Visual Servoing, PDE Partial Differential Equation, PDF Probability Density Function, PE Persistence

Conclusions et annexes

Le Chapitre 7 conclut la description des principales contributions de ce travail.

En plus du contenu décrit jusqu'ici, la thèse contient également un chapitre conclusif et trois annexes.

Au Chapitre 8 nous fournissons un examen final d'ensemble des principaux résultats de la thèse en soulignant aussi quelques questions ouvertes qui restent encore à résoudre. Le Chapitre 8 propose aussi un certain nombre d'extensions possibles à ce travail qui mériteraient d'être étudiées. Certains d'entre elles sont en effet les sujets de l'activité de recherche actuelle de l'auteur.

Dans l'Annexe A nous incluons quelques détails techniques supplémentaires pour la dérivation de certains résultats présentés dans la thèse. Ce contenu n'est pas indispensable pour comprendre le reste de ce travail, mais il est néanmoins inclus ici pour les lecteurs intéressés.

Dans l'Annexe B nous présentons des résultats préliminaires dans le contexte de l'estimation 3-D dense en utilisant directement des informations photométriques. Dans ce cas, l'image de la caméra (perçue comme une nappe dense de niveau de luminosité) est utilisée directement pour calculer le terme d'innovation de l'observateur SfM, supprimant la nécessité de tout traitement préliminaire de l'image (par exemple l'extraction, la recherche de correspondances et le suivi des primitives). Dans l'Annexe C nous donnons une introduction très courte et plutôt informelle au vaste sujet des systèmes port-Hamiltoniens car ils fournissent une interprétation intéressante, intuitive et physique des shémas d'estimation et d'optimisation utilisés dans cette thèse. Nous introduisons également, très brièvement, la représentation à graphe de liaisons de tels systèmes. vi

The purpose of an experiment is to make Nature speak intelligently:

all that then remains is to listen Bill Diamond (1978) Résumé D epuis ses origines, dans les années 1980, la robotique a été définie comme la science qui étudie la « connexion intelligente entre la perception et l'action » [START_REF] Siciliano | Springer handbook of robotics[END_REF]. Dans cette définition, il est clair qu'il n'y a pas de poids particulier sur les composantes de perception et d'action d'un système robotique. Ce qui rend un robot (ainsi qu'un être vivant) vraiment intelligent n'est pas, en fait, l'excellence dans une de ces composantes (ou même dans les deux), mais, plutôt, le correct équilibre entre les deux. Si, d'une part, il est immédiatement intuitif que la précision de l'estimation de l'état du robot par rapport à l'environnement qui l'entoure a un fort impact sur la précision de ses actions, il ne faut pas oublier, d'autre part, qu'il a été démontré à plusieurs reprises que la perception est un processus actif aussi bien pour les êtres vivants [START_REF] Gibson | Observations on active touch[END_REF] que pour les systèmes robotiques [START_REF] Bajcsy | Active perception[END_REF].

Comme pour de nombreux travaux en robotique, nous nous sentons partiellement obligés de « justifier » notre étude en partant d'un exemple représentatif du monde biologique. Il a été montré dans [START_REF] Tucker | Curved flight paths and sideways vision in peregrine falcons (Falco peregrinus)[END_REF][START_REF] Tucker | The deep fovea, sideways vision and spiral flight paths in raptors[END_REF] que les rapaces approchent leur proie en suivant une trajectoire en spirale (voir Fig. 1(b)) plutôt qu'une (plus courte) ligne droite. L'explication reconnue de ce comportement, apparemment contre-productif, est que la ligne de visée de la fovéa, la région rétinienne spécialisée dans la vision aiguë, vise à environ 45 degrés à droite ou à gauche de l'axe frontal de l'oiseau, voir Fig. 1(a). En suivant une trajectoire en spirale autour de la proie, le rapace peut voir la cible dans le champ de vue de la fovéa tout en conservant la tête alignée avec le reste de son corps. Pour voler en ligne droite, le faucon serait forcé de tourner sa tête sur le côté, augmentant ainsi considérablement la résistance de l'air durant le vol ce qui réduirait sa vitesse. Cette trajectoire en spirale est donc le résultat (naturel) d'une maximisation conjointe des performances à la fois de la perception et de l'action. Dans de nombreuses applications robotisées basées sur des capteurs extéroceptifs, l'état d'un robot par rapport à son environnement peut seulement être par- tiellement récupéré par ses capteurs embarqués. Dans ces situations, des schémas d'estimation d'état peuvent être exploités pour récupérer en ligne les « informations manquantes » et les fournir à n'importe quel planificateur/contrôleur de mouvement, à la place des états actuels non mesurables. Quand on considère des cas non-triviaux, cependant, l'estimation de l'état doit souvent faire face aux relations non linéaires entre l'environnement et l'espace des capteurs (la projection perspective effectuée par les caméras étant un exemple classique dans ce contexte [START_REF] Ma | An invitation to 3D vision[END_REF]). En raison de ces non-linéarités, la convergence et la précision de l'estimation peuvent être fortement affectées par la trajectoire particulière suivie par le robot/capteur qui, au sens large, doit garantir un niveau suffisant d'excitation pendant le mouvement [START_REF] Cristofaro | Optimal Trajectories for Multi Robot Localization[END_REF][START_REF] Achtelik | Path planning for motion dependent state estimation on micro aerial vehicles[END_REF]. Par exemple, dans le contexte de la reconstruction 3-D à partir de la vision ("Structure from Motion -SfM"), un mauvais choix des entrées du système (par exemple, la vitesse de translation de la caméra) peut rendre la structure 3-D de la scène non observable quelle que soit la stratégie d'estimation utilisée [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Eudes | Visuo-inertial fusion for homography-based filtering and estimation[END_REF][START_REF] Grabe | A Comparison of Scale Estimation Schemes for a Quadrotor UAV based on Optical Flow and IMU Measurements[END_REF], ce qui conduit, dans la pratique, à une estimation inexacte de l'état pour des trajectoires avec un faible contenu d'information. Ceci, à son tour, peut dégrader les performances de tout planificateur/contrôleur qui doit générer des actions en fonction de l'état reconstruit, conduisant éventuellement à des échecs ou des instabilités en cas de trop grandes erreurs d'estimation [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF][START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF]. La dépendance entre la performance d'estimation de la trajectoire du robot et la performance du contrôle de la précision d'estimation, crée clairement une relation étroite entre perception et action : la perception doit être optimisée pour améliorer la performance de l'action, et les actions choisies doivent ii permettre la maximisation des informations recueillies pendant le mouvement pour faciliter la tâche d'estimation [START_REF] Valente | Information Gathering Control via Exploratory Path Planning[END_REF].

Sujet de la thèse

Avec ces considérations en tête, nous adoptons dans cette thèse la définition classique de la robotique et nous analysons la relation entre perception et action dans les systèmes robotiques. Étant donné l'ampleur du sujet, nous nous concentrons sur le contexte de l'estimation et de la commande basées sur la vision et, en particulier, sur la classe des shémas de asservissement visuel basé-image (IBVS) [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]. En effet, en plus d'être une technique basée sur des capteurs très répandus, voir par exemple [TC05, GH07, MS12], l'IBVS est aussi un bon exemple de tous les problèmes mentionnés précédemment : d'une part, quel que soit l'ensemble choisi des primitives visuelles (par exemple, des points, des droites, des plans et ainsi de suite), la matrice d'interaction associée dépend toujours d'informations 3-D supplémentaires qui ne sont pas directement mesurables à partir d'une image (par exemple, la profondeur d'un point caractéristique ou le rayon d'une sphère). Cette information 3-D doit alors être approximée ou estimée en ligne par l'intermédiaire d'un algorithme de Structure from Motion (SFM), et une connaissance imprécise (en raison, par exemple, de mauvaises approximations ou d'une mauvaise performance du SfM) peut dégrader l'asservissement et même conduire à des instabilités ou à l'échec du suivi de la primitive dans l'image [START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF]. Par contre, la performance du SfM est directement affectée par la trajectoire particulière suivie par la caméra lors de l'asservissement [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF] : le contrôleur IBVS doit alors être en mesure de réaliser la tâche visuelle principale et, en même temps, d'assurer un niveau suffisant de gain d'information pour permettre une estimation précise de l'état. À cet égard, nous proposons une stratégie d'optimisation de trajectoire en ligne qui permet de maximiser le taux de convergence d'un estimateur SfM, compte tenu de certaines limites sur la norme maximum acceptable de la vitesse de translation de la caméra. Nous montrons aussi comment cette technique peut être associée avec l'exécution simultanée d'une tâche IBVS en utilisant des techniques de résolution de redondance appropriées. Tous les résultats théoriques présentés dans cette thèse sont validés par une vaste campagne expérimentale qui, à notre avis, supporte pleinement nos affirmations.

Structure de la thèse

Le corps de cette thèse est divisé en trois parties principales. La première présente un examen et un état de l'art des principales techniques utilisées en vision par oriii dinateur, commande de robot, estimation d'état et perception active. Les deuxième et troisième parties présentent, par contre, les contributions originales de ce travail dans le contexte de l'estimation 3-D active et de son couplage avec la réalisation d'une tache d'asservissement. Dans ce qui suit, nous proposons un bref résumé du contenu de chaque partie.

Aperçu de la Partie I

Dans la Partie I, nous introduisons quelques préliminaires liés à la vision par ordinateur et à la robotique et nous proposons un examen approfondi de la littérature existante. A cet égard, nous essayons de focaliser l'attention du lecteur sur l'importance d'un couplage correct entre la perception et la commande en soulignant les problèmes éventuels qui peuvent survenir lorsqu'un tel couplage n'est pas correctement assuré.

Au Chapitre 2 nous commençons par résumer le modèle de base de la formation d'une image et les relations géométriques qui existent entre deux vues de la même scène prises à partir de différents points de vue de la caméra. Le chapitre se poursuit par la présentation des techniques standard utilisées pour la modélisation de la cinématique d'un robot et pour commander son mouvement. Nous nous concentrons, en particulier, sur la classe des techniques de commande de mouvement basé sur la vision, avec un accent sur les schémas d'asservissement visuel basé image (IBVS).

Au Chapitre 3 nous passons en revue les techniques classiques utilisées dans la littérature pour estimer l'état d'un robot et/ou la structure de l'environnement à partir de l'information visuelle. Nous proposons une comparaison entre les techniques déterministes et probabilistes et, en particulier, entre les indicateurs les plus couramment utilisés dans les deux approches pour mesurer le « conditionnement » de l'estimation. Ceci nous conduit naturellement à l'introduction de la notion de perception active comme le paradigme correct à adopter pour assurer un couplage efficace entre la perception et l'action.

Aperçu de la Partie II

La Partie II de la thèse présente les contributions de ce travail dans le contexte de l'estimation 3-D active par une caméra en mouvement. Les résultats contenus dans cette partie sont les sujets des contributions originales de l'auteur publiées dans [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF][START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF][START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF][START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF][START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF][START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF].

Au Chapitre 4 nous proposons une caractérisation de la dynamique de l'erreur d'estimation dans la SfM. Malgré la non-linéarité du problème, on montre que la iv dynamique d'erreur peut être rendue approximativement équivalente à celle d'un système linéaire du second ordre avec des pôles donnés si les gains d'estimation et la vitesse de translation de la caméra sont correctement sélectionnés en ligne en fonction des mesures visuelles courantes. Par la suite, le chapitre propose l'application de ce schéma général de perception active à un ensemble de primitives géométriques de base : points, plans et primitives cylindriques et sphériques.

Au Chapitre 5 nous présentons une vaste validation expérimentale de l'ensemble des thèses soutenues au Chapitre 4 avec un robot manipulateur équipé d'une caméra embarquée. En particulier, nous montrons que pour l'ensemble des primitives géométriques décrites dans le Chapitre 4, notre stratégie permet de contrôler correctement et de maximiser (autant que possible) le taux de convergence de l'estimateur sous certaines contraintes sur la norme maximale de la vitesse de la caméra.

Aperçu de la Partie III

La Partie III de ce travail aborde le problème plus difficle d'associer la perception active avec l'exécution d'une tâche d'asservissement visuel (IBVS). Une partie de ce travail a été présenté dans les publications de l'auteur [START_REF] Spica | Coupling Visual Servoing with Active Structure from Motion[END_REF][START_REF] Spica | A Framework for Coupling Visual Control and Active Structure from Motion[END_REF] et [START_REF] Spica | Bridging Visual Control and Active Perception via a Large Projection Operator[END_REF], qui, au moment de la rédaction, est sous évaluation en vue de publication.

In many sensor-based robot applications, the state of a robot w.r.t. the environment can only be partially retrieved from his on-board sensors. In these situations, state estimation schemes can be exploited for recovering online the 'missing information' then fed to any planner/motion controller in place of the actual unmeasurable states. When considering non-trivial cases, however, state estimation must often cope with the nonlinear sensor mappings from the observed environment to the sensor space that make the estimation convergence and accuracy strongly affected by the particular trajectory followed by the robot/sensor.

In this thesis we restrict our attention to the problem of vision based robot control. In fact vision is probably the most important sensor in biological systems and endowing robots with the possibility of controlling their motion using visual information is considered to be one of the hardest, but also most promising, challenges in modern robotics. When relying on vision based control techniques, such as Image Based Visual Servoing (IBVS), some knowledge about the 3-D structure of the scene is needed for a correct execution of the task. However, this 3-D information cannot, in general, be extracted from a single camera image without additional assumptions on the scene. In these cases, one can exploit a Structure from Motion (SfM) estimavii tion process for reconstructing this missing 3-D information. However performance of any SfM estimator is known to be highly affected by the trajectory followed by the camera during the estimation process, thus creating a tight coupling between camera motion (needed to, e.g., realize a visual task) and performance/accuracy of the estimated 3-D structure.

In this context, a main contribution of this thesis is the development of an online trajectory optimization strategy that allows maximization of the converge rate of a SfM estimator by (actively) affecting the camera motion. The optimization is based on the classical Persistence of Excitation (PE) condition used in the adaptive control literature to characterize the well-posedness of an estimation problem. This metric, however, is also strongly related to the Fisher Information Matrix (FIM) employed in probabilistic estimation frameworks for similar purposes. The optimization strategy that we propose can be run online because it is computationally efficient and it is only based on available information (visual measurements and camera velocity).

We also show how this technique can be coupled with the concurrent execution of a IBVS task using appropriate redundancy resolution techniques. In particular we employ a large projection operator that allows to maximize the robot redundancy by controlling the visual task error norm instead of the full task.

All of the theoretical results presented in this thesis are validated by an extensive experimental campaign run using a real robotic manipulator equipped with a camera in-hand. In our opinion, the experiments fully support our claims showing how the observability of a SfM estimation problem can be conveniently increased, even while concurrently executing a IBVS task, and that this results in improved performance for both the estimation and the control processes.

Keywords: active perception, control of robotic systems, visual servoing, structure from motion. viii I am very thankful to my long date friends too. Those in Rome and those now spreading all around the world. This quite long experience abroad was not always a bed of roses and it was relieving to know that you would always "be there".

Finally, I want to dedicate these last lines of acknowledgment to my parents and my sister. For just so many reasons that I do not need to explain, thank you!

General notation conventions

Throughout this thesis, the following notation conventions will be used:

• Scalar quantities are represented by lowercase symbols such as a, b, and so on.

• Elements of R n and similar sets are interpreted as column vectors and represented by bold lowercase symbols such as a, b, and so on.

• We use the notation (a, b, c) to indicate a vertical concatenation of elements (scalars, vectors or matrices) and [a b c] for horizontal concatenations.

• I n is used to represent the identity matrix of dimension n × n.

• O n×m is used to represent the n × m matrix with all elements equal to zero.

If m = 1 we also use ∅ n .

• We use the notation A 0 to indicate a positive definite matrix, i.e. such that a T Aa > 0, ∀a. In a similar way we define A 0 as a semi-positive matrix. We also write A ≺ 0 if -A 0 and A B if A -B 0.

• We use the symbol c a t b to indicate the vector that goes from the origin of reference frame F a to that of reference frame F b and is expressed in reference frame F c . We also use the notation a p = a a t p for the vector that goes from the origin of reference frame F a to a point p and is expressed in the reference frame F a . We use a similar notation to indicate velocities, e.g. c a v b is the velocity of frame F b w.r.t. frame F a expressed in frame F c . However we use the compact notation v and ω to indicate the linear and angular velocities of the camera frame w.r.t. the world expressed in the camera frame. • [a] × the is skew-symmetric matrix built with the components of the 3-D vector a and representing the cross product operator for 3-D vectors, i.e.

[a] × b = a × b.

• a indicates an estimation or approximation of a.

• a = aa is the error between the approximation of a and its actual value.

• ǎ is used to indicate the expression of a in another set of coordinates.

• A † indicates a generalized inverse or the pseudoinverse of A.

Nabla and friends

Let f : R n → R, p → f (p) be a generic scalar function of a vector argument. We indicate with

∇ p f (p) =             ∂f (p) ∂p 1 p ∂f (p) ∂p 2 p . . . ∂f (p) ∂pn p            
the column vector built by stacking the partial derivatives of f w.r.t the elements of p. This vector is also called the gradient of f w.r.t p and it can be thought of as the product of f by the nabla (column) vector:

∇ p =       ∂ ∂p 1 ∂ ∂p 2 . . . ∂ ∂pn       . xvi
This notation is convenient because it allows us to easily define the Hessian of a function, i.e. the matrix of all second order partial derivatives of f w.r.t. p as . . .

∇∇ T p f (p) = (∇ p ∇ T p )f (p) =             ∂ 2 f (p) ∂p 2
∂ 2 f (p) ∂p 2 n p             ∈ R n×n .
Also the Laplacian is easily defined as

∇ 2 p f (p) = (∇ T p ∇ p )f (p) = tr (∇∇ T p f (p)) = n i=1 ∂ 2 f (p) ∂p 2 i p ∈ R.
Now let f : R n → R m , p → f (p) be a generic (column) vector function of a vector argument. We can extend the gradient operation and define the Jacobian of f w.r.t. p as the matrix:

∇ p f (p) T =       ∇ p f 1 (p) T ∇ p f 2 (p) T . . . ∇ p f m (p) T       ∈ R m×n .
Note that this notation is somehow inappropriate since, if both ∇ p and f are represented as column vectors, one should write the Jacobian as f ∇ T p = (∇ p f T ) T .

However we accept this notation as it is common in the literature.

For all of the differential operators introduced above, we will sometimes omit the subscript representing the variable w.r.t. which the differentiation is taken whenever this does not lead to confusion, i.e. whenever the function that is being differentiated depends on a single (vector) variable, e.g., ∇ p f (p) = ∇f (p).

Introduction

S

ince its very origins, in the years 1980s, robotics was defined as the science that studies the "intelligent connection between perception and action" [START_REF] Siciliano | Springer handbook of robotics[END_REF]. Clearly in this definition there is no particular stress on either the perception or the action components of a robotic system. What makes a robot (as well as a natural being) really intelligent is not, in fact, the excellence in either one (or even both) of them, but, instead, the correct interplay between the two. On one hand, it is immediately intuitive that the accuracy in the estimation of the robot state w.r.t. the environment that surrounds it has a strong impact on the accuracy of its actions. On the other hand, however, one should not forget that perception has been demonstrated many times to be an active process both for natural beings [START_REF] Gibson | Observations on active touch[END_REF] and for robotic systems [START_REF] Bajcsy | Active perception[END_REF].

As done in many robotic works, we feel partially obliged to "justify" our study by bringing up a representative example from the biological world. It has been shown in [START_REF] Tucker | Curved flight paths and sideways vision in peregrine falcons (Falco peregrinus)[END_REF][START_REF] Tucker | The deep fovea, sideways vision and spiral flight paths in raptors[END_REF] that raptors approach their preys by following a spiral trajectory (see Fig. 1.1(b)) rather than a (shorter) straight path. The accepted explanation for this, at first, counter-intuitive behavior is that the Line Of Sight (LOS) of the deep fovea, the retina region specialized in acute vision, points at approximately 45 degrees to the right or left of the frontal axis of the bird, see Fig. 1.1(a). By following a spiral trajectory around the pray, the raptor can then keep the target within the deep fovea field of view while still maintaining its head aligned with the rest its body. Flying on a straight line, instead, would force the falcon to turn its head to the side thus considerably increasing air drag and significantly reducing the flight speed. This trajectory is then the (natural) result of a joint performance maximization of both perception and action.

In many sensor-based robot applications, the state of a robot w.r.t. the environment can only be partially retrieved from its on-board sensors. In these situations, state estimation schemes can be exploited for recovering online the 'missing informa- tion' then fed to any planner/motion controller in place of the actual unmeasurable states. When considering non-trivial cases, however, state estimation must often cope with the nonlinear sensor mappings from the observed environment to the sensor space (the perspective projection performed by cameras being a classical example in this context [START_REF] Ma | An invitation to 3D vision[END_REF]). Because of these nonlinearities, the estimation convergence and accuracy can be strongly affected by the particular trajectory followed by the robot/sensor which, loosely speaking, must guarantee a sufficient level of excitation during motion [START_REF] Cristofaro | Optimal Trajectories for Multi Robot Localization[END_REF][START_REF] Achtelik | Path planning for motion dependent state estimation on micro aerial vehicles[END_REF].

For example, in the context of Structure from Motion (SfM), a poor choice of the system inputs (e.g., the camera linear velocity) can make the 3-D scene structure non-observable whatever the employed estimation strategy [Mar12, [START_REF] Eudes | Visuo-inertial fusion for homography-based filtering and estimation[END_REF][START_REF] Grabe | A Comparison of Scale Estimation Schemes for a Quadrotor UAV based on Optical Flow and IMU Measurements[END_REF], resulting, in practice, in inaccurate state estimation for trajectories with low information content. This, in turn, can degrade the performance of any planner/controller that needs to generate actions as a function of the reconstructed states, possibly leading to failures/instabilities in case of too large estimation errors [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF][START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF].

The dependence of the estimation performance on the robot trajectory, and of the control performance on the estimation accuracy, clearly creates a tight coupling between perception and action: perception should be optimized for the sake of improving the action performance, and the chosen actions should allow maximization of the information gathered during motion for facilitating the estimation task [START_REF] Valente | Information Gathering Control via Exploratory Path Planning[END_REF].

Topic of the thesis

With these considerations in mind, in this thesis we embrace the classical definition of robotics and we analyze the relationship between perception and action in robotic systems. Given the extent of the topic, we concentrate on the context of robot visual estimation/control and, in particular, on the class of Image Based Visual Servoing (IBVS) [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF] control schemes. Indeed, besides being a widespread sensor-based technique, see, e.g., [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF][START_REF] Gans | Stable visual servoing through hybrid switched-system control[END_REF][START_REF] Mahony | A port-hamiltonian approach to image-based visual servo control for dynamic systems[END_REF], IBVS is also a good example of all the aforementioned issues: on the one hand, whatever the chosen set of visual features (e.g., points, lines, planar patches and so on), the associated interaction matrix always depends on some additional 3-D information not directly measurable from the visual input (e.g., the depth of a feature point or the radius of a sphere). This 3-D information must then be approximated or estimated online via a SfM algorithm, and an inaccurate knowledge (because of, e.g., wrong approximations or poor SfM performance) can degrade the servoing execution and also lead to instabilities or loss of feature tracking [START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF]. On the other hand, the SfM performance is directly affected by the particular trajectory followed by the camera during the servoing [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF]: the IBVS controller should then be able to realize the main visual task while, at the same time, ensuring a sufficient level of information gain for allowing an accurate state estimation.

In this context, we propose an online trajectory optimization strategy that allows to maximize the converge rate of a SfM estimator, given some limitations on the maximum acceptable norm of the camera linear velocity. We also show how this technique can be coupled with the concurrent execution of a IBVS task using appropriate redundancy resolution techniques. All of the theoretical results presented in this thesis are further validated by an extensive experimental campaign that, in our opinion, fully support our claims.

Structure of the thesis

The core of this thesis is divided into three main parts. The first one presents a review and a state of the art of the main techniques used in computer vision, robot control, state estimation and active perception. The second and third parts present, instead the original contributions of this work in the context of active structure estimation from motion and its coupling with the realization of a servoing task. In the following we propose a brief summary of the content of each part.

Outline of Part I

In Part I we introduce some preliminaries related to computer vision and robotics and we propose an extensive review of the existing literature in the field. In this regard, we try to keep the focus of the reader on the importance of a correct coupling between perception and control by highlighting the possible issues that can arise when such a coupling is not correctly ensured.

In Chapt. 2 we start by summarizing, the basic model of image formation and the geometrical relationships between two images of the same scene taken from different camera points of view. The chapter continues by presenting the standard techniques used for modeling the kinematics of a robot and for controlling its motion. We concentrate, in particular, on the class of vision based motion control techniques with an emphasis on IBVS schemes.

In Chapt. 3 we review the standard techniques used in the literature for estimating the state of a robot and/or the structure of the environment from visual information. We propose a comparison between deterministic frameworks and probabilistic ones and, in particular, between the most common metrics used in either approach for measuring the "conditioning" of the estimation. This naturally leads us to the introduction of the concept of active perception as the correct paradigm to deal with the coupling between perception and action.

Outline of Part II

The second part of the thesis presents the contributions of this work in the context of active structure estimation from motion. The results contained in this part were the subjects of the author's original publications [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF][START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF][START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF][START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF][START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF][START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF].

In Chapt. [START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF] we propose a characterization of the dynamics of the estimation error in SfM. We show that, despite the nonlinearity of the problem, the error dynamics can be made approximately equivalent to that of a second order linear system with assigned poles if the estimation gains and the camera linear velocity are correctly selected online as a function of the current visual measurements. Subsequently, the chapter proposes the application of this general active perception scheme to a set of basic geometric primitives: point features, planes, and spherical and cylindrical targets.

In Chapt. [START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF] we present an extensive experimental validation of all of the theoretical claims of Chapt. 4 on a real robotic manipulator equipped with a camera in-hand. In particular, we show that, for all of the geometric primitives described in Chapt. 4, our strategy allows to correctly control and maximize (whenever possible) the convergence rate of the estimator given some constraints of the maximum allowed camera velocity norm.

Outline of Part III

The third part of this work addresses the more challenging problem of coupling active perception with the execution of a visual servoing (IBVS) task. Part of this material was presented in the author's publications [START_REF] Spica | Coupling Visual Servoing with Active Structure from Motion[END_REF][START_REF] Spica | A Framework for Coupling Visual Control and Active Structure from Motion[END_REF] and in [START_REF] Spica | Bridging Visual Control and Active Perception via a Large Projection Operator[END_REF] which, at the time of writing, is under consideration for publication.

In Chapt. [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF] we suggest a sensible control strategy for increasing the convergence rate of a SfM estimator in a way that is compatible with the execution of a main visual servoing task. We propose to use a large projection operator that allows to maximize the redundancy of the robot so as to give more "freedom" to the active estimation maximization, while still realizing the IBVS task. Next, we extend our solution by introducing an adaptive strategy that allows to tune the effect of the active estimation and even trigger its activation/deactivation as a function of the current state of the estimation error.

In Chapt. 7 we report, as done in Chapt. 5, a thorough experimental verification of the results of Chapt. [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF]. We demonstrate that the optimization of the camera motion during the transient of a IBVS scheme maximizes the convergence rate of a SfM estimator. Since the estimated quantities are used to calculate the interaction matrix for the considered IBVS task, this results, in turn, in a substantial improvement of the control performance. Finally we show that, by using an adaptive strategy based on the status of the estimator, one can also reduce the negative deformation effects of the active SfM optimization on the camera trajectory.

Conclusions and appendices

Chapter 7 concludes the description of the main contributions of this work. In addition to the content outlined so far, the thesis also contains an additional conclusive chapter and three appendices.

In Chapt. 8 we provide a final overall review of the main results of the thesis by also highlighting some open issues that still remain to be solved. Chapter 8 also proposes a certain number of possible extensions to this work that could be worth investigating. Some of them, are, indeed the subjects of the author's current research activity.

In Appendix A we include some additional technical details for the derivation of some of the results contained in the thesis. This content is not essential to understand the rest of this work, but it is nevertheless included here for the interested reader.

In Appendix B we report some preliminary original results in the context of dense structure estimation from motion using photometric information directly. In this case the camera image (intended as a dense luminance level map) is directly used to compute the innovation term of the SfM observer removing the need for any preliminary image processing (e.g. feature extraction, matching and tracking) step.

In Appendix C we give a very short and rather informal introduction to the vast topic of port-Hamiltonian (pH) systems since they provide an interesting intuitive and physical interpretation of the estimation and optimization schemes used in this thesis. We also introduce very briefly the bond-graph graphical representation of such systems.

Part I

Preliminaries and state of the art

Chapter 2

Computer vision and robotics fundamentals T his chapter provides an overview of the main topics this thesis is concerned with and the related state of the art. The material and results included here are well established and can be found in many computer vision and robotics textbooks. By no means, we can provide a complete picture of such a vast literature, but we will try, at least, to introduce the most essential concepts.

We begin the chapter with an introduction to computer vision in Sect. 2.1. A short historical prospective, in Sect. 2.1.1, foreruns a description of the basic geometrical aspects of the camera projection process (Sect. 2.1.2) and of the geometric relationships between two images taken from different points of view in Sect. 2.1.3.

After this, in Sect. 2.2, we move our focus to robotics. We start by introducing the classical modeling of robot kinematics (Sect. 2.2.1) and differential kinematics (Sect. 2.2.2) w.r.t. a certain task. This gives us all the necessary background information to tackle the problem of robot motion control in Sect. 2.2.4.

We conclude the chapter by putting together robotics and computer vision in the so-called Visual Servoing (VS) framework in Sect. 2.3. In particular, we concentrate our attention on Image Based Visual Servoing (IBVS) which is thoroughly described in Sect. 2.3.2.

Computer and robot vision

Computer vision is the research field that studies how a computer algorithm can exploit an image or, more in general, a sequence of images, to take decisions such as, for example, identify a possible danger, recognize a person or devise appropriate control inputs for a robotic system. This analysis is obviously many-sided as it can be expected given the richness and complexity of information contained in even a single image. In this section, however, we limit ourselves to reviewing the basic geometrical formalism of computer vision.

A brief history of electronic imaging

The history of computer vision starts in the 5-th century BC, in ancient China, when the Mohist philosopher Mozi documented, for the first time, the use of a pinhole to project, on a wall, the inverted image of a person [START_REF] Needham | [END_REF]. Western civilizations made the same discovery about a century later, when the Greek philosopher Aristotle described the effect in his book "Problems". This natural phenomenon, can sometimes be observed when sunlight filters through dense foliage and it is the working principle of some primitive animal eyes [START_REF] Land | The optical structures of animal eyes[END_REF] such as those of the cephalopod Nautilus in Fig. 2.1(a).

In the centuries that followed, many scientists, philosophers and artists experimented with the pinhole and contributed to the development of the so called camera obscura (see Fig. 2.1(b)), an elementary optical device that allows to project a neat image of the environment on a planar surface. In 1584, in his work "Magiae naturalis", the Italian scientist and philosopher Giambattista della Porta proposed, for the first time, the addition of concave and convex lenses to improve the focus and brightness of the images projected by the camera obscura. It was only in the 19-th century, however, that, thanks to the contributions of Thomas Wedgwood and Nicéphore Niépce, the device would be used, in conjunction with light-sensitive materials, to create the first permanent pictures (Fig. 2.1(c)).

These primitive cameras required an exposure time of several minutes and were, thus, not suited for real-time video capturing. The invention of the celluloid film by Kodak in 1889 finally made it possible to take instantaneous snapshots and videos at a reasonable cost. On February 12, 1892 Léon Bouly first patented the cinématographe, a single device that would allow the capturing and projection of motion movies. Unable to afford the patent fees and to further develop the idea, Bouly would sell the rights to the French engineers Auguste and Louis Lumière who would realize the first movie "Sortie de l'usine Lumière de Lyon" in 1895.

Cameras would have been of little use for robot control without the invention of electric image sensors. The first attempts to transduce light information into electric signals date back to the beginning of the 20-th century when Alan Archibald Campbell-Swinton discussed the use of cathode ray tubes as both capture and display devices to realize a "Distant Electric Vision" [START_REF] Campbell-Swinton | Distant electric vision[END_REF]. Such a device would actually come to light only thirty years later, when H. Miller and J. W. Strange at EMI, and H. Iams and A. Rose at RCA independently succeeded in transmitting the first images.

Equally fundamental for the development of computer vision, was the discovery of solid-state electronics. The unilateral conducting properties of certain crystals had been discovered by the German scientist Karl Ferdinand Braun in 1874, but it was not until the end of the 1930s that the American physicist R Shoemaker Ohl, at Bell Labs, would obtain a sufficiently pure silicon crystal to realize an efficient rectifying effect. Ohl also discovered and described the photo sensitivity of semiconductor junctions. As the story goes [START_REF] Riordan | The origins of the pn junction[END_REF], Ohl was examining the conducting properties of a cracked silicon crystal when he realized, by accident, that the voltage across the rod would suddenly increase when the crystal was flashed by a desk lamp. This discovery would later lead to the development of the solid-state photo-diode.

In the late 1960s attempts were made to use multiple photo-diodes, arranged in a matrix, to realize image sensors. Particularly promising was the Active Pixel Sensor (APS) technology in which each pixel photo-diode was coupled with its own integrated Complementary Metal-Oxide-Semiconductor (CMOS) transistor amplifier to increase noise rejection and reduce readout time. The beginning of electronic imaging was, however, marked by the invention of the Charge-Coupled Device (CCD) by Willard Boyle and George E. Smith at Bell Labs in 1969 [START_REF] Boyle | Charge coupled semiconductor devices[END_REF]. Michael Tompsett, also at Bell Labs, later demonstrated the potential of CCD devices for electronic imaging in 1971 [ABT71, TAB + 71]. The high variability in Metal-Oxide-Semiconductor (MOS) production processes and the instability, over time, of MOS transistors characteristics, eclipsed APS-MOS technology in favor of CCDs for image sensors. In the late 1980s and early 1990s, however, advances in the production process determined a resurgence in the use of CMOS sensors that nowadays dominate the market.

The pinhole camera model

Modern vision sensors are far more sophisticated than a simple pinhole camera. A complex optical system, composed of multiple lenses, is used to focus light on the actual image sensor. Each of these lenses is characterized by its own optical properties including its shape, refraction, absorption and reflectance indices, chromatic aberration, presence of impurities and so on. The projection process can, consequently, be extremely complex to describe accurately. In this work, however, as it is often the case for computer vision and robotic applications, we make the assumption that the camera optics is "good enough" to reduce all distorting effects so that the projection model can be reasonably approximated by that of an ideal pinhole camera with an infinitely small aperture. Under this assumption, only the light rays passing through the aperture position o C can enter the pinhole camera and hit the sensing surface on the back wall (see Fig. 2.1). We call this point the camera optical center. The axis passing through this point and perpendicular to the image plane is, instead, called the camera optical axis. The distance f between the image plane and the optical center (measured on the optical axis) is called the focal length.

It is convenient to define a reference frame F C centered in o C and with the z axis parallel to the camera optical axis. This is also referred to as a canonical retinal frame. Consider a generic 3-D point p = [X, Y, Z] T in frame F C . Using elementary geometry and triangle similarities, one can conclude that the light ray originating from p and passing through o C intersects the image plane in the point

ρ = -f X Z , -f Y Z , -f T in F C
, also called the perspective projection of p, see [START_REF] Ma | An invitation to 3D vision[END_REF]. Note that the same is true for any point along the projection line from o C to ρ, i.e. for any point defined as p = αp. A camera is, therefore, a scale invariant sensor: the projection of the 3-D world on the 2-D sensing surface causes a "loss" of information about the scale and the distance of the environment. Small objects close to the camera will look the same as larger objects further away from the sensor. This depth ambiguity is the core of the astonishing portraits by the Dutch artist Maurits Cornelis Escher an example of which can be seen in Fig. 2.2.

Vector ρ should then be considered as representing a 2-D direction rather than a 3-D point. More precisely one can say that ρ belongs to the 2-D projective space, i.e. the set of one dimensional subspaces (lines through the origin) of R 3 . Due to this scale ambiguity, it is perfectly licit to "re-scale" ρ in any way that is found convenient from a mathematical or numerical point of view. Moreover one can also By dividing ρ by its last component -f , e.g., one obtains the vector

π = ρ -f = 1 Z p ∈ P 2 (2.1)
where P 2 is the space of 3-D homogeneous vectors defined as

P 2 = {a : a ∈ R 3 , e T 3 a = 1}. (2.2)
Note that, the notation P 2 is often used in the literature to represent the projective space of dimension 2. For us, instead, it represents a particular representation of such space (the one based on homogeneous vectors). In this representation, π also corresponds to the projection of p on a virtual image plane, parallel to the vision sensor and placed at a unitary distance from o C in front of the optical center (see Fig. In this way, one obtains the unit-norm vector

η = π π = 1 p p ∈ S 2
where S 2 is the unit sphere, i.e. the space of 3-D unit-norm vectors defined as

S 2 = {a : a ∈ R 3 , a = 1}. (2.3)
In this representation, also called the spherical perspective projection, η corresponds to the intersection of the projection ray of p with a virtual spherical imaging surface with unit radius and centered in o C (see again Fig. 2.4). The spherical model is particularly convenient when dealing with cameras with a large Field Of View (FOV), such as catadioptric cameras, or when fusing images from different cameras pointing in different directions (generalized cameras) [START_REF] Fomena | Improvements on Visual Servoing From Spherical Targets Using a Spherical Projection Model[END_REF]. In fact one has, obviously, η = π/ π and, since π ≥ π 3 = 1, η is always well defined where as π = η/η 3 which is not defined for η 3 = 0 (points on the plane orthogonal to e 3 and passing through o C ).

Before concluding this section, we want to point the attention to the fact that, in an actual camera sensor, measurements are not obtained directly w.r.t. the canonical frame introduced above, but, instead, in terms of non-negative integer pixel indices (j, k) ∈ N 2 w.r.t. an image origin which, typically, corresponds to the upper left corner of the image. Assuming that all the pixels are rectangular and have the same size, the transformation from pixel indices to homogeneous coordinates π is affine and can then be written in a matrix form as

   j k 1    =    f d x 0 j o C 0 f d y k o C 0 0 1       x y 1    = Kπ (2.4)
where d x and d y are the pixel dimensions in metric units and (j o C , k o C ) are the coordinates, in pixels, of the camera principal point: the point in which the optical axis intersects the image plane. The matrix K contains a set of parameters that are peculiar to a specific camera and therefore is also called the intrinsic parameter matrix. Identifying K requires, in general, a calibration process and this motivates the fact that K is also called the camera calibration matrix. Camera calibration can be a complex problem and, as such, it has generated a vast literature (see, for example [START_REF] Duane | Close-range camera calibration[END_REF][START_REF] Tsai | A versatile camera calibration technique for highaccuracy 3d machine vision metrology using off-the-shelf tv cameras and lenses[END_REF][START_REF] Zhengyou | A flexible new technique for camera calibration[END_REF]). More complex, possibly nonlinear, transformations can also be considered, instead of (2.4), to model distortion effects. This topic is, however, beyond the scope of this thesis and an incredible amount of libraries and toolboxes are available to perform the calibration task. Among these one can mention ViSP [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] and OpenCV [START_REF] Bradski | Learning OpenCV: Computer Vision in C++ with the OpenCV Library[END_REF] just to give some examples. From this point on, therefore, we will always assume that the camera in use has been preliminarily perfectly calibrated and thus the intrinsic parameters are known so that it is always possible to recover the metric measurement π (or equivalently η) from the raw measurement (j, k). This allows us to carry on all further numerical developments in terms of the metric quantities π and η with the implicit assumption that (2.4) (or a more complex model) will be used to transform the sensor measurements in the actual implementation of the algorithms. Calibration errors will be ignored or treated as exogenous noise.

Finally one should mention that, for some computer vision applications, such as, for example, 3-D reconstruction from online videos [PNF + 08, Har94], it is not desirable or even possible to perform any camera calibration procedure. A vast literature exists concerning the use of uncalibrated cameras in computer vision and robot control. This is, again, beyond the scope of this thesis.

Geometry of multiple views

In Sect. 2.1.2 we explained how the light, emanating from a particular point in the environment, hits the image sensor in a specific position. This section is, instead, devoted to the relationship between the projection of the same point p on two different image planes belonging to different cameras or, equivalently, corresponding to a different position of a single moving camera.

Note that, in general, identifying the same point in two different images is all but a trivial task. The problem goes generally under the name of correspondence resolution or feature matching and tracking or, more in general, image registration and has been addressed in countless works (see, e.g., [START_REF] Mikolajczyk | A performance evaluation of local descriptors[END_REF][START_REF] Lucas | An Iterative Image Registration Technique with an Application to Stereo Vision[END_REF]). This thesis, however, is not concerned with the correspondence problem resolution and we assume that a robust tracking algorithm, such as those implemented in the ViSP or OpenCV libraries, allows to correctly identify the same point (or other geometric primitives) in different images.

The epipolar constraint

Consider a generic point p, fix two different camera poses corresponding to the reference frames F a , F b and let a R b represent the orientation of F b expressed in F a and a a t b be the coordinates of the origin of F b expressed in F a . The coordinates of p in the two frames are related by the affine transformation

a p = a R b b p + a a t b . (2.5)
From (2.1) one can write

a Z a π = a R b ( b Z b π) + a a t b .
To eliminate the (unknown) depths from the equation one can multiply both sides by

[ a a t b ] × obtaining a Z [ a a t b ] × a π = b Z [ a a t b ] × a R b b π.
Scalarly multiplying both sides by a π yields

0 = b Z a π T [ a a t b ] × a R b b π.
Finally, assuming b Z = 0, one can divide by b Z and obtain the epipolar constraint

a π T a E b b π = 0, (2.6)
where matrix a E b = [ a a t b ] × a R b is the essential matrix encoding the relative pose between F a and F b .

The epipolar constraint (2.6) can also be easily derived from geometric considerations. By looking at Fig. 2.5 one can immediately notice that the origins of the two frames o a and o b and the point p form a triangle and therefore lie on the same plane. The triple product between the three vectors a a t b , a π, and b π (all expressed in the same frame) must, therefore, be equal to zero. From Fig. 2.5 one can also immediately realize that such triangle collapses onto a line if o a , o b and p are aligned, i.e. if a a t b = ∅ 3 (pure camera rotation) or if a a t b is parallel to a π (translation along the projection ray of p). In this case the epipolar constraint (2.6) degenerates.

Matrix a E b belongs to the essential space, the subset of 3 × 3 real matrices obtained as the product of a skew-symmetric matrix and a rotation matrix:

E = E ∈ R 3×3 |E = [a] × R, R ∈ SO(3), a ∈ R 3 , where SO(3) = {R ∈ R 3×3 |RR T = R T R = I 3 , det (R) = 1}
is the special orthonormal group of dimension three. Since the epipolar constraint (2.6) is linear in the elements of the essential matrix, it is possible to estimate a E b , up to a single scalar factor (due to the homogeneous form of (2.6)), from a sufficient number of point correspondences. The classical work [START_REF] Longuet-Higgins | A computer algorithm for reconstructing a scene from two projections[END_REF] introduced an efficient algorithm based on the use of eight point correspondences to reconstruct a E b . This method is probably the most widely known and it is still adopted nowadays. Nevertheless, more recent algorithms have significantly reduced the number of necessary points. As a matter of fact one can notice that, despite having 9 elements, matrix a E b only possess 5 Degrees of Freedom (DOFs): 3 for the rotation and 2 for the translation up to a scalar factor. Kruppa [START_REF] Kruppa | Zur Ermittlung eines Objektes aus zwei Perspektiven mit innerer Orientierung[END_REF][START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF] demonstrated, indeed, that it is possible to reduce the number of necessary (non degenerate) point correspondences to 5, although the solution cannot be obtained in closed form. An efficient algorithm for solving the 5 points problem was proposed in [START_REF] Nistér | An efficient solution to the five-point relative pose problem[END_REF], and it is significantly more involved than the 8 points one.

Once a E b has been reconstructed, one can also decompose it into four solutions

[Lon81] a R b = U R T z ± π 2 V T , [ a a t b ] × = U R z ± π 2 SU T
with all possible combinations of ± and with U , V and S being the matrices of the Singular Value Decomposition (SVD) of a E b = U SV T , and R z (θ) being the matrix corresponding to a rotation of an angle θ about the z-axis. By imposing that a Z and b Z are both positive (otherwise p would be behind the image plane and hence not visible by the camera) one can actually exclude three of the above solutions and finally select the correct one. The resulting solution is still defined up to a single scalar factor due to the scale ambiguity of the projection process. If additional metric information is available, such as the norm of a a t b , one can then recover the scale and fully reconstruct the environment structure (p) and the transformation between the two camera poses ( a a t b , a R b ). In human beings, for example, learned knowledge of the distance between the eyes and of familiar objects size are used as priors for environment structure reconstruction and accurate navigation.

The homography constraint

It can be shown that, if the points used for the reconstruction of the epipolar matrix form some particular degenerate configurations, called critical surfaces [START_REF] Adiv | Determining three-dimensional motion and structure from optical flow generated by several moving objects[END_REF][START_REF] Longuet-Higgins | Multiple Interpretations of a Pair of Images of a Surface[END_REF], the standard 8-points algorithm fails. Most of these critical surfaces rarely occur in practice, however one of them is extremely common in all artificial environments: the plane. If the selected points lie on the same planar surface, one additional constraint can be exploited to obtain a more sensible algorithm for reconstructing the structure of the scene.

a p = a R b - a a t b b d b n T b p = a H b b p
where a H b is the homography matrix encoding both the roto-translation between F a and F b and the geometric parameters of plane P. Using again (2.1), multiplying both sides by matrix [ a π] × and assuming, again, b Z = 0, one finally obtains the homography constraint

[ a π] × a H b b π = ∅ 3 . (2.7)
We can now immediately understand why the epipolar constraint (2.6) cannot be used to reconstruct the essential matrix b E a in a planar case. In fact for any 3-D vector a one has:

a π T [a] × a H b b π = -a T [ a π] × a H b b π = 0, i.e. the constraint a π T E b π is satisfied for a set of matrices E = [a] × a H b that are
different from a E b and do not belong to the essential space.

As for the epipolar constraint, the homography constraint (2.7) is linear in the elements of the homography matrix. Differently from the epipolar case, however, only 4 point correspondences ( a π i , b π i ) are necessary in this case, provided that no three of them are collinear. In fact there is a total of 7 DOFs (3 for the rotation, 2 for the translation up to a scalar factor and 2 for the unit-norm plane normal) but each point contributes with 2 constraints using (2.7).

Following, e.g., [START_REF] Ma | An invitation to 3D vision[END_REF], the homography constraint (2.7) can be rearranged as . Since the homography matrix is defined up to a scale factor and the leftmultiplication by a H b is a left group action, one can also think of a H b as an element of the special linear group of dimension 3 SL(3) = {A ∈ R 3 | det (A) = 1}. This fact was exploited in [HMT + 11] to derive a nonlinear observer for the homography matrix.

b T i x = 0 where b i = b π i ⊗[ a π i ] × ∈ R 9×3 (
Using standard algorithms [START_REF] Ma | An invitation to 3D vision[END_REF], it is finally possible to decompose the associated recovered homography a H b into 4 solutions for R, t d and n (expressed in either frame). The positive depth constraint can be used, as done for the essential matrix, to exclude only two of them. The ambiguity among the remaining physically admissible solutions can only be resolved by exploiting prior knowledge of the scene (e.g., approximated known direction of n in one of the two frames, or comparison against the homography estimated from a third frame). Furthermore, as with the epipolar constraint, the structure of the scene and the camera translation remain defined up to a scalar factor (only the ratio t d can be recovered). Additional knowledge about either the camera motion or the scene 3-D structure is then still needed for solving this ambiguity. Moreover, similarly to the epipolar case, one can easily verify that if a a t b = ∅ 3 (pure camera rotation) or a a t b is parallel to a π (translation along the projection ray of p), constraint (2.8) is satisfied regardless of the value of t d and n and therefore point p cannot be used to identify these parameters.

The interaction matrix

For the development of the Visual Servoing control laws described in Sect. 2.3 it will be necessary to derive a description of the relationship between the camera motion and the motion of the projection of the environment on the image. To determine such a relationship we start by differentiating w.r.t. time the projection equation (2.1) obtaining

π = 1 Z ṗ - 1 Z 2 p Ż = 1 Z I 3 -πe T 3 ṗ.
Assuming that the point p is static in the environment one can write

ṗ = -v -[ω] × p
where v and ω are the camera linear and angular velocity expressed in the camera frame. Wrapping everything up one concludes that

π = -ζ I 3 -πe T 3 [π] × u = L π u.
(2.9)

with ζ = 1 Z and u = [v T , ω T ] T . Matrix L π ∈ R 3×6
is called the Interaction Matrix of the point feature. Note that the last row of L π ∈ R 3×6 contains only zeros. For this reason, in general, only the first two rows of (2.9) are considered:

ẋ ẏ =    - 1 Z 0 x Z xy -1 + x 2 y 0 - 1 Z y Z 1 + y 2 -xy -x    u. (2.10)
By repeating the same calculations one can easily show that [HM02]

η = -δ I 3 -ηη T [η] × u = L η u (2.11) with δ = 1 p . Note that L η ∈ R 3×6 , however rank (L η ) = 2 because of the con- straint η = 1.

Robot modeling and control

From a mechanical point of view, a manipulator is a set of rigid bodies, called links, connected by joints possibly actuated by motors. In particular, we will concentrate our attention to fixed-base, open-chain, fully-actuated robotic manipulators. In such robots, the first link, called base, is rigidly attached to the ground so that it does not move during normal operation. A single sequence (chain) of n -1 links, coupled by n joints, connects the base to the end-effector of the manipulator, where a tool, specific to the desired task (e.g. a camera), is attached. Moreover, each one of the joints is actuated by a motor.

Robot kinematics

As well known, the configuration space of a rigid body, i.e. its pose, comprises both its position and orientation w.r.t. a fixed inertial frame F W and can hence be thought of as an element of the special Euclidean group of dimension three:

SE(3) = R 3 × SO(3)
. The configuration space of n + 1 free rigid bodies of a manipulator is then given by SE(3)

× SE(3) × SE(3) • • • = SE(3) n+1 .
Nevertheless we made the assumption that the base of the manipulator is fixed to the ground and therefore its position and orientation are constant during normal operation. The links are also connected to each other by joints that constraint the space of possible motions by applying mechanical reaction forces to the links. Assuming infinitely stiff joints, this introduces kinematic constraints to the space of possible link velocities. Such constraints are of the holonomic type: they can be integrated into geometrical constraints on the manipulator configuration variables. This restricts the actual robot configuration space to a n-dimensional smooth sub-manifold Q of SE(3) n+1 , locally diffeomorphic to R n , that contains all possible configurations of the robot links. This subspace can be parametrized by a vector q = [q 1 , q 2 , . . . , q n ] T ∈ Q of n generalized coordinates. The vector q = [ q1 , q2 , . . . , qn ] T ∈ T q Q represents the generalized velocity of the manipulator and it is an element of T q Q, the tangent space of Q at q.

If the manipulator kinematic parameters (relative position and orientation of the joint axes) are known, the position and orientation of each link, and in particular of the robot end-effector, w.r.t. the fixed frame F W can be computed as a function of the robot joint configuration q only. This mapping is also called the robot direct kinematics. Standard conventions, such as the Denavit-Hartenberg parametrization, exist to evaluate the direct kinematics [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF]. Software toolboxes are also available to simplify the task [START_REF] Corke | Robotics, Vision & Control: Fundamental Algorithms in Matlab[END_REF]. Identifying the kinematic parameters of a robot is a process called kinematic calibration that usually requires moving the robot in different joint configurations while measuring the pose of its end-effector [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF].

The process can be considerably involved but many solutions have been proposed in the literature and specific software tools have been developed for the purpose [START_REF] Beyer | Practical robot calibration with ROSY[END_REF]. Often the robot manufacturer provides the geometric parameters of the robot links and the only thing that remains to identify is the hand-eye transformation between the end-effector frame and the reference frame of the tool/sensor (e.g. the camera) w.r.t. which the task is assigned [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF]. In the rest of the thesis, we will then assume that a preliminary calibration process has been performed for the robot in use so that the manipulator kinematic parameters are know with high accuracy.

Depending on the specific task that needs to be accomplished, one can then specify a m-dimensional task vector r ∈ R (e.g., the position and/or orientation of the end-effector, the distance between two points on two different links, the distance between the end-effector and a point in the world, and so on) and use the direct kinematics to calculate a task function or task-oriented direct kinematics

r : Q → R, q → r = r(q)
(2.12) that maps each joint configuration to the value of the task vector. The task space R is in general a smooth manifold, locally diffeomorphic to R m . In some cases, due to joint limits, only a subset Q a of the configuration space Q is actually admissible. The image of Q a through r is also called the workspace of the robot

WS = {r ∈ R | r = r(q) for some q ∈ Q a }
Note that the task function is, in general, not injective: a robot can, in fact, be redundant w.r.t. the task r ∈ WS and an infinite set of joint configurations q ∈ Q a might result in the same r. Moreover, in most cases, r is not surjective either: if r ∈ WS, there exists no configuration q in Q a (and possibly even in Q) such that r = r(q). The identification of the inverse mapping of (2.12), the inverse kinematics problem, is therefore a complex task and many resolution strategies have been proposed [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF].

Robot differential kinematics

Equation (2.12) relates the space of joint configurations Q to that of the task R. The task-oriented differential kinematics describes, instead, the relationship between the joint generalized velocities q ∈ T q Q and the task velocities ṙ ∈ T r R. This can easily be obtained by differentiating the task function (2.12) w.r.t. the joint generalized coordinates:

ṙ = J r (q) q (2.13)
where J r = ∇ q r T ∈ R m×n is the (analytic) task Jacobian. When the task vector r contains the pose of the end-effector w.r.t. a fixed world frame F W , one also calls (2.13) the robot analytic differential kinematics.

In many situations, the task is naturally expressed in terms of the end-effector pose. This is typically the case, for example, in robotic vision applications where the task is expressed in terms of a desired camera pose or a desired position of some objects in the camera image and the camera itself is rigidly attached to the robot end-effector. In these cases it is often convenient to think of r as composed of two parts:(i ) a robot direct kinematic function that, given the configuration q returns the pose of the robot end-effector w.r.t. the inertial frame; (ii ) an additional function that describes the task itself in terms of the end-effector pose. For these situations, as it will be clear in Sect. 2.3 it can be useful to calculate the robot geometric Jacobian that relates the joint generalized velocities q to the end-effector linear and angular velocities, typically expressed in the end-effector frame itself

v E W E ω E W E = u E = J E (q) q (2.14)
with J E ∈ R 6×n . Note that the geometric Jacobian does not contain partial derivatives as (2.13). In fact, if r contains the end-effector position t E ∈ R 3 and a minimal parametrization φ E ∈ R 3 of the end-effector orientation (e.g. Euler angles, Tait-Bryan angles, and so on), then one has:

v E W E ω E W E = I 3 O 3×3 O 3×3 T (φ E ) ṫE φE = I 3 O 3×3 O 3×3 T (φ E ) ∇ q r T q
where T (φ E ) ∈ R 3×3 describes the relationship between the parametrization derivative and the angular velocity. One can then conclude that

J E (q) = I 3 O 3×3 O 3×3 T (φ E ) ∇ q r T . (2.15)
Note, however, that, in general, J E (q) can be computed directly, and more conveniently, by exploiting the robot kinematics rather than using the expression (2.15) as explained, e.g., in [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF].

Another important role of the geometric Jacobian can be highlighted by considering (2.14) as representing the constitutive equations of a power preserving Modulated Transformer (MTF) in the jargon of bond graphs and port-Hamiltonian (pH) systems (see Appendix C). The joint and end-effector velocities q and u E = ( v

E W E , ω E W E
) clearly represent the flow variables in the transformer. One can then introduce the corresponding dual effort variables, τ and , that represent the generalized forces applied to the joints and those applied to the end-effector respectively MTF u E τ q (q) (see Fig. 2.7). Imposing that the total power is preserved by the MTF, one can write

qT τ = u T E = qT J E (q) T ⇔ τ = J E (q) T ,
i.e. the transpose of the geometric Jacobian describes the relationship between the generalized forces applied to the end-effector and the ones applied to the joints. This relationship, that goes under the name of kineto-static duality, can also be extended to the analytic Jacobian (2.13) as well as to the velocity transformation described by the feature interaction matrix (2.28). This latter possibility, for example, was exploited in [START_REF] Mahony | A port-hamiltonian approach to image-based visual servo control for dynamic systems[END_REF] to extend the classical VS framework to the control of robot dynamics.

Finally the second-order differential kinematics can also be easily obtained by differentiation w.r.t. time of the above first-order relationships: r = J r (q) q + Jr (q) q (2.16)

and v

E W E ω E W E
= J E (q) q + JE (q) q.

Kinematic singularities

For both the task analytic Jacobian in (2.13) and the geometric one in (2.14) there can exist particular configurations q for which the Jacobian looses rank. In correspondence to such configurations, called kinematic singularities, one can, e.g., find an infinite set of task velocities ṙ that cannot be realized by any choice of q. Moreover, close to singular configurations, large joint velocities q can result in very small task velocities ṙ. Exploiting the duality relationship between generalized velocities and forces, one can immediately realize that in correspondence to kinematic singularities there exist infinite forces in the task space that correspond to a zero torque for at least one of the joints. It is then intuitive that such configurations will arise, e.g., when two ore more joint axes are aligned as shown in Fig. 2.8. In this configurations, in fact, the applied force can be totally absorbed using torques e τ = 0 applied to the joints before and after the aligned links and by the robot structure of the aligned links.

Robot motion control

Once the robot kinematics has been identified, the control problem can be stated as follows: with the robot in an initial configuration q 0 ∈ Q a , identify a sequence of motor commands that allows to reach (one of) the robot configuration(s) q d ∈ Q a corresponding to a desired value of the task vector r d ∈ WS, i.e. such that r(q d ) = r d . Obviously the problem can only be solved if q d exists and q d ∈ Q a and q 0 ∈ Q a lie in the same connected component of Q a . From now on, we will always assume this to be true.

In a physical robotic manipulator, the joints are actuated by motors that apply forces/torques on the robot links. The resulting motion of the links is then a result of the robot dynamics which depend, in addition to the robot geometric properties (the same involved in the kinematic model described above), of the joint/links dynamic parameters such as mass, inertia tensor, elasticity, viscosity and so on. The dynamic model of the robot can be obtained, using either Lagrange or Newton methods as described, e.g. in [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF]. The dynamic parameters can be identified via an additional dynamic calibration procedure. Once this is done, dynamic control techniques can be used to compute the necessary motor commands and regulate the task to the desired value (see [ACGM94, DLMO00, MS12], and again [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF] for some examples of such dynamic control techniques). In most cases, however, knowledge of the dynamic model is not strictly necessary to obtain satisfactory performance. A kinematic command (typically a velocity) can, in fact, often be considered as system input in lieu of the motor forces and torques. This is made possible by the presence of lower level feedback control loops that are capable of executing any desired velocity reference, provided that this is physically admissible (i.e. sufficiently smooth) and does not exceed the robot actuators capabilities. The control architecture can thus be split into a velocity-level controller that generates velocity references, and a dynamics controller that ensures their realization. Commercial robots are typically equipped with such low-level feedback loops within "closed" architectures that, most of the time, do not even allow the user to have direct control over the torques and forces applied by the motors.

In this thesis we do not address the problem of controlling the robot dynamics. Instead, we assume that, thanks to the presence of a low-level dynamic control feedback, the robot can be modeled as an ideal integrator that takes as input a velocity reference q and produces as output a configuration q(t) = t t 0 q dτ . For our purposes, we will also devise some control laws that generate an acceleration level command q. This command will then be numerically integrated over time before sending it (as a velocity level command) to the robot low-level controller.

Control in the configuration space

A first possibility for controlling the robot motion is to first solve the inverse kinematics problem and calculate (one of) the joint configurations q d that satisfy r(q d ) = r d . If a solution exists, then the control law q = -k (q -q d ) , k > 0 (2.17) results in the exponential convergence of q to q d q(t) = q 0 e -λt + q d 1 -e -λt .

With this solution, the joint configuration will evolve from q 0 to q d in a straight line. Since, in general, r(q) is not a linear function, this results in unpredictable trajectories in the task space. In addition to this, if one wished to specify a time varying task trajectory r d (t), this strategy would require the resolution of the inverse kinematic problem online.

Control in the task space -the non-redundant case

An alternative solution is to devise a control law that regulates directly the task variable r avoiding the resolution of the inverse kinematics problem. This can be done by inverting the robot kinematics at a differential level, i.e. by solving the inverse differential kinematics problem. Assume that a task velocity ṙ is assigned. Also assume, for the moment, that n = m and that the task Jacobian is not singular, i.e. det (J r ) = 0. Under these assumptions, a joint velocity that realizes the desired task velocity can be easily obtained by inverting (2.13)

q = J -1 r ṙ. (2.18)
At this stage, the problem is simplified to that of choosing a sensible ṙ. If r d = const, using

ṙ = -k (r -r d ) = -ke, k > 0 (2.19)
with e = rr d , will result in the task error dynamics ė = -ke which yields the error behavior

e(t) = e(t 0 )e -λ(t-t 0 ) ∀t ≥ t 0 , (2.20)
and thus ensures the exponential convergence of r to r d . In this case, the task vector r will evolve in a straight line from its initial value r 0 = r(q 0 ) to r d1 . Since, in general, r(q) is not linear, the joint vector will instead follow a generic, non straight, trajectory. If one wished to solve a tracking problem in which the reference ṙd is not constant, then

ṙ = ṙd -k (r -r d ) = ṙd -ke, k > 0 (2.21)
with ṙd = dr d dt should be used instead of (2.19). If the robot is in a singular configuration then det (J r ) = 0 and the inverse Jacobian cannot be computed. More in general if ρ = rank (J r ) < m the linear system (2.13) does not have a solution and the robot kinematics are over constrained. Leveraging classical linear least-squares results, we can reformulate the inverse kinematics problem as an optimization one: we seek to find a solution q that results in a task velocity that is as close as possible, according to a specified metric W ∈ R m×m 0, to the desired one min q 1 2 (J r qṙ) T W (J r qṙ).

(2.22)

As well known, thanks to the convexity of the problem, a unique solution always exists and can be computed by using the (weighted) right Moore-Penrose pseudoinverse q = J † r r.

(2.23)

In particular, if rank (J r ) = n, one has

J † r = W -1 J r (J r W -1 J T r ) -1 .
If rank (J r ) < n, the pseudoinverse can still be computed by resorting on a SVD of J r [START_REF] Maciejewski | The singular value decomposition: Computation and applications to robotics[END_REF]. Note however that, while (2.23) is certainly useful for symbolic manipulations, the numerical calculation of the pseudoinverse plus its multiplication by the task velocity ṙ is less numerically accurate and much more computationally expensive than the direct resolution of the linear optimization problem (2.22) and, therefore, it should be avoided if possible [START_REF] Gill | Practical Optimization[END_REF]. The full control law can finally be calculated by substituting ṙ with, e.g., the expressions in (2.19) and results in the error dynamics ė = -kJ r J † r e.

Differently from before, the error evolution is not exponential in general and one can only prove local convergence of the task error to zero2 . Matrix J † r has a null space of dimension m -ρ and there exist values of the task error e ∈ ker (J † r ) that correspond to local minima in which the controller can get stuck [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF].

Control in the task space -the redundant case

A dual situation is the one in which n > m and rank (J r ) = m. In this case the kinematics of the robot are redundant w.r.t. the assigned task r and infinite solutions q can be found that result in the same value of ṙ. One simple solution in this case is to introduce an additional (n -m)-dimensional task function defined as:

r 2 = r 2 (q) ṙ2 = J r 2 (q) q , r 2 ∈ R (n-m) , J r 2 (q) ∈ R (n-m)×n .
One can then apply the same strategy as in the square case to the extended system:

ṙ ṙ2 = J r J r 2 q = J ex q, J ex ∈ R n×n .
This resolution framework, introduced for the first time in [START_REF] Seraji | Configuration control of redundant manipulators: Theory and implementation[END_REF], takes the name of task augmentation or extended Jacobian method. Its main disadvantage is the introduction of algorithmic singularities that arise when rank (J ex ) < n in spite of the fact that rank (J r ) = m and rank (J r 2 ) = n -m.

An alternative approach is to describe the problem, as done for the overconstrained case, with the jargon of linear least-squares optimization as, e.g.

min q 1 2 qT W q s.t. J r q -ṙ = ∅ m
where W ∈ R n×n 0. A unique solution always exists and can be computed by using the (weighted) left Moore-Penrose pseudoinverse q = J † r ṙ.

(2.24)

In particular, if rank (J r ) = m, one has

J † r = (J T r W J r ) -1 J T r W . (2.25)
The same considerations as above, concerning the disadvantages of computing the pseudoinverse instead of directly solving the optimization problem, remain valid for this case. Moreover, as shown in [START_REF] Chevallereau | A new method for the solution of the inverse kinematics of redundant robots[END_REF], a direct symbolic computation of the joint velocity, without explicit calculation of the whole Jacobian pseudoinverse, is also possible thanks to appropriate block decomposition of J r .

As it is well known from linear algebra, if J r ∈ R m×n and rank (J r ) = ρ < n, then the linear mapping (2.13) has a (n-ρ)-dimensional null space or kernel defined as ker

(J r ) = { q ∈ R n |J r q = ∅ m } .
Let us denote as P r ∈ R n×n the projection matrix whose image space corresponds to the null space of J r , i.e. J r P r = O m×n . If qr is a solution to (2.13), then also q = qr + P r qw , ∀ qw ∈ R n is a solution to (2.13). Using Lagrange's multipliers one can, in fact, easily demonstrate [START_REF] Siciliano | Robotics: modelling, planning and control[END_REF] that the solution to the optimization problem

min q 1 2 ( q -qw ) T W ( q -qw ) s.t. J r q -ṙ = ∅ m
is given by q = J † r ṙ + I n -J † r J r qw .

(2.26) with J † r as in (2.25). The matrix I n -J † r J r is then one of the possible matrices P r that allow to project vector qw into the null space of J r thus ensuring that (2.13) remains satisfied. Usually the joint velocity qw is chosen in the direction of the gradient of a secondary (scalar) objective function that one wishes to maximize qw = k r 2 ∇ q w(q), k r 2 > 0.

For this reason (2.26) is also called the projected gradient redundancy resolution technique [START_REF] Liegeois | Automatic Supervisory Control of the Configuration and Behavior of Multibody Mechanisms[END_REF]. Using (2.26), with ṙ as in (2.19) or (2.21), results again in the globally exponentially stable task error dynamics ė = -ke since the effect of qw is not visible in the space R. Moreover the dynamics of the secondary task will be described by:

ẇ = (∇ q w) T q = (∇ q w) T J † r r + k w I n -J † r J r ∇ q w .
If the primary and secondary task are compatible, i.e. (∇ q w) T J † r (r -J r ∇ q w) ≥ 0, then ẇ ≥ k w ∇ q w 2 > 0 and the secondary cost function will be optimized.

The projected gradient method has proven very effective for the resolution of the inverse differential kinematics problem for redundant manipulators, nevertheless its implementation requires a considerable computational effort when many DOFs are involved. In this regard, alternative and more efficient solutions have been proposed such as the reduced gradient method [START_REF] Luca | The reduced gradient method for solving redundancy in robot arms[END_REF]. When the application involves many different tasks that inherently have different priority levels, one can also consider the use of a task priority resolution framework [START_REF] Nakamura | Task-Priority Based Redundancy Control of Robot Manipulators[END_REF] that explicitly models the effect of the higher priority tasks on the lower priority ones. Along the same lines, the task sequencing redundancy resolution method [START_REF] Chiacchio | Closedloop inverse kinematics schemes for constrained redundant manipulators with task space augmentation and task priority strategy[END_REF] regulates each task, one at a time, from the one with the highest priority to the one with lowest one. This ensures that an "artificial" degree of redundancy is maintained during the resolution of all but the least priority task. Finally an incredible number of variations and possibly combinations of the above mentioned methods exists in the literature. For a survey on the matter, we suggest to refer to [START_REF] Siciliano | Springer handbook of robotics[END_REF]. For the purpose of this paper, however, given the simplicity of the involved task and the relatively small number of DOFs considered, we will limit our attention to the use of a simple projected gradient resolution strategy with the understanding that an extension to different redundancy resolution frameworks would also be possible.

Finally note that the problem of controlling the robot motion in the configuration space, described in Sect. 2.2.4.1, can be regarded as a special case of the more general task space control problem in which one has r = q and the task function and the task Jacobian correspond to the identity function. In this case, obviously, the task Jacobian is always square and full rank and (2.18), with (2.19), trivially reduces to (2.17).

Control at the acceleration level

We conclude this section by considering the extension of the redundancy resolution frameworks introduced so far to the case of an acceleration-level kinematic control. From (2.16) one immediately has r -Jr q = J r q, therefore the second order differential kinematics inversion problem is formally equivalent to the first order one, and all of the control schemes introduced above can be easily extended to the acceleration level by adopting the following substitutions in the formulas q → q, ṙ → r -Jr q, qw → qw .

Differently from before, however, at the acceleration level one also needs to make sure that a sufficient level of damping is introduced in the system to stabilize the second-order dynamics of the task error. If one designs

r = rd -k d ( ṙ -ṙd ) -k p (r -r d ) , k d > 0, k p > 0
with, possibly, rd = ṙd = ∅ m for regulation problems, then, in the redundant and square cases, the dynamics of the task error becomes

ë = -k d ė -k p e
and it is always globally exponentially stable.

A damping in the null space of the task must also be introduced to dissipate internal motions. This is obtained by using, e.g.

qw = ∇ qw( q) -k d,2 q.
If the internal low-level robot controller only accepts velocity-level commands (as it is the case for the robot used in the experiments of Chapts. 5 and 7) the acceleration reference, generated using the above strategy, must be numerically integrated over time before actually feeding it to the robot control unit.

Visual Servoing

For the control laws described in Sect. 2.2.4 to be implemented in practice, one needs to recover the current value of the task variable r (and possibly ṙ) needed to calculate the feedback terms in (2.19-2.21). In other words, one needs to recover an estimation of the current robot state in relation to the assigned task. This can be done by exploiting the presence of sensors, such as joint encoders, lasers, sonars, and so on. According to the definition given in [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF], the term Visual Servoing (VS), or visual servo control, refers, in particular, to the use of computer vision data (extracted from camera images) to control the robot motion. The first attempts of using a camera for robot control can be dated back to the early 70s [START_REF] Bolles | The use of sensory feedback in a programmable assembly system[END_REF][START_REF] Shirai | Guiding a robot by visual feedback in assembling tasks[END_REF]. These works where based on an approach to the problem that is fundamentally different from modern VS techniques: the camera would be used to take snapshots of the scene, understand the current situation and devise from that a new reference trajectory that would be fed to a motion control architecture; this latter would then move the robot "blindly", without directly exploiting any additional visual information, for a certain amount of time after which a new snapshot would be taken. This approach was mainly imposed by the very limited performance of the computing architectures available at that time and was abandoned with the progressive improvements in computational power. The term Visual Servoing was apparently introduced by the authors of [START_REF] Hill | Real time control of a robot with a mobile camera[END_REF] to distinguish their work from this pioneering use of visual feedback : the novelty of their work was the use of simple but fast image processing algorithms to allow the exploitation of visual information in real-time reactive behaviors. In essence, this is still the accepted definition of VS [START_REF] Corke | Visual Control of Robot Manipulators -A Review[END_REF]. As such, VS is more of a general paradigm rather than a specific collection of techniques. In fact, a variety of different implementations are possible tailored to the specific applications. Different types of cameras (perspective, catadioptric [START_REF] Barreto | Visual servoing/tracking using central catadioptric images[END_REF], or generalized cameras [START_REF] Comport | A visual servoing model for generalised cameras: Case study of non-overlapping cameras[END_REF]) both monocular and stereoscopic [START_REF] Hager | Robot feedback control based on stereo vision: Towards calibration-free hand-eye coordination[END_REF] can be used. Cameras can be mounted either on the robot end-effector (eye-in-hand [START_REF] Weiss | Dynamic visual servo control of robots: An adaptive image-based approach[END_REF]) or on an external fixed base (eye-to-hand [WWR93, RK98, HDE98]) or in a combination of both (as it is typical in humanoid robotics [START_REF] Agravante | Visual servoing for the reem humanoid robot's upper body[END_REF][START_REF] Taylor | Flexible self-calibrated visual servoing for a humanoid robot[END_REF]). Both fixed-base and mobile manipulators [MOP07, CAK99] can be considered. VS control formalism can also be applied to other sensors than traditional cameras such as RGB-D sensors [START_REF] Teuliere | Direct 3D servoing using dense depth maps[END_REF], camera/laser-stripe sensors [KMM + 96] and ultrasound probes [START_REF] Mebarki | Image moments-based ultrasound visual servoing[END_REF]. The development of a fully functioning VS system requires deep mastering of a wide range of techniques spanning the fields of image processing, computer vision, and control and estimation theory. Since this thesis is more concerned with the latter aspects, we refer the reader to specialized textbooks such as, e.g. [START_REF] Russ | The Image Processing Handbook[END_REF][START_REF] Szeliski | Computer Vision: Algorithms and Applications[END_REF], and we concentrate our attention to the control and estimation aspects involved.

General classification of Visual Servoing approaches

A large variety of different visual control schemes have been proposed in the literature. The reader can refer to the classical works [Cor93, HHC + 96] or the more recent two-parts review paper [CH06, CH07] for a complete overview. This multiplicity of solutions, however, are conceptually equivalent from a control point and can be treated with a unique formulation that is based on the interpretation of the visual information as a task variable in the sense explained in Sect. 2.2.4. Depending on the type of information that enters in the definition of the task variable, which in the context of VS is usually called the visual features vector, one can distinguish two main approaches:

• in Position Based Visual Servoing (PBVS) [START_REF] Wilson | Relative end-effector control using cartesian position based visual servoing[END_REF][START_REF] Thuilot | Position based visual servoing: keeping the object in the field of vision[END_REF] the visual information is used to estimate a set of 3-D parameters such as, e.g., the pose of the camera (or the robot end-effector) w.r.t. some reference coordinate frame.

In the computer vision literature, this is referred to as the 3-D localization problem. Once the localization is solved, the control problem is equivalent to a classical geometric control of the robot end-effector pose.

• in Image Based Visual Servoing (IBVS) the information extracted from the camera (e.g. the position of some key points or the contour of an object in the image) enters directly in the definition of the task vector and the robot pose is never reconstructed. The goal configuration is described in terms of the value that the features assume when camera is in the desired pose.

Both strategies present their advantages and disadvantages and the choice between the two can sometimes be determined by the specific application requirements. Since, for our purposes, both solutions are valid, we refer again the reader to [Cor93, HHC + 96, CH06, CH07] for a deeper discussion about the two approaches, and we dedicate the next session to provide additional details about the IBVS control framework.

Image Based Visual Servoing

Consider a camera, with an attached reference frame F C , that measures a set of visual features s ∈ R m (e.g., the x and y coordinates of a point, the parameters of some lines, and so on), possibly by resorting on some image processing algorithm. We make the assumption [ECR92] that the measurements s only depend on the shape of the part O of environment observed by the camera (e.g., the radius of a sphere/cylinder, the contour of a planar patch, and so on) and on the pose of the part w.r.t. the camera (e.g., the position p of a point, the orientation and distance of a plane, and so on). Let the shape be identified by a set of constant parameters θ. Let also F O be a reference frame attached to the object. Its pose w.r.t. the camera frame F C is then represented by the homogeneous transformation matrix

C M O = C R O C C t O ∅ T 3 1
.

If a fixed reference frame F W is also defined, we can express C M O as

C M O = C M W W M O
where W M O and W M C represent the pose of the object and the camera w.r.t. the world fixed frame.

In a eye-in-hand configuration, the object pose is fixed w.r.t. the world and the camera is rigidly attached to the robot end-effector with a constant transformation C M E so that its pose can be expressed as a function of the robot joint configuration q exploiting the direct kinematics as described in Sect. 2.2.1. One can then write

s = s( C M O , θ) = s( C M E E M W (q) W M O , θ) = s(q, C M E , W M O , θ).
In a eye-to-hand configuration, instead, the camera pose w.r.t. the world frame is fixed and the object of interest (e.g., a tool) is mounted on the robot end-effector with a constant transformation E M O so that its pose can be expressed, again, as a function of the robot joint configuration, hence

s = s( C M O , θ) = s( C M W W M E (q) E M O , θ) = s(q, C M W , E M O , θ).
A hybrid between the two situations is also possible. All of these configurations, despite their differences, are all formally equivalent in the sense that, by considering the C M E and W M O , or C M W and E M O , as part of θ, one can always write s = s(q, θ).

Due to this formal equivalence, from now on we will limit our attention to the eye-in-hand configuration case.

Note that, in some applications such as target tracking [START_REF] Chaumette | Positioning of a robot with respect to an object, tracking it and estimating its velocity by visual servoing[END_REF], one might have that neither W M O nor C M W can be considered constant because some external non-controlled agent is causing them to change with time. In these cases, which are not considered in this thesis, one might have s = s(q, θ, t).

The basic idea behind IBVS is to define the task vector in Sect. 2.2.1 as a function of the sole image features: r = s(q, θ).

(2.27)

The first step to define the VS control problem is to define a desired value of the task. Consider, for simplicity, the case of a regulation task in which a desired constant value s d is specified. As for the generic task oriented control framework described in Sects. 2.2.4.2 and 2.2.4.3, s d is defined as the value of s when the robot (or the camera) is in the desired configuration. This value can be either computed using (2.27) or, if possible, experimentally measured by manually moving the robot to the desired configuration. In this latter case, the goal of VS is to ensure that the robot can then reach this learned configuration regardless of its initial configuration.

In order to use one of the control schemes introduced in Sects. 2.2.4.2 and 2.2.4.3, one also needs to calculate the task Jacobian for the image features. In VS, rather than symbolically differentiating (2.27) w.r.t. q, it is in general convenient to exploit the fact that s is a function of the camera pose C M W and, hence, its derivative can be expressed as a function of the camera linear and angular velocity. In particular, as shown in Sect. 2.1.3.3, the following relationship holds [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF]

ṡ = L s (s, χ)u (2.28)
where L s ∈ R m×6 is the interaction matrix of the considered visual features, χ ∈ R p is a vector of unmeasurable 3-D quantities associated to s (e.g., the depth Z for a point feature or the radius R for a sphere), and u = (v, ω) ∈ R 6 is the camera linear/angular velocity expressed in the camera frame. Furthermore, one has

u = v ω = C T E u E = C T E J E (q) q
with J E (q) being the robot geometric Jacobian, introduced in (2.14), relating endeffector and robot joint velocities and

C T E = C R E C t E × C R E O 3×3 C R E
being the twist matrix that transforms linear and angular velocities from the endeffector frame to the camera frame. Note that matrix C T E contains the (constant) roto-translation between the end-effector frame and the camera frame. These quantities must be identified by a, so called, hand-to-eye calibration process, see e.g. the classical work [START_REF] Tsai | A new technique for fully autonomous and efficient 3d robotics hand/eye calibration[END_REF]. One can then introduce a camera geometric Jacobian, defined as

J C (q) = J v (q) J ω (q) = C T E J E (q), J C (q) ∈ R 6×n , (2.29)
and conclude that

J s (s, χ, q) = L s (s, χ)J C (q) ∈ R m×n (2.30)
is the visual task Jacobian associated to (2.27). To simplify the notation, whenever this does not lead to confusion, we will neglect the subscript s in J s . It is worth noting that, because of the structure in (2.30),

ρ = rank (J ) ≤ min{rank (L s ), rank (J C )} ≤ min{m, n, 6}
for any m and n. By then defining e = ss d as the visual error vector, one has ė = J q.

(2.31)

As explained in Sect. 2.2.4.3, in a redundant case w.r.t. the visual task (ρ < n), the standard choice for regulating e(t) → ∅ m is to apply the control law

q = -λJ † e + (I n -J † J ) qw = -λJ † e + P qw , λ > 0, (2.32)
where J † denotes the Moore-Penrose pseudoinverse of matrix J and qw ∈ R n is an arbitrary vector projected on the null-space of the main visual task through the action of the projector P = (I n -J † J ) ∈ R n×n . Vector qw is typically exploited for additional optimization purposes during the servoing, such as maximization/minimization of some suitable scalar cost function w(q). In non-redundant cases (ρ = n), P = O n×n and the control action (2.32) reduces to q = -λJ † e, λ > 0, (2.33) with all the system DOFs constrained by the realization of the visual task. If, instead, ρ = m (possible only if m ≤ min{n, 6}), i.e., if the servoing task is feasible for the given camera/robot system, both control actions (2.32-2.33) will result in a perfectly decoupled and exponential convergence for the visual error e(t) as in (2.20). Finally, if ρ < m (e.g., when m > n and/or m > 6), the visual task is overconstrained w.r.t. the camera/robot system and the ideal exponential behavior (2.20) will, in general, only be approximated during motion.

Remark 2.1. Note that, in general, the feature interaction matrix (2.28), and thus the IBVS task Jacobian (2.30), depend on some unmeasurable quantity χ, related to the scene 3-D structure, that cannot be directly measured using a camera sensor only. In particular, these geometric quantities only appear in the first 3 columns of the feature interaction matrix (2.28), i.e. those related to the camera linear velocity, see, e.g. 1/Z in (2.10). In practice, then, whenever the specified IBVS tasks involves the camera translational DOFs, none of the IBVS control laws introduced in this section, can be implemented exactly, but it will be necessary to substitute the actual J s (s, χ, q) with an approximation J s = J s (s, χ, q) with J s → J s for χ → χ. Regardless of the degree of redundancy of the considered robot/visual task combination, the error dynamics will be governed by the eigenvalues of matrix J s J † s . It is already clear, then, that ensuring a high estimation accuracy for χ will be crucial to guarantee that J s J † s 0 (so that e → ∅ m ) and, more in general, to improve performance of any IBVS control law.

Chapter 3 State estimation

As discussed in Sect. 2.3, the first attempts of using a camera to control robot motion used the vision sensor only to reconstruct the current state of the robot at some distant time instants. More modern PBVS approaches rely on visual information to recover an estimation of the current camera pose. Finally, even in IBVS frameworks, despite the fact that the camera pose is not directly entering in the definition of the task, the features interaction matrix in (2.28) and, as a consequence, the task Jacobian in (2.30), still depend on some 3-D geometric parameters that cannot be directly extracted from a camera image but, in general, must be estimated using a sequence of images together with some additional metric information. This need for recovering (online) an approximation of the current state of the system is not peculiar to VS applications, but characterizes pretty much all control problems to the point that it has led to the emergence of an entire dedicated branch of research that can take different names depending on the particular application and assumptions.

In this chapter we introduce and analyze the problem of state estimation in general, but with a focus to visual applications in particular. We start, in Sect. 3.1, by introducing the problem of state/parameter estimation from vision and by reviewing the main "branches" in which this has evolved in the literature. We dedicate the final part of Sect. 3.1 to position our work w.r.t. to these different approaches. In Sects. 3.2 and 3.3 we introduce the basic deterministic and probabilistic frameworks that have been proposed to solve the estimation problem. We also explain and compare the standard metrics that are commonly used to quantify the "well-posedness" of the estimation problem and the amount of information available about the quantities to be estimated. This naturally leads us to the introduction, in Sect. 3.4, of the active perception problem which is the main topic and motivation of this thesis.

Estimation from vision

Almost all animal species have, at least, a very simple eye-spot and about 96% of all known species have a proper eye, capable of recognizing patterns and control locomotion [START_REF] Land | The evolution of eyes[END_REF]. During the course about 600 million years, animal eyes have evolved in a multiplicity of different forms [START_REF] Land | The evolution of eyes[END_REF][START_REF] Land | The optical structures of animal eyes[END_REF]. The sense of vision has then brilliantly succeeded the test of natural selection and it is probably the most essential in our daily life experience. The main reason for this is to be found in the tremendous amount of information that vision can provide about the geometry, appearance (texture and color) and even the semantic content of the environment. In a similar way, a camera is probably one of the most informative sensors that a robotic system can be equipped with. Camera sensors also have the advantage of being very small and lightweight and usually, due to their intrinsically passive nature, more power efficient than other sensors (e.g. laser range finders, ultrasound and sonar sensors need to actively generate a signal to be able to acquire the information).

The power of vision, however, also comes at a considerable cost. It is estimated that about 40% of human brain pathways are connected to the retina and up to 50% of the neural tissue might be directly or indirectly devoted to the processing of visual information, more than all other senses combined. Visual information processing also takes two thirds of the electrical activity of our brain when we open our eyes [Fix57,[START_REF] Bowan | Integrating vision with the other senses[END_REF]. In a similar way, processing information from a vision sensor has proven to be an incredible challenge for engineering and science. Computer vision algorithms are often extremely eager of computational power to the point of being one of the main factors pushing forward the development of highly efficient and specialized computer architectures [START_REF] Fung | Openvidia: parallel gpu computer vision[END_REF][START_REF] Dias | Hardware, Design and Implementation Issues on a Fpga-Based Smart Camera[END_REF].

In this thesis, we are only concerned with the geometric aspects of computer vision. The first work that was directly dedicated to the mathematical modeling of the problem of reconstructing the geometric structure of a scene from two (projected) views is attributed to Kruppa [START_REF] Kruppa | Zur Ermittlung eines Objektes aus zwei Perspektiven mit innerer Orientierung[END_REF][START_REF] Ma | An invitation to 3D vision[END_REF]. Kruppa was, in particular, interested in the problem of estimating both the structure of the scene and the relative pose between the two view points. This problem is known in computer vision as Structure from Motion (SfM). As explained in Sect. 2.1.3.1, Kruppa proved that 5 point correspondences from two different points of view, are sufficient to solve the problem up to a finite number of solutions (and to a scale factor). With the increase of computers computational power, research became interested in using a large number of images (instead of just two), both calibrated and not calibrated, to accurately estimate both the structure of complex environments (represented by thousands of points) and the camera poses in each of the view points. In this context one can mainly distinguish two different approaches.

Typical SfM algorithms, stemming from the computer vision community, process altogether a large number of images, usually from uncalibrated cameras, in an offline optimization process. This optimization is also known as bundle adjustment [START_REF] Triggs | Bundle adjustment -a modern synthesis[END_REF] and was initially conceived for photogrammetry applications. It can lead to very impressive results in the reconstruction of complex scenes as demonstrated in, e.g., [START_REF] Snavely | Photo tourism: Exploring photo collections in 3d[END_REF].

A different approach, more typical of robotic applications, is to process the set of images sequentially, as new frames are captured by the camera, in real time. In these approaches one is typically mainly interested in the relative pose C k-1 M C k of the camera between frame k -1 and frame k (although sometimes more than two frames might be processed to improve accuracy). If necessary, a pose w.r.t. the initial frame (usually chosen as the fixed reference) can be computed by integrating the sequence of estimated transformations, i.e.

W M C k = C 0 M C 1 C 1 M C 2 . . . C k-1 M C k .
Because of this integration process, the problem is typically known as Visual Odometry (VO) [START_REF] Scaramuzza | Visual odometry, part 1: The first 30 years and fundamentals[END_REF][START_REF] Fraundorfer | Visual odometry, part ii: Matching, robustness, optimization, and applications[END_REF] for its similarity with the more classical wheel odometry. Even if VO is not directly interested in the reconstruction of the scene geometry, bundle adjustment can sometimes be applied to a limited selection of the most recent frames (sliding window bundle adjustment) to improve accuracy.

Somewhat in between these two approaches is the, so called, Vision based SLAM (V-SLAM) problem [START_REF] Paz | Large-scale 6-DOF SLAM with stereo-in-hand[END_REF][START_REF] Davison | Real-time simultaneous localisation and mapping with a single camera[END_REF]. This is an extension of the more general Simultaneous Localization and Mapping (SLAM) problem [START_REF] Durrant-Whyte | Simultaneous Localization and Mapping: Part I[END_REF][START_REF] Bailey | Simultaneous Localization and Mapping (SLAM): Part II[END_REF] to the case of visual measurements. In V-SLAM, similarly to SfM, the focus is on reconstructing both the camera pose (localization) and the structure of the environment (mapping) w.r.t. a global reference frame (typically coincident with the first image). Differently from SfM, however, in V-SLAM there is a temporal causality relationship between the frames. The images (and the corresponding camera poses) are normally processed in a sequence, and the camera motion is integrated, similarly to what happens in VO, until the moving camera revisits a certain part of the scene. When this loop closure arises, a global optimization is performed that involves both the camera pose and the section of the map correlated (in a statistical sense) to the loop path. While in SfM the estimation problem is usually solved using batch optimization techniques (such as bundle adjustment) both V-SLAM and VO mostly adopt recursive filtering strategies, such as (Extended/Unscented) Kalman Filters, Particle Filters and Recursive Least Squares/Maximum Likelihood estimators.

Note that the boundary line between V-SLAM and SfM is often very blurry. The use of the two terms seems sometimes mostly related to the "traditions" of different scientific communities with SfM being more common in the computer vision community and V-SLAM being more widely used in the robotics one. Sometimes, in fact, the term real-time SfM is also used to refer to V-SLAM [CFJS02, DRMS07, SMD10].

Finally, both VO and V-SLAM algorithms, usually assume that the camera intrinsic parameters are known from some preliminary calibration process although some exceptions can be found in the literature [START_REF] Koch | Ground robot navigation using uncalibrated cameras[END_REF][START_REF] Taddei | Planar motion estimation and linear ground plane rectification using an uncalibrated generic camera[END_REF].

Regardless of the approach taken, the problem of estimating the 3-D geometric structure of a scene and/or the camera pose from a sequence of 2-D images acquired by a camera belongs to the class of the so-called inverse problems: starting from the result of a physical process (the images obtained by projection) one has to infer the cause (the 3-D structure and camera pose) that originated it. This type of problems is usually difficult to solve and mathematically ill-conditioned because the information contained in the measurements is typically not sufficient to entirely reconstruct the process, unless additional priors (or measurements) can be introduced. In the case of vision, the projection process described in Sect. 5.1.1 reduces the 3-D world into a 2-D image. The third dimension is "lost" in the process and this causes the scale ambiguity that affects image measurements (see Sect. 5.1.1). By no means this scale can be reconstructed from the 2-D images only: additional metric information (such as the known dimension of an object in the scene or the metric distance between the camera view points in the different images) must also be included. Priors on the position/shape of some observed objects or on the initial camera motion have been assumed, e.g., in [ZKA + 09, KM07]. Another possibility, often exploited in SfM [START_REF] Xue | A new approach for the bundle adjustment problem with fixed constraints in stereo vision[END_REF], VO [START_REF] Nistér | Visual odometry for ground vehicle applications[END_REF] and V-SLAM [SBO + 10] is to use a stereo vision sensor with a known fixed distance between the left and right cameras. This strategy is also adopted by many animals, including humans, that have (at least) two front facing eyes with overlapping fields of view. An alternative, is to use a single monocular moving camera and exploit additional sensors to retrieve a metric measurement of the distance between multiple views. In some cases a monocular camera can be mounted on a slider mechanism as in [START_REF] Moravec | Obstacle avoidance and navigation in the real world by a seeing robot rover[END_REF]. This is conceptually equivalent to stereo-vision and it is a strategy also adopted in nature: to increase their FOV and effectively detect predators, some animals (like pigeons) have sideways eyes with scarcely overlapping fields of view; by bobbing their heads back and forth they can however recover depth perception. Another possibility is to use additional metric sensors, such as joint encoders [START_REF] Chaumette | Structure from controlled motion[END_REF], Inertial Measurement Units (IMUs) [START_REF] Grabe | Nonlinear ego-motion estimation from optical flow for online control of a quadrotor uav[END_REF][START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF], sonar [START_REF] Honegger | An open source and open hardware embedded metric optical flow cmos camera for indoor and outdoor applications[END_REF] and so on, to recover some metric information such as the motion of the camera or the distance from the scene and thus reconstruct the scale of the environment. Finally, note that the monocular estimation problem is also interesting when dealing with a stereo sensor. As a matter of fact, when the distance to the environment is much larger than the distance between the left and right cameras, stereo-vision degenerates to the monocular case. This is the reason why, e.g., panorama pictures look in general more realistic than close range ones.

With respect to these previous works, in this thesis we address the problem of 3-D structure reconstruction from monocular image measurements and known and controlled camera motion [START_REF] Chaumette | Structure from controlled motion[END_REF]. This is, in fact, the typical situation when considering robotic fixed-base manipulators, where the camera is attached to the robot end-effector and its velocity can be accurately measured and controlled using the robot differential kinematics (2.14) and joint encoders and motors. Even when dealing with mobile or flying robots, one can sometimes assume that at least a rough estimate of the camera velocity is known from wheel odometry or aerodynamic drag [START_REF] Allibert | Estimating body-fixed frame velocity and attitude from inertial measurements for a quadrotor vehicle[END_REF]. However, presence of non negligible dynamics, non-holonomic constraints [START_REF] Fruchard | A framework for the control of nonholonomic mobile manipulators[END_REF][START_REF] Siciliano | Robotics: modelling, planning and control[END_REF] and underactuation [START_REF] Altuğ | Control of a quadrotor helicopter using visual feedback[END_REF][START_REF] Asl | An adaptive scheme for image-based visual servoing of an underactuated UAV[END_REF] complicates the control problem and thus the study of this kind of platforms was not considered in this thesis and is left to future extensions.

As in SfM, we are mostly interested in retrieving the scene 3-D geometry. However, w.r.t. SfM works, we do not consider arbitrarily complex scenes, but instead we limit our attention to a class of basic geometric primitives (points, spheres, cylinders, planes) that can be treated in close form and efficiently estimated. Similarly to VO and V-SLAM, we use a real-time recursive estimation framework. Our approach is particularly similar to the "sensor-based" or "ego-centric" filtering strategy presented in [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF]. In both cases, the robot/camera builds a 3-D model of the environment in its own body/sensor frame via a filtering technique: a Kalman Filter (KF) in [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF] and similar works, and a deterministic nonlinear filter in our case. In fact, differently from most V-SLAM papers, we do not explicitly model noise in our estimation process but, instead, we adopt a deterministic observer, originally introduced in [START_REF] De Luca | On-Line Estimation of Feature Depth for Image-Based Visual Servoing Schemes[END_REF], that is capable (thanks to its stability properties) of rejecting the effect of noise as an exogenous disturbance. The main advantage in doing so, is that this will allow us to recover the dynamics of the error in closed form and thus to predict the performance of the estimation and also to actively optimize it using classical control techniques. The main focus of this thesis, in fact, is not the estimation problem per se, but rather the control of the camera motion to guarantee that the estimation is well defined. Indeed, as already anticipated in Sect. 2.1.3, certain camera motions may result in a degeneration of the multiple view geometry. As it will be experimentally demonstrated in Chapt. 5, the control policy that we propose results in improved estimation performance regardless of the adopted estimation algorithm.

Deterministic frameworks

In deterministic estimation frameworks the presence of noise in the system dynamics or in the measurement process is ignored. The design of the estimation algorithm is based on the nominal system or, equivalently, on the expected value of a stochastic system. Care is taken to always ensure that the resulting estimation error dynamics is (possibly globally and exponentially) asymptotically stable. In essence this means that the estimation will always "tend to" match with the real system state and will reject the disturbing effect of unmodeled parts of the system dynamics such as noise.

The Luenberger observer

The first mathematical theory behind recursive estimation for linear systems is due to David Luenberger [START_REF] Luenberger | Observing the state of a linear system[END_REF][START_REF] Luenberger | Observers for multivariable systems[END_REF]. Consider the state space representation of a linear time-invariant dynamic system

P : ẋ = Ax + Bu s = Cx + Du (3.1)
where x ∈ R q is the system dynamic state that is initially equal to x 0 (unknown), u ∈ R v is the input vector, s ∈ R m is the measurable output, A ∈ R q×q is the system matrix, B ∈ R q×v is the input matrix, C ∈ R m×q is the output matrix and finally D ∈ R m×q . The idea behind the Luenberger observer is to construct an artificial dynamic system that "imitates" the real one. The reasoning behind this is that if, given the same input u, the outputs s and s generated by the real system and by the observer are identical (up to the differential level q), then the two systems must be in the same state and one can use the state x of the observer as an estimation of the state x of the real system. If the matrices A, B, C and D are known, a sensible way of constructing an observer for (3.1) is the Luenberger Observer (LO)

˙ x = A x + Bu -K ( s -s) s = C x + Du (3.2)
where K ∈ R q×m 0 is a constant gain. With this choice it is easy to verify that the dynamics of the error x = xx is given by

˙ x = (A -KC) x
and its solution

x = e (A-KC)(t-t 0 ) x 0 converges to zero, regardless of the value of x 0 , if (A -KC) ≺ 0. Note that the input u does not appear in the error dynamics and thus, it has no effect on the convergence of the estimation. This property only characterizes, in general, linear systems.

The conditions for the existence of a gain K s.t. (A -KC) ≺ 0 were studied by Kalman who first demonstrated that the gain K always exists iff the system is observable [START_REF] Kalman | Contributions to the theory of optimal control[END_REF], i.e. if and only if

rank O = q, with O =       C CA . . . CA q-1       . (3.3)
or, equivalently,

G = t t 0 e A T (τ -t 0 ) C T Ce A(τ -t 0 ) dτ 0, for some t > t 0 .
where G ∈ R q×q is also called the Observability Gramian (OG) of the system. Note, again, that for linear time-invariant systems, the observability is an intrinsic property of the system and only depends on the structure of matrix A and C and not on the inputs applied to the system. Interesting for the following considerations is the fact that a system is observable iff one cannot find a state transformation such that:

       ẋ = Ǎ11 O m×(q-m) Ǎ21 Ǎ22 x + Bu š = Č1 O m×(q-m) x + Ďu
i.e. a part of the state (the bottom one in this case) does not have any effect on either the rest of the state or the measurements.

The observability results also extend to linear time-varying systems of the form:

P : ẋ = A(t)x + B(t)u s = C(t)x + D(t)u . ( 3.4)
In this case, the OG is given by

G = t t 0 Φ(τ, t 0 ) T C(τ ) T C(τ )Φ(τ, t 0 ) dτ 0, for some t > 0. (3.5)
where Φ(t, t 0 ) is the state transition matrix corresponding to A(t) that satisfies

x(t) = Φ(t, t 0 )x(t 0 ) + t t 0 Φ(t, τ )B(τ )u(τ ) dτ.

Nonlinear state observation

The case of nonlinear systems is generally more complex than that of linear ones. Consider a generic nonlinear system P described by

P : ẋ = f (x, u) s = g(x, u)
.

As with the linear case one can build a state observer whose dynamics is a composition of a prediction term, which tries to make the observer evolve as the real system, and an innovation term, which uses the discrepancy between the outputs s and s generated by the two systems to compensate for the fact that the observer was initialized from an initial state x 0 that is different, in general, from the initial state x 0 of the real system. Differently from linear systems, however, the design of these terms must be done, in general, case by case without relying on any general design rule apart from some specific classes of nonlinear systems. An extension of the concept of observability to nonlinear systems was proposed in [START_REF] Hermann | Nonlinear controllability and observability[END_REF]. There, two states x 0 and x 1 are defined as indistinguishable if, starting from x 0 and x 1 and for every admissible input u(t), t ∈ [t 0 , t f ], the system P produces identical outputs s(t), t ∈ [t 0 , t f ]. A system is then observable if for every state x, the set of states indistinguishable from x is equal to x itself. If this is true, then it is possible to reconstruct the state from which the system has started (and consequently the current state) by looking at the input-output map over a certain period of time. Note that the fact that a (nonlinear) system is observable does not imply that every admissible input u(t), t ∈ [t 0 , t f ] allows to distinguish two states by looking at the output s, but only that there exists at least one input u(t), t ∈ [t 0 , t f ] that allows such a distinction. Moreover, in general, a system may not be instantaneously observable: it might be necessary to travel for a long distance/time to be able to distinguish a state x from other states. Because of this, the authors of [START_REF] Hermann | Nonlinear controllability and observability[END_REF] introduce the concept of local weak observability. Let U be a subset of the state space, x 0 is Uindistinguishable from x 1 if none of the inputs u(t), t ∈ [t 0 , t f ] that, starting both x 0 and x 1 , result in a trajectory that lies in U , i.e such that x(t) ∈ U, ∀t ∈ [t 0 , t f ], allows to distinguish x 0 from x 1 . The system P is then said locally weakly observable if, for each state x ∈ R q , there exists an open neighborhood U such that, for every open neighborhood V ⊆ U of x, there is no state in V (other than x) that is V -indistinguishable from x. Intuitively this means that there exist some input that allows to instantaneously distinguish a state from its neighbours, see [START_REF] Hermann | Nonlinear controllability and observability[END_REF]. If a system is locally weakly observable, then the system state can be estimated from the measured outputs, the known control inputs and a certain number (depending on the particular system) of their derivatives. We want to stress again the fact that not all admissible inputs might be appropriate for the estimation to be effective, even if the system is observable. [HK77] also introduces a simple algebraic test for local weak observability. To simplify the algebra we limit our attention to driftless control-affine systems that have the form:

P :        ẋ = L(x)u = v i=1 l i (x)u i s = g(x)
.

with L(x) ∈ R q×v and l i (x) ∈ R q . Let us denote as L l g the Lie derivative of g w.r.t. l defined as:

L l g(x) = ∇ x g(x) T l(x) ∈ R q .
Lie differentiation is a recursive operation and we can define

L l 2 L l 1 g(x) = ∇ x L l 1 g(x) T l 2 (x), L k l g(x) = ∇ x L k-1 l g(x) T l(x), with L 0 g(x) = g(x).
We can now build a matrix O analogous to the one in (3.3) by piling up the single matrices obtained by taking the Jacobians of the Lie derivatives of g(x) w.r.t. l i .

If this matrix has rank q, then the system is locally weakly observable and an estimator can be designed that reconstructs x from measurements of s and known (and appropriate) inputs u.

To give an example let us consider the dynamics of a single point feature. Let us define x = (η, δ) ∈ R 4 and u = (v, ω) as in Sect. 2.1.3.3. The derivative of δ can be easily calculated as:

δ = d dt 1 p = p T p 3 ṗ = p T p 3 (-v -[p] × ω) = -δ 2 η T v.
Therefore, using also (2.11), we have

P :        ẋ = -δ(I 3 -ηη T ) [η] × δ 2 η T ∅ T 3 u s = η = I 3 ∅ 3 x . (3.6)
We begin by defining the zero-th order Lie derivative and its Jacobian:

L 0 g = η, ∇ x L 0 g T = I 3 ∅ 3 (3.7)
The first order Lie derivatives w.r.t. l i , i = 1, . . . 6 are given by the columns of

L 1 l i g = column i of -δ(I 3 -ηη T ) [η] × .
Piling up their gradients and (3.7), we conclude

O =              I 3 ∅ 3 δ(η T e 1 I 3 -ηe T 1 ) -(I 3 -ηη T )e 1 δ(η T e 2 I 3 -ηe T 2 ) -(I 3 -ηη T )e 2 δ(η T e 3 I 3 -ηe T 3 ) -(I 3 -ηη T )e 3 -[e 1 ] × ∅ 3 -[e 2 ] × ∅ 3 -[e 3 ] × ∅ 3              ← v x ← v y ← v z ← ω x ← ω y ← ω z
where we highlighted the rows corresponding to each input. It is easy to verify that O has rank 4 by taking the rows number 1, 2, 3, 4, 8, 12

           1 0 0 0 0 1 0 0 0 0 1 0 2δη x 0 0 η 2 x -1 0 2δη y 0 η 2 y -1 0 0 2δη z η 2 z -1           
and considering that, since η = 1, the last column cannot contain only zeros. The Structure from Known Motion problem is, therefore, observable as already shown, e.g. in [START_REF] Soatto | On the Exact Linearization of Structure From Motion[END_REF]. However, this does not mean that any input u allows to distinguish between different states. In fact if v = αη the system reduces to:

P :        ẋ = ∅ 3 [η] × δ 2 ∅ T 3 u s = η = I 3 ∅ 3 x
.

with now u = u = (α, ω). If we repeat the calculation of O we obtain

O =         I 3 ∅ 3 O 3×3 ∅ 3 -[e 1 ] × ∅ 3 -[e 2 ] × ∅ 3 -[e 3 ] × ∅ 3         ← α ← ω x ← ω y ← ω z
that can never have rank 4. This is clearly due to the fact that, with this choice of inputs, a part of the system state (δ) does not affect neither the output nor the dynamics of the other state components. In general, in the case of nonlinear systems, together with the above observability conditions, one will have to impose a persistent excitation condition on the system inputs. In other words, apart from trivial cases in which the state can be directly measured by inverting s, one will have to guarantee that the inputs are selected in such a way that every component of the state x has an effect (possibly only for a limited amount of time) on the history of the output s.

A nonlinear observer for SfM

In this section we focus our attention to a particular class of nonlinear systems which are typical of SfM problems. For the reader's convenience, we start recalling here a classical formulation of the Persistence of Excitation (PE) Lemma in the context of adaptive control as stated in, e.g., [START_REF] Marino | Nonlinear Control Design: Geometric, Adaptive and Robust[END_REF].

Lemma 3.1 (Persistence of Excitation). Consider the system1 :

˙ s = -H s + Ω T (t) χ ˙ χ = -αΩ (t) s . (3.8)
where s ∈ R m , χ ∈ R p , H 0, and α > 0. If Ω (t) and Ω (t) are uniformly bounded and the PE condition is satisfied, that is, there exists a time interval T > 0 and a scalar γ > 0 such that

t+T t Ω (τ ) Ω T (τ ) dτ γI p 0, ∀t ≥ t 0 , (3.9)
then ( s, χ) = (∅ m , ∅ p ) is a globally exponentially stable equilibrium point.

Let now x = [s T χ T ] T ∈ R m+p be the state of a dynamical system, where s ∈ R m represents a measurable component of x and χ ∈ R p an unmeasurable one. Assume further that the following holds

ṡ = f s (s, u) + Ω T (s, u)χ χ = f χ (s, χ, u) (3.10)
with u ∈ R v being an input vector, and Ω(t) ∈ R p×m a generic but known timevarying quantity. Note that in formulation (3.10) vector χ is required to appear linearly in the dynamics of s (first equation). Furthermore, matrix Ω(•) ∈ R p×m and vectors f s (•) ∈ R m and f χ (•) ∈ R p are assumed to be generic but known and sufficiently smooth functions w.r.t. their arguments which are all available apart from the unknown value of χ in f χ (•). For the case of structure from motion for point feature, considering, again, (3.6), one has for example s = η, χ = δ, and

u = u        f s (s, u) = [s] × ω Ω(s, u) = -(I 3 -ss T ) f χ (s, χ, u) = χ 2 s T v
Exploiting (3.8-3.9), a sensible estimation scheme for retrieving the (unmeasurable) value of χ can be devised, as suggested by [DOR07, RDO08, DOR08], in the following way: let x = [ s, χ] T ∈ R n+p be the estimated state, s = ss, χ = χχ, x = [ s T χ T ] T , and design the following update rule

˙ s = f s (s, u) + Ω T (s, u) χ -H s ˙ χ = f χ (s, χ, u) -αΩ(s, u) s . (3.11)
The error dynamics then takes the form

       ˙ s = -H s + Ω T (s, u) χ ˙ χ = -αΩ(s, u) s + [f χ (s, χ, u) -f χ (s, χ, u)] = -αΩ(s, u) s + d( x, t)
.

(3.12) System (3.12) matches almost perfectly the formulation (3.8) apart from the 'spurious' term d( x, t). This can be considered as a vanishing disturbance for the nominal system (3.8), i.e., such that d(∅ p , t) = ∅ p . Therefore, it is typically possible to still prove local exponential convergence of the origin of (3.12) by resorting to Lyapunov arguments and by imposing suitable bounds on the initial condition x(t 0 ) and/or on d( x, t) , see [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF][START_REF] Giordano | 3D structure identification from image moments[END_REF] for some examples in this sense.

The PE condition (3.9) plays the role of an observability criterion: convergence of the estimation error x(t) → ∅ q is possible iff the square matrix Ω (t) Ω T (t) ∈ R p×p keeps being full rank in the integral sense of (3.9). We note that if m ≥ p, that is, if the number of independent available measurements s is larger or equal to the number of independent estimated quantities χ, then it is in principle possible to instantaneously satisfy (3.9) by enforcing

Ω (t) Ω T (t) γ T I p , ∀t ≥ t 0 . (3.13)
On the other hand, if m < p then det(Ω (t) Ω T (t)) ≡ 0 by construction. Nevertheless, in this case, it could still be possible to satisfy (3.9) in an integral sense if the l-dimensional range space of Ω (t) Ω T (t) (l ≤ m) can span R p during the period T . In this work, however, we will only consider the first situation m ≥ p and thus aim at fulfilling the (more restrictive) condition (3.13).

Remark 3.1. Note that the local stability properties of the error dynamics (3.12) are due to the perturbation term d( x, t) which affects an otherwise globally exponentially stable error system. Indeed, in the special case χ = ∅ p (unknown but constant parameters), one has d( x, t) ≡ ∅ p and global exponential convergence for the error system (3.12). This is, for instance, the case of the structure estimation problems for spherical and cylindrical objects considered in Sects. 4.5.3 and 4.5.4. We stress, however, that the estimation scheme (3.11) is not restricted to this particular situation but can be applied (with, in this case, only local convergence guarantees) to the more general case of state observation problems in which the unknown χ is subject to a non-negligible dynamics as in (3.10). The structure estimation for a point feature and a planar surface discussed in Sects. 4.5.1 and 4.5.2 falls in this second class.

Before concluding this section we want to mention that many other deterministic observers have been proposed in the literature for solving the SfM problem. Some references and a recent comparison of some of these solutions can be found, e.g., in [GBC + 15].

Probabilistic frameworks

Differently from deterministic frameworks, probabilistic ones explicitly take into consideration, in the modeling and design phase, the presence of stochastic terms in the system dynamics. Both the system state x and the measurements s are treated as random processes and the estimation in general consists in finding a (deterministic) x that minimizes some statistical property of the estimation error which is also considered as a random process. In this section we briefly present two very common probabilistic frameworks: the Maximum Likelihood Estimator (MLE) is only treated here because it allows to simply introduce the Cramer-Rao bound and the Fisher Information Matrix; the Kalman Filter (KF) will instead be used in Chapt. 5 for a basic comparison between deterministic and probabilistic estimation frameworks.

The Maximum Likelihood Estimator

Assume that one wants to estimate the value of a random vector χ ∈ R p . We use the notation χ ∼ p χ (χ) to indicate the Probability Density Function (PDF) of χ. The quantity p χ (χ) is also called the a priori PDF of χ as it represents the confidence that one has about the different values that χ may have before taking any additional measurement. Assume now that a measurement s ∈ R m is taken and that this measurement (also a random vector) is correlated with χ, meaning that

p s,χ (s, χ) = p s (s)p χ (χ)
where p s,χ (s, χ) is the joint probability density function of the two random vectors or, equivalently,

p χ|s (χ|s) = p χ (χ)
i.e. the observation of s "tells something" about χ that makes one change his confidence (the PDF) of χ. The quantity p χ|s (χ|s) is also called the a posteriori PDF of χ as it represents the confidence that one has about the different values that χ may have after taking the measurement s. The Maximum Likelihood Estimator (MLE) technique consists in choosing the estimation χ as the value that maximizes the probability of obtaining the measurement s

[BSLK04] χ = arg max χ p s|χ (s|χ).
Assuming that p s|χ (s|χ) is differentiable, one can find this value by equating to zero its derivative w.r.t. χ which gives rise to the likelihood equation

∇ χ p s|χ (s|χ) χ = ∅ p .
In practice it is often more convenient to work with the logarithm of the likelihood equation, called the log-likelihood equation:

∇ χ log p s|χ (s|χ) χ = ∅ p . (3.14)
To make an example, assume, that the a priori PDF of χ is a Gaussian distribution with mean E {χ} = χ 0 and covariance

E (χ -χ 0 )(χ -χ 0 ) T = Σ 0 ∈ R p×p so that p χ (χ) = 1 (2π) p det (Σ 0 ) e -1 2 (χ-χ 0 ) T Σ -1 0 (χ-χ 0 ) . (3.15)
We also write χ ∼ N (χ 0 , Σ 0 ). Assume now that the measurement model is linear with additive noise

s = Ω T χ + v (3.16)
where Ω ∈ R m×p is deterministic and v ∈ R m represents the measurement noise, which is assumed to be Gaussian distributed with zero mean and covariance matrix E vv T = R ∈ R m×m . The PDF of s given χ is easily computed as a multivariate Gaussian distribution

p s|χ (s|χ) = 1 (2π) m det (R) e -1 2 (s-Ω T χ) T R -1 (s-Ω T χ) . (3.17) Using (3.14) one obtains the maximum likelihood estimation of χ χ = arg min χ 1 2 s -Ω T χ T R -1 s -Ω T χ = ΩR -1 Ω T -1 ΩR -1 s. (3.18)
Since we have only considered one measurement, in fact, the best we can do is to use a weighted pseudoinverse based on the noise content of each component of the measurement. Note that the estimation is only possible if ΩR -1 Ω T 0. The information provided by the prior (3.15) was not considered for the calculation of (3.18). The prior can be treated as an additional virtual measurement affected by a Gaussian uncertainty

χ 0 = χ + v 0 , v 0 ∼ N (∅ p , Σ 0 ) ⇒ p χ 0 |χ (χ 0 |χ) = 1 √ (2π) q det (Σ 0 ) e -1 2 (χ 0 -χ) T Σ -1 0 (χ 0 -χ) .
The joint probability distribution of (χ 0 , s), under the assumption that v 0 and v are not correlated, is given by

p χ 0 ,s|χ (χ 0 , s|χ) = p χ 0 |χ (χ 0 |χ)p s|χ (s|χ) = c e -1 2 (s-Ω T χ) T R -1 (s-Ω T χ)+(χ 0 -χ) T Σ -1 0 (χ 0 -χ)
where c is a normalizing constant that does not depend of χ and hence can be ignored. Using again (3.14) one obtains

χ = ΩR -1 Ω T + Σ -1 0 -1 ΩR -1 s + Σ -1 0 χ 0 . (3.19)
It can be shown [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF] that this result would also be obtained by considering an alternative estimation technique: the Maximum a Posteriori Estimator (MPE). This latter defines the optimal estimation χ as the one that maximizes the a posteriori distribution of χ, i.e. χ = arg max χ p χ|s,χ 0 (χ|s, χ 0 ).

Taking the expected value of the estimation error χ = χχ with χ in (3.19), one has

E { χ -χ} = E { χ} -E {χ} = ΩR -1 Ω T + Σ -1 0 -1 ΩR -1 E {s} + Σ -1 0 χ 0 -χ 0 = ΩR -1 Ω T + Σ -1 0 -1 ΩR -1 Ω T E {χ} + Σ -1 0 χ 0 -χ 0 = ∅ p
and the estimation χ is said to be unbiased.

The MLE can easily be extended to the case in which one takes multiple independent measurements s k = s(t k ), k = 1, . . . K, which are different samples of a random process s(t), and wants to use them, together with the initial prior (3.15), to estimate the value χ k of another random process χ(t) at the same instants t k . We indicate with X = χ 0 , χ 1 , . . . , χ K the set of unknowns and with S = χ 0 , s 1 , s 2 , . . . , s K the set of measurements. Considering a generic nonlinear measurement model with additive Gaussian noise

s(t k ) = g(χ(t k ), t k ) + v(t k ) with v k = v(t k ) ∼ N (∅ m , R k )
and assuming that the variables s k , χ k and v k are all independent, we can write the joint PDF of the measurements as

p S|X (S|X ) = c e [-1 2 K k=0 (s k -g(χ k ,t k )) T R -1 k (s k -g(χ k ,t k ))]
where we used s 0 = χ 0 , g(χ(t 0 ), t 0 ) = χ 0 , R 0 = Σ 0 , and c is a normalizing factor that does not depend on X and thus can be ignored. Therefore the MLE (3.14) returns

X = arg min X 1 2 K k=1 (s k -g(χ k , t k )) T R -1 k (s k -g(χ k , t k ))
which is a Nonlinear Least Squares (NLS) problem. This kind of batch resolution strategies, based on MLE and NLS, have been exploited in many SLAM works where χ k typically represents a set of robot and landmark locations and s k is the set of all taken measurements. The problem in this case also takes the name of trajectory Smoothing And Mapping (SAM) [START_REF] Dellaert | Square Root SAM: Simultaneous localization and mapping via square root information smoothing[END_REF][START_REF] Kaess | isam: Incremental smoothing and mapping[END_REF] because of its similarity to the signal smoothing problem. In this context, in general, one also exploits the fact that the different robot poses χ k are not independent but, instead, they are related by the dynamics of the system:

χ k = f (χ k-1 , u k ) + w k where u k ∈ R v is a control input and w k ∈ R p is a Gaussian distributed process noise w k ∼ N (∅ p , Q k ). Therefore one can write p χ k |χ k-1 (χ k |χ k-1 ) = e -1 2 (χ k -f (χ k-1 ,u k )) T Q -1 k (χ k -f (χ k-1 ,u k )) .
The joint measurements PDF can be easily obtained, by exploiting the uncorrelation, as a simple product between the single Gaussian PDFs

p S|X (S|X ) = c p χ 0 |χ 0 (χ 0 |χ 0 ) K k=1 p χ k |χ k-1 (χ k |χ k-1 )p s k |χ k (s k |χ k ) = c e -1 2 [(χ0-χ 0 ) T Σ -1 0 (χ 0 -χ 0 )+ K k=1 (χ k -f k ) T Q -1 k (χ k -f k )+(s k -g k ) T R -1 k (s k -g k )]
where c is a normalization factor that does not depend on X and, for brevity of notation, we wrote

f k = f (χ k-1 , u k ) and g k = g(χ k , t k ). The application of (3.14) leads to [KGS + 11] X = arg min X 1 2 (χ 0 -χ 0 ) T Σ -1 0 (χ 0 -χ 0 ) + K k=1 (χ k -f k ) T Q -1 k (χ k -f k ) + (s k -g k ) T R -1 k (s k -g k )
Different techniques can be used to find the numerical solution to the NLS problem [START_REF] Dennis | Numerical methods for unconstrained optimization and nonlinear equations[END_REF]. Iterative methods, such as Gauss-Newton or Levenberg-Marquardt algorithms are often preferred because of their efficiency [KGS + 11]. Incremental NLS solutions have also been proposed in [KJR + 11, PSI + 13] to reduce computational time by exploiting the sparsity of the SLAM problem and efficient block-matrix operations.

The Fisher Information Matrix and the Cramer-Rao bound

Let us calculate the estimation error covariance for the MLE (3.19). Assuming that χ and v are not correlated and after some tedious but straightforward calculation one obtains

Σ = E ( χ -χ) T ( χ -χ) = ΩR -1 Ω T + Σ -1 0 -1 . (3.20)
It is clear, than, that the quantity ΩR -1 Ω T represents the "gain", in terms of reduction of uncertainty, that one has by using s to ameliorate the estimation of χ. This concept is formalized by the so called Fisher Information Matrix (FIM)

I F = -E (∇ χ log p S|χ (S|χ))(∇ χ log p S|χ (S|χ)) T . (3.21)
The quantity ∇ χ log p S|χ (S|χ), the gradient of the log-likelihood function that appears in (3.14), is also called the score of the measurement S since it represents the amount of information that S contains about X or the sensitivity of the measurements w.r.t. the unknown quantities. One can also prove [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF] that the following definition of I F is equivalent to (3.21)

I F = -E ∇∇ T χ log p S|χ (S|χ) (3.22)
and hence the FIM represents the curvature of the log-likelihood function that appears in (3.14). As such it represents a quantitative measure of the "quality" of the maximum point found with (3.14). The importance of the FIM lies in the fact that it can be proved (see again [START_REF] Bar-Shalom | Estimation with applications to tracking and navigation: theory algorithms and software[END_REF]) that for any unbiased estimation χ, the covariance of the estimation error satisfies the Cramer-Rao lower bound :

E ( χ -χ) T ( χ -χ) I -1 F . (3.23)
In our first example, we used the prior (3.15) and the measurement (3.17), therefore S = {χ 0 , s} and

-log (p S|χ (S|χ)) = -log (p χ (χ)p s|χ (s|χ)) = -log p χ (χ) -log (p s|χ (s|χ)) = (χ -χ 0 ) T Σ -1 0 (χ -χ 0 ) + s -Ω T χ T R -1 s -Ω T χ
and taking the expected value of the second order partial derivatives w.r.t. χ we conclude

I F = ΩR -1 Ω T + Σ -1 0
which is equal to the covariance of the MLE in (3.20). In the case of MLE, then, the Cramer-Rao bound(3.23) is satisfied with the equal sign and therefore the estimator is called efficient.

The Fisher information matrix for the multiple measurement case can also be easily computed as

I F = Σ -1 0 + K k=1 ∇ χ k g(χ k , t k ) T R -1 k ∇ χ k g(χ k , t k ).
In a simple case, analogous to (3.16), where one has a linear time varying measurement model

s k = Ω T k χ k + v k this results in I F = Σ -1 0 + K k=1 Ω k R -1 k Ω T k . (3.24)
Note the resemblance between the expression (3.24) and the quantity involved in the PE condition (3.9). As a matter of fact (3.24) is the expression of the FIM that one would obtain from system (3.10) by assuming as measurement the quantity ṡ-f s (s, u). In SfM, this corresponds to the de-rotated optical flow field, sometimes used for 3-D reconstruction [START_REF] Grabe | Nonlinear ego-motion estimation from optical flow for online control of a quadrotor uav[END_REF]. The similarity is more evident if we consider the expression of (3.24) for R k = r 2 s I m and in the limit for

∆ t = t k -t k-1 → 0.

With this assumption the summation can be approximated by a continuous time integral in a similar way to what done in [WSM14]

I F = Σ -1 0 + 1 r 2 s K k=1 Ω k Ω T k ≈ I 0 + i s t+T t Ω(τ )Ω(τ ) T dτ.
where

I 0 = Σ -1 0 and i s = 1 r 2
s represent the amount of information contained in the initial prior and in each measurement. We can then conclude that:

t+T t Ω(τ )Ω(τ ) T dτ = I F -I 0 i s
i.e. the integral in (3.9) can be interpreted as an index of the efficiency of the experiment: how much information was acquired during the experiment (I F -I 0 ) w.r.t. the information contained in the measurements (i s ). Note that the FIM (3.22) and the PE matrix in (3.9) are characteristic properties of the system and of the experiment only and do not depend in any way of the kind of estimator that is used to recover χ. The Cramer-Rao bound (3.23), on the contrary, qualifies the particular estimation scheme that is being used: only an efficient estimator will make correctly use of all the available information and produce an estimation with the least possible uncertainty.

We conclude this section by reporting the expression of the FIM for nonlinear continuous-process discrete-measurement systems of the form:

ẋ = f (x, u, t) s k = g(x k , t k ) + v k .
Our derivation is very similar to [START_REF] Taylor | The cramer-rao estimation error lower bound computation for deterministic nonlinear systems[END_REF], however, instead of computing the FIM relative to the most recent system state x K we are interested in knowing the FIM w.r.t. the estimation of the initial state x 0 : this will respond the question of how much information the measurements s k , k = 1, . . . , K give about the initial state x 0 of the system. This is a more similar question to the one addressed in observability analysis. Following [START_REF] Taylor | The cramer-rao estimation error lower bound computation for deterministic nonlinear systems[END_REF] we can consider S = {x 0 , s 1 , . . . , s K }, X = {x 0 , . . . , x K } and write

-log (p S|X (S|X )) = c + (x 0 -x 0 ) T Σ -1 0 (x 0 -x 0 ) + K k=1 (s k -g k ) T R -1 k (s k -g k )
and taking the second order differentials w.r.t. x 0 we conclude

I F = Σ -1 0 + K k=1 Φ T k,0 C T k R -1 k C k Φ k,0
where

C k = ∇ x k g(x k , t k ) T and Φ k,0 = Φ(t k , t 0 ) is the solution to the ordinary differential equation ∂ ∂t Φ(t, t 0 ) = ∇ x f (x, u, t) T Φ(t, t 0 ) from t = t 0 to t = t k subject to the initial condition Φ(t 0 , t 0 ) = I q .
If we consider the linear time varying system (3.4), we have

C k = C(t k ), ∂ ∂t Φ(t, t 0 ) = A(t)Φ(t, t 0 )
so Φ k,0 is the state transition matrix corresponding to A(t) and we have

I F = Σ -1 0 + K k=1 Φ T k,0 C T k R -1 k C k Φ k,0 . (3.25)
Note the strong similarity between (3.25) and the OG in (3.5). The similarity is more evident if we assume R -1 k = i s I m , we write Σ -1 0 = I 0 and we make the sample time ∆t = t k -t k-1 tend to zero. Then we have

I F (t) -I 0 i s = t t 0 Φ(τ, t 0 ) T C(τ )C(τ )Φ(τ, t 0 ) dτ = G(t).
We can conclude that the OG (3.5), the PE condition (3.9) and the FIM (3.22) are, in essence, just different forms of the same intuitive concept: regardless of the type of estimation algorithm that one decides to use, the maximum possible performance (in terms of reduced final uncertainty or converge rate) is determined, all gains being equal, by the amount of information that is contained in the measurements. This latter is an intrinsic characteristic of the system and, in the nonlinear case, of the trajectory followed by the system during the experiment. If one has control on the system evolution, through the input u, one can then engage in an Experiment Design problem, i.e. in the study of optimal input profiles u(τ ), τ ∈ [t 0 , t] that result in the maximum information gain. This problem and how to conciliate it with the execution of additional tasks, is the main focus of this work.

The Kalman-Bucy filter

Probably the most well known and widely used probabilistic state estimator is the Kalman Filter (KF), sometimes called the Kalman-Bucy Filter when presented in its continuous-time version. This algorithm takes its name from Rudolf E. Kalman, who introduced the first discrete-time version of the filter in [START_REF] Kalman | Contributions to the theory of optimal control[END_REF], and Richard S. Bucy, who contributed extending the filter to continuous time in [START_REF] Kalman | New results in linear filtering and prediction theory[END_REF]. The filter, originally intended for linear system, was later extended to nonlinear system dynamics by the NASA Ames Research Center where it was incorporated in the Apollo on-board computer for estimating the trajectory of the lunar module [START_REF] Smith | Application of statistical filter theory to the optimal estimation of position and velocity on board a circumlunar vehicle[END_REF][START_REF] Mcgee | Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry[END_REF]. An interesting historical perspective as well as an overview of past and current applications of Kalman filtering can be found in [START_REF] Grewal | Applications of Kalman Filtering in Aerospace 1960 to the Present[END_REF].

Consider a generic time-varying linear system P with state-space dynamic equations:

P : ẋ = A(t)x + B(t)u + G(t)w s = C(t)x + D(t)u + H(t)w + v (3.26) where x ∈ R q is the system dynamic state, u ∈ R v is the input vector, s ∈ R m is the measurable output, A(t) ∈ R q×q is the system matrix, B(t) ∈ R q×v is the input matrix, C(t) ∈ R m×q is the output matrix, G(t) ∈ R q×w , H(t) ∈ R m×w and
finally, w ∈ R w and v ∈ R m are two Gaussian-distributed random processes with zero mean and known covariance matrices, i.e.

E {w(t)} = ∅ w , E w(t)w(τ ) T = δ(t -τ )Q(t), E {v(t)} = ∅ m , E v(t)v(τ ) T = δ(t -τ )R(t), E w(t)v(τ ) T = δ(t -τ )M (t), (3.27)
where δ is the Dirac's delta function. We further assume that P is observable according to the definition introduced in Sect. 3.2.1 and that the initial state x(t 0 ) is a Gaussian random vector with known mean and covariance matrix

E {x(t 0 )} = x 0 , E [x(t 0 ) -x 0 ] [x(t 0 ) -x 0 ] T = Σ 0
and that x 0 is independent on w and v.

The structure of the KF is similar to (3.2), but uses a time varying update gain

K(t) ∈ R q×m : ˙ x = A(t) x + B(t)u -K(t) [C(t) x + D(t)u -s] .
(3.28) Observer (3.28) generates an unbiased estimation of x as it can be easily verified by calculating the expected value of the estimation error. If the filter is initialized with x(t 0 ) = x 0 , then

E { x(t 0 )} = E { x(t 0 ) -x(t 0 )} = E {x 0 -x(t 0 )} = x 0 -E {x(t 0 )} = ∅ q .
Moreover, by subtracting (3.26-3.28), one obtains the error dynamics:

˙ x = [A(t) -K(t)C(t)] x -[G(t) -K(t)H(t)] w + K(t)v. (3.29)
Since A(t), K(t), C(t), G(t) and H(t) are deterministic matrices and E {w} = ∅ w , E {v} = ∅ m by hypothesis, the expected value of the error derivative in (3.29) is

E ˙ x = d E { x} dt = [A(t) -K(t)C(t)] E { x},
and remains identically zero if E { x(t 0 )} = ∅ q as already shown.

Up to this point, the KF does not differ in any way from the deterministic Luenberger Observer (LO) introduced in Sect. 3.2.1. The core difference between the two estimators is, in fact, in the computation of the update gain K(t). As a matter of fact, in the KF, this gain is selected online as an optimal trade-off between the process and measurement noise contents. As shown in, e.g., [START_REF] Kalman | New results in linear filtering and prediction theory[END_REF] and briefly reported in Appendix A.1.1, the dynamics of the error covariance matrix

Σ(t) = E x(t) x(t) T is given by Σ(t) = [A(t) -K(t)C(t)] Σ(t) + Σ(t)[A(t) -K(t)C(t)] T + [G(t) -K(t)H(t)]Q(t)[G(t) -K(t)H(t)] T + K(t)R(t)K(t) T -K(t)M (t)[G(t) -K(t)H(t)] T -[G(t) -K(t)H(t)] M (t) T K(t) T (3.30)
while the initial error covariance can be easily computed as

Σ(t 0 ) = E x(t 0 ) x(t 0 ) T = E [ x(t 0 ) -x(t 0 )] [ x(t 0 ) -x(t 0 )] T = E [x 0 -x(t 0 )] [x 0 -x(t 0 )] T = Σ 0 .
The Pontryagin minimum principle can be used to minimize the cost functional E x 2 , as shown in [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF] and in Appendix A.1.1, resulting in the optimal update gain

K(t) = Σ(t)C(t) T + M (t) Ř(t) -1 , (3.31a)
with

M (t) = G(t) Q(t)H(t) T + M (t) T , Ř(t) = H(t)Q(t)H(t) T + R(t) + H(t)M (t) T + M (t)H(t) T . (3.31b) (3.31c)
Injecting (3.31a) in (3.30) and wrapping everything up, one finally obtains the optimal KF dynamics for estimating the state of (3.26):

Σ(t) = A(t)Σ(t) + Σ(t)A(t) T + G(t)Q(t)G(t) T -K(t) Ř(t)K(t) T ˙ x = A(t) x + B(t)u -K(t) [C(t) x + D(t)u -s] . (3.31d) (3.31e)
Note that the computation of K(t) in (3.31a) requires knowledge of the current Σ(t) which can only be obtained by integrating (3.31d) over time. This represents a considerable additional computational effort w.r.t. the non-optimal LO described in Sect. 3.2.1.

An alternative, and equivalent, form of the KF is the so-called Information Filter (IF) which is based on use of the canonical parametrization of the multivariate Gaussian distribution that can be obtained by using the transformation

i(t) = Σ(t) -1 x(t) I(t) = Σ(t) -1 . ( 3.32a) (3.32b)
i(t) ∈ R q and I(t) ∈ R q×q are usually called the information vector and information matrix respectively. To avoid confusion with the FIM described in Sect. 3.3.2, however, we prefer to refer to I(t) as the precision matrix as suggested in [START_REF] Thrun | Probabilistic Robotics[END_REF].

The dynamics of I(t) can be easily calculated by considering that I(t)Σ(t) = I q and hence, taking the time derivative of both sides, one obtains

İ(t)Σ(t) + I(t) Σ(t) = O q×q ⇒ İ(t) = -I(t) Σ(t)I(t). (3.33)
As for i(t) one obviously has:

i(t) = İ(t)x(t) + I(t) ẋ(t). ( 3
         İ(t) = -I(t)A(t) -A(t) T I(t) -I(t)G(t)Q(t)G(t) T I(t) -K(t) Ř(t)K(t) T ˙ i = -A(t) T + I(t)G(t)Q(t)G(t) T I(t) -K(t) M (t) T i + I(t)B(t)u -K(t) [D(t)u -s] (3.35a) (3.35b) with K(t) = C(t) T + I(t) M (t) Ř(t) -1 M (t) = G(t) Q(t)H(t) T + M (t) T Ř(t) = H(t)Q(t)H(t) T + R(t) + H(t)M (t) T + M (t)H(t) T . (3.35c) (3.35d) (3.35e)
The extension of KF to nonlinear system dynamics of the form

ẋ = f (x, u, w) s = g(x, u, w) + v
can be easily obtained by linearizing the dynamics around a nominal state trajectory [START_REF] Smith | Application of statistical filter theory to the optimal estimation of position and velocity on board a circumlunar vehicle[END_REF] or around the current estimated state x(t) as described in [START_REF] Mcgee | Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry[END_REF]. The latter solution, in particular, usually results in a superior estimation accuracy and takes the name of Extended Kalman Filter (EKF). Its dynamics are given by

˙ x = f (x, u, w) x= x w=∅w -K(t)   g(x, u, w) x= x w=∅w -s   (3.36)
and (3.31a) to (3.31d) with

A(t) = ∇ x f T x= x w=∅w , B(t) = ∇ u f T x= x w=∅w , G(t) = ∇ w f T x= x w=∅w , C(t) = ∇ x g T x= x w=∅w , D(t) = ∇ u g T x= x w=∅w , H(t) = ∇ w g T x= x w=∅w . (3.37)
Contrarily to the KF, in general, only local convergence of the EKF can be proved [START_REF] Krener | The convergence of the extended kalman filter[END_REF] and, in some cases, the filter can even converge to biased estimations.

In a similar way, one can also devise an Extended Information Filter (EIF), whose dynamics are:

˙ i = -A(t) T + I(t)G(t)Q(t)G(t) T I(t) -K(t) M (t) T i + I(t)   f (x, u, w) x=I -1 i w=∅w -A(t) x   -K(t)   g( x, u, w) x=I -1 i w=∅w -s -C x   (3.38)
with (3.35a) and (3.35c) to (3.35e) and using the same linearizations introduced in (3.37). Note that the EIF requires to retrieve an estimation of the system state x = I -1 i for computing the process and output functions. This represents one of the main disadvantages of the EIF w.r.t. the EKF and motivates the wider adoption of the EKF, especially for estimation problems that involve a large state space. In some cases, however, the interaction between state variables is only local and, as a result, the precision matrix becomes sparse. One can then exploit this sparsity (which does not extend to Σ) to improve the computational efficiency of the IF filter [TLK + 04]. In addition to this, the IF and EIF allow to easily represent global uncertainty as I = O q×q , where as the KF and EKF would require the use of an infinite covariance matrix. More in general, the IF tends to be numerically more stable than the KF.

Another alternative extension of the KF to nonlinear system dynamics, also considerably popular in the literature (see, e.g., [LCG + 13, HKM08] and references therein), is the so-called Unscented Kalman Filter (UKF) proposed in [START_REF] Julier | A New Extension of the Kalman Filter to Nonlinear Systems[END_REF]. This filter is based on a different strategy (namely the Unscented Transform [START_REF] Julier | A new approach for filtering nonlinear systems[END_REF]) for propagating Gaussian distributions through nonlinear system dynamics: instead of propagating the expected value and covariance through a linearization of the system equations (as done in EKF), the UKF uses the nonlinear system dynamics to propagate an appropriately selected set of samples of the original Gaussian PDF; the propagated samples are then used to find a new "best-fitting" Gaussian distribution. The UKF can, sometimes, be more computationally expensive than the EKF but it is easier to develop (it does not require the symbolic calculation of the linearizations (3.37)) and can outperform the EKF in terms of accuracy, especially in presence of highly nonlinear system dynamics.

For more details about the KF, IF and their extensions to nonlinear system, we suggest to refer to [START_REF] Thrun | Probabilistic Robotics[END_REF][START_REF] Lewis | Optimal and robust estimation: with an introduction to stochastic control theory[END_REF]. We also want to report an interesting result demonstrated in [START_REF] Taylor | The cramer-rao estimation error lower bound computation for deterministic nonlinear systems[END_REF]: for the case of nonlinear systems with negligible process noise (w ≡ ∅ w ), the dynamics of the FIM have the same form as those of the precision matrix in the EKF, the only difference being that the linearizations (3.37) are evaluated along the true state trajectory x, instead of the current estimation x. This allows to conclude that, if the EKF estimate x actually converges to x, then, from that moment on, the EKF will be efficient and make perfect use of all available information. We conclude this section by showing how the general EKF structure specializes to the dynamics (3.10) which is typical of SfM applications. Since we assumed that s is measurable, we can directly exploit its knowledge for the computation of the prediction (∼feedforward) component of the EKF thus obtaining the following estimation dynamics analogous to (3.11)

˙ s = f s (s, u) + Ω T (s, u) χ -K s s ˙ χ = f χ (s, χ, u) -K χ s. (3.39)
where K = (K s , K χ ) is computed using (3.31a) to (3.31d). Note that this is equivalent to treating s as an additional component of the input vector u. A similar strategy was exploited in [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF] to obtain an asymptotically stable EKF filter for SLAM applications. As done in (3.11), we also still carry on an estimation of s that allows us to compute the innovation term for the estimator as a function of s = ss. Finally we assume that s and u are affected by independent Gaussian distributed additive noise with E ss T = Σ s and E uu T = Σ u . With these choices we have

Q = Σ s O m×v O v×m Σ u , R = Σ s , M = Σ s O v×m
and the linearization (3.37) becomes 2

A(t) = O m×m Ω(s, u) T O p×m ∇ χ f χ T (s, χ, u) B(t) = G(t) = ∇ s (f s + Ω T χ) T (s, u) L(s, χ) ∇ s f χ T (s, χ, u) ∇ u f χ T (s, χ, u) C(t) = I m O m×p , D(t) = H(t) = O m×(m+v) , (3.40)
where L is the interaction matrix of the considered visual measurements s.

Active perception

As we have seen in the previous sections of this chapter, when dealing with nonlinear system dynamics and measurement models, the performance of the estimation process is not only determined by the efficiency of the chosen observer, i.e. its ability to exploit the information in a sensible way, but also by the amount of information that is available to perform the estimation, which is represented and quantified, in a similar way, by the observability Gramian, the persistence of excitation condition and the Fisher information matrix. All these quantities, in general, are strongly dependent on the trajectory followed by the system during the estimation process, also called the experiment. If the system is controllable through some input ports u one can consider the problem of actively driving the system so that the maximum amount of information is gathered during the experiment. The problem, however, would not be well defined (the objective function would be unbounded from above), without some additional constraint on, e.g., the duration T of the experiment or the total energy available. This view of perception and sensing as an active process can be found, under different forms, in many research fields.

don't simply see, we look. The visual system is a motor system as well as a sensory one. When we seek information in an optic array, the head turns, the eyes turn to fixate, the lens accommodates to focus" [START_REF] Gibson | Exploratory behavior in the development of perceiving, acting, and the acquiring of knowledge[END_REF][START_REF] Gibson | The ecological approach to visual perception[END_REF]. We do not touch, but we feel [START_REF] Gibson | Observations on active touch[END_REF].

Possibly the research field that traditionally has been the most interested in this problem is that of statistics. A statistical analysis is, in fact, meaningless without a corresponding measure of the reliability of the results. Since, in general, one can only perform a limited number of experiments, it is important to select the ones that are most significant. Ronald Fisher is regarded as the founder of the modern methods for experimental design [START_REF] Yates | Sir ronald fisher and the design of experiments[END_REF]. For the first time, in fact, he realized that the main responsibility of the statistician was not the results of its analysis but rather "the processes by which the data had come into existence". He started formally investigating the data gathering phase that had previously been conducted using empirical considerations. Fisher's work was initially applied to agricultural engineering and crop selection but, since then, statistical techniques for optimal experimental design have been used in medical diagnostics [AEK + 04, SGL + 13], biology [START_REF] Cho | Experimental design in systems biology, based on parameter sensitivity analysis using a monte carlo method: A case study for the tnfα-mediated nf-κ b signal transduction pathway[END_REF][START_REF] Kreutz | Systems biology: experimental design[END_REF], chemistry [START_REF] Lindner | Experimental design for optimal parameter estimation of an enzyme kinetic process based on the analysis of the fisher information matrix[END_REF][START_REF] Leardi | Experimental design in chemistry: a tutorial[END_REF], public opinion polling and many other fields.

As pointed out by R. Bajcsy [START_REF] Bajcsy | Active perception[END_REF], who contributed extending the concept to the robotics and computer vision fields, the term active perception is not to be confused with the active sensing strategies utilized in many robotics and computer vision works. In that case, in fact, the word active refers to the fact that the sensor, in order to be able to extract some information, needs to preliminary inject information into the system. E.g. an echographic probe or a laser path finder need to actively generate a signal to be able to perceive the environment. While active sensing can be considered as a particular instance of active perception, this latter concept is more general and refers to the way a sensor is used rather than to its intrinsic functioning mechanisms. Furthermore, it is perfectly possible to use a passive sensor (such as a camera) to do some active perception [START_REF] Bajcsy | Active perception[END_REF].

In the system identification and adaptive control literature, the problem is usually referred to as input design or optimal experiment design [START_REF] Mehra | Optimal input signals for parameter estimation in dynamic systems-survey and new results[END_REF]. The first results in this field are attributed to [START_REF] Levin | Optimum estimation of impulse response in the presence of noise[END_REF] who first studied the problem of identifying the parameters of linear SISO dynamic systems with the minimum possible uncertainty, i.e., by minimizing the estimation covariance matrix. In this case, one aims at designing the set of inputs that allow to reconstruct, looking at the corresponding system outputs for a certain amount of time, the dynamic parameters of the system, e.g., its poles and zeros. The problem of persistence of excitation of the input signal arises 3 . The analysis is usually done in the spectral domain and, as well know [START_REF] Boyd | On parameter convergence in adaptive control[END_REF], one needs an input signal with at least n spectral lines to identify a system with n parameters with white noise being, consequently, the most exciting signal [START_REF] Levin | Optimum estimation of impulse response in the presence of noise[END_REF]. In adaptive control a term that is often used is that of non-uniformly observable systems [START_REF] Besançon | On uniform observation of nonuniformly observable systems[END_REF] with reference to the fact that, for generic nonlinear systems, the convergence of estimation schemes might depend on the input. Input optimization solutions based on the minimization of the covariance matrix [START_REF] Besançon | Input selection in observer design for non-uniformly observable systems[END_REF] or on the maximization of the observability Gramian [START_REF] Scola | Input optimization for observability of state affine systems[END_REF] have been proposed. Mutual information between the measured outputs and the unknown system parameters, was also exploited as a metric for the quality of an experiment in [START_REF] Arimoto | Optimum input test signals for system identification-an information-theoretical approach[END_REF].

A similar problem arises in experimental robot calibration. In this case, one needs to find the robot joint trajectory that results in a sufficient excitation of the robot dynamics/kinematics such that the identification process can be successful and robust w.r.t. noise and other disturbances. In most cases, the calibration procedure is done by resolving a system of (possibly nonlinear) equations using (weighted) least squares techniques. The condition number of such system is often used as a quantitative measure and optimization criterion for the selection of robot trajectories [GK92, RVA + 06]. In a similar way, B. Armstrong suggests to maximize the condition number of the persistent excitation matrix over the robot trajectory [START_REF] Armstrong | On finding exciting trajectories for identification experiments involving systems with nonlinear dynamics[END_REF]. In some cases the non deterministic properties of the measurements are explicitly taken into account and the problem is solved using maximum likelihood techniques [SGT + 97, WSM14]. In this case the optimization criterion is some metric of the (expected) parameters estimate covariance matrix or of the FIM. The search space for the optimization is usually constructed via a time-discretization of the system inputs [START_REF] Wilson | Trajectory synthesis for fisher information maximization[END_REF] or using a set of basis functions [Arm89, Par06, RLH12].

Another similar issue is that of optimal sensor placement or dynamic sensor placement for sensor networks [START_REF] Kubrusly | Sensors and controllers location in distributed systems -a survey[END_REF][START_REF] Zhang | Optimal sensor placement[END_REF][START_REF] Ucinski | Time-optimal path planning of moving sensors for parameter estimation of distributed systems[END_REF]. In this case the different measurements are distributed in space rather than in time: one needs to observe a distributed phenomenon (e.g. for air/water quality monitoring, fault detection, weather forecasting, and so on) through the use of a (limited) set of sensors displaced in different positions of the environments. The problem is then that of identifying the most significant quantities to measure and the optimal positions (possibly timevarying) of the sensors. The optimization function is usually a metric of the FIM [UC05] or of the estimation covariance. The constraint is usually on the total number of sensors and possibly on their dynamics (if they can move). In this context, a term that is often used is that of optimal coverage [START_REF] Cortes | Coverage control for mobile sensing networks[END_REF] with reference to the maximum coverage problem [START_REF] Cohen | The generalized maximum coverage problem[END_REF] in statistics and computational geometry.

and not its state. The observability of the state of a linear system, in fact, does not depend on the inputs as shown in Sect. 3.2.1. This is usually posed in this way: given a set of sets (e.g. the fields of view covered by some sensors when placed in certain discretized positions), select k sets, among them, in such a way that they contain the maximum number of elements (for the sensor placement problem we can think of the total observed area as the quantity to maximize). A related topic, in computational geometry, is the art gallery problem [O'r87] in which one tries to find the minimum number of guards, and their distribution, such that every section of a museum lies in the FOV of at least one guard. This problem was demonstrated to be NP-hard in [START_REF] Lee | Computational complexity of art gallery problems[END_REF] but some sub-optimal approximated solutions have been proposed (see, e.g. [BDRDS + 13]). Given the complexity of the problem, most of these works use off-line solutions.

The maximization of information is also at the core of Active SLAM or Simultaneous Planning Localization and Mapping (SPLAM). In this framework, one addresses the problem of designing a robot trajectory that optimally explores the environment while the map is built. The objective to optimize is, in general, constructed in terms of entropy or information gain [SR05, SB03, BMW + 02] or estimate covariance matrix [START_REF] Leung | Active slam using model predictive control and attractor based exploration[END_REF]. In this context one often distinguishes two different phases:(i ) in the exploration phase the robot enlarges the map by visiting new sections of the environment; (ii ) in the localization phase the robot revisits know sections of the map (loop closure) to reduce the uncertainty related to its position. The optimal policy is usually found by selecting the best control action (in terms of expected information gain) among a limited set of candidates. Since the actual information gain depends on the measurements, which are not known in advance, the process must be repeated at each iteration based on the current estimate of the environment.

Other interesting strategies are those based on modeling robot motion and observations as a Partially Observable Markov Decision Process (POMDP). This framework is probably the most general one as the uncertainty about the system state is explicitly modeled in the calculation of the control action. The resolution algorithms are, however, very complex and, in general, not suitable for a real-time implementation. Examples of the use of POMDP for active sensing can be found, e.g., in [VM07, CH10, SVL10].

Finally, more specific to vision, we can mention the problem of Active vision or Next Best View calculation or View Path Planning for object reconstruction [Pit99, BWDA00, SRR03] and recognition [HK + 89, RCB04]. As with the active SLAM, also in this case the decision about the next viewpoint position is in general made by computing the expected information gain (in terms of entropy or coverage) on a limited set of candidate action and selecting the one that is expected to be the most convenient. The seminal work [START_REF] Aloimonos | Active vision[END_REF] showed that some complex and illposed problems such as structure from motion, become much easier if the observer is active and can control the geometric parameters (e.g. the position or orientation) of the sensor. SLAM and active vision can also be used jointly to reconstruct a map of the environment and localize a robot within it with the best possible accuracy with the aim of performing some task. In [START_REF] Davison | Simultaneous localization and map-building using active vision[END_REF], which is considered as a pioneering work in this field, the robot trajectory is assigned and determined by a certain task. While the robot is following it, the goal is to change the gaze of a stereo head so that it points toward the feature, in the current map segment, with the largest uncertainty. [START_REF] Achtelik | Path planning for motion dependent state estimation on micro aerial vehicles[END_REF] runs a probabilistic planner on a known map to find a path from the starting configuration to the assigned goal that minimizes uncertainty measured as a function of the state covariance matrix. The authors of [START_REF] Forster | Appearance-based Active, Monocular, Dense Reconstruction for Micro Aerial Vehicle[END_REF] suggest a planning strategy that takes into account both structure and texture since, as well known, vision algorithms perform worse in presence of uniform patterns. The authors also show that local greedy control strategies perform worse than finite horizon planning techniques.

With respect to all these works, in this thesis we address the problem of active vision from a classical control perspective. As it will be shown in Chapt. 4, the nonlinear deterministic observer that we use, described in Sect. 3.2.3, enforces an evolution of the estimation error that is equivalent to that of a second order linear system. Moreover, a complete online control over the eigenvalues of such evolution is possible by acting both on the estimation gains and on the camera linear veloc- ity. In particular, as it can already be intuitively expected from the developments in Sect. 3.2.3, the "natural frequencies" of the error dynamics are directly proportional to the instantaneous persistence of excitation condition introduced in (3.13). Thanks to the structure of the SfM problem, the PE condition in (3.9) and, in particular, its instantaneous value (3.13) can not just be calculated online using only known information (the visual measurements and the camera linear velocity), but it can also be controlled by acting (online) on the camera linear velocity. One can then use standard control techniques, such as those described in Sect. 2.2.4, to regulate some metric of (3.13) (e.g. its smallest eigenvalue) to obtain the fastest possible convergence rate (and similarly the minimum estimation uncertainty) given some additional constraints on the control inputs. In the next part of the thesis, we will assume that the SfM is the primary task that must be accomplished. The active estimation strategy, thus, has complete freedom on choosing the value of the camera velocity and only a constraint on its norm (that otherwise would grow indefinitely) is imposed. In the last part of the thesis, instead, constraints on the camera velocity will be introduced by the introduction of a main IBVS task that must be executed. The maximization of the observability will then have to be realized only in the null space of this primary task, using some of the redundancy resolution techniques described in Sect. 2.2.4.

Contrarily to some probabilistic frameworks described in this section, we do not take into account the presence of noise neither in the measurements nor in the system dynamics. Probabilistic techniques, in fact, usually do not allow for a complete characterization of the estimation error dynamics. The robustness of the proposed approach w.r.t. noise is however demonstrated by the reporting many real experiments in Chapts. 5 and 7. Moreover, since the excitation of a system (and similarly the acquired Fisher information) does not depend on the choice of the observer, the optimized camera trajectory that our method produces will result in improved performance also for probabilistic estimation techniques as it will be briefly shown in Chapt. 5.

Finally we use a greedy optimization technique and do not plan on a finite horizon as some of the discussed techniques do. The reason why we do this is that the evaluation of the PE condition (and similarly of the OG and FIM) in the future, here as in all other works, requires a prediction of the future measurements which in turn depends on the estimated quantity. Only locally optimal solutions can then be found, in general, even if a finite planning horizon is considered. Nevertheless, as it will be discussed in the conclusions, the use of a longer planning horizon, coupled with a online re-planning, could result in better results especially once the estimation is close to convergence.

Part II

Active structure from motion

Chapter 4

A framework for active Structure from Motion T he previous chapters briefly introduced the fundamental theoretical concepts that are necessary for the understanding of the rest of this thesis.

From this chapter on, we will instead explain the contributions of this work in the context of active structure from motion. As already explained in depth, SfM is a nonlinear estimation problem. As such it results in non uniformly observable system dynamics: singular inputs u (i.e. camera velocities) exist such that the system evolution starting from two different initial states and under the action of u results in identical measurements s(t). If such an input is used, then obviously the system input-output mapping (u, s) does not allow to reconstruct the full state of the system. For SfM this results in the impossibility of reconstructing the environment geometry (e.g. the depth of a point feature or the radius of sphere). The strategy that we propose allows to calculate online, based only on measurable quantities, the best motion, i.e. the one that results in the maximum excitation of the system dynamics (measured by the PE condition in (3.13)) and in the maximum amount of acquired information (in the sense of the FIM introduced in Sect. [START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF]

.3.2).

We start the chapter by highlighting, in Sect. 4.1, some interesting properties of the nonlinear SfM estimator described in Sect. 3.2.3 that will provide intuitive interpretations for some of the results of this thesis. We continue by characterizing the dynamics of the SfM estimation error as resulting from the use of the nonlinear observer (3.10), and showing its dependence on the observer gains and on system inputs in Sect. 4.2. We then describe how to actively tune these latter online, as a function of the current measurements, to fix the desired damping factor (Sect. 4.3) and natural frequencies (Sect. 4.4). We conclude the chapter by detailing, in Sect. 4.5, how this general policy specializes to specific geometric primitives: points (Sect. 4.5.1), planes (Sect. 4.5.2), spheres (Sect. 4.5.3), and cylinders (Sect. 4.5.4).

The results contained in this chapter have been presented in different international venues [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF][START_REF] Giordano | An Active Strategy for Plane Detection and Estimation for a Monocular Camera[END_REF][START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF][START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF][START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF] as well as in a journal publication [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere and Cylinder[END_REF].

Interesting properties of the nonlinear Structure from Motion estimator

We now perform some manipulations of system (3.12) in order to slightly simplify its structure and highlight some important features exploited in the following.

Being α > 0, we can consider the following invertible change of coordinates

     ˇ s = s ˇ χ = χ √ α . (4.1)
In the new coordinates, system (3.12) takes the form

˙ s ˙ χ = O m×m ΩT (t) -Ω (t) O p×p - Ȟ O m×p O p×m O p×p ˇ s ˇ χ + ∅ m ď( ˇ x, t) , (4.2)
with Ȟ = H, Ω(t) = √ αΩ (t), and ď = 1 √ α d. We can then note the following facts:

1. In the new coordinates, system (4.2) has an evident port-Hamiltonian (pH) structure (see Appendix C) which is perfectly recovered in the unperturbed case ( ď ≡ ∅ p ). The Hamiltonian (storage function) for (4.2) is the lowerbounded scalar function

H( ˇ s, ˇ χ) = 1 2 ˇ s T ˇ s + 1 2 ˇ χ T ˇ χ = 1 2 s T s + 1 2α χ T χ ≥ 0. (4.3)
Following the pH interpretation, the symmetric component of Ȟ, Ȟs = 1 2 ( Ȟ + ȞT ), represents the dissipative action in the system, while matrix Ω (t) defines the internal power-preserving interconnection among the ˇ s and ˇ χ components of the state. The PE condition (3.13) can then be interpreted as a requirement of a persistent energy exchange among the two parts of the system which permits a complete depletion of the total stored energy thanks to the dissipative action provided by Ȟs .

2. The gain α is a free design parameter and can be suitably exploited to fulfill two objectives. First, since

Ḣ = -ˇ s T Ȟs ˇ s + ˇ χ T ď = -s T H s s + 1 α χ T d,
one can conclude that, for a bounded disturbance d ≤ M , it is always possible to attenuate at will its (possibly destabilizing) contribution by letting the gain α → ∞, basically obtaining a 'semi-global' vs. local stability condition. Furthermore, being

Ω(t) ΩT (t) = αΩ(t)Ω T (t), (4.4)
it is also possible to directly affect the norm of Ω ΩT by acting on the gain α. Having an explicit control over the norm of Ω ΩT , or equivalently over its (real) eigenvalues, will be pivotal for the next developments. We note however that increasing α might also result in a higher sensitivity of the observer to measurement noise that was not taken into account in this analysis.

∅ m = ΩT (t) ˇ χ.
Assuming, as previously stated, m ≥ p and full (instantaneous) rank of ΩΩ T as in (3.13) also implies full rankness of the (high-rectangular) matrix ΩT ∈ R m×p and, consequently, that ˇ χ ≡ ∅ p as well (thus concluding the proof).

It is now worth noting that the same proof still holds for a sufficiently smooth time-varying dissipation matrix

h 1 I m Ȟ(t) h 2 I m , 0 < h 1 ≤ h 2 <
∞, with bounded Ḣ(t) : this important feature opens the possibility of suitably shaping the dissipation matrix Ȟ over time in order to fulfill additional objectives of interest, as it will be the case in the next developments.

Characterization of the system transient behavior

In this section, we will give a characterization of the transient behavior of system (4.2). We will assume that Ω(t) = Ω(s, ς, u) where ς ∈ R r indicates a set of time-varying measurable quantities that are not included neither in s (i.e. they do not appear in the observer update term) nor in u (because they cannot be directly controlled). This is a quite natural requirement in many situations, and it is certainly the case for the SfM applications considered in this work, in which, as it will be shown, Ω always depends on a set of visual measurements and on the linear velocity of the camera v. This structure allows to (actively) exploit the input vector u(t) in order to affect matrix Ω and, as a consequence, the system transient response. Note that the introduction of ς was not necessary for the developments of Sect. 3.2.3 because, from an estimation point of view, the elements of ς can be thought of as being part of vector u in the sense that they are measurable quantities used to calculate the prediction term of the estimator. In this section, however, we start dealing with the control problem and thus we need to make this distinction since the value of ς, differently from u, cannot be arbitrarily assigned.

Finally, for this analysis, we will neglect the disturbance term ď(t), since, as explained before, its distorting effects can be typically made arbitrarily small by a proper choice of the gain α. This claim will also be confirmed by the simulation and experimental results in Chapts. 5 and 7.

Following the pH interpretation of system (4.2), in particular with in mind a standard mechanical system, one can identify vector ˇ χ as playing the role of a 'position'-like quantity, and vector ˇ s as that of a 'velocity'-like quantity upon which a dissipative action is present. Therefore, analogously to a mechanical system, we focus the analysis on the dynamics of vector ¨ χ.

Being ˙ χ = -Ωˇ s, it is ¨ χ = -Ωˇ s - Ω ˙ s = -Ωˇ s -Ω(-Ȟˇ s + ΩT ˇ χ) = =( Ω Ȟ -Ω) ˇ s -Ω ΩT ˇ χ = ( Ω Ω † -Ω Ȟ Ω † ) ˙ χ -Ω ΩT ˇ χ (4.5)
with Ω † ∈ R m×p denoting the pseudo-inverse of matrix Ω. Let Ǔ Š V T = Ω be the SVD of matrix Ω, where

Š = [ Σ O p×(m-p) ], Σ = diag(σ i ) ∈ R p×p , and 0 ≤ σ1 ≤ • • • ≤ σp , from which it directly follows Ω ΩT = Ǔ Σ2 Ǔ T and Ω † = V T Š † Ǔ where, as usual, Š † = [ Σ-1 O p×(m-p) ] T with Σ-1 = diag(σ † i ), σ † i = 1/σ i if σi > 0 and σ † i = 0 otherwise. As for Ω it is Ω = U Š V T + Ǔ Ṡ V T + Ǔ Š V T . Exploiting the orthonormality of Ǔ , we have Ǔ T Ǔ = I p =⇒ U T Ǔ + Ǔ T U = O p×p .
Denoting the skew-symmetric matrix Ǔ T U = ΓU , it is U = Ǔ ΓU and, following the same arguments, one has

V T V = ΓV = -ΓT V and V T = ΓV V T . Therefore, Ω = Ǔ ( ΓU Š + Ṡ + Š ΓV ) V T . (4.6)
We highlight that, as shown in [START_REF] Papadopoulo | Estimating the Jacobian of the Singular Value Decomposition: Theory and Applications[END_REF], matrices ΓU , ΓV and Ṡ can be computed in closed-form from the knowledge of Ǔ , V , Š and of the closed-form expression of Ω. This is also valid in our context since an explicit expression of Ω is assumed available, while matrices Ǔ , V and Š can be numerically retrieved from Ω via any standard SVD routine. Finally, exploiting (4.6) we have

Ω Ω † = Ǔ ΓU Š Š † Ǔ T + Ǔ Ṡ Š † Ǔ T + Ǔ Š ΓV Š † Ǔ T = Ǔ ( ΓU + Σ Σ-1 + ΣΓ V Σ-1 ) Ǔ T (4.7)
where ΓV = -ΓT V is the p × p upper-left block of matrix ΓV . At this point, the dissipation matrix is purposely taken as

Ȟ = V D 1 O p×(m-p) O (m-p)×p D 2 V T (4.8) with D 1 ∈ R p×p 0, D 2 ∈ R (m-p)×(m-p)
0, and, thus, Ȟ 0 as well. This choice in fact yields

Ω Ȟ Ω † = Ǔ ΣD 1 Σ-1 Ǔ T . (4.9)
By combining (4.5) with (4.7-4.9), and exploiting the diagonal form of matrix Σ, we finally obtain

¨ χ = Ǔ ( ΓU + Σ Σ-1 + ΣΓ V Σ-1 -ΣD 1 Σ-1 ) Ǔ T ˙ χ -Ǔ Σ2 Ǔ T ˇ χ =( Ǔ Σ)( Σ-1 ΓU Σ + Σ Σ-1 + ΓV -D 1 )( Σ-1 Ǔ T ) ˙ χ -Ǔ Σ2 Ǔ T ˇ χ =( Ǔ Σ)( Π -D 1 )( Σ-1 Ǔ T ) ˙ χ -( Ǔ Σ) Σ2 ( Σ-1 Ǔ T ) ˇ χ (4.10)
where

Π = Σ-1 ΓU Σ + Σ Σ-1 + ΓV . (4.11)
The expression obtained in (4.10) has a clear and neat structure: it indicates presence of a change of coordinates

= ( Σ-1 Ǔ T ) ˇ χ (4.12)
in which, in the approximation Σ-1 Ǔ T ≈ const, the system exhibits the simple (and almost diagonal) form

¨ = ( Π -D 1 ) ˙ -Σ2 , (4.13)
that is, a (unit-)mass-spring-damper system with diagonal stiffness matrix Σ2 .

The convergence rate of (4.13) is then related to the slowest mode of the system, i.e., that associated to the element σ2 1 in Σ2 . Therefore, in order to impose a given convergence speed and overall transient behavior to (4.13) (and to the estimation error dynamics (4.5)), one can try to 'place the poles' of (4.13) by:(i ) regulating σ2 1 to a desired value σ2 1,d and, at the same time, (ii ) shaping the damping factor D 1 in order to prevent the occurrence of oscillatory modes (∼ complex poles). Sections 4.3 and 4.4 explain how to achieve these two objectives. Remark 4.1. Note that, in the special situation p = 1 (only one quantity to be estimated), if σ 1 (t) ≡ const then Σ-1 Ǔ T ≡ const in (4.12) and matrix Π has no disturbing effects on (4.13). Therefore, in this case it is always possible to exactly enforce the ideal estimation error dynamics (4.15) by just keeping Ω(t) 2 = σ 2 1 (t) = const during the camera motion. This situation will apply to the case studies discussed in Sects. 4.5.1, 4.5.3 and 4.5.4.

Shaping the damping factor

A reasonable choice for matrix D 1 could be

D 1 = Π + C (4.14)
with C any positive definite matrix, without loss of generality (w.l.o.g.) a diagonal one C = diag(c i ), c i > 0, so as to obtain a completely decoupled transient behavior for (4.13)

¨ i + c i ˙ i + σ2 i i = 0, i = 1 . . . p. (4.15)
For instance, taking c i = c * i = 2σ i would (conveniently) result in a critically damped state evolution.

Matrix D 1 , however, is bound to remain positive definite over time, a constraint which, clearly, is not necessarily met by (4.14) for any arbitrary pair (C, Π). Let Πs and Πa represent the symmetric/skew-symmetric components of Π, and similarly for D 1 and C, with then

   Πs = 1 2 ( Σ-1 ΓU Σ -ΣΓ U Σ-1 ) + Σ Σ-1 D 1s = Πs + C s . (4.16) It is obviously D 1 0 ⇐⇒ C s -Πs .
In the special case of Σ = σI p = const (constant and coincident singular values), Πs = O p×p and thus (4.14) can be safely implemented for any choice of C 0. This possibility, however, requires a very stringent constraint on matrix Ω ΩT which may be hard to enforce in practice. Alternatively, a less stringent condition may be obtained by suitably bounding Πs qI p , q ≥ 0: in this case, any C s qI p would then guarantee D 1 0.

While this latter possibility is certainly viable, we note however the following: in the general case, satisfying the requirement C s qI p would guarantee positive definiteness of D 1 in (4.14) but at the possible expense of imposing an over-damped transient behavior to the system. In fact, in the general case, one could have C s qI p diag(c * i ). In other words, having aimed at obtaining a completely decoupled behavior for the evolution of (t) as in (4.15) could entail a unnecessary degradation of the transient response.

A full characterization of possible bounds on Πs is out of the scope of this work and thus will not be addressed here apart from the following qualitative considerations. Boundedness of Ω(t) and Ω(t) required by Lemma 3.1 together with (3.13) are sufficient conditions for guaranteeing boundedness of Πs as well. In fact, by a rough inspection of Πs , we can conclude that it can be made arbitrarily small by:(i ) limiting Σ (implied by limited Ω ), (ii ) limiting det( Σ) from below in order to prevent Σ-1 → ∞ (implied by (3.13)), and (iii ) limiting the rate of change of Σ and Ǔ , that is, by bounding Σ and ΓU (implied by limited Ω ).

In any case, in absence of as a deeper analysis, in the following we will not aim for a cancellation of matrix Π, but we will rather neglect its effects on the transient by just taking

D 1 = diag(c * i ) > 0.
This can of course result in a poorer overall behavior (for not compensating for Π), but avoids the introduction of any unnecessary lower bound on D 1 . The simulation and experimental results reported in Chapt. 5 will anyway show that not compensating for matrix Π has a marginal effect.

Tuning the stiffness matrix

We recall that matrix Σ2 = diag(σ 2 i ) contains the p eigenvalues of the square symmetric matrix Ω ΩT in (4.4). Let then Σ 2 = diag(σ 2 i ) represent the eigenvalues of matrix ΩΩ T in the original coordinates ( s, χ). From (4.4) it follows that, in order to affect Σ2 , one can either(i ) act on the gain α for a given Σ 2 , or (ii ) actively adjust Σ 2 for a given gain α (or, of course, any combination of both actions). The effect of gain α has already been discussed in Sect. 4.1: in short, one can exploit it to freely amplify/attenuate the eigenvalues of Σ2 as clear from (4.4). However, we note that, regardless of any choice of the gain, one still needs to ensure a minimum threshold σ 2 1 (t) ≥ σ 2 min > 0 for the estimation to converge, i.e., for fulfilling condition (3.13): this can only be achieved by actively tuning matrix Σ 2 . The rest of the section is then devoted to this issue.

We start by noting that, from (4.4), one has Σ2 = αΣ 2 =⇒ σ2 i = ασ 2 i . Therefore, seeking a desired value σ2 i,d is equivalent to imposing

σ 2 i → σ 2 i,d = σ2 i,d /(α)
. We can then focus on the regulation of the eigenvalues σ 2

i . An explicit expression of the time derivative of the eigenvalues σ 2 i can be obtained as follows: being Ω(t) = Ω(y, u), with y = (s, ς), it is σ 2 i (t) = σ 2 i (y, u) which, exploiting the results of [PL00, YFG + 10], allows to conclude

d dt σ 2 i = v j=1 v T i ∂(ΩΩ T ) ∂u j v i uj + n j=1 v T i ∂(ΩΩ T ) ∂s j v i ṡj (4.17)
where v i ∈ R p is the normalized eigenvector associated to σ 2 i . Letting

J u,i = v T i ∂(ΩΩ T ) ∂u 1 v i . . . v T i ∂(ΩΩ T ) ∂u v v i ∈ R 1×v (4.18) and J y,i = v T i ∂(ΩΩ T ) ∂y 1 v i . . . v T i ∂(ΩΩ T ) ∂y n v i ∈ R 1×n , (4.19)
eq. ( 4.17) can be compactly rewritten as

(σ 2 i ) = J u,i u + J y,i ẏ. (4.20)
Note, again, that the Jacobian matrices J u,i and J y,i in (4.18-4.19) can be computed in closed-form from the knowledge of the eigenvectors v i and of a closed-form expression for matrix Ω.

At this point, any differential inversion technique can be applied to (4.20) in order to affect the behavior of the i-th eigenvalue σ 2 i (t) by acting upon vector u: this must then be treated as the 'actual' input vector, with u regarded, instead, as an internal state. The eigenvalues σ 2 i can, in fact, be used to construct a task vector r or a cost function w( q) and the techniques described in Sect. 2.2.4 (in particular the ones in Sect. 2.2.4.4) can be used to regulate/maximize its value. Sect. 4.5 will discuss some examples in this sense. As illustration, the classical choice

u = J † u,i -k(σ 2 i -σ 2 i,d ) -J y,i ẏ , k > 0, (4.21)
would result in a perfect exponential convergence of

σ 2 i (t) → σ 2 i,d .
Remark 4.2. We note, however, that in general it is not possible to compensate for the term J y,i ẏ as simply done in (4.21) (or in any other equivalent law). Indeed, the formulation (3.10) implies a direct dependency of ṡ from the unmeasurable χ, and a similar dynamics can, in general, be found for ς, so that an exact evaluation of ẏ is not obtainable in practice. A possible workaround is to replace ẏ with an estimation ẏ obtained by plugging χ in (3.10) and in the dynamics of ς, in place of χ. As χ → χ one obviously has ẏ → ẏ, thus allowing for an asymptotic compensation of J y,i ẏ. Another possibility, when viable, is to enforce ẏ ≡ ∅ m+r during the system evolution. A combination of both strategies is, of course, also possible. The next sections will present some examples in this sense.

In all SfM problems, matrix ΩΩ T and its eigenvalues σ 2 i are only function of y and of the camera linear velocity v (never of the angular one). This is a direct consequence of the fact that, as already pointed out, the unknown 3-D parameters χ only appear in the columns of the interaction matrix that are related to the camera translational motion. Therefore (4.20) reduces to

(σ 2 i ) = J v,i v + J y,i ẏ ≈ J v,i v (4.22)
where the approximation is valid under the assumption that ẏ is kept (approximately) constant. In particular, roughly speaking, ΩΩ T is monotonically increasing with v 2 : the faster the camera motion the faster the SfM convergence regardless of any other optimization action. Use of the control law (4.21) could then result in a sub-optimal velocity direction compensated by a growth of v . To avoid such a situation, we prefer to consider the optimization of the observability as a secondary objective function to be maximized in the null space of a main task that has the only purpose of keeping the linear velocity norm constant:

v = -k 1 v v 2 (κ -κ d ) + k 2 I 3 - vv T v 2 ∇ v w(ΩΩ T ) (4.23) with k 1 > 0, k 2 ≥ 0, κ = 1 2 v T v, κ d = 1 2 v T 0 v 0 ,
and w(ΩΩ T ) is a function of matrix ΩΩ T that quantifies the observability. The analysis in Sect. [START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF].2 shows that the smallest eigenvalue σ 2 1 directly affects the convergence rate of the employed estimator, and thus a reasonable choice is to maximize σ 2 1 over time. This corresponds to the so called E-optimality criterion introduced in [START_REF] Ehrenfeld | On the efficiency of experimental design[END_REF].

w(ΩΩ T ) = Φ E = σ 2 1 ⇒ ∇ v w(ΩΩ T ) = J T v,1 .
Another optimality criterion, often used in the context of experimental design, is the A-optimality introduced by [START_REF] Chernoff | Locally optimal designs for estimating parameters[END_REF]. This aims at maximizing the trace of matrix ΩΩ T :

w(ΩΩ T ) = Φ A = tr(ΩΩ T ) = p i=1 σ 2 i ⇒ ∇ v w(ΩΩ T ) = p i=1 J T v,i . (4.24)
Unfortunately, the evaluation of the derivative/gradient of an eigenvalue as in (4.17) is not well-defined for repeated eigenvalues [START_REF] Friswell | The derivatives of repeated eigenvalues and their associated eigenvectors[END_REF]. In order to avoid this issue, one can also consider the quantity

w(ΩΩ T ) = Φ D = det(ΩΩ T ) = p i=1 σ 2 i (4.25)
as a conditioning measure for matrix ΩΩ T . Indeed, from classical linear algebra [START_REF] Bernstein | Matrix Mathematics: Theory, Facts, and Formulas[END_REF] the following relationship holds for a square matrix A

d dt det(A) = tr adj (A) dA dt (4.26)
with tr (•) and adj (•) being the trace and adjugate operators, respectively. Contrarily to the derivative of an eigenvalue, the relationship (4.26) is always well-defined with, in particular, no possible ill-conditioning due to repeated eigenvalues. Optimization of (4.25) goes under the name of D-optimality criterion and was first proposed by [START_REF] Wald | On the efficient design of statistical investigations[END_REF].

A variety of other optimality criteria have been proposed in the literature. We suggest to refer to [START_REF] Rady | Relationships among several optimality criteria[END_REF] for an overview and comparison of the different ones. Some of them turn out to be equivalent under certain assumptions [START_REF] Kiefer | General equivalence theory for optimum designs (approximate theory)[END_REF]. Finally, if p = 1, one obviously has

Φ E = Φ A = Φ D = σ 2 1 .

Application to a class of geometric primitives

In this section we illustrate the application of the proposed active estimation framework to four concrete SfM problems:(i ) estimation of the 3-D coordinates of a point feature, (ii ) estimation of the distance and orientation of a planar surface, (iii ) estimation of the 3-D position and radius of a spherical target, and (iv ) estimation of the 3-D position and radius of a cylindrical target.

In the point feature case, the effects of the adopted projection model on the estimation convergence are also explicitly considered by discussing the differences between the two popular choices of planar and spherical projection models introduced in Sect. 2.1.2.

For the planar target, we consider both the case of a collection of discrete points of interest extracted and tracked form the surface of a textured planar object, and that of a dense planar patch segmented in the image. Discrete and dense image moments can be used (among others) as a measurement to recover the plane 3-D parameters in the two cases. Since the choice of image moments affects the performance of the estimation, we also propose to adaptively select on-line the most informative image moments to use.

Finally for the spherical and cylindrical targets, we propose the use of two novel minimal parametrization that allow to express the sphere/cylinder 3-D structures in terms of measured visual features and of a single unknown parameter (the sphere/cylinder radius). This allows, in both cases, to reduce the SfM task to the estimation of a single unknown quantity (the sphere/cylinder radius), thus satisfying the requirements of Remark 4.1 for exactly imposing the ideal dynamics (4.15) to the estimation error.

Active Structure from Motion for a point

The first case study that we will consider is that of the estimation of the depth of a single point feature. We propose two different estimation schemes using either a planar or a spherical projection model. We will show that the two schemes have different and somehow complementary properties.

Planar projection model

Let π = (x, y, 1) = (X/Z, Y /Z, 1) ∈ R 3 be the perspective projection of a 3-D point p = (X, Y, Z) onto the image plane of a calibrated pinhole camera. As it is well know (see [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF] and Sect. 2.1.3.3), the differential relationship between the image motion of a point feature and the camera linear/angular velocity u = (v, ω) ∈ R 6 expressed in camera frame is given by the interaction matrix (2.10) as a function of the depth Z of the feature point. The dynamics of Z is

Ż = 0 0 -1 -yZ xZ 0 u. (4.27)
The expression in (2.10) is not linear in Z but it is linear in ζ = 1/Z. Therefore, by defining s = (x, y) ∈ R 2 and χ = ζ ∈ R, with then m = 2 and p = 1, we obtain for (3.10)

             f s (s, ω) = xy -1 + x 2 y 1 + y 2 -xy -x ω Ω (s, v) = xv z -v x yv z -v y f χ (s, χ, u) = v z χ 2 + (yω x -xω y ) χ , (4.28)
with the perturbation term d( x, t) in (3.12) taking the expression

d( χ, t) = v z χ 2 -χ 2 + (yω x -xω y ) χ, (4.29)
so that d(0, t) = 0 as expected. Note that, once χ has been estimated, one can obviously retrieve the 3-D position of the point feature as p = π/ζ = s/χ.

In the point feature case matrix ΩΩ T reduces to its single eigenvalue which, for a planar projection model, takes the expression

σ 2 1 = Ω 2 = (xv z -v x ) 2 + (yv z -v y ) 2 . (4.30)
Furthermore, as explained in Remark 4.1, in this case if σ 1 (t) ≡ const > 0 then by construction Σ-1 Ǔ T = const in (4.12) and matrix Π has no distorting effect on the behavior of (4.13). Therefore, it is always possible to exactly enforce the 'ideal' estimation error dynamics (4.15) by keeping Ω 2 = σ 2 1 = const. Moreover, using (4.30), the Jacobian J v,1 in (4.22) is given by

J v,1 = 2    v x -xv z v y -yv z (xv z -v x ) x + (yv z -v y ) y    T .
(4.31)

Since σ 2 1 does not depend on ω (this is always true for SfM problems), it is possible to freely exploit the camera angular velocity for fulfilling additional goals of interest without interfering with the regulation of σ 2 1 (t) (only affected by v). For instance, as in [START_REF] Chaumette | Structure from controlled motion[END_REF], one can use ω for keeping s const so as to make the effects of ṡ negligible when inverting (4.22) w.r.t. v, see Remark 4.2.

We now note that σ 2 1 in (4.30) depends on both the camera linear velocity v and on the location π of the feature point on the image plane. Since the value of σ 2 1 directly affects the convergence speed of the estimation error, it is interesting to study what conditions on s and v result in the largest possible σ 2 1 (i.e., the fastest possible convergence for a given gain α). Letting e 3 = [0, 0, 1] T being the camera optical axis, it is (by inspection)

Ω T 0 = [e 3 ] × [π] × v
where [a] × is the skew-symmetric matrix representing the cross product operator for 3-D vectors (i.e., [a] × b = a × b). Therefore,

σ 2 1 = Ω 0 Ω T 0 = [e 3 ] × [π] × v 2 = π 2 v 2 sin 2 (θ π,v ) sin 2 θ e 3 ,[π] × v
where θ π,v and θ e 3 ,[π] × v represent the angles between vectors (π, v) and vectors (e 3 , [π] × v), respectively. The maximum attainable value for σ 2 1 is then

σ 2 max = max π, v (σ 2 1 ) = π 2 v 2 . (4.32)
This maximum is obtained when the camera linear velocity v is such that π ⊥ v and e 3 ⊥ [π] × v, i.e., rearranging in matrix form

π T e T 3 [π] × v = x y 1 -y x 0 v = 0. (4.33)
If π = e 3 (point feature not at the center of the image plane), system (4.33) has (full) rank 2 and admits the unique solution (up to a scalar factor)

v ∼ = [π] 2 × e 3 .
This requires the linear velocity v to be orthogonal to π and to lie on the plane defined by vectors π and e 3 (i.e., v must belong to a straight line as shown in Fig. 4.1(a)).

If π = e 3 (point feature at the center of the image plane), system (4.33) loses rank and any v ⊥ e 3 is a valid solution, see Fig. 4.1(b).

It is then possible to draw the following conclusions: for a given norm of the linear velocity v (i.e., the amount of 'control effort'), system (4.33) determines the direction of v resulting in σ 2 1 = σ 2 max (maximization of σ 2 1 ). These conditions are summarized in Figs. [START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF].1(a) and 4.1(b). The value of σ 2 max is, however, also a function of the feature point location π which can be arbitrarily positioned on the image plane. In particular, σ 2 max = v 2 for π = e 3 and σ

2 max = π 2 v 2 > v 2 ∀ π = e 3 , with lim π →∞ σ 2 max (π) = ∞.
The value of π (distance of the point feature from the projection center) thus acts as an amplification factor for σ 2 max . Therefore, 1. the smallest σ 2 max (i.e., the slowest 'optimal' convergence for the depth estimation error) is obtained for the smallest value of π , i.e., when π = e 3 =⇒ π = 1 (feature point at the center of the image plane). It is worth noting that in this case v z = 0 (from the condition v ⊥ π) and σ 2 max = v 2 = v 2 x +v 2 y : the camera moves on the surface of a sphere with a constant radius (depth) pointing at the feature point. Also, being in this case χ = Ż/Z 2 = 0, one has d( χ, t) ≡ 0 and global convergence for the estimation error (see Remark 3.1); 2. the largest σ 2 max (i.e., the fastest 'optimal' convergence for the depth estimation error) is obtained for the largest possible value of π . In the usual case of a rectangular image plane centered at the origin, this translates into keeping the feature point positioned at one of the four image corners. However, compared with the previous case, this results in a d( χ, t) = 0 and only local convergence for the estimation error. Moreover, as shown in [START_REF] Chaumette | Structure from controlled motion[END_REF], this also results in an increased impact of measurement errors (e.g. discretization and other undeterministic effects) on the estimation.

Spherical projection model

We now develop the depth estimation machinery for the spherical projection model. In this case, the following quantity is taken as visual feature measured on the image plane

s = η = π π = p p ∈ S 2 ,
where S 2 represents the unit sphere and, as well-known (see [START_REF] Hamel | Visual servoing of an under-actuated rigid body system: An image based approach[END_REF]) and shown in Sect. 2.1.3.3, Hence by taking χ = δ one obtains for (3.10)

ṡ = δ ss T -I 3 [s] × u, with δ = 1 p and δ = -δ 2 d p dt = -δ 2 s T ṗ = δ 2 s T v. ( 4
       f s (s, u) = [s] × Ω Ω (s, v) = -v T I 3 -ss T f χ (s, χ, u) = χ 2 s T v (4.35)
with m = 3, p = 1, and d( χ, t) = ( χ 2 -χ 2 )s T v for the perturbation term in (3.12). We note that, although in this case m = 3, vector s is subject to the constraint s = 1, thus resulting in only two independent measurements (as in the previous case of planar projection). Moreover, from the estimated χ one can easily retrieve p = η/δ = s/χ.

For the spherical projection model, the eigenvalue determining the convergence of the estimation error is

σ 2 1 = ΩΩ T = v T v -(s T v) 2 ,
with thus

J v,1 = 2v T (I 3 -ss T ). (4.36)
As before, σ 2 1 does not depend on ω which can then be exploited to fulfill any additional task of interest (e.g., keeping s const during motion).

As for the conditions on s and v that yield maximization of σ 2 1 , one clearly has

σ 2 1 = σ 2 max = max s, v (σ 2 1 ) = v 2 (4.37)
iff s T v = 0 (linear velocity orthogonal to the projection ray passing through p). We also note that, in this case, one has χ = 0 and d( χ, t) ≡ 0 (constant unknown state and global convergence for the estimation error) regardless of the location of s on the image plane.

Comparison between planar and spherical projection models

For a spherical projection model, maximization of the eigenvalue σ 2 1 imposes only one condition for the linear velocity v (η T v = 0). When this condition is met, one has σ 2 1 = σ 2 max = v 2 and global convergence for the estimation error whatever the location of the feature point η. This is equivalent to what was obtained for the planar projection case in the special situation π = e 3 (indeed the two projection models coincide for π = η = e 3 ). However, with a spherical projection model one also loses the possibility to increase the estimation convergence rate by suitably positioning the point feature s on the image plane (since in this case σ 2 max does not depend on s).

It is then worth noting the complementarity of the two cases: for a given v , and provided the optimal condition π T v = 0 is satisfied, the planar projection allows obtaining a faster error convergence at the price of local stability (increase of the perturbation d) by suitably positioning s = [x, y] T (the larger s the faster the convergence). The spherical projection guarantees global error convergence for any location of the feature point, but at the price of being always subject to the same convergence rate only function of the control effort v .

Active Structure from Motion for a plane

Plane detection and estimation from raw visual data is a classical problem in sensorbased robot control, especially in the context of mobile robotics. Indeed, planes are widespread in artificial (man-made) and natural environments, and therefore constitute the typical 3-D structure one tries to segment in order to, e.g., plan safe paths among planar obstacles (e.g., vertical walls), or navigate by keeping a desired attitude or distance from special planes (e.g., ground plane for flying robots). The ability to classify and reconstruct planes in the perceived environment is therefore an important feature for several sensor-based applications. When dealing with images taken by a (possibly moving) camera, a number of approaches has been developed for solving the problem of detecting and identifying planes from visual data.

Several methods for instance exploit known correspondences across frames to identify point features (or directly pixels) as whether belonging to a common plane together with the associated plane parameters [ASNM10, LJPS10, GBR12]. These methods usually rely on special geometric constraints linking two views of a pla-nar scene such as the homography constraint. We have already explained, in Sect. 2.1.3.2, some techniques for exploiting the homography constraint between two views of the same set of points to recover the parameters of the plane and the camera motion up to a scalar factor. We will use the classical 8-point algorithm, in the following referred to as method A, as a baseline for comparison and evaluation of the other original ones proposed in this section.

Alternative strategies, such as those suggested in [VBP08, SSN11, BELN11] and references therein, instead, attempt to directly measure (using special sensors such as the RGB-D camera) or recover (exploiting structure from motion algorithms) a 'depth map' of the observed images, for then dealing with the issue of clustering and extracting planes from clouds of 3-D points. In these cases, the problem is rather on how to fit planes to sets of 3-D points and on how to cluster them according to some reasonable 'planarity measure'. We propose, in Sect. 4.5.2.1, a solution that can be ascribed to this second class because it uses a simple least-squares fitting strategy to extract a plane from a point cloud of estimated points. In Sect. 4.5.2.2 we will discuss, instead, an alternative solution that estimates the plane parameters directly from a set of discrete image moments. Finally Sect. 4.5.2.4 explains how a similar method could be applied to a set of dense image moments computed on a segmented planar patch.

Plane reconstruction from 3-D points

Assume that a visual tracker is able to extract and track, in a sequence of images, the projection of a set of N points p k belonging to the same plane P.

Using the active strategies discussed in Sect. 4.5.1, one can (optimally) retrieve an estimation Z k of the unknown depth Z k of each point (similarly, one could also use a spherical projection model to find an estimate p k of the point distances p k ). Then, from each measured point feature π k (or η k ) one can recover an estimation p k = Z k π k (or p k = p k η k ) of the corresponding 3-D point p k in the current camera frame. Let P : n T E + d = 0 be the equation of the sought plane, with n ∈ S 2 and d ∈ R representing the unit normal vector and distance in camera frame. For the estimated points p k to belong to P, it must hold

n T p k + d = 0, i = 1 . . . N. ( 4
    p T 1 1 . . . . . . p T N 1     n d = A n d = ∅ N (4.39)
with A ∈ R N ×4 . Assuming N ≥ 4 and rank (A) = 3, the linear system (4.39) has a unique solution (up to a scalar factor) for the pair (n, d) which can be found by standard least-square techniques. Let U A S A V T A = A be the SVD of matrix A, with σ 1,A ≤ • • • ≤ σ 4,A being the associated singular values. As well-known, a (leastsquare) solution of the homogeneous system (4.39) is given by v 1 = [v 11 , v 12 , v 13 ] T , the column of V A associated to σ 1,A . Furthermore, the inverse of the condition number σ A = σ 1,A /σ 4,A can be taken as a normalized measure of the planarity of the N points p k , in fact rank (A) = 3 ⇐⇒ σ A = 0. From v 1 one can then recover

n d = ± v 1 v 2 11 + v 2 12 + v 2 13 , (4.40)
i.e., by imposing n = 1. The final sign ambiguity can be resolved by fixing the sign of d according to the adopted convention.

As for the issue of optimally recovering the unknown depths Z k for the N tracked point features π k , this can be addressed by exploiting the SfM scheme (3.11). Let s = [x 1 , y 1 , . . . , x N , y N ] T ∈ R 2N be the vector of measured visual features, and χ = [1/Z 1 , . . . , 1/Z N ] T ∈ R N be the 3-D structure to be estimated (the depths of all tracked points). This choice results in the matrix

ΩΩ T = diag(σ 2 1,1 , σ 2 1,2 , . . . σ 2 1,N ), (4.41)
with

σ 2 1,i = (x k v z -v x ) 2 + (y k v z -v y ) 2 (4.42)
being the eigenvalue determining the convergence speed of the k-th estimation error

χ k (t) = χ k (t) -χ k (t) = 1/ Z k (t) -1/Z k (t)
for the k-th feature point. Exploiting (4.22), optimization of the convergence of the whole vector χ(t) can then be obtained by, e.g., maximizing the minimum eigenvalue

σ 2 m = min i=1...N σ 2 1,i (4.43)
w.r.t. the camera linear velocity v.

We finally note that this method does not require the exact matching of point features across distant frames (initial and current ones) as it is instead the case for the homography reconstruction method described in Sect. 2.1.3.2, but it only needs a frame-by-frame tracking. As a consequence, the method can straightforwardly cope with loss/gain of feature points because of, e.g., limited FOV: new estimated points p k can be added to system (4.39) by initializing the corresponding estimated depth Z k so as to belong to the current estimation of the plane P. The only assumption (common to all the methods) is that all the tracked points seen by the moving camera belong to a common plane1 . In the following, this second possibility for recovering (n, d) will be denoted as method B.

Plane reconstruction from discrete image moments

Another possibility for estimating the structure of a plane is based on the machinery of point-based image moments originally introduced in [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF]. This method, hereafter denoted as method C, can be seen as a further improvement of method B in that it exploits the active estimation scheme (3.11) for directly estimating the pair (n, d) (3 independent quantities) instead of the N depths Z k of the N considered point features π i for then algebraically solving system (4.39). Thus, the complexity of the SfM scheme results reduced w.r.t. method B as the number of estimated states is independent of the number of tracked points. Furthermore, since (n, d) are directly estimated via a filtering process, one can expect method C to be more robust than method B w.r.t. non perfectly planar scenes as no algebraic step is involved (contrarily to method B that still requires the least-square solution of the linear system (4.39)). Indeed, these considerations are also supported by the experimental results of Sect. 5.2.

Consider then the (i, j)-th moment m ij evaluated on the collection of N observed feature points π k = (x k , y k , 1)

m ij = N k=1 x i k y j k . (4.44)
As shown in [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF], dividing the plane equation (4.38) by Z k and d, the depth Z k on any 3-D point p k ∈ R 3 lying on this plane can be expressed in terms of its normalized image coordinates π k as

1 Z k = ζ k = - n T d π k = ν T π k , (4.45)
where ν = -n/d ∈ R 3 represents an unmeasurable 3-D scene structure (as with Z for the point feature case). Exploiting this fact, [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] shows that the dynamics of m ij takes the expression

ṁij = f m ij (m kl , ω) + T m ij (m kl , v)ν (4.46)
where m kl stands for the generic (k, l)-th moment of order up to i+j +1. Analogous considerations hold for the centered moments

µ ij = N k=1 (x k -x g ) i (y k -y g ) j
with x g = m 10 /m 00 and y g = m 01 /m 00 being the barycenter coordinates, and m 00 = N = const in this case. Furthermore, it is (see, e.g., [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF])

ν = νν T v -[ω] × ν = f ν (ν, u). (4.47)
The estimation scheme (3.11) can then be directly applied for recovering χ = ν by including in s a suitable collection of m ≥ 3 image moments s = (m i 1 j 1 , . . . , m imjm ), and thus letting ς = (m k 1 l 1 , . . . , m krlr ) be the set of moments that appear in the dynamics of s but are not included in s,

f s = [f m i 1 j 1 . . . f m imjm ] T ∈ R m , f χ (χ, u) = f ν (ν, u) and Ω = [ m i 1 j 1 . . . m imjm ] ∈ R 3×m . (4.48)
This estimation strategy, however, lacks the possibility of taking into account the loss/gain of feature points over time (as it is instead the case with method B). When a feature point leaves visibility, a practical workaround could be to just redefine a moment m ij as the sum over the remaining N -1 points and feed the estimation scheme with this new measurement (and analogously for new points entering visibility). However, this would clearly introduce a discontinuity in the measured m ij -a discontinuity not modeled by the dynamics (4.46) which predicts the moment evolution as only a function of the camera own motion (v, ω). Therefore, we now propose a redefinition of weighted image moments meant to explicitly cope with this issue.

Assume presence of a countable number of feature points on the plane π k = [x k , y k , 1] T , k = 1 . . . ∞, and define the (i, j)-th weighted moment as

m ij = ∞ k=1 w(x k , y k , t -t k )x i k y j k , (4.49)
where the weighting function w(x, y, τ ) : R 3 → [0, 1] is a sufficiently smooth map, and t k represents the time at which the point feature π k is considered for the first time.

The weight w can be exploited to assign a 'quality' measure to each feature point so as to enforce a smooth change in m ij whenever a tracked feature leaves visibility or a new feature is taken into consideration (regardless of its position on the image plane). In particular, we design the weight w(x, y, τ ) as the product of three scalar functions w(x, y, τ ) = w 1 (x)w 2 (y)w 3 (τ ).

Weights w 1 (x) and w 2 (y) are designed so as to vanish at the image borders and are meant to smoothly take into account features entering/exiting the image plane. Weight w 3 (τ ) is finally intended to smoothly take into account the introduction of a newly detected feature point π k when already within visibility (for instance, when starting to track a new point close to the image center). Figure 4.2 shows a possible shape for w 1 (x) (also representative of w 2 (y)) and w 3 (τ ).

Exploiting the definition (4.49), it is easy (although tedious) to obtain an expression conceptually equivalent to (4.46) for the dynamics of the weighted moments ṁij . Some details in this sense are reported in Appendix A.2. This then allows to directly apply the SfM scheme (3.11) to the case of weighted moments. We finally note that, in practice, the summation (4.49) is clearly evaluated only on the (finite but time-varying) set of currently tracked point features since w(x k , y k , t -t k ) = 0 for any π k not visible or not considered at any time t ≤ t k .

As for which moments to consider for the estimation of χ, after some experimental tests we opted for

s = (x g , y g , µ 20 , µ 02 , µ 11 ) ∈ R m , m = 5. (4.50)
This choice is partially motivated by [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF] which proposed the triple (x g , y g , µ 20 + µ 02 ) as a good set of features for controlling the camera translational DOFs in a VS loop. However, we empirically found this latter set to be ill-conditioned for what concerns the estimation of χ, with instead (4.50) providing enough information (i.e., full rankness of matrix ΩΩ T ) for the estimation convergence. Alternatively, one could also resort to an adaptive/online selection of the best set of image moments as discussed in Sect. 4.5.2.3.

Finally, analogously to the previous cases, optimization of the structure estimation convergence from image moments can be achieved by maximizing w.r.t. v the smallest eigenvalue σ 2 1 of the square 3 × 3 matrix ΩΩ T from (4.48).

Optimizing online the selection of moments

As already anticipated in the previous section, the selection of a good set of image moments for visual control or SfM is still an open problem. Ideally, for VS applications, one would like to find a unique set of visual features resulting in the 'most linear' control problem with the largest convergence domain. In case of SfM tasks, instead, one is in general interested in maximizing observability for a given camera displacement. However, to the best of our knowledge, only local, partial (e.g., depending of the particular shape of the object) or heuristic results are currently available. For instance, [TC05, KDS + 07, BCM13] propose different combinations of image moments able to only guarantee local stability of the servoing loop around the desired pose, with a basin of attraction to be heuristically determined case by case. As for what concerns the SfM case, the choice of which moments to exploit for allowing a converging estimation of the scene structure is also not straightforward.

In [START_REF] Giordano | 3D structure identification from image moments[END_REF] the area a and barycenter coordinates (x g , y g ) of a dense region are successfully fed to a SfM scheme based on the (intuitive) motivation that the same set (a, x g , y g ) is also the typical choice for controlling the camera translational motion in a servoing loop [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF]. However, this intuition breaks down when considering moments of a discrete point cloud: in this case, the typical choice for controlling the camera translational motion, that is, the set (x g , y g , µ 20 + µ 02 ) (see [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF]), is empirically shown in [START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF] to not provide enough information for allowing a converging estimation of the scene structure.

One could argue that the hope of finding a unique set of visual features optimal in all situations might eventually prove to be unrealistic, if not impossible, while it could just be more appropriate (and reasonable) to rely on an automatic and online selection of the best feature set (within a given class) tailored to the particular task at hand. Motivated by these considerations, we propose, in this section, a generalization of the image moments definition that aims in this direction.

Let w = w(x, y, θ) be a smooth function of the coordinates (x, y) on the image plane and of a vector of parameters θ ∈ R h . One can generalize (4.44) and define a weighted parametric image moment for N observed features π k

m w (θ) = N k=1 w(x k , y k , θ), (4.51)
with, obviously, m w = m ij for w(x, y, θ) = x i y j . Function w(x, y, θ) can be seen as the class of all the considered image moments (e.g., a quadratic form in x, y) parametrized by vector θ (e.g., the coefficients of the quadratic form). Consider now the following additional definitions

m x w ij (θ) = N k=1 x i k y j k ∂w(x, y, θ) ∂x (x k , y k ) m y w ij (θ) = N k=1 x i k y j k ∂w(x, y, θ) ∂y (x k , y k ) m θ w (θ) = N k=1 ∇ θ w(x, y, θ) T (x k , y k ) , (4.52)
and note that m θ w is a row vector of dimension h. Following the derivations in [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF], it is easy to show that the dynamics of m w (θ) takes the expression (reminiscent of (4.46))

ṁw (θ) =[m A (v, θ) m B (v, θ) m C (v, θ)]ν + [m ωx (θ) m ωy (θ) m ωz (θ)]ω + m θ w (θ) θ (4.53) with                          m A = -m x w 10 v x -m y w 10 v y + (m x w 20 + m y w 11 )v z m B = -m x w 01 v x -m y w 01 v y + (m x w 11 + m y w 02 )v z m C = -m x w 00 v x -m y w 00 v y + (m x w 10 + m y w 01 )v z m ωx =(m x w 11 + m y w 02 + m y w 00 ) m ωy =(-m x w 00 -m x w 20 -m y w 11 ) m ωz =(m x w 01 -m y w 10 ) , (4.54
)

and ν = [A, B, C] T = -n/d.
One can then exploit (4.51-4.54) for implementing a visual control or estimation algorithm as in the classical case, but with the additional possibility of acting on vector θ (a free parameter) for optimizing any criterion of interest, e.g., the norm of the observability matrix ΩΩ T during an estimation task.

Clearly, there exist many possibilities for designing the weighting function w(•), i.e., the class of moments spanned by vector θ. A convenient choice, in our opinion, is to take w(•) as some polynomial basis in x and y with θ being the vector of coefficients. Indeed, in this way the weighted moments (4.51), the expressions in (4.52) and, eventually, all the terms in (4.54) will reduce to linear combinations of the unweighted moments m ij in (4.44). The overall computational complexity will then result equivalent to the classical case [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF].

As for which polynomial basis to exploit, many choices are possible depending on the constraints/requirements of the particular application. Within the scope of this work, two possibilities are considered:

Polynomial basis of fixed degree First, one can take w(•) as a polynomial in x and y of a given degree δ ∈ N + , that is, The class (4.56) can then specialize into, e.g., the barycenter coordinate x g for θ = (1/N, 0, 0, 0, 0), the centered moment µ 02 for θ = (0, -y g , 0, 0, 1), and so on. Clearly, the larger the value of the degree δ, the richer the basis representation power in encoding the scene geometry, but at the (well-known) cost of an increasing noise level with the moment order.

w(x, y, θ) = δ j=1 j k=0 θ T j +k x (j-k) y k
Constrained polynomial basis A second possibility is to design a constrained polynomial basis for coping with the possible loss/gain of point features during the camera motion because of the limited camera FOV. Indeed, by imposing that w(•) vanishes (with vanishing derivative) at the image borders, any point feature close to the limits will smoothly enter or leave the image plane and, thus, prevent any discontinuity in the moment dynamics (4.53).

Let then x min < x max and y min < y max represent the limits of a rectangular image plane, and consider a weighting function w(•) partitioned as w(x, y, θ) = w x (x, θ x )w y (y, θ y ), (4.57)

where w x (x, θ x ) and w x (x, θ y ) are polynomial bases and θ = (θ x , θ y ) ∈ R hx+hy , h x + h y = h, is the vector of coefficients. Assuming h x ≥ 4 and imposing

     w x (x min , θ x ) = w x (x max , θ x ) = 0 ∂w x (x, θ x ) ∂x x min = ∂w x (x, θ x ) ∂x xmax = 0 , (4.58)
one can solve for a set of 4 parameters in vector θ x for shaping w x (x, θ x ) as desired.

For instance, by taking

w x (x, θ x ) = θ x 1 x 5 + θ x 2 x 4 + θ x 3 x 3 + θ x 4 x 2 + θ x 5 x + θ x 6 (4.59)
and by (arbitrarily) choosing the pair (θ x 1 , θ x 2 ) as free parameters in vector θ x , system (4.58) yields

                         θ x 3 = (-3x 2 max -3x 2 min -4x max x min )θ x 1 + (-2x min -2x max )θ x 2 θ x 4 = (2x 3 max + 8x 2 max x min + 8x max x 2 min + 2x 3 min )θ x 1 + (x 2 max + 4x max x min + x 2 min )θ x 2 θ x 5 = (-7x 2 max x 2 min -4x 3 min x max -4x 3 max x min )θ x 1 + (-2x max x 2 min -2x 2 max x min )θ x 2 θ x 6 = (2x 3 max x 2 min + 2x 2 max x 3 min )θ x 1 + x 2 max x 2 min θ x 2 .
(4.60)

Imposing analogous conditions to function w y (y, θ y ) at y min and y max (with again h y ≥ 4) will then constrain a total of 8 parameters in vector θ, with the remaining h -8 coefficients still free to be exploited for optimization purposes. For the sake of illustration, Figure 4. We conclude by noting that, compared to the previous case (4.55), this latter possibility necessitates of a polynomial basis (4.57) with a degree of at least 7. Indeed, as explained, the vanishing conditions at the image border will constrain 4 + 4 coefficients in θ x and θ y , thus forcing both w x (x, θ x ) and w y (y, θ y ) to have a degree of (at least) 3 for ensuring h x ≥ 4 and h y ≥ 4 as required (see (4.59)). However, for any optimization of the coefficient vector θ to be possible, either h x > 4 or h y > 4 must hold for allowing presence of at least one free coefficient to be optimized besides those already constrained by the vanishing conditions. On the other hand, if either h x > 4 or h y > 4, the final polynomial basis (4.57) will necessarily result of at least degree 7. Therefore, the use of higher-order moments (of at least order 7) is the 'price to pay' for smoothly taking into account the loss/gain of point features during the camera motion2 . In contrast, the degree of the polynomial basis in (4.55) can be chosen at will and thus adjusted, if necessary, for limiting the noise level in the measured moments.

Ω(s, v, θ) =    m A (s, v, θ 1 ) • • • m A (s, v, θ m ) m B (s, v, θ 1 ) • • • m B (s, v, θ m ) m C (s, v, θ 1 ) • • • m C (s, v, θ m )    ∈ R 3×m . (4.61)
Therefore, when employing the weighted parametric moments (4.51) instead of the classical moments (4.44), one gains the additional possibility of also acting on vector θ (i.e., on the 'moment shape') for affecting matrix ΩΩ T .

By applying (4.26) to matrix ΩΩ T and expanding the various terms, one obtains

ΦD = i tr adj ΩΩ T ∂(ΩΩ T ) ∂v i vi + i tr adj ΩΩ T ∂(ΩΩ T ) ∂θ i θi + i tr adj ΩΩ T ∂(ΩΩ T ) ∂s i ṡi = J v v + J θ θ + J s ṡ (4.62)
where the Jacobian matrices

J v = . . . tr adj ΩΩ T ∂(ΩΩ T ) ∂v i . . . ∈ R 1×3 J θ = . . . tr adj ΩΩ T ∂(ΩΩ T ) ∂θ i . . . ∈ R 1×h J s = . . . tr adj ΩΩ T ∂(ΩΩ T ) ∂s i . . . ∈ R 1×m (4.63)
are function of (s, v, θ) (all available quantities). We stress that all the terms in (4.63) can be computed in closed-form.

The relation (4.62) can then be exploited for affecting Φ D (t) over time by acting on v (the camera linear acceleration) and/or θ (the parameter vector). Among the many possibilities, we considered here the following update rules

         v = k v I 3 - vv T v T v J T v θ = k θ I 3 - θθ T θ T θ J T θ , k v > 0, k θ > 0 (4.64)
which are meant to maximize Φ D (t) by following its gradient w.r.t. (v, θ) projected on the null-spaces of the constant-norm constraints v(t) = const and θ(t) = const. As explained in Sect. 4.4, the constraint v(t) = const is meant to prevent a better conditioning of matrix ΩΩ T only due to a faster camera motion while observing the scene. The second constraint θ(t) = const is motivated by similar arguments: an increasing θ(t) would (artificially) magnify Φ D (t) at the cost of an increased noise level (all the terms in (4.54) would just result amplified).

The optimization action (4.64) will then maximize the observability measure det(ΩΩ T ) for the SfM task at hand by:(i ) adjusting the direction of the camera linear velocity v (as with the other case studies presented so far) and, at the same time, (ii ) by adapting the shape of the m weighted moments s = (m w (θ 1 ), . . . , m w (θ m )) as only a function of the perceived scene and camera motion. We also remark that (4.64) (or any other equivalent strategy) assumes the possibility of acting at will on the direction of the linear camera velocity v. There could be cases where this is not (fully) possible, and v (or components of it) are given (for example during a combined estimation/servoing loop as ii will be discussed in Chapt. 6). In all these cases, it is obviously still possible to just keep on optimizing θ(t) during the (given/known) camera motion in order to adapt, as best as possible, the moment shape. Finally, since Ω(t) (and thus Φ D (t)) does not depend on the camera angular velocity, one can freely choose ω to fulfill any additional goal of interest.

Using dense image moments

We conclude the analysis of the plane estimation case study by discussing a possible extension of these techniques to the case of dense image moments. In some context, in fact, the (planar) scene observed by the camera might not be sufficiently textured to allow for the extraction and tracking of a sufficient number of point features in a reliable way. For this kind of situations it might be better to segment and track some objects or "patches" in the images and use a "dense" definition of the (i, j)-th order image moments in which the summation in (4.44) (and similar definitions) is substituted by a continuous integral over the image of object O

m ij = O x i y j dxdy.
(4.65)

Still from [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF], the dynamics of m ij can be shown to take the expression

ṁij = L m ij (m kl , χ) u = f m ij (m kl , ω) + T m ij (m kl , v)ν (4.66)
where m kl stands for a generic (k, l)-th moment of order up to i + j + 1 and, again, ν = -n/d. Note that (4.66) is linear in the unmeasurable χ, while all the other quantities are available to measurement. Let then s = [m i 1 j 1 . . . m imjm ] T ∈ R m be a collection of m image moments, ς = [m k 1 l 1 . . . m krlr ] T ∈ R r be the set of moments that appear in the dynamics of s but are not part of s, and χ = ν. Using again (4.47), formulation (3.10) can be recovered with

                 f s (s, ς, ω) = f m i 1 j 1 (s, ς, ω) . . . f m imjm (s, ς, ω) T Ω (s, ς, v) = m i 1 j 1 (s, ς, v) . . . m imjm (s, ς, v) f χ (χ, u) = χχ T v -[ω] × χ d( χ, t) = ( χ χ T -χχ T )v -[ω] × e 3 .
where

Ω ∈ R p×m , p = 3. As before, it is d(∅ 3 , t) = ∅ 3 .
As with the discrete moments case, choice of which moments to include in s is in general not obvious as many possibilities exist (in number and kind). The adaptive techniques, described in the previous sections, for selecting online the set of moments to use and for dealing with the limitations of the camera FOV, can easily be extended to the case of dense image moments. However, for the results in Sect. 5.2.5, we limited our analysis to the use of the lowest-order moments because of their robustness w.r.t. image noise: s = [a, x g , y g ] T , i.e., the area a and barycenter (x g , y g ) of the observed object. This choice, originally suggested by [START_REF] Giordano | 3D structure identification from image moments[END_REF], implies m = p = 3, thus yielding a 'square' problem (square matrix Ω). Note that, in this case, vector ς = [n 20 , n 11 , n 02 ] T ∈ R3 (the normalized centered moments of order 2) thus yielding r = 3, see [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. For the case under consideration, matrix Ω then takes the expression

Ω =    3ax g v z -av z x 2 g + 4n 20 v z -x g v z (x g y g + 4n 11 ) v z -x g v y 3ay g v z -av y (x g y g + 4n 11 ) v z -y g v z y 2 g + 4n 02 v z -y g v y 2av z x g v z -v z y g v z -v y    (4.67)
We can note that, as in all other cases, Ω = Ω(s, ς, v) thus allowing to exploit the camera angular velocity ω for fulfilling additional tasks of interest without affecting observability. For example, analogously to the point feature case, one could try to keep the barycenter at a constant position (x g , y g ) const in order to mitigate the effects of ṡ in (4.22). Furthermore, it is interesting to note that matrix Ω (and, therefore, matrix ΩΩ T as well) loses rank whenever v z = 0: in order to meet condition (3.13), that is, to keep σ 2 1 (t) > 0, the camera then necessarily needs to translate with a non-zero component along the optical axis regardless of the orientation of the plane 3 . No special insights can be gained, instead, from the inspection of the Jacobian matrix J v . We conclude this section by mentioning the interesting possibility of employing the photometric image moments, as defined in [START_REF] Bakthavatchalam | Photometric moments: New promising candidates for visual servoing[END_REF], to estimate the plane structure. The general expression of the (i, j)-th order photometric image moment is in fact

m ij = I Y (x, y)x i y j dxdy. (4.68)
where Y (x, y) is the intensity level of the image in position (x, y) and the integral is extended to the entire image I. With this new definition, the image processing steps (segmentation, tracking and so on) are simplified even further since it is not necessary to identify neither a specific set of points p k nor a particular object O as it is the case, instead for (4.44-4.65) and other similar definitions introduced in this section. Along the same line, Appendix B will present some preliminary original results on the direct use of the photometric information for SfM estimation.

Active Structure from Motion for a sphere

We now detail the application of the proposed estimation machinery to the case of a spherical target. Consider a sphere O S of radius R and let p 0 = (X 0 , Y 0 , Z 0 ) be the coordinates of its center in the camera frame. Let also

L : n T p + d = 0
represent the planar limb surface associated to the sphere in the camera frame, where p ∈ R 3 is any 3-D point on the plane, n ∈ S 2 is the plane unit normal vector and d ∈ R the plane distance to the camera center [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. Figure 4.4 shows the quantities of interest.

The depth Z of any point p lying on L can be expressed in terms of its normalized image coordinates π = (x, y, 1) as

1 Z = X 0 K x + Y 0 K y + Z 0 K = ν T π, (4.69)
where K = p T 0 p 0 -R 2 and ν = p 0 /K = -n/d ∈ R 3 represent unmeasurable quantities (analogously to Z for the point feature case), see [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF] for all the details. As discussed in Sect. 4.5.2, the interaction matrix of a generic (i, j)-th order moment m ij evaluated on the image of O S depends linearly on ν, see again [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF][START_REF] Giordano | 3D structure identification from image moments[END_REF]. Therefore, a first possibility to retrieve the sphere 3-D parameters (p 0 , R) would be to implement the estimation scheme (3.11) with s being a suitable collection of image moments (e.g., area and barycenter) and χ = ν. It is in fact possible to show that (see Appendix A.3)

χ = - v K -[ω] × χ + 2χχ T v
and that K can be expressed in terms of image moments and of vector χ itself, so that, having estimated χ, one can consequently retrieve p 0 = χK and R = p T 0 p 0 -K.

Although conceptually valid, this solution requires the concurrent estimation of three time-varying quantities (vector χ(t)). On the other hand, inspired by [START_REF] Fomena | Improvements on Visual Servoing From Spherical Targets Using a Spherical Projection Model[END_REF], we now describe a novel representation of the sphere projection on the image plane that allows to reformulate the structure estimation task in terms of a single unknown constant parameter, i.e., the sphere radius R.

To this end, define vector s = (s x , s y , s z ) ∈ R 3 as

                 s x = x g s z a 2 1 s y = y g s z a 2 1 s z = 1 + a 2 1 a 2 1 , (4.70)
where (x g , y g , n 20 , n 11 , n 02 ) represent the barycenter and normalized centered moments of order 2 measured from the elliptical projection of the sphere O S on the image plane, and a 1 is the minor axis of the observed ellipse with [Cha04]

a 2 1 = 2 n 20 + n 02 -(n 20 -n 02 ) 2 + 4n 11 . (4.71)
We thus note that vector s can be directly evaluated in terms of measured image quantities. From [Cha04, FC09] one also has

x g = X 0 Z 0 Z 2 0 -R 2 , y g = Y 0 Z 0 Z 2 0 -R 2 , a 2 1 = R 2 Z 2 0 -R 2 (4.72)
which, when plugged in (4.70-4.71), result in the equivalent expression s = p 0 /R. Since vector s can be computed from image measurements as in (4.70), estimation of the (unknown) sphere radius R allows to recover the 3-D sphere center as p 0 = sR.

Exploiting now the results of [START_REF] Fomena | Improvements on Visual Servoing From Spherical Targets Using a Spherical Projection Model[END_REF], it is possible to show that

ṡ = - 1 R I 3 [s] × v. (4.73)
Since (4.73) is linear in 1/R, we can define χ = 1/R, with then m = 3 and p = 1, and obtain for (3.10-3.12)

             f s (s, ω) = [s] × ω Ω (s, v) = -v T f χ (s, χ, u) = 0 d( x, t) = 0 . (4.74)
We note that in this case it is always possible to obtain global convergence for the estimation error since χ = 0 and therefore d( x, t) = 0 by construction (see Remark 3.1). Furthermore, matrix ΩΩ T reduces again to its single eigenvalue σ 2 1 = v 2 and, if σ 2 1 (t) ≡ const > 0, the 'ideal' estimation error dynamics (4.15) can be exactly obtained. One also has Ω = Ω(v) and J v,1 = 2v T .

We finally note the following facts: first of all, contrarily to the previous cases, here ṡ has no effect on the regulation of σ 2 1 which is only function of the camera linear velocity v. As usual, it is of course still possible to freely exploit the camera angular velocity ω for, e.g., keeping the sphere at the center of the image by regulating (s x , s y ) to zero. Second, we note the strong similarities with the previous optimal results obtained for a point feature under a spherical projection model (σ 2 max in (4.37)): in both cases the maximum estimation convergence rate for a given v does not depend on the position of the observed object on the image plane. Differently from the case of the point, however, one now has σ 2 1 = v 2 regardless of the direction of v: due to the spherical symmetry of the observed object any direction of motion is equally informative. Moreover, since the sphere has a finite non-zero dimension, moving in its direction causes a variation of the measurement s (the sphere will appear bigger or smaller in the image) that makes this motion informative for the estimation task, differently from the point feature case.

Active Structure from Motion for a cylinder

We now finally consider the case of SfM for a 3-D cylindrical object. A cylinder O C can be described by its radius R > 0 and by its main axis a ∈ S 2 passing through a 3-D point p 0 = (X 0 , Y 0 , Z 0 ), with a = 1 and, w.l.o.g., a T p 0 = 0 (p 0 can be chosen as the closest point on a to the origin of the camera frame [START_REF] Chaumette | Structure from controlled motion[END_REF]). Moreover, analogously to the sphere, a cylinder is also associated with a planar limb surface L such that (4.69) holds for any point on L with projection π = (x, y, 1). Therefore, as for the case of a sphere, a first possibility is to estimate the three unknown parameters of the limb plane L (with χ = ν) by exploiting (at least) three image measurements, see [START_REF] Chaumette | Structure from controlled motion[END_REF] and Appendix A. [START_REF] Spica | Plane Estimation by Active Vision from Point Features and Image Moments[END_REF] for some details in this sense. However, following the previous developments, we now propose a novel representation of the cylinder projection on the image plane which, again, allows to obtain the cylinder parameters (p 0 , a, R) in terms of image measurements and of the unknown but constant cylinder radius R which, therefore, represents the only quantity to be estimated.

Let (ρ 1 , θ 1 ) and (ρ 2 , θ 2 ) be the (measured ) distance/angle parameters of the two straight lines resulting from the projection of the cylinder on the image plane, and

n 1 = (cos θ 1 , sin θ 1 , -ρ 1 ), n 2 = (cos θ 2 , sin θ 2 , -ρ 2 ) (4.75)
be the normal vectors to the two planes passing through the origin of the camera frame and the two above-mentioned projected lines4 . Figure 4.5 gives a graphical representation of the quantities of interest. Note that vectors n 1 and n 2 can be directly evaluated from image measurements (the line parameters). We then define vector s ∈ R 3 as

s = ∆ ∆ 2 (4.76) with ∆ = 1 2 n 1 n 1 + n 2 n 2 . (4.77)
Vector s is, thus, also directly obtainable in terms of image quantities.

We now note that, from [START_REF] Chaumette | Visual servoing using image features defined upon geometrical primitives[END_REF], an equivalent expression for vectors n 1 , n 2 in terms of the cylinder 3-D geometry can be obtained as

n 1 = 1 N 1        R X 0 √ K -a R Y 0 √ K -b R Z 0 √ K -c        , n 2 = 1 N 2       R X 0 √ K + a R Y 0 √ K + b R Z 0 √ K + c       (4.78) with                          K = p T 0 p 0 -R 2 (a, b, c) = [p 0 ] × a N 1 = R X 0 √ K -a 2 + R Y 0 √ K -b 2 N 2 = R X 0 √ K + a 2 + R Y 0 √ K -b 2 , (4.79) thus yielding                      n 1 n 1 = 1 p T 0 p 0    RX 0 -a √ K RY 0 -b √ K RZ 0 -c √ K    n 2 n 2 = 1 p T 0 p 0    RX 0 + a √ K RY 0 + b √ K RZ 0 + c √ K    . ( 4
s = ∆ ∆ 2 = p 0 R . ( 4

.81)

As for the cylinder axis a, exploiting (4.78) one has

[n 2 ] × n 1 = 2R N 1 N 2 √ K    Z 0 b -Y 0 c X 0 c -Z 0 a Y 0 a -X 0 b    = 2R N 1 N 2 √ K    a b c    × p 0 = 2R N 1 N 2 √ K [p 0 ] × n × p 0 = 2Rp T 0 p 0 N 1 N 2 √ K a (4.82)
where in the last step the property a T p 0 = 0 was used. Since a = 1, from (4.82) it is

a = [n 2 ] × n 1 [n 2 ] × n 1 . (4.83)
The cylinder axis a can then be directly obtained in terms of only measured quantities.

We now note that, as in the sphere case, the only unknown left is the cylinder radius R: once known, the cylinder 3-D structure can be fully recovered from image measurements as p 0 = Rs from (4.81) and a from (4.83). An estimation scheme for R can be obtained exploiting the following differential relationship whose derivation is given in Appendix A.5

ṡ = - 1 R I 3 -aa T [s] × u. (4.84)
Note the similarity of (4.84) with (4.73) for the sphere case.

Being (4.84) linear in 1/R, one can then apply observer (3.11) by choosing s = s, ς = a, χ = 1/R with m = 3, r = 3 and p = 1, and obtaining

             f s (s, ω) = [s] × Ω Ω (s, ς, v) = -v T I 3 -ςς T f χ (s, χ, u) = 0 d( x, t) = 0 . ( 4.85)
Note how, again, being χ = 0 it is d( x, t) = 0 (global convergence for the error system (3.12) as in the sphere case).

Matrix ΩΩ T reduces to its single eigenvalue

σ 2 1 = ΩΩ T = v 2 -(ς T v) 2 . (4.86)
It is worth comparing (4.86) with the result obtained for the sphere (σ 2 1 = v 2 ). In the cylinder case, the convergence rate of the estimation error is affected by both the norm and the direction of the linear velocity v. In particular, for a given v = const, the maximum value for σ 2 1 is obtained when v has a null component along the cylinder axis a (a T v = 0) with, in this case, σ 2 1 = σ 2 max = v 2 . Intuitively, any camera motion along the cylinder axis does not provide any useful information to the estimation task. Furthermore, as in all previous cases with p = 1, keeping a σ 2 1 (t) = const allows to exactly enforce the ideal estimation error dynamics (4.15), see Remark 4.1.

Finally, from (4.86) one has

(σ 2 1 ) = J v,1 v + J a,1 ȧ = J v,1 v + J a,1 [a] × Ω (4.87)
with J v,1 = 2v T I 3 -aa T and J a,1 = 2v T av T . Although (4.87) also depends on the angular velocity Ω, it is possible to fully compensate for the effects of J a,1 [a] × Ω (a known quantity) when inverting (4.87) w.r.t. v as discussed in Sect. 5.4. Therefore, one can act on v to regulate the value of σ 2 1 (t) and, at the same time and in a decoupled way, exploit the camera angular velocity Ω for implementing additional tasks of interest such as keeping the cylinder axis a at the center of the image plane by enforcing (s x , s y ) = ∅ 2 .

Conclusions

In this chapter we addressed the problem of active SfM for recovering the 3-D structure of some basic but common geometric primitives: a point feature, a planar object, and a spherical and a cylindrical targets. We proposed a novel active estimation strategy tailored to the four cases under consideration. Using a nonlinear observer we showed how one can impose to the estimation error a transient evolution that is (almost) equivalent to that of a second order linear system. More importantly one can act online on the camera linear velocity to maximize the excitation of the system and thus reduce the estimation error convergence time given some constraints on the maximum allowed camera velocity.

For the depth estimation of a point feature, two possibilities differing in the adopted projection model (planar or spherical) were proposed and critically compared highlighting the complementarity of the two models in terms of attainable convergence rates and basin of attraction for the estimation error.

For the planar case, we first applied a simple standard strategy to extract a best fitting plane, in a least-squares sense, from a (actively) estimated point cloud. Then, we showed how to conveniently exploit image moments (in both their discrete and dense definitions) to estimate directly the parameters of the plane. For this second solution, we also explained how the standard definition of image moments can be extended to(i ) cope with the possible loss/addition of point features due to a limited camera FOV and, (ii ) automate the selection of which image moments order to use for the estimation to further improve the system observability.

Finally, in the spherical and cylindrical cases, we showed how an adequate choice of the measured visual features allows to reduce the SfM task to the estimation of a single unknown constant quantity (the sphere/cylinder radius R) in place of the classical (and time-varying) three parameters (scaled normal vector of the planar limb surface). Availability of this quantity allows to then retrieve the full 3-D structure of the observed targets.

In Chapt. 5 some simulative and experimental results will be reported to fully Chapter 5

Experiments and simulations of active structure from motion I n this chapter we show some experimental results meant to validate the theoretical developments of Chapt. 4. For all of the case studies presented there (point features, planar objects and spherical and cylindrical targets) we show here the advantages of adopting an active strategy for selecting on line the best camera linear velocity direction (the velocity norm is kept constant) for the sake of maximizing the convergence rate of the SfM estimator.

The experiments were run by employing a Point Grey Dragonfly2 greyscale camera (Fig. 5.1(a)). This has a resolution of 640 × 480 px and a framerate of 30 fps. The open-source ViSP library [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF] was used to accurately calibrate the intrinsic parameters of the camera before running the experiments. Also the image processing and feature tracking were implemented using ViSP as it will be detailed in the next sections.

The camera was mounted on the end-effector of a 6-DOFs Gantry robot, a picture of which is shown in Fig. 5.1(b). This robot has 3 linear orthogonal axes and a spherical wrist. This configuration is particularly convenient for our goals as it minimizes the number of kinematic singularities. The employment of the presented techniques on other robotic platforms such as, e.g., anthropomorphic serial manipulators, is certainly possible but might require to explicitly deal with the presence of robot kinematic singularities, a topic that is well assessed in the literature, but out of the scope of this thesis. The robot geometric Jacobian can be expressed as:

J E =           
0 0 s 5 -γ 56 1           
with c i = cos (q i ), s i = sin (q i ), c 56 = cos (q 6 -γ 56 q 5 ), s 56 = sin (q 6 -γ 56 q 5 ), l 56 = 0.06924 m, and γ 56 = 0.009091.

Since the robot only accepts velocity commands, the acceleration signal generated by the proposed control strategies was numerically integrated before being sent to the robot as a joint velocity command. The SfM observers and the controllers (and their internal states) were updated at 1 kHz, while the commands were sent to the robot at 100 Hz. The estimation and control algorithms were implemented in Matlab/Simulink where as the communication with the robot is ensured by ViSP in a compiled C++ executable. The communication between the two software components is ensured by the Robot Operating System (ROS) with the framework described in [10].

Videos representing some of the experiments and simulations shown here can be downloaded from the pages associated with the publications [1, 2, 3, 4, 5, 6] at http://ieeexplore.ieee.org. The videos are also available at the following links:

• for the point feature and for spherical and cylindrical targets: https://www. youtube.com/watch?v=i-9xxNNV82Q;

• for the planar case using point features and discrete image moments: https: //www.youtube.com/watch?v=QNXrkZj4NU0;

• for the planar case using adaptive discrete image moments: https://www. youtube.com/watch?v=5PtrbXuhtd0.

As for the spherical projection model, Fig. 5.2(d) reports the behavior of the estimation error χ(t) for the three cases under consideration, together with the ideal response (4.15). Here, the symbol χ s (t) is used to denote the estimation error in the spherical projection case in order to distinguish it from the error obtained with the planar projection model. All the plots result perfectly superimposed as expected from the analysis of Sect. 4.5.1.2. Indeed, in the spherical projection case, σ 2 max = v 2 regardless of the location of η and d(t) ≡ 0. However, absence of perturbation terms is obtained at the expense of the convergence rate of χ s (t), which indeed results slower or equal to that of χ(t) in the planar projection case. This is shown in Fig. 5.2(e) where the behavior of χ(t) -χ s (t) is reported for the three cases. We can then note how χ(t) -χ s (t) = 0 only in case I, as the planar and spherical models coincide when the feature point is at the center of the image plane.

These results then fully confirm the validity of the theoretical analysis reported in Sects. 4.5.1.1 and 4.5.1.2. However, we also note the marginal effects of the two projection models on the estimation performance when applied to an image plane of size comparable to that of the real camera used in our experimental setup. Therefore, in the following experimental results we will only consider the case of planar projection model.

time [s]

Depth estimation for a point feature

We here report some experimental results for the depth estimation of a point feature under a planar projection model (Sect. 4.5.1.1). The following experiments are meant to demonstrate how the proposed active estimation framework can be exploited to select online the 'best' camera motion. As visual target, we made use of a circular white dot of 5 mm radius painted on a planar black surface and sufficiently far from the camera in order to safely consider it as a 'point feature' (see Fig. 5.3(a)).

Comparison between the nonlinear observer and the EKF

In this section we propose a basic comparison between the nonlinear observer scheme introduced in Sect. 3.2.3, and used in the rest of this thesis, and the probabilistic EKF described in Sect. 3.3.3. Figure 5.5(a) shows the behavior of the estimation error when employing an EKF for estimating the (inverse) depth of the point feature using the same experimental data and camera trajectory as in the previous cases I and II. We notice that, also when using the EKF, the estimation performance still benefits from the use of an active strategy for choosing the best camera trajectory in order to improve observability. In fact, the times at which the estimation error χ(t) becomes smaller than 3 cm, are T 1 = 2.64 s in case I and T 2 = 2.84 s in case II (vertical dashed lines in Fig. 5.5(a)). Moreover, we obtained a standard deviation of approx. 3.7 and 6.4 mm evaluated on a time window of 1 s after the times T 1 and T 2 , respectively. These results are not surprising since, from the developments in Chapt. 3, observability is a property of the system itself and not of the estimation algorithm and therefore one should expect any estimation scheme (EKF included) to benefit from an optimization of the camera trajectory. This is further confirmed by the fact that the control law (5.1) does not depend on any estimated quantity, but only on the measurements. We also notice that, in both cases I and II, the EKF filter outperforms the nonlinear observer for the final covariance. This fact, however, is still expected from the theoretical analysis. Indeed, the use of the measured s (the (x, y) coordinates of the point feature) in place of the estimated s in the prediction step of the EKF transforms the nonlinear system dynamics in a linear time-varying one with an additional disturbing term due to the non perfect cancellation of the dynamics of χ, see also [START_REF] Guerreiro | Globally Asymptotically Stable Sensor-Based Simultaneous Localization and Mapping[END_REF]. Since the EKF is optimal for linear time-varying systems, it (correctly) outperforms the nonlinear observer, especially in case I where, thanks to the active camera velocity optimization, the depth of the point feature remains constant and the disturbing term disappears. On the other hand, the EKF has, in general, less predictable stability properties and convergence rate than the nonlinear filter. For instance, one cannot expect, for the EKF, to obtain a good match with the dynamics of a second order system as the one shown in Fig. 5.4(a) for the nonlinear observer. Moreover, the convergence time of the EKF also depends on the initialization of the state covariance matrix Σ 0 . Figure 5.5(b) shows, in fact, with a green line, the results obtained when the state covariance is initialized with a value 10 times smaller than in the previous case I. As one can see, with this initialization, the EKF convergences slower than the nonlinear observer. Other works, such as [START_REF] Grabe | Nonlinear ego-motion estimation from optical flow for online control of a quadrotor uav[END_REF], have also shown that nonlinear estimation techniques can, in some cases, perform better than the EKF. Because of all these considerations, we decided to use the nonlinear observer (3.11) for the rest of our experiments.

time [s]

Active structure estimation for a plane

This section reports some experimental results meant to illustrate and compare the various plane estimation methods introduced in Sect. 4.5.2. In the first set of experiments (Sects. 5.2.1 and 5.2.2) a simple dotted pattern was used for feature extraction and matching. This solution was meant to reduce as much as possible the variability between each experimental run by ensuring tracking of the very same set of points across all trials. In the last experiment (Sect. 5.2.3) a more realistic scene was considered with the Pyramidal Kanade Lucas Tomasi feature tracker (KLT) implemented in OpenCV used for tracking points on the surface of a (planar) topographic map (see Fig. 5.6).

The convergence rate of methods B and C was optimized by actively maximizing the minimum eigenvalue σ2 m in (4.43) for method B, and the smallest eigenvalue σ 2 1 of the 3 × 3 matrix ΩΩ T from (4.48) for method C. Exploiting (4.22), and recalling that the Jacobian J v can be computed in closed form in both cases, the update rule (5.1) was implemented also in this case with k 1 > 0 and k 2 ≥ 0. This allows, again, to optimize the direction of the camera velocity so as to maximize the SfM estimation convergence while keeping v(t) = const.

As for the angular velocity ω, it was exploited, in the experiments in Sects. 5.2.1 and 5.2.2, to keep the centroid of the tracked point features at the center of the image and, in the experiments in Sect. 5.2.3, to align the camera optical axis with the (estimated) plane normal n. Indeed, we remark again that matrix Ω(s, v) in (3.10) does not depend on ω and thus one can freely choose the camera angular velocity without affecting the estimation convergence.

Plane estimation from 3-D points (method B)

We report here the results in estimating the plane parameters (n, d) with method B: plane fitting from an estimated point cloud. The experiment started from an initial guess ( n(t 0 ), d(t 0 )) with an error of 40 deg w.r.t the true n(t 0 ) and a relative error of 50% w.r.t. the true d(t 0 ). The initial depths of all the tracked points π k were initialized so as to force p k (t 0 ) to belong to the estimated plane described by ( n(t 0 ), d(t 0 )). In order to demonstrate the importance of the active camera velocity optimization, we first ran a set of four experiments starting from the same initial conditions but using different initial camera velocities with the same norm v(t 0 ) = 0.0224m/s. In these experiments we used k 1 = 10 in (5.1) but we either substituted k 2 J T v with a random acceleration vector (purple dashed line) or we set k 2 = 0, thus keeping a v(t) = v(t 0 ) = const during motion (green, red and cyan dashed lines). Finally we started the experiment again from the same initial camera velocity as in the experiment that performed worst in the previous set (cyan line) and we adopted the update rule (5.1) with k 1 = 10 and k 2 = 1.

Finally, for the sake of allowing a fair comparison between the convergence rates of methods B and C, we first collected all the data during a first execution of all trajectories, and then ran the two estimation schemes offline on the collected dataset by properly adjusting the estimation gains α B and α C of both methods 2 . Indeed, let Fig. 5.7(a) shows the behavior of the norm of the estimation error χ between the real and estimated inverse feature depths, normalized w.r.t. its initial value. The normalization is meant to allow a comparison of this plot with the analogous one in Fig. 5.8(b). The angle between vectors n(t) and n(t) and the relative error ( d(t) -d(t))/d(t) are also shown in Fig. 5.7(b). We can then note how the plane estimation task is solved in all cases (the estimation errors converge towards zero) but, clearly, in the active case (blue line) the error convergence is significantly faster than in the other experiments. This is further evident from Fig. 5.7(c) where the value of the α B σ m (t) is shown for all experiments (same color code): thanks to the active optimization of the direction of v(t), during the active experiment, α B σ m (t) results approximately 12.5 times larger than in the worst experiment (cyan) which started from the same initial camera velocity. Finally, Figs. 5.7(e) and 5.7(f) depict the camera trajectory in all cases with arrows indicating the direction of the camera optical axis. The green patch represents the location of the plane to be estimated. We encourage the reader to also look at the attached video for better appreciating the effects of the active strategy on the camera trajectory.

σ 2 m = 1 T t 0 +T t 0 σ 2 m (τ )dτ and σ 2 1 = 1 T t 0 +T t 0 σ 2

Plane estimation from discrete image moments (method C)

In this second set of experiments we show the results of using the weighted discrete image moments for the estimation of the plane parameters. As before the initial guess for χ(t 0 ) has an error of approximately 40 • w.r.t. n(t 0 ) and a relative error of around 50% w.r.t. d(t 0 ). Again, we first ran a set of four experiments starting from the same initial conditions but using different initial camera velocities with the same norm v(t 0 ) = 0.0206m/s and using (5.1) with k 1 = 10 and either substituting k 2 J T v with a random acceleration vector (purple dashed line) or setting k 2 = 0, thus keeping a v(t) = v(t 0 ) = const during motion (green, red and cyan dashed lines). Finally, in the active case we started again from the initial camera velocity of the experiment that performed worst in the previous set (cyan line), and we adopted the update rule (5.1) with k 1 = 10 and k 2 = 1. In all cases, as explained in Sect. 5.2.1, we set α C = 20000.

We show again in

Comparison of the three methods A, B and C

This set of experiments is meant to provide a comparative analysis of the differences between methods B and C against the classical method A taken as a baseline condition. The 'convergence' of method A for the estimation of the plane normal direction is, in general, faster w.r.t. the other two methods due to its algebraic nature (no filtering process is present in this case). On the other hand the use of an estimation scheme in methods B and C allows for the possibility of tuning the estimation gain α (a free parameter) against the noise level present in the system (i.e., trading off convergence speed for noise robustness).

In order to test the three methods in a more challenging scenario, we added to the scene a small planar picture with a non-negligible inclination w.r.t. the main plane (see Fig. 5.9) so as to introduce the presence of some 'outliers' w.r.t. the main dominant plane 4 . The picture was located to be in visibility at the beginning of the experiment and to leave the camera FOV shortly after. The camera linear velocity was optimized via (5.1) by maximizing the smallest eigenvalue σ 2 1 of the matrix ΩΩ T for the image moment case, and then the same trajectory, depicted in Figs. [START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF].10(e) and 5.10(f), was used for the other two methods. This resulted in a non-optimal, but still observable, trajectory for method B.

As done in the previous experimental sections, for the sake of obtaining a fair comparison between the convergence rates of methods B and C, we adjusted the estimation gains of both methods in such a way that α B σ 2 m = α C σ 2 1 , where σ 2 m and σ 2 1 are the average values of the smallest eigenvalues for the two estimators along the (this time common) trajectory. This resulted in α B = 200 for method B and α C = 26179 for method C.

The behavior of the estimation error on the plane parameters is depicted in Figs. [START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF].10(a) and 5.10(b) for method A (green lines), method B (blue lines) and method C (red lines). In Fig. 5.10(c) the products α B σ 2 m (t) and α C σ 2 1 (t) are plotted. It can be noticed that in all three cases at the beginning of the experiment (i.e. when the 'outlier' effect of the inclined image over the main planar scene is more present) the error in the estimation of the normal is significant although not diverging. All methods estimate a plane with an intermediate normal direction (as one would expect). Subsequently, the estimation errors for method C and method B start converging toward zero at t ≈ 8 s (first dashed vertical line), that is, when the outlier image starts leaving the image plane. However, note how the homography method still yields a very noisy estimation during this phase. Furthermore, once all the outliers are lost (t ≈ 20.3 s and second vertical dashed lines in the plots) all the methods yield a converging estimation error. However we can still notice two facts:(i ) method C results in the fastest convergence. This is also because the weight w of the outliers starts approaching 0 as they get close to the image border (and thus their disturbing effect is more quickly discarded); (ii ) method A has a faster convergence rate w.r.t. method B once all the outliers are lost, but it also yields a noisier estimation until the end of the experiment. In particular one can notice the presence of considerable "jumps" in the estimation of method A due to the reinitialization performed each time the number of matched features falls below a given threshold.

time [s]

In order to demonstrate the effectiveness of the adopted weighting functions in the computation of the discrete moments, the behavior of m 00 (t) is shown in Fig. 5.10(d). This is meant to illustrate how the number of active points changes over time due to losses at the image border or detection of new features. The presence of the weighting strategy function guarantees the desired continuity of m 00 (t) (and similarly of all other image moments not plotted here). x direction and then along the y direction) and its total weight goes to zero by the action of both w 1 (x) and w 2 (y). After the feature has completely left the plane, the tracker detects a new feature (the blue one) at t ≈ 12 s. Being far from the image border, it is smoothly taken into account thanks to the effect of weight w 3 (t) (note the zoomed views on the right side of the plots where the smooth rise of w 3 (t) can be seen). Finally, the green feature is close to the border of the image at the time of detection. In this case, even if weight w 3 (t) is rising towards 1, the total wight of the feature is kept small by the action of w 2 (y).

Simulation results for the use of adaptive moments

In this section we report some numerical simulations concerning the use of the adactive strategy described in Sect. 4.5.2.3 to select online the best discrete image moments to use for estimating the structure of a plane. All the following simulations consider a free-flying camera observing a planar scene P consisting of N = 30 points, and with plane parameters n = (0, 0, -1) and d = 1.5 m in 0 F C (the initial camera frame at t = t 0 ). The initial estimations of the plane normal and distance are always taken as n(t 0 ) = (-0.87, 0, -0. initial incertitude of ≈ 60 deg on the real normal direction and of 0.5 m on the real distance to the plane. Finally, the point features p k , k = 1 . . . N , are sampled at 60 Hz and then corrupted component-wise by a uniformly distributed random noise of magnitude 2 pixels before being processed for evaluating the image moments.

The camera motion (and the optimization (4.64)) is instead updated at 100 Hz.

Unconstrained polynomial basis

We start with the results obtained by making use of the unconstrained polynomial basis (4.55) of fixed degree δ introduced in Sect. 4.5.2.3. We tested our method by considering a set of m = 3 weighted moments s = (m w (θ 1 ), m w (θ 2 ), m w (θ 3 )) ∈ R 3 with degree δ = 2 defined as in (4.56), with then θ i ∈ R 5 , i = 1 . . . 3, and θ = (θ 1 , θ 2 , θ 3 ) ∈ R h , h = 15. This choice was meant to provide a direct comparison against the use of:

1. the more 'classical set' (x g , y g , µ 20 + µ 02 ) that, as explained, is known to be an optimal choice for controlling the camera translational motion but also to yield poor results when employed for SfM purposes;

2. the set of five moments (x g , y g , µ 20 , µ 11 , µ 02 ) which, as reported in Sect. 5.2.2, does allow for a converging estimation but at cost of an increased complexity (need of propagating five image moments).

The goal of the comparison is to prove that estimation of vector χ is, instead, fully possible when a suitable combination of just three moments of order up to 2 is selected. . For a comparison, in all plots dashed lines correspond to the use of the five moments (x g , y g , µ 20 , µ 11 , µ 02 ): it is worth noting how, despite the increased measurement set (five moments vs. only three), the estimation convergence results still slower than in the weighted moment case.

time [s]

Figures 5.12(a) and 5.12(b) start showing the results obtained by employing the set (x g , y g , µ 20 + µ 02 ) for estimating vector χ while, at the same time, optimizing the camera linear velocity v by implementing the first row of (4.64) with k v = 1 (similarly to previous experiments). The linear velocity was initially set to v(t 0 ) = [0 0.1 0] T m/s with then v(t) = v(t 0 ) = 0.1 m/s during the camera motion. As expected, and even despite the velocity optimization, the value of Φ D (t) keeps (numerically) very close to 0 with a maximum of ≈ 1.2•10 -9 (Fig. 5.12(a)). Thus, the chosen set (x g , y g , µ 20 + µ 02 ) is not able to provide enough information for allowing convergence of the SfM scheme, and indeed the estimation error χ(t) = χ(t) -χ(t) even starts diverging (Fig. 5.12(b)).

On the other hand, exploiting the three weighted moments of degree 2 yields a much more satisfactory estimation performance: Fig. 5.13 reports in solid lines the results obtained by implementing (4.64) with k v = 1 and k θ = 3, and by taking again v(t 0 ) = [0 0.1 0] T m as in the previous case. The parameter vector θ was instead chosen at random under the constraint θ i (t 0 ) = 1, i = 1 . . . 3.

Looking at Fig. 5.13(a) one can then verify how now Φ D (t) attains an overall quite larger value compared to Fig. 5.12(a), with a maximum of ≈ 1.8 • 10 -2 (thus, more than 10 7 times larger than in the previous case). As a result, the estimation error χ(t) is able to quickly converge towards ∅ 3 in about 4 s (Fig. 5.13(b)). For a better appreciation of the estimation performance, Figs. 5.13(c) and 5.13(d) also report the behavior of d(t) = d(t) -d(t) (the error in estimating the plane distance d) and n = arccos(n T (t) n(t)) (the angular error in estimating the direction of the plane normal n) with d = 1/ χ and n = -χ/ χ . Finally, Fig. 5.13 superimposes in dashed lines the behavior of Φ D (t) and of the estimation errors when instead relying on the set of m = 5 moments (x g , y g , µ 20 , µ 11 , µ 02 ) for estimating χ: in this case, the estimation error does actually converge (as expected from the results in Sect. 5.2.2), but nevertheless at a slower rate compared to the weighted moment case (indeed, the maximum value of Φ D (t) is now 'only' ≈ 5.9 • 10 -4 ). We then believe these results clearly show the advantages of the proposed approach: the SfM scheme has its best performance when relying on the optimization (4.64) for automatically selecting (online) the best combination of three moments of order up to 2.

As an additional evaluation, Fig. 5.14 shows the results obtained when only optimizing the parameter vector θ while keeping a constant linear velocity v(t) = v(t 0 ) during the whole motion (thus, by setting k v = 0 in (4.64)). This case is meant to assess the optimization performance in a situation in which the camera velocity cannot be arbitrarily adjusted but it must be considered as 'given' by an external source. Thus, the only possibility for improving the conditioning of the observability matrix ΩΩ T is to act on vector θ, i.e., on the moment shape. Nevertheless also in this situation Φ D (t) still reaches a range of values comparable with the previous case, with indeed max Φ D (t) ≈ 1.05 • 10 -2 against the previous 1.6 • 10 -2 (thus, still ≈ 10 7 times larger than when employing the classical set (x g , y g , µ 20 + µ 02 )). As a result, vector χ(t) keeps converging to ∅ 3 in about 4 s (Fig. 5.14(b)) even if slightly more slowly w.r.t. the previous case of Fig. 5.13(b) (as one could expect because of the smaller value of Φ D (t)). In any case, we believe it is worth noting how the sole optimization of the moment shape (via vector θ) is still able to yield a very satisfactory SfM performance even for a non-optimal camera motion.

We finally remark that in all simulations the camera angular velocity ω was exploited for keeping the centroid of the observed point features π i at the center of the image plane (we recall that matrix Ω and, thus, Φ D (t) do not depend on ω time [s] that can then be freely chosen without affecting the estimation performance).

Constrained polynomial basis

We now address the case of the constrained polynomial basis (4.57-4.58)) described in Sect. 4.5.2.3 and meant to smoothly take into account the loss/gain of point features because of the camera limited FOV. We consider again a set of m = 3 weighted moments m = (m w (θ 1 ), m w (θ 2 ), m w (θ 3 )) ∈ R 3 with both functions w x (•) and w y (•) taken as the fifth-order polynomials given in (4.59) with, therefore, a total of h x + h y -8 = 4 parameters to be optimized. The initial camera velocity v(t 0 ) was set as in the previous cases, and the optimization action (4.64) was again implemented with k v = 1 and k θ = 3. The camera angular velocity ω was instead kept null for facilitating the loss or point features during motion.

Figure 5.15(a) shows the camera trajectory during the estimation task, and In dashed lines, the behavior that all quantities would have had in case no optimization of θ had been performed.

X w [m] -1 0 1 1 Y w [m] 0 -1 1.5 0 0.5 1 Z w [m]
1 y 0 -1 1 0 x 0.2 0 -0.2 -1 w(x; y; 3 1 ) (a) 1 y 0 -1 1 0 x 0 -0.2 0.2 -1 w(x; y; 3 1 ) (b) 1 y 0 -1 1 0 x 0.2 0 -0.2 -1 w(x; y; 3 1 ) (c) 1 y 0 -1 1 0 x 0 -0.2 0.2 -1 w(x; y; 3 1 ) (d)
Figure 5.16 -Four snapshots of the weighting function w(x, y, θ 1 ) taken during the camera motion. It is interesting to visualize how function w(•) automatically adjusts its shape as a function of the observed scene (e.g., it tends to peak around clusters of points). Fig. 5.15(b) reports the number N p (t) of tracked features over time: after about 5 s some points start being lost, dropping from a total of 30 to a minimum of 6 at t = 10 s. Nevertheless, thanks to the adopted constrained weighted moments, the scene structure is still correctly estimated without suffering from discontinuities or numerical instabilities because of the lost features. Figure 5.15(c) reports again the behavior of Φ D (t) (blue solid line) that reaches a maximum of ≈ 2.9 • 10 -4 before starting to decrease at t ≈ 5 s because of the fewer tracked points. As a comparison, Fig. 5.15(c) also reports the superimposed behavior of Φ D (t) in case no optimization of vector θ had been performed (the almost horizontal red dashed line). In this case, the maximum attained value for Φ D (t) would have been ≈ 1.2 • 10 -6 (100 times smaller), thus proving again the importance of properly optimizing the shape of the chosen weighting function w(•). Figures 5.15(d) to 5.15(f) then show (in solid lines) the behavior of the estimation error χ(t) and of the corresponding quantities d(t) and n(t) that smoothly reach convergence in about 10 s of motion despite the loss of point features. Again, for a comparison, Figs. [START_REF] Giordano | Learning the Shape of Image Moments for Optimal 3D Structure Estimation[END_REF].15(d) to 5.15(f) also report the superimposed behavior (in dashed lines) of the estimation errors in case of no optimization of vector θ (all quantities have a slower convergence rate as expected).

Finally, Fig. 5.16 depicts four snapshots of the shape of function w(x, y, θ 1 ) used to compute the first constrained weighted moment. One can then appreciate how the function shape evolves over time and, in particular, automatically polarizes its peaks around the location of the tracked point features.

Simulation results of plane estimation from dense image moments

We conclude the analysis of the planar case by showing some preliminary simulative results of the active estimation framework applied to the case of dense image moments discussed in Sect. 4.5.2.4. As explained, in this case the problem is 'square': 3 available measurements (area and barycenter coordinates) for 3 unknowns (vector ν). Let J T v,1 be the Jacobian associated with the first (smallest) eigenvalue σ 2 1 of matrix ΩΩ T from (4.67), the control law (5.1) can be used also in this case to maximize σ 2 1 while keeping a constant norm of v. Similarly to what done for the point feature case, two simulations were run in which v(t) = v 0 but with its direction being either case I: optimized to maximize the estimation convergence rate (red line) using (5.1) or case II: kept constant so that v(t) = v 0 = const (blue line).

In particular, we used: α = 5 • 10 4 , s(t 0 ) = ∅ 3 , χ(t 0 ) = [-2.16 4.5 1.27] T , v(t 0 ) = [0.02 -0.05 -0.005] T , k 1 = 10, k 2 = 100 in case I and k 2 = 0 in case II, and

κ d = 1 2 v T 0 v 0 .
The moments were generated from a planar circle of radius R = 0.2 (see Fig. 5.17(c)).

For this simulation, we also exploited the camera angular velocity ω in order to keep the observed barycenter (x g , y g ) stationary during motion. As explained at the end of Sect. 4.4, this was meant to (partially) mitigate the effects of a non-zero ẏ when inverting (4.22), with y = [a x g y g n 20 n 11 n 02 ] for the moment case. Figure 5.17(b) depicts the behavior of the three eigenvalues σ 2 1 (t), σ 2 2 (t), and σ 2 3 (t) over time for case I (red lines) and case II (blue lines). We can then note how, while the smallest eigenvalue σ 2 1 (t) is correctly maximized, in case I, by the effect of the control law (5.1), the other two eigenvalues (σ 2 2 (t), σ 2 3 (t)) (which are not being controlled) are larger in case II. The consequence of this is that, at the beginning, the error convergence rate in case II is faster than in case I, see Fig. 5.17(a). Nevertheless, once the fastest modes have converged, the slowest one, associated with σ 2 1 (t), dominates the error behavior and, as a consequence, having maximized σ 2 1 (t) results in an overall faster convergence in case I. This then confirms the validity of our analysis also in the case of dense image moments. Finally, Fig. 5.17(c) shows the 3-D trajectory followed by the camera for case I (red line) and case II (blue line): one can note how(i ) the camera keeps looking at the barycenter as expected, and how (ii ) in case I the camera velocity has a larger component along the Z-axis that results in an increase of σ 2 1 (t).

Y w [m] 0 -0.2 -0.4 -0.6 0.2 0 -0.2 X w [m] -0.4 0.6 0.4 0 0.2 Z w [m] case I case II (c)

Active structure estimation for a sphere

We now discuss some experimental results concerning the estimation of the radius of a spherical target: indeed, as explained in Sect. 4.5.3, estimation of R allows to fully recover the sphere 3-D position p 0 = sR where vector s is directly obtainable from image measurements, see (4.70).

As object to be tracked, we made use of a white table tennis ball placed on a black table and with a radius of 1.9 cm (see Fig. 5.3(b)). As explained in Sect. 4.5.3, the convergence rate of the estimation error for the sphere case only depends on the norm of the linear velocity v and not on its direction. This fact is proven by the first experiment where the estimation task is run twice starting from two different positions and imposing two different camera velocities but with same norm. These values were used during the experiments: [START_REF] Spica | A Framework for Coupling Visual Control and Active Structure from Motion[END_REF], and v = (-0.05, 0, 0) m/s for case I and v = (0, 0.045, 0.02) m/s for case II, with v = 0.05 m/s in both cases. The camera angular velocity ω was exploited to keep (s x , s y ) (0, 0) (centered sphere). Figure 5.18(a) shows the behavior of the estimation errors (solid blue and red lines): note how the error transient response for the two cases is essentially coincident, and also equivalent to that of the reference second order system (4.15) with the desired poles, i.e., by setting σ 2 1 = v 2 = const and c 1 = c * 1 in (4.15) (dashed black line). The higher noise level in case II (red line) is due to the larger distance between the camera and the spherical target (see Fig. 5.18(b)) which negatively affects the estimation task. The standard deviation of the radius estimation error, computed on a time window of 1 s after χ(t) has become smaller than 1 mm (vertical dashed lines in the plot), is 0.3 mm for case I and 0.2 mm for case II: we can note, again, the very satisfactory results obtained with the proposed estimation scheme in terms of accuracy of the reconstructed sphere radius. Note also how, in the two cases, the estimation error χ(t) drops below the threshold of 1 mm at essentially the same time, as expected (same error transient response).

α = 2 • 10 3 , c 1 = c * 1 = 2 √ ασ for D 1 in (4.
Since the direction of the velocity does not play any role in this case, no optimization of σ 2 1 can be performed under the constraint v = const. On the other hand, the analysis of Sect. 4.3 clearly indicates the importance of choosing a proper value of c 1 for the damping matrix D 1 in (4.8). To show this fact, we report here three experiments characterized by the same camera trajectory of the previous case I, but by employing three different values for c 1 , that is, c * 1 , 2c * 1 and 0.5c * 1 . These correspond to a critically damped, overdamped and underdamped response for the ideal system (4.15), respectively. The experimental results reported in Fig. 5.19 show that the behavior of the estimation error χ (solid lines) has an excellent match with that of (4.15) (represented by dashed lines), thus fully confirming(i ) the validity of the proposed theoretical analysis, and (ii ) the importance of choosing the 'right' damping matrix D 1 for optimizing the convergence speed in addition to a proper regulation of σ 2 1 . (red line). The dashed lines represent the response of an 'ideal' second order system with the corresponding poles. Note again the almost perfect match between the plots. is meant to show how the convergence properties of the estimator are not affected by the direction of the camera linear velocity as long as it stays orthogonal to the cylinder axis a.

time [s]

The experiments were run with the following conditions: α = 500, c 1 = c * 1 for D 1 in (4.8), k 1 = 10, k 2 = 1 for cases I and III, and k 2 = 0 for case II. As for the linear velocity, we set v(t 0 ) = v 0 = (-0.01, 0.05, 0.05) m/s for cases I and II, and v(t 0 ) = v 0 = (-0.05, 0.05, 0.01) m/s for case III (note how v 0 2 = 5.1 × 10 -3 m 2 /s 2 in all three cases).

The behavior of σ 2 1 (t) is shown in Fig. 5.20(a): as explained at the end of Sect. 4.5.4, under the constraint v = const, one has max v σ 2 1 = v 2 as the largest possible value for σ 2 1 (obtained when v T a = 0). It is then possible to verify that, indeed, σ 2 1 (t) → v 0 2 in cases I and III despite the different initial conditions of the experiments (different camera pose and direction of v). The optimization in (5.3) results in a null component of v along a, thus allowing to move faster in the 'useful' directions (while keeping a constant v ), and to increase the value of σ 2 1 to its maximum possible value. Also, note how the value of σ 2 1 (t) for case II results smaller than in the other two cases (as expected) since no optimization is present in this case.

The behavior of the estimation error χ(t) is shown in Fig. 5.20(b): again, we can note that the transient response for cases I and III results essentially coincident and in almost perfect agreement with that of the reference system (4.15) with desired poles (dashed black line). As expected, the response for case II (red line) is slower than in cases I and III. As in the point feature case, Fig. 5.20(b) reports, for the three cases under consideration, the times T 1 = 2.74 s, T 2 = 4.78 s and T 3 = 2.66 s at which the estimation error drops below the threshold 2 mm (vertical dashed lines). The standard deviation of the error evaluated on a time window of 1 s after convergence has been 'reached' resulted in the values of 0.4, 0.6 and 0.7 mm, respectively. We can then appreciate, again, the high accuracy of the proposed approach in estimating the cylinder radius R while also optimizing online for the camera motion. The higher estimation error in case III can be ascribed to the larger distance between the camera and the observed target, which increases the effect of discretization errors. Note also how T 1 ≈ T 3 < T 2 thanks to the active optimization of the error convergence rate. Finally, Fig. 5.20(c) depicts the camera trajectories for the three experiments with arrows indicating the direction of the optical axis. In case II the camera simply travels along a straight line (v(t) ≡ v 0 ), while in cases I and III the direction of v is suitably modified resulting in a trajectory lying on a plane orthogonal to a.

Conclusions

In this chapter we reported a large collection of simulation and experimental trials meant to validate the theoretical results of Chapt. 4 in the context of active structure estimation from motion.

All of the geometric primitives considered in Sect. 4.5 (i.e. point features, planes, spheres and cylinders) were re-discussed, in this chapter, from a numerical/experimental point of view. The experiments were run on a 6-DOFs manipulator equipped with a camera in-hand. We showed that, even in non ideal experimental conditions, the use of the nonlinear observer described in Sect. 3.2.3 allows to obtain a good match between the dynamics of the SfM estimation error and that of a linear second order system with assigned poles when the quantity to be estimated is a single scalar (i.e. for the point feature, sphere and cylinder cases) in agreement with Remark 4.1. More importantly, we experimentally demonstrated that the use of an active strategy for selecting the camera motion policy can, as predicted by the theoretical analysis, increase the convergence rate of the estimation and reduce the final estimation covariance for the same control effort (same camera linear velocity norm). This latter result is also true regardless of the chosen estimation scheme and, in particular, a brief comparison between the adopted nonlinear observer and a standard EKF scheme demonstrated that the camera trajectory optimization is also beneficial for this latter alternative estimation algorithm.

For the point feature case, we also compared, in simulation, the two different estimation schemes, proposed in Sect. 4.5.1.1, based on either a planar or a spherical projection model. We pointed out the differences between the two approaches in terms of robustness and estimation convergence rate. We also demonstrated however, that for our real camera parameters, the difference between the two projection models is negligible.

For the planar case, we experimentally compared the active plane estimation strategies introduced in Sects. 4.5.2.1 and 4.5.2.2 with a more standard homography based reconstruction technique showing the advantages of the former in terms of intrinsic robustness w.r.t. the presence of outliers. Moreover we presented some simulations in which the use of an adaptive strategy for selecting the image moments to use for plane estimation was investigated showing very promising results in terms of additional improvement of the estimation convergence rate. Finally, we reported some simulation results showing how the proposed machinery could be extended to the case of dense image moments.

For the sphere case, we showed that, due to the symmetry of the scene, the direction of the camera linear velocity does not affect the convergence rate of the estimation, however it is still possible to tune the observer gains online so as to fix a desired damping coefficient of the error dynamics.

Finally, for the cylinder case, we experimentally proved that, similarly to the sphere, as long as the camera does not translate along the direction of the cylinder axis, the direction of the camera velocity does not affect the estimation error converge rate.

The results reported in this chapter confirm, in our opinion, the theoretical analysis of Chapt. 4.

Chapter 6

Coupling active SfM and IBVS I n the first part of this thesis, we introduced the basic concepts behind computer vision, robot control and state estimation. We showed that a camera is a scale invariant sensor in the sense that the perspective projection that it operates, transforms the 3-D world in a 2-D image in which the information about the scale is lost. In general, if one wishes to use a camera to control the full pose of a robot, knowledge of this scale will become necessary for either reconstructing the complete camera state (in PBVS frameworks) or for correctly estimating the feature interaction matrix of a IBVS control scheme (see Sect. 2.1.3.3 and Sect. 2.3.2 and [START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF]). The scale can be estimated by taking multiple images from different camera positions and feeding them to, e.g., one of the estimation schemes discussed in Chapt. 3. Due to the nonlinearity of the problem, however, the estimation is only possible if the camera trajectory is sufficiently persistently exciting and thus enough information is collected. A IBVS controller, e.g., should then be able to realize the main visual task while, at the same time, ensuring a sufficient level of information gain for allowing an accurate state estimation.

In Part II we put the classical IBVS control problem to the side and we concentrated on the estimation problem. In particular we devised and experimentally validated an estimation/control strategy that gives full control over the eigenvalues of the estimation error dynamics (which is made equivalent to that of a second order linear system) by acting online on the estimation gains and, more importantly, on the camera velocity. While doing this, we considered the structure estimation as the main task that had to be accomplished by the robot and we did not constrain the camera velocity in any way, apart from requiring a constant norm.

In this chapter, and in the next one, we consider, instead, a more common situation in which the estimation is not directly the main task that the robot is required to accomplish but still some estimated quantities are used to compute the robot control law and thus the accuracy of the estimation affects the control performance. In particular, we investigate the possible coupling between the framework for active SfM introduced in Part II and the execution of a standard IBVS task. The main idea is to project any optimization of the camera motion (aimed at improving the SfM performance) within the null-space of the considered task in order to not degrade the servoing execution (see Sect. 2.2.4.3). For any reasonable IBVS application, however, a simple null-space projection of a camera trajectory optimization (as in (2.26)) turns out to be ineffective because of a structural lack of redundancy. Therefore, in order to gain the needed freedom for implementing the SfM optimization, we suitably exploit and extend the redundancy framework introduced in [MC10] which grants a large projection operator by considering the norm of the visual error as main task. In addition, an adaptive mechanism is also introduced with the aim of activating/deactivating online the camera trajectory optimization as a function of the accuracy of the estimated 3-D structure. Thanks to this addition, it is then possible to enable the SfM optimization only when strictly needed such as, e.g., when the 3-D estimation error grows larger than some desired minimum threshold.

As discussed in depth in Sect. 3.4, there exists a vast literature on the topic of trajectory optimization for improving the identification/estimation of some unknown parameters/states. Similarly, a large number of works has addressed the so-called Next Best View (NBV) problem, i.e., loosely speaking, how to actively control the motion of a vision sensor so as to reconstruct the 3-D shape of an object of interest or of the surrounding environment while, e.g., minimizing the estimation uncertainty, the traveled distance, the number of acquired images, or any other meaningful criterion. However, many of these strategies are meant for an offline use (a whole trajectory is planned, executed, and then possibly re-planned based on the obtained results), and, in any case, do not take into account the online realization of a visual task concurrently to the optimization of the estimation performance. At the other end of the spectrum, several works have already investigated how to plug the online estimation of the 3-D structure into a IBVS loop (i.e., considering the realization of a visual task), but without any concurrent optimization of the camera motion for improving the estimation performance, see, e.g., [MHMM09, FKS07, DOR08, PCCB10, Cor10, MS12]. In all of these works, the SfM scheme is just fed with the camera trajectory generated by the IBVS controller which, on the other hand, has no guarantee of generating a sufficient level of excitation w.r.t. the estimation task.

With respect to this previous literature, this chapter provides, instead, a unified and online solution to the problem of concurrently optimizing execution of a IBVS task (visual control) and performance of the 3-D structure estimation (active perception). We also wish to stress that the proposed machinery is not restricted to the sole class of IBVS problems considered in this work: indeed, one can easily gen-eralize the reported ideas to other servoing tasks, or apply them to other contexts not necessarily related to visual control (as long as the chosen robot trajectory has an effect on the state estimation performance).

The rest of the chapter is organized as follows: after a short introduction in Sect. 6.1, Sect. 6.2 details the machinery needed for coupling IBVS execution and optimization of the 3-D structure estimation, with a particular emphasis on the second-order extension of the strategy described in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] for increasing the redundancy w.r.t. the considered visual task. Subsequently, Sect. 6.3 introduces an extension of the strategy detailed in Sect. 6.2 for allowing a smooth activation/deactivation of the camera trajectory optimization as a function of the current estimation accuracy. Finally, Sect. 6.4 concludes the chapter with some final considerations. Some (quite) preliminary results in this context were presented in [START_REF] Spica | Coupling Visual Servoing with Active Structure from Motion[END_REF]. W.r.t. this previous work, we provide, in this chapter, a more complete analysis from both a theoretical and experimental point of view. Most of the material presented here can be found in [START_REF] Spica | Bridging Visual Control and Active Perception via a Large Projection Operator[END_REF] which, at the time of writing, is under consideration for publication. Part of these more recent results were also presented in [START_REF] Spica | A Framework for Coupling Visual Control and Active Structure from Motion[END_REF].

Problem description

Let us consider again the classical situation of a robot manipulator with joint configuration vector q ∈ R n carrying an eye-in-hand camera that measures a set of visual features s ∈ R m (e.g., the x and y coordinates of a point feature). We have already discussed in Sect. 2.3.2 some control strategies that allow to regulate s to a desired constant value s d . As already anticipated in Remark 2.1, whatever the particular case (redundant/non-redundant, feasible/non-feasible), any implementation of (2.32-2.33) (or variants) must also deal with the lack of a direct measurement of vector χ which prevents the exact on-line computation of J . A common workaround is to replace the exact task Jacobian J (s, χ, q) with an estimation J = J (s, χ, q) evaluated on some approximation χ of the unknown true vector χ, for instance the value at the desired pose χ = χ d = const [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF], assuming that this is known or easily measurable in advance by additional sensors. In this approximated case, assuming for simplicity qw ≡ ∅ m , the closed-loop dynamics, obtained by, plugging (2.32-2.33) into (2.31), becomes ė = -λJ J † e.

(6.1)

Local stability of (6.1) in a neighbourhood of e = ∅ m is then determined by the eigenvalues of matrix J (s d , χ d , q)J (s d , χ, q) † .

In case the approximation χ = χ d is used, then (6.1) will be locally asymptotically stable around e = ∅ m (with, however, a possibly small convergence domain).

Nonetheless, in this case, the ideal closed-loop behavior (2.20) will no longer be obtained (even in the feasible case ρ = m) because of the approximation in evaluating J away from the desired pose. In addition to this shortcoming, different choices of χ (including rough estimations of the true χ d at the final pose) may also move (a subset of) the eigenvalues of (6.1) to the right-half complex plane, and thus yield an unstable closed-loop system (and failure of the servoing) even when starting arbitrarily close to the final pose. An illustrative example of such instability is demonstrated in Sect. 7.1.3, while further discussions about the robustness of visual servoing schemes against uncertainties on χ can be found in [START_REF] Malis | Robustness of Image-Based Visual Servoing With a Calibrated Camera in the Presence of Uncertainties in the Three-Dimensional Structure[END_REF].

Special approximations such as χ = χ d can then, at best, only guarantee local stability in a neighbourhood of s d and, in any case, require some prior knowledge on the scene (the value of χ d must be obtained independently from the execution of the servoing task). Additionally, too rough estimations of the final χ d (or other approximation choices for χ) may result in a poor, or even unstable, closed-loop behavior for the servoing causing, e.g., loss of feature tracking. In this context, the use of an incremental filtering (SfM) scheme able to generate a converging χ(t) → χ(t) from (ideally) any initial approximation χ(t 0 ) can represent an effective alternative. Indeed, a SfM scheme can improve the servoing execution by approximating the ideal closed-loop behavior (2.20) also when far from the desired pose and without needing special assumptions/approximations of χ, since as χ(t) → χ(t) one obviously has J → J (see, e.g., [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF]).

We now note that, in this conceptual framework, the estimation of the 3-D structure χ can then take place only during the transient phase of the servoing task, i.e., while the camera is in motion towards its goal location. Being this phase of limited duration, with the camera reaching a full stop at the end of the servoing, one should clearly aim at imposing the fastest possible convergence to the estimation error for a given camera displacement (from initial to final pose). As demonstrated in the previous chapters, other factors (e.g., estimation gains) being equal, the convergence rate of a SfM scheme is mainly affected by the particular trajectory followed by the camera w.r.t. the observed scene, with some trajectories being more informative/exciting than other ones. Therefore, the IBVS controller should select (online) the 'most informative' camera trajectory, among all the possible ones solving the visual task, for the sake of obtaining the fastest possible SfM convergence during the servoing transient. Chapter 4 described a strategy for generating such an informative camera motion policy. The rest of this chapter will, instead, detail how to exploit the results of Chapt. 4 for improving the performance of a IBVS controller by suitably taking advantage of (and possibly maximizing) the redundancy of the considered visual task.

Plugging active sensing in IBVS schemes

The execution of a servoing task can be naturally coupled with the (concurrent) optimization of the estimation of vector χ by exploiting vector qw in (2.32) for projecting any action aimed at maximizing, e.g., the smallest eigenvalue σ 2 1 of the PE matrix ΩΩ T in the null-space of the visual task. The expression in (4.22) shows that optimization of σ 2 1 (t) requires an action at the joint acceleration level. In particular, since

∇ qσ 2 1 = ∇ v σ 2 1 T ∇ qv T T = J T v J T σv (6.2)
(where the relationship v = J v (q) q was used), local maximization of σ 2 1 can be achieved by just following its positive gradient via a joint acceleration vector

qσ = k σ ∇ qσ 2 1 = k σ J T v J T σv , k σ > 0. (6.3)
Following the developments of Sect. 2.2.4.4 and being ė = J q and, thus, ë = J q + J q, the second-order/acceleration level counterpart of the classical law (2.32) for regulating the error vector e(t) to ∅ m is simply

q = qe = J † (-k v ė -k p e -J q) + P qw (6.4)
with k p > 0 and k v > 0. Therefore, by setting qw = qσ in (6.4), one would obtain the desired maximization of σ 2 1 (i.e., of the convergence rate of the 3-D estimation error) concurrently to the execution of the main visual task. This straightforward strategy, although appealing for its simplicity, is unfortunately not viable in most practical situations because of the structural lack of enough redundancy for implementing action (6.3) (or any equivalent one) in the null-space of the main visual task in (6.4). This important limitation is illustrated by the following Proposition. Proposition 6.1. Assume rank(L s ) = 6, that is, the chosen set of visual features s allows controlling the full 6-DOFs camera pose. Then, for any scalar function w(v( q)), one has P ∇ qw = ∅ n .

Proof. We first note that, thanks to the assumption rank(L s ) = 6 (which implies m ≥ 6 and, thus, full column-rank for L s ), one has ker(L s J C ) = ker(J C ). Being P the orthogonal projector on ker(J ), the following then holds

P J T = P J T C L T s = P J T C = ∅ n .
On the other hand, ∇ qw = J T v ∇ qw T . Since J T v belongs to the range space of J T C (see (2.29)), it follows that P ∇ qw = ∅ n as claimed. Proposition 6.1 formalizes an intuitive consideration: if the feature set s is rich enough to constrain all the camera DOFs, then no optimization of the camera linear velocity v can be performed via the null-space projector operator P since (obviously) the camera motion is already fully specified by the chosen visual task. Being classical VS tasks purposely built upon a feature set s able to ensure full (visual) control over the free camera DOFs, Prop. 6.1 clearly prevents any optimization meant to affect the value of σ 2 1 during the servoing execution1 . This fundamental limitation motivates the development of the alternative strategy presented in the following section.

Second-order VS using a Large Projection Operator

An alternative control strategy that is able to circumvent the limitations imposed by Prop. 6.1 can be devised by suitably exploiting the redundancy framework originally proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]. In this work it is shown how regulation of the full visual error vector e (a m-dimensional task) can be replaced by the regulation of its norm e (a 1-dimensional task). This manipulation results in a null-space of (maximal) dimension n -1 available for additional optimizations including, in our case, those prevented by Prop. 6.1.

The machinery presented in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] can be exploited as follows: by letting ν = e , we have ν = e T J e q = J ν q, J ν ∈ R 1×n , and, at second-order, ν = J ν q + Jν q.

Regulation of ν(t) → 0 can then be achieved by the following control law

q = qν = J † ν (-k v ν -k p ν -Jν q) + P ν qw , (6.5)
with k p > 0, k v > 0, J † ν = e e T J J T e J T e and P ν = I n -J T ee T J e T J J T e being the null-space projection operator of the error norm with rank n -1 (see [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]).

By implementing controller (6.5) in place of (6.4) one can thus still obtain regulation of the whole visual task error since, obviously, ν(t) = e(t) → 0 implies e(t) → ∅ m . However, contrarily to (6.4), the new null-space projector P ν allows implementing a broader range of optimization actions including (6.3) or equivalent ones. In this respect, next Sect. 6.2.2 addresses the design of a suitable cost function V(v( q)) able to trade off maximization of σ 2 1 with the boundedness of the camera/robot self-motions in the null-space of the main visual task.

We also note, however, that controller (6.5) suffers from some shortcomings: in particular, the Jacobian J ν is singular for e = 0, while the projection matrix P ν and the pseudoinverse J † ν are not well-defined for e = 0 and for e ∈ ker(J T ). As discussed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], the singularity at e = 0 can be avoided by switching from controller (6.5) to the classical law (6.4) when 'close enough' to convergence (i.e., when e becomes sufficiently small). However, since the 'first-order' switching strategy proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] is not directly transposable to the second-order case, Sect. 6.2.3 details a suitable 'second-order' strategy able to guarantee a proper switching from (6.5) to the classical law (6.4). Remark 6.1. We wish to emphasize the different roles of the two singularities e = 0 and e ∈ ker(J T ) affecting controller (6.5). The singularity occurring at e = 0 is a consequence of the choice of controlling the norm of the error vector and, thus, it does not affect other schemes such as the classical one (6.4) (therefore one can safely switch to (6.4) when e → 0). The other singularity e ∈ ker(J T ) corresponds, instead, to a local minimum for the servoing itself since, if e ∈ ker(J T ), no camera motion can instantaneously realize the task. Therefore, any 'local' control action (including (6.4) and (6.5)) would be equally affected by the condition e ∈ ker(J T ), and no simple switching strategy could be employed in this case (local minima escaping strategies, such as random walks or global optimizations, are out of the scope of this thesis).

Optimization of the 3-D Reconstruction

Being the convergence rate of the 3-D estimation error χ(t) = χ(t)-χ(t) determined by the eigenvalue σ 2 1 , a straightforward choice is to attempt maximization of a cost function in the form

V( q) = k σ σ 2 1 (v( q)), k σ > 0, (6.6)
in a similar way to what done in Part II. This would result in the joint acceleration qσ in (6.3) to be plugged in vector qw in (6.5). However, a cost function such as (6.6) is unbounded from above w.r.t. q (sup q V( q) = ∞) since, in general, the faster the camera motion the larger σ 2 1 . Indeed, on the one hand, σ 2 1 ∝ v 2 (see (4.31) for the point feature case) and, on the other hand, v 2 ∝ q 2 being J v in (2.29) a bounded quantity. Therefore, maximization of (6.6) via (6.3) would simply make the camera velocity to grow unbounded in the null-space of the main servoing task.

In order to cope with this issue, it is then necessary to modify (6.6) for allowing existence of a finite upper bound w.r.t. q . The addition of a classical quadratic

k _ qk 2 0 1 2 3 4 5 V -0.2 -0.1 0 0.1 0.2 Figure 6
.1 -A representative graph of the cost function used to calculate the IBVS secondary task in (6.8) plotted against q for k σ = 1, k d = 0.2, γ = 0.1, and assuming σ 2 1 = q 2 . Note the presence of a finite upper bound for V( q) as desired.

penalty (damping) term in the form

V( q) = k σ σ 2 1 ( q) - k d 2 q 2 , k d > 0, (6.7)
is, nevertheless, still not a valid solution: as both terms in (6.7) are proportional to q 2 , existence of a finite (non-zero) upper bound would depend, case by case, by the particular combination of gains and of the camera trajectory. Because of these considerations, we then opted for the following cost function

V( q) = k σ γ log γ + σ 2 1 ( q) γ - k d 2 q 2 , γ > 0, (6.8)
for which a representative graph is depicted in Fig. 6.1. This choice is motivated by considering that, for large velocities ( q → ∞), the damping term k d 2 q 2 will always be dominant w.r.t. the first term regardless of the choice of gains or of the particular camera trajectory. Indeed, since σ 2 1 ∝ q 2 and since log(x) = o(g(x)) for any polynomial function g(x), it follows that

lim q →∞ V( q) = -∞.
Therefore, maximization of V( q) in (6.8) will guarantee maximization of σ 2 1 with, embedded, a bound on the maximum allowed joint velocity q . The location of this maximum and of the slope of (6.8) for q → 0 and q → ∞ is determined by the gains k σ , γ and k d . Maximization of V( q) is then obtained by plugging in vector qw in (6.5) the following joint acceleration vector

qV = ∇ qV = k σ γ γ + σ 2 1 ∇ qσ 2 1 -k d qT .
(6.9)

Second-order Switching Strategy

We now discuss a second-order switching strategy meant to avoid the singularity of controller (6.5) when ν(t) = e(t) → 0. We start noting that, in closed-loop, controller qν in (6.5) imposes the following second-order dynamics to the error norm

ν + k v ν + k p ν = 0. (6.10)
Define ν e (t) as the solution of (6.10) for a given initial condition (ν(t 0 ), ν(t 0 )): ν e (t) thus represents the 'ideal' evolution of the error norm, that is, the behavior one would obtain if controller (6.5) could be implemented ∀t ≥ t 0 .

Let now t 1 > t 0 be the time at which the switch from controller (6.5) to the classical law qe in (6.4) occurs (e.g., triggered by some threshold on e as proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]). For t ≥ t 1 , controller qe , under the assumption2 rank(J ) = ρ = m, yields in closed-loop

ë + k v ė + k p e = 0. (6.11)
Let e * (t) be the solution of (6.11) with initial conditions (e(t 1 ), ė(t 1 )), and let ν * (t) = e * (t) be the corresponding behavior of the error norm. Ideally, one would like to have

ν * (t) ≡ ν e (t), ∀t ≥ t 1 . (6.12)
In other words, the behavior of the error norm should not be affected by the control switch at time t 1 , but ν * (t) (obtained from (6.11)) should exactly match the 'ideal' evolution ν e (t) generated by (6.10) as if no switch had taken place.

While condition (6.12) is easily satisfied at first-order [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], this is not necessarily the case at the second-order level. Indeed, when moving to the second-order the following result can be shown (see Appendix A.5.1) Proposition 6.2. For the second-order error dynamics (6.10-6.11), condition (6.12) holds iff, at the switching time t 1 , vectors e(t 1 ) and ė(t 1 ) are parallel.

It is then necessary to introduce an intermediate phase, before the switch, during which any component of ė orthogonal to e is made negligible.

To this end, let

P e = I m - ee T e T e ∈ R m×m
be the null-space projector spanning the (m -1)-dimensional space orthogonal to vector e. Let also δ = P e ė = P e J q. (6.13)

The scalar quantity δ T δ ≥ 0 provides a measure of the misalignment among the directions of vectors e and ė (δ T δ = 0 iff e and ė are parallel, ∀e = ∅ m , ė = ∅ m ). One can then minimize δ T δ compatibly with the main task (regulation of the error norm) by choosing vector qw in (6.5) as

qδ = - k δ 2 ∇ q(δ T δ) = -k δ J T P e J q = -k δ J T δ (6.14)
where the properties P e = P T e = P e P e were used. A possible switching strategy, shown in the flowchart in Fig. 6.2, then consists of the following three different control phases: phase 1): apply the norm controller qν given in (6.5) with the null-space vector qw = qV as defined in (6.9) as long as ν(t) ≥ ν T , with ν T > 0 being a suitable threshold on the error norm. During this phase, the error norm will be governed by the closed-loop dynamics (6.10) and the convergence rate in estimating χ will be maximized thanks to (6.9); phase 2): when ν(t) = ν T , keep applying controller qν but replace (6.9) with (6.14) in vector qw . Stay in this phase as long as some terminal condition on the minimization of δ T δ is reached. In our case, we opted for a threshold δ T on the minimum norm of vector P ν J δ as an indication of when no further minimization of δ T δ is possible in the null-space of the error norm. Note also that, during this second phase, ν(t) keeps being governed by the closed-loop dynamics (6.10) since qw acts in the null-space of the error norm (i.e., no distorting effect is produced on the behavior of ν(t) by the change in qw );

phase 3): when δ T δ has been minimized, switch to the classical controller qe given in (6.4) until completion of the task. The minimization of δ T δ will ensure a smooth switch as per Prop. 6.2.

We finally note that this strategy could cause a discontinuity in the commanded acceleration q when moving from phase 1) to phase 2) because of the instantaneous change of vector qw from (6.9) to (6.14). If needed, this discontinuity could be easily dealt with by resorting to a smoothing procedure as that proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF]. As for the switch from phase 2) to phase 3), discontinuities in the commanded q are, instead, avoided thanks to the alignment among vector e and ė occurring during phase 2).

Before concluding this section we remark that the proposed scheme (active SfM (3.11) coupled to the second-order VS (6.4-6.5)), null-space terms (6.9-6.14) and associated switching strategy of Fig. 6.2) only requires, as measured quantities, phase 1): use (6.5) with qw = qV in (6.9) start ν(t) ≥ ν T phase 2): use (6.5) with qw = qδ in (6.14) the visual features s, the robot joint configuration vector q and the joint velocities q (in addition to the usual assumption of known intrinsic and camera-to-robot parameters). Indeed from the estimated χ, a (possibly approximated) evaluation of all the other quantities entering the various steps of the second-order control strategy (e.g., the task Jacobians J , J ν and their time derivatives J , Jν ) can be obtained from (s, χ, q) and q (the only 'velocity' information actually needed). We also note that the level of approximation is clearly a monotonic function of χχ (i.e., the uncertainty in knowing χ): thus, all the previous quantities will asymptotically match their real values as the estimation error χ(t) = χ(t) -χ(t) converges to zero (thus, the faster the convergence of χ(t), the sooner the ideal closed-loop behaviors (6.10-6.11) will be realized).

P ν J δ ≥ δ T
We finally remark that, due to the nonlinear nature of the estimation and servoing schemes, stability of each individual block does not imply, in general, stability of their composition since one cannot invoke the separation principle only valid for linear time-invariant systems 3 . Nevertheless, the experimental results reported in Chapt. 7 show a promising level of robustness in this sense.

Adaptive switching

We now propose a further improvement to the strategy detailed in Sect. 6.2. The goal is to introduce an automatic mechanism for adaptively activating/deactivat-ing optimization of the 3-D structure estimation as a function of the accuracy in estimating χ(t). This modification is motivated by the following considerations w.r.t. the flowchart of Fig. 6.2:

• the optimization of σ 2 is active during the whole phase 1), i.e., as long as the error norm is larger than some predefined threshold (i.e., ν(t) ≥ ν T ). However, this is obtained at the expense of a possible distortion of the camera trajectory as it will be shown in, e.g., Figs. 7.1(d) and 7.3(d) which depict the camera spiralling motion due to action (7.1) while approaching the final pose.

Clearly, a more efficient strategy would implement (7.1) only when strictly needed, e.g., as long as the estimation error χ(t) = χ(t) -χ(t) is larger than some minimum threshold;

• similarly, once in phases 2) and 3), the flowchart of Fig. 6.2 does not allow any reactivation of the optimization of σ 2 . On the other hand, a reactivation could be necessary in case of unforeseen events such as, e.g., an unpredictable motion of the target that would make the estimation error χ(t) to abruptly increase.

We then now detail a modification of the previous strategy of Sect. 6.2 for addressing these issues. To this end, we first introduce a way to quantify the uncertainty level in the estimation of the unknown vector χ(t). Since the estimation error χ(t) is (obviously) not directly measurable, we consider instead the following measurable quantity

E(t) = 1 T t t-T s T (τ ) s(τ )dτ, T ≥ > 0, (6.15)
where T represents the integration window and s = s-s is the feedback term driving observer (3.11). Indeed, as discussed in Appendix A.5.2, E(t) plays a role comparable with the unmeasurable χ(t), that is, it provides a measure of the uncertainty of the estimated χ vs. the actual χ. In particular, provided the camera trajectory is sufficiently exciting (i.e., σ 2 1 (t) > 0 during motion), E(t) ≡ 0 iff χ(t) ≡ 0 (i.e., the estimation has converged) and E(t) > 0 otherwise . One can then leverage knowledge of E(t) for, e.g.,(i ) automatically switching from phase 1) to phase 2) when the estimation error becomes smaller than a desired threshold, (ii ) automatically switching from phase 3) back to phase 1) when the estimation error grows larger than a desired threshold, and (iii ) adaptively weighting the first term in action (6.9) for smoothly activating/deactivating the optimization of σ 2 1 . Let then 0 ≤ E < E be a fixed minimum/maximum threshold for E(t) and define as a monotonically increasing smooth map from the interval [E, E] to the interval [0, 1]. For instance, one could take

k E (E) : [E, E] → [0, 1] E E E k E (
k E (E) =            0 if E ≤ E 1 2 - 1 2 cos π E -E E -E if E < E < E 1 if E ≥ E (6.16)
for which the graph for E = 1 × 10 -5 and E = 1 × 10 -4 is depicted in Fig. 6.3.

Function k E (E) can be exploited for suitably weighting the optimization of σ 2 1 : a simple but effective possibility is to just modify the cost function (6.8) as

V E ( q, E) = k σ k E (E)γ log γ + σ 2 1 ( q) γ - k d 2 q 2 , (6.17)
resulting in the new optimization action

qV E = ∇ qV E = k σ k E (E)γ γ + σ 2 1 J T v J T σv -k d q (6.18)
to be plugged in vector qw in (6.5). This modification clearly grants a smooth modulation of the first term in (6.18) from a full activation in case of large estimation inaccuracies (k

E (E) = 1 for E ≥ E), to a full deactivation in case of small estimation inaccuracies (k E (E) = 0 for E ≤ E).
Exploiting E(t) and the modified optimization action (6.18), we then propose the new (adaptive) switching strategy depicted in Fig. 6.4. This new strategy consists of the same three phases of Sect. 6.2.3, but it now exploits knowledge of E(t) for implementing an improved switching policy among the various phases:

• at the beginning of the servoing task start in phase 3) (instead of phase 1)), thus implementing the classical controller (6.4). Remain in this phase while E(t) ≤ E or ν(t) ≤ ν T , otherwise switch to phase 1); phase 1): use (6.5) with qw = qV E in (6.18) ν(t) ≥ ν T and E ≥ E phase 2): use (6.5) with qw = qδ in (6.14) • when in phase 1), implement the norm controller (6.5) with the new optimization action (6.18). Stay in phase 1) as long as E(t) ≥ E and ν(t) ≥ ν T , otherwise switch to phase 2);

E > E and ν(t) ≥ ν T P ν J δ ≥ δ T phase 3): use (6.4) E ≤ E or ν(t) < ν T
• when in phase 2), implement the norm controller (6.5) with the optimization action (6.14). If E(t) ≥ E and ν(t) ≥ ν T , switch back to phase 1), otherwise, if P ν J δ ≤ δ T (terminating condition for the alignment among vectors e and ė) switch to phase 3).

We highlight the following features of this new adaptive strategy: first of all, the initial (possible) switch from phase 3) to phase 1) is performed only if E(t) ≥ E (the estimation error is large enough for justifying an optimization of the camera motion) and ν(t) ≥ ν T (the visual error norm is large enough for preventing singularities in (6.5)). As illustration, two scenarios will typically trigger this switch:(i ) a camera starting far enough from the desired pose and with a poor enough initial estimation χ(t 0 ), or (ii ) an unpredicted motion of the target object during the servoing task that causes an increase in the error norm and in the estimation uncertainty. The experiments of the next Sects. 7.2 and 7.3 will indeed address these two practical cases. Furthermore, while in phase 1), the optimization of the camera motion will be performed only until either a good enough accuracy has been reached (E(t) < E), or controller (6.5) is close to become singular (ν(t) < ν T ). The new switching condition E(t) < E will then help in minimizing the distortion of the camera trajectory by allowing a quick switch to phase 2) as soon as the estimation accuracy reaches a satisfactory level (see again the experiments in Sects. 7.2 and 7.3).

As a final step, we comment about the choice of the two thresholds E and E exploited for triggering the various switches and for modulating the activation of the optimization of σ 2 1 in (6.18). Assume the range of possible values of E(t) during the camera motion can be lower/upper bounded as 0 ≤ E min ≤ E(t) ≤ E max . It would obviously be meaningful to choose E and E such that E min ≤ E < E ≤ E max for properly tuning the adaptive switching strategy.

Concerning the lower bound E min , being E(t) ≥ 0, a straightforward choice would be E min = 0. However, presence of measurement noise and other nonidealities can, in practice, prevent E(t) to fall below some minimum value even after convergence of the estimation error (up to some residual noise). If needed, this minimum value can be, e.g., experimentally determined by simply averaging, across a sufficient number of different camera trajectories, the (steady-state) value reached by E(t) once the estimation has converged 4 . As for E max , any (arbitrarily large) positive value would in principle be a valid choice since, the larger the initial approximation error χ(t 0 ) = χ(t 0 )-χ(t 0 ) , the wider the possible range of E(t). It is, however, possible to show that, exploiting the properties of observer (3.11) and, in particular, its port-Hamiltonian nature, the following bound holds (see Appendix A.5.2)

E(t) ≤ χ(t 0 ) 2 α . (6.19)
Therefore, if an upper bound χ(t 0 ) ≤ z max on the initial estimation error can be assumed (as in most practical situations), one can exploit (6.19) and set

E max = z 2 max α . ( 6
.20)

Conclusions

In this chapter we investigated how to couple the execution of a VS task with an active SfM strategy meant to optimize the reconstruction of the 3-D scene structure. This result was achieved by projecting the active SfM action within the null-space of the considered IBVS task. In general IBVS, however, tasks are intentionally constructed in such a way that all the camera DOFs are constrained and, therefore, they typically lack any additional redundancy to exploit for optimizing the estimation convergence using, e.g., some of the techniques proposed in Chapt. 4.

To cope with this problem, we proposed to suitably extend to the second order the framework originally introduced in [MC10] for granting the needed redundancy for an effective optimization of the camera motion by controlling the task error norm instead of the task itself. A thorough analysis of the closed-loop convergence performance, including a switching strategy meant to avoid some structural singularities of [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF], was also provided. As an additional contribution, we detailed an adaptive strategy able to automatically activate/deactivate and, more in general, tune the optimization of the SfM convergence as a function of the current accuracy of the estimated 3-D structure. This was obtained by exploiting the prediction error s (in particular the average of its norm over a finite time interval) as an indication of the current convergence status of the estimation relying, for this, the prediction error and the pH nature of the error system dynamics.

The next Chapt. 7 will report a thorough experimental validation of all of the theoretical results obtained in this chapter. target object in order to show the viability of our method also in more realistic situations.

As for what concerns the optimization of the 3-D reconstruction, we note that each feature point is characterized by its own (independent) eigenvalue σ 2 1,i . Optimization of the estimation of the whole vector χ (i.e., of the depth of all points) was then obtained by considering the average of the N eigenvalues

σ 2 = 1 N N i=1 σ 2
1,i as quantity to be optimized. This obviously corresponds to using the A-optimality condition introduced in (4.24), normalized w.r.t. the number of points. Being, obviously,

∇ qσ 2 = 1 N J T v N i=1 J T σv i
the acceleration command (6.9) was then simply replaced by

qV = 1 N k σ γ γ + σ 2 J T v N i=1 J T σv i -k d q (7.1)
during phase 1) of all the following experiments.

The rest of the chapter is organized as follows: Sect. 7.1 is meant to validate the basic machinery, described in Sect. 6.2, for coupling IBVS execution and optimization of the 3-D structure estimation. Subsequently, in Sect. 7.2, we will report the results of some experiments using the strategy detailed in Sect. 6.3 for allowing a smooth activation/deactivation of the camera trajectory optimization as a function of the current estimation accuracy. This extension is then further validated in Sect. 7.3 in a more typical experimental scenario. Section 7.4 concludes the chapter with some final considerations.

Videos for the experiments shown in this chapter can be downloaded from the web page associated with the publication [START_REF] Spica | Coupling Visual Servoing with Active Structure from Motion[END_REF] at http://ieeexplore.ieee. org. Additional videos are also available at https://www.youtube.com/watch?v= IYX6C2qYInA and https://www.youtube.com/watch?v=kgoWUu-9fhs.

Using a basic switching strategy 7.1.1 First set of experiments

In this first set of experiments we aim at illustrating the benefits arising from the coupling between the execution of a VS task and the concurrent active optimization of the 3-D structure estimation. To this end, we consider the following four different cases, all starting from the same initial conditions: Let us first focus on Fig. 7.1(b): from the plots one can note how the use of observer (3.11) in cases I and II makes it possible for the estimation/approximation error χ(t) to converge faster than in case III where convergence is reached only at the end of the task, when χ(t) → χ = χ d (as obvious). Furthermore, the convergence of χ(t) is clearly faster in case I (about 4 s) than in case II (about 12 s, thus three times slower). This improvement is due to the active optimization of the camera velocity occurring during phase 1) of case I. Indeed, looking at Fig. 7.1(c), one can note how the value of σ 2 (t) of case I (blue line) is approximately 4 times larger than in case II (red line) for the whole duration of phase 1) as a result of the more 'exciting' trajectory performed by the camera under the action of the optimization term (7.1).

time [s] 0 20 40 60 < 2 #10 -3 0 1 2 3 case I case II (c) 0.2 Y w [m] 0 -0.2 0.4 0.2 X w [m] 0 -0.2 -0.1 -0.5 -0.2 -0.3 -0.4 Z w [m] ph. 1) ph. 2) ph. 3) (d) (e)
A similar pattern can also be found in Fig. 7.1(a): indeed, the behavior of ν(t) for case I (blue line)(i ) quickly reaches a good match with the ideal behavior of case IV (dashed black line), and, more importantly, (ii ) keeps monotonically decreasing during all the various phases. This is clearly achieved thanks to the fast convergence of χ(t) → 0 that translates into a fast accurate evaluation of the task Jacobian J and any related quantity. On the other hand, due to the larger error in estimating χ(t), both cases II and III present an initial divergence of the error norm ν(t) that starts increasing (rather than decreasing as in case I) because of the poorer approximation in the evaluation of the task Jacobian J . It is worth noting how this initial divergent phase has, nevertheless, a shorter duration for case II w.r.t. case III thanks, again, to the use of observer (3.11) which is eventually able to provide a sufficiently accurate estimation of χ(t) starting from t ≈ 12 s.

The camera trajectory, depicted in Fig. 7.1(d), is also helpful for better understanding the effects of the active optimization of the camera motion during phase 1) of case I. Note, indeed, how the camera initially moves along an approximately circular path (blue line) because of the null-space term (7.1) that generates an 'exciting' motion for the estimation of the four point depths Z i . This then allows to quickly reduce the estimation error χ(t) as reported in Fig. 7.1(b). It is also possible to, again, appreciate the benefits of having employed the norm controller (6.5) during phase 1): indeed, it is only thanks to the large redundancy granted by controller (6.5) that the camera is made able to follow a quite 'unusual' trajectory while, at the same time, ensuring a convergent behavior for the error norm ν(t) (Fig. 7.1(a)). For completeness, the red line in Fig. 7.1(d) represents (the quite short) phase 2) of the switching strategy (i.e., the alignment among vectors e and ė), while the green line represents phase 3), i.e., the use of the classical controller (6.4) for completing the servoing task. Finally in Fig. 7.1(e) the image plane trajectory of the four point features is reported exploiting the same color code of Fig. 7.1(d).

As a supplementary evaluation of the theoretical analysis of Sect. 6.2.3, we now report, for case I only, an additional experiment aimed at assessing the importance of having introduced phase 2) in the switching strategy of Sect. 6.2.3 (i.e., of having enforced the alignment of e and ė before switching to the classical controller (6.4). To this end, Fig. 7.2(a) shows the behavior of the error norm ν(t) for the previous case I (blue line) together with the behavior of ν(t) when not implementing phase 2) but, instead, directly switching from phase 1) to phase 3) (cyan line). The two (almost coincident) blue vertical lines represent the switch from phase 1) to phase 2) and then phase 3) for the first experiment, and the direct switch from phase 1) to phase 3) for the second experiment. One can then note how, in the second experiment, the error norm ν(t) has a large overshoot when switching to phase 3) because of the misalignment of vectors e and ė at the switching time. In particular, this overshoot makes the error norm temporarily increase instead of monotonically decrease as desired. This overshoot is, instead, clearly not present in the first experiment where ν(t) keeps (correctly) converging during all phases.

Similarly, Fig. 7.2(b) reports the behavior of δ from (6.13), i.e., the measure of misalignment among vectors e and ė minimized during phase 2). One can then verify how, in the first experiment, δ is correctly (and very quickly) minimized at the end of phase 2) thanks to the action of (6.14). Finally, the effects of implementing/non-implementing phase 2) are also illustrated in Fig. 7.1(e), where the point feature trajectory with phase 2) activated (solid lines) and deactivated (dashed lines) are reported. One can again note how, in this latter case, the point features are subject to a significant overshoot at the switching time which is, instead, avoided when implementing phase 2).

Second set of experiments

We now discuss a second set of experiments that involve the same four cases I to IV introduced at the beginning of the previous section but with the camera starting from a different initial pose and with a different desired configuration s d w.r.t. the previous run. The results are reported in Fig. 7.3 by following the same pattern and color codes of the previous Fig. In both plots, the blue lines represent the behavior of case I (full implementation of the switching strategy of Sect. 6.2.3), while cyan lines represent the direct switch from phase 1) to phase 3) without the action of vector qw = qδ in (6.14). The small picture-in-picture plots provide a zoomed view of the switching phase.

(red line in Fig. 7.3(a)), i.e., when coupling the classical controller (6.4) with observer (3.11) but without optimizing for the convergence rate of χ(t) . In fact, in this case, the very small value of σ(t) during the camera motion (red line in Fig. 7.1(c)) makes the estimation task ill-conditioned w.r.t. noise and other unmodeled effects (including the disturbance d( χ, t) in (3.12)), resulting in a divergence of the estimation error χ(t) at t ≈ 9 s (red line in Fig. 7.3(b)). On the other hand, the active optimization of case I is able to increase σ(t) by approximately a factor of 40 w.r.t. case II, thus ensuring a sufficiently high level of excitation for the camera motion and, consequently, a quick convergence of the estimation error χ(t) . Failure of the servoing is finally obtained also in case III, i.e., when exploiting the exact final value χ(t) = χ d , because of the large initial error of the visual task that causes the features to leave the camera FOV (green line in Fig. 7.3(a)).

Finally, Figs. 7.3(d) and 7.3(e) depict the camera and feature trajectories during case I. One can again appreciate, in Fig. 7.3(d), the initial spiralling motion of the camera that allows the increase of σ(t) during phase 1). It is also worth noting how, in case I, the error norm ν(t) keeps a monotonic decrease during the whole motion (as desired) despite the various switches among the three phases and the 'unusual' initial camera trajectory (blue line in Fig. 7.3(a)).

Analogously to what done in Sect. 7.1.1, we conclude by highlighting again the fundamental role played by phase 2) for ensuring a monotonic convergence of the error norm ν(t). To this end, Fig. 7.4(a) shows the behavior of ν(t) when activating (blue line) or not activating (cyan line) phase 2). One can note, again, ). In both plots, the blue lines represent the behavior of case I (full implementation of the switching strategy of Sect. 6.2.3), while cyan lines represent the direct switch from phase 1) to phase 3) without the action of vector qw = qδ in (6.14). The small picture-inpicture plots provide a zoomed view of the switching phase.

the large overshoot (and temporary divergence) introduced at the switching time when parallelism among vectors e and ė is not enforced. Figure 7.4(b) depicts the associated behavior of the misalignment measure δ(t) that proves, again, how the action (6.14) during phase 2) (blue line) is able to quickly minimize δ(t) as desired.

Third set of experiments

In this section, we report the results of two experiments meant to show how even relatively small inaccuracies in determining the value χ d at the desired pose can cause failure of the servoing when setting χ(t) = χ d as classically done in many visual servoing applications. The two experiments involve the same setting of the previous cases (regulation of 4 point features) and differ from the starting location of the camera w.r.t. the target object: in the first experiment, the camera starts (relatively) far from the desired pose while, in the second experiment, the camera starts at almost the desired pose. In both cases, the classical second order control (6.4) by taking χ = (I p + diag( ))χ d with ∈ R p being a random vector with magnitude 0.09 (thus, simulating an uncertainty of 9% in the accuracy of χ d ). Figure 7.5(a) shows the behavior of the error norm ν(t) for both cases: in the first experiment (blue line), the visual error starts converging from its initial (large) value but then, at about t ≈ 8 s, the servoing diverges and the features leave the camera FOV. An even more interesting result is obtained in the second experiment (red line): in this case, the error ν(t) starts at a very small value since the camera is already quite close to its desired pose. However, controller (6.4) is not able to impose a stable closed-loop behavior, and the error norm starts diverging until loss of tracking of the feature points at about t ≈ 2.5 s.

These results then provide an experimental demonstration of the effects discussed in Sect. 2.3.2 and originally introduced in [MMR10]: a (rather small) error in approximating χ d can be sufficient to move part of the eigenvalues of matrix -J (s d , χ d , q) J (s d , χ, q) † to the right-half complex plane, thus resulting in an unstable closed-loop dynamics even when starting arbitrarily close to the desired pose. By, instead, resorting to an online (and optimized) estimation of χ(t), one can obtain a considerably larger degree of robustness against uncertainties in evaluating χ d or similar approximation errors as extensively shown by the previous results.

Using an adaptive switching strategy

For the experiments in this section, we considered the same experimental setup of Sect. 7.1, that is, regulation of N = 4 point features belonging to the surface of a cube. However, we now employed the full adaptive strategy described in Sect. 6.3 for triggering the activation/deactivation of the optimization of the 3-D structure estimation (switch from phase 3) to phase 1) and vice-versa) and, more in general, for tuning its effect (when in phase 1)) as a function of the accuracy in estimating χ(t)

As done in the previous experiments, we initialized the SfM observer (3.11) with vector χ(t 0 ) taken coincident with the (assumed known) χ d at the final pose, and s(t 0 ) = s(t 0 ). As explained in Appendix A.5.2, this results in a bound χ(t 0 ) 2 /α = 5.3e-3 in (6.19). As for the adaptive strategy thresholds, we set E = 10 -5 and E = 10 -4 . The results of the experiment are reported in Fig. 7.6: during the trial, the target object is purposely displaced at t ≈ 5.9 s, t ≈ 10.6 s and t ≈ 17.2 s for introducing an "external disturbance" able to increase both the servoing and the estimation errors above their minimum thresholds with a corresponding (re-)activation of the camera motion optimization.

At the beginning of the motion (phase 3)), the eigenvalue σ 2 1 is considerably small due to the low information content of the camera trajectory (Fig. 7.6(c)) and, analogously to case II in Sect. 7.1.2, the estimation error χ(t) even starts diverging because of measurement noise, the disturbance term d in (3.12), and other nonidealities (Fig. 7.6(b)). At time t ≈ 1.1 s, however, the quantity E(t) increases over the threshold E because of the high uncertainty in the estimated χ (Fig. 7.6(d)), thus triggering the switch to phase 1) and the corresponding optimization of the camera motion. The optimization action (6.18) results in a fast increase of the mean eigenvalue σ(t) (Fig. 7.6(c)) and, as a consequence, in a fast convergence of the estimation error χ(t) (Fig. 7.6(b)) that practically vanishes at time t ≈ 4 s. As a consequence, E(t) decreases again below the minimum threshold E indicating that a sufficient level of accuracy has been reached. This then triggers the (very quick) switch to phase 2) and, then, the switch back to phase 3) at t ≈ 4.4 s. Note how the adaptive gain k E (E) used in (6.18) correctly (and smoothly) activates and deactivates the optimization of σ 2 during phase 1) as clear from Fig. 7.6(e).

It is worth noting that the switch from phase 1) to phase 3) occurs when the error norm ν(t) is still well above the threshold ν T indicating singularity of controller (6.5). Therefore, the distortion of the camera trajectory (depicted in Fig. 7.6(f)), needed to maximize σ 2 , lasts considerably less than in the non-adaptive case where the switch would have occurred only at ν(t) = ν T . Finally, one can also appreciate how the error norm ν(t) correctly converges monotonically towards zero once the estimation error χ(t) becomes small enough, i.e., for t ≥ 4 s, see Fig. 7.6(a). At t ≈ 5.9 s the target object is purposely displaced, as explained, causing both the servoing and the estimation error to grow with a corresponding increase of E(t) above the threshold E. This, in turn, triggers the switch to phase 1) at t ≈ 6.1 s for (re-)activating the optimization of the camera motion until convergence of the estimation error is again reached at t ≈ 9.1 s. The same pattern then repeats two more times at t ≈ 10.6 s and t ≈ 17.2 s because of the two additional displacements of the target object during the camera motion.

As explained in Sect. 6.3, the switch from phase 1) to phase 3) (and vice-versa) is also a function of the current value of the error norm ν(t) for avoiding possible singularities in (6.5). This is, indeed, the case of the third switch from phase 1) to phase 3) triggered at t ≈ 13.3 s by the error norm falling below the threshold ν T with E(t) still above the minimum value E. Similarly, the fourth switch from phase 3) to phase 1) at t ≈ 17.9 s is triggered only when ν(t) ≥ ν T even though E(t) has already grown over the threshold E. By looking at Fig. 7.6(d), it is finally worth noting how E(t) always keeps below the theoretical bound χ(t 0 ) 2 /α = 5.3e-3 given in (6.19) despite the three intentional target displacements occurred during the servoing (see Appendix A.5.2).

Using a standard Kanade Lucas Tomasi feature tracker

This last experiment is meant to illustrate the feasibility of our approach in more realistic conditions compared to the use of simple black dots on a white background as done so far. To this end, we considered regulation of 10 point features belonging to a much less structured object, that is, the shrunken piece of textured paper shown in Fig. 7.7(g). Extraction and tracking of the 10 features was achieved by exploiting the well-known KLT algorithm implemented in OpenCV. Finally, we made use of the threshold E = 0.0015 and E = 0.03, and initialized χ(t 0 ) = χ d as before, with, in this case, χ(t 0 ) 2 /α = 6.3e-3 for bound (6.19). Figure 7.7 reports the results of the experiment: the robot starts in phase 3) driven by the classical law (6.4) but, being the mean eigenvalue σ 2 rather small during this phase, the estimation error χ(t) does not converge, and likewise the error norm ν(t) because of the too rough approximation in χ. However, the quantity E(t) starts to grow and, at t ≈ 1 s, it exceeds the threshold E triggering the switch to phase 1) (Fig. 7.7(d)). During this phase (which lasts until t ≈ 5 s) the optimization of the camera motion is then able to maximize the eigenvalue σ 2 resulting in a quick convergence of the estimation error that practically vanishes at t ≈ 4.5 s. Similarly, the quantity E(t) first reaches a maximum peak value (which is anyway lower than the theoretical bound (6.19) as expected), and then starts decreasing back to zero thus allowing a smooth deactivation of the optimization action thanks to the adaptive gain k E (Fig. 7.7(e)). Finally, at t ≈ 5 s the error norm ν(t) falls below the threshold ν T inducing a quick switch to phase 2) (alignment of e and ė) followed by a last switch back to phase 3) until completion of the servoing task.

Looking at these results, it is then possible to appreciate how the overall behavior of the adaptive strategy is essentially equivalent to what obtained in the previous case studies, thus confirming that the proposed approach can be seamlessly applied to more complex/realistic situations. We also believe it is particularly worth noting how, again, the error norm ν(t)(i ) starts decreasing as soon as the estimation error χ(t) becomes small enough in t ≈ 3 s (Fig. 7.7(a)), and (ii ) keeps a monotonic convergence (as desired) until the end of the servoing despite the various switches among the phases and the 'distorted' camera motion during phase 1).

time [s]

(e) Y w [m] 0 -0.1 -0.2 -0.3 0.3 0.2 0.1 0 X w [m] -0.1 -0.2 -0.5 -0.4 -0.3 Z w [m] ph. 1) ph. 2) ph. 3) (f) (g)

Conclusions

This chapter was entirely dedicated to the experimental validation of the strategy introduced in Chapt. 6 for coupling the execution of a IBVS task with the concurrent active optimization of the camera velocity meant to maximize performance in the reconstruction of 3-D scene. The reported experimental campaign clearly shows, in our opinion, the benefits gained by using the proposed strategy in terms of:(i ) obtaining a faster convergence of the structure estimation error during the servoing transient w.r.t. non-active cases, (ii ) imposing an improved closed-loop IBVS behavior by significantly mitigating the negative effects of an inaccurate knowledge of the scene structure, (iii ) minimizing the deformation of the camera trajectory (consequence of the active SfM action) thanks to the adaptive activation/deactivation of the SfM optimization. The last point, in particular, allows to seamlessly deal with the unexpected growth of the estimation error due to unpredicted disturbances (the target object being purposely displaced). The experiments were run under different initial camera configurations and using different feature trackers (a simple dot tracker and a more standard KLT one) demonstrating the robustness of our approach.

Chapter 8

Conclusions and future work I n this last chapter of the thesis, we wish to summarize the main theoretical and experimental contributions of this work highlighting some of the issues that still remain unsolved and suggesting, whenever possible, some directions for improvement and further investigation.

Summary and contributions

The goal of this thesis was to investigate the problem of active sensing and control of robotic systems, in particular by emphasizing the relation between the two aspects. After a short introduction to the basic tools that would serve our analysis, we started the thesis by focusing on the estimation problem. In this context, we proposed to employ a nonlinear observer stemming from the adaptive control literature (see, e.g., [START_REF] Marino | Nonlinear Control Design: Geometric, Adaptive and Robust[END_REF]) and already exploited in [START_REF] Giordano | 3D structure identification from image moments[END_REF][START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF] for SfM applications. The advantage of this choice lies in the fact that, for such an observer, one can fully characterize and control the dynamics of the estimation error. In particular we demonstrated that: (i) the dynamics of the estimation error has a clear port-Hamiltonian form;

(ii) by tuning the observer gains online as a function of measurable quantities only, one can impose to the estimation error a dynamics (approximately) equivalent to that of a second order linear system with assigned poles and, in particular, (iii) by actively controlling the direction of the camera linear velocity one can maximize the performance of the estimation for a given threshold on the maximum admissible control effort (the camera velocity norm);

(iv) in addition to this, one can also gain additional performance by adaptively changing the measurements that are used for the estimation, e.g., by using a generalized definition of image moments with some weight parameters that can be changed online to maximize the "excitation" of the system.

The theoretical results were then applied to the reconstruction of the structure for different geometric primitives hereafter summarized:

(v) point features: we compared the effects of using a planar and a spherical projection model showing how the former can potentially lead to faster convergence of the estimation error at the cost of a higher sensitivity to disturbance and noise w.r.t. the latter.

(vi) planar scenes: we proposed a comparison between the standard homography based reconstruction technique and two "active" strategies based on either the extraction of a plane from an actively estimated point cloud (using leastsquares techniques) or on the direct estimation of the plane parameters from discrete or dense image moments. Finally, as already mentioned, we suggested an adaptive strategy to automatically select on line the best image moments to use for the estimation.

(vii) spherical objects: we showed that, using a suitable parametrization suggested by [START_REF] Fomena | Improvements on Visual Servoing From Spherical Targets Using a Spherical Projection Model[END_REF], the estimation problem can be recast in such a way that the only unknown is the constant sphere radius, thus granting globally exponential stability properties of the observer. Due to the spherical symmetry in this case, the direction of the camera motion does not have any effect on the estimation performance, however, it is still possible to tune at will the "damping factor" of the error dynamics.

(viii) cylindrical objects: we proposed a novel parametrization that, similarly to the sphere case, reduces the estimation problem to that of retrieving a constant parameter: the cylinder radius. This results, again, in a globally exponential stability of the observer. Differently from the spherical case, however, here the estimation performance does depend on the camera motion and thus our active strategy allows, again, to maximize the convergence rate.

We reported an extensive simulative and experimental validation of our approach that, in our opinion, fully confirmed the validity of the theoretical claims.

After this, we moved our attention to the more challenging, but also more interesting, problem of simultaneous structure estimation and control of a robot. We considered, in particular, the case of a IBVS control task for which we showed that:

(ix) the performance of a IBVS scheme clearly benefits from the use of an active strategy that deforms online the trajectory of the camera, during the servo-ing transient, so as to maximize the observability of the unmeasurable 3-D quantities that appear in the feature interaction matrix;

(x) the redundancy of the system can be conveniently maximized by using a second order extension of the strategy originally proposed in [START_REF] Marey | A new large projection operator for the redundancy framework[END_REF] which grants a large null-space projection operator by considering the regulation of the visual task norm instead of the task itself;

(xi) the deforming effects on the camera trajectory of the active observability maximization strategy can be effectively reduced by using an adaptive technique that allows to activate/deactivate, and, more in general, tune it as a function of the current estimation status.

Again, all of the theoretical claims were supported by a number of different experiments run on a real robotic manipulator equipped with a in-hand camera. The results demonstrated, in particular, the robustness of the proposed approach w.r.t. unmodeled disturbances such as sudden unexpected displacements of the target object used for the definition of the visual task.

Open issues and future perspectives

The results of this thesis, although encouraging, also show a number of limitations that affect our approach.

First of all the camera trajectory optimization strategy that we propose is local. In fact it is based on the greedy optimization of the instantaneous version of the PE condition (3.13). This has two drawbacks:(i ) it can, in general, only produce locally optimal solutions and might fail to produce a globally optimal camera trajectory and, more importantly, (ii ) it does not allow to tackle applications in which the number of available measurements is smaller than the number of unknown since, in this case, (3.13) is never full-rank and its smallest eigenvalue is identically zero.

To overcome this limitation one should consider the original integral version of the PE condition (3.9) (we remind the reader that this is the only necessary condition for convergence of the estimation, with (3.13) being, instead, a sufficient but not necessary one). The error transient dynamics analysis proposed in Sect. 4.2 could be generalized by using averaging techniques [START_REF] Sastry | Adaptive Control: Stability, Convergence and Robustness[END_REF], i.e. by considering the system:

         ˙ s av = lim T →∞ t+T t -H s + Ω T χ dτ = f sav ( s av , χ av ) ˙ χ av = lim T →∞ t+T t (-αΩ s + d) dτ = f χav ( s av , χ av )
instead of (3.12). In fact, we expect the eigenvalues of this system to be related to the PE integral condition (3.9). Note however that the maximization of (some norm of) (3.9) requires, as already stressed, the evaluation of matrix Ω(τ ) in the future, i.e. for τ > t. This, in turn, requires a prediction of the future measurements s(τ ) for τ > t which is only possible if the structure of the scene (χ) is known. In some works, such as, e.g. [START_REF] Wilson | Optimal trajectory design for well-conditioned parameter estimation[END_REF][START_REF] Wilson | Trajectory synthesis for fisher information maximization[END_REF] this issue is solved by planning and executing multiple experimental trajectories, each time leveraging the information retrieved during the previous experiment to obtain a better estimation of the unknown quantities that is then exploited in the new planning stage. The possible solution that we envision, instead, would employ a Model Predictive Control (MPC)-like strategy [START_REF] Garcia | Model predictive control: theory and practice -a survey[END_REF][START_REF] Leung | Active slam using model predictive control and attractor based exploration[END_REF]: the optimization of the camera trajectory could be done, at time t, by maximizing (3.9) over a finite horizon T , using the current estimation χ(t) to predict the future measurements s(τ ), τ ∈ [t, t + T ]; only a small portion (lasting, say ∆t T ) of the resulting trajectory would, however, actually be executed, this would allow to collect some additional information about χ that would result in an improved estimation χ(t + ∆t); the optimization process would then be repeated again, based on this more accurate estimation of the scene 3-D geometry, until full convergence of the estimation error.

Another limitation of our approach is the fact that we do not explicitly model the effect of noise. To cope with this, one could adopt a probabilistic estimation/control framework instead of the fully deterministic one proposed in this work. On the one hand this would potentially result in improved robustness w.r.t. non deterministic disturbances; on the other hand, however, one would probably have to give up on a formal characterization of the estimation error dynamics such as the one proposed here. An approach that we find particularly interesting in this context, because of its potential to address a large number of applications, is that of POMDPs. The significant required computational effort remains, however, and to the best of our knowledge, a major limitation of this approach.

As for the estimation problem, we are currently investigating novel strategies for eliminating (or reducing to the minimum) the need of preliminary image processing techniques for extraction and tracking of features. We are, in particular, interested in the class of direct/photometric methods [HS81, MKS89, CM11, BCM13] in which the camera images (i.e. the intensity level of each pixel) are used directly "as they are" to estimate the scene or control the robot motion. We believe that the use of photometric information directly could allow the estimation of a dense pixel-level depth map in an extremely efficient way. In fact, if one can ensure that the estimation algorithm can be run for each pixel using information that is both local in space (e.g. it uses the intensity level of the neighbour pixels only) and in time (using recursive estimation techniques) then the resulting observer would be perfectly suitable for a highly parallel implementation on parallel hardware architectures such as FPGAs or GPUs with potentially extremely high performance both in terms of computation time and in terms of energy efficiency, see [START_REF] Adarve | A Filtering Approach for Computation of Real-Time Dense Optical-flow for Robotic Applications[END_REF]. These factors are particularly crucial for embedded systems applications. Some preliminary results in this context can be found in Appendix B.

As for the last part of this thesis, we note that the stability properties of the coupling between active SfM and IBVS are still to be investigated. In fact, due to the nonlinear dynamics of the system, one can not invoke, as in the linear case, the principle of separation: the fact that both the SfM and IBVS algorithms are separately stable does not imply that the overall system will converge. Although the experimental results are encouraging from this point of view, a more formal analysis should be done in the future. In a similar way, we are also concerned about the possibility that the active optimization of the estimation performance might excessively deform the camera trajectory to the point that, e.g. the camera loses track of the features or the robot reaches one of the joint limits. In this respect two possible approaches could be taken. The first possibility would be to use a more advanced kinematics resolution framework such as, e.g., the ones proposed in [START_REF] Mansard | Task sequencing for high-level sensor-based control[END_REF][START_REF] Mansard | A unified approach to integrate unilateral constraints in the stack of tasks[END_REF], that allow to deal with the presence of multiple motion constraints that can be activated/deactivate online based on priorities and on the system status. Another possibility for dealing with the limited camera FOV would be to employ one of the techniques proposed in, e.g., [FC05, CDW + 14] to allow for a temporary loss of the feature track by resorting on a prediction of the feature motion. We note that such a prediction could obviously benefit from the observability maximization strategy proposed here since it would be based on the most recent estimation of the 3-D parameters associated with the features that went out of the FOV.

Another interesting direction, certainly worth further investigation, is the adaption of the proposed machinery to mobile and flying robots. We have already briefly commented that the difficulty with this kind of platforms mainly comes from the presence of non-holonomic constraints and non negligible dynamics as well as from the higher level of non idealities that affect their actuation which results in the difficulty of retrieving a reliable estimation of the camera velocity, especially for its linear component (for the angular one, the use of good quality gyroscopes can alleviate this issue). Usually a measurement of the camera acceleration is, instead, more accessible thanks to the presence of on-board accelerometers. The nonlinear observer exploited in this work can be extended to use acceleration measurements instead of the camera linear velocity (see [START_REF] Grabe | Nonlinear ego-motion estimation from optical flow for online control of a quadrotor uav[END_REF]). Note, however, that, in this case, the PE condition would impose constraints on the camera acceleration (e.g. the robot needs to keep accelerating during the estimation, see again [START_REF] Grabe | Nonlinear ego-motion estimation from optical flow for online control of a quadrotor uav[END_REF]) that are, in general, harder to deal with from a practical point of view than the velocity ones, especially if the instantaneous version of the PE condition (3.13) is considered.

Finally, the use of mobile and flying robots also opens the way to a number of new potential applications of the proposed machinery in the context of multirobot systems. In many situations, in fact, a team of robot is required to perform some collaborative estimation task concerning either the internal formation state (e.g. estimation of the degree of connectivity [START_REF] Giordano | A passivity-based decentralized strategy for generalized connectivity maintenance[END_REF] or of the rigidity [START_REF] Zelazo | Decentralized rigidity maintenance control with range measurements for multi-robot systems[END_REF] or of the scale [START_REF] Zelazo | Rigidity theory in se(2) for unscaled relative position estimation using only bearing measurements[END_REF] of the formation) or some external quantity (e.g. localization of the formation in the environment [START_REF] Nerurkar | Distributed maximum a posteriori estimation for multi-robot cooperative localization[END_REF] and/or reconstruction of a 3-D scene [START_REF] Thrun | Multi-robot SLAM with sparse extended information filers[END_REF] and so on). In these applications, the estimation problem is typically nonlinear and the trajectory followed by the agents does have an effect on the observability of the system so that the use of an active strategy for deciding an optimal motion policy can have a significant impact, especially considering the high number of DOFs that these systems typically have. For instance, the authors of [START_REF] Morbidi | Active target tracking and cooperative localization for teams of aerial vehicles[END_REF] use simplified process and measurement models to address the problem of controlling the motion of a flock of aerial vehicles in order to minimize the uncertainty in both agent self-localization and multiple target tracking. The case of multi-robot systems also introduces an additional challenge: for most applications, due to technological limitations and scalability considerations, it is not possible (or at least not desirable) to rely on a central node that processes all the information collected by the agents. Each of the agents should instead be able to independently perform the estimation task and take decisions as to which trajectory to follow in a completely decentralized way using only local information collected by its own sensors or by a subset of agents in its neighbourhood.

Appendix A Technical details T his appendix includes additional technical details about the derivations and results contained in the rest of thesis. The material included here is not essential for the understanding of the main results, but provides further insights to the interested reader.

A.1 Derivation of the optimal Kalman-Bucy filter

This section contains some additional technical details concerning the derivation of the optimal Kalman Filter (KF) equations for continuous-time, linear time-varying systems. The developments in this section are mainly based upon [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF] which is extended here to the case of correlated input and measurement noise. Alternative derivations of the optimal KF equations can also be found in the original work [START_REF] Kalman | New results in linear filtering and prediction theory[END_REF] as well as in many other optimal estimation textbooks, such as [START_REF] Lewis | Optimal and robust estimation: with an introduction to stochastic control theory[END_REF].

A.1.1 Propagation equation for the error covariance matrix

We are interested in finding the dynamic differential equation governing the evolution of the estimation error covariance matrix Σ(t) = E x(t) x(t) T . Thanks to the linearity of the derivative and expected value operations one can write

Σ(t) = E d dt x(t) x(t) T = E x(t) ˙ x(t) T + ˙ x(t) x(t) T = E x(t) ˙ x(t) T + E x(t) ˙ x(t) T T (A.1)
Plugging (3.29) in (A.1), and dropping time dependence, one obtains

E x(t) ˙ x(t) T = E x[(A -KC) x -(G -KH)w + Kv] T = Σ(A -KC) T -E xw T (G -KH) T + E xv T K T .
The solution of (3.29) can be written as

x(t) = Φ(t, t 0 ) x(t 0 ) + t t 0 Φ(t, τ ){K(τ )v(τ ) -[G(τ ) -K(τ )H(τ )] w(τ )} τ dτ
where Φ(t, τ ) is the state transition matrix of (A -KC), therefore one can write

E x(t)w(t) T = Φ(t, t 0 ) E x(t 0 )w(t) T + t t 0 Φ(t, τ )K(τ ) E v(τ )w(t) T dτ - t t 0 Φ(t, τ ) [G(τ ) -K(τ )H(τ )] E w(τ )w(t) T dτ. (A.2)
Since x(t 0 ) and w(t) were assumed to be independent, E x(t 0 )w(t) T = O q×w and the first term in (A.2) disappears. Using (3.27), equation (A.2) reduces to

E xw T = t t 0 Φ(t, τ ) {K(τ )M (t) -[G(τ ) -K(τ )H(τ )] Q(t)} δ(t -τ ) dτ = 1 2 KM - 1 2 (G -KH) Q
because the integral interval stops exactly at the impulse position and therefore we consider only half of the impulse weight. A similar strategy can be used to show that

E xv T = 1 2 KR - 1 2 (G -KH) M T so that one can conclude Σ = (A -KC) Σ + Σ(A -KC) T + (G -KH) Q(G -KH) T + KRK T -KM (G -KH) T -(G -KH) M T K T .
(A.3)

A.1.2 Derivation of the optimal Kalman Filter gain

Given the matrix differential equation (A.3) with the initial condition Σ(t 0 ) = Σ 0 , we seek to find the optimal gain matrix K(t), t ∈ [t 0 , t f ] that minimizes, at the terminal time t f , the cost functional

J = E x(t f ) T x(t f ) = E tr x(t f ) x(t f ) T = tr (Σ(t f )),
where tr (•) indicates the trace operation. As suggested in [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF], this problem can be solved by regarding it as an optimal control problem where the elements of matrix K(t) are the "control variables" to be optimized w.r.t. a cost functional defined in terms of the "state variables" given by the elements of Σ(t). One can then resort on the Pontryagin's minimum principle [START_REF] Pontryagin | The Mathematical Theory of Optimal Processes[END_REF] to find the optimal solution. To this end we define a co-state matrix P (t) ∈ R q×m and the Hamiltonian function H(t) = tr Σ(t)P (t) T and the we impose the Pontryagin's necessary optimality conditions

∂H(t) ∂K(t) K(t)= K(t) Σ(t)= Σ(t) = O q×m Ṗ (t) = - ∂H(t) ∂Σ(t) K(t)= K(t) Σ(t)= Σ(t) P (t f ) = ∂J ∂Σ(t f ) K(t)= K(t) Σ(t)= Σ(t) (A.4a) (A.4b) (A.4c)
where (•) indicates the optimal value of the corresponding quantity and we used the concept of gradient matrix as in [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF], e.g.

∂H(t) ∂K(t) =     ∂H(t)
∂K 1,1 (t) . . .

∂H(t) ∂K 1,m (t)
. . . . . . . . .

∂H(t)

∂K q,1 (t) . . .

∂H(t) ∂Kq,m(t)     ∈ R q×m .
From (A.3) and using the matrix differentiation rules in [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF], (A.4b) returns the matrix Ordinary Differential Equation (ODE) Ṗ (t) = -P (t) T A(t) -K(t)C(t) -A(t) -K(t)C(t)

T P (t).

(A.5) Moreover, from (A.4c), one obtains that P (t f ) = I q . Since (A.5) is linear and P (t f ) is symmetric and positive definite, then one also has P (t) = P (t) T 0, ∀t ∈ [t 0 , t f ]. Finally, using again (A.3), the optimality condititon (A.4a) results in

P -ΣC T -G QH T + M T + K R + HQH T + HM T + M H T = O q×m .
Since we have already demonstrated that P (t) is positive definite, we canclude that the optimal gain is given by

K = ΣC T + M Ř-1 (A.6) with M = G QH T + M T , Ř = HQH T + R + HM T + M H T .
Note that the Pontryagin's minimum principle imposes only necessary optimality conditions. Nevertheless, in the case of Kalman filtering, these conditions are also sufficient to prove optimality as discussed in [START_REF] Athans | A Direct Derivation of the Optimal Linear Filter Using the Maximum Principle[END_REF]. which, being ṗ0 = -v -[ω] × p 0 and exploiting the property p T 0 [ω] × p 0 = 0, can be rewritten as

χ = - v K - [ω] × p 0 K + 2 p 0 p T 0 v K 2 = - v K -[ω] × χ + 2χχ T v. (A.10) Letting s z = Z 0 /R (s z > 1)
, one also has .11) This then shows how 1/K can be expressed in terms of χ and of s 2 z , with s z being directly obtainable from image measurements, as shown in [START_REF] Fomena | Improvements on Visual Servoing From Spherical Targets Using a Spherical Projection Model[END_REF] and further discussed in (4.70). Having estimated χ, one can consequently retrieve p 0 = χK and R = p T 0 p 0 -K.

χ T χ - 1 s 2 z χ 2 z = X 2 0 + Y 2 0 + Z 2 0 K 2 - R 2 Z 2 0 Z 2 0 K 2 = 1 K . ( A

A.4 Estimation of the limb surface parameter for a cylindrical target

In order to estimate the parameters of the limb surface associated to a cylindrical object, one could consider as measurement the 2 + 2 angle-distance parameters (θ i , ρ i ) of the straight lines resulting from the projection of the cylinder on the image plane. From [START_REF] Chaumette | Structure from controlled motion[END_REF][START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF], the interaction matrix in this case is given by:

L =       λ ρ 1 c 1 λ ρ 1 s 1 -λ ρ 1 ρ 1 (1 + ρ 2 1 )s 1 -(1 + ρ 2 1 )c 1 0 λ θ 1 c 1 λ θ 1 s 1 -λ θ 1 ρ 1 -ρ 1 c 1 -ρ 1 s 1 -1 λ ρ 2 c 2 λ ρ 2 s 2 -λ ρ 2 ρ 2 (1 + ρ 2 2 )s 2 -(1 + ρ 2 2 )c 2 0 λ θ 2 c 2 λ θ 2 s 2 -λ θ 2 ρ 2 -ρ 2 c 2 -ρ 2 s 2 -1       (A.12)
with s i = sin θ i , c i = cos θ i , and

λ ρ i = -(χ x ρ i c i + χ y ρ i s i + χ z ) λ θ i = χ y c i -χ x s i . (A.13)
Therefore, being (A.12-A.13) linear in the unknown χ, one can again apply the estimation scheme (3.11) with s taken as the vector of measured quantities on the image plane, i.e., s = (ρ 1 , θ 1 , ρ 2 , θ 2 ).

As for the dynamics of χ, since (4.69) still holds for a cylindrical object (see [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]), one can again exploit (A.9) with, however, in this case

ṗ0 = -I 3 -aa T v -[ω] × p 0 and thus χ = - 1 K I 3 -2χχ T I 3 -aa T v -[ω] × χ.
Finally, one can invoke (A.11) in order to express 1/K as a function of χ and s 2 z , with s z being the third element of vector s in (4.81).

A.5 Derivation of equation (4.84)

We note that the cylinder axis a can be determined by the intersection of two planes

P i : n T i X -d i = 0, i = 1, 2, with n 1 = [a] × p 0 p 0 , d 1 = 0, n 2 = - p 0 p 0 , d 2 = p 0 , (A.14)
see Fig. 4.5. In particular, plane P 1 passes through the camera optical center, it is orthogonal to plane P 2 , and both planes contain the axis a passing through p 0 (by construction).

Since Rs = p 0 and p 0 belongs to the cylinder axis a, we have Rn T i s -d i = 0, i = 1, 2 (the point Rs belongs to both planes P i ). Taking the time derivative of these latter constraints (with R = const), one has

n T i ṡ = 1 R ḋi -s T ṅi , i = 1, 2. (A.15) Since ṅi = [n i ] × ω and ḋi = n T i v (see [RDO08]
), eq. (A.15) can be rewritten as

n T i ṡ = 1 R n T i v -s T [n i ] × ω, i = 1, 2. (A.16)
Finally, from a T p 0 = 0 and p 0 = Rs we have a T s = 0 implying that

a T ṡ = -s T ȧ = -s T [a] × ω. (A.17)
We now note that equations (A.16-A.17) provide three linear constraints for ṡ which, by using (A.14), can be rearranged in matrix form as the following linear system

       p T 0 p 0 a T [a] × p 0 T p 0        ṡ = 1 R        p T 0 p 0 v -p T 0 [a] × ω p 0 a T ω + [a] × p 0 T p 0 v        . (A.18)
It is easy to verify that the 3×3 matrix on the left hand side of (A.18) is orthonormal: by then solving (A.18) for ṡ and performing some simplifications we finally obtain the sought result a.e. =⇒ s(t) > 0 a.e., the moving average (6.15) over any non-infinitesimal integration window T ≥ > 0 will necessarily stay positive, thus implying that E(t) > 0.

ṡ = - 1 R I 3 -aa T [s] × u. A.5.1 Proof of Prop. 6.2 Let Φ(t) = [Φ ij (t)] ∈ R 2×2
We conclude with the following remarks: since E(t) > 0 as long as the estimation error has not converged, the adaptive gain k E (E) in (6.18) is also guaranteed to never vanish during the estimation transient (by properly placing, if needed, the minimum threshold E). As a consequence, the optimization of the camera motion (i.e., of σ 2 1 (t)) will always be active during phase 1). We also note that, in general, no special characterization is possible for the behavior of E(t) during the estimation transient (apart from the above-mentioned condition E(t) > 0). Nevertheless, if σ 2 1 (t) ≈ const > 0 during motion, then the error system (3.12) behaves (in its dominant dynamics) as a second-order critically-damped linear system, with χ(t) playing the role of the 'position variables' and s(t) that of 'velocity variables', see Sect. 4.2. In this situation, s(t) 2 (and, thus, E(t) as well) will approximate a 'bell-shaped' profile with a monotonic increase towards a maximum value followed by a monotonic decrease towards zero. By looking at Figs. 7.6(d) and 7.7(d), it is worth noticing that this is indeed the profile followed by E(t) during the active phases of all the reported experiments, since maximization of (6.17) does result (as a byproduct) in an approximately constant σ We now note that, being the feature vector s a measurable quantity, one can always initialize s(t 0 ) = s(t 0 ) resulting in s(t 0 ) = ∅ m . By employing this initialization (adopted in all the reported case studies), and exploiting (4.3-A.27), the following bound easily follows

1 2 s(t) 2 ≤ H( s(t), χ(t)) ≤ H( s(t 0 ), χ(t 0 )) = 1 2α χ(t 0 ) 2 . (A.28)
The proof is then completed by noting that, from standard calculus,

E(t) = 1 T t t-T s T (τ ) s(τ )dτ ≤ max τ ∈[t-T, t] s T (τ ) s(τ ) ≤ χ(t 0 ) 2 α . (A.29)
For the interested reader, this result can be given an interesting energetic interpretation as a consequence of the pH structure of the dynamics (3.8). In this completely determine the two dimensional optical flow vector field. The introduction of priors or regularization terms, usually based on the L 2 or L 1 norms of the optical flow gradient, is therefore necessary, see [BJB94, BSL + 11].

Variational methods can also be used to estimate the disparity map directly from two consecutive images and knowledge of the camera velocity as shown in [START_REF] Zarrouati | Robust depth regularization explicitly constrained by camera motion[END_REF]. Even in this case, regularization terms based on the L 2 or L 1 norm of the disparity map gradient can be used to "fill-in" the areas where the problem is ill-conditioned due to scarcely textured images, and to increase robustness w.r.t. noise.

Rough disparity measurements based on differentiating consecutive images can be incorporated in the calculation of innovation terms in asymptotic incremental depth estimators based on Kalman Filters [START_REF] Matthies | Incremental Estimation of Dense Depth Maps from Image Sequences[END_REF] or deterministic observers [START_REF] Zarrouati | SO(3)-invariant asymptotic observers for dense depth field estimation based on visual data and known camera motion[END_REF]. The prediction, in this case, is based on the known camera velocity. This strategy allows to reduce the effect of noise and discretization errors.

All these methods require the differentiation of consecutive images over time which potentially introduces noise. An improvement in this sense can be obtained by considering a larger number of sampled images when performing the differentiation [START_REF] Barron | Performance of optical flow techniques[END_REF].

Another possibility is to use region-based matching methods [START_REF] Barron | Performance of optical flow techniques[END_REF] that try to find the image displacement that maximizes some similarity measure between the current image and the previous one. Similarity criteria include Sum of Squared Difference (SSD) and mutual information [XMX + 10].

The main difference between the method discussed in this appendix and the ones mentioned above is that we avoid a direct differentiation of the video sequence, but we still keep the calculation local in time (we only consider the last received image in the calculation of the update term) by exploiting a recursive observer similar to the ones used in the rest of the thesis. Our strategy is, therefore, mainly inspired to the one reported in [START_REF] Adarve | A Filtering Approach for Computation of Real-Time Dense Optical-flow for Robotic Applications[END_REF] for the real-time computation of a dense optical flow. By doing so, we expect to obtain an estimation less sensitive to discretization noise, especially at a higher frame rate, but still very efficient (we do not use any batch technique).

The rest of the appendix is organized as follows. First we derive the system of partial differential equations governing the dynamics of the image and the disparity map using a planar (Appendix B.1) or a spherical (Appendix B.2) projection model. In Appendix B.3 we propose an asymptotic observer for estimating the dense disparity map using the photometric information and the camera velocity and we informally discuss observability and stability issues. In Appendix B.4 we introduce a strategy for dealing with the lack of observability in certain areas of the image by introducing an adaptive smoothing term in the estimated disparity map propagation dynamics. Appendix B.5 discusses how to model and propagate depth discontinuities. In Appendix B.6 we provide details on the actual numerical implementation of the observer Partial Differential Equations (PDEs). Finally Appendix B.7 concludes the chapter with comments on current results and future research directions. This work was done in collaboration with Prof. Robert Mahony at the Australian National University branch of the Australian Research Council Centre of Excellence for Robotic Vision1 . We also wish to thank Juan David Adarve for his fruitful suggestions.

B.1 System dynamics with planar projection

As discussed in the introduction of this chapter, we now consider as measurement the luminance Y (t, π) of a pixel located at a point π (in homogeneous coordinates) at a time t. This is a scalar mapping:

Y : R × P 2 → R + , (t, π) → Y (t, π).
The luminance map represents the flux of the (light) electro-magnetic power, at time t, through an infinitesimal area region of the image plane centered in π. The value of Y (t, π) is determined by both geometric (shape and position of the objects at time t) and physical (absorption, reflectivity and transmissivity of the materials as well as position and intensity of light sources) properties of the environment. The exact physics of the image formation process is extremely complex to model but different approximations have been proposed in the literature. In particular, we make the assumption that the materials in the scene obey a Lambertian reflectance model [START_REF] Lambert | Photometria Sive de Mensura et Gradibus Luminus, Colorum et Umbrae. sumptibus viduae Eberhardi Klett[END_REF][START_REF] Basri | Lambertian reflectance and linear subspaces[END_REF], i.e., they absorb and reflect (part of) the light that hits their surface with equal (isotropic) intensity in all directions. This simple model approximates reasonably well the physics of image formation for "matte" objects and has been used in many vision applications. Finally, we assume that the environment is static. A direct consequence of these choices is the constant brightness assumption

d dt Y (t, π) = 0, (B.1)
that essentially states that, as the image of the environment moves (due to camera motion), its brightness does not change, see, e.g. [START_REF] Horn | Determining Optical Flow[END_REF]. We are interested in the way the intensity changes, in a fixed position on the image, due to camera motion, i.e. in the quantity:

lim ∆t→0 Y (t + ∆t, π) -Y (t, π) ∆t = ∂Y ∂t . (B.2)
Using (B.1), one can write:

dY dt = ∇ π Y T π + ∂Y ∂t = 0 ⇐⇒ ∂Y ∂t = -∇ π Y T π (B.3)
If π is associated with a point of constant luminance, then it follows that its time derivative is determined by the optic flow Φ (see (2.10))

π = Φ(t, π) = Ψ(t, π) + Θ(t, π) = 1 Z(t, π) ψ(t, π) + Θ(t, π) = ζ(t, π)ψ(t, π) + Θ(t, π) (B.4)
where Z(t, π) is the depth map, ζ(t, π) is its inverse, also called the disparity map, and Θ(t, π) and ψ(t, π) are, respectively, the angular and scaled linear components of the optical flow, given by (see, again, (2.10)):

ψ(t, π) = [e 3 ] × [π] × v(t) Θ(t, π) = -[e 3 ] × [π] 2 × ω(t). (B.5)
The depth and disparity maps must be considered as scalar functions

Z : t × P 2 → R + , (t, π) → Z(t, π) ζ : t × P 2 → R + , (t, π) → ζ(t, π),
constituting an ego-centric representation of the environment: for each direction π at each time t there is a ray radiating out from the optic center that intercepts the closest point of the environment in the 3-D point p(t, π) = Z(t, π)π. Substituting (B.4) in (B.3) one finally obtains

∂Y ∂t = -∇ π Y T Θ(t, π) -ζ(t, π)∇ π Y T ψ(t, π). (B.6)
Now let us compute, similarly to (B.2), how the disparity map changes in time, in a fixed position on the image plane, due to camera motion, i.e. the quantity:

lim ∆t→0 ζ(t + ∆t, π) -ζ(t, π) ∆t = ∂ζ ∂t .
We start by noting that, as already discussed (4.27),

dZ dt = -e T 3 v + Ze T 3 [π] × ω
and hence we can write

dζ dt = - 1 Z 2 Ż = ζ 2 e T 3 v -ζe T 3 [π] × ω. (B.7)
Then, analogously to the case of the luminance, we can write

dζ dt = ∇ π ζ T π + ∂ζ ∂t , (B.8)
and, comparing (B.8) with (B.7) and introducing (B.4), we conclude:

∂ζ ∂t = -∇ π ζ T Θ(t, π) -ζ(t, π)∇ π ζ T ψ(t, π) + ζ(t, π) 2 e T 3 v(t) -ζ(t, π)e T 3 [π] × ω(t)
Dropping function arguments, we finally write the system dynamics as:

     ∂Y ∂t = -∇ π Y T Θ -ζ∇ π Y T ψ ∂ζ ∂t = -∇ π ζ T Θ -ζ∇ π ζ T ψ + ζe T 3 ζv -[π] × ω . (B.9a) (B.9b)

B.2 System dynamics with spherical projection

A similar strategy can be applied to the spherical projection model. In this case we consider as measurement the (scalar) luminance Y (t, η) of a pixel located in η (in spherical coordinates) at a time t. Again, this is a scalar mapping:

Y : R × S 2 → R + , (t, η) → Y (t, η).
Following identical steps to the planar projection case, one can use the constant brightness assumption (B.1), the dynamics of the inverse range map δ = 1/ p in (4.34) and the spherical version of the point feature interaction matrix (2.11)

to show that        ∂Y ∂t = -∇ η Y T Θ -δ∇ η Y T ψ ∂δ ∂t = -∇ η δ T Θ -δ∇ η δ T ψ + δ 2 η T v. (B.10a) (B.10b)
where Θ(t, η) and ψ(t, η) are, respectively, the angular and scaled linear components of the optical flow, given by (see, again, (2.11)) In particular, for the planar projection case (similar results can be found for the spherical one), defining

ψ(t, η) = -I 3 -ηη T v(t) Θ(t, η) = [η] × ω(t).
       f s (s, ω(t)) = -∇ π s T Θ(ω(t), π) Ω(s, v(t)) = -∇ π s T ψ(v(t), π) f χ (ζ, v(t), ω(t)) = -∇ π ζ T Φ(v(t), ω(t), π) + ζe T 3 ζv(t) -[π] × ω(t) , (B.12)
we can rewrite the system equations (B.9), as2

     ∂s ∂t = f s (s, ω) + Ω(s, v) T ζ ∂χ ∂t = f χ (ζ, v, ω) (B.13)
and devise an observer as:

     ∂ s ∂t = f s (s, ω) + Ω(s, v) T ζ -h s ∂ χ ∂t = f χ ( χ, v, ω) -αΩ(s, v) s. (B.14)
where h and α are positive gain (maps). Following the developments of Sect. 4.3, the gain h can be chosen as:

h(t) = h(π, v) = 2|Ω(π, v)| (B.
d(π, ζ, ζ, v, ω) = f χ (π, ζ, v, ω) -f χ (π, ζ, v, ω) = -∇ π ζ T Θ -∇ π ζ T ζ -∇ π ζ T ζ ψ + ζ 2 -ζ 2 e T 3 v -ζ [π] × ω = -∇ π ζ T Θ - 1 2 ∇ π ζ 2 -ζ 2 T ψ + ζ 2 -ζ 2 e T 3 v -ζ [π] × ω.
Due to the presence of some gradients in this expression, for the disturbance d to be vanishing, one needs the estimation error ζ(π) → 0, to go to zero everywhere, i.e. for all image points π. In other words, the presence of spacial derivatives (disparity map gradients) makes the estimation error on a certain image point π have some effects on a neighbourhood of π. As a consequence, we expect the disturbing term to have a more significant impact on the estimation performance w.r.t. the finite dimensional cases considered in the rest of this thesis.

Remark B. [START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF]. By looking at the PE

1 × 1 matrix Ω(s, v)Ω(s, v) T = σ 2 (s, v) = (∇ π s T l v v)
2 , one can notice the following (intuitive) facts:

• one cannot estimate the depth map if the camera does not translate ( v = 0);

• one cannot estimate the depth map in areas of the environment that are not sufficiently textured ( ∇ π Y ≈ 0);

• the "informative" camera velocities are those that make the image move in the direction of the image gradient, i.e. orthogonally to the luminance level sets (the objects contours);

• if one has control over the camera linear velocity v, an optimization strategy, similar to the ones used for the other SfM case studies, can be devised to maximize observability. Note however that care must be taken to ensure that the optimization does not attempt to increase "excitation" for areas of the image that are scarcely textured (this would happen, e.g. if one tried to naively maximize min π σ 2 (t, π)) and therefore are intrinsically less observable. In this context it would probably make more sense, instead, to try to maximize the excitation level for the highly textured areas and then rely on some regularization term, as those discussed in Appendix B.4, to "fill-in" the untextured parts. One also needs to make sure that the entire image is sufficiently excited at least for a short time. In this context, finite horizon planning techniques would probably be more suited than the instantaneous greedy optimization strategy used for the other case studies. The image gradient ∇ π s was calculated exploiting the algebraic model of the parabolic texture where as the gradient of the estimated disparity map ∇ π χ was approximated using first order upwind finite differences, see [START_REF] Thomas | Numerical partial differential equations: finite difference methods[END_REF]. Finally we used a simulation time-step of 1 ms.

X w [m] 0 -1 -1 0 Y w [m]
As predicted by the theory, in the first part of the trajectory (Fig. B.1(b)), the observability index σ 2 (second series of plots) is different from zero only on the left and right side of the uniform region. In fact(i ) in the central area the image gradient is zero, and (ii ) in the top and bottom areas the image gradient is orthogonal to the direction of the linear optical flow ψ. As a result only in the observable regions the error correctly starts converging towards zero. In the second quarter of trajectory (Fig. B.1(c)), the camera changes direction of motion by 90 deg and, consequently, now the areas on the top and on the bottom of the uniform region are "excited" (see σ 2 in the second series of plots) and the error starts converging there too. The areas on the left and right side of the uniform region are not observable in this case because the image gradient, in these areas, is orthogonal to the direction of the linear optical flow ψ. Thanks to the practically ideal conditions of the simulation (e.g. absence of noise), however, the error does not significantly grow. In the second half of trajectory the above process is repeated again and, if one kept using this trajectory, for this particular texture, almost the entire image would be observable for at least half of the time. Nevertheless due to the uniformity of the central area, the geometry of the environment is never observable in this region and by no meas one can expect to ever obtain a correct estimation of the depth in this part, unless some additional prior is considered. This is, in fact, the topic addressed in the next Appendix B.4. along luminance level contours). In such areas the estimation will be undetermined and the observer will be highly affected by noise and other unmodeled effects. To overcome this problem it is necessary to introduce some smoothing/regularization based on priors on the underlying 3-D structure of the scene. Some regularization could also be introduced, if desired, in the areas with high level of information to increase robustness w.r.t. noise and discretization effects. More in general, the amount of regularization can be locally fine tuned based on the value of σ 2 in each pixel position.

One way of addressing the regularization problem, is to model it as a heat diffusion process (see, e.g. [START_REF] Widder | The heat equation[END_REF]): intuitively the information about the disparity map ζ should be "diffused" from the areas in which the estimation is well defined (that can be thought of as "sources of disparity", similarly to heat sources), to those in which it is unobservable. This is similar to the way heat is diffused in an object from some sources, displaced in certain positions, to the rest of the material. As well known the two-dimensional heat diffusion, in absence of sources, is described by the parabolic PDE

du dt = q∇ 2 (x,y) u (B.16)
where u(x, y) is the temperature map and q > 0 represents the material thermal diffusivity. Equation (B.16) converges to an equilibrium state in which, In the assumption that the band is infinitely thin, one can neglect the forces associated with the torsion of the band and only consider the elastic forces due to stretching. In other words the band can be modeled as a sequence of elementary springs connected to each other. As well known the potential energy of a spring is associated with its length and thus, intuitively, the equilibrium position will correspond to the minimum length of the band which is obtained when the band is straight. As a matter of fact, let us assume that the band is curved as in the red line in Fig. B.2 and let us call u(x) the amount of deflection from the straight configuration. If the deformation is small, the length of an elementary component of the band going from x to x + ∆x is given by:

∆L ≈ [u(x + ∆x) -u(x)] 2 + ∆x 2 ≈ [u(x) + u x ∆x -u(x)] 2 + ∆x 2 = ∆x 2 √ 1 + u x 2
where u x is the derivative of u w.r.t. x. For ∆x → 0, and summing the contribution of each elementary term, one concludes:

L ≈ l 0 1 + u 2 x dx.
The difference of potential energy w.r.t. the equilibrium configuration is proportional to the change in length

L -l = l 0 1 + u 2 x -1 dx ≈ 1 2 l 0 u 2 x dx
where we used a Taylor series expansion to the first order of 1 + u 2 x . One can then conclude that the potential energy is given by:

U = 1 2 q l 0 u 2 x dx
where q is now the elastic coefficient of the band.

The two-dimensional equivalent of the elastic band is the thin membrane under tension (see [START_REF] Courant | Methods of mathematical physics[END_REF]). In this case one can assume the potential energy to be proportional to the surface area of the membrane which can be computed as

S 1 + u 2 x + u 2 y dx dy ≈ S 1 + 1 2 (u 2 x + u 2 y ) dx dy
where S represents the membrane domain. Apart from a constant factor that can be ignored, the potential energy is then given by

U = 1 2 q S ∇ (x,y) u 2 dx dy. (B.18)
The equilibrium configuration for the membrane is given by the deformation u(x, y) that minimizes the potential energy (B.18) and satisfies additional boundary conditions assigned on the boundary of S. It can be shown (see [START_REF] Courant | Methods of mathematical physics[END_REF]) that finding such minimum is equivalent to solving the boundary value problem for the PDE (B.17) with the assigned boundary conditions. One can conclude that, if (B.16) were used as an additional term to update the estimated disparity map, the solution would tend to a membrane-like shape of the depth map: in the observable regions the value of ζ would be imposed by the estimation algorithm; in the non observable regions the disparity map ζ would tend to the minimum surface solution which is the minimum curvature interpolation of the conditions imposed at its boundaries (by the observable areas). This regularization term, then, has similar effects to the classical minimization of the L 2 norm of the disparity map (or, equivalently, of the optical flow) gradient exploited in many other works, e.g. [START_REF] Horn | Determining Optical Flow[END_REF]. The surface with minimum curvature is the plane and therefore the solution will tend, as much as possible and compatibly with the boundary conditions, to be linear in π. Note that, assuming a linear ζ in π (or equivalently a linear δ in η) is equivalent to assuming that the unobservable object is a plane in the 3-D space. In fact, as shown in (4.45), the variation of ζ for a planar object is linear in the image coordinates.

As for the regularization gain q, it would make sense to construct it as a function of σ 2 in such a way that in areas with more information a good match between the estimation and actual geometry is obtained and, conversely, smoothness is favoured in areas with poor information. One possibility would be, e.g.,

q(t, π) = sat 1 σ 2 (t, π) = q(π, v, ∇ π Y T )
where sat refers to a generic monotonic and saturated function of the argument. This choice might however result in a strongly time-varying q. A better solution might be to use an averaged value of σ 2 over a finite time window.

q(t, π) = sat 1 1 T t t-T σ 2 (τ, π)dτ
Another problem arises when the camera does not translate ( v = 0). In this case, obviously σ 2 = 0 everywhere and the regulation would be active on the entire disparity map thus making the estimation converge to a solution that is "flat" everywhere. To avoid this, one could also consider the norm of the camera linear velocity in the calculation of q, e.g.:

q(t, π) = sat v σ 2 (t, π) = q(π, v, ∇ π Y T ).
Finally, from a computation point of view, it might be convenient to use |Ω| = √ σ 2 , instead of σ 2 , since this is less expensive to compute and it is already involved in the calculation of h in (B.15). Adding this kind of regularization term to (B.8) one obtains a new observer in the form:

     ∂ s ∂t = f s (s, ω) + Ω(s, v) ζ -h(t) s ∂ χ ∂t = f χ ( χ, v, ω) -αΩ(s, v) s + q(t, π)∇ 2 π χ.
q(t, π) = 200 v 1 h + 1 ,
where h is computed as in (B.15). With this choice, we clearly have q ≈ 200 if h 1 and q ≈ 0 if h 1. The evolution of the smoothing gain q during the simulation is reported in the central plot sequences of Figs. B.3(a) to B.3(c). Note how this is "complementary" to the observability index σ 2 in Figs. B.1(b) to B.1(d). The use of the regularization term allows to(i ) increase the converge rate of the areas that are only temporarily (50% of the time) observable and, more importantly, (ii ) attain convergence in the central uniform area. Note, however, that the exact convergence, in this central area, is only possible because the observed scene, in this simulation, is actually planar in this region. For a general 3-D structure, the estimation error will not converge to zero in unobservable areas as expected from Remark B.3.

Before concluding this section, we briefly note that this solution could also be extended to operate an anisotropic regularization by using a smoothing term in the form:

(∇ π T Q∇ π ) χ
where Q ∈ R 2×2 is a symmetric gain. This should allow, e.g., to smoothen the disparity map only (or mainly) in the direction orthogonal to edges/discontinuities.

B.5 Propagation of depth discontinuities

Equations (B.9-B.10) represent the correct evolution of the system only under the assumption that all involved derivatives are defined and continuous. In the general case, it is hard to make any smoothness assumption on the luminance Y since the image texture can present discontinuities (contours). However, in principle, one might be able to apply the same model to a low pass filtered version of the input image, thus assuring continuity of the inputs. Moreover, in the actual implementation of the observer, the continuous space gradients are substituted by discrete numerical differences which will, in fact, induce some low-pass filtering effect.

The satisfaction of the continuity and differentiability assumptions for ζ, instead, depends on the convexity of the observed scene. As it is shown in Fig. B.4(a), the presence of an occlusion will, in fact, induce a discontinuity in Z from Z h to Z l and hence in ζ from ζ h = 1/Z h to ζ l = 1/Z l . In general we can expect ζ(π, t) to present some jump discontinuities in presence of occlusions. Note that, differently from Z(π, t), which can grow to infinity for far objects, if we assume that Z(π, t) > 0, the disparity map ζ(π, t) will always remain limited, therefore ζ(π, t) will not present asymptotic discontinuities.

To understand how to model the evolution of ζ in presence of such discontinuities, it is convenient to restrict our attention to a simpler one dimensional case: let us assume that v = (v x (t), 0, 0) and ω = (0, 0, 0) with then π = (-ζv x , 0, 0). Let us also consider the behavior of ζ(π, t) along the image line π = (x, 0, 1), i.e. the horizontal line passing through the center of the image. In this case (B.9b) can be rewritten as:

∂ ∂t ζ(x, t) -v x (t)ζ(x, t) ∂ ∂x ζ(x, t) = 0. (B.20) If ζ ∈ C 1
, the equation can also be rewritten as:

∂ ∂t ζ(x, t) + ∂ ∂x f (ζ(x, t), t) = 0. (B.21) with f (ζ(x, t), t) = - 1 2 v x (t)ζ(x,
V e Z h V o Z l (a) V e Z h V o Z l (b)
(t) = f (ζ(X(t), t)) = v x (t)ζ(X(t), t) (B.25) one has ζ(X(t), t) = 0. Equation (B.25) is called the characteristic curve of the PDE. If v x (t) = v x = const, since ζ is constant along the curve X(t)
, then, from (B.25), also Ẋ(t) is constant and therefore the characteristic curve is a straight line in the plane (x, t) starting from X(0) and with slope v x ζ(X(0), 0). If v x is timevarying, instead, then the characteristic lines are generic curves described by the integral of (B.25) over time, but one always has ζ(X(0)+ t 0 Ẋ(τ )dτ, t) = ζ(X(0), 0). In a linear conservation law one has f (ζ, t) = a(t)ζ + b(t) and hence all the characteristic curves, starting from any point X(0), are parallel since Ẋ(t) = a(t) does not depend on X. In this case, therefore, the solution ζ is simply translating in space-time. If a is also constant, one simply has ζ(x, t+T ) = ζ(x-aT, t). In our case, however, we have f = f (ζ(x, t), t), i.e. the flux derivative depends on ζ. Assuming, for simplicity v x = const, for each X(t), the characteristic lines will still be straight, but with a (possibly) different slope, determined by v x ζ(X(0), 0). In other words, in the nonlinear case, the characteristic lines are not all parallel, and, as a consequence, they can diverge or intersect each other. To understand what happens in this case, it is convenient to consider a more intuitive physical example sharing the same dynamics of our case. Equations (B.21-B.22), with constant v x , are a particular example of the so called inviscid Burgers' equation [START_REF] Burgers | The nonlinear diffusion equation : asymptotic solutions and statistical problems[END_REF], a simplification of the Navier-Stokes equation describing the dynamics of a free incompressible fluid when neglecting pressure. In this case, the average momentum of fluid particles ν in a space region, only changes, over time, due to the difference between the kinetic energy f = [START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF][START_REF] Thomas | Numerical partial differential equations: conservation laws and elliptic equations[END_REF]. As already mentioned, due to the nonlinearity, the characteristic curves are not parallel since Ẋ(t) = ν(X(t)). The solution of such equation does not simply translate uniformly in space-time but, instead, it deforms. In particular, since the characteristic curves are not parallel they can converge to the same position or diverge from a point in the plane (x, t). These two phenomenons are called shock and rarefaction waves respectively. This picture is similar to some reported in [START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF]. .

B.5.1 Rarefaction waves

One can notice that these curves diverge from the point x = 0, t = 0.

This effect is called a rarefaction wave. It presents itself whenever the camera is moving in the direction that reduces an occluded region as it is the case in We stress the fact that there is not much one can do to fix this situation: since the information about the occluded area is not present in the initial condition, it is impossible, in general, to correctly reconstruct the depth map in the new camera configuration by simply integrating the initial depth map. Given these premises, we can accept the natural hypothesis produced by the simple propagation of the initial condition as a plausible solution for the actual depth map, and rely on the innovation part of the observer for improving the depth map if such hypothesis turns out to be wrong.

To give an example, in Fig. B.6 we report some simulation results for the propagation (α = h = q = 0 in (B.19)) of a disparity rarefaction wave. For this simulation we considered a one-dimensional image and we constrained the camera to move only on the (X, Z) plane in the camera reference frame. The simulated camera had a resolution of 640 px and intrinsic parameters f d x = 300, j o C = 319.5 in (2.4). Finally, we used an integration time step of 0.002 s. The camera is observing a simple environment that contains a single jump discontinuity, see the gray area in ) with α = h = q = 0 from an initial value of χ(π, 0) = χ(π, 0). As a numerical scheme, we opted for an upwind finite difference method (see [START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF]) using the conservative version of the PDE for χ, i.e. ∂ χ ∂t = -∂ ∂x

1 2 v x χ 2 .
As expected from the developments of this section, the propagation maintains the initial hypothesis about the structure of the environment in the area that was occluded in the first frame (the one used for initialization). The environment reconstruction (solid lines in with the fact that, in these conditions, one obtains, from (B.7), dζ dt = 0. The only small (but rather important) variation is due to the dissipation introduced by the numerical differentiation which smoothens the depth map at each iteration. Obviously such a solution is not physically acceptable for the particles dynamics case: fluid particles can only have one momentum in each position. When modeling physical systems, such non-physical solutions in general appear when some viscosity is neglected in the modeling phase. Moreover, the use of a numerical scheme to integrate (B.21) will always introduce some level of numerical diffusion or dissipation which is due to the approximation of the derivatives with finite differences. Finally, we have already commented that some diffusion term can be added to smoothen the depth map, especially in the areas that are scarcely observable. The PDE that one ends up solving is then: with > 0. Equation (B.28) is also called the viscous form of the differential equation and a solution to (B.28) for → 0 is called a vanishing-viscosity solution for (B.26), see [START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF]. Using the vanishing-viscosity approach, it can be shown that the correct solution can be obtained from the nonphysical multi-valued one buy applying the equal area rule: this rule is a consequence of the conservation law and imposes the physical solution to have a discontinuity in a position such that the area "under" the multi-valued solution is the same as the one under the singlevalued one or, equivalently, that the areas added (violet) and removed (orange) from the nonphysical solution are equal. A direct consequence of this is that the discontinuity will travel at a velocity v s given by the average between the velocities (the slope in the plane (x, t)) of the characteristic lines to the right and to the left of the discontinuity. This last result can also be obtained by looking at the integral of the conservation law (B.23) as shown in [START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF]. Suppose that the shock wave is moving at a constant velocity v s from x 1 to x 1 + ∆x in a time ∆t (see Fig. In the camera projection system (B.9), shock waves appear whenever some part of the environment is occluded as it is the case in We note that the introduction of this term requires, in principle, the identification and tracking of the shock-type depth discontinuities which would make the implementation more complex and less suited for parallelization. However, since the additional term is proportional to the squared first order difference between the disparity levels of neighbour pixels (the ones on each side of the discontinuity), its effect should be negligible in the areas in which the disparity map is smooth. Therefore we expect that introducing this additional source term everywhere in the image (and not only on the sides of a discontinuity of the disparity map) should not affect the propagation in a unacceptable way, with, on the other hand, significant advantages from the point of view of the simplicity and parallelizability of the algorithm. The only problematic areas, could be the rarefaction-type occlusions that present depth discontinuities but do not require the addition of this source term. This areas, should, however, be easy to recognize since one has, for them, ∂ζ ∂t = -∂f (ζ) ∂π ≤ 0 (i.e. the disparity map does not grow in these region) and checking this condition requires, again, only local information.

B.5.2 Shock waves

∂ ∂t ζ(x,
We demonstrate the effects of this additional source term by running a similar simulation as in We want to stress that the fact that the source term (B.30) seems to only work correctly when the discontinuity moves by exactly one pixel per iteration is a significant limitation to its use. In fact, in general, the observed scene will contain more that one shock-type depth discontinuities each moving at different speeds and, therefore, we cannot expect this condition to be realized in practice. The reasons why this happens are still not clear, but we suspect that this might be related to numerical discretization.

B.6 Notes on the numerical implementation

In an actual implementation, the continuous time observer equations of (B.14) or (B.19) are approximated using a numerical resolution scheme. The discussion about the choice of the particular method to use could take an entire thesis, and, in fact, many books have been written on the topic, see again [START_REF] Thomas | Numerical partial differential equations: finite difference methods[END_REF][START_REF] Thomas | Numerical partial differential equations: conservation laws and elliptic equations[END_REF][START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF]. We are, in particular, interested in finite difference and finite volume methods for their potential to be implemented in a highly parallel architecture. To give just a simple example, a naive numerical scheme for integrating (B.14) or (B.19) could be obtained by using first order central differences in space and forward differences in time to approximate all the derivatives. Considering just a one dimensional case one could then write, e.g., for the first row of (B.14)

s k+1 i -s k i δt = - s k i+1 -s k i-1 2δx (Θ k i + ψ k i ζ k i ) -h s k i
where s k i = s(x = x k , t = t k ) (and similarly for the other quantities) and δx and δt are the spatial and temporal discretization steps respectively. One can then "isolate" the new estimation as

s k+1 i = s k i + - s k i+1 -s k i-1 2δx (Θ k i -ψ k i ζ k i ) -h s k i δt.
Note how the amount of calculation required is always constant at each iteration and it is identical for each pixel. Moreover, for each pixel, the calculation only requires to access the observer state and the measurements in a small neighbourhood of the pixel (in this case only the pixels to the left and to the right) and only at the time t k . This properties make it possible to envision an implementation of this technique on highly parallel computer architectures such as GPUs and FPGAs, with potentially extremely high performance, see [START_REF] Adarve | A Filtering Approach for Computation of Real-Time Dense Optical-flow for Robotic Applications[END_REF].

In general, an important aspect to consider is that the stability of numerical methods for solving PDEs depends, in general on the relationship between the spatial and temporal discretization steps. A large class of methods, e.g., becomes numerically unstable if the characteristic curves move by more than one pixel per iteration, i.e. in the one-dimensional case, if

Ẋ = f > δx δt .
Increasing δx introduces a larger discretization error thus affecting the overall accuracy. It might seem obvious that δx (and similarly for δ y ) should correspond to the actual camera pixel distances, however the spacial resolution can be both increased and reduced at will with some effects on the performance of the estimation. On the other hand, reducing δt clearly has a positive impact on the stability at the cost of a higher computational effort. Note that, as for the spatial one, the time step for the time discretization does not have to correspond to the camera frame rate.

In fact, depending on the camera velocity and on the distance of the environment (which enter in the calculation of f ), the time ∆t passing between two frames might be considerably larger than the minimum integration time step necessary to obtain a stable numerical scheme. Given these considerations it is useful to distinguish χ k+1 = χ k+ -α∆T Ω(s ? , v) s k+ -s k+1 .

(B.32)

As highlighted by the presence of the question mark s ? in (B.31-B.32), a problem that one faces when implementing (B.31-B.32) is that of choosing which image s to use for the calculation of f s and Ω (both depend, in fact, on the gradient of s). If the latest measurement s = s k = Y k is used and the integration time step for (B.31) is smaller than the camera time step, the risk is that, during the "inner" integration steps of (B.31), s is not aligned with the other quantities 3 . One possible solution would be to propagate the last measurement Y k together with the observer state during the propagation part of the estimation. However this implies a considerable computational burden. Another possibility would be, instead, to use the estimated image s = Y in the calculation of f s and Ω. To show the effect of this choice more clearly, we prefer to consider again the fully continuous observer dynamics (B.14) and to write the explicit expression of the different quantities. The observer dynamics, for the planar case, would hence be written as

               ∂ Y ∂t = -∇ π Y T Θ -∇ π Y T ψ ζ -h Y ∂ ζ ∂t = -∇ π ζ T Θ -∇ π ζ T ψ ζ + α∇ π Y T ψ Y + ζe T 3 ζv -[π] × ω + q∇ 2 π ζ,
and the error dynamics would become

               ∂ Y ∂t = -∇ π Y T Φ -h Y -∇ π Y T ψ ζ ∂ ζ ∂t = -∇ π ζ T Φ + α∇ π Y T ψ Y + q∇ 2 π ζ -∇ π ζ T ψ ζ -ζ [π] × ω + ζ 2 -ζ 2 e T 3 v. (B.34a) (B.34b)
The expression (B.34) presents some intuitive interpretations whose actual implications are still to be investigated. First of all we can still recognize a skew-symmetric structure with dissipation term h as in the other cases, but now with the coupling matrix being Ω( s, v) = ∇ π Y T ψ instead of Ω(s, v). Secondly we can notice the presence of an additional term in the form

       ∂ Y ∂t = -∇ π Y T Φ ∂ ζ ∂t = -∇ π ζ T Φ .
This term is a linear (Φ depends on ζ and not ζ) variable-coefficient convection equation (see [START_REF] Thomas | Numerical partial differential equations: finite difference methods[END_REF]) for the image and the disparity map that should not either reduce or increase the error but just "move it around" with a velocity determined by the optical flow Φ.

Another important effect of the use of a numerical resolution scheme is the, already mentioned, introduction of undesired numerical dissipation/smoothing effects. We have already seen this phenomenon in the propagation of the depth map in Fig. B.6. More important, however, are the effects on the propagation of the image s. As a matter of fact, due to numerical damping, the prediction error s will not only reflect the depth estimation error, but it will also be due to numerical effects. This clearly affects the effectiveness of s in the update phase. We believe that, since only the low frequency components of s are preserved during the propagation (which tends to damp the high frequency ones), these are the only ones that should be relied on during the innovation step. One possible solution that we envision, then, is to apply a low-pass filtering action to the prediction error s before calculating the update term (B.32) of the filter. The optimal shape and pass-band of such filter, however, are still to be determined. Finally it might be worth investigating the use of alternative metrics for the discrepancy between s and s such as, e.g. the mutual information successfully exploited in [START_REF] Teuliere | Direct 3D servoing using dense depth maps[END_REF] for visual control purposes.

B.7 Conclusions

In this appendix we reported some preliminary results in the context of dense photometric structure estimation from motion. We modeled the system dynamics with a system of PDEs and we proposed an observer (also in the form of a system of PDEs) characterized by a structure reminiscent of the one used for the other geometric primitives presented in this thesis. We also underlined, however, some important differences between this case and the other sparse estimation problems addressed in this work. Due to the infinite dimensionality of the problem, for instance, one cannot simply extend the finite dimensional case stability proof to this infinite dimensional case. We also discussed some intuitive observability properties of the system that are similar to those characterizing the other SfM problems. After this, we proposed a physically inspired regularization strategy that can improve the estimation in areas that are scarcely observable, especially if the environment is planar in these areas. We then pointed out the limitations of this modeling and estimation strategy when dealing with occlusions and consequent depth discontinuities. Finally we briefly discussed some practical considerations that might suggest the use of some alternative observer structure (use of the estimated image in the propagation phase and low-pass filtering of the prediction error).

At the moment, we have obtained promising results for simple simulated conditions, but we still lack a proper validation in more realistic scenarios. However, we believe that our approach has some potential and we are working towards the resolution of the current issues. which is the canonical Hamiltonian equation for a mechanical system, see e.g. [START_REF] Fasano | Analytical mechanics: an introduction[END_REF]. Note that matrix S = 0 1 -1 0 = -S T , also called the system structure matrix, is skew-symmetric. Matrix B is, instead called the input matrix. This structure has important consequences on the system energy balance as it will be explained in the following.

If the system also contains some viscous friction, then one has where x is an element of a q-dimensional manifold X , S(x) = -S(x) T , R(x) = R(x) T 0, u ∈ R v is a control input and y ∈ R v is the system output.

As already mentioned, these systems have very interesting energy balance properties. To show this, let us compute the dynamics of H(x) for a system in the form (C.5). One easily finds that Ḣ(x) = ∇ x H(x) T ẋ = -∇ x H(x) T R(x)∇ x H(x) + y T u which, integrated back, gives H(x(t)) = H(x(t 0 )) - i.e. the amount of energy that can be extracted from the system in the interval [t 0 , t] (represented by the last term) cannot be larger than the initial amount of energy, H(x(t 0 )), contained in the system at time t 0 . In other words, if H is lower bounded, the pH system (C.5) does not internally generate energy and, therefore, it is said to be passive. The couple (u, y) can be used to inject or extract energy from the system and, for this reason, it is also called a power port.

Passivity is a very important concept in control theory because it guarantees some stability properties: intuitively, if a system cannot generate energy on its own, then, if the external input u is set to zero, the system will either start oscillating or (if R 0) it will converge to an equilibrium, but the system trajectories will never diverge. This intuitive fact can be formally demonstrated using Lyapunov's analysis.

While passivity gives an overall energy characterization of a system from an input-output point of view, the pH framework provides more insights also on the internal structure of a system and on how the energy is actually exchanged between its components. To show this, let us consider again the simple mass-spring system. This latter can also be thought of as an interconnection between two different subsystems (the mass and the spring) connected in a "feedback" configuration (see Note that both K and U have a pH structure (C.5) with x U = x and x K = p respectively, and with S U = S K = 0, R U = R K = 0 and B U = B K = 1. These two systems represent two energy storage elements associated with the potential and kinetic energies of the mass-spring system given by H U and H K respectively. The full system energy is given by the sum of these two contributions and its dynamics is Ḣ(x, p) = ∂H ∂x ẋ + ∂H ∂p ṗ = ∂H ∂p e = ẋe and thus the mass-spring system is passive, as expected, and it can exchange energy with the environment through the power variables (e, ẋ). The two subsystems also have their own power ports, in fact:

ḢU = ∂H U ∂x ẋ = y U u U , ḢK = ∂H K ∂p ṗ = y K u K
and, through them, they can exchange energy between each other in a conservative way (due to passivity, no energy can either be created or destroyed "inside" the mass-spring system). This internal energy exchange is regulated, in fact, by the power-preserving feedback connection

u U u K = 0 1 -1 0 y U y K + 0 e ,
between the two systems, that results in the skew-symmetric structure matrix S in (C.3).

When modeling complex physical systems one often ends up with a system of both differential and algebraic equations (DAEs). These can be modeled in the pH framework by replacing the structure matrix S(x) with a more general Dirac structure defined as follows. Let F be a finite-dimensional vector space, called the flow space and let F * be its dual space (i.e. the space of linear functions on F), called the effort space. In general the state space of the pH system X is a manifold and the flow space is its tangent space T x X at x where as the effort space is the cotangent space T * x X . The space of power variables is given by the Cartesian product between the two F × F * where the power is defined by the duality product between efforts and flows:

W = f |e , f ∈ F, e ∈ F *
In table C.1 we report a list of power variables for some typical modeling domains. On F × F * one can define the canonical symmetric bilinear form , F×F * as (f 1 , e 1 ), (f 2 , e 2 ) F×F * = f 1 |e 1 + f 2 |e 2 , f i ∈ F, e i ∈ F * Given a subspace D ∈ F × F * , its orthogonal complement w.r.t. the bilinear form is defined as: In the most general case, then, a pH system can be represented formally, as a Dirac structure with four power ports (see Fig. S is interconnected to internal energy-storage elements. The energy is defined by the Hamiltonian function H : X → R and the power balance can be written as Ḣ = -f S |e S ;

D ⊥ = (f 1 , e
R is interconnected to internal energy-dissipation elements. The power variables at this port satisfy the constraint f R |e R ≤ 0;

C is interconnected to the control action;

I is the port associated with the interaction of the system with its environment.

The interest in pH systems comes from the fact that they have some modularity properties: as shown for the mass-spring example, an interconnection between two pH systems through a Dirac structure is, again, a pH system with total energy (the Hamiltonian function) given by the sum of the energies of the two subsystems. Furthermore, if its Hamiltonian function H is lower bounded, then a pH system is passive. Conversely, most passive systems can be written in the pH form.

Further generalizations of the pH systems are also possible. In particular, the concept can be extended to infinite dimensional systems governed by algebraic and partial differential equations. We believe that this could be useful to analyze the stability of dense structure estimation schemes as those described in Appendix B, but, at the moment, this is more of a conjecture. For more information about pH systems we suggest, again, to refer to [van06].

C.2 Bond-graphs

A convenient graphical representation of pH systems, that highlights their network properties, is that of bond-graphs, see Fig. C.3. In these graphs each system element is represented by a node and the links between different nodes (called bonds) represent, in general, power connections. The power variables associated to each bond are usually written in the middle of the bond with the effort variable conventionally on the top (or left) and the flow variable on the bottom (or right). The direction in which the power flows positively through the bond is indicated by a half-arrow pointing in the direction of the flow variable. Finally a vertical trait indicates the causality of the bond, i.e. the side of the bond on which the effort is imposed. The interconnection between different elements can be done via power-continuous nodes in which energy can only flow and can never be created/accumulated/dissipated: 0 junctions represent interconnections in which all power ports have the same effort and the flows sum to zero. They are equivalent to parallel electric connections and represent the Kirchoff's current law;

1 junctions represent interconnections in which all power ports have the same flow and the efforts sum to zero. They are equivalent to serial electric connections and represent the Kirchhoff's voltage law;

TF transformers describe relations between efforts and flows at the input and output ports, e.g. e 2 = ae 1 and a T f 2 = f 1 . Note that the power conservation imposes e T 2 f 2 = e T 1 f 1 . Transformers can also be modulated (MTF) in the sense that the transformation can depend on some external signal. The velocity transformation described by the robot Jacobian is an example in this sense where one has f 2 = ṙ = J (q) q = J (q)f 1 , and, because of the power conservation condition, e 1 = τ = J (q) T = J (q) T e 2 .

GY gyrators describe crossed relations between efforts and flows at the input and output ports, e.g. e 2 = af 1 and a T f 2 = e 1 . Again note that e T 2 f 2 = e T 1 f 1 . As for transformers, also gyrators can be modulated (MGY). To give a simple example, the mass-spring-damper system could be represented, using bond-graphs, as in Fig. C.3. For more information about bond graphs and their use for modeling pH systems one can refer to, e.g., [START_REF] Duindam | Modeling and Control of Complex Physical Systems: The Port-Hamiltonian Approach[END_REF].

Figure 1 -

 1 Figure 1 -Optimisation de la trajectoire chez les faucons pèlerins. Fig. (a) : structure anatomique de l'oeil du faucon avec la fovéa (la zone spécialisée dans la vision aiguë) visant à environ 45 degrés par rapport à l'axe frontal, image prise de [Tuc00]. Fig. (b) : trajectoire en spirale suivie par le faucon afin d'optimiser la perception visuelle et l'aérodynamique pendant le vol vers sa proie, image prise de [TTAE00].

  xv • b R a indicates the rotation matrix that transforms vectors from frame F a to frame F b . We can then write b p = b R a a p + b b t a . We also use the notation b M a to indicate such transformation in a compact way using the homogeneous representation of 3-D vectors:

Figure 1 . 1 -

 11 Figure 1.1 -Trajectory optimization in peregrine falcons. Fig. (a): anatomical structure of the falcon eye with the deep fovea (the area specialized in acute vision) pointing approximately 45 degrees to the side of the frontal line, image from [Tuc00]. Fig. (b): spiral trajectory followed by the falcon to optimize visual perception and air drag while flying toward its prey, image from [TTAE00].

Figure 2 . 1 -

 21 Figure 2.1 -History of electronic imaging. Fig. (a): picture of a cephalopod Nautilus. The pinhole camera eye is visible in the center of the image. "Nautilus Palau" by Manuae -Own work. Licensed under CC BY-SA 3.0 via Commons -https://commons.wikimedia.org. Fig. (b): artist using a camera obscura as an aid for drawing, from [Bea72]. Fig. (c): first surviving picture ever taken dating circa 1826. "View from the Window at Le Gras" by Joseph Nicéphore Niépce -Rebecca A. Moss, Coordinator of Visual Resources and Digital Content Library. College of Liberal Arts Office of Information Technology, University of Minnesota. Licensed under Public Domain via Commons -https://commons.wikimedia.org.

Figure 2 . 2 -Figure 2 . 3 -

 2223 Figure 2.2 -M. C. Escher (Leeuwarden 1898 -Laren 1972), "Convex and Concave". Lithograph print, first printed in March 1955.

Figure 2 . 4 -

 24 Figure 2.4 -Planar and spherical perspective projection models.

Figure 2 . 5 -

 25 Figure 2.5 -Geometrical representation of the epipolar constraint. The points a π and b π represent the projections of the same point p from two camera vantage points related by the rigid transformation described by the rotation matrix a R b and the translation vector a a t b .

Figure 2 . 6 -

 26 Figure 2.6 -Geometrical representation of the homography constraint. The points a π and b π represent the projections of the same point p from two camera vantage points related by the rigid transformation described by the rotation matrix a R b and the translation vector a a t b .

Let b n ∈ S 2

 2 be the unit vector normal to the plane P expressed in frame F b . Let also b d indicate the distance between P and the origin of frame F b . Any point b p on the plane surface must then satisfy the planarity constraint: b n T b p + b d = 0, ∀ b p ∈ P. Assuming b d = 0, one can then writeb n T b d b p = 1 and plug this in (2.5) obtaining

Figure 2 . 7 -

 27 Figure 2.7 -Bond graph representation of the robot geometric Jacobian. The Jacobian (2.14) can be represented as a Modulated Transformer (MTF), function of the robot joint configuration (top arrow). The generalized effort and flows variables are indicated on the top and bottom of the MTF ports.

Figure 2 . 8 -

 28 Figure 2.8 -Kinematic singularity. The compensation of the the applied force does not require any action from the third joint since the force will be absorbed by first two joints and by the structure of the last two aligned links.

  .34) Using (3.31, (3.33) and (3.34), one obtains the propagation equations for the IF

Figure 3 . 1 -

 31 Figure 3.1 -Resolution of a real-life art gallery problem for the city center of Bremen. From [BDRDS + 13].

Figure 3 . 2 -

 32 Figure 3.2 -Trajectory planning to minimize uncertainty for micro aerial vehicles. The objective is to reach the final pose (to the left) while minimizing the localization uncertainty. The resulting trajectory (black line) is more informative than the direct magenta path. From [AWCS13].

Figure 4 . 1 -

 41 Figure 4.1 -Optimality conditions for the camera linear velocity v as dictated by system (4.33). Fig. (a): when π = e 3 , vector v must be orthogonal to π and lie on the plane S spanned by π and e 3 (that is, v must belong to a specific straight line). Fig. (b): when π = e 3 , any v ⊥ e 3 is a valid solution to (4.33).

Figure 4 . 2 -

 42 Figure 4.2 -Possible shape of the weight functions used to cope with a limited camera FOV. Left: shape of weight w 1 (x) with limits x min = -x max = 0.2884 (normalized size of the image plane). Right: shape of weight w 3 (τ ).

T j = j+1 2 and

 2 θ = (θ 1 , . . . . . . θ T δ +δ ) ∈ R T δ +δ . Indeed, this allows (4.51) to span all the moment linear combinations of order up to δ with coefficients in vector θ. As illustration, by choosing δ = 2 in (4.55), one obtains the following quadratic polynomial w(x, y, θ) = θ 1 x + θ 2 y + θ 3 x 2 + θ 4 xy + θ 5 y 2 that, when plugged in (4.51), yields m w (θ) = θ 1 m 10 + θ 2 m 01 + θ 3 m 20 + θ 4 m 11 + θ 5 m 02 . (4.56)

Figure 4 . 3 -

 43 Figure 4.3 -Three examples of the constrained polynomial basis w(x, y, θ) in (4.57-4.60). Note how w(x, y, θ) smoothly vanishes at the image borders of size [-1, 1] × [-1, 1]

  [START_REF] Spica | Active Structure from Motion for Spherical and Cylindrical Targets[END_REF] shows three examples of weighting functions w(•) smoothly vanishing at the borders of an image plane of size [-1, 1] × [-1, 1] and obtained by picking at random three values for the free parameters in vector θ.

  Optimization of the weighted parametric image moments Consider now a set of m ≥ 3 weighted moments s = (m w (θ 1 ), . . . , m w (θ m )) ∈ R m with θ = (θ 1 , . . . , θ m ) ∈ R h being the stack of all parameters. Plugging the weighted moment dynamics (4.53-4.54) in the definition (4.48), one has

Figure 4 . 4 -

 44 Figure 4.4 -Geometry of a spherical object. Spherical target O S and planar limb surface L.

Figure 4 . 5 -

 45 Figure 4.5 -Geometry of a cylindrical object. Camera C and cylindrical target O C with the planar limb surface L and the other planes of interest P 1 and P 2

Figure 5 . 1 -

 51 Figure 5.1 -Experimental setup. The Point Grey Dragonfly2 greyscale camera Fig. (a) and the 6-DOFs Gantry robot Fig. (b) used for all experiments.

s 4 s 5 c

 5 56 + c 4 s 56 -c 4 s 5 c 56 + s 4 s 56 c 5 c 56 l 56 s 5 c 56 0 0 -s 4 s 5 s 56 + c 4 c 56 c 4 s 5 s 56 + s 4 c 56 -c 5 s 56 -l 56 s 5 s 56

Figure 5 . 2 -

 52 Figure5.2 -Simulation results comparing the planar and spherical projection models for the depth estimation of a point feature. The following color coding is adopted for the three considered cases: red-case I, green-case II, blue-case III. Fig. (a)behavior of the estimation error χ(t) in the planar projection case (solid lines) with superimposed the corresponding ideal response (4.15) (dashed lines). The convergence results slowest in case I and fastest in case III with, however, a corresponding increasing mismatch among χ(t) and its ideal response (as expected). Fig. (b) behavior of σ 2 1 (t) for the three cases with, again, the largest σ 2 1 (t) in case III. Fig. (c) behavior of the perturbation term d( x, t) for the three cases. As expected, d(t) ≡ 0 in case I, and it is largest in case III (thus, explaining the increasing mismatch among χ(t) and the corresponding ideal response). Fig. (d) behavior of the estimation error χ s (t) for the spherical projection model in the three cases. The three plots result exactly superimposed as predicted by the theory (no influence of the location of η on the estimation convergence). Fig. (e) behavior of χ(t) -χ s (t). As expected, the two projection models give rise to the same estimation error dynamics only in in case I (feature point at the center of the image plane).

Figure 5 .Figure 5 . 3 -Figure 5 . 4 -

 55354 Figure 5.4(a) shows the evolution of the estimation error χ(t) = 1/ Z(t) -1/Z(t) for two experiments 1 in which v(t) = v 0 but with its direction being either

Figure 5 . 5 -

 55 Figure 5.5 -Experimental results for the depth estimation of a point feature using an EKF. Fig. (a): behavior of the estimation error for case I (solid red line) and case II (solid blue line). The two vertical dashed lines indicate the times T 1 = 2.64 s and T 2 = 2.84 s at which the estimation error drops below the threshold of 3 cm. Note how the use of an active strategy for optimizing the direction of the camera linear velocity results, again, in improved performance of the estimator. Fig. (b): behavior of the estimation error for the nonlinear observer (solid red line) and the EKF (solid blue line) in case I. The green line (labeled EKF * in the legend) finally represents the estimation error for the EKF in case I when the estimator is initialized with a covariance of χ(t 0 ) 10 times larger than in the blue line case.

Figure 5 . 6 -

 56 Figure 5.6 -Experimental set-up for plane estimation with the dotted pattern and the topographic map used for feature extraction and tracking.

1

 1 (τ )dτ be the average values of the eigenvalues σ 2 m (t) and σ 2 1 (t) during motion in the active estimation cases, with T representing the experiment duration (blue lines in Figs. 5.7 and 5.8). After having computed σ 2 m and σ 2 1 during the first run, the estimation gains α B and α C were chosen so as to satisfy α B σ 2 m = α C σ 2 1 for imposing the same closed-loop dynamics to both methods B and C 3 . This resulted in gain α B = 1043.4 (used in these experiments), and in gain α C = 20000 (used in the experiments of the next Sect. 5.2.2).

Fig. 5 .Figure 5 . 7 -

 557 Figure 5.7 -Experimental results for the estimation of the plane parameters using 3-D points (method B) with an active strategy (blue lines) or a random acceleration (purple line) or a constant linear velocity (green, red and cyan lines). Fig. (a): normalized norm of the estimation error χ; Fig. (b): relative error between estimated and actual distance d and angle between the estimated and actual normal n. Fig. (c): smallest eigenvalue σ 2 m of the N × N matrix ΩΩ T multiplied by α B . Fig. (d): camera linear velocity norm. Fig. (e) and Fig. (f): geometric 3-D trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated.

Figure 5 . 8 -

 58 Figure 5.8 -Experimental results for the estimation of the plane parameters using discrete image moments method C with an active strategy (blue lines) or a random acceleration (purple line) or a constant linear velocity (green, red and cyan lines). Fig. (a): normalized norm of the estimation error χ. Fig. (b): relative error between estimated and actual distance d and angle between the estimated and actual normal n. Fig. (c): smallest eigenvalue σ 2 1 of the 3 × 3 matrix ΩΩ T multiplied by α C . Fig. (d): camera linear velocity norm. Fig. (e) and Fig. (f): geometric 3-D trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated.

Figure 5 . 9 -

 59 Figure 5.9 -Experimental setup for the estimation of the plane parameters in presence of outlier measurements. Note the introduction of the inclined picture in the observed scene on the right.

Finally Fig. 5 .Figure 5 . 10 -

 5510 Figure 5.10 -Experimental results for plane estimation in presence of outlier measurements using homography decomposition (method A -green lines), 3-D points estimation (method B -blue lines) or image moments (method C -red lines). Fig. (a): relative error between estimated and actual distance d; Fig. (b): angle between the estimated and actual normal n; Fig. (c): product ασ methods B and C; Fig. (d): evolution of the image moment m 00 ; Fig. (e) and Fig. (f): geometric 3-D trajectory of the camera with arrows indicating the optical axis and a green patch representing the plane to be estimated.

Figure 5 . 11 -

 511 Figure 5.11 -Evolution of the switching functions and of their product for three representative point features. On the right: detailed views of the corresponding plots on the left in the time interval immediately following the introduction of the blue feature in the estimator.

Figure 5 . 12 -

 512 Figure 5.12 -Simulation results obtained by employing the classical discrete moment set (x g , y g , µ 20 + µ 02 ) and optimizing for the camera linear velocity v. In this case Φ D (t) keeps very close to zero (the scale of Fig. (a) is 10 -9 ) and as a consequence the estimation error χ(t) does not converge (Fig. (b)).

Figure 5 . 13 -

 513 Figure 5.13 -Simulation results for plane estimation employing three adaptive moments of degree δ = 2 and optimizing for both vector θ and the camera linear velocity v (solid lines). Fig. (a): the value of Φ D is now ≈ 10 7 times larger than in the previous case of Fig. 5.12(a). This then allows for a quick convergence of the error quantities χ(t) (Fig. (b)), d(t) (Fig. (c)) and n(t) = arccos(n T (t) n(t)) (Fig. (d)). For a comparison, in all plots dashed lines correspond to the use of the five moments (x g , y g , µ 20 , µ 11 , µ 02 ): it is worth noting how, despite the increased measurement set (five moments vs. only three), the estimation convergence results still slower than in the weighted moment case.

Figure 5 . 14 -

 514 Figure 5.14 -Simulation results for plane estimation employing three adaptive moments of degree δ = 2 and only optimizing for vector θ. Fig. (a): note how Φ D (t) still reaches a range of values comparable with the previous case of Fig. 5.13(a) despite the lack of any optimization of the camera velocity v. The other figures show the behavior of the error quantities χ(t) (Fig. (b)), d(t) (Fig. (c)) and n(t) (Fig. (d)).

Figure 5 . 15 -

 515 Figure 5.15 -Simulation results for plane estimation employing three constrained adaptive moments, a camera with limited FOV, and optimizing for both vector θ and the linear velocity v (solid lines). Fig. (a): camera trajectory and direction of the optical axis during the estimation task. Fig. (b): number N p of tracked point features over time. Fig. (c): behavior of Φ D (t) which reaches a maximum of ≈ 2.9 • 10 -4 before starting to decrease because of the fewer tracked points. Finally the other figures show the behavior of the error quantities χ(t) (Fig. (d)), d(t) (Fig. (e)) and n(t) (Fig. (f).In dashed lines, the behavior that all quantities would have had in case no optimization of θ had been performed.

Figure 5 .

 5 Figure 5.17 -Simulation results for the dense moments case. Fig. (a): behavior of χ(t) for case I (red lines) and case II (blue lines). Fig. (b): behavior of the three eigenvalues of matrix ΩΩ T for case I (red lines) and case II (blue lines). Note how the convergence rate of case II is initially faster than in case I due to a higher value of the two largest eigenvalues σ 2 2 , σ 2 3 (see Fig. (b)) which are not being controlled. However, once the smallest eigenvalue σ 2 1 becomes dominant, the maximization of σ 2 1 operated in case I results, as expected, in a faster convergence rate of the estimation error. Fig. (c): 3-D Cartesian trajectory followed by the camera for case I (red line) and case II (blue line) together with the planar circle used to generate image moments. In case I, the camera has a larger velocity along the Z-axis that results in a larger σ 2 1 .

Figure 5 . 1 c 1 = c $ 1 c 1 = 2c $ 1 Figure 5 . 19 -

 511111519 Figure 5.18 -Experimental results for the estimation of the radius of a sphere using different constant camera velocities with the same norm. Fig. (a): behavior of the estimation error χ(t) for the two cases (solid blue and red lines), and for an 'ideal' second order system with poles at the desired locations (dashed black line). The vertical dashed lines indicate the times at which the estimation error χ(t) drops below the threshold of 1 mm. Fig. (b): camera trajectories for case I (blue line) and case II (red line) with arrows indicating the direction of the camera optical axis.

Figure 5 . 20 -

 520 Figure 5.20 -Experimental results for the estimation of the radius of a cylinder with the following color coding: blue -case I, red -case II, green -case III. Fig. (a): behavior of σ 2 1 (t) for the three cases (coincident for cases I and III and larger than in case II). Fig. (b): behavior of χ(t). The three vertical dashed lines indicate the times T 1 = 2.74 s , T 2 = 4.78 s and T 3 = 2.66 s at which the estimation error drops below the threshold of 2 mm. Note how T 1 ≈ T 3 and T 1 < T 2 as expected. Fig. (c): two views of the camera trajectories for the three cases with arrows indicating the direction of the camera optical axis.

Figure 6 . 2 -

 62 Figure 6.2 -Flowchart representation of the basic switching control strategy.

Figure 6 . 3 -

 63 Figure 6.3 -Qualitative plot of the adaptive gain k E (E) for E = 1 × 10 -5 and E = 1 × 10 -4 used to tune the effect of the active camera velocity optimization depending on the current status of the structure from motion estimation.

Figure 6 . 4 -

 64 Figure 6.4 -Flowchart representation of the adaptive switching strategy exploiting the measurable error energy for triggering changes of status.

Figure 7 . 1 -

 71 Figure 7.1 -Regulation of 4 point features. Fig. (a): behavior of the error norm ν(t) when using the full strategy of Sect. 6.2 and the estimated χ(t) (case I -blue line); the classical controller (6.4) and the estimated χ(t) (case II -red line); the classical controller (6.4) by employing χ(t) = χ d (case III -green line); the classical controller (6.4) by using the ground truth χ(t) (case IV -black dashed line). Fig. (b): behavior of the norm of the approximation error χ(t) = χ(t) -χ(t) with the same color code. Fig. (c): behavior of σ 2 (t) when actively optimizing the camera motion (case I -blue line) or not performing any optimization (case II -red line). In the previous plots, the (practically coincident) vertical dashed blue lines represent the switching times between the various control phases used in case I. Fig. (d): 3-D camera trajectory during case I with arrows representing the camera optical axis and square and circular markers representing the camera initial and final poses respectively. The three phases of Sect. 6.2.3 are denoted by the following color code: blue -phase 1), red -phase 2), green -phase 3). Fig. (e): trajectory of the four point features in the image plane during case I using the same color code, and with crosses indicating the desired feature positions. Superimposed, the initial and final camera images. Finally, solid lines represent the result of (correctly) implementing phase 2), while dashed lines represent the effects of a direct switch from phase 1) to phase 3) without the action of vector qw = qδ in (6.14).

Figures 7 .

 7 Figures 7.1(a) and 7.1(b) show the evolution of the error norm ν(t) and of the

Figure 7 .

 7 1(c) reports, instead, the evolution of the average eigenvalue σ 2 (t) for case I (blue line) and case II (red line), and finally Figs. 7.1(d) and 7.1(e) depict the camera and feature trajectory for case I.

7

 7

  .

1 .Figure 7 . 2 -

 172 Figure 7.2 -Importance of implementing phase 2) of Fig. 6.2 in the regulation of 4 point features. Behavior of the error norm ν(t) (Fig. (a)) and of δ , the measure of misalignment between vectors e and ė (Fig. (b)). In both plots, the blue lines represent the behavior of case I (full implementation of the switching strategy of Sect. 6.2.3), while cyan lines represent the direct switch from phase 1) to phase 3) without the action of vector qw = qδ in (6.14). The small picture-in-picture plots provide a zoomed view of the switching phase.

Figure 7 . 3 -Figure 7 . 4 -

 7374 Figure 7.3 -Regulation of 4 point features starting from a different initial camera pose w.r.t. the experiments reported in Fig. 7.1. The plot pattern and color codes are the same as in Fig. 7.1. Note how, this time, a correct realization of the servoing task is obtained only in case I (blue line in Fig. 7.3(a)). In case II, the estimation error χ(t) diverges (red line in Fig. 7.3(b)) because of the too small value of σ 2 (t) (red line in Fig. 7.3(c)) during the camera motion which makes the estimation task ill-conditioned. In case III, the visual task error starts diverging because of the too rough approximation χ = χ d and the point features leave the camera FOV (green line in Fig. 7.3(a)). Note also the initial spiralling motion of the camera (blue line in Fig. 7.3(d)) that allows maximization of the value of σ 2 (t) during phase 1) of case I.

Figure 7 . 5 -

 75 Figure 7.5 -IBVS of 4 point features using a constant approximation χ(t) = χ d where the value of χ d is corrupted by a relative error of 9%. In the first experiment the camera starts far from the desired pose, while in the second experiment the camera starts at almost the desired pose. Fig. (a): behavior of the error norm ν(t) for the first (blue line) and second (red line) experiments. Because of the approximated value of χ d , the servoing is not able to converge in both cases (thus also when starting very close to the desired pose), resulting in a loss of tracking for the point features. Fig. (b): image plane trajectory of the 4 point features during the first experiment. The four crosses indicate the desired feature positions, and the initial and final (i.e., until loss of tracking) camera images are superimposed.

Figure 7 . 6 -

 76 Figure 7.6 -Regulation of 4 point features using the adaptive strategy of Sect. 6.3. The three phases of Fig. 6.4 are denoted by the following color code: blue -phase 1), redphase 2), green -phase 3). Fig. (a): behavior of the error norm ν(t) with superimposed a horizontal dashed black line indicating the threshold ν T . Fig. (b): behavior of the norm of the estimation error χ(t) = χ(t) -χ(t) . Fig. (c): behavior of the mean eigenvalue σ 2 . Fig. (d): behavior of E(t) with, superimposed, two dashed horizontal lines indicating the minimum and maximum thresholds E and E. Fig. (e): behavior of the adaptive gain k E (E). In all of the previous plots, vertical dashed lines represent the times at which the target object was intentionally displaced. Fig. (f): camera 3-D trajectory with arrows representing the camera optical axis and square and circular markers representing the camera initial and final poses, respectively.

Figure 7 . 7 -

 77 Figure 7.7 -Regulation of 10 point features on an unstructured object using a KLT tracker and the adaptive strategy of Sect. 6.3. The same quantities of the previous Fig. 7.6 are reported here with the only exception of Fig. (g) that depicts the trajectory of the 10 point features on the image plane with crosses indicating the desired feature position and, superimposed, two (semi-transparent) camera screenshots taken at the initial and final robot configuration.

(B. 11 )B. 3 A

 113 nonlinear observer for photometric Structure from Motion We can immediately notice that both (B.9) and (B.10) present a similar structure to the, well known by now, dynamics (3.10) with s = Y and χ = ζ or χ = δ and hence m = p = 1.

Figure B. 1

 1 Figure B.1 shows some simulation results obtained by applying observer (B.14) to estimate the structure of a simple scene. The simulated scenario contains only a plane with a surface texture that is uniform in a central rectangular area and grows parabolically from its borders, see Fig. B.1(a). As a consequence the image resulting from the projection process on the simulated camera, contains uniform region, see the top lines of Figs. B.1(b) to B.1(d). The camera moves along a square trajectory whose sides are parallel to the sides of the uniform region (see, again, Fig. B.1(a)) at a constant velocity norm v(t) = 1 m/s and for a total time T = 6 s. By doing so, it maintains a constant distance of 5 m from the plane.

1

 1

Figure B. 1 -

 1 Figure B.1 -Photometric depth estimation for a planar scene. Fig. (a): camera trajectory with arrows indicating the direction of the camera optical axis and the observed textured plane. In the other plots: evolution of the image s = Y , observability eigenvalue σ 2 , and disparity estimation error χ maps for the first (Fig. (b)) and second (Fig. (c)) quarters of period and for the second half of the time (Fig. (d)). Note that a RGB color map was used for these plots for better readability, however all of these maps are scalar.

  is particularly interesting because it allows us to give another physically intuitive interpretation of the regularization process. Let us first consider a one-dimensional case: that of an elastic band stretched between two fixed points in x = 0 and x = l, see Fig. B.2 and [CH66].

Figure B. 2 -

 2 Figure B.2 -Simple model of a deformed elastic band. The band is rigidly attached at the two extrema and, in the equilibrium condition, it assumes a straight configuration between the two (black line) thus having a length l. The red line represents a deformed elastic band with u(x) representing the amount of deflection w.r.t. the straight configuration.

Figure B. 3 -

 3 Figure B.3 -Photometric depth estimation for a planar scene with regularization of the disparity map. Evolution of the image s = Y , regularization gain q, and disparity estimation error χ maps for the first (Fig. (a)) and second (Fig. (b)) quarters of period and for the second half of the time (Fig. (c)). A RGB color map was used for these plots for better readability, however all of these maps are scalar. Note how the regularization term is mostly active in the unobservable areas (compare q here with the plots of σ 2 in Fig. B.1).

Figure B. 4 -

 4 Figure B.4 -Depth discontinuities in presence of an occlusion. Fig. (a): depth discontinuity in presence of an occlusion. The environment V e is represented in blue. The green area V o is occluded. The camera is represented by a circle with an arrow indicating the direction of the optical axis which is assumed coincident with the Z-axis. Fig. (b): shock and rarefaction waves interpretation in the geometry reconstruction. The red region represents the area that is being uncovered or occluded depending on whether the camera moves from right to left or from left to right respectively. The blue dashed line and the blue dots represent a triple valued solution and its geometrical interpretation. The gray line and grey dots represent the first double valued solution in Fig. B.5(b). Finally the red line and red dots represent the correct solution.

Figure B. 5 -

 5 Figure B.5 -Time-space evolution of a rarefaction and shock wave. Fig. (a): evolution of a rarefaction wave. Fig. (b): evolution of a shock wave. For both figures, from bottom to top we show: (i) the initial condition ν(x, 0), (ii) some of the characteristic curves emanating from x = -10 + k with k = 0, 1, . . . , 20, and (iii) the final condition ν(x, 10).This picture is similar to some reported in[START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF].

Figure B. 5 (

 5 Figure B.5(a) shows some of the characteristic curves of (B.26) starting from X(0) = -10 + k with k = 1, 2, . . . , 20 and with an initial condition:

  Fig. B.4(b) if the camera moves from the right to the left configuration. In this situation new parts of the environment (the red region in Fig. B.4(b)) appear in the camera FOV and the information contained in the initial condition is not sufficient to reconstruct ζ in the discovered area. Simply integrating (B.21) will result in the solution represented by the dashed line in the top plot of Fig. B.5(a). This solution would correspond to the actual evolution of ζ only if the initially occluded area (green plus red region in Fig. B.4(b)) were actually part of the environment (i.e. if nothing is actually being discovered). However the actual depth map, for the case depicted in Fig. B.4(b), instead, remains discontinuous in the new camera position.

  Fig. B.6(b). The trajectory of the camera, represented by a black dashed line in Fig. B.6(b), is traveled at a constant velocity v = [.8 0 0] T m/s and ω = 0 from the right to the left of the figure. The position of the camera at some equally spaced iterations is represented by coloured arrows, indicating the direction of the camera optical axis. For the same iterations, and with the same color code, we represented, with dashed lines, in Fig. B.6(a), the actual value of the disparity map in the camera normalized coordinate x. The dashed lines in Fig. B.6(a) represent, instead, a reconstruction of the environment from the above disparity map, calculated using the parametric expression W p(π, t) = 1 χ(π, t) C R W π + W t C (t). (B.27) Note that the dependence of this reconstructed environment on t is due to the fact that, depending on the camera position (that changes with time) different portions of the environment are occluded as it can be seen in Fig. B.6(b) (the dashed lines are not all coincident). In other words, in the discontinuity, (B.8) is violated and new source terms (i.e. contributions to the total time derivative of the disparity map) appear. This effect will also characterize the shock waves. The solid lines in Figs. B.6(a) and B.6(b) finally represent the estimation of the depth map and the corresponding environment reconstruction, when using (B.19

Figure B. 6 -

 6 Figure B.6 -Photometric depth estimation for a rarefaction wave. Fig. (a) evolution of the actual (dashed) and numerically propagated (solid) disparity map at equally spaced iterations (check the legend). Fig. (b) reconstruction of the environment from the actual (dashed) and numerically propagated depth maps with arrows representing the camera position and the direction of the optical axis, and using the same color code as in Fig. (a).

A.

  dual situation to the rarefaction waves is the one represented in Fig. B.5(b), which shows some of the characteristic curves starting from X(0) = -10 + k, with k = 1, 2, . . . , 20, but now with: ν(x, 0) =    ν r = .1 if x > 0 ν l = .5 if x < 0 In this case the characteristic curves intersect and the solution becomes triple-valued (dashed line in the top plot of Fig. B.5(b)). This effect is called a shock wave.

1 +∆t t 1 f 1 +∆t t 1 f 2 ν 2 r -ν 2 l ν r -ν l = ν r + ν l 2 .

 11112222 B.7). Integrating the conservation law (B.23) in the region[x 1 , x 1 + ∆x] × [t 1 , t 1 + ∆t] one obtains x 1 +∆x x 1 ν(x, t 1 + ∆t) dx -x 1 +∆x x 1 ν(x, t 1 ) dx = t (ν(x 1 , t)) dt -t (ν(x 1 + ∆x, t)) dt. The space-time region [x 1 , x 1 + ∆x] × [t 1 , t 1 + ∆t] is divided (see Fig. B.7) into a left and a right triangles in which the value of ν is roughly equal to ν l and ν r respectively, therefore one obtains ∆xν l -∆xν r ≈ ∆tf (ν l ) -∆tf (ν r ), and, in the limit ∆t → 0, one has, for (B.26)∆x ∆t ≈ Ẋ = v s = f (ν r ) -f (ν l ) ν r -ν l ,which, for (B.26), results in v s = 1

  Fig. B.4(b) if the camera moves from the left to the right position. The dashed blue line in Fig. B.4(b) representsthe double valued solution returned by the equal area rule that intersects the initial environment hypothesis (given by V e ∪ V o ) in the two blue dots in the figure. Note that, differently from the particles case, however, the disparity dynamics does not contain any viscosity (apart from the artificial one due to numerical discretization or regularization) and the two points in which the camera intersects the environment in presence of an occlusion have completely independent dynamics. Hence the solution of (B.28) does not represent correctly the evolution of the system in this situation. The correct solution in the projection case is clearly the one represented in red in Figs. B.4(b) and B.5(b), which preserves the position of the occlusion. To obtain this solution we need to introduce a "source" term (an additional contribution to the total derivative of ζ) in the PDE that corresponds to the areas represented in green and orange in Fig.B.5(b). For an integration time ∆t, this area is given by∆A = (ν l -ν r )[f (ν l ) -v s ]∆t = 1 2 (ν l -ν r ) 2 ∆tand hence for ∆t → 0 one has the source term -ν r ) 2 . (B.30)

  Fig. B.6, but now with the camera traveling in the opposite direction (i.e. from left to right), see Fig. B.8(b). The results of the simulation are reported in Fig. B.8 with, as before, dashed lines representing the ground truth and solid lines representing the results of the pure propagation (α = h = q = 0) of (B.19) from an initial estimation error χ = 0. The first series of plots, Figs. B.8(a) and B.8(b), show the results obtained without the source term (B.30).As one can see, the propagation is correct before the formation of the shock wave (around iteration 240, green lines in the plots). After this, the propagation starts accumulating delays w.r.t. the ground truth. This is due to the fact that the shock wave is propagated at a velocity v s = 1 2 (χ l + χ r )ψ, as predicted using (B.29), instead of χ r ψ, where χ r and χ l are the values of χ at the right and left of the discontinuity. The point in which the environment reconstruction lines intersect in Fig.B.8(b) corresponds to the depth value of Z = 1 2 (χ l + χ r ) -1 ≈ 0.66 m, i.e. to the point that is actually supposed to travel at the shock speed v s .

Figure B. 8 -

 8 Figure B.8 -Photometric depth estimation for a shock wave. Fig. (a) evolution of the actual (dashed) and numerically propagated (solid) disparity map at equally spaced iterations (check the legend) and for a time step calculated in such a way that the shock wave moves by exactly one pixel per iteration. Fig. (b) reconstruction of the environment from the actual (dashed) and numerically propagated depth maps with arrows representing the camera position and the direction of the optical axis, and using the same color code as in Fig. (a). The same kind of plots are reported in Fig. (c) and Fig. (d) for the case in which the additional source term (B.30) is used to improve the shock wave propagation. Finally Fig. (e) and Fig. (f) report the results obtained by still using the source term (B.30) but with a time step of 2 ms, i.e. smaller than in the previous case.

Figures B. 8 (

 8 Figures B.8(c) and B.8(d) represent the results obtained by introducing the additional source term (B.30) in the propagation equation for χ.One can note that now the propagation is correct both before and after the formation of the shock wave. In this case, however, (as in the previous one) the time step for the propagation was chosen in such a way that the shock wave moved by exactly one pixel per iteration. This was obtained with δt ≈ 2.08 ms. By repeating the simulation with a time-step of δt = 2 ms, one can see that the propagation of the shock wave is again not perfect, see Figs. B.8(e) and B.8(f) even though it is still significantly better than in Figs. B.8(c) and B.8(d). We want to stress that the fact that the source term (B.30) seems to only work correctly when the discontinuity moves by exactly one pixel per iteration is a significant limitation to its use. In fact, in general, the observed scene will contain more that one shock-type depth discontinuities each moving at different speeds and, therefore, we cannot expect this condition to be realized in practice. The reasons why this happens are still not clear, but we suspect that this might be related to numerical discretization.

  can also be rewritten in terms of the linear momentum p = m ẋ as ṗ + kx = e.Defining the state vector x = [x, p] T and the system Hamiltonian S∇ x H(x) + Be, (C.3)

  mẍ + d ẋ + kx = ewhere d ≥ 0 is the damping coefficient and (C.the system resistive matrix and H is still given by (C.2). Equation (C.4) can be further generalized by making the structure matrix S, the resistive matrix R and the input matrix B state dependent. By doing so, one obtains the generic form of input-state-output port-Hamiltonian systems that represent a large class of physical processes. The dynamics of these systems have the from ẋ = [S(x) -R(x)] ∇ x H(x) + B(x)u y = B(x) T ∇ x H(x) (C.5)

t t 0 ∇

 0 x H(x(τ )) T R(x(τ ))∇ x H(x(τ ))dτ + t t 0 y(τ ) T u(τ )dτ.Now let us assume that H is lower bounded and that, w.l.o.g., H ≥ 0 as it is the case for (C.2). Under this assumption, one can conclude thatH(x(t 0 )) = H(x(t)) + t t 0 ∇ x H(x(τ )) T R(x(τ ))∇ x H(x(τ ))dτ -) T u(τ )dτ

Figkx 2 ,

 2 H K (p) = 1 2m p 2 , H = H U (x) + H K (p).

Figure C. 1 -

 1 Figure C.1 -Representation of the mass-spring system as an interconnection between two subsystems. K is associated to the mass and stores the kynetic energy of the system. U is associated to the spring and stores the potential energy of the system.

Figure C. 2 -

 2 Figure C.2 -General structure of a port-Hamiltonian system with the Dirac structure D in the center and the four ports representing internal energy-storage (S) and dissipative (R) elements and the interconnection with the control action (C) and the environment (I).

1 )

 1 ∈ F × F * | (f 1 , e 1 ), (f 2 , e 2 ) F×F * = 0, ∀(f 2 , e 2 ) ∈ D If D = D ⊥ then D is a constant Dirac structure. Note that one has dim D = dim D ⊥ = dim(F × F * ) -dim D = 2 dim F -dim D ⇐⇒ dim D = dim F.The Dirac structure defines a power-preserving or power-continuous relation be-tween the power variables, in fact if (f , e) ∈ D, then (f , e) ∈ D ⊥ and the bilinear form returns:(f , e), (f , e) F×F * = 2 f |e = 0.

  C.2):

Figure C. 3 - 1 m

 31 Figure C.3 -Bond graph representation of the mass-spring-damper system. The constitive equations for each element are R: e d = d ẋ; I: e k = k ẋ(τ )dτ ; C: ẋ = 1 m e m (τ )dτ , and finally, for the 1-junction, e -e k -e d -e m = 0 where the signs in the summation were chosen according to the direction of positive power represented by the half-arrows.

  Equation (B.23) signifies that the total variation of the average inverse depth over time in the interval is only due to the difference of "flux of inverse depth" through the left and the right boundaries of the interval. Equations in the form of (B.21) appear very often in physics for instance when modelling fluid dynamics, see[START_REF] Thomas | Numerical partial differential equations: conservation laws and elliptic equations[END_REF][START_REF] Leveque | Finite volume methods for hyperbolic problems[END_REF] for some examples.

	where f (ζ, t) = ∂ ∂ζ f (ζ, t). One can easily verify that, along any curve X(t) satisfy-
	ing the ODE
				Ẋ
	with (B.21) rather than (B.20), see [Tho13b, Tho13a]. In particular (B.21) leads
	to the conservation law associated with (B.20). This latter is obtained by integrat-
	ing (B.21) in the interval [x 1 , x 2 ]:
	d dt	x 2 x 1	ζ(x, t) dx =	d dt	ζ(t) = -f (ζ(x, t), t)	x 2 x 1	,	(B.23)
	By differentiating the flux function f (ζ, t), one obtains the quasi-linear form of
	the conservation law (B.21):
		∂ ∂t	ζ(x, t) + f (ζ(x, t), t)	∂ ∂x	ζ(x, t) = 0,	(B.24)

where ζ(t) is the average value of ζ(x, t) over the interval [x 1 , x 2 ].

  1 2m ν 2 flowing to/from the left and the right boundary of the region, therefore, assuming unitary particles mass, one has

	∂ν ∂t	+	∂ ∂x	1 2	ν 2 = 0,	(B.26)
	or, equivalently
		∂ν ∂t	+ ν	∂ν ∂x	= 0.
	Equation (B.21) can be written as (B.26) by defining ν = v x ζ. Burgers' equa-
	tion (B.26) is probably the most typical textbook example of nonlinear flux conser-
	vation law, see, e.g.

  Figure B.7 -Shock speed propagation on the plane (x, t). Figure modified from [LeV02].

	t 1 + "t
	8 = 8 l
				8 = 8 r
	x 1 t 1				x 1 + "x
	t) +	∂ ∂x	f (ζ(x, t)) =	∂ 2 ∂x 2 ζ(x, t).	(B.28)

  in (B.19) a propagation term and an innovation component. The former is meant to align the previous estimate to the latest measurement while the latter uses the actual measurement to update the estimation. Let us indicate with ( s k , χ k ) the state of the observer at time t = T k = t 0 + k∆t when the image s k becomes available. Let us also indicate with ( s k+ , χ k+ ) the value obtained by integrating the following PDE obtained by setting h = α = 0 in (B.19)

	  	∂ s ∂t	= f
	 

s (s ? , ω) + Ω(s

? , v) ζ ∂ χ ∂t = f χ ( χ, v, ω) + q(t, π)∇ 2 π χ. (B.

31) from the initial condition ( s k , χ k ) for a time ∆T . When a new camera image is available, we calculate the new estimate at time T k+1 as    s k+1 = s k+ -h∆T s k+ -s k+1

Au Chapitre 6 nous proposons une stratégie intelligente pour augmenter le taux de convergence de l'estimation de la structure 3-D d'une manière qui est compatible avec l'exécution d'une tâche principale d'asservissement visuel. Nous proposons d'utiliser un opérateur de projection « large » qui permet de maximiser la redondance du robot de manière à donner plus de « liberté » à la maximisation de l'estimation active, tout en réalisant la tâche IBVS. Ensuite, nous étendons notre solution par l'introduction d'une stratégie adaptative qui permet de régler l'effet de l'estimation active et même de déclencher son activation/désactivation en fonction de l'état actuel de l'erreur d'estimation.Au Chapitre 7 nous présentons, comme déjà fait au Chapitre 5, une vérification expérimentale et approfondie des résultats du Chapitre 6. Nous démontrons que l'optimisation du mouvement de la caméra pendant le transitoire d'un contrôleur IBVS maximise le taux de convergence d'un estimateur SfM. Vu que les quantités estimées sont utilisées pour calculer la matrice d'interaction pour la tâche IBVS considéré, cela se traduit, à son tour, par une amélioration substancielle de la performance de la commande. Enfin, nous montrons qu'en utilisant une stratégie adaptative fondée sur l'état actuel de l'estimateur, on peut également réduire l'effet négatif de déformation que la perception active a sur la trajectoire de la caméra.

Note that a straight line trajectory in the task space R does not result, in general, in a straight line trajectory of the robot end-effector in the 3-D Eclidean space.

In this overconstrained situation, the ideal behavior (2.20) could still be obtained if the mdimensional error vector e happens to be spanned by the ρ-dimensional range space of Jr (ρ < m) during the whole task execution. However, in most cases this special condition cannot be expected to hold.

A more general version of this lemma was discussed in[START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. Such a generalization, however, is not necessary for the developments of the rest of this thesis and therefore will be omitted.

J. J. Gibson and his wife E. J. Gibson, among the most important psychologists of the 20th century, formulated their theory of the active observer in the 1960s. For them "perceiving is active, a process of obtaining information about the world. We 2 Note that we could have equivalently considered R = Om×m and H =[Im, Om×v]

Note that we are now considering the problem of estimating the parameters of a linear system

The results of Sect. 5.2 will nevertheless test the robustness of the methods against this hypothesis.

Of course, the use of different functional bases, also non-polynomial, could be possible.

A requirement not present in the point-feature case where any non-zero linear velocity not aligned with the projection ray could guarantee fulfillment of (3.13).

The two planes are therefore tangent to the surface of the cylinder.

The ground truth Z0(t) was obtained from a previous offline estimation of the 3-D position p in the world frame, and by then using the information on the camera position provided by the robot forward kinematics.

This is possible because the control law (5.1) does not depend on any estimated quantity.

As explained in[START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], the convergence rate of the SfM scheme (3.11) is actually dictated by the smallest eigenvalue of ΩΩ T times the chosen estimation gain α.

Of course one could utilize a RANdom SAmple Consensus (RANSAC) based classification (exploiting the homography constraint) for preliminarily segmenting the two planes so as to only consider the points belonging to the main dominant plane for the estimation task. However, in a real situation, the accuracy of any classification method can never be perfect and some outliers will fail to be detected. Therefore, in this experiment we intentionally decided to not include any preliminary RANSAC based pruning in order to just assess the "intrinsic" robustness of the proposed algorithms (which would clearly be improved by any preliminary outlier rejection step).

Note that, if n > m ≥ 6, there would still be room in(6.4) for implementing an action in the null-space of the main task. However, presence of this redundancy would be useless for the sake of optimizing the camera trajectory as it would only result in an internal reconfiguration of the robotic arm without effects on the camera motion.

If ρ < m, as in the case studies reported in Chapt. 7, the ideal behavior (6.11) can, in general, only be approximately imposed, see Sect. 2.3.2.

Analogous theoretical difficulties affect any (non-trivial) robotic application in which an estimation step is plugged into a control loop (e.g., whenever exploiting an EKF for feeding online a motion controller with the estimated state).

This is indeed the solution adopted for the experiments in Sects. 7.2 and 7.3.

http://roboticvision.org/

Note that Ω is actually a scalar map, but the transpose was included for a better visual resemblance with (3.10).

Note that, in principle, a similar issue existed for the other SfM problems considered in this thesis, whenever the integration step of the observer was chosen smaller than the camera frame rate. However, while in the other cases its effects where negligible, as demonstrated by the experimental results, initial simulation tests seem to indicate a higher sensitivity w.r.t. this problem in the dense case.

Acknowledgements

Dr. François Chaumette for always guiding my research with his expert advise. He also honoured me with his trust and gave me a number of opportunities that significantly contributed enriching my Doctorate adventure. , I hope, will continue in the future.

During my Doctorate, I also had

confirm the validity of the theoretical analysis presented here and, in particular, the ability of the proposed active estimation strategy to improve, in all cases, the transient response of the estimation error.

Active structure estimation for a point

Comparison of planar and spherical projection models

We start by comparing via simulation results the effects of adopting a planar and spherical projection model for the depth estimation of a point feature as extensively discussed in Sect. 4.5.1.1 and Sect. 4.5.1.2. We considered three cases differing for the location on the image plane at which the point feature was (purposely) kept exploiting the camera angular velocity ω: case I: the point feature was kept at the center of the image plane (red line in the following plots);

case II: the point feature was kept at one of the corners of an image plane with the same size of the camera used in the experiments (green line in the following plots); case III: the point feature was kept at one of the corners of an image plane with a size five times larger than case II (blue line in the following plots).

In all cases, a constant camera velocity v(t) ≡ v(t 0 ) = const was kept during motion, with the initial condition v(t 0 ) chosen so as to comply with the optimality conditions discussed in Sects. 4.5.1.1 and 4.5.1.2 for letting σ 2 1 = σ 2 max (e.g., with v(t 0 ) being a solution of (4.33) in the planar projection case). Figure 5.2(a) shows the behavior of χ(t) for the three cases when using a planar projection model. We can then note how the convergence rate of the estimation error increases from case I (slowest convergence) to case III (fastest convergence) as predicted by the theory (for the same v a larger π results in a larger σ 2 max ). Similarly, Fig. 5.2(b) reports the behavior of σ 2 1 (t) for the three cases: as expected, σ 2 1 (t) results largest for case III. Note also how σ 2 1 (t) for case II (green line) is only slightly larger than case I (red line). This is due to relatively small size of the image plane of case II whose dimensions were set as those of the real camera used for the experiments. Finally, Fig. 5.2(c) shows the behavior of the perturbation term d( x, t) in the three cases: here, one can verify how d = 0 for case I, with then an increasing |d| for cases II and III. Indeed, as discussed in Sect. 4.5.1.1, the 'amplification' effect on σ 2 max obtained by increasing π comes at the price of an increased magnitude of the perturbation d. This is also evident in Fig. 5.2(a) where the ideal response of (4.15) is plotted with dashed lines for the three considered cases. We can thus note how χ(t) in case I presents a perfect match with its corresponding ideal response, with then an increasing (albeit very limited) mismatch in the other two cases due to the increased effect of the perturbation d.

case II: kept constant so that v(t) = v 0 = const (blue line). This effect was obtained by using the following control law (equivalent to (4.23))

with

given by (4.31). In fact, the first term in (5.1) enforces the constraint v(t) = v 0 (same control effort in both cases), while the second term (maximization of σ 2 1 within the null-space of the first constraint) allows to implement either case I (k 2 > 0) or case II (k 2 = 0). In both cases, the angular velocity ω was exploited for keeping the point feature at the center of the image plane (x, y) → (0, 0). We note that, as discussed in Sect. 4.5.1.1, when (x, y) = (0, 0) one has σ 2 max = v 2 x + v 2 y from (4.32) and σ 2 1 = σ 2 max iff v z = 0 (circular motion around the point feature). The experiments were run with the following parameters: α = 10 3 , c 1 = c * 1 for D 1 in (4.8), v(t 0 ) = v 0 = (0.03, 0, -0.04) m/s, k 1 = 5 and k 2 = 10 4 , thus resulting in the maximum value σ 2 max = 0.0025 for the eigenvalue σ 2 1 . As clear from Fig. 5.4(d), while in case II the camera gets closer to the point feature, the use of the active strategy of case I results in a null component of v along the projection ray of the point feature (i.e., v z = 0) and in an associated circular trajectory centered on the tracked point as predicted by the theoretical analysis of Sect. 5.1.1. This then allows to move faster in the 'useful' directions (while keeping the same constant v ), and, thus, to increase the value of σ 2 1 towards its theoretical maximum σ 2 max = 0.0025 (Fig. 5.4(b)), resulting in an overall faster convergence for the estimation error (Fig. 5.4(a)). Furthermore, Fig. 5.4(a) also reports the ideal response of (4.15) with desired poles at σ 2 max (dashed black line). We can then note the almost perfect match with case I (solid red line): indeed, as explained in Remark 4.1, imposing a σ 2 1 (t) = const allows to exactly obtain the ideal behavior governed by (4.15). It is finally worth noting the accuracy of the reconstructed depth: Fig. 5.4(a) reports two vertical dashed lines indicating, for the two cases under consideration, the times T 1 = 3.54 s and T 2 = 5.81 s at which the estimation error χ(t) becomes smaller than 3 cm. We then obtained a standard deviation of approx. 7.5 and 8.4 mm evaluated on a time window of 1 s after the times T 1 and T 2 , respectively. These results then also confirm the robustness of the proposed estimation approach despite the unavoidable presence of noise and discretization in the image acquisition. Note also that, as expected, the estimation error in the (active) case I reaches 'convergence' (i.e., drops below the threshold of 3 cm) significantly faster than case II (T 1 < T 2 ).

Active structure estimation for a cylinder

In this final section we report some experimental results concerning the active estimation of the radius of a cylindrical object. Indeed, as in the sphere case, knowledge of R allows to fully recover the 3-D point p 0 = Rs, with vector s from (4.81) and the cylinder axis a in (4.83) being directly obtainable from image measurements. For these experiments we used a white cardboard cylinder placed on a black table (see Fig. 5.3(c)). The radius of the cylinder was approximately 4.2 cm.

In the cylinder case, the convergence rate of the estimation error depends both on the norm of the camera linear velocity v and on its direction w.r.t. the cylinder axis a, see (4.86). It is then interesting to optimize the direction of v under the constraint v = const for maximizing the eigenvalue σ 2 1 (i.e., so as to obtain the fastest convergence rate for a given 'control effort' v ).

From (4.87), maximization of σ 2 1 (t) w.r.t. vector v can be obtained by choosing

with, i.e., by following the gradient of σ 2 1 w.r.t. v and by compensating for the (known) effects of input ω. In order to additionally enforce the constraint v = const during the eigenvalue maximization, (5.2) can be modified, similarly to (5.1), as

with k 1 > 0 and k 2 > 0. Analogously to the point feature case, the first term in (5.3) asymptotically guarantees v(t) = v 0 while the second term projects (5.2) onto the null-space of the constraint v(t) = const. As for the angular velocity ω, we exploited it for keeping the axis of the cylinder at the center of the image plane by regulating (s x , s y ) to (0, 0).

We now present three experimental results structured as follows:

case I: in the first experiment, the update rule (5.3) is fully implemented (k 1 > 0, k 2 > 0) for actively optimizing the direction of v;

case II: in the second experiment, the camera starts from the same initial pose and velocity as in case I, but (5.3) is implemented with k 1 > 0 and k 2 = 0, i.e., without performing any optimization of σ T his section reports the results of several experiments meant to validate the approach proposed in Chapt. 6 for coupling the execution of a VS task with the concurrent optimization of the 3-D structure estimation. All experiments were run by making use of the same experimental setup described in Chapt. 5.

In the reported experiments, we considered, as visual task, the regulation of N = 4 point features π i with, thus, s = (π 1 , . . . , π N ) ∈ R m , and L s = (L s 1 , . . . , L s N ) ∈ R m×6 , m = 8, with L s i being the standard 2 × 6 interaction matrix for a point feature (2.10). As for vector χ, we then have χ = (χ 1 , . . . , χ N ) ∈ R p , p = 4, where χ i = 1/Z i as explained in Sect. 4.5.1. For most of the experiments, the tracked points were black non-coplanar dots belonging to the surface of a white cube and with relative 3-D positions O p i (w.r.t. the center of the cube)

Knowledge of these 3-D coordinates was exploited in a standard pose estimation algorithm for obtaining the ground truth value of χ(t) from the known object model and the measured features s(t).

Because of the high contrast between black dots and white cube surface, the segmentation and tracking of the N points could be easily obtained at video-rate via the blob tracker available in ViSP. Besides easing the image processing step, this experimental setting made it also possible the reproduction of (practically) identical initial experimental conditions across the several trials illustrated in the following sections. The results reported in Sect. 7.3 will instead resort to a KLT tracker for segmenting and tracking a generic set of points lying on a much less structured case I: the full strategy (three phases) illustrated in Sect. 6.2 and Fig. 6.2 is implemented for regulating the visual error e(t). The estimator (3.11) is run in parallel to the servoing task for generating the estimated χ(t) fed to all the various control terms. The active optimization of the camera motion (7.1) takes place for the whole duration of phase 1);

case II: the classical control law (6.4) is implemented for regulating the visual error e(t). The estimator (3.11) is still run in parallel to the servoing task for generating the estimated χ(t) fed to all the various control terms. However, in this case, no optimization of the estimation error convergence is performed;

case III: the classical control law (6.4) is again implemented for regulating the visual error e(t). However, the estimator (3.11) is not run and vector χ(t) is taken coincident with its value at the desired pose, i.e., χ(t) = χ d = const, as customary in many VS applications;

case IV: finally, as a reference 'ground truth', the classical control law (6.4) is again implemented but by exploiting knowledge of the ground truth value χ(t) = χ(t) during the whole servoing execution. This case then represents the 'ideal' behavior one could obtain were χ(t) available from direct measurement.

The following gains and thresholds were used in the experiments: α = 2000 in (3.11), k p = 0.0225 and k v = 0.3 in (6.4-6.5). Moreover, only for case I, we used k σ = 20, γ = 0.001 and k d = 18 in (7.1), ν T = 0.21 and δ T = 0.004 in the flowchart of Fig. 6.2 and finally k δ = 100 in (6.14). Furthermore, in cases I and II, vector χ was initialized as χ(t 0 ) = χ d , that is, starting from the (assumed known) value at the desired pose χ d also exploited in case III. A.2 Dynamics of the weighted image moments with

time [s]

) m vz = A(jm i+1,j + im i+1,j + m x i+2,j + m y i+1,j+1 ) + B(im i,j+1 + jm i,j+1 + m x i+1,j+1 + m y i,j+2 )+ + C(jm i,j + im i,j + m x i+1,j + m y i,j+1 ) m wx = jm i,j+1 + im i,j+1 + jm i,j-1 + m x i+1,j+1 + m y i,j + m y i,j+2

We can note that the dynamics (A.8) involves moments of order up to (i + j + 2) associated to the terms m x ij and m y ij . Also, it is easy to check that in the unweighted case (w ≡ 1 and m x ij = m y ij = m t ij ≡ 0), one obviously recovers the classical moment dynamics reported in [START_REF] Tahri | Point-Based and Region-Based Image Moments for Visual Servoing of Planar Objects[END_REF].

A.3 Time-derivative of the limb surface parameters for a spherical target

Differentiation of χ from (4.69) w.r.t. time yields

where we set ν 1 = ν(t 1 ) and ν1 = ν(t 1 ) for simplicity.

We also note that (6.11) is governed, component-wise, by the same dynamics of (6.10). Therefore, the solution of (6.11) is

where, again, e 1 = e(t 1 ) and ė1 = ė(t 1 ).

If e 1 and ė1 are parallel then (6.12) holds: assuming e 1 and ė1 are parallel, vector ė1 can be expressed as

thus showing that ν * (t) ≡ ν e (t), i.e. fulfilment of condition (6.12) as claimed.

If (6.12) holds then e 1 and ė1 are parallel: from (A.19-A.20) we have (omitting the time dependency for brevity) Since ν1 is just the projection of vector ė1 along the direction of e 1 (see again (A.23)), condition (A.26) necessarily requires vectors e 1 and ė1 to be parallel as claimed.

A.5.2 Properties of E(t)

Relationship between E(t) and the estimation error χ(t): if σ 2 1 (t) > 0 during the camera motion then E(t) ≡ 0 iff χ(t) ≡ 0 (i.e., the estimation has converged) and E(t) > 0 otherwise (i.e., the estimation has not yet converged).

In order to prove this claim, we start by showing the following facts: Proposition A.1. If the camera motion is exciting (i.e., σ 2 1 (t) > 0), then s(t) ≡ 0 ⇐⇒ χ(t) ≡ 0 and s(t) > 0 a.e. ⇐⇒ χ(t) > 0 a.e.

Proof. Being σ 2 1 the smallest eigenvalue of matrix ΩΩ T , the hypothesis σ 2 1 > 0 implies full row-rankness of the (low-rectangular) p × m matrix Ω. Considering now the error dynamics (3.12), the following holds

The first row of (3.12) then reduces to Ω T χ ≡ ∅ m which implies χ(t) ≡ 0 since matrix Ω is full row-rank by hypothesis;

• χ(t) ≡ 0 =⇒ s(t) ≡ 0: if χ(t) ≡ 0, the first row of (3.8) reduces to ˙ s = -H s. Being the matrix gain H positive definite, it follows that, at steady-state, the only possible solution is s(t) ≡ ∅ m . These two implications then prove the first item of the Proposition, that is, s(t) ≡ 0 ⇐⇒ χ(t) ≡ 0. The proof is concluded by noting that the remaining two (reverse) implications χ(t) > 0 a.e. =⇒ s(t) > 0 a.e. and s(t) > 0 a.e. =⇒ χ(t) > 0 a.e. (needed for proving the second item of the Proposition) are just the logical negations the two ones listed above.

Prop. A.1 can now be exploited for proving the initial main claim. Indeed, since E(t) is defined as the moving average of signal s(t) 2 (see (6.15)), it follows that E(t) = 0 if χ(t) ≡ 0 over (at least) the integration window T . Therefore, convergence of the estimation error χ(t) will necessarily make the quantity E(t) vanish as desired. It now remains to show that the reverse condition χ(t) > 0 a.e. =⇒ E(t) > 0 holds as well, i.e., that E(t) = 0 only if convergence of the estimation error has been reached. This again easily follows from Prop. A.1: since χ(t) > 0 interpretation, the Hamiltonian (4.3) represents the total energy of system (3.8) and consists of two energy storages: H m ( s) = 1 2 s T s (the energy of the measurable states) and H u ( χ) = 1 2α χ T χ (the energy of the unmeasurable states). Matrix Ω in (3.8) modulates the (power-preserving) interconnection between the two storages, while matrix H implements a dissipative action on the storage H m ( s). Full-rankness of matrix ΩΩ T (i.e., the usual condition σ 2 1 (t) > 0) then translates into requiring a persistent energy exchange among H m ( s) and H u ( χ) until full depletion of the initial stored energy H( s(t 0 ), χ(t 0 )) via the dissipation induced by H. The bound (A.28) then simply states that, over time, the energy stored in H m ( s) cannot exceed the total initial energy at time t = t 0 which, thanks to the initialization s(t 0 ) = s(t 0 ), takes the expression 1 2α χ(t 0 ) 2 . We conclude by noting that (A.27) (and, as a consequence, (A.28-A.29) as well) is obviously no longer valid in presence of (unmodeled) perturbations such as the several target displacemets discussed in Sect. 7.2. In this case, an external amount of energy could (in general) be injected into system (3.8) with a consequent increase of the total energy H(t) and violation of bound (A.27) (a violation which, nevertheless, did not occur in the experimental results of Sect. 7.2 because of the limited 'extra' energy produced by the unmodeled target motion).

Appendix B

Dense photometric structure estimation from motion A ll of the structure estimation problems analyzed so far in this thesis concerned a limited number of 3-D geometric structures (e.g. points, planes and so on) whose projection was assumed to correspond to a certain (limited) amount of visual features that could be identified and tracked on each image of a video sequence. In practice feature identification, tracking and matching are significantly complex tasks. Although very effective strategies have been proposed in the literature (which explains the successful experimental results reported in this thesis) the process is still prone to failure. In this appendix we wish to report some preliminary results in the context of photometric structure estimation from motion, where the term "photometric" is used with reference to a set of visual estimation and control techniques that are based on the direct use of the images (intended as a pixel light intensity map) without any (or limiting as much as possible the need for) preliminary processing step. Photometric techniques have been shown to be effective for the visual control of robotic manipulators in [START_REF] Collewet | Photometric visual servoing[END_REF][START_REF] Bakthavatchalam | Photometric moments: New promising candidates for visual servoing[END_REF]. As for the estimation problem, a certain amount of literature has been produced in this context, especially concerning the evaluation of optical flow from image sequences. The seminal works [START_REF] Horn | Determining Optical Flow[END_REF] and [LK81] demonstrated, for example, how a dense optical flow can be calculated using variational methods or local least squares optimization techniques respectively. Note that, in principle, once the optical flow has been calculated from an image sequence, then, if the camera velocity is known, a dense depth map can be estimated by, e.g., inverting (2.10) (in a least-squares sense). One difficulty that arises when trying to estimate optical flow, however, is that the problem is highly ill-posed due to the fact that the brightness constancy condition (typically used as a model for the image brightness propagation) imposes only one constraint which is not sufficient to

B.4 Surface regularization and smoothing

Regardless of the specific observer, as underlined in Remark B.3, convergence properties will depend, for each pixel, on the level of "excitation" measured as:

In general, there will always be areas of the image where σ 2 is small due to low texture level or (possibly temporarily) non-optimal camera velocity (image sliding Appendix C A primer on port-Hamiltonian systems I n this appendix we wish to provide a short and informal overview on the topic of port-Hamiltonian (pH) systems. For a more complete and formal survey on the topic we suggest to refer to [van06], from which most of this material is inspired, or to the many classical works referenced therein. We also give, at the end of the chapter, a brief introduction to bond-graphs, often used for a graphical representation of pH systems.

C.1 Introduction to port-Hamiltonian systems

Port-Hamiltonian systems theory historically originates from the combination of two very classical frameworks: on one hand the Hamiltonian approach which was initially developed in the context of analytic mechanics and, on the other hand, the network approach commonly used in many electrical engineering applications. The pH framework, in fact, provides a powerful tool to analyze (and, more importantly, control) complex systems that can be thought of as an interconnection of simpler Hamiltonian systems. The "appeal" of the pH framework also comes from the fact that it can often generate intuitive physical interpretations for very complex control systems.

Let us consider a very simple example: a mass-spring system. As well known, the dynamics of this system are governed by the second-order differential equation

where m is the mass, k is the spring elastic coefficient, e is an external force and x is the system configuration variable, i.e. the position of the mass. Equation (C.1)