CAPE (Cylinder And Plane Extraction) est une méthode extrêmement efficace d’extraction de plans et cylindres dans des images RGB-D, basée sur une méthode AHC. Malgré l’efficacité de cette méthode, l’implémentation C++ du papier présente de nombreux défauts.
Dans ce post, je vais expliquer comment j’ai réparé la plupart de ces problèmes.
CAPE est une méthode efficace, fiable et rapide (< 4ms pour une image de 640 * 480 pixels), dont l’idée est basée sur PEAC. CAPE améliore la méthode PEAC en accélérant la détection de plans, et en ajoutant une détection de cylindres par un algorithme RANSAC. Si la méthode présentée dans le papier est très impressionnante, son implémentation présente de nombreux défauts, notamment au niveau de la gestion de mémoire.
Petite présentation de l’AHC
AHC (Agglomerative hierarchical clustering) est une méthode introduite par PEAC en 2014, qui consiste à analyser les images RGB-D en les découpant en une grille de NxM rectangles. Les pixels dans ces rectangles sont ensuite analysés individuellement pour déterminer s’ils font partie d’un plan 3D, et si c’est le cas, utiliser la position relative de ces rectangles dans l’image pour fusionner les plans proches et construire un graph de l’ensemble des plans.
Cette méthode présente l’avantage de traiter une image 3D comme une image 2D, et est donc bien plus rapide qu’une analyse d’un nuage de points. Elle est cependant limitée en l’état à l’analyse d’images RGB-D.
Lors de sa publication, PEAC était la méthode de détection de plan 3D la plus rapide (de loin), et présentant des performances équivalentes a l’état de l’art des algorithmes de détection de plans 3D.
Corriger CAPE
L’implémentation de CAPE, fournie par les auteurs, propose des performances impressionnantes de rapidité et fiabilité. Elle est capable de gérer l’étape de détection de plans en moins de 4 millisecondes, ce qui est plus que suffisant pour du temps réel.
Cependant, le code est mal organisé et présente de nombreux défauts d’implémentation, notamment pour la gestion de mémoire et les copies de classes dans les vector.
Corriger ces erreurs améliore légèrement les performances de cette méthode.
Je ne vais pas préciser ici la correction des erreurs classiques comme l’absence des ifndef dans les .hpp, ou l’absence des const et const& pour les paramètres en entrée des fonctions et classe.
Classe CAPE
Allocations inutiles
Dans le constructeur, le vector Grid, contenant une liste de N pointeurs vers une classe (PlaneSegment*), est initialisé par une boucle qui le rempli de nullptr (utiliser la méthode reserve aurait été plus adapté).
Les pointeurs de Grid sont ensuite réaffectés dans la fonction process, appelée pour chaque nouvelle image RGBD, via la ligne Grid[id] = new PlaneSeg(....);.
Utiliser new pour remplacer un nullptr est ok, mais au deuxième appel a la fonction process, on va surtout allouer une nouvelle zone mémoire sans désallouer l’ancienne.
Une petite fuite de mémoire potentielle donc.
Mais une petite minute… Grid utilise la classe vector avec un nombre constant d’éléments, mais n’utilise jamais les propriétés de vector. Pourquoi utiliser un vector ?
Je l’ai donc remplacé par un tableau de unique_ptr initialisé via un new unique_ptr<PlaneSegment>[N], puis ajouté un constructeur basique a la classe PlaneSegment, appelée dans le constructeur, ainsi qu’une fonction init pour éviter N allocations à chaque appel de la fonction process.
J’ai également ajouté un destructeur à la classe CAPE pour libérer Grid.
Une fuite bouchée, une amélioration (minime) des performances en évitant N allocations par appel.
Une réinitialisation évitable
Dans la fonction process, appelée à chaque analyse d’image, une variable H de la classe Histogram est alloué, contenant deux vector avec chacun une taille constante de 20 éléments.
Le fait de le déclarer dans le bloc de la fonction évite les fuites mémoires, mais nécessite de rajouter 40 allocations par appel à la fonction process.
J’ai donc fait de H une variable membre de la classe CAPE, remplacé les vector de Histogram par des tableaux, puis ajouté à la fonction initHistogram un appel a fill_n pour initialiser les éléments de ces tableaux a 0.
Sans oublier bien sûr un petit Destructeur pour désallouer ces tableaux.
Une erreur d’index
La boucle for ligne 126 a pour but de chercher l’index de la cellule avec l’erreur minimum.
La valeur de minMSE est comparée à Grid[seed_candidate]->MSE, et si elle est supérieure, on lui affecte min_MSE = Grid[i]->MSE;.
J’ai donc corrigé la sélection d’index par min_MSE = Grid[seed_candidate]->MSE;.
J’ai également modifié toutes les classes pour que les membres soient private et ajouté des fonctions get.
Une copie en arrière-plan
PlaneSeg new_ps = *Grid[seed_id];.
À première vue, c’est assez innocent.
Ce qui se passe en arrière-plan est un peu moins Innocent: c’est un appel à un constructeur par copie.
Si on n’a pas de constructeur par copie déclaré dans notre classe, le compilateur va en créer un par défaut, dont le comportement n’est peut-être pas celui désiré.
Une bonne pratique pour éviter ce genre de copies par défaut est de déclarer le constructeur par défaut ainsi que l’opérateur = en privé dans notre classe.
Le compilateur nous préviendra des copies à l’avenir, avec une bonne erreur des familles.
Ajouter ces méthodes fait aussi sortir un autre message d’erreur que je n’avais pas vu venir : vector.push_back(class) appelle le constructeur par copie de la classe.
À mon avis, la meilleure manière de contourner ce comportement est d’utiliser des vecteurs de pointeurs, notamment de shared_ptr dans ce cas précis.
Une autre erreur d’index
Ligne 238, la distance au plan est calculée par le produit scalaire de la normale d’un plan par la valeur mean d’un autre plan.
Cependant, le premier terme plane_segments[r].normal[0] devrait être plane_segments[plane_id].normal[0].
Pour éviter les erreurs de copier-coller, j’ai remplacé les tableaux normal et mean de la classe PlaneSegment par des Eigen::Vector3d afin d’utiliser directement la fonction Vector3d.dot(Vector3d).
Une autre méthode pour continuer d’utiliser des tableaux de double aurait été de définir une fonction double dot(const double& vecA[3], const double& vecB[3]) pour éviter le code redondant, ou encore de définir une fonction membre getDistance dans la classe PlaneSegment, qui a l’avantage de ne pas utiliser de getter.
Boucles redondantes
La boucle ligne 434 à 445 sert à copier le résultat de l’analyse des cylindres dans le vector avec les cylindres finaux.
Son existence est justifiée pour une certaine clarté du code, mais une petite optimisation peut être réalisée en faisant la copie des résultats finaux dans la boucle précédente (vers la ligne 348).
De même, maintenant que nous avons remplacé Grid par un tableau fixe non réalloué à chaque appel de fonction, la boucle de 449 à 455 est également à éliminer.
Dans la fonction getConnectedComponents, une autre boucle peut être supprimée.
Il s’agit de la double boucle de la ligne 476 à 480, qui rend la matrice planes_association_matrix symétrique.
Il suffit de copier directement les valeurs symétriques au niveau des lignes 471 et 472.
Résultat des courses
J’ai vérifié mon travail à l’aide de valgrind, et j’ai pu confirmer le rebouchage des fuites mémoire, avec une amélioration négligeable (de l’ordre de la demi milliseconde) des performances sans incidence sur la sortie. La prochaine étape consistera à chercher des pistes d’amélioration de performances générales de l’algo.
J’ai profité de mes modifications pour ajouter des métriques d’évaluation, inspirée de PEAC, a la place de certaines constantes.