Guide PnP — Reconstituer la pose de l'appareil photo à partir de simples points 3D et d'une image

Supposons qu'une image prise par une caméra comporte plusieurs points 3D connus sur une carte, correspondant à des points de l'image. Le problème consistant à déterminer la position de la caméra et son orientation est un problème PnP (Perspective-n-Point). Il est couramment utilisé pour le suivi de cartes SLAM visuel, la superposition d'objets virtuels en réalité augmentée, l'étalonnage main-œil pour les robots et l'estimation de pose pour les caméras de topographie.

0. Résumé en 30 secondes

• Les entrées sont la matrice intrinsèque de la caméra K, les points 3D connus \mathbf X_i et leurs points image correspondants \mathbf u_i. La sortie est une rotation R et une translation t.

• Il minimise l'erreur de reprojection de l'équation de projection \mathbf u_i\sim K(R\mathbf X_i+t). Avec 3 points, P3P fournit des solutions candidates. Avec 4 points de contrôle ou plus, la redondance permet de détecter les valeurs aberrantes.

• EPnP exprime chaque point comme une combinaison linéaire de 4 points de contrôle virtuels, ce qui permet de résoudre rapidement un grand nombre de points. Une optimisation non linéaire finale, telle que l'algorithme de Levenberg-Marquardt, affine ensuite le résultat.

• Si des valeurs aberrantes sont intégrées aux correspondances, l'estimation de pose entière peut s'effondrer. Elle est donc vérifiée à l'aide de RANSAC-PnP, de contrôles de profondeur positive et de la cohérence d'une image à l'autre.

• La dégénérescence et la divergence sont fréquentes lorsque les points sont presque coplanaires, que la parallaxe est faible, que les paramètres intrinsèques sont incorrects ou qu'il y a un effet de rolling shutter ou des objets dynamiques dans la scène.

1. Le modèle de projection

Diagram 1 · Use the button to switch views

Figure 1 — Les identifiants correspondants définissent les correspondances 3D–2D. PnP forme une hypothèse de pose, rejette les valeurs aberrantes par résidu de reprojection et affine R,t, qui transforme les coordonnées du monde en coordonnées de la caméra.

Soit \mathbf X_c=R\mathbf X_w+t un point dans le repère de la caméra. Dans le modèle sténopé, les coordonnées normalisées de l'image sont :

et les coordonnées des pixels sont obtenues via la matrice intrinsèque :

comme \mathbf u\sim K\mathbf X_c . R\in SO(3) représente la rotation et t la translation. En cas de distorsion de l'objectif, une correction de distorsion est nécessaire avant et après la projection.

Les inconnues correspondent aux 6 degrés de liberté (3 rotationnels et 3 translationnels). Étant donné les correspondances n entre les points 3D \mathbf X_i et les observations \mathbf u_i, l'erreur de reprojection

est minimisée. \pi représente la division perspective et \rho une fonction de perte robuste, telle que celle de Huber.

Ne pas confondre la transformation avec la position de la caméra

OpenCV renvoie solvePnP pour la transformation qui projette les points de l'objet/du monde dans le repère de la caméra. Pour obtenir le centre de la caméra dans le repère du monde. Pour les coordonnées, utilisez \mathbf C_w=-R^Tt ; pour la pose de la caméra, inversez T_{cw} afin d'obtenir T_{wc}. Considérer tvec comme la position mondiale de la caméra est une erreur fréquente. Évitez également d'appliquer une distorsion deux fois si les points de l'image d'entrée ont déjà été corrigés.

2. P3P, AP3P et EPnP

P3P (Perspective-3-Point), qui calcule la distance par rapport au centre de la caméra à partir des angles d'image de 3 points et des distances entre les points 3D, admet jusqu'à 4 solutions. La solution correcte est sélectionnée en la comparant à un 4e point ou à la pose connue de la carte. AP3P est une variante rapide qui réorganise la solution algébriquement.

Lorsqu'il y a un grand nombre de points, EPnP (Efficient PnP) exprime chaque point 3D comme une somme pondérée de 4 points de contrôle virtuels.

Les coordonnées caméra des points de contrôle sont calculées à partir d'équations linéaires, et la rotation et la translation sont ensuite déduites. Son coût de calcul étant quasi linéaire par rapport au nombre de points, cette méthode est particulièrement adaptée à la construction d'une pose initiale à partir des nombreux points de repère SLAM. Après la solution initiale, l'erreur de reprojection est affinée itérativement par l'algorithme de Levenberg-Marquardt.

3. RANSAC-PnP

Les correspondances entre points caractéristiques sont souvent perturbées par des motifs similaires, des objets en mouvement et des identifiants de carte erronés. L'approche standard est RANSAC : construire une pose provisoire à partir d'un ensemble minimal de points, reprojeter chaque correspondance et compter le nombre de points correspondants (inliers) dont les valeurs se situent en dessous d'un seuil donné. Le nombre d'itérations nécessaires, compte tenu du taux d'anomalies, de la taille minimale de l'échantillon et de la probabilité de succès, est déterminé par :

Le nombre requis Le nombre d'itérations augmente fortement avec le taux de valeurs aberrantes. Il est conseillé de réduire préalablement \epsilon à l'aide d'un test de ratio de points caractéristiques, d'une dispersion basée sur une grille ou d'un masque d'objet dynamique. solvePnPRansac d'OpenCV est utilisé en spécifiant explicitement le nombre de points, les indicateurs (EPNP, P3P, SQPNP, etc.), le seuil de reprojection et le niveau de confiance.

