Encadrants ECA
Benoit Morisset Pierrick Daniel Encadrant ENSPS
Bernard Bayle
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
Septembre 2011
1
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.
2
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.
3
Sommaire
A. Introduction ... 6
B. Objectifs du projet... 7
C. Introduction au problème du SLAM ... 8
I. Autonomie des robots ... 8
1. Robotique: de la Fiction à la Réalité ... 8
2. Qu'est ce qu'un robot autonome? ... 9
3. Percevoir son environnement ... 9
II. Odométrie Visuelle ... 11
1. Points d’intérêt ... 11
2. RANSAC ... 12
3. ICP ... 13
III. SLAM ... 15
1. Définition du problème ... 15
2. Principaux paradigmes ... 16
3. Recherches actuelles ... 19
IV. TORO : SLAM 3D ... 20
1. Principe ... 20
2. Graph-SLAM ... 21
3. TORO : formalisme ... 21
V. Segmentation des nuages de points et reconstruction 3D ... 25
1. Principe de la segmentation ... 25
2. Mise en œuvre: cas des objets plans ... 26
D. Contribution au SLAM ... 28
I. Contexte général de l’étude ... 28
1. ROS ... 28
2. Kinect ... 29
II. Analyse de l’existant ... 33
4
III. Solutions retenues... 40
IV. RGBD Mapping ... 41
1. L’algorithme ... 41
2. Estimation de mouvement ... 42
3. Optimisation globale du graphe et fermeture de boucle ... 45
4. Implémentation ... 46
5. Schéma de fonctionnement du SLAM ... 48
V. Descripteur NARFs ... 49
1. Tests du détecteur ... 49
2. Conclusion générale sur les NARFs... 50
VI. Segmentation et Reconstruction 3D ... 51
1. Introduction ... 51
2. Détection de plans dans un nuage de points ... 52
3. Détection des contours d’un plan ... 55
4. Appariement de plans ... 57
5. Problème lié à la reconstruction 3D temps réel ... 59
6. Recollage des textures ... 59
7. Organisation du code pour la reconstruction 3D ... 60
E. Résultats ... 61
I. Matériel de tests ... 61
II. Test du SLAM ... 62
1. Fidélité de la reconstruction 3D ... 62
2. Amélioration due à ICP ... 63
3. performances avec et sans supervision ... 65
4. Robustesse aux variations de luminosité ... 66
III. Test de la reconstruction 3D ... 67
1. Test de la détection du sol en temps réel ... 67
2. Test de la reconstruction 3D ... 68
IV. Discussion sur le SLAM et la reconstruction 3D ... 73
F. Conclusion de l’étude ... 75
G. Bibliographie ... 77
5
H. Annexes ... 78
I. ECA ... 78
1. Présentation du groupe ECA ... 78
2. Les Activités du Pôle de Saclay ... 79
II. Kd-tree ... 83
III. SIFT ... 84
1. Détection des points d'intérêt ... 84
2. Calcul des descripteurs ... 86
IV. SURF (Speeded Up Robust Feature) ... 87
1. Détection de points d'intérêts ... 87
2. Calcul des descripteurs ... 88
V. Précision subpixellique ... 89
6
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é SLAM1. 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 lui- mê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
7
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é.
8
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.
9
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.
10 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.
11
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’interpolation2.
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
12 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
13 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
14 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.
15
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 xt 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 xt 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.
16 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.
17 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.
2.3.1 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.
18 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
2.3.2 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.
19
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.
IV. TORO : SLAM 3D
Cette partie présente TORO, l’algorithme de SLAM utilisé dans la suite. Après un rapide descriptif des prin
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
é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 temp jaunes sont les contraintes dues à l’odométrie ou à la vision.
Figure 4 : schématisation de principe de base exploité dans TORO instants sont transformées pour créer des c
: 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
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
Position du robot (triangles bleus) et des amers (étoiles) au cours du temp jaunes sont les contraintes dues à l’odométrie ou à la vision.
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 Cette partie présente TORO, l’algorithme de SLAM utilisé dans la suite. Après cipes utilisés dans TORO, le formalisme est présenté en
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 positions des amers dans son champ d'observation ont évoluées relativement à lui. Il créer donc une contrainte entre deux poses
Position du robot (triangles bleus) et des amers (étoiles) au cours du temps. Les flèches
: les contraintes visuelles à différents ontraintes entre des poses non successives (flèches jaunes
21 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);
22 δjila contrainte entre le nœud i et j, qui correspond à l'arrête ij ;
Ωji la matrice modélisant les incertitudes sur les contraintes;
fji (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 eji, par : 678 () = 978 () – ;78
On peut aussi définir le résidu rij par :
*78 () = −678 ().
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 fij
4 par :
<78 () ∝ (978 () − ;78 ), >78 (978 () − ;78 )
= 678 () >78 678 ()
= *78 () >78 *78 ().
En supposant que les observations sont indépendantes5, 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
23
< () = ? <78 ()
@A,BCDE
= ? *78 (), >78 *78 ()
@A,BCDE
Finalement l'objectif est de trouver la configuration x* qui maximise la probabilité des observations, ce qui s'écrit :
∗ = )*'8" < ()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 :
HIJ = H + K ・LMJNBA,>BA*BA
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
problème est que pour l'optimisation d'une contrainte entre les nœuds i et k, 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.
de décomposer le problème en plusieurs sous p
correspondent soit à des contraintes de l'arbre soit des contraintes additionnelles (fermeture de boucle).
Figure
Chaque équation du SGD réso
algorithme on traite les rotations et les translations séparément.
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
terme :
Pour ce qui est de la translation la solution très intuitive consiste simplement à distribuer
Figure 6: distribution des erreurs en rot
problème est que pour l'optimisation d'une contrainte entre les nœuds i et k, faudra mettre à jour tous les nœuds j pour j=i à k.
une autre méthode exploitant au mieux la topologie du graphe a utilisant un arbre pour la représentation des données.
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
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.
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
O = K ・LMJNBA,>BA*BA
Pour ce qui est de la translation la solution très intuitive consiste distribuer le déplacement le long de la trajectoire.
distribution des erreurs en rotation et translation le long de la trajectoire
24 problème est que pour l'optimisation d'une contrainte entre les nœuds i et k, il
une autre méthode exploitant au mieux la topologie du graphe a utilisant un arbre pour la représentation des données. Ceci permet roblèmes plus simples qui correspondent soit à des contraintes de l'arbre soit des contraintes additionnelles
: passage du graphe à un arbre pour la phase d'optimisation
problèmes. Dans cet Si l'on considère les rotations, elles induisent des non linéarités importantes dans le problème. Le SGD trajectoire. Ceci se fait au travers du
Pour ce qui est de la translation la solution très intuitive consiste
ation et translation le long de la trajectoire
25
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
26 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:
) + PQ + + R = 0 Pour a, b, c et d choisis.
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 ) + PQ + 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 =1
" ? 8
W
AXJ
Où pi est la position d’un point.
Ensuite on calcule la matrice :
Y = ZГW(, ) ГW(, Q) ГW(, ) ГW(Q, ) ГW(Q, Q) ГW(Q, ) ГW(, ) ГW(, Q) ГW(, )\ Où ГW(), P) = ∑ () − WA ])(P − ^) .
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
27 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).
28
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.
1.1. 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.
29 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
30 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.
31 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
32 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.
33
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
34 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
35 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 OpenMP7 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.
