SLAM Visuel 3D pour robot mobile autonome

Transcription

1 Master de Sciences Mention «Imagerie, Robotique et Ingénierie pour le Vivant» - Projet de Fin D étude - SLAM Visuel 3D pour robot mobile autonome Romain Drouilly Encadrants ECA Benoit Morisset Pierrick Daniel Encadrant ENSPS Bernard Bayle Septembre 2011

2 Résumé Un robot mobile est une machine capable de se mouvoir de manière autonome quelque soit l endroit où elle se trouve et sans la supervision de l homme. Pour cela le robot doit pouvoir cartographier son environnement à mesure qu il l explore. Ce problème de la navigation autonome d un robot dans un environnement inconnu est nommé SLAM (SLAM signifie Localisation et Cartographie Simultanées). C est un domaine de recherche très actif depuis plus de vingt ans et il est maintenant possible de faire du SLAM en 2D. Le nouveau challenge est de faire du SLAM en 3D. C est le but de cette étude. Comme il sera montré il est désormais possible de résoudre le problème du SLAM3D grâce à des capteurs innovants et des algorithmes performants. Nous montrerons qu il est possible de construire en temps réel des cartes en 3D très précises pour modéliser des environnements intérieurs. En ajoutant des textures il est alors possible de créer des vues très réalistes de la zone explorée. Par ailleurs on peut extraire en temps réel des données à partir de la carte fournie par le SLAM, dans le but de fournir au robot des informations sur les objets présents dans la scène. Ceci permet d améliorer significativement la connaissance que le robot a de l environnement et permet de le faire agir intelligemment. Abstract A mobile robot is an intelligent machine able to navigate by itself in any places without human s supervision. To do this robot has to map its environment while it is exploring it. The problem of a robot navigating an unknown environment is called the SLAM problem (SLAM stands for Simultaneous Localization And Mapping). It has been a very active field of research for more than twenty years and we now have solutions to address the 2D SLAM problem. The new challenge is to tackle the 3D SLAM question. This is the goal of this study. As it will be shown in this report it is now possible to address the 3D SLAM problem thanks to cutting edge sensors and very effective algorithms. We will demonstrate that it is possible to build in real-time very precise 3D maps to model indoor environments. Adding texture it is possible to create very realistic views of the explored area. Moreover we can extract data in real-time from the model built by the SLAM to provide the robot with information about objects in the scene. This significantly improves the knowledge the robot has of its environment and allows to make it act in an intelligent way. 1

3 Remerciements En tout premier lieu je tiens à remercier Benoit Morisset qui a suivit mon travail tout au long de ces six mois. Nos échanges réguliers ont été pour moi une très forte source de motivations et sont pour beaucoup dans la réussite de ce stage. Ses conseils avisés m ont toujours permis d avancer efficacement dans mon travail. Ensuite je tiens à remercier Pierrick Daniel, mon maitre de stage, qui m a permis de travailler sur le sujet passionnant et plus que jamais d actualité qu est le SLAM et grâce à qui j ai découvert ce merveilleux outil qu est la Kinect. Je tiens aussi à remercier Gwenaël Dunand pour m avoir fait découvrir les Design Pattern et m avoir fait partager son expérience de la recherche et son expertise en programmation. Mes sincères remerciements vont à toute l équipe d ECA Saclay pour l accueil chaleureux dont j ai bénéficié et à toutes les personnes avec qui j ai pu partager ma passion pour la robotique. Enfin je tiens à remercier tous ceux qui, à Willow Garage ou ailleurs, au travers d initiatives collectives ou personnelles, mettent à disposition leur travail pour les progrès de tous. 2

4 Sommaire A. Introduction... 6 B. Objectifs du projet... 7 C. Introduction au problème du SLAM... 8 I. Autonomie des robots Robotique: de la Fiction à la Réalité Qu'est ce qu'un robot autonome? Percevoir son environnement... 9 II. Odométrie Visuelle Points d intérêt RANSAC ICP III. SLAM Définition du problème Principaux paradigmes Recherches actuelles IV. TORO : SLAM 3D Principe Graph-SLAM TORO : formalisme V. Segmentation des nuages de points et reconstruction 3D Principe de la segmentation Mise en œuvre: cas des objets plans D. Contribution au SLAM I. Contexte général de l étude ROS Kinect II. Analyse de l existant

5 III. Solutions retenues IV. RGBD Mapping L algorithme Estimation de mouvement Optimisation globale du graphe et fermeture de boucle Implémentation Schéma de fonctionnement du SLAM V. Descripteur NARFs Tests du détecteur Conclusion générale sur les NARFs VI. Segmentation et Reconstruction 3D Introduction Détection de plans dans un nuage de points Détection des contours d un plan Appariement de plans Problème lié à la reconstruction 3D temps réel Recollage des textures Organisation du code pour la reconstruction 3D E. Résultats I. Matériel de tests II. Test du SLAM Fidélité de la reconstruction 3D Amélioration due à ICP performances avec et sans supervision Robustesse aux variations de luminosité III. Test de la reconstruction 3D Test de la détection du sol en temps réel Test de la reconstruction 3D IV. Discussion sur le SLAM et la reconstruction 3D F. Conclusion de l étude G. Bibliographie

6 H. Annexes I. ECA Présentation du groupe ECA Les Activités du Pôle de Saclay II. Kd-tree III. SIFT Détection des points d'intérêt Calcul des descripteurs IV. SURF (Speeded Up Robust Feature) Détection de points d'intérêts Calcul des descripteurs V. Précision subpixellique

7 A. Introduction ECA conçoit des robots pour l intervention en situations extrêmes : incendie, tremblement de terre, accident nucléaire, déminage, intervention policière, renseignements etc. Dans toutes ces situations le robot vient en aide à l Homme en explorant des zones inaccessibles ou trop dangereuses pour envoyer, au moins dans un premier temps, des êtres vivants. Les robots doivent donc d une part être suffisamment autonomes pour remplir leur mission sans la supervision de l Homme et d autre part être capable de fournir à l utilisateur des informations sur la topographie du lieu, le plus souvent sous la forme d une carte. L objectif du projet est donc de développer un module permettant de rendre le robot autonome dans la tâche d exploration. Plus précisément, le module doit permettre au robot de dresser la carte d un environnement inconnu au fur et à mesure qu il l explore. Cette carte servira à la fois au robot à se localiser pour évoluer de manière autonome et à l utilisateur humain pour récupérer des informations sur les lieux explorés. Le problème de l autonomie d un robot dans la tâche d exploration d un lieu inconnu est appelé SLAM 1. C est un domaine de recherche très actif depuis plusieurs décennies et beaucoup de solutions existent pour la cartographie 2D. Le défi est ici de réaliser la modélisation en 3D de l environnement et ce uniquement à partir d informations visuelles. Après une présentation des objectifs du projet, les fondements du problème du SLAM seront présentés en détails en les replaçant dans leur contexte originel. Une présentation formelle du problème sera introduite en insistant plus particulièrement sur le paradigme qui sera utilisé dans l étude. C est ensuite le travail de stage luimême qui sera abordé avec la présentation successive des études sur lesquels il est fondé, des solutions proposées et des résultats obtenus. Chaque fois que nécessaire les choix effectués seront justifiés. Finalement les résultats seront discutés et un certain nombre d observations seront faite sur la robotique moderne. 1 Simultaneous Localization and Mapping 6

8 B. Objectifs du projet Lorsqu un robot est employé pour explorer une zone inaccessible à l homme, il doit être capable de fournir des informations à l utilisateur sur la topologie des lieux visités. Il faut donc qu il construise une carte de son environnement. Il doit en outre faire preuve d une autonomie suffisante pour pouvoir explorer la zone et trouver son chemin même si les communications sont interrompues. Le robot doit donc être capable de se localiser en permanence dans l environnement qu il explore. L objectif du stage est donc de mettre en place un module de SLAM pour un robot d intervention en milieu hostile. Plus précisément, le projet vise à doter le robot de la capacité à cartographier en 3D un environnement inconnu. Un capteur innovant a été retenu pour cette tâche : la Kinect de Microsoft. Il s agit d une caméra RGBD, autrement dit d un système d acquisition fournissant à la fois des images et des données métriques. Il faudra donc exploiter au mieux la richesse des informations fournies pour construire un modèle 3D de l environnement visuellement réaliste. Les contraintes principales sont donc de : - réaliser un modèle précis de l environnement permettant au robot de se localiser en vue de planifier sa trajectoire - d offrir un rendu réaliste de la scène pour un observateur humain - d effectuer la reconstruction en temps réel Une contrainte secondaire est d offrir une bonne robustesse aux variations de luminosité. 7

9 C. Introduction au problème du SLAM L objectif de ce chapitre est de présenter le contexte général du stage et d expliquer ce qui a motivé la problématique en la replaçant dans son contexte originel. Plusieurs questions seront abordées concernant les développements présents et futurs de la robotique, notamment: quels sont les verrous technologiques qui empêchent aujourd hui un robot de conduire une mission de manière parfaitement autonome? Quelles sont les pistes de recherche actuelles pour résoudre ces problèmes? Quels sont les succès déjà remportés? I. Autonomie des robots 1. Robotique: de la Fiction à la Réalité Loin des images des films de science fiction, montrant hommes et robots cohabitant harmonieusement dans des environnements complexes, la robotique s est pendant longtemps limitée à ses applications industrielles, se contentant de remplacer l homme dans l exécution de tâches simples et répétitives. Cette limitation venait notamment de l impossibilité de créer des systèmes capables d évoluer dans un environnement réel. En effet gérer l extrême diversité des situations dans lesquels un robot peut se trouver est très difficile. Il est clairement impossible de prévoir tous les cas de figure pour préparer des comportements types en fonction des situations rencontrées. Il faut dès lors que le robot soit à même de percevoir son environnement intelligemment pour apprendre et comprendre cet environnement en vu de planifier ses actions. Loin d avoir résolu totalement ce problème la robotique a cependant connu ces dernières années plusieurs victoires importantes qui ont conduit à de profonds changements. Qu ils soient à pattes ou à roues, volant ou rampant, les robots sont désormais capables de se mouvoir dans des environnements aussi complexes que le sol martien ou un hall de musée avec très peu de supervision de la part de l homme. Voyons comment ces succès ont étés rendus possibles. 8

10 2. Qu'est ce qu'un robot autonome? L'autonomie des robots est un enjeu clé de la robotique moderne. Un système robotisé n'a de raison d'être que s'il est capable d'effectuer un maximum de tâches sans la supervision de l'homme. Un système téléguidé représente bien moins d'intérêt puisque l'opérateur doit prendre en charge les tâches de bas niveau comme la navigation au lieu de se concentrer sur les objectifs essentiels de sa mission. Si l'on veut définir clairement ce qu'est un robot mobile autonome on peut dire qu'il s'agit d'un système capable de: se localiser dans son environnement. Ceci répond à la question «où suis-je?». trouver des zones d'intérêt à explorer ou des objets dans son environnement et liés à sa mission. Ceci répond à la question «où dois-je aller?». de planifier ses actions, pour par exemple définir une trajectoire pour se rendre d'un point A à un point B. Ceci répond à la question «comment dois-je y aller?». d'interagir, le cas échéant, avec son environnement pour réaliser certaines tâches. Il peut s'agir par exemple de trouver et d'actionner une poignée de porte pour pouvoir passer d'une pièce à une autre. Ceci répond à la question «Quelle est la fonction de cet objet?» On distingue donc deux types principaux de contraintes pour l'autonomie des robots: la capacité d'extraire des données de l'environnement (localiser des objets, des obstacles, le robot etc.) et la capacité à traiter ces informations intelligemment (prendre des décisions, savoir comment interagir avec tel ou tel objet). A ceci il faudrait encore ajouter d'autres aspects concernant notamment la sécurité du robot ou des êtres vivants avec lesquels il cohabite. Cependant c'est surtout le premier point qui nous intéressera dans la suite. 3. Percevoir son environnement Quelque soit la mission qui lui est confiée, un robot mobile doit être capable de percevoir intelligemment son environnement. C'est à dire qu'il doit être en mesure de capter l'information et de la traiter en vue de produire des données exploitables. 9

11 Le robot doit notamment être capable de construire sa propre représentation de l'environnement. Cette tâche est nommée la cartographie. Il doit pouvoir fabriquer, sur la base des informations fournies par ses capteurs, une carte lui permettant de définir des zones d'intérêt à explorer, des zones dangereuses à éviter ou encore trouver les zones navigables. Cette carte pourra aussi servir de modèle de l'environnement à un opérateur humain qui voudrait intervenir dans une zone a priori inconnue. Or lorsque le robot explore une zone inconnue il doit aussi être capable de se localiser sur la carte à mesure qu'il la construit. La réalisation simultanée de la cartographie et de la localisation est un problème fondamental en robotique mobile connu sous le nom de SLAM (Simultaneous Localisation and Mapping). Sa résolution est un préalable indispensable à l'automatisation totale des systèmes, sans quoi les robots seraient incapables de se mouvoir dans un environnement inconnu. Dans la suite nous verrons donc quelles sont les étapes nécessaires à la réalisation du SLAM. 10

12 II. Odométrie Visuelle Pour la réalisation d une carte sur la base d informations purement visuelles, il faut pouvoir estimer le mouvement de la caméra pour mettre en correspondance les différentes vues. Dans le cas d un système monoculaire cette tâche est rendue difficile par l impossibilité d estimer les distances à partir d une seule image. Il faut donc utiliser des séquences d images pour initialiser les distances. Ici la Kinect fournit directement la mesure de distance, on peut donc connaitre la position des points observés dans chaque image. 1. Points d intérêt Pour estimer le mouvement de la caméra entre deux images il faut pouvoir déterminer comment ont bougés les objets observés. Pour cela on ne peut utiliser que des objets visibles dans les deux images et que l on est capable de retrouver facilement dans chacune d elles. Pour des raisons de simplicité, on utilise en général des points d intérêt comme objets de référence. Un point d'intérêt est un point qui caractérise de façon unique une partie de l'image. Plus précisément il s agit d un point localisé finement, augmenté de son voisinage. Pour être utile ce point doit être très bien identifié à l aide d un descripteur unique et il doit être possible de le retrouver facilement. Il existe une multitude de méthode de détection/description de points d intérêt parmi lesquelles SIFT et SURF. Il s agit de méthodes particulièrement appréciées pour leur robustesse et leur efficacité. Elles sont décrites en détails en annexe. Le principe de détection correspond à rechercher des zones d intensité lumineuse particulière et offrant une grande stabilité. Pour cela on utilise le plus souvent des méthodes basées sur le calcul du gradient de luminosité. Les extrema correspondent aux points où s annule le gradient. Il faut noter que ces points d intérêt ne sont pas nécessairement des pixels. En effet on peut obtenir la position des points avec une précision subpixellique par des méthodes d interpolation 2. Une fois les points identifiés il faut décrire l'information locale dans l'image de façon unique et aussi invariante que possible de l'échelle d observation, des rotations, des variations de luminosité etc. Pour cela on calcule un descripteur pour chaque point. On commence par déterminer une orientation privilégiée basée sur l observation de l entourage du point. Le descripteur est alors simplement une suite 2 Voir annexes 11

13 de valeurs (64 ou 128 le plus souvent) qui décrivent le voisinage à partir de cette direction privilégiée. Ceci permet de rester invariant par rotation. Figure 1: Points d'intérêt détectés entre deux images. Les traits verts symbolisent les déplacements entre deux itérations. Les traits rouges signalent les déplacements non conformes par rapport aux autres points 2. RANSAC Une fois les points d intérêts calculés dans chaque image il faut pouvoir déterminer quel est le mouvement global de la caméra. Pour cela on doit trouver quelle transformation permet de recoller correctement les deux nuages de points formés par les points d intérêt. La recherche de cette transformation est une tâche complexe, d'autant plus que les nuages sont bruités. Il existe plusieurs types de méthodes qui présentent toutes des avantages et des inconvénients, la rapidité de calcul étant souvent incompatible avec la précision. L'algorithme RANSAC (RANdom SAmple Consensus) est classiquement utilisé pour extraire un modèle d'un ensemble de données bruitées. Dans le cas présent RANSAC est utilisé pour déterminer quels sont les points qui permettent d estimer la transformation entre les deux nuages le plus correctement possible. Autrement dit RANSAC permet de filtrer les points trop bruités. Concrètement trois points dans chaque nuage sont tirés au hasard pour déterminer une base. La transformation liant les deux bases est alors calculée. Les nuages sont recollés suivant cette transformation et la distance entre chaque couple de point est estimée. Un score est alors attribué à la transformation : si beaucoup de couples de points se superposent le score est élevé, sinon il est faible. Finalement la 12

