Recaler un relevé LiDAR sous terre, sans lumière

Lidark est une application iOS qui relève des galeries souterraines avec le capteur LiDAR de l'iPhone : on marche, on balaie les parois, et l'application construit le plan au fur et à mesure. Une fonctionnalité paraissait acquise : reprendre plus tard un relevé commencé, et le prolonger. Le premier retour venu du terrain disait exactement l'inverse : la reprise ne se recalait jamais.

Voici comment j'ai remplacé la relocalisation d'ARKit par un recalage géométrique maison, et surtout comment l'algorithme apprend à dire « je ne sais pas ».

Pourquoi ARKit se perd à la torche

ARKit sait sauvegarder une carte de l'environnement (ARWorldMap) et s'y relocaliser plus tard. Mais il le fait en comparant des images de caméra. En surface, un mur éclairé par le jour ressemble à lui-même d'une visite à l'autre. Sous terre, la seule lumière est une torche qui bouge avec la main : le même mur ne ressemble jamais deux fois à lui-même.

Les données remontées de l'appareil le montraient sans ambiguïté. La photo « visez ici » enregistrée avec le relevé était une image noire de 4 Ko. La carte ARKit sauvegardée était 5 à 8 fois plus petite que d'habitude : ARKit n'avait presque rien trouvé à mémoriser.

Un premier correctif a traité un effet aggravant. Une reprise ratée sauvegardait sa carte provisoire par-dessus la bonne, si bien que chaque échec rendait le suivant plus probable. Depuis, la carte n'est enregistrée que si la session est réellement ancrée au relevé. Cela a arrêté la dégradation, pas l'échec de départ.

La vraie solution était ailleurs : la géométrie mesurée par le LiDAR ne dépend pas de la lumière. Un carrefour, un pilier ou une niche ont la même forme à la torche qu'en plein jour. Il fallait donc recaler par la forme.

Réduire le problème

Les deux nuages de points, celui du relevé et celui du nouveau scan, sont déjà alignés sur la gravité par ARKit. Il ne reste que quatre degrés de liberté : une rotation autour de la verticale (le lacet) et une translation en trois dimensions. La hauteur se déduit à part, par la médiane des points du sol. Le cœur du problème se ramène donc à un recalage 2D, vu de dessus.

Chaque nuage passe d'abord dans une grille de voxels de 10 cm. On garde un point par voxel, placé au barycentre de ce qui y tombe, et non au centre de la case. Deux scans du même mur l'échantillonnent alors presque aux mêmes endroits, et c'est ce qui permet ensuite de converger au centimètre. Les points sont classés par leur normale : paroi quand elle est presque horizontale, sol quand elle pointe vers le haut ; les plafonds sont ignorés.

Étape 1 : un vote de Hough sur les parois

Un ICP démarré au hasard converge vers le minimum local le plus proche, c'est-à-dire n'importe où. Il faut d'abord une estimation grossière, et elle doit être globale.

Je teste 60 lacets, tous les 6 degrés. Pour chacun, les points de paroi sont projetés vue de dessus dans des cellules de 25 cm. Chaque couple (cellule du nouveau scan, cellule du relevé) vote alors pour la translation qui les superposerait. Là où les deux scans se recouvrent vraiment, des centaines de couples votent pour la même translation, et un pic apparaît dans l'accumulateur :

var accumulator = [UInt16](repeating: 0, count: width * height)
accumulator.withUnsafeMutableBufferPointer { votes in
    for s in sourceCells {
        for t in targetCells {
            let ix = Int(t.x - s.x - originX), iz = Int(t.z - s.z - originZ)
            votes[iz * width + ix] &+= 1
        }
    }
}
// La quantification répartit les votes entre cases voisines :
// une case est notée avec ses 4 voisines.
let score = (Float(center) + 0.5 * around) / Float(sourceCells.count)

Pour que le calcul reste de l'ordre de la fraction de seconde, le nouveau scan est réduit à 160 cellules et le relevé à 14 000 au plus. Je garde jusqu'à trois pics par lacet, espacés d'au moins 2 m. C'est délibéré : le long d'un couloir, les positions concurrentes partagent le même lacet et diffèrent seulement par un glissement. Après fusion des pics trop proches, il reste au plus six candidats.

Étape 2 : ICP, d'abord point-à-point, puis point-à-plan

Chaque candidat est ensuite affiné par ICP, avec un rayon de recherche qui se resserre : 50, 30, 15 puis 10 cm. La première passe est point-à-point, résolue en forme fermée, pour rapprocher les deux nuages. Les suivantes sont point-à-plan :

