Pourquoi ICP échoue : initialisation, valeurs aberrantes et géométrie symétrique

L'algorithme ICP alterne entre la correspondance des plus proches voisins et l'estimation par transformation rigide. Un résidu plus faible ne garantit pas l'exactitude de la pose reconstruite. Des formes 2D simples permettent d'isoler l'initialisation, les valeurs aberrantes et la symétrie, indépendamment du bruit du capteur.

Objectif

Cette implémentation minimise les carrés des distances point à point. À chaque itération, elle recherche les voisins cibles des points sources transformés, conserve les correspondances à une distance inférieure à un seuil et estime la rotation/translation par SVD. Une correction du déterminant empêche la réflexion. L'algorithme s'arrête après 100 itérations au maximum. Le tutoriel Open3D ICP distingue les objectifs point à point et point à plan ; seul le premier est implémenté ici.

Entrées et exécution

Téléchargez le script et exécutez-le. Ce test a été réalisé avec Python 3.12.3, NumPy 1.26.4 et Matplotlib 3.6.3. Open3D n'est pas requis et cette comparaison de performances n'est pas effectuée avec son implémentation.

La cible est un L composé de segments de 3 m et 2 m, soit 102 points au total, avec un coin dupliqué. Les points sources sont générés par l'inverse d'une rotation réelle de 20° et d'une translation de (0,4 ; −0,3) m. Aucun bruit de capteur n'est ajouté. L'initialisation correcte est de 18° et (0,35 ; −0,25) m ; la mauvaise est de 110° et (1,5 ; 1) m.

Lire trois types de données

Le résidu correspond à l'erreur quadratique moyenne (RMSE) calculée sur les distances aux plus proches voisins acceptées. La couverture représente la fraction de tous les points sources situés à l'intérieur du seuil. La vérité terrain synthétique autorise également les erreurs de rotation et de translation.

Diagram 1 · Use the button to switch views

Même une bonne initialisation laisse une erreur : les points voisins sur les segments régulièrement échantillonnés permettent une solution locale décalée. Une couverture de 1 000 à elle seule ne suffirait pas. Les valeurs aberrantes se situent à la source, x = 4–7 m, y = 3 m. Téléchargez la cible, les entrées/sorties des valeurs aberrantes et le résumé.

Directions inobservables dans les cercles et les murs

Le cercle a un rayon de 1 m et 120 points, l’identité étant définie comme la transformation réelle. Son ensemble de points correspond également après une rotation de 90°, de sorte qu’un résidu pratiquement nul coexiste avec une erreur de pose de 90°. Des itérations supplémentaires ne peuvent pas fournir les informations manquantes.

Un mur pose un problème similaire, mais différent. L'erreur point-plan mesure le déplacement le long de la normale au mur et contraint mal le mouvement tangentiel. Les murs finis point à point peuvent tirer des informations des extrémités et de l'emplacement des points, mais cette méthode s'avère peu efficace dans un long couloir sans extrémités visibles. Le cercle représente une expérience réalisée ; la discussion sur le mur explique les contraintes directionnelles de l'objectif.

Un seuil plus bas n'est pas une solution universelle

Le seuil de 0,5 m élimine ces valeurs aberrantes, mais peut également supprimer des correspondances correctes lorsque l'initialisation est éloignée. Différents seuils sélectionnent différents points, de sorte que les résidus seuls ne sont pas directement comparables. Ce code cesse de se mettre à jour avec moins de trois points acceptés ; trois points ne garantissent pas à eux seuls une bonne géométrie.

Les options pratiques incluent l'initialisation de l'odométrie, l'alignement grossier-fin, le filtrage des valeurs aberrantes et l'observation de surfaces avec différentes orientations. Mesurez leurs effets sur les mêmes intervalles et dans les mêmes conditions de référence.

Poursuivre avec l'évaluation SLAM

Il s'agit d'un recalage 2D sur une seule image, et non d'un test de précision de conduite 3D ou de robustesse face aux objets en mouvement. L'évaluation SLAM (/fr/blog/posts/slam-evaluation.html) ajoute les erreurs de trajectoire, l'alignement temporel et les intervalles ayant échoué. Commencez par modifier uniquement l'angle initial pour comprendre pourquoi la convergence ne peut être résumée par une seule valeur résiduelle.

Exécuter le même calcul localement

Extrayez le fichier ZIP contenant le code source complet et exécutable (/uploads/labs/playground/lab-sources.zip) ou enregistrez le point d'entrée de l'expérience (/uploads/labs/playground/experiment.py), les noyaux générés (/uploads/labs/playground/kernels.py) et le code source PID original (/uploads/labs/pid_lab.py) dans un même dossier. PID utilise la bibliothèque standard Python ; Kalman et ICP nécessitent NumPy. Les commandes de génération de figures originales restent disponibles.

python3 -m pip install numpy==1.26.4
python3 experiment.py icp --parameters '{"case":"good_init","threshold":0.5}' > result.json

Essayez bad_init, outliers et symmetric_ring. Comparez les seuils 10 et 0,5 pour la détection des valeurs aberrantes. Vérifiez que la bague conserve une erreur de rotation de 90 degrés même avec un résidu quasi nul.

python3 reproduce.py --experiment icp