14 transformation qui possède le plus haut score est conservée : c est elle qui permet de recoller le plus de points ensemble, c est donc elle qui a le plus de chance d être juste. Cette méthode permet en plus d évaluer la transformation liant les deux nuages de point de filtrer les points qui n entrent pas dans le modèle principal. L estimation de mouvement avec RANSAC est en générale assez bonne mais dans le cadre de la construction d une carte il est préférable d affiner cette estimation avec une autre méthode comme ICP. 3. ICP ICP (Iterative Closest Point) est un algorithme qui permet de minimiser la distance entre deux nuages de points A et B. Il est utilisé notamment lorsque l'on veut reconstruire des objets 3D à partir de différentes vues, pour la construction de cartes etc. L'avantage d'icp par rapport à RANSAC est qu'il permet d'estimer les mouvements avec une grande finesse. Il nécessite cependant en entrée une estimation de la transformation a trouver sans quoi l'algorithme peut rester bloqué dans une configuration non optimale (minimum local). Le principe de l'algorithme est assez simple. Il consiste à appareiller les points deux à deux en utilisant une fonction de cout quadratique (distance euclidienne). A chaque itération on estime la transformation entre les deux nuages A et B, la source et la cible, à partir de ces couples de points. On déplace le nuage cible conformément à la transformation calculée et la distance entre les deux nuages est alors réévaluée. L'algorithme stoppe lorsque la distance minimale entre les deux nuages est atteinte. Celle-ci est fixée au début de l'algorithme en fonction de la précision souhaitée. De cette manière on obtient une estimation fine de la transformation entre les deux nuages de points qui permet un recollage presque parfait. Avec ces méthodes combinées on peut donc estimer le déplacement de la caméra entre deux images successives. On reconstruit alors au fur et à mesure des déplacements la trajectoire de la caméra tout en modélisant l espace 3D. Cependant bien que la précision de ces méthodes soit bonne, on constate une dérive de la position au cours du temps due à l accumulation des erreurs de mesure. Ceci s observe notamment lorsque le robot après avoir parcouru un certain trajet revient à une position déjà visitée. La localisation courante du robot est alors souvent différente de la localisation initiale. Il est donc nécessaire de créer une boucle de supervision qui détecte lorsque le robot repasse par un endroit déjà exploré. L algorithme permet alors en constatant l erreur sur l estimation de position de corriger la trajectoire et de la rendre plus cohérente. Cette étape est nommée fermeture de boucle. Par ailleurs la carte créée par le robot peut être optimisée en fonction de la connaissance que l on a des erreurs de mesure. L optimisation de la 13

15 carte et la détection de la fermeture de boucle sont essentielle à l obtention d une carte de qualité. C est l algorithme de SLAM qui réalise ces étapes. 14

16 III. SLAM Le SLAM est considéré comme l'un des problèmes les plus fondamentaux de la robotique moderne. Comme évoqué plus haut il permet de corriger les erreurs accumulées pendant la phase d estimation de mouvement en vue d obtenir un modèle cohérent de l environnement. Cette partie présente le problème formellement ainsi que ses trois principaux paradigmes. 1. Définition du problème Un robot mobile, doté de la capacité de percevoir son environnement, explore un milieu inconnu. Il part d'un point de coordonnées connues x0. L'estimation du mouvement du robot est entachée d'incertitudes à chaque mesure, qui rendent l'estimation de position de plus en plus imprécise. Le SLAM vise à résoudre le problème de la création d'une carte non biaisée de l'environnement dans ce contexte. Formellement on utilise les probabilités pour décrire le problème. Soit x t la position du robot à l'instant t (en 2D ou en 3D), on note ={ 0, 1,..., } La trajectoire du robot jusqu'à l'instant T. Soit ut l'estimation de mouvement faite entre les instant t-1 et t par l'odométrie. On note: ={ 0, 1,..., } L ensemble du chemin parcouru jusqu'à l'instant T. Enfin soit m la carte de l'environnement réel, considéré comme statique. Les mesure faites par le robot mettent en relation les estimations de déplacement x t et la carte observée, m. Si l'on considère que le robot fait une mesure en chaque point alors on peut noter la séquence de mesures: = { 1, 2, 3,..., } Le problème est maintenant de retrouver un modèle de l'environnement m ainsi que la trajectoire XT du robot à partir de l'odométrie UT et des observations ZT. 15

17 Il existe deux formes de SLAM. Le premier nommé «online problem» consiste à calculer à chaque instant la position actuelle du robot en fonction des estimations de mouvements et des observations ce qui se note: (,, ) La seconde est nommée «full SLAM problem» et consiste cette fois à calculer à chaque instant l'ensemble de la trajectoire à partir de l'odométrie et des observations. On définit alors ce problème par: (,, ) Enfin pour résoudre le problème du SLAM, le robot a besoin de deux informations supplémentaires que sont : le modèle reliant les mesures issues de l'odométrie avec les positions du robot le modèle permettant de relier les mesures zt avec la position du robot. Ces modèles sont exprimés respectivement par les formules ( 1, ) qui traduit la probabilité d'être à un point de coordonnées xt en partant du point xt-1 et en parcourant le chemin ut ; et (, ) qui est la probabilité de faire l'observation zt en étant à la position donnée xt connaissant m. 2. Principaux paradigmes Il existe trois manières principales de traiter le problème du SLAM dont dérivent beaucoup d'algorithmes. 2.1 EKF-SLAM La première méthode apparue est basée sur le filtrage de Kalman Etendue. Elle utilise un vecteur d'état pour représenter la position du robot et des amers dans la scène, auquel est associé une matrice d'erreur représentant les incertitudes sur les positions, les observations et les corrélations entre les différentes variables du vecteur d'état. Alors que le robot se déplace le vecteur d'état et la matrice d'erreurs sont mis à jour en utilisant un filtre de Kalman étendu. A chaque nouvel amer observé de nouvelles variables d'état sont ajoutés au vecteur d'état du système; la taille de la matrice de covariance croit quadratiquement. 16

18 Cette approche a été la première développée. Elle est de moins en moins utilisée aujourd'hui notamment du fait des temps de calcul qui la rendent moins intéressante que les autres. 2.2 SLAM basé sur les filtres particulaires Une seconde approche pour traiter le problème du slam est basée sur l'utilisation de filtres particulaires. Le principe est de suivre un grand nombre d'hypothèses en parallèle qui sont autant de trajectoires possibles. Ces différentes hypothèses correspondent à un échantillonnage de la distribution de probabilité des trajectoires. Pour chacune d'elles on construit la carte en fonction des perceptions du robot à l'aide d'un filtre de Kalman. Cependant dans ce cas le traitement est simplifié puisque la trajectoire est connue: les perceptions successives des différents amers ne sont plus corrélées et la matrice de covariance se simplifie puisqu'on ne mémorise plus que les variances individuelles des amers. La complexité des calculs passe ainsi de 0(N²) à O(N). Le problème principal de cette technique est que la représentation de la carte et de la trajectoire du robot devient vite très lourde. En effet il faut que le nombre de particules soit suffisant pour échantillonner correctement la distribution de probabilité des trajectoires. Par ailleurs lors de fermeture de boucle, seules les trajectoires correctes sont retenues ce qui entraine un ré-échantillonnage du filtre particulaire, conduisant à une forte perte d'information. Ce problème est d'autant plus important que l'environnement contient plusieurs cycles (plusieurs ré échantillonnages successifs). 2.3 Graph-SLAM La troisième méthode est basée sur la théorie des graphes. Elle consiste à considérer les positions successives du robot et des différents amers comme les nœuds d'un graphe. Les arrêtes sont alors constituées des contraintes fournies par l'odométrie ou par l'observation des amers. Pour illustrer ce propos voyons comment procède le robot pour construire la carte Construction d'un graphe A l'instant de départ le robot observe le amer 1. Le graphe est donc constitué de deux nœuds, la position du robot et celle du amer, ainsi que d'une arrête : la contrainte observationnelle entre le robot et le amer 1. 17

19 A l'instant t2 le robot a avancé d'une distance u2 fournie par l'odométrie et observe les amers 1 et 2. Le graphe est maintenant constitué de quatre nœuds: les positions du robot à t1 et t2, reliées par l'estimation de déplacement (odométrie); les positions des amers, reliées aux positions du robot par les contraintes observationnelles. Figure 2 : exemple de création d un graphe Optimisation du graphe Une fois le graphe construit on cherche à l'optimiser en minimisant l'erreur sur les contraintes du graphe. Du fait des incertitudes sur les mesures il existe en effet des erreurs dans l'estimation de position du robot et des amers. Le «full SLAM problem» a été défini plus haut comme la probabilité d'avoir une trajectoire et un modèle de l'environnement pour un ensemble de mesures données (odométrie plus observations), ce qui s écrit (,, ). En prenant le log on obtient: log (,, ) = + log ( 1, ) + (, ) Chacun des éléments de cette somme correspond à un mouvement du robot et forme une arrête du graphe. L'optimisation du graphe consiste donc finalement à calculer :, =, (,, ) Où * est le symbole pour la solution optimale. Le gros avantage du Graph-SLAM est qu'il permet de gérer les cartes composées d'un très grand nombre de nœuds (>10⁸ à ce jour) ce qui est impossible avec les autres techniques. Cependant l'optimisation du graphe peut être très lourde. 18

20 3. Recherches actuelles Le SLAM a fait l'objet de nombreux développements ces dernières années qui ont permis d'arriver à des solutions variées plus ou moins performantes. Il existe aujourd'hui plusieurs algorithmes ayant fait leurs preuves pour la cartographie 2D avec des caméras ou des Lasers et plusieurs logiciels proposent leur propre module de SLAM 2D. L'objectif est aujourd'hui de passer à la 3D. Reconstruire l'environnement intégralement permettrait de surpasser les performances des robots actuels en leur permettant d'accéder à des données beaucoup plus riches. La cartographie 3D autorise en effet de rapprocher considérablement la perception des robots de la vision animale et donc d'améliorer sensiblement leur potentielle compréhension de leur environnement. Pour évoluer dans un environnement complexe il faut en effet être capable d'en percevoir la complexité. La cartographie 3D permettrait en outre de surmonter des problèmes classiques rencontrés en 2D comme la détection d'une table ou d'obstacles situés à différentes hauteurs, d'envisager l'exploration de milieux en plusieurs dimensions (plusieurs étages d'un bâtiment, milieu naturel etc) d'ajouter d'autres types d'informations aux cartes que les informations métriques, comme par exemple la couleur. Le passage à la 3D présente bien sur des défis supplémentaires notamment en termes de volume de données à traiter. Cependant comme nous le verrons dans la suite ceci est aujourd'hui possible. 19

21 IV. TORO : SLAM 3D Cette partie présente TORO, l algorithme de SLAM utilisé dans la suite. Après un rapide descriptif des principes utilisés dans TORO, le formalisme est présenté en détail. 1. Principe Lorsqu'un robot se déplace, il observe des amers dont il évalue la position dans la scène. Entre deux poses successives le robot peut estimer sa trajectoire en observant comment les positions des amers dans son champ d'observation ont évoluées relativement à lui. Il créer donc une contrainte entre deux poses successives. Figure 3 : Position du robot (triangles bleus) et des amers (étoiles) au cours du temps. Les flèches jaunes sont les contraintes dues à l odométrie ou à la vision. Figure 4 : schématisation de principe de base exploité dans TORO : les contraintes visuelles à différents instants sont transformées pour créer des contraintes entre des poses non successives (flèches jaunes pales). 20

22 Cependant, il arrive fréquemment que le robot observe plus de deux fois le même amer. Il est alors possible de créer de nouvelles contraintes tout au long de la trajectoire entre des poses non successives. Ceci permet de réévaluer la trajectoire à chaque itération en prenant en compte l'ensemble des observations. C'est cette idée qui est exploitée dans TORO. 2. Graph-SLAM TORO utilise une représentation des données sous forme de graphe. Autrement dit il n'existe pas de carte métrique au sens usuel du terme. Pour construire le graphe TORO réduit l'ensemble des données à une succession de nœuds, les poses du robot, reliées par des contraintes entre ses nœuds correspondant aux déplacements estimés. Les contraintes sont déterminées à partir des observations faites à chaque itération. Ici on ne considère pas la position d'un amer comme une contrainte. On déduit à partir de la position estimée à différentes poses des relations entre les nœuds qui elles sont prises en compte comme des contraintes. L'avantage principal de cette représentation est que les contraintes modélisent mieux les dépendances entre les données que dans une représentation classique. En effet avec cette représentation sont mis en relation un ensemble d'observations faites à un instant t avec un autre ensemble d'observations faites à un instant ultérieur, et non pas les observations elles-mêmes. Par ailleurs dans cette représentation l'optimisation est complètement découplée du modèle sous-jacent (optimisation de graphe), ce qui permet une grande souplesse d'utilisation de l'algorithme. La contrepartie est cependant que pour un amers observé N fois il existe N² arrêtes, donc la complexité de la résolution croît rapidement. 3. TORO : formalisme L'algorithme consiste à trouver la configuration des nœuds du graphe qui minimise l'erreur introduite par les contraintes. Ainsi en notant : x = (x1 xn ) le vecteur des paramètres décrivant la configuration des nœuds (qui ne sont nécessairement des relations métriques); 21

23 δ ji la contrainte entre le nœud i et j, qui correspond à l'arrête ij ; Ω ji la matrice modélisant les incertitudes sur les contraintes; f ji (x) la fonction définissant la contrainte non bruitée entre les points i et j; On peut définir l'erreur sur la contrainte entre les nœuds j et i, notée e ji, par : ( ) = ( ) On peut aussi définir le résidu r ij par : ( ) = ( ). L'intérêt de l'utilisation du résidu est que ce vecteur donne la 'direction' dans laquelle il faut changer le graphe (l erreur diminue dans ce sens). Si on fait l'hypothèse d'un bruit gaussien on peut exprimer le log négatif de la probabilité 3 de f ij 4 par : ( ) ( ( ) ) ( ( ) ) = ( ) ( ) = ( ) ( ). En supposant que les observations sont indépendantes 5, le log négatif de la probabilité pour une configuration x est : 3 L intérêt de prendre le log négatif est de ramener la probabilité sur une plage de 0 à l infini, de transformer les produits en sommes et de se ramener à une fonction monotone. 4 Voir chapitre consacré au Graph-SLAM 5 Ceci suppose que la probabilité de A et B est la probabilité de A fois celle de B 22

24 ( ) = ( ), = ( ) ( ), Finalement l'objectif est de trouver la configuration x* qui maximise la probabilité des observations, ce qui s'écrit : = ( ) 6 Où * est le symbole pour désigner la solution optimale. 3.1 Descente de gradient stochastique(sgd) Pour minimiser la fonction F une approche présenté dans par Olson [7] consiste à sélectionner de manière itérative les contraintes et à déplacer les nœuds dans le sens permettant une diminution de l'erreur. Les nœuds sont mis à jour selon la formule : = + Où x représente l ensemble des variables décrivant la localisation de la pose dans le graphe. H est la hessienne du système qui représente la courbure de la fonction d erreur. J est la jacobienne. Le facteur lambda sert simplement à éviter des oscillations dans l'algorithme en diminuant progressivement la part du résidu utilisée dans la mise à jour des variables. 3.2 Parametrisation La qualité de la parametrisation joue un rôle important pour l'efficacité de l'algorithme. En effet avec la méthode SGD il faut à chaque itération définir quelle partie du graphe doit être mise à jour. Avec la méthode présentée par Olson [7] le 6 On démontre qu en prenant le log négatif rechercher la probabilité maximum revient à calculer argmin 23