4. Détection de la dégénérescence

Ensembles de points coplanaires

Si tous les points 3D se trouvent dans le même plan, la profondeur et la pose issues de PnP deviennent ambiguës, et la géométrie peut tout aussi bien être expliquée par une homographie. L'étalonnage en damier utilise délibérément un plan, mais il est nécessaire de choisir des points de vue et des dispositions de points qui contraignent suffisamment les degrés de liberté de la pose. L'instabilité en profondeur d'un marqueur AR unique, vu de face, illustre le même phénomène.

Couverture d'image étroite et longue portée

PnP utilise directement une image et des points 3D connus ; la parallaxe inter-images est donc… Ce n'est pas une entrée obligatoire en soi. Cependant, sa qualité est médiocre lorsque les correspondances n'occupent qu'une petite zone de l'image, que la cible est éloignée et apparaît petite, ou que les points 3D présentent peu de variations de profondeur. Ne vous fiez pas uniquement au nombre de points correspondants : examinez la couverture de l'image, l'erreur quadratique moyenne de reprojection et la covariance de la pose ou sa sensibilité aux perturbations, puis fusionnez les données d'une centrale inertielle ou d'un capteur de profondeur si nécessaire.

Calibrage et synchronisation

Les erreurs de focale, de point principal et de distorsion se traduisent par une erreur de reprojection systématique sur l'ensemble des points. Un objectif dont la matrice intrinsèque varie avec le zoom, la température ou la mise au point doit être recalibré. Dans les véhicules et les drones, si la synchronisation des lignes d'un obturateur roulant n'est pas synchronisée avec celle de la centrale inertielle, PnP renverra une pose de caméra incorrecte.

5. Le rôle de PnP dans le SLAM visuel

Dans le SLAM visuel, à mesure que le nombre de points de carte triangulés augmente, la pose de la caméra peut être suivie image par image grâce à PnP. La pose étant maintenue fixe, les nouveaux points sont… Après triangulation, et une fois un nombre suffisant d'images clés accumulées, l'ajustement de faisceau optimise conjointement la pose et la carte. On peut le comprendre comme une division du travail : PnP constitue l'interface légère, et l'ajustement de faisceau assure la cohérence globale.

6. Liste de vérification pour l'implémentation

1. Calibrer K et la distorsion à l'aide d'un damier ou d'un outil similaire, et enregistrer l'erreur de reprojection.

2. Aligner les unités (m/mm) et le repère des points 3D avec l'état de correction de distorsion des points de l'image.

3. Affiner les correspondances par un test de ratio, la recherche des plus proches voisins mutuels et le suivi temporel.

4. Supprimer les valeurs aberrantes avec RANSAC-PnP, et enregistrer la distribution des valeurs conformes et l'erreur de reprojection.

5. Vérifier que la profondeur est positive, que le changement de pose est physiquement plausible et que la différence avec l'image précédente est raisonnable.

6. En cas de conditions défavorables, utiliser une centrale inertielle (IMU), la profondeur, une homographie ou une réinitialisation.

Implémentation minimale Séquence

Avec OpenCV, commencez par obtenir rvec, tvec, inliers à partir de solvePnPRansac, transmettez uniquement les points correspondants à solvePnPRefineLM, et enfin utilisez projectPoints pour calculer vous-même l'erreur quadratique moyenne (RMSE) et la distribution des erreurs des points correspondants. Un retour API réussi ne permet pas, à lui seul, de détecter les discordances excessives, les points regroupés ou une pose physiquement impossible.

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist,
    flags=cv2.SOLVEPNP_EPNP,
    reprojectionError=3.0, confidence=0.999, iterationsCount=200,
)
if not ok or inliers is None or len(inliers) < 6:
    raise RuntimeError("PnP failed or has too few inliers")

idx = inliers.ravel()
rvec, tvec = cv2.solvePnPRefineLM(
    object_points[idx], image_points[idx], K, dist, rvec, tvec
)
projected, _ = cv2.projectPoints(object_points[idx], rvec, tvec, K, dist)
rmse = np.sqrt(np.mean(np.sum(
    (projected.reshape(-1, 2) - image_points[idx].reshape(-1, 2)) ** 2,
    axis=1,
)))
R, _ = cv2.Rodrigues(rvec)
camera_center_world = -R.T @ tvec.reshape(3, 1)

Ni 3.0 px ni six points correspondants ne constituent un seuil d'acceptation universel ; il ne s'agit que de valeurs initiales pour cet exemple. Déterminez les seuils en fonction de la résolution de l'image, de la précision des caractéristiques et de l'erreur de pose autorisée par l'application. Vérifiez également que les points correspondants ne sont pas regroupés dans un coin de l'image et que chaque point a une profondeur de Z_c>0 par rapport au repère de la caméra.

7. Résumé

PnP est l'interface qui convertit les correspondances entre une carte 3D et une image 2D en une pose de caméra à 6 degrés de liberté. Élaborez une solution initiale avec P3P/EPnP, éliminez les valeurs aberrantes avec RANSAC et affinez-la par optimisation non linéaire. Seule une gestion conjointe de la disposition des points, de l'étalonnage, de la parallaxe et de la synchronisation temporelle permet d'obtenir une estimation de pose stable pour le SLAM visuel ou la RA.

Références

• OpenCV — Étalonnage de la caméra et reconstruction 3D

• Lepetit et al., EPnP

• [Gao et al., Classification complète des solutions pour le Problème de perspective à trois points

• Hartley et Zisserman, Géométrie à vues multiples