Partie 1: Introduction & detection de features
Certaines personnes racontent qu’ils ont un mauvais sens de l’orientation. Ils peuvent se comparer à un robot et se sentir bien meilleurs.
Ça fait un petit moment que je n’ai rien publié ici ! Je n’ai pas été inactif pour autant, rassurez-vous.
Je travaille depuis septembre 2020 sur la réalisation de mon propre système SLAM, un classique de la robotique. Ce projet a commencé avec une longue phase de recherche, documentation & planing, puis plus récemment par les premières implémentations, suivies des premiers résultats.
Dans cette série d’articles, je vais remonter un peu le temps et décrire le fonctionnement précis de mon approche.
Pour celles et ceux qui sont déjà familiers avec le concept du SLAM, et en particulier la détection de features, les descripteurs et l’association des features en question, sautez directement à la Partie 2.
Qu’est-ce qu’un SLAM
Simultaneous Localization And Mapping (ou Cartographie Et Localisation Simultanée en bon Francois) concerne la capacité d’un système à cartographier et se repérer dans un environnement inconnu. La plupart du temps, on pense à des systèmes robotiques en environnement inconnu, mais de très nombreuses applications sont également concernées, comme la numérisation d’objets par exemple (voir: structure from motion).
Une fermeture de boucle ? (cliquer pour étendre)
Imaginez sortir de chez vous et faire un tour du quartier, avant de revenir chez vous. Comment savez-vous que vous êtes revenus chez vous ?
Regardez une carte postale d’un endroit connu. Qu’est-ce qui vous permet visuellement de savoir que c’est un endroit connu ? Pourriez vous le reconnaitre s’il était par exemple sous la neige, ou en noir et blanc ? Il y a de bonnes chances pour que ce soit le cas.
Ce problème de reconnaitre un endroit visité précédemment est celui qu’on appelle le problème de la fermeture de boucles (loop closing). Notre système SLAM va accumuler les erreurs de positions, qui même si elles sont minimes, finiront par créer une erreur de position globale importante. La fermeture de boucle permet de supprimer cette erreur, en reconnaissant une zone déjà observée dans le passé (avec une erreur inférieure donc) et d’ajuster notre position et celle de notre environnement en fonction de la position connue. On réduit ainsi l’erreur accumulée pour revenir à une pose et une map plus proche du réel.
Le problème du SLAM n’est pas encore un problème fermé, de nombreuses limitations existent encore. On citera notamment la localisation longue durée (life long mapping), les fermetures de boucle en temps réel et la gestion d’environnements dynamiques, qui sont les problèmes majeurs qui n’ont pas encore de solution arrêtée.
On va ici se limiter aux SLAM visuels, basés sur les images issues d’une ou deux caméras.
On peut diviser les approches de SLAM en deux catégories majeures :
- Dense : Basé sur le traitement de l’ensemble des pixels de l’image. Toutes les informations sont exploitées ! Le traitement associé est en général lourd et non favorable aux utilisations temps réel.
- Sparse : Basé sur des features (point, lignes, plans, …), en général plus rapide.
J’ai décidé de m’orienter vers un SLAM sparse, basé sur des points, lignes, plans et formes superquadriques. Je garde un objectif temps réel (> 30 FPS), et j’utiliserai les images issues d’une caméra RGB-D.
Mon système se basera sur l’extraction de features de chaque image, suivie d’une phase de matching des features d’une image sur les features stockées dans une map locale, puis une optimisation de pose. La fermeture de boucles et le mapping en mémoire sera réalisé ultérieurement.
Plusieurs itérations auront lieux :
- V0:
- Basée sur des points, lignes, plans et cylindres dans un environnement supposé statique,
- Pas de fermeture de boucle
- V1:
- Utilisation des superquadriques (en fonction des résultats)
- Multithreading
- V2:
- Fermeture de boucles
- Environnement dynamique
Au début octobre 2021, j’achève la réalisation des fonctions de base de la V0 (détection de feature, optimisation de pose).
Première étape : détection des features
La détection de points caractéristiques est, à ma connaissance, un des plus vieux sujets de computer vision. En conséquence, c’est un domaine très développé, avec de nombreuses méthodes, implémentations et documentation. Une large palette de choix donc !
Détection
Tout d’abord, même si le problème de détection de points caractéristiques est “maitrisé”, le processus reste assez lent pour être un facteur limitant pour le temps réel.
On peut considérer une approche plus efficace que simplement re détecter les points à chaque image : une fois que les points ont été détectés pour la première image, on va utiliser une méthode dite de optical flow. Cette méthode est plus rapide et permet d’obtenir directement les matchs de features entre les images en se basant sur le mouvement associé des features d’une image a l’autre.
// TODO IMAGE OPTICAL FLOW
Afin de ne pas perdre tous les points quand on se déplace trop, une détection de points est lancée sur la nouvelle image en masquant les zones autour des points existants (voir Keypoint detection in RGBD images based on an Efficient Viewpoint-Covariant Multiscale Representation (2016))
Dans notre SLAM, on va également chercher à détecter les features de haut niveau, comme des lignes, des plans et des cylindres. Pour les lignes, j’utiliserais LSD (Line Segment Detector). La détection des plans et cylindres sera effectuée par CAPE, présenté dans un article précédent (De CAPE et d’opes).
On pourra aussi détecter des lignes formées par deux plans, qui sont une feature avec une contrainte “forte” au moment de l’optimisation de pose. Certaines publications proposent d’utiliser des plans et lignes “supposées” (voir Point-Plane SLAM Using Supposed Planes for Indoor Environments (2019)), mais leurs résultats ne sont évalués qu’en environnement orthogonal, ou les angles entre deux plans proches de 90° seront supposés perpendiculaires. Cette hypothèse fonctionne idéalement dans le cas d’un environnement orthogonal (intérieur d’un bâtiment par exemple), mais ses résultats sont réduits dans les environnements non orthogonaux (la majorité de la planète !).
Descripteurs
Afin d’associer un point a un autre, il nous faut être capables de comparer les différents points et chiffrer leur proximité. On utilise pour cela des descripteurs, qui représentent un point et son espace proche. Ce descripteur permet de comparer les points de façon consistante.
Il existe beaucoup de types de descriptors, avec différentes caractéristiques, vitesse de calcul et comparaison, … Ce qui m’intéresse ici est un descripteur rapide à calculer, et rapide à comparer. Ses caractéristiques d’invariance en rotation et en échelle ne sont pas vraiment les caractéristiques principales dans notre cas : avec nos 30 images par secondes, nos descripteurs seront mis à jour régulièrement et leur précision sont donc secondaires.
Beaucoup de ces descripteurs ne se basent que sur l’information 2D du point et ses alentours, là ou on pourrait utiliser également la mesure de profondeur donnée par notre caméra. Pour quelques exemples de descripteurs de points un peu plus avancés, jetez un œil a :
- Binary RGBD descriptor based on a pattern projection
- LIFT: Learned Invariant Feature Transform (2016)
- MatchNet: Unifying Feature and Metric Learning for Patch-Based Matching (2015)
- Local Features for RGB-D image matching under Viewpoint Changes - Chapitre 4 (2016): proposition d’un descripteur pour features RGBD
- Keypoint Detection in RGBD Images Based on Efficient Viewpoint-Covariant Multiscale Representation (2016) qui décrit une procédure de lissage d’images pour améliorer la qualité des features.
Bien sûr dans notre cas, nous avons d’autre features que des points (pour le moment, des plans et lignes). Il existe des descripteurs pour les lignes et les plans, mais ce domaine est moins développé que celui des descripteurs de points. Dans notre cas temps réel, utiliser des descripteurs de plans et lignes n’est pas nécessaire : on peut être assez confident en affirmant qu’une feature détectée dans une image reviendra au moins en partie dans la prochaine image. L’association peut donc se faire de façon fiable en comparant les caractéristiques “fortes” de nos features : Pour les plans, on peut par exemple comparer les vecteurs normaux à la surface du plan, calculer l’IoU (Inter Over Union) de deux surfaces, … Pour les lignes, on se limitera à la comparaison des points de départ/arrivé, l’orientation, le placement dans l’image, …
Et pourtant même si cette association est fonctionnelle, il manque tout de même un descripteur à notre objet, afin de le retrouver au moment d’une fermeture de boucle. Dans ce cas, les normales ne seront surement pas les mêmes, l’IoU ne fait plus vraiment de sens, les points de départ et d’arrivé non plus. Nous utiliserons dont tout de même des descripteurs pour ces features “haut niveau”.
Pour les plans, une technique remarquablement efficace utilises un autoencodeur pour créer un descripteur de plan très robuste (voir PlaneMatch: Patch Coplanarity Prediction for Robust RGB-D Reconstruction (2018)). Pour les lignes, il existe des méthodes communes (comme LSD Line Segment Descriptor, a ne pas confondre avec LSD Line Segment Detector), mais je pense m’orienter également vers une approche basée sur le machine learning. Le ML a démontré depuis longtemps son efficacité contre les features “hand crafted”.
Deuxième étape : Association des features
On attaque la partie intéressante. Il nous faut maintenant associer les features détectées dans nos images. Cette étape est très importante, car la moindre erreur provoquera au mieux des ralentissements dans l’optimisation de pose, et au pire faussera le résultat.
On a déjà mentionné dans la section précédente plusieurs manières d’associer les features de “haut niveau”, comme les lignes et plans. Malgré leur simplicité, ces méthodes sont fiables, parce qu’on retrouve généralement très peu de features haut niveau dans les images. Il y a moins de plans qu’il n’y a de lignes, et de lignes qu’il n’y a de points.
Pour les points, j’utilise RANSAC, déjà mentionné dans un post différent (Monocular Depth Map). Utiliser cette méthode pour l’association de points est rapide et efficace, mais se base sur les descripteurs calculés plus tôt. Ces descripteurs ne sont pas toujours fiables, et j’ai donc ajouté une zone maximum de détection autour de chaque point à matcher, pour exclure les points les plus éloignés. Cette approximation ne marchera qu’en application temps réel, ou les points se déplacent peu entre les images.
Environnement dynamique
Avant de passer à la suite, nous devons mentionner un point important : Les environnements dynamiques. Jusque ici, on a supposé que notre environnement était statique (ie chaque feature est invariante d’une image sur l’autre). Cette supposition n’est pas vraie dans 99% des cas réels.
Pour estimer notre position, nous nous baserons sur une optimisation rigide, qui sera perturbée par des features mobiles. Éliminer ces features mobiles n’est pas facile en temps réel, car leur mouvement est en général assez lent par rapport au rafraichissement d’image.
Des approches se basent sur la détection d’objets dynamiques connus (humain, voitures, animaux, …) pour les exclure de l’optimisation (voir Toward Real-time Semantic RGBD-SLAM in Dynamic environments (2021)). La limite de cette approche est évidemment que tous les objets mobiles ne sont pas dans la base d’entrainement. Les chercheurs ajoutent un critère de détection que je trouve particulièrement intéressant : En supposant qu’un élément dynamique à toujours une surface, ils segmentent l’image en zone de détections et déterminent au fur et mesure du temps quels sont les zones contenant des objets dynamiques. Ces zones sont ensuite éliminées totalement de l’optimisation de pose et du mapping.
Je suis encore trop “neuf” dans ce projet pour essayer autre chose que de supposer que l’environnement est statique, ce traitement sera réalisé pour la V1.
Rendez-vous la Partie 2 pour discuter optimisation de pose !