25 problème est que pour l'optimisation d'une contrainte entre les nœuds i et k, il faudra mettre à jour tous les nœuds j pour j=i à k. Dans TORO une autre méthode exploitant au mieux la topologie du graphe a été développée utilisant un arbre pour la représentation des données. Ceci permet de décomposer le problème en plusieurs sous problèmes plus simples qui correspondent soit à des contraintes de l'arbre soit des contraintes additionnelles (fermeture de boucle). Figure 5: passage du graphe à un arbre pour la phase d'optimisation Chaque équation du SGD résout l'un de ces sous-problèmes. Dans cet algorithme on traite les rotations et les translations séparément. Si l'on considère les rotations, elles induisent des non linéarités importantes dans le problème. Le SGD répartit simplement le résidu le long de la trajectoire. Ceci se fait au travers du terme : = Pour ce qui est de la translation la solution très intuitive consiste simplement à distribuer le déplacement le long de la trajectoire. Figure 6: distribution des erreurs en rotation et translation le long de la trajectoire 24

26 V. Segmentation des nuages de points et reconstruction 3D Pour représenter une scène en 3D il faut pouvoir mesurer la position des objets observés dans l espace. Pour l'heure les capteurs permettant de déterminer les positions en 3D utilisent soit des lasers soit des systèmes vidéos et fournissent une représentation de la scène sous forme de nuages de points. La visualisation d une carte sous cette forme n est pas optimale d une part parce que les points n ont pas de signification physique réelle, ils sont simplement une vision échantillonnée de la réalité sous-jacente, et d autre part parce que pour représenter des surfaces il faut un très grand nombre de points. Cette manière de représenter les données impose donc une étape de reconstruction de la scène qui vise à extraire du nuage de point les données véritablement pertinentes, à savoir les primitives géométriques. Prenons l'exemple d'une sphère. Lorsqu'elle est observée par un Laser le résultat fourni est un groupe de points répartis dans l'espace selon une relation bien précise donnée par la géométrie de l'objet, ici une surface sphérique. La question qui se pose est donc de savoir comment retrouver les propriétés de la surface à partir de l ensemble des points qui la forment. Dans la pratique un objet n'est jamais observé isolement, il est inclus dans un nuage représentant la scène globale et sa taille relativement au reste du nuage peut être faible. Il faut donc dans le même temps segmenter le nuage de point pour en extraire les sous-ensembles formés par les objets. 1. Principe de la segmentation Pour pouvoir identifier un ensemble de points correspondant à un objet particulier il faut connaître la relation liant les points. Il faut donc disposer d'un modèle de l'objet que l on cherche. Le principe de la recherche consiste dans un premier temps à identifier un ensemble minimal décrivant correctement le modèle (par exemple trois points pour un plan) puis à essayer d étendre le support de ce modèle en cherchant les points qui s accordent avec lui. Pour l ensemble des formes géométriques simples il est possible de définir un modèle paramétrique donc cette technique ne pose pas de problème. Cependant pour des objets plus complexes il n'existe pas toujours de modèle mathématiques simples. On peut alors imaginer décomposer l'objet en formes simples puis inclure dans un second temps des contraintes entre ces formes élémentaires. Par exemple si l'on souhaite détecter un cube on peut chercher chacune de ces faces, des plans, et 25

27 ensuite vérifier qu'elles sont bien perpendiculaires ou parallèles les unes aux autres. L avantage de cette méthode est qu elle permet de s appuyer sur les techniques de détection classiques. En revanche elle n utilise pas toute l information disponible directement pour la détection. 2. Mise en œuvre: cas des objets plans Ici on se limite au cas des objets plans. Ceci se justifie du fait de l environnement urbain dans lequel se trouve le robot. Les objets ont en majorité des formes simples composées de faces planes : sol, murs, portes, meubles, ordinateurs etc. On cherche à identifier un plan dans un nuage. Il faut trouver l'ensemble des points dont les positions remplissent le critère: Pour a, b, c et d choisis = 0 La première étape consiste à tirer un nombre de point minimal (trois) dans le nuage permettant de calculer un premier jeu de paramètres du modèle (a, b, c et d). Avec ces trois points est formé le vecteur perpendiculaire au plan dont les coordonnées sont les paramètres a, b et c. d est identifié en calculant + + pour l un des points. On évalue alors le nombre de point qui concordent avec le modèle. Pour cela on vérifie que les coordonnées de chaque point candidat respectent l équation paramétrique (à un seuil près). Si c est le cas alors le point est ajouté à l ensemble et les coefficients du plan sont réévalués. L évaluation des coefficients du plan optimal pour un ensemble de point se fait en plusieurs étapes. Premièrement on calcule le barycentre du nuage Où pi est la position d un point. Ensuite on calcule la matrice : = 1 Г (, ) Г (, ) Г (, ) = Г (, ) Г (, ) Г (, ) Г (, ) Г (, ) Г (, ) Où Г (, )= ( )( ). Le vecteur normal optimal à ce plan est le vecteur correspondant à la valeur propre la plus petite de la matrice C. Finalement on trouve le coefficient d comme précédemment en utilisant le centre de masse comme point. L ajout de point se fait de manière itérative jusqu à ce qu il n y ait plus aucun point à ajouter. Si le nombre de point est trop faible le modèle est rejeté pour éviter les fausses détections. Sinon 26

28 il est accepté et un premier sous-ensemble peut être identifié. Une fois l objet détecté on dispose donc d un modèle qui renseigne le robot sur la nature de l'objet mais aussi sur ses propriétés. On peut en fonction de la valeur des paramètres arriver à classer l objet (un cube de 20cm peut être une boite, un cube de 20m est plus probablement un immeuble!) ou adapter la manière d interagir avec eux (un objet de 5cm peut être saisie avec une pince, s il fait 50cm il est plus judicieux de la saisir avec deux bras (robot humanoïde). 27

29 D. Contribution au SLAM Cette partie présente le cœur du travail de stage sur le SLAM. Après une rapide présentation des outils utilisés sont présentés en détails les différentes pistes explorées et la solution finalement choisie pour réaliser le SLAM Visuel 3D. C est surtout la démarche qui sera mise en avant. I. Contexte général de l étude 1. ROS Avec l'avènement de l'informatique et d'internet des communautés d'utilisateurs se sont rapidement formés pour développer des logiciels Open Source, le meilleur exemple étant très probablement LINUX. La communauté robotique voit à son tour apparaître un certain nombre de travaux produits par des communautés de roboticiens mettant en commun leur savoir pour réaliser des logiciels complexes qu'il n'aurait pas été possible de réaliser autrement. Parmi ces travaux l'un des plus abouti est sans doute ROS. ROS est un méta système d'exploitation open-source qui fournit l'ensemble des services classiquement fournis par un système d'exploitation comme la gestion des tâches de bas niveau ou les communications inter-process. Il fournit aussi un très grand nombre d'outils et de bibliothèques facilitant les développements Communication Inter-process Dans Ros chaque processus est appelé nœud. Il appartient à un réseau de nœuds (graphes) communicant entre eux. Il est possible de mettre en place plusieurs types de communication entre ces nœuds, soit par le système de publication/souscription à un topique soit via les services. Pour décrire la première manière de communiquer imagine que nous ayons un premier nœud qui récupère les données de la kinect et effectue un prétraitement dessus. Deux autres utilisent ces données prétraitées sur lesquelles ils effectuent d'autres traitements. L'idée utilisée dans ROS est de créer un topique sur lequel le premier nœud va publier ces données. Les autres qui sont intéressés par ces données vont alors souscrire à ce topique pour recevoir ces données. 28

30 Le premier nœud connait la liste de ceux qui ont souscris au topique. A chaque fois qu'il publie des informations nouvelles dessus il prévient les programmes ayant souscris au topique que des informations nouvelles sont disponibles. De mêmes les autres nœuds peuvent à leur tour publier des données sur d'autres topiques qui intéresseront d'autres programmes. Ainsi chacun peut à la fois souscrire à certains topiques et publier sur d'autres. Les services fonctionnent un peu différemment. L'idée est ici de permettre à un processus d'accéder à la donnée dont il a besoin sur demande. Un nœud envoie une requête à un autre nœud qui lui envoie une réponse correspondant aux données demandées. 1.2 Bibliothèques utilisées ROS fournit un certain nombre de bibliothèques qui ont étés utilisée pour l'écriture du code. La plus utile est sans aucun doute PCL Lib. Il s'agit d'une bibliothèque dédiée au traitement de données type nuage de points (Point Cloud Librairy). Elle fournit un grand nombre de méthodes permettant le filtrage de nuage de points, la construction d'octree et la recherche du plus proche voisin, la détection de point d'intérêt, la segmentation etc. 2. Kinect Le passage de cartes 2D à des cartes 3D requière, outre le fait de mettre au point des algorithmes performants, le recours à des capteurs appropriés, permettant de mesurer les distances dans l'espace 3D. Par ailleurs si l'on souhaite en plus des données métriques ajouter des informations de couleurs/textures à la carte, il faut coupler le système de mesure 3D à une caméra. Ce type de matériel, nommé caméra RGB-D, existe depuis plusieurs années mais représente un cout prohibitif pour la plupart des applications robotiques. Cependant un nouveau type de matériel permettant d'atteindre cet objectif a fait son apparition récemment: la Kinect de Microsoft/PrimeSense. Initialement 29

31 destinée au monde du jeu vidéo, la Kinect représente une véritable révolution dans le domaine de la robotique. Pour une somme modique (130euros environ), elle permet de disposer d'un capteur fournissant à la fois des données 3D et des images. 2.1 Fonctionnement La Kinect est composée de trois systèmes optiques : une caméra dans l'infrarouge proche une caméra RGB un laser La caméra RGB permet d'accéder aux images qui seront traitées avec les outils usuels en vision par ordinateur. Le couple caméra IR/Laser sert à l'estimation de distance. Le laser projette dans la scène des patterns qui ne sont visibles que par la caméra IR. L'image IR permet alors, connaissant la distance de la base entre le laser et la caméra, d'estimer la distance d'un point de l'espace par triangulation. La résolution de la caméra IR permet d'obtenir un nuage de point de taille 640x480, qui correspond à la résolution de l'image. Il est alors possible d'attribuer une couleur précise à chaque point en faisant correspondre les points du nuage issu des mesures 3D avec l'image RGB. 2.2 Driver Au début du stage aucun driver officiel n'était disponible pour la kinect. Cependant la communauté robotique avait mis au point un driver permettant d'accéder aux données de la kinect: openni_kinect. C'est ce driver qui a été utilisé. Il permet d'accéder principalement aux données suivantes: 1. Image RGB de la caméra optique 2. Image IR de la caméra IR 3. Image de profondeur (la valeur des pixels code la distance sur 11bits) 4. Nuage de points 3D issu des données IR avec ou sans couleurs La Kinect se présente donc comme une véritable innovation pour la robotique qui préfigure sans doute l'avènement d'une nouvelle classe de capteurs intelligents dotés de capacité de prétraitement de l'information et permettant l'accès à une information beaucoup plus riche que les capteurs actuels. 30

32 2.3 Etalonnage Pour réaliser les tests la kinect a due être étalonnée. Ceci se fait en trois phases. Premièrement il faut calibrer la caméra optique comme une caméra classique. La procédure consiste à observer un damier sous plusieurs angles pour déterminer notamment les défauts des lentilles et de l alignement de la matrice de pixel avec l axe optique. Ensuite le système infrarouge est étalonné. D une part la caméra IR est calibrée comme la caméra RGB dans le but de déterminer les défauts des lentilles et d alignement. Ceci est rendu possible du fait de l utilisation de l IR proche. D autre part le système laser/caméra IR est calibré comme un système de stéréovision dans le but de déterminer la distance entre le laser et la caméra. Enfin le système global est calibré en utilisant le damier pour déterminer la distance entre les deux caméras. L ensemble de ses opérations est simplifié par la mise à disposition dans ROS d un programme prenant en charge les calculs. 2.4 Précision Pour caractériser complètement le capteur, la précision de la mesure de distance fournie par la kinect a été évaluée. La procédure mise en place (Willow Garage) pour mesurer la précision consiste à observer un plan à différentes distance. Le nuage de point correspondant au plan est récupéré et un plan moyen est calculé. La distance de chaque point du nuage à ce plan moyen est alors mesurée et détermine l erreur moyenne faite dans la mesure de distance. Figure 7: évolution de la précision de l'estimation de distance de la Kinect en fonction de l'éloignement de l'objet 31

33 Les résultats montrent que la précision de la Kinect est relativement bonne pour de faibles distances mais qu au-delà de quelques mètres elle devient trop importante pour être exploité correctement. Pour le SLAM on limitera donc les mesures à la distance de 2.5 mètres. 32

34 II. Analyse de l existant Cette partie présente un tour d horizon des recherches qui ont inspiré ou ont servi de base à cette étude. RGBD Mapping Au début du stage il y avait assez peu d études faisant l objet de publications dans le domaine du SLAM 3D à base de caméra RGB-D, la Kinect n ayant fait son apparition il n y a que quelques mois. On peut cependant citer l article de Dieter Fox et ses collègues [1], une des premières équipes à avoir utilisé la Kinect pour du SLAM. L algorithme présenté dans la publication permet de tirer partie à la fois des données visuelles et des données Laser. La première étape consiste à extraire des données visuelles des points d intérêt grâce à l algorithme SURF. Ces points sont ensuite projetés dans l espace grâce aux données de profondeur fournis par la caméra IR. Ils forment un nuage de points (PCL) restreint qui va permettre d estimer le déplacement entre deux images successives. Pour se faire l algorithme RANSAC est utilisé et permet d extraire la transformation la plus vraisemblable en se basant sur le support de point le plus important. Ensuite cette estimation est affinée en utilisant l algorithme ICP qui permet un matching fin des nuages de points (en utilisant l ensemble des points cette fois-ci). La fermeture de boucle (loop-closure) est détectée en comparant chaque nouveau nuage de points d intérêt avec l ensemble des précédents. L algorithme RANSAC est une nouvelle fois utilisé pour déterminer s il existe un nombre de point suffisant pour dire qu il s agit d une fermeture de boucle. Enfin l algorithme Toro est utilisé pour optimiser globalement le graphe créé par l algorithme. L intérêt principal de cette approche dans le cadre qui nous intéresse ici, outre le fait d utiliser le même capteur, est sa robustesse. En effet d après la publication cet algorithme peut être utilisé dans le noir complet. C est un point très intéressant puisque le robot amené à évoluer dans un environnement réel devra être capable de fonctionner en toute circonstance. Cette robustesse vient notamment de la richesse des informations utilisées. En effet il y a une redondance partielle entre informations visuelles et 3D. Ceci permet de palier les défaillances ou bien la lenteur d une partie du traitement des données. On voit par exemple que le temps de convergence d ICP est de 500ms environ lorsque initialisé avec les données visuelles tandis qu il augmente jusqu à 1 seconde sans initialisation. Le volume de données à traiter augmente cependant. A contrario dans certains cas rares, la défaillance d un des systèmes entraîne la défaillance de l ensemble. En effet comme précisé dans l article, il arrive que le laser ne soit pas à même de mesurer certaines distances par exemple lorsque la surface est réfléchissante ou la cible trop lointaine. Tous les points 33

35 d intérêt détectés dans la zone sont alors inutilisables. Ceci amène à des situations où les seuls points détectés et utilisables sont concentrés dans une zone restreinte de l image. L estimation du déplacement est alors très mauvaise et la localisation peut rapidement être perdue. L utilisation de donnés à la fois visuelles et laser offre donc une grande robustesse car la redondance d information permet à l algorithme de fonctionner dans des cas où des algorithmes n utilisant qu un seul système ne seraient pas fonctionnels. Il faut cependant noter que dans certains cas la défaillance du système visuel peut conduire à une perte de localisation du fait de l interdépendance partielle des systèmes. Une autre approche a été présentée par A. Nuechter et une implémentation de ce code est disponible sur OpenSlam.org (Slam6D). Il s agit cette fois de n utiliser que des nuages de points pour réaliser l estimation de pose 3D. Seules les données du laser sont utilisées. Le calcul de la transformation entre nuages de points successifs se fait ici aussi grâce à l algorithme ICP. La différence vient de l implémentation de l algorithme ICP qui présente plusieurs variantes. L une d elles est présentée dans l article [2]. Bien que n utilisant pas de Kinect mais une caméra ToF (Time Of Flight), l article présente un intérêt notamment du fait de la mise au point de cette nouvelle variante de l algorithme ICP. En effet comme on l a vue plus haut le temps de convergence d ICP est le point faible de l algorithme de SLAM 3D. Ce point est donc le plus critique et nécessite d être amélioré. On peut donc à juste titre chercher à optimiser cette partie du traitement qui représente le temps de calcul le plus important. L idée présentée dans l article est de 1) rendre plus robuste l algorithme 2) diminuer le volume de données à traiter en ne conservant que les données susceptibles d être utilisées. Pour cela la méthode proposée consiste à : définir un volume (cône) observable estimé à chaque itération et ne conserver que les points appartenant à la zone recouverte par deux cônes successifs. En effet, seuls les points présents dans les deux nuages de points successifs peuvent faire l objet d un appariement. Cette méthode permet donc de limiter le nombre de points utilisés, et donc d accélérer la convergence de l algorithme. Un second article des mêmes auteurs offre lui aussi une approche intéressante pour le traitement de l algorithme ICP [3]. Plus précisément c est la recherche du plus proche voisin, sous-jacente à l algorithme ICP, qui fait ici l objet d une amélioration. Le principe est de paralléliser massivement le calcul pour le rendre exécutable sur un GPU. Ici l implémentation est faite sur une carte NVIDIA à architecture CUDA. Cette architecture permet la programmation du GPU à l aide d un langage de programmation proche du C et par ailleurs open source, ce qui facilite grandement les développements. La recherche du plus proche voisin de chaque point se fait traditionnellement en utilisant un kd-tree. Il s agit d une représentation particulière de l information contenue dans un nuage de points. Elle prend la forme d un arbre dont chaque embranchement divise l espace en k parties. Ceci permet d accélérer l accès à l information par rapport à un tableau classique. La forme la plus courante de kd-tree 34