for radius: Float in [0.5, 0.3, 0.15, 0.1] {
    let index = SpatialIndex(points: target.walls, cell: radius)
    for _ in 0..<(radius <= 0.1 ? 25 : 6) {
        guard let step = icpStep(
            source: source.walls, target: target, index: index,
            yaw: yaw, translation: translation, radius: radius, pointToPlane: radius < 0.5
        ) else { break }
        // ... mise à jour de yaw et translation, arrêt si le pas devient négligeable
    }
    translation.y = heightOffset(source: source, target: target, yaw: yaw, translation: translation)
}

La distinction n'est pas un détail. Le point-à-point seul laissait un biais de 15 cm : deux échantillonnages d'un même mur plat ne tombent jamais sur les mêmes points, et l'algorithme tire la position dans une direction arbitraire pour les apparier. Le point-à-plan ne mesure que l'écart perpendiculaire à la paroi. Les murs plats peuvent alors glisser l'un sur l'autre sans rien fausser, et ce sont les angles et les murs sécants qui fixent la position. Un léger amortissement sur le système linéarisé empêche la solution de diverger quand un seul mur est visible.

Étape 3 : refuser plutôt que se tromper

C'est la partie qui compte le plus. Un recalage faux est pire qu'une absence de recalage : l'utilisateur prolonge son plan au mauvais endroit, sans le savoir, sous terre.

Le cas d'école est le couloir droit. Un tronçon de galerie rectiligne s'ajuste aussi bien un mètre plus loin, ou dix. Une pièce rectangulaire a le même défaut à 90 ou 180 degrés près. Le vote de Hough produit alors plusieurs candidats honorables, et c'est justement pour cela que les six sont affinés avant tout verdict : seul l'ajustement fin les départage.

Le critère d'ambiguïté est une marge : la proportion de points bien ajustés de la meilleure position, moins celle de la meilleure position concurrente distincte. Proche de zéro, le scan s'ajuste aussi bien ailleurs. Une position n'est acceptée que si les quatre conditions sont réunies :

• au moins 800 points de paroi dans le nouveau scan ;
• au moins 60 % de ces points à moins de 10 cm d'une paroi du relevé ;
• une erreur quadratique moyenne perpendiculaire aux parois de 6 cm au plus ;
• une marge d'au moins 0,12 sur la meilleure alternative.

Sinon, l'application ne recale pas et continue d'essayer toutes les 2,5 secondes. L'écran demande de balayer lentement une zone caractéristique déjà scannée, « un carrefour, un angle, un pilier », et précise que la lumière ne compte pas. Une barre indique le taux de correspondance atteint. Au bout de 25 secondes, l'utilisateur peut continuer sans recalage : un nouveau relevé s'ouvre et l'ancien reste intact.

Toute la séquence tient dans ce compromis : ARKit dispose de 8 secondes pour réussir à sa manière, puis la session redémarre dans un repère provisoire et le recalage par la forme prend le relais. En cas de succès, l'origine du monde ARKit est déplacée sur celle du relevé, et l'enregistrement reprend.

Ce que valent les chiffres

Les tests automatisés reposent sur une galerie synthétique de 24 m, avec une branche latérale, un pilier et une niche, et une vérité terrain connue (137 degrés de rotation, plus une translation). Ils vérifient quatre comportements : la reprise au niveau d'un carrefour retombe à moins de 3 cm ; le couloir droit est refusé faute de marge ; une pièce étrangère au relevé est refusée ; un fragment trop petit ne produit aucun verdict.

Les mesures faites pendant le développement donnent :

• 0,3 mm d'erreur sur une reprise synthétique au niveau d'un carrefour ;
• 0,1 degré et 3 cm sur de la géométrie réelle scannée à l'iPhone, déplacée par une transformation connue ;
• entre 0,06 et 0,5 s par tentative sur l'appareil.

Je tiens à la nuance : le deuxième chiffre vient de vrais scans, mais pas d'un second passage physique dans la galerie. Chaque tentative réelle est donc journalisée sur l'appareil (points de paroi, taux de correspondance, erreur, marge, décision) pour régler les seuils sur des cas de terrain, à mesure qu'ils arrivent.

Ce que j'en retiens

• Lire les données avant le code. Une photo noire de 4 Ko et une carte cinq fois trop petite disaient la cause plus vite que n'importe quelle session de débogage.
• Global d'abord, local ensuite. Un ICP ne vaut que par son point de départ ; le vote de Hough lui donne des départs plausibles, pas une réponse.
• Choisir la bonne distance. Passer du point-à-point au point-à-plan a supprimé un biais de 15 cm sans toucher au reste.
• Un algorithme de recalage doit savoir refuser. La marge sur la meilleure alternative coûte une ligne de code et évite les erreurs silencieuses, les seules vraiment dangereuses.

Un projet similaire, avec des capteurs, de la vision ou du traitement de nuages de points sur mobile ? Parlons-en.