36 est l octree dans lequel chaque nœud a huit «fils» (partition de l espace en huit parties). La première étape est de sérialiser le kd-tree. Ceci consiste à numéroter les branches pour permettre de placer l arbre dans un tableau afin de tirer partie aux mieux des spécificités de l architecture des GPU. En effet du fait de l organisation de la mémoire il n est pas possible de stocker le kd-tree directement en mémoire. Ensuite est mis en œuvre un algorithme de recherche prioritaire qui permet d extraire les éléments ayant une distance minimale par rapport au point recherché. Cet algorithme remplace les algorithmes classiques sur les kd-tree qui ne sont pas applicables lorsque utilisés sur GPU. La troisième étape consiste finalement à déterminer le plus proche voisin parmi les points trouvés. La méthode utilisée ici pour optimiser l espace consiste à fixer une longueur maximale de la file dans laquelle la recherche est faite. Des tests montrent de très bonnes performances avec cette méthode et une convergence plus rapide. Plus généralement les résultats présentés montrent une très grande efficacité de l algorithme GPU-ICP comparativement aux algorithmes utilisant OpenMP 7 ou une implémentation classique sur CPU multicoeur. Ainsi en termes d implémentation l emploi d un GPU offre un gain de temps très important. De la même manière d autres points peuvent être améliorés pour accélérer l exécution de l algorithme. Notamment l implémentation de la détection des points d intérêt SURF peut faire l objet d améliorations. C est ce qui a été fait par une équipe de l université de Waterloo. Bien que leur travail n ait pas encore fait l objet d une publication, le code source disponible permet d avoir accès à l algorithme utilisé. Ici le principe est de paralléliser l algorithme SURF avec un GPU en utilisant GPUSURF [9]. Là encore l avantage est de pouvoir accélérer la phase assez longue de détection/extraction des points d intérêt. Comme présenté dans l article [9] GPUSURF permet de traiter des images de 640x480 à un taux de 100 images par seconde sur un pc doté d une carte graphique GF8800GTX. Ceci est tout à fait compatible avec les attentes du SLAM 3D et l utilisation de GPUSURF peut permettre d accélérer considérablement l algorithme. Cependant l utilisation d un GPU additionnel implique d une part une plus grande consommation énergétique ce qui n est pas souhaitable pour un robot mobile (diminution de l autonomie) et d autre part un coût plus élevé du fait de l achat de matériel supplémentaire. A titre indicatif les cartes NVIDIA sont annoncées pour consommer de 35 à 100W ce qui équivaut ou dépasse la consommation de la carte mère du robot! Le choix d ajouter ou non un GPU à une application doit donc être faite en connaissance de cause et selon les objectifs du projet. 7 OpenMP est une interface de programmation pour le calcul parallèle sur CPU offrant une bibliothèque puissante pour le développement d applications en C. 35

37 Ainsi de ces articles il ressort que : - le calcul de l algorithme ICP est la partie la plus longue dans l approche citée ci-dessous et consomme énormément de ressources. - Ce calcul peut être amélioré en limitant le nombre de point des nuages à matcher à l aide d une estimation de la pose de la caméra. - L implémentation sur GPU offre un avantage conséquent en termes de temps de calcul aussi bien pour l algorithme ICP que pour l algorithme SURF. Points d intérêt et descripteurs NARFs Ainsi à travers ces différents articles a été présentée une première approche offrant différentes variantes et possibilités d amélioration. Elle offre donc des performances satisfaisantes pour entreprendre une étude plus poussée. On peut cependant envisager d autres techniques basées sur l exploitation de l information contenue dans les nuages de points. En effet de la même manière que des points d intérêts peuvent être extrait de façon robuste d une image, on peut chercher à définir des points d intérêt dans un nuage de points caractérisant de façon robuste la géométrie d un lieu. L idée serait de remplacer les informations issues de SURF par des informations issues uniquement du nuage de point et ce afin d augmenter la robustesse par rapport aux variations de luminosité notamment. De nombreuses études dans ce sens ont étés faites, notamment dans le domaine de la reconnaissance d objets et des descripteurs variés ont étés mis au point pour identifier de façon robuste des objets en tenant parfois compte de la possibilité d occlusion. Dans [4] est présenté un nouveau type de descripteurs nommés NARF qui permettent de caractériser de façon robuste des zones du nuage de point. Dans cette méthode la détection de point se fait de la manière suivante : - détermination de comment la surface change autour de chaque point et calcule d une direction privilégiée de changement - Calcule d une valeur d intérêt du point prenant en compte la manière dont la direction privilégiée change dans le voisinage et comment la surface change au niveau du point. - lissage des valeurs - suppression des non maximums L un des paramètres important de l algorithme est la taille du voisinage de chaque point dans lequel on cherche à déterminer les changements. D après les tests les auteurs préconise une taille de 25% de l objet observé environ. Pour assurer l appariement efficace des points de deux nuages consécutifs il faut ensuite extraire les propriétés de chacun des points d intérêt. Les exigences concernant le descripteur est qu il permette de: -définir l espace occupé et l espace libre en 3D de façon à décrire la forme de l objet 36

38 - qu il soit robuste au bruit et invariable selon la position du point d intérêt -qu il permette d extraire un référentiel local unique en chaque point L algorithme pour le calcul des descripteurs NARF d un point est le suivant: - calcul de l image de profondeur 8 locale en ce point - créer en ce point un schéma en forme d étoile dont les valeurs des pixels le long des bras seront les valeurs du descripteur - extraire l orientation du descripteur -initialiser le descripteur par rapport à sa direction privilégiée pour le rendre invariant par rotation L une des exigences majeures pour ce type d algorithme est sa stabilité. En effet pour pouvoir faire correspondre dans deux nuages de points successifs les points d intérêt il faut que ceux-ci soient à la même position. D après l auteur, en observant un même objet avec deux orientations différentes de 20 on retrouve environ 70% de points d intérêt commun d un nuage à l autre. Pour deux observations à 70 on trouve 55% de points en commun. D après l article les tests de mise en correspondance de deux nuages de points successifs grâce aux descripteurs NARFs montrent de bonnes performances pour de faibles angles de rotation. L intérêt de ce type d approche a déjà été souligné: elle permet de se passer des données visuelles et de n utiliser que des données du nuage de points : en reprenant l algorithme présenté au début de cette étude on peut remplacer le calcul et l extraction des points d intérêt SURF dans l image par l extraction de point d intérêt dans le nuage de point directement. A partir de l estimation de déplacement faite du matching des deux nuages de points successifs avec la méthode RANSAC on affine le calcul avec l algorithme ICP agissant sur l ensemble des points. L utilisation de point d intérêt et de descripteur NARFs est donc une piste intéressante bien que nécessitant des tests complémentaires pour s assurer de la robustesse du procédé. Cette approche fera donc l objet d une évaluation puis de développement éventuellement. 8 «Range image» : image dont les pixels codent non pas l intensité lumineuse mais la distance 37

39 Segmentation et reconstruction 3D Enfin un autre type d approche est envisageable pour le SLAM 3D. Au lieu de chercher à faire correspondre des points contenus dans des nuages successifs, on peut chercher à extraire des nuages les primitives géométriques tels que les plans formant le sol, les murs et le plafond pour ensuite établir une correspondance entre ces mêmes plans. Ceci impose de pouvoir extraire rapidement des informations à partir de nuages de points pour ensuite chercher les correspondances. Il existe plusieurs études portant sur la recherche de plans dans des nuages de points. Parmi les travaux les plus récents on trouve notamment [6] qui utilise la transformée de Hough 3D pour réaliser la détection. En plus de faire un récapitulatif des différentes méthodes l article présente une nouvelle variante de la transformée de Hough 3D pour la détection de plans. Elle consiste en un nouvel accumulateur sphérique qui se différencie principalement par la manière dont l espace de Hough est discrétisé. Il offre l avantage d être composé de cellule de même taille qui offre donc une meilleure homogénéité dans la détection des plans. D après les résultats présentés la méthode semble satisfaisante pour notre application. Cependant la carte d un bâtiment sera constituée d une multitude de plans de tailles relativement petite par rapport à l ensemble du nuage. La détection basée sur des accumulateurs risque donc de ne pas être efficace car le remplissage des cellules tendra à s uniformiser. On pourrait être tenté de dire que des directions privilégiées pourraient tout de même être extraites, tous les murs étant verticaux et les sols horizontaux. Cependant si de nombreux objets sont présents dans la pièce la détection des plans peut être brouillée. Face à cette difficulté une approche locale peut peut-être offrir de meilleurs résultats. D autres méthodes plus classiques utilisant notamment les variantes de l algorithme RANSAC sont présentées dans [10]. Les performances en détection sont très bonnes puisque avec l algorithme présenté il est possible de détecter des formes d une taille relative de 1/3000! Autrement dit il est possible de détecter des formes dont le nombre de point les codant n est que de 1/3000 du nombre total de points du nuage. L algorithme présenté apporte une nouveauté dans la manière dont sont détectée les formes: ici à chaque point est ajouté une série de paramètres constituée des coordonnées du vecteur normal estimé en ce point. Ainsi il suffit de deux points au maximum pour détecter la plupart des formes géométriques de base. La méthode consiste donc dans un premier temps à estimer la forme locale du nuage de point en se reportant à des formes de base : cône, plan et. Chaque «individu» se voit alors noté en fonction de ses performances estimée par l algorithme RANSAC. Les primitives géométriques sont finalement extraites de manière statistique en tenant compte du score de chaque candidat. On peut aussi adopter une approche un peu différente qui consiste à créer un mesh à partir du nuage de point et rechercher les plans à partir de l observation des normales de chacune des facettes. Les plans sont 38

40 formés des facettes dont les normales ne varient que peu autour d une direction moyenne dont le seuil est à définir. On peut ainsi reconstruire les plans dans une scène à partir d un nuage de points diffus. Cette méthode est proche des techniques utilisées pour le lissage des surfaces dans le domaine de la modélisation 3D. Le problème principal de cette approche concerne la phase de triangulation. Malheureusement les techniques actuelles sont trop lentes pour envisager leur utilisation temps réelle nécessaire ici [11]. 39

41 III. Solutions retenues De l'ensemble des études qui ont été passées en revue ci-dessus ont émergées deux pistes principales pour la réalisation du SLAM. La première consiste à réutiliser l algorithme RGBD Mapping. En effet c'est l'étude qui se rapproche le plus des objectifs de notre projet et les résultats présentés démontrent que l approche est viable. Par ailleurs cet algorithme a déjà été implémenté dans au moins deux packages de ROS et la mise au point du code peut s'en trouver accélérée. Une implémentation du code a été réalisée par une équipe de l université de Freiburg et est disponible dans ROS. Le package nommé RGBDSLAM est présenté comme permettant de scanner une pièce entière ou des objets. Les premiers tests du package ont été fait sur une carte mère dotée d un processeur core2 duo. Le programme s est avéré être très lent en plus de saturer rapidement la mémoire. Par ailleurs la localisation est perdue rapidement sans possibilité de retour en arrière. Des modifications ont étés apportées au programme pour le faire travailler avec un jeu de données réduit, l objectif étant d accélérer les calculs pour éviter le décrochage lors du mouvement. Cependant, le problème évoqué plus haut, de perte de précision due à la concentration des points d intérêts exploitables dans une zone restreinte de l image, devient prépondérant avec un jeu réduit de données et limite de fait la précision de l estimation de mouvement, et donc rend le code inutilisable. Ce code visiblement mal écrit ne sera donc pas réutilisée 9. Une autre implémentation d un algorithme similaire est disponible dans ROS cependant ce code a été écrit en partie pour tourner sur GPU (utilisation notamment de GPUSURF). Le choix ayant été fait de ne pas utiliser de GPU, du moins dans un premier temps, la solution retenue consiste à réutiliser la partie du code qui est exploitable et à réécrire le reste. La seconde piste retenue est l'approche utilisant les NARFs. En effet on peut assez facilement imaginer une stratégie comparable à celle employée dans RGBDSLAM mais remplaçant la partie recherche de points d'intérêt dans l'image par la recherche de points d'intérêt dans le nuage. On peut raisonnablement imaginer trouver des résultats comparables avec cependant l'avantage d'être complètement indépendant de l'information RGB. Ceci permettrait notamment d'envisager du SLAM 3D avec des Lasers et d'augmenter la robustesse du SLAM aux variations de luminosité. Cependant contrairement à la première, il reste à montrer que la seconde solution est viable. Elle fera donc l'objet de tests particuliers, notamment pour évaluer la robustesse des détecteur/descripteurs NARFs. 9 A l heure où s achève l écriture de ce rapport une nouvelle implémentation du code a été réalisée et publiée. Elle se base sur l utilisation d un GPU à architecture CUDA et n a pas fait l objet d une évaluation 40

42 IV. RGBD Mapping Cette partie présente la solution basée sur RGBD Mapping. Après un rappel de l algorithme les étapes importantes sont présentées en détails ainsi que l implémentation. 1. L algorithme L idée fondamentale ici est d utiliser le maximum d informations disponibles sur la scène. L avantage de la Kinect est de fournir à la fois de données sur les couleurs/textures et sur les positions. Il faut donc exploiter toutes ces données pour aboutir à un algorithme performant. Pour cela on utilise les algorithmes les mieux adaptés pour traiter chaque type d informations (nuage de points ou image). A chaque itération la Kinect fournit une image et un nuage de points de la scène observée. Il existe plusieurs algorithmes performants permettant de mettre en relation des images à partir de points particuliers de celle-ci. Cependant une image seule ne permet pas de reconstruire la position 3D des points. On utilise donc l information des nuages des points pour projeter les points d intérêt dans la scène 3D. On dispose donc d un nuage 3D formé des points d intérêt. A partir de ce nuage on peut appliquer les méthodes classiquement utilisées pour recoller deux nuages de points. Ainsi en mêlant traitement d image et traitement de nuage de points on peut estimer efficacement les déplacements de la caméra entre deux instants. Les poses successives forment la trajectoire de la caméra (du robot). Une première boucle de supervision de la trajectoire calcule à chaque itération les corrections à apporter au graphe, c est l algorithme de SLAM TORO. Une autre boucle détecte les fermetures de boucles. Figure 8: Principe de l algorithme RGBD Mapping. Ici les mêmes étapes sont suivies mis à part la dernière consistant à reconstruire la scène sous la forme d un mesh 41

43 2. Estimation de mouvement L estimation de mouvement est la première phase pour la réalisation du SLAM. Elle présente de nombreux challenges notamment du fait des calculs importants qu elle nécessite. La première phase consiste à détecter les points d intérêt et calculer leurs descripteurs. Ceci se fait aisément avec OpenCV. Une fois les points d intérêt détectés, ceux-ci sont reprojetés dans la scène en utilisant les données de distance. On dispose alors de deux nuages pouvant aller de quelques dizaines à quelques centaines de points. La recherche de la transformation qui permet de recoller convenablement ces deux nuages de points se fait avec la combinaison RANSAC + ICP. Ainsi on obtient une première estimation du mouvement avec une méthode rapide puis on l affine par la suite avec une méthode précise initialisée avec ce résultat. On bénéficie donc à la fois de la vitesse de calcul relativement bonne et d'une précision correcte du fait du second algorithme. 2.1 RANSAC La première estimation de mouvement est faite avec RANSAC. On note A et B les deux nuages considérés, correspondant aux acquisitions aux temps t et t+1. La première étape de l'algorithme consiste à choisir trois points de manière aléatoire dans le nuage A et à trouver les points correspondant dans le nuage B. Pour chaque triplet de points on calcul alors une base correspondante: on note Xi les points du nuage X et Vxi les vecteurs de la base associée au nuage X. On définit: 1= 2 1 2= ( 1.( 3 1)) 3= 1 2 Où x est le produit vectoriel. On fait de même pour le nuage b. Une fois les deux bases définies on calcule la matrice de rotation et de translation entre les nuages a et b. Les points de l'ensemble du nuage a sont alors reprojetés suivant la transformation calculée de manière à recoller les deux nuages. 42

44 On compte ensuite le nombre de pair de points appartenant aux nuages a et b qui sont espacés d'une distance inférieure à un certain seuil. Ce nombre de point donne un score à une transformation. La transformation ayant le meilleur score correspond à la transformation la plus probable. L'algorithme permet finalement de ne conserver que les points correspondant à la meilleure transformation. 2.2 ICP Une fois le mouvement estimé, la transformation est affinée avec ICP Kd-tree Les principes d'icp sont assez simples mais l'implémentation pour une application temps réel peut être délicate du fait notamment que le temps de convergence ne peut pas être maitrise. C'est notamment l'étape de recherche du plus proche voisin qui est délicate. Le plus souvent on utilise pour cette tache une organisation des données sous une forme particulière nommée kd-tree. Le code s appuie notamment sur l implémentation faite dans la bibliothèque PCL qui fournit des fonctions de recherche dans du plus proche voisin très efficace basée sur ANN Implémentation d ICP Phase1 : plus proche voisin La première phase de l algorithme est la plus importante. Elle consiste à choisir pour chaque point du nuage A le point du nuage B de telle sorte que la distance entre ces deux points soit minimale (distance euclidienne). C est la phase d appariement. On utilise ici la bibliothèque PCL, et plus spécifiquement le package ANN (Approximate Nearest Neighbor). Phase2 : calcul des centroïdes La seconde phase consiste à calculer les centroïdes des deux nuages de point. Ceci se fait simplement en calculant pour chaque nuage la moyenne des coordonnées des points. On déplace alors les nuages à l origine pour la suite des calcules. 10 Approximate Nearest Neighbor : méthode de recherche approchée du plus proche voisin très efficace et fiable. Elle est intégrée à la bibliothèque PCL. 43

45 Phase3 : SVD A ce stade on dispose de deux nuages de points appareillés à partir desquels on veut calculer la transformation les liants (rotation). Pour ce faire on utilise la décomposition en valeur singulière ou SVD. Le principe d une SVD est basé sur l idée qu une matrice M, de dimension mxn, peut être décomposée en une matrice U, de dimension mxm, une matrice diagonale,de dimension mxn, et composée de réels positifs, et une matrice notée V T de dimension nxn. La matrice M est la somme des multiplications des coordonnées des points du nuage A et du point qui leur est appareillée dans le nuage B. Autrement dit on a : = ( ) ( ) Ou encore... = Phase4 : calcul de la transformation La quatrième phase consiste à calculer R et T, la rotation et la translation. Une fois la matrice M calculée ainsi que la SVD, on calcule R ainsi: = Il faut ensuite vérifier que ( )=1 pour s assurer qu il s agit bien d une rotation car l algorithme peut échouer (rarement). La translation est calculée par la formule : = Où Ac et Bc sont les positions des centroïdes respectivement des nuages A et B et où R est la matrice rotation précédemment calculée. Phase5 : distance moyenne des deux nuages La dernière phase consiste à calculer la distance des deux nuages. Pour ce faire le nuage cible est «recollé» sur le nuage de base avec la transformation précédemment calculée et les distances euclidiennes des points appareillés sont calculées. 44

46 Le programme exécute ces cinq phases de manière itérative. Finalement l algorithme s arrête lorsque la distance des nuages est inférieure au seuil spécifié. 3. Optimisation globale du graphe et fermeture de boucle La dernière étape consiste en la détection de la fermeture de boucle. Il s agit de détecter si la zone courante est une zone déjà explorée ou non. Si c est le cas en plus du nouveau nœud est créée une contrainte supplémentaire reliant ce nœud à celui correspondant à l observation précédente de la zone. Cette nouvelle contrainte permet de recalculer la configuration du graphe et de réduire considérablement l erreur accumulée depuis le départ. Pratiquement on estime la ressemblance entre les points d intérêt détectés dans l image courante et dans les images précédentes. Un score permet d évaluer cette ressemblance. Si ce score est supérieur à un certain seuil alors la fermeture de boucle est détectée et la nouvelle contrainte est ajoutée au graphe, si le score est inférieur il n y a pas fermeture de boucle. Figure 9: exemple de carte obtenue avant et après la fermeture de boucle et l optimisation du graphe. La qualité de la reconstruction est grandement améliorée par ces deux opérations Finalement Toro réalise l optimisation du graphe conformément à ce qui a été expliqué plus haut. Il faut noter ici qu une modification a été apportée par rapport à l algorithme initial. En effet ICP utilise comme entrée le nuage de points d intérêt et pas le nuage entier. Ceci accélère considérablement le traitement et ne joue que très peu sur la précision. La première implémentation du package utilisait les points formant le contour des objets de la scène. Cette technique en plus d utiliser un ensemble de point plus conséquent nécessitait aussi de devoir calculer les contours à chaque itération. C est pourquoi elle n a pas été réutilisée. 45

47 4. Implémentation ROS permet, comme expliqué plus haut, de faciliter les communications interprocessus. Une architecture classique d'un package de ROS sera donc constitué de petites unités fonctionnant en parallèle et communiquant selon les procédés décris plus haut. Ici seul le mécanisme de souscription/publication est utilisé. Organisation du package de SLAM Le code est divisé en six packages tous organisés autour de la classe Keyframe. Cette classe contient les données de la kinect (image RGB, nuage de point) ainsi que les résultats des différents traitements (point d'intérêts, pose de la caméra à cet instant etc). Chaque instance de cette classe modélise en fait un nœud du graphe (voir paragraphe sur Toro). Ainsi à mesure que le robot se déplace de nouvelles instances sont créées, contenant les informations nécessaires à la mise à jour du graphe. OptFlow Le premier package est Optflow qui comme son nom l'indique calcule le flux optique en utilisant une pyramide laplacienne. L'objectif d'opflow est de définir quand est-ce qu'il faut créer un nouveau nœud dans le graphe. En effet il est inutile et impossible de calculer à chaque itération la position du robot. Premièrement parce que le faire imposerait de créer une vingtaine de nœud par seconde, le graphe deviendrait vite énorme! Deuxièmement parce que l'ensemble des traitements prend trop de temps pour être fait à chaque nouvelle acquisition. Le package calcule une estimation de la distance parcourue depuis la création de la dernière instance de la classe Keyframe. Si cette distance est supérieure à un seuil alors une nouvelle instance est créée et les informations sont publiées sur le topique concerné. Ceci évite de devoir constamment recalculer les transformations et de surcharger la mémoire et le CPU. Surf Ce package était initialement écrit pour tourner sur GPU et a été recodé pour tourner sur CPU. Son objectif est de trouver les points d'intérêt SURF et de calculer 46

48 leurs descripteurs. Une fois cette tâche effectuée il met à jour les données de l'instance de Keyframe concernée et publie cette mise à jour sur le topic dédié. Vodom Vodom, pour Visual Odométrie, estime le déplacement à partir avec RANSAC et ICP. Ce package a été modifié partiellement notamment pour le calcul de la transformation avec ICP qui a été réécrite en utilisant les outils de la bibliothèque PCL. Le package prend en entrée les points SURF de deux Keyframes successives et calcul la transformation entre les deux nuages de points qu'ils représentent avec RANSAC puis ICP comme décrit précédemment. Loop Ce package gère les fermetures de boucle. Il détecte si, pour chaque nouvelle Keyframe, les points d'intérêt observés correspondent à des points déjà observés. Pour ce faire il compare l'ensemble des points observés aux points courants et détermine par un système de vote si la nouvelle instance a déjà été observée ou pas. Si c'est le cas une contrainte est ajoutée entre ce nœud et le nœud correspondant à l'observation précédente. La mise à jour est publiée sur le topic approprié. Toro Ce package réalise l'optimisation globale du graphe en construisant l arbre du graphe et en mettant à jour les contraintes conformément à l algorithme Toro décrit présédemment. Render Ce package gère le rendu de la scène. Il a été réécrit et des fonctionnalités supplémentaires ont été ajoutées pour permettre notamment l'enregistrement de la carte et la navigation dans la carte. Il permet le rendu en temps réel de la carte sous forme de nuages de points. 47

49 5. Schéma de fonctionnement du SLAM Le schéma ci-dessous représente le fonctionnement général du SLAM. Les flèches indiquent dans quel sens sont transmises les informations. On remarque notamment que Render dépend directement de vodom et de TORO. En fait Render représente les données issues de l odométrie qui sont périodiquement mises à jour par TORO. Ce graphe est obtenue avec la commande «rxgraph» de ROS. Les différents nœuds sont signalés par des ovales et les topics sont signalés par les flèches entres les process. 48

50 V. Descripteur NARFs Une seconde approche pour le SLAM a été étudiée. Il s agit d utiliser les détecteurs/descripteurs NARF pour le calcul des points d intérêt dans le nuage de point. La première phase du développement de cette approche a consistée à évaluer les performances en détection pour déterminer si la robustesse des NARF était suffisante pour être utilisée pour du SLAM. En effet il est nécessaire que des points soient détectés de manière robuste quelque soit l angle sous lequel est vu la géométrie qu ils décrivent. 1. Tests du détecteur Pour évaluer la robustesse de la détection le principe du test a consisté à capturer avec la Kinect deux nuages de point de la même scène sans déplacer la caméra et à calculer l image de profondeur correspondante 11. Du fait des erreurs de mesure de distance et du bruit de mesure les nuages de points obtenues ne sont pas exactement les mêmes à deux instants différents. L algorithme de détection doit cependant permettre de retrouver les mêmes points d intérêt puisqu elle ne dépend en principe que de la géométrie sous jacente et pas de la position des points qui la modélisent. Les tests ont cependant montré un manque de robustesse du détecteur. Les points détectés ne sont pas les mêmes pour chacune des images. De plus le nombre de points varie d une image à l autre. Plusieurs raisons peuvent être invoquées pour expliquer le problème. Premièrement il peut s agir d un mauvais paramétrage. La caractérisation de la géométrie se fait en prenant en compte un certain voisinage. Si la taille de ce voisinage n est pas suffisante elle peut être perturbée par la variation de position des points d une image à l autre. Si au contraire le voisinage est trop grand la description de la géométrie sera moins pertinente, voir instable. Plusieurs valeurs de paramètres ont été testées conformément aux recommandations des auteurs de l étude, mais les résultats sont restés les mêmes ce qui laisse penser que le problème a une autre origine. 11 Une image de profondeur est une image dont les pixels codent la distance du point qu ils représentent 49

51 Deux autres explications peuvent être avancées : - une erreur dans le code de la bibliothèque PCL concernant les fonctions de détection de points NARF - un manque de robustesse du détecteur NARF Chacune d elles nécessiterait une investigation en profondeur du code et des algorithmes de détection des NARFs. Bien que l intérêt de la mise au point d un algorithme de SLAM basé sur les NARF soit évident cette solution n a pas été retenue pour plusieurs raisons. Premièrement on dispose déjà d une solution viable pour le SLAM 3D basée sur l algorithme RGBD Mapping et le développement d une seconde approche représente un intérêt moindre. Deuxièmement les descripteurs NARFs sont très récents et leur robustesse n a pour l heure pas fait l objet de suffisamment d études. Il n existe donc aucune garantie de résultats. Par ailleurs à l heure où ces tests ont étés fait les NARFs étaient encore en cours de développement, de même que la bibliothèque codant les fonctions de détection/descriptions etc. Leur utilisation peut donc s avérer hasardeuse. Enfin un autre domaine pour la poursuite des recherches s est avérer représenter un plus grand intérêt, il s agit du domaine de la reconstruction 3D qui fera l objet du prochain chapitre. 2. Conclusion générale sur les NARFs Les NARFs présentent un intérêt majeur pour l extraction de données depuis des nuages de points. Tout comme SIFT ou SURF sont aujourd hui indispensables en traitement d images, la recherche de points d intérêts dans un nuage de points et leur description robuste sera sans aucun doute essentielle à l avenir. En effet avec le développement rapide de la 3D, beaucoup d applications utilisant des Lasers ou des capteurs comme la Kinect, donc des nuages de points, verront le jour et nécessiteront pour la reconnaissance d objets, le SLAM ou d autres applications, de pouvoir caractériser de façon robuste une géométrie décrite par des points. La recherche dans cette voie est donc indispensable et bien qu il semble que pour l heure et avec ces outils, il soit difficilement envisageable de faire du SLAM, cette approche présente sans aucun doute un grand intérêt pour le futur. 50

52 VI. Segmentation et Reconstruction 3D Ce chapitre présente le travail de stage sur la segmentation et la reconstruction 3D. Elle concerne l extraction d information depuis la carte créée par le SLAM. Après une présentation des objectifs et une justification de la démarche sont présentés en détails les développements effectués et les résultats obtenus. 1. Introduction Le SLAM permet d'obtenir une carte 3D de l'environnement sous la forme d'un nuage de points. Le problème de cette représentation est que le volume de données nécessaires à la représentation est très largement supérieur au volume de données réellement pertinent. En effet si l'on considère un plan, il suffit de quatre coefficients pour le décrire entièrement, auxquels s'ajoutent bien sur les coordonnées des points qui le délimitent. Sa représentation sous forme de nuage de points nécessite par contre un très grand nombre de points pour couvrir la surface. On a donc tout intérêt à extraire du nuage de point fourni par le SLAM les informations réellement pertinentes telles que les équations des plans. Ceci permet d'une part d'envisager le transfère des données du robot vers une station déportée, ce qui est inenvisageable autrement vu le volume de données. D'autre part il est envisageable de doter le robot de fonctions intelligentes, notamment dans le cadre de la planification de trajectoire si le robot est capable de détecter le sol ou les zones navigables par exemple. La suite de l'étude sera donc dédiée à l'extraction de données pertinentes de la scène à partir de la carte dans le but de: -reconstruire l'environnement pour faciliter la visualisation par l'utilisateur humain -fournir au robot des informations exploitables pour la planification de trajectoire, la détection d'obstacle, la reconnaissance d'objets etc. On cherchera notamment à extraire d'un nuage de point brut les plans formant la scène. 51

53 2. Détection de plans dans un nuage de points Dans un environnement urbain, une hypothèse raisonnable consiste à dire qu il existe une majorité d objets pouvant être décrit par un ou des plans, ce qui est effectivement le cas de la plupart des objets communs: murs, sol, armoires, livre, écran d ordinateur etc. La première tâche va donc consister à rechercher comment extraire efficacement des plans d un nuage de points. RANSAC La première approche étudiée pour la détection de plans utilise l'algorithme RANSAC. Comme précisé plus haut RANSAC permet d'extraire un modèle à partir d'un ensemble de données bruitées. Figure 10: détection d'un plan avec l'algorithme RANSAC. Trois points sont tirés au hasard. Chez point est alors évalué pour savoir s'il appartient ou non au plan. Ici l'algorithme consiste à tirer trois points au hasard dans le nuage pour former un plan. Chaque point du nuage est alors évalué pour savoir s'il appartient ou non à ce plan. Ceci est répété plusieurs fois et le plan ayant le support le plus grand est retenu. Cette approche est très efficace pour la détection de plans importants. Elle permet par exemple la détection du sol très rapidement dans une scène complexe. Cependant elle s'avère inefficace pour la détection de surfaces plus petites qui ont un support de dimension faible au regard de la taille du nuage complet. En effet ces surfaces même si elles sont correctement détectées auront un poids plus faible qu'une surface de grande envergure, même moins pertinente, et ne seront donc pas retenues. 52

54 Figure 11: illustration d'un problème rencontré avec RANSAC: l'algorithme détecte un plan alors qu'il s agit de deux plans distincts Par ailleurs cette méthode ne peut discerner un plan unique de deux plans parallèles séparés, comme la surface de deux tables par exemple (schéma ci-dessus). Ceci peut poser des problèmes pour la suite et nécessiterait au moins une étape supplémentaire de calcul pour séparer dans chaque plan les différents ensembles détectés. Une autre méthode a donc été développée pour la détection des surfaces plus petites. Figure 12: Carte de l'établissement réalisée avec le SLAM. En bleu le sol détecté par RANSAC, en rouge le reste du nuage Détection de plans par croissance Le problème de l'approche précédente est l'insuffisance du critère utilisé pour évaluer la pertinence d'une détection: le seul paramètre pris en compte est la taille du plan. Or un plan n'est pas forcement une réunion d'un très grand nombre de points, c'est avant tous la réunion de points proches les uns des autres. L'hypothèse 53

55 du très grand nombre de point est donc à remplacer par celle de la proximité de ceux-ci. Un nouvel algorithme a donc été développé qui utilise ce nouveau critère. Il opère en quatre étapes: 1. un point du nuage est tiré au hasard 2. ses 2 plus proches voisins sont sélectionnés pour former un plan local 3. les points du voisinage sont évalués pour savoir s'ils appartiennent ou non à ce plan 4. s'il existe au moins un point dans ce cas, il est intégré au plan et la normale est réévaluée. Sinon l'algorithme est stoppé: le plan entier a été détecté. Une limite minimale de taille a été ajoutée pour éviter les fausses détections. Figure 13: Illustration de la création d un plan avec le nouvel algorithme. à gauche : en vert le point initial et ces deux plus proches voisins en bleu qui forment un plan local. A droite : le plan local formé des trois points et le point rouge dont l appartenance au plan va faire l objet d une évaluation. D'après les tests qui ont été fait cette approche permet de détecter des plans de très petite taille relativement à l'ensemble du nuage. Elle permet donc de corriger les défauts de l'approche précédente. Cependant cette approche est plus lente que la précédente du fait de la nécessité de recourir à un kd-tree pour trouver les plus proches voisins. Si l'objectif n'est que de détecter le ou les plans dominants, l'algorithme utilisant RANSAC sera privilégié puisqu'il permet de détecter préférentiellement les grands ensembles. Si l'on souhaite reconstruire l'ensemble de la scène alors il faudra privilégier le second. Application: détection d'un escalier Une application particulière de cet algorithme a été envisagée. Il s'agit de détecter un escalier dans un nuage de point bruité. Le principe est de détecter le plus de plans possible dans la scène puis de rechercher parmi les plans identifiées ceux qui sont parallèles et espacés d'une distance constante (escalier droit). 54

56 Un modèle d'escalier a été construit avec le SLAM puis le nuage créé a été étudié avec l'algorithme de détection par croissance. Les tests montrent que les plans des marches sont bien détectés. Cependant l'implémentation de la recherche de plans parallèles n'a pas été faite. Figure 14: En haut: modèle de l'escalier scanné avec l'algorithme de SLAM. En bas: en violet foncé, plans identifiés comme appartenant à l'escalier. Note: les marches du haut de l'escalier ne sont pas détectées parce qu'elles n'ont pas été observées avec le SLAM. 3. Détection des contours d un plan Pour représenter correctement un plan il convient de détecter ses contours. Cette tâche est non triviale dès lors que le contour d'un nuage de point ne peut être définit sur la base de critères simples. Deux approches ont étés développés pour la détection des contours. La première est basée sur l'idée qu'un point sur le bord du nuage aura forcement moins de voisins que les points situés au centre. Elle consiste donc à dénombrer pour chaque point le nombre de ses voisins. Les points ayant le moins de voisins sont alors retenus. 55

57 Les résultats obtenus avec cette approche se sont montré décevants. Plusieurs raisons ont étés identifiées. Premièrement pour que la différence entre un point du bord et de l'intérieur soit nette il faut rechercher les points voisins sur une grande surface. Or plus la zone de recherche est grande plus le temps de calcul croit. nuage Figure 15: Figure : un point sur l extérieur a peu de voisins comparé à un point de l intérieur du Par ailleurs il faut définir le rayon de la zone dans laquelle dénombrer les points, or celui-ci dépend largement des situations considérées: nombre de points dans le nuage, taille du nuage, distance moyenne entre les points etc. Enfin avec cette technique les points sont détectés de manière aléatoire, ce qui n'est pas souhaitable comme on le verra plus tard. Une seconde approche a donc été mise au point. Elle consiste simplement à projeter le nuage de point dans un espace 2D puis à appliquer un voxel grid 2D dans cet espace. On parcourt alors les cases à la recherche des points. Le premier point rencontré dans chaque colonne correspond à un point du contour. On trouve ainsi l'ensemble des points de proche en proche et on obtient un contour orienté. 56

58 Figure 16: Illustration du principe de détection d'un contour. Le nuage est projeté dans un espace 2D et une grille lui est appliquée. Le premier point de chaque case est retenu comme point du contour. Cette méthode permet de bien mieux modéliser la forme du plan qu avec les méthodes précédentes. Cependant le temps de calcul est plus long notamment du fait qu il faut rechercher dans chaque case s il y a ou non un point du nuage. 4. Appariement de plans Pour reconstruire un environnement 3D en temps réel, il faut pouvoir mettre en relations les plans détectés à chaque nouvelle itération avec les plans précédemment détectés pour appareiller les différentes instances d'un même plan (éviter les doublons). La difficulté majeure ici est la définition d'un critère efficace qui puisse rendre compte de la Proximité de deux plans qui peuvent avoir une forme quelconque et une position différente. En effet un mur peut être observé en plusieurs fois: à chaque itération apparaît une partie seulement de ce mur qui occupera une place différente de la partie détectée à l'itération précédente. Comment déterminer alors s'il s'agit du même mur? L'idée exploitée ici est d'utiliser plusieurs critères pour valider l'appartenance d'un plan détecté à un ensemble déjà existant. Ainsi un plan sera associé à un autre déjà existant si: - les vecteurs normaux des plans sont colinéaires 57

59 - la distance relative des deux plans suivant la normale est inférieure à un seuil donné - il y a superposition des plans Les deux premiers critères sont triviaux et se vérifie directement. Le troisième critère est plus délicat à déterminer. Deux manières d'implémenter ce problème ont été envisagées. La première assez simple consiste approximer le plan par une forme géométriquement simple pour estimer s'il y a ou non superposition. Deux versions ont été testées. L'une consiste à calculer le rayon moyen de chaque plan en calculant la distance de tous les points au centroid. La superposition est détectée si la distance des centroids des plans est inférieure à la somme des rayons. L'autre alternative consiste à calculer les rectangles englobant des plans. La faiblesse de cette approche est qu'elle ne tient pas compte du facteur de forme du plan: dans le premier cas par exemple un plan allongé aura un rayon trop petit dans la direction principale et trop grand autrement. Elle offre cependant l'avantage de la rapidité puisque peu de calculs sont nécessaires à chaque itération. Les tests avec cette méthode ont montré qu elle fonctionne très rapidement (0.1ms en moyenne, varie selon la taille des plans) mais réalise occasionnellement de mauvais appariement lorsque les plans ont des formes qui s éloignent trop du rectangle ou du cercle. La seconde approche utilise l'information de contour des plans extraite précédemment. Elle consiste à vérifier s'il existe au moins un point du nuage A qui se situe à l'intérieur du contour délimité par le bord du nuage B. Pour ce faire on vérifie que pour chaque couple de point consécutifs du contour de B, le point de A considéré se trouve à l'intérieur du parcourt. La notion d'intérieur est délicate à définir dans le cas général. La manière qui a semblé la plus efficace pour traiter ce problème est de considérer un contour orienté: l'intérieur de la forme se situe alors toujours du même coté dans le sens de parcours. C'est pourquoi la méthode de détection du contour ci-dessus a été mise en place. Cette seconde méthode s est avérée plus performante pour l'appariement des plans en permettant d'éviter les faux appariements. Cependant le temps d'exécution moyen est plus long, de l'ordre de 0.9ms. 58

60 5. Problème lié à la reconstruction 3D temps réel La reconstruction 3D en temps réel a été testée et fonctionne correctement en parallèle du SLAM. Cependant le fait de reconstruire la carte en temps réel pose certains problèmes. En effet l'algorithme de supervision du SLAM agit après avoir observé la scène, une fois que les plans ont été détectés. Si l'on recolle les plans avant la mise à jour on court le risque de recoller des plans qui ne devrait pas l'être. Or la mise à jour peut très bien survenir à la fin de la séquence d'acquisition. Il faudrait donc recoller les plans à la fin de l acquisition donc gérer en plus du nuage de point volumineux l'ensemble des plans détectés ce qui va à l'encontre de l'un des objectifs initiaux (réduire le volume des données à gérer). Le choix a donc été fait de reconstruire la scène avec les plans a posteriori pour des raisons de pertinence de la reconstruction. 6. Recollage des textures Une dernière étape pour le rendu correcte de la carte est de lui donner une apparence réelle en recollant sur les plans identifiés les textures. Pour cela il faut procéder en deux étapes: créer les textures puis les plaquer sur les plans. La création des textures se fait à partir du sous nuage de point support du plan. Ce sous nuage est projeté dans l'espace 2D du plan et une image est créée à partir du rectangle englobant le nuage. Les couleurs sont déterminées pour chaque pixel en fonction de la résolution souhaitée de la texture. Les pixels prennent simplement la valeur du point du nuage le plus proche. Ainsi on obtient une carte construite à partir de plans texturés et non plus d'un simple nuage de point. Les images ci-dessous illustrent les résultats obtenues. 59

61 7. Organisation du code pour la reconstruction 3D La partie reconstruction 3D est organisée en deux packages: reco3d et reco3drt. Comme leur nom l indique le premier permet la reconstruction 3D à posteriori et le second la reconstruction 3D temps réel. Reco3D Ce package contient plusieurs exécutables permettant de reconstruire la scène a posteriori avec des plans texturés ou non, permettant la détection du sol dans une carte 3D ou permettant la visualisation d une carte précédemment sauvegardée. Build_static_map Ce programme construit la carte d un environnement sous forme de plan à partir d un nuage de point fournit par le SLAM. Il permet la reconstruction de grands ensembles. Ce programme est utile pour les applications où seule la forme compte et où la mémoire est restreinte. Build_static_map_with_texture Ce programme construit la carte texturée d un environnement sous forme de plan à partir d un nuage de point fournit par le SLAM. Il permet la reconstruction de grands ensembles. Il est plus lent que le précédent du fait de la nécessitée de calculer les textures et de les plaquer sur les plans détecté mais offre une meilleur représentation de l environnement. Ce programme demande aussi plus de mémoire. c_viewer c_viewer pour «cloud viewer», est un programme permettant la visualisation d une carte sous forme d un nuage de points. Le format de la carte en entrée est soit un nuage de points de type (x,y,z,r,g,b) soit (x,y,z,r,g,b,a). Reco3DRT Ce package contient plusieurs exécutables permettant de reconstruire la scène avec des plans non texturés en temps réel et de détecter le sol en temps réel. constructrt Ce programme construit une carte formée de plan non texturés à mesure que la kinect se déplace. Ground_detect Ce programme permet la détection du sol en temps réel à mesure que la caméra se déplace. Il prend directement en entrée les données de la kinect et ne nécessite pas de faire tourner le SLAM en parallèle. 60

62 E. Résultats Cette partie présente les résultats obtenus à partir des développements exposés précédemment. Chaque test est décrit en détail et justifié. Les résultats sont discutés à la fin de cette partie. I. Matériel de tests Le matériel utilisé a varié au cours des tests. Dans un premier temps faute d avoir un robot disponible, les mesures ont été faites en montant la kinect et l ordinateur sur un chariot à roulette poussé à la main pour simuler le déplacement du robot. Dans un second temps la kinect a été montée sur le robot de test Pixou et tous les tests ont étés refait dans cette configuration. Le robot était piloté par joystick et l ordinateur était monté sur la plateforme du robot. Note sur le robot Pixou Le robot Pixou est un robot à 2 fois 2 roues dont l orientation est pilotée par le différentiel moteur entre les cotés gauche et droit. L inconvénient majeur de ce type de robot est que des frottements importants sont générés dans les phases de rotation, ce qui créé de fortes vibrations. Il présente par ailleurs de forte 61

63 accélérations au démarrage qui le rendent difficile à manier lorsqu une vitesse lente est souhaitée, ce qui est le cas ici du fait du temps d exécution du programme. On peut donc considérer que les tests ont été réalisés dans des conditions plutôt défavorables. II. Test du SLAM 1. Fidélité de la reconstruction 3D Si la mesure des temps d'exécution d'un algorithme est aisée, mesurer la qualité de la reconstruction 3D dans le cas général n'est pas triviale, et ce pour deux raisons. D'abord parce que cela suppose de pouvoir comparer le modèle fourni par le SLAM à un modèle numérisé de la réalité qui n'est pas toujours disponible. Ensuite parce que même si ce modèle était disponible il faudrait définir un moyen de mesurer la proximité de deux modèles, ce qui n'est pas trivial. Cependant en se plaçant dans un cas particulier et sous certaines hypothèses il est possible d'évaluer la qualité de la reconstruction. En l'occurrence le choix a été fait de se placer dans le cas de la reconstruction d'un couloir. L'avantage de ce cas particulier et que l'on dispose a priori d'un modèle de l'environnement simple (murs perpendiculaires au sol et parallèles entre eux) et que le SLAM peut être testé sur une distance relativement grande (selon la taille du couloir). Par ailleurs on peut assez facilement trouver les murs et le sol par segmentation dans ce type de scène, ce qui sera utile pour le test. Deux situations seront envisagées: - La première consiste à reconstruire le couloir avec le robot en translation - La seconde consiste à reconstruire le couloir en pivotant autour d'un point Ces tests permettent d'estimer respectivement la précision de la reconstruction en translation et en rotation. Concrètement la mise en œuvre consiste en trois étapes successives: - scan de l'environnement avec le SLAM - recherche dans le nuage de points des plans formant les murs et le sol - calcul des angles entre les murs et entre les murs et le sol 62

64 Résultats Les résultats des tests sont présentés dans le tableau ci-dessous. Test en translation Test en rotation Angle entre le mur1 et le sol 90.5 Erreur = 0.56% 92.3 Erreur = 2.6% Angle entre le mur2 et le sol 91.5 Erreur = 1.56% 88.6 Erreur = 1.56% Angle entre les murs 0.7 Erreur = 0.78% 1 Erreur = 1.1% Erreur moyenne 0.97% 1.75% Figure 17: Carte du couloir utilisée pour le test (Ici reconstruit en translation). En vert clair et bleu les murs détectés, en vert foncé le sol. Les parties rouges correspondent au reste du nuage associé à aucun de ces ensembles. Les tests montrent donc que la fidélité de la reconstruction est tout à fait satisfaisante. En effet des erreurs de l ordre de 1% sont acceptables, l œil humain ne pouvant distinguer ce genre d erreur. On remarque que l erreur en rotation est un peu plus importante qu en translation. Cela vient de l a plus grand difficulté qu il y a à calculer les rotations que les translations Amélioration due à ICP Une autre série de tests a été faite pour évaluer plus spécifiquement l'amélioration de l'estimation de pose due à ICP. Ce test se justifie du fait de la lenteur d'icp, selon les cas si l'amélioration de la pose n'est pas significative il peut être intéressant de ne conserver que RANSAC pour gagner en vitesse d'exécution. 12 Imprécision lors du calcul, vibration du robot dans la phase de rotation etc. 63

65 Le principe de ces tests est d'estimer la transformation entre deux nuages avec RANSAC ou RANSAC+ICP et de mesurer la distance entre les nuages recollé avec chacune des méthodes. La distance des deux nuages est définie comme la somme des distances des points appareillés deux à deux dans les nuages. Le schéma ci dessous illustre ce principe. Résultats Les tests montrent que les résultats dépendent largement de la scène observée. Dans le cas de scène simple RANSAC est performant et suffit à estimer correctement les transformations successives. Lorsque la scène devient plus complexe en revanche ICP devient plus important pour la correction de l erreur faite par RANSAC. Pratiquement ICP permet une amélioration de l estimation de pose de 0.5% dans les cas simples à 4.5% dans les cas les plus complexes. Figure 18: recollage de nuages avec RANSAC seul (en haut) et RANSAC + ICP. Les détails montrent qu'icp permet de mieux recoller les nuages de points (pas de dédoublement de plans). 64

66 3. performances avec et sans supervision Pour caractériser l amélioration de la reconstruction 3D due à la supervision (Toro) par rapport au cas où seule l estimation de mouvement est utilisée, un test a été mis en place consistant à scanner l environnement avec ou sans l algorithme de supervision activé. Figure 19: Comparaison des performances avec ou sans supervision. On remarque que sans algorithme de supervision le recollage des nuages est entaché d'erreur (dédoublement) alors qu'avec l'algorithme de supervision les erreurs ont disparues. Le couloir est scanné en faisant effectuer par le robot un aller retour, de cette façon d une part la taille de la zone scannée est doublée, d autre par le robot repasse par des zones déjà visitées et l effet de l algorithme de supervision est observable. On constate que sans algorithme de supervision la dérive des mesures de position est observable rapidement. Même de faibles valeurs perturbent le modèle de façon significative en dédoublant les plans. Ceci est particulièrement gênant pour la seconde phase de détection de plans dans le modèle 3D. L algorithme de supervision joue donc un rôle fondamental dans l optimisation du graphe et permet une amélioration significative de la qualité du modèle. 65

67 4. Robustesse aux variations de luminosité L algorithme RGBD Mapping est présenté dans [1] comme offrant une bonne robustesse aux variations de luminosité. Les tests ont en effet montré qu il est possible de reconstruire une pièce plongée dans l obscurité sans difficulté. L utilisation en extérieur en revanche n a pas été possible. Les conditions de test sont les suivantes : temps ensoleillé et sol réfléchissant (carrelage). La défaillance vient de la Kinect qui ne fonctionne pas dans ces conditions. Le problème est très probablement lié à la caméra IR qui ne peut discerner les patterns projetés par le laser si la luminosité est trop forte. 66

68 III. Test de la reconstruction 3D Comme précisé plus haut deux packages ont été développés pour la reconstruction 3D. L un fonctionnant en temps réel pour la modélisation sans texture et l autre fonctionnant a posteriori qui réalise une carte avec des textures. Ces deux packages utilisent les mêmes algorithmes pour la segmentation. Après validation du fonctionnement en temps réel les tests ont donc surtout porté sur la reconstruction a posteriori qui présente l intérêt d offrir un meilleur rendu en plus d être plus simple à tester (mis en œuvre moins lourde). 1. Test de la détection du sol en temps réel L un des principaux objectifs de la reconstruction 3D est de fournir au robot des informations pertinentes sur la nature des objets qu il observe. Plus particulièrement la détection du sol et des zones navigables est une tâche essentielle pour la planification de trajectoire. C est donc logiquement la première fonction qui a été testée. Le test a consisté essentiellement à évaluer la robustesse de la détection notamment en fonction des inclinaisons que peut prendre le robot. La Kinect a été incliné à plus de trente degré sans que la détection ne soit mise en échec. Ce programme montre donc une détection robuste du sol est possible en temps réel. Figure 20: détection du sol robuste. Même avec des inclinaisons de plus de 30 le sol est toujours détecté correctement 67

69 2. Test de la reconstruction 3D Les algorithmes de segmentation sont ajustés par plusieurs paramètres qui jouent sur la qualité de la reconstruction. Avant de commencer la segmentation il convient de filtrer le nuage de points. Sans cette étape la recherche dans l ensemble du nuage est très lente du fait de la quantité importante de points à traiter. Le filtrage utilise des voxels. Il s agit de partitions de l espace en cases de dimension fixe. Si la case contient des points alors n est conservé que le barycentre des points. Sinon la case reste vide. On peut ainsi limiter fortement la quantité de points dans le nuage tout en gardant une bonne cohérence du modèle. Le premier paramètre à faire varier est la taille des cases. Ensuite l algorithme de recherche des plans utilise deux paramètres. La taille du voisinage dans lequel rechercher les points à ajouter au plan et le seuil définissant la distance maximum pour accepter un point candidat. Chacun des ces paramètre a une influence sur la pertinence des plans trouvés. Taille des voxels La taille des voxels conditionne d une part la vitesse d exécution, puisque le nombre de point est fonction de la taille des cases, et d autre part la taille minimal des objets observable. Une valeur raisonnable pour un environnement de grande taille est de quelques centimètres. Cette valeur permet de limiter les temps de calcul tout en permettant d observer des objets de relativement petite taille. Les tests ont montré qu une valeur de 4cm donne de bons résultats. Seuil Le nuage de point initial est bruité du fait des erreurs de mesure de la kinect auxquelles s ajoutent des erreurs d estimation de pose qui ont tendance à faire dériver les mesures de position. Si le seuil est trop bas l algorithme a tendance à couper les plans en plusieurs parties comme ci-dessous. S il est trop haut l algorithme tend à fusionner des plans qui ne devraient pas l être. La valeur de seuil choisie ici est 1.8cm. 68

70 Voisinage Le voisinage définit la taille de la zone dans laquelle seront recherchés les points à ajouter au plan. Plus la taille est grande plus le temps de calcul est grand car la recherche du plus proche voisin prend plus de temps. Dans la pratique il faut que le voisinage soit plus grand que la distance moyenne entre les points définie par la taille des voxels, mais en même temps il faut que cette taille soit la plus faible possible. La distance utilisée ici est 4.5cm. Résultats Avec les valeurs données précédemment on obtient une reconstruction correcte de la carte présentée ci-dessous (Figure20 photo b et c) en environ 40 secondes. 69

71 Figure 21: page précédente: carte sous forme de nuage de points; en haut: carte sous forme de plans non texturés; en bas: carte avec plans texturés. 70

72 Espace mémoire des différentes représentations Le troisième objectif de la segmentation et la reconstruction 3D était de permettre de diminuer la taille de la carte pour permettre son transfert via le wifi notamment. Dans le cas de la carte texturée l économie de mémoire dépend largement de la résolution utilisée pour les textures. Le programme fournit des statistiques quant au nombre de points formant chaque plan et chaque contour. Il faut noter que ces statistiques dépendent du nombre de plans détectés dans la scène : une fois les plans principaux détectés seules de petits plans resteront et les statistiques évolueront dans le sens d un ratio points dans le nuage/points du contour plus important. En moyenne pour 500 plans détectés le contour d un plan est formé de 28,28 points. Dans la représentation sous forme de nuage de points chaque points est défini par une position (3 floats = 3 * 4 octets) et une couleur (rgba = 4 octets) soit 16 octets par points. Dans la représentation sous forme de plans texturés chaque plan est représenté par les points du contour (3 float = 12octets) plus une texture RGB (128² * 3 octets ou 64² * 3). Le nuage initial utilisé pour les tests comporte environ 1.6Million de points ce qui représente 25.6 Moctets de données. Dans la représentation sous forme de plan avec 500 plans on a 500*(128²*3+28,28 *12) soit environ 24.74Moctets dans le cas de texture 128x128pixels et 6.3Moctets dans le cas de textures 64x64. Autrement dit les 71

73 économies de place mémoire avec des textures de 128² ou 64² sont respectivement de 3.5% et 73.5%. L économie de place n est donc significative que pour des textures de dimension relativement faible. On peut cependant remarquer que les plans de taille importante, susceptibles de nécessiter des textures de grande taille, ne représentent qu une petite partie des plans détectés. On peut alors envisager utiliser des textures de 256x256pixels pour les 10 plus grands plans et des textures de 64x64pixels pour les autres plans. De cette manière la taille de la carte serait ramenée à 8.1Moctets soit une économie de 69% environ. Si maintenant on se limite au cas de plan non texturés alors la taille de la carte devient 0,17Moctets soit une réduction d un facteur 150!!! 72

74 IV. Discussion sur le SLAM et la reconstruction 3D Les résultats montrent qu il est possible de reconstruire en temps réel un environnement 3D grâce à la Kinect et en même temps de s y localiser. Par ailleurs on peut arriver à extraire de la carte ainsi construite des données comme la nature des objets simples (sol, mur, porte etc) représentés dans la carte. Enfin il est possible de reconstruire la carte a posteriori sous forme de plans texturés. Les avantages de cette représentation sont d une part que les plans ont une signification plus grande que les points puisqu ils modélisent des objets réels, et ce aussi bien pour le robot que pour l observateur humain, d autre part la représentation sous forme de plan permet d économiser de l espace mémoire de manière significative, surtout dans le cas d une représentation non texturée. Cependant un certain nombre de remarques peuvent être faites quant aux résultats obtenus. Premièrement il convient de noter que la qualité de la reconstruction 3D est largement conditionnée par la qualité de la carte fournie par le SLAM. En effet si des plans sont mal recollés dans la phase de SLAM alors lors de la reconstruction 3D au lieu d un unique plan se seront plusieurs plans qui seront détectés et la représentation perdra en qualité. Il faut donc impérativement que la phase de SLAM soit efficace. Pour cela il est souhaitable d utiliser l algorithme ICP pour affiner l estimation de pose. Deuxièmement comme expliqué plus haut la phase de segmentation dépend sensiblement des valeurs de trois paramètres : le voisinage, le seuil et la taille des voxels. Si ce dernier paramètre et du même coup la taille du voisinage dépendent essentiellement d un choix de l utilisateur (résolution souhaitée), la valeur du seuil elle dépend davantage des conditions d observation de la scène. Par exemple si le nuage est très bruité il faudra privilégier un seuil élevé. S il est faiblement bruité on pourra choisir un seuil bas. Le choix du seuil implique donc de la part de l utilisateur ou du robot une connaissance de la scène observée. Ceci n est pas forcément souhaitable dans le cadre du développement d une méthode robuste dans toute situation. La méthode pourrait sans doute être améliorée par l utilisation d un seuillage adaptatif. Enfin on peut s interroger sur la pertinence de la représentation sous forme de plans. Cette représentation est clairement plus proche de la réalité que celle sous forme de points. Cependant elle présente elle aussi des limites. La pertinence des plans importants comme le sol ou les murs n est pas à démontrer mais pour ce qui est des plus petites structures utiliser des plans n est pas forcément la meilleure solution. Une représentation plus cohérente pourrait être par exemple une représentation hybride points/plans ou mieux plans/maillage. Les points qui ne seraient pas associés à des plans seraient alors utilisés pour créer un maillage de 73

75 l objet local. Ceci suppose cependant de résoudre le problème de la polygonalisation (création du maillage) en temps réel. Finalement cette étude a contribuée à montrer que la 3D n est pas le futur mais le présent de la robotique. Il est aujourd hui possible de traiter en temps réel un certain nombre des problèmes posés par ce nouvel environnement complexe. Cependant il reste des défis à relever pour permettre au robot de modéliser et de comprendre son environnement dans toute sa richesse. 74

76 F. Conclusion de l étude L apparition de la Kinect représente une véritable révolution pour le domaine de la robotique en permettant le passage d un monde 2D, relativement pauvre en information à une vision 3D capable de rendre compte de toute la richesse d un environnement réel et ce pour un coût dérisoire. La Kinect contribue donc fortement à franchir un pas décisif vers l automatisation des robots en ouvrant de nouvelles perspectives pour le SLAM, la reconnaissance d objets, la reconstruction 3D etc. Comme l a montré cette étude il est aujourd hui possible de réaliser du SLAM 3D avec ce matériel avec des performances très intéressantes. Il a été montré notamment que le SLAM 3D permet de reconstruire un environnement intérieur fidèle au modèle réel et visuellement proche de la réalité. Il est par ailleurs possible d extraire de ce modèle un très grand nombre d informations qui permettent d identifier la nature des objets observés (mur, sol, escalier etc) et donc d enrichir la connaissance qu a le robot de son environnement. En outre l extraction de données permet de reconstruire une carte simplifiée de l environnement, plus légère et offrant un meilleur rendu. Cependant il reste encore de nombreux défis à relever pour mettre au point du SLAM 3D capable de fonctionner en condition réelle. Le premier d entre eux vient du très grand nombre de données à gérer dans un environnement 3D. L utilisation de la segmentation pour le filtrage des données présentée dans ce rapport permet de diminuer significativement la quantité d informations à transmettre. Cependant pour envisager de scanner de très grands environnement il faudrait probablement concevoir d autres algorithmes de SLAM qui permettraient de gérer efficacement la masse d information. Il faudrait probablement inventer une nouvelle manière de représenter l information spatiale. Un autre défi majeur à relever est le cas de la scène dynamique, c'est-à-dire d une scène dans laquelle les objets se déplacent. Pour réaliser du SLAM dans ce cadre il faudrait pouvoir différencier le mouvement d un objet de celui du robot. Loin d être triviale cette question fait l objet de recherches actives à l heure actuelle. Sa résolution sera un préalable essentiel à la mise au point d un SLAM réellement opérationnel en toute condition. Un second point qu il convient de noter est que l avènement de la Kinect préfigure sans doute l arrivée prochaine de nouveaux types de capteurs destinés non plus à mesurer une grandeur physique unique mais une multitude de grandeurs de nature différente et dont la connaissance simultanée permettrait d accéder à des informations de haut niveau sur l environnement. Développer des machines intelligentes sera très probablement conditionnée par le développement de ce type de capteurs intelligents. 75

77 Enfin l arrivée de la Kinect sur le marché permet aussi de faire un certain nombre d observations sur les mécanismes qui conduisent à des avancées importantes dans un domaine comme la robotique. Le développement d un tel capteur n aurait pas été possible sans les fonds mobilisés par une grande structure comme Microsoft. Les grandes entreprises sont donc essentielles pour la mise au point de nouveaux matériels innovants dont le développement est très couteux. Cependant la Kinect n aurait pas connu un tel succès sans la mise à disposition de driver open source par des hackers grâce à qui cette étude a été possible. En permettant à tous de travailler avec cet outil, ils ont offert à la communauté robotique (entre autre) la possibilité d exprimer sa formidable créativité, son ingéniosité et son dynamisme. Chercheurs, ingénieurs étudiants ou simples passionnés, tous ont pu contribuer aux développements d applications aussi diverses qu innovantes et tous ont permis de démontrer à quel point la mise en commun des savoirs est essentielle. Aujourd hui face à l engouement inattendu suscité par la Kinect Microsoft a publié un driver officiel pour la Kinect, preuve que les efforts de la communauté n ont pas été vint. Cela souligne l importance et le rôle incontournable que jouent aujourd hui le monde de l open source dans la recherche appliquée. 76

78 G. Bibliographie Articles [1] Peter Henry, Michael Krainin, Evan Herbst, Xiaofeng Ren, and Dieter Fox, RGB- D Mapping: Using Depth Cameras for Dense 3D Modeling of Indoor Environments [2] Stefan May, Stefan Fuchs, David Droeschel, Dirk Holz and Andreas Nuchter, Robust 3D-Mapping with Time-of-Flight Cameras [3] Bastian Steder Radu Bogdan Rusu Kurt Konolige Wolfram Burgard, Point Feature Extraction on 3D Range ScansTaking into Account Object Boundaries [4] Bastian Steder Radu Bogdan Rusu Kurt Konolige Wolfram Burgard,, NARF: 3D Range Image Features for Object Recognitio [5] Deyuan Qiu, Stefan May, and Andreas Nuchter, GPU-Accelerated Nearest Neighbor Search for3d Registration [6] Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, The 3D Hough Transform for Plane Detection in Point Clouds: A Review and a new Accumulator Design [7] E. Olson, J.J. Leonard, and S. Teller. Fast iterative optimization of posegraphs with poor initial estimates. In Proc. of the IEEE Int. Conf. On Robotics & Automation, pages , 2006 [8] Deyuan Qiu, Stefan May, and Andreas Nuchter, GPU-Accelerated Nearest Neighbor Search for 3D Registration [9] Nico Cornelis, Luc Van Gool, Fast Scale Invariant Feature Detection and Matchin on Programmable Graphics Hardwar. CVPR 2008 Workshop [10] Ruwen Schnabel, Roland Wahl, Reinhard Klein, Efficient RANSAC for Point-Cloud ShapeDetection [11] Zoltan Csaba Marton, Radu Bogdan Rusu, Michael Beetz, On Fast Surface Reconstruction Methods for Large and Noisy Point Clouds Livres [12] Springer Handbook of Robotics, Siciliano, Bruno; Khatib, Oussama (Eds.), Springer, 2008 [13] Probabilistic Robotics, Sebastian Thrun, Wolfram Burgard, Dietr Fox, The MIT Press,

79 H. Annexes I. ECA 1. Présentation du groupe ECA Le groupe ECA dont le siège social est basé à Toulon est spécialisé dans la conception et la production de systèmes automatisés tels que les drones de surface (USV), les drones sous-marins autonomes (AUV) et les robots terrestres (UGV). L ensemble des sociétés qui composent le groupe ECA travaillent dans les domaines de la défense, du nucléaire et de l aéronautique. Le groupe qui contrôle aussi des filiales en Turquie, Italie, Etats-Unis et Grande Bretagne compte 600 salariés a réalisé, en 2010, près de 100 M de chiffre d affaires. ECA, côté au second marché EURONEXT à hauteur de 46 %, est contrôlé à 53.7 % par le groupe Français et indépendant GORGE. Défense Robotique Contrôle et commande de système Robotique Imagerie Simulation Civil Aéronautique Société ECA Robotics ECA est spécialisée dans l étude et la réalisation de systèmes automatisés opérant en milieux contraignants. 78

80 Depuis 1936, la société ECA conçoit et réalise des systèmes automatisés pour l intervention dans les milieux hostiles. S appuyant sur une structure articulée autour de chefs de projets expérimentés, la société ECA a su acquérir une maîtrise d œuvre industrielle lui permettant de manager des programmes militaires et civils et de répondre aux exigences de performances, de coûts et de délais de ses clients tant en France qu à l export (50% du CA réalisé à l export). La société ECA est spécialisée dans les systèmes complexes: Défense & Robotique : Equipement pour le déminage sous-marin, Equipement pour les Forces Navales : moteur, traitement du signal, convertisseur, etc. Equipement pour les forces terrestres : protection du soldat, robot de reconnaissance et de déminage, etc. Robotique Civile : Nucléaire : conception et réalisation de machine spéciale de démantèlement et de manutention, télé opération, robotique d inspection, Offshore : Systèmes d inspection et d action à distance : robots, systèmes automatisés. L offre ECA couvre toutes les phases du cycle de vie de ses systèmes : de la conception au support logistique. Ainsi ECA accompagne ses clients pendant toute la durée de vie de ses produits et possède ainsi la capacité à assurer la maintenance opérationnelle de ses produits afin d offrir à ses clients une prestation professionnelle, complète et pérenne. 2. Les Activités du Pôle de Saclay Organisation générale Le pôle de Saclay s'appuie sur une Organisation Matricielle, c'est à dire un système qui s'organise autour de ce que l'on peut appeler la "Works force". Cette "Works force" est composée de toutes les ressources humaines disponibles pour mener à bien un Projet (Ingénieur spécialisés, Ingénieur généraux, Techniciens...). Autour de cette "Works force", on retrouve les Fonctions Support (Administration, Commerciaux...) et la hiérarchie (Chef de Projet, Responsable d'activité et Responsable d'etablissement). 79

81 L'organisation d ECA Saclay peut être schématisée de la façon suivante : Produits Rappelons que le pôle de Saclay s'intègre au secteur "Robotique Terrestre de Défense et Sécurité", aussi l'ensemble de ces activités s'intègrent naturellement dans ce secteur d'activités. Les Activités de Pôle de Saclay s'articulent autour de 4 Robots phares, qui peuvent être adaptés à différents besoins ou aux différentes demandes du client: L INBOT : L INBOT est le plus petit robot terrestre commercialisé par ECA (250 (L) x 155 (W) x 110 (H)). Léger, petit, discret il permet d'explorer des environnements étroits. De plus avec son autonomie de 2h, il permet des explorations de qualité. Equipé d'un système éclairage, et de lumières infrarouges, il peut explorer des endroits obscurs. 80

82 Le COBRA : Le COBRA permet d'avantage de chose que l INBOT présenté précédemment. Il est légèrement plus grand (364 (L) x 392 (W) x 170 (H)) sa capacité d'emport est de 4kg ce qui est prodigieux pour un robot qui ne pèse que 6kg. Il possède des modules missions divers et variés, par exemple 2 supports canons de taille différente, capables de détruire des colis ou tout autre menace. Enfin il a la capacité de ce retourner, ainsi même à distance l'opérateur peut continuer à piloter le robot après un retournement. Le CAMELEON : Comme son nom l'indique, le CALELEON est capable de s'adapter à une multitude de demandes clients. Sa taille modérée (670 (L) x 520 (W) x 190 (H)) lui permet d'être à l'aise en intérieur comme en extérieur. il possède une multitude de modules missions (prévention Nucléaire, défense terrestre, détection,...) Ces 2 "flippeurs" avant lui permettent de se surélever pour augmenter sa portée, ou bien ils lui permettent de se stabiliser sur des terrains accidentés. 81

83 Le TSR 202 : Le TSR est le plus gros robot terrestre que commercialise ECA (1,2 (L) x 0,67 (l) x 1 (H)) pour 270kg. Avec son bras à 6 degrés de liberté et une capacité de levage de 70kg, Il s'adapte aux missions les plus périlleuses pour l'homme. Avec son porté de 400 m et ses 2 supports canons il peut détruire des colis de grande envergure. 82

84 II. Kd-tree Un kd tree, arbre kd en français, est une structure de données qui partitionne l'espace pour stocker des points. Chaque nœud de l'arbre contient un point du nuage. Tous les nœuds non terminaux divisent l'espace en deux suivant x,y, ou z alternativement (dans le cas d'un 3d tree, c'est a dire un kd tree pour lequel k = 3, ce qui est le cas ici). Les points situés dans chacun des deux demi-espaces forment les branches gauche et droite du nœud considéré. On peut par ailleurs souhaiter avoir un arbre équilibré, c'est à dire dont les branches comportent le même nombre de nœuds. On choisit alors à chaque itération le point médian dans la direction choisie. Une autre solution serait de prendre le point le plus proche de la distance moyenne entre le max et le min dans la direction choisie. L'avantage de ces structures est qu'elles permettent un accès à l'information plus rapide que dans un simple tableau. Beaucoup d'algorithmes ont étés mis au point qui permettent de rechercher de manière très efficace des informations dans un tel arbre, comme par exemple le plus proche voisin d'un point. 83

85 III. SIFT SIFT est un algorithme qui permet la détection, la description et l'appariement de points d'intérêts. Il fut décrit pour la première fois dans []. Comme son nom l'indique SIFT est un algorithme qui permet la description de points d'intérêt de manière indépendante de l'échelle, ce qui le rend robuste aux variations qui peuvent survenir dans l'image. La méthode est composée de deux volets: un algorithme de détection des points et de calcul des descripteurs et un algorithme de mise en correspondance des points. 1. Détection des points d'intérêt La première étape consiste en la recherche d extrémums locaux de luminosité dans l'image. L'algorithme utilise un espace 3D, nommé espace des échelles, de lequel les points ont pour coordonnées x,y, la position dans l'image et un coefficient sigma correspondant à un facteur d'échelle. Ce facteur caractérise la taille d'une zone d'intérêt de rayon proportionnel à ce facteur. L'espace d'échelle d'une image, notée L(x,y,o) est défini par la convolution d'une fonction gaussienne de paramètre variable et de l'image originelle. (,, ) = (,, ) (, ) Dans la pratique on créer un espace discret en calculant une pyramide gaussienne à partir de l'image. La détection des points d'intérêt se fait alors en calculant les extrema de la différence de gaussiennes (DoG) entre deux niveaux successifs de la pyramide: (,, ) = ( (,, ) (,, )) (, ) = (,, ) (,, ). Les points où s'annule la fonction DoG sont les points d'intérêt (extremums). Le principal avantage d'utiliser la fonction DoG est la rapidité de calcul: il suffit de faire une simple différence entre deux images pour l'obtenir. 84

86 Par ailleurs la fonction DoG représente une bonne approximation de la fonction LoG normalisée (Laplacian of Gaussian), qui comme Linberg l'a montré, permet une véritable invariance d'échelle. De plus Mikolajczyk (2002) a montré que les extrema de cette fonction sont des points d'intérêt beaucoup plus stables que la plupart des autres solutions envisagées. Dans la pratique la recherche des extremums se fait en comparant chaque pixel à ces huit voisins dans l'image considérée et ces neufs voisins dans les images d'échelle précédente et suivante. Le point est considéré comme un extremum que s'il est supérieur ou inférieur à tous ses voisins. Cette étape permet la détection d'un très grand nombre de points mais ceux-ci sont assez mal localisés du fait de l'utilisation de gaussiennes qui réduisent la résolution de l'image, d'autant plus que l'ordre des gaussiennes est élevé. Il faut alors améliorer la localisation en ayant recours à l'interpolation. Ceci se fait classiquement en utilisant le développement de Taylor du second degré de la fonction L autour du point considéré et en calculant le point où s'annulent la dérivée de L par rapport à la position. Une fois cette étape réalisée il reste encore beaucoup de points qui sont instables du fait par exemple d'un trop faible contraste ou de leur position sur une zone non discriminante (contours). Il faut alors réduire le nombre de ces points pour ne conserver que ceux qui sont réellement intéressants et susceptible d'être retrouvé dans les deux images. Les points de faible contraste sont détectés en observant la dérivé de L aux alentours du maximum. Si sa valeur est trop faible le point n'est pas retenu. Pour détecter si le point se situe sur une arrête il faut déterminer la forme de la fonction DoG autour du point d'intérêt. En effet un contour est défini par un rayon de courbure important dans une direction et faible dans la direction normale. Les courbures principales se calculent à partir de la fonction hessienne : = Les valeurs propres de H sont proportionnelles aux rayons de courbures recherchés. Cependant pour éviter le couteux calcul des valeurs propre et puisque seul leur rapport de ces valeurs est intéressant, on calcule la trace et le déterminant de H ( ) = + = +, ( ) = ( )2 =. 85

87 D'où l'on extrait, en posant =, l'expression du ratio des valeurs propres : En fixant r on peut alors définir un seuil à partir duquel les points seront retenus ou non. Une fois ce filtrage effectué, une orientation est calculée pour chaque point détecté. Ce calcul est d'autant plus important que la direction trouvée sera utilisée pour la calcul des descripteurs et c'est elle qui garantira l'invariance des descripteurs par rotation. Le calcul se fait selon la procédure suivante: - On estime le gradient à chaque position dans le voisinage du point d'intérêt par différences finies (norme et orientation) - on calcul un histogramme des orientations dans le voisinage. Cet histogramme est pondéré par le rayon de la zone d'intérêt égale à 1.5 fois le facteur d'échelle ainsi que par l'amplitude de chaque point. - Les pics de l'histogramme dont la valeur est au moins égale à 80% de la valeur maximale sont utilisés pour créer des points supplémentaires qui ne différent que par leur orientation principale. Cette étape achevée, les points d'intérêt sont décrit par quatre coefficients, leur trois coordonnées dans l'espace des échelles et l'angle définissant leur orientation. On peut alors calculer les descripteurs correspondant à chacun de ces points. 2. Calcul des descripteurs Le principe du calcul des descripteurs est le suivant. Premièrement on définit pour chaque point d'intérêt un repère local à partir de l'angle précédemment calculé. On considère ensuite une zone de 16x16 pixels autour du point d'intérêt subdivisé en 4 zones égales (note: la subdivision peut se faire avec d'autres valeurs). Pour chacune d'elles on calcule un histogramme des orientations de 8 intervalles entre 0 et 2pi. Le calcul de l'histogramme se fait comme décrit plus haut. On applique par ailleurs une double pondération, par l'amplitude et part une fenêtre gaussienne centré sur le point d'intérêt. 86

88 Les histogrammes correspondant aux 16 zones définies sont alors concaténés et renormalisé. Dans le but de rendre le descripteur robuste aux variations de luminosité les valeurs supérieures à 0.2 sont misent à 0.2 et l'histogramme est renormalisé avec ces nouvelles valeurs. On obtient finalement le descripteur de ce point sous la forme d'un vecteur composé des 128 valeurs de l'histogramme. IV. SURF (Speeded Up Robust Feature) Tous comme SIFT, SURF est à la fois un détecteur et un descripteur de points d'intérêt. Comme précisé plus haut il s'inspire ne partie de SIFT mais est plus performant en termes de temps de calcul grâce à deux approximations. 1. Détection de points d'intérêts L'objectif du calcul des points d'intérêt par cette méthode est comparable à celui de SIFT: détecter les maximums et minimums de luminosité. Cependant la méthode diffère sur plusieurs points, notamment du fait de l'utilisation des images intégrales. Integral image Les images intégrales ont étés introduite pour la première fois par Viola and Jones dans []. Il s'agit d'image dont la valeur au point p(x,y) est telle que : ( ) = (, ) L'avantage qu'apporte cette représentation par rapport à une image classique est qu'il suffit de trois soustractions pour déterminer la somme des intensités dans un rectangle donné, quelque soit sa taille. Ceci est d'autant plus intéressant que l'on utilise des filtres de grande taille. 87

89 Hessian matrix Pour améliorer encore l'efficacité de l'algorithme (en termes de temps de calcul), la détection de points se fait avec une approximation de la matrice hessienne permettant de réduire le nombre de calculs. On cherche alors les extremums de ( )= (. )² Espace des échelles Par rapport à ce qui était fait pour SIFT, le fait d'utiliser les images intégrales avec une approximation de la Hessienne fait qu'au lieu d'appliquer successivement le même filtre sur des images successives pour la construction de la pyramide gaussienne, on peut maintenant calculer en parallèle les niveaux de la pyramide à partir de l'image d'origine. Ainsi l'espace des échelles discret est construit en agrandissant la taille du filtre. Cet espace est divisé en octave qui correspond à une série de niveaux créé à partir de la convolution de l'image initiale par des filtres de taille croissante. Le changement d'octave correspond à la multiplication de la taille du filtre par deux. 2. Calcul des descripteurs Le principe du calcul des descripteurs est comparable à celui de SIFT. On détermine dans un premier temps une orientation principale qui servira de référence pour le calcul d'un descripteur invariant par rotation. Ensuite le Calcul de l'orientation Pour chaque point d'intérêt on calcul une direction principale qui permettra d'être rouste en rotation. Pour ce faire on calcule la somme des réponses d'ondelette de Haar dans les directions x et y dans le voisinage du point d'intérêt égal a 6 fois l'échelle de détection du point. Dans la pratique il s'agit d'appliquer des filtres cidessous a l'image. La encore l'utilisation d'images intégrales permet de faire ces traitements en seulement 6 itérations. Une fois les réponses calculées et pondères par une gaussienne centrée sur le point d'intérêt la réponse est représente par un point dans un espace dont les valeurs en abscisse sont les réponses horizontales et les valeurs en ordonnées la réponse verticale. On fait ensuite tourner une fenêtre d'angle pi/3 autour du point d'intérêt et on somme l'ensemble des réponses dans cette fenêtre pour déterminer obtenir un vecteur d'orientation locale. L'orientation principale est donnée par le vecteur le plus long. 88

90 Calcul du descripteur La première étape pour le calcul du descripteur consiste a construire une zone carre de taille 20 sigma autour du point d'intérêt et oriente selon la direction principale définie précédemment. La zone est séparée en 16 sous régions de même taille dans chacune desquelles on calcule les réponses en onde de Haar pour 4 points régulièrement espaces Les valeurs des réponses sont pondérées par une gaussienne centrée sur le point d'intérêt puis somme dans chacune des sous régions. Pour conserver l'information d'orientation de chaque sous région sont aussi conserves les valeurs absolues des réponses suivant x et y, soit au total 4 coefficients par zone. En concaténant l'ensemble des valeurs on obtient finalement un descripteur de 64 valeurs pour décrire chaque point d'intérêt. V. Précision subpixellique Lorsque une grande précision est requise, il peut être nécessaire de déterminer la position d un point plus finement que ne le permettent les pixels d une image. On peut alors avoir recourt à des méthodes d interpolations. Le principe est de déterminer une fonction décrivant la manière dont varie la luminosité de l image entre plusieurs pixels, le plus souvent un polynôme, et d utiliser la valeur de cette fonction pour déterminer la position d un point. L image ci-dessus illustre ce principe. Les pixels permettent d estimer la courbe passant par eux (ici de la forme sinc(x)). La recherche du maximum de luminosité se fait ensuite directement sur la fonction d interpolation. 89