FR 2960082 A1 20111118 FR 1053803 A 20100517 PROCEDE ET SYSTEME POUR FUSIONNER DES DONNEES ISSUES DE CAPTEURS D'IMAGES ET DE CAPTEURS DE MOUVEMENT OU DE POSITION L'objet de la présente invention concerne un système et un procédé permettant de fusionner des données issues de capteurs d'images et de capteurs de mouvement ou de position pour une localisation robuste, c'est-à-dire peu sensible aux perturbations extérieures (mouvement brusques, éclairage, etc., et précise d'un système visuel, d'une caméra, etc. 1 o Elle s'applique, par exemple, pour la localisation et la modélisation en 3 dimensions ou 3D d'un environnement. Elle permet aussi le calcul d'une trajectoire en 3D en temps réel, et peut ainsi être utilisée afin de mettre en oeuvre des techniques de « Réalité augmentée ». L'invention s'applique pour des systèmes comprenant une caméra 15 embarquée ou non (par exemple les caméras HD dans le cinéma) et un capteur de mouvement (inertial) ou tout autre capteur de position (GPS pour Global Position System, odomètres, etc.) De manière plus générale, elle s'applique pour des techniques de vision par ordinateur, le domaine de la réalité augmentée, la reconstruction 20 3D et la modélisation, robotique, etc. Définitions Le mot pose intègre la notion de position et d'orientation d'une caméra ou d'un dispositif composé d'une caméra et d'un module de mesure de mouvement. La notion de fonction de coût dans le domaine de 25 l'ajustement de faisceaux est définie selon l'art antérieur comme étant associée à l'algorithme d'optimisation qui calcule l'erreur de projection (minimisation au sens des moindres carrés de la distance entre les observations 2D (mesures sur les images) et les reprojections théoriques. Le mot scène désigne un ensemble de primitives caractérisées 30 notamment par leurs positions en 3 dimensions (par exemple, des points 3D, des segments 3D, etc.). Ces éléments ou primitives forment la reconstruction de l'environnement réel, filmé par la ou les caméras. En résumé, le mot "scène" correspond à un environnement reconstruit. Le terme « dispositif » sera utilisé dans la description pour désigner l'ensemble formé par une caméra intégrant un module de mesure de mouvement ou un capteur d'images et un capteur de mouvement ou de position. Le mot « système » désigne l'ensemble qui comprend en plus du dispositif prédéfini, des moyens de traitement des informations ou des données, ainsi que des moyens de communication. 1 o Le système calcule la pose du dispositif (qui est en mouvement). On ne calcule pas la pose du système car si l'unité de traitement n'est pas sur un même bâti que le dispositif, alors les poses sont différentes. En fait dans le procédé selon l'invention, on détermine la pose des éléments mobiles du système. 15 Chaque capteur observe le monde (la scène) et donne une mesure de son propre déplacement (et non celui du dispositif). Donc pour obtenir le mouvement du dispositif, on applique un simple changement de repère, connu de l'Homme du métier, (modélisé aussi dans la fonction Hk définie plus loin dans la description) 20 Le problème posé dans la présente invention est notamment de répondre au problème de la fusion de données image et inertielle dans le but de déterminer une trajectoire d'un objet ou de reconstruire un environnement en 3D sans a priori sur les incertitudes liées aux différents capteurs, ou 25 encore de manière plus générale de localiser un dispositif. En effet, il n'est pas toujours possible d'avoir les caractéristiques du bruit (image, inertiel, GPS) venant brouiller les images ou les informations délivrées par les capteurs bruit qui, de plus, peuvent varier dans le temps. La majorité des techniques de localisation qui intègrent des données vision 30 et inertielles sont basées sur une méthode d'optimisation par Filtre de Kalman Etendu, voire par un filtre à particules connus de l'Homme du métier. Ces méthodes estiment, la pose d'un système à l'aide des algorithmes de vision, et fusionnent ensuite cette mesure "vision" par un filtrage de Kalman étendu. Pour fusionner ces données hétérogènes, il leur est nécessaire d'introduire un terme de covariance du bruit sur les différentes mesures. Ce terme, très important, est souvent difficile à estimer et est généralement fixé a priori. La précision de la localisation dépend fortement de ce paramètre. Un autre problème de l'optimisation par Kalman est son inefficacité à gérer une importante masse de données de la scène. Ces techniques optimisent deux principaux éléments qui sont l'état du système et la carte de l'environnement (la scène). L'état du système dépend du modèle de mouvement choisi pour modéliser la dynamique du système. On y retrouve généralement la pose du système (le mot pose intégrant la notion de position et d'orientation), exprimé dans un repère global, ainsi qu'éventuellement les vitesses, voire les accélérations. La carte de l'environnement est constituée d'éléments localisés, par exemple, des points de la scène filmée. Dans les conditions du temps-réel, les méthodes de localisation et modélisation d'environnement en temps réel plus connues sous l'abréviation anglo-saxonne « SLAM » (Simultaneous Localization and Mapping) basées sur Kalman doivent limiter le nombre d'éléments de la carte (quelques dizaines). Cette réduction a pour effet de diminuer fortement la précision de la localisation finale. De plus, il a été montré dans l'art antérieur que le filtre de Kalman Etendu est une approximation de l'optimisation de Gauss-Newton (première itération). En conclusion, malgré les avantages offerts par ces méthodes, elles présentent toutefois les inconvénients suivants. La localisation par vision seule est peu robuste (problème des mouvements rapides, des mouvements critiques comme les rotations pures, des occultations temporaires de la caméra, etc.). De plus, la caméra peut se décalibrer (chocs, changements de température,..). La localisation par intégration des données inertielles ne permet pas une localisation précise à moyen et long terme: problèmes de dérive, de biais de capteurs, de bruit, etc. Il est donc nécessaire d'utiliser plusieurs capteurs pour localiser de manière précise un système. t = instant donné w: Indice d'un capteur quelconque soit une caméra et donc w=c, soit un capteur de mouvement w=k c : indice désignant un capteur caméra (ou l'indice du capteur image lorsqu'il y a plusieurs capteurs) xt vecteur contenant l'estimation de la scène (ou une partie) et des poses de la caméra (ou une partie) à l'instant t zr vecteur d'observations lié à une mesure effectuée par le capteur w à l'instant t s : la scène (l'environnement reconstruit) p : une pose d'une caméra, celle-ci contient la position T et l'orientation R Le système et le procédé selon l'invention reposent sur une nouvelle approche qui met en oeuvre une pondération automatique entre les données de déplacement et/ou de position avec les données liées aux images par l'intermédiaire d'ajustement de faisceaux. L'invention concerne un procédé pour fusionner des données issues d'un premier capteur images et d'un module de mesure de mouvement et/ou de position, ledit premier capteur et ledit module de mesure de mouvement et/ou de position étant solidaire et formant un dispositif, le procédé étant exécuté par un module de traitement afin de localiser ledit dispositif caractérisé en ce qu'il comporte au moins les étapes suivantes : - Une première étape consistant à acquérir plusieurs points sur les images pour définir une pose pr du dispositif (caméra image et module de mesure mouvement et/ou position) pour un instant donné t - Une étape de prédiction de la pose pr du dispositif (caméra et capteur mouvement/position) pour un même instant donné t et en utilisant les mesures du module de mesure de mouvement et/ou de position 5 - Une étape où l'on détermine une contrainte Ek associée pour le dispositif (caméra et capteur) k, - Une étape où l'on détermine automatiquement un coefficient de pondération Àt de ladite contrainte, - Une étape d'estimation de la pose courante pt dudit dispositif, - Une étape d'optimisation simultanée par la technique d'ajustement des faisceaux en utilisant la fonction de coût suivante : r-- 1 t EBA = (X t, Z) _ EE, (L[~t'S 1~ j+ E Ep (Lpt's ], Zt t'=t-(N-1) avec Ep~xt,Zt)= cc ~xtzt )+%,t sk «xt,zt o : erreurs de reprojection de la scène dans les images o Xt : = [psi la pose du dispositif et la scène o Zt : = les observations de la scène détectées dans l'image de la caméra c à l'instant t o z= z z' ensemble de toutes les mesures pour tous les capteurs caméra c et des capteurs k ou module de mouvement et/ou de position à un instant t Le procédé comporte, par exemple, une étape d'initialisation des bases de données des poses clés et des éléments de scène (3D) en reconstruisant l'environnement du capteur images en utilisant un nombre donné de premières poses et en exécutant une triangulation des primitives 3D à partir des éléments d'intérêts les constituant pour les poses choisies. Selon un mode de réalisation, l'étape de sélection automatique du coefficient de pondération utilise le résultat d'une étape de raffinement de la pose du dispositif en partant d'une première estimée de pose du dispositif délivrée par le module d'estimation de la pose par vision et en tenant compte des éléments de scène précédemment estimés et en ajoutant au terme de coût, l'erreur de la prédiction (contrainte) délivrée par le module de sélection automatique de la pondération et pondérée par un facteur lambda ou appris dans le module de sélection automatique de pondération, à la fin de l'étape de raffinement, la pose courante du système est ajoutée à la base de 1 o données (historique) des poses. L'étape de sélection automatique de la pondération comporte, par exemple, une étape de raffinement de la pose du dispositif, dans le module de raffinement, la fonction de coût est uniquement composée des erreurs de reprojections des primitives 3D de la scène sur la caméra dont la pose est 15 actuellement raffinée suivie d'une étape de construction de la contrainte, puis de l'étape de sélection automatique de la pondération, à la fin de l'étape de raffinement, la pose courante du système est ajoutée à la base de données (historique) des poses. Le procédé peut aussi comporter une étape de sélection d'images 20 clés comprenant les étapes suivantes : sélectionner une première image 11 comme une image clé, puis une seconde image 12 choisie aussi éloignée que possible de 11 dans la vidéo et possédant au moins M points de correspondance d'intérêt avec 11, puis choisir une troisième image 13, la plus éloignée de 12 de façon telle que il y a au moins M points d'intérêts 25 correspondant entre 13 et 12 et au moins M' points d'intérêts correspondant entre 13 et 11, et ainsi de suite pour les images ln suivantes, procéder de même en vérifiant que les images ln-2, ln-1 et ln possèdent au moins M points d'intérêts correspondant à la même primitive 3D. La valeur de la contrainte est, par exemple, choisie parmi la liste 30 des contraintes suivante : Contrainte de position ek (xr,z, )_ Tk -T Contrainte d'orientation : ER(xt,z, )= RY RY -13V avec R la matrice de rotation paramétrisant l'orientation, / la matrice Identité, Contrainte de facteur d'échelle (ou de la norme de la translation) k) Tk-Tt-1-T-T1 Le procédé peut utiliser pour prédire la pose pr du dispositif le modèle d'observation inverse Ht f qui offre une estimation de l'état du dispositif xr à un instant t' Afin de mieux faire comprendre le principe et les modules mis en oeuvre dans l'invention, l'exemple qui suit est donné à titre illustratif et nullement limitatif pour un système comprenant une caméra image et un capteur de mouvement et/ou de position en relation avec un dispositif de traitement adapté pour exécuter les étapes des différents modes de réalisation du procédé. Dans certains cas, il sera possible d'utiliser une caméra pourvue d'un module permettant de mesurer son mouvement. En résumé, le procédé selon l'invention va intégrer des contraintes inter-pose pondérées (c'est-à-dire une contrainte sur la pose mesurée par la vision liée au dispositif de mesure de trajectoire) dans une étape "d'Ajustement de faisceaux", dans une application de localisation (SLAM). Les données inertielles associées à un modèle de mouvement servent à prédire la pose du système à l'instant d'une nouvelle image. Cette prédiction est ensuite utilisée afin de construire une contrainte inter-pose. Une technique issue de la théorie de l'apprentissage sélectionne le coefficient de pondération (ratio des covariances de bruit des signaux) de manière efficace, c'est-à-dire automatique et adaptée au contexte directement dans l'ajustement de faisceaux en temps réel). La contrainte inter-pose ainsi pondérée est ensuite intégrée dans la fonction de coût de l'ajustement de faisceaux. L'ajustement de faisceaux contraint est plus précis que les algorithmes de type Kalman car il effectue notamment plusieurs itérations de Gauss-Newton. La figure 1 représente un exemple de schéma d'une architecture système dans laquelle le procédé peut être mis en oeuvre. Le système selon l'invention 100 comporte: - un capteur d'images 101, tel qu'une caméra sténopé (pinhole), omnidirectionnelle, caméra 3D-TOF, caméra stéréoscopique, Webcam, Infrarouge, - un capteur de mouvement ou de position 102, tel qu'un GPS, un odomètre, le capteur d'images 101 et le capteur de mouvement peuvent être sur un même bâti, le capteur de mouvement ou de position 102 peut aussi comporter une mémoire tampon 107 mémorisant les données qu'il génère ou qu'il mesure, - une unité de traitement 103 qui peut elle être sur le même bâti ou encore délocalisée. Dans le cas où cette unité de traitement est éloignée des capteurs d'images et de position, elle échange les informations ou les données à l'aide de moyens de communication physiques (filaires, Internet) ou encore des moyens radios ou sans fil, - l'unité de traitement 103 est composée, par exemple d'un processeur 104 adapté à exécuter les étapes du procédé selon l'invention, des moyens de stockage des données, par exemple, une base de données 105 des poses du système, une base de données 106 des primitives 3D de la scène et des relations liant les primitives 3D et leurs observations dans les images. Dans le cas où le capteur de mouvement ne comporte pas de mémoire tampon pour mémoriser les données produites par le capteur de mouvement, cette dernière sera insérée au niveau de l'unité de traitement 103. Un moyen de visualisation 108 par un opérateur, tel qu'un écran LCD, ou un moniteur de PC, un module 109 commandant un actionneur (moteur, etc.) dans le but de faire par exemple de la navigation autonome d'un robot (on déplace le robot grâce au procédé selon l'invention en commandant les moteurs du robot par exemple.) Chaque capteur observe le monde (la scène) et donne une mesure de son propre déplacement (et non celui du dispositif). Donc, pour obtenir le mouvement du dispositif, dans le cas d'un dispositif composé d'un capteur caméra et d'un capteur de mouvement, on applique un simple changement de repère, connu de l'Homme du métier (modélisé aussi dans la fonction H décrite ci-après). Dans le cas d'une caméra comprenant un dispositif de traitement, l'unité de traitement 103 sera intégrée à la caméra. Avant de décrire les différentes étapes mises en oeuvre par le procédé selon l'invention, quelques rappels utiles pour la compréhension du procédé selon l'invention sont donnés. Rappels Le problème de fusion des capteurs peut être formulé comme un problème inverse, discrétisé dans le domaine temporel. Le processus d'observation est défini pour un capteur w par l'expression du vecteur d'observation suivant : zt = Ht (xt )+ nt Où il' est une fonction de projection exécutée par le capteur w à un instant donné t. Elle associe un objet réel ou une information xt (ici la localisation du système et la scène) à un vecteur d'observation zr . La fonction H inclut le changement de repère qui permet le passage d'observations exprimées dans le repère propre au capteur concerné au repère du système selon l'invention. Ceci peut être estimé par une procédure de calibrage connue de l'Homme du métier. Le vecteur erreur nr est habituellement représenté par un vecteur de fonctions gaussiennes caractérisées par une moyenne nulle (bruit blanc). Le vecteur erreur nr modélise l'imprécision introduite pendant les phases de mesure. L'objectif du problème inverse est de récupérer le vecteur réel inconnu xt à partir d'observations multiples délivrées par les capteurs. Pour un système dynamique, un modèle de mouvement définissant le comportement du dispositif au cours du temps doit être employé. Le modèle 3o utilisé sera xt = Ft (xt_j+ wt Où Ft définit le nouvel état wt du dispositif (comprenant notamment la pose) à partir de son état précédant. Les systèmes statiques, comme par exemple la scène s, sont par 5 définition constants au cours du temps et peuvent donc être exprimés sans l'indice temporel t. Estimation Bayesien récursive Le filtrage statistique consiste à estimer l'état d'un système dynamique qui valide au mieux les mesures incertaines et a priori, sachant 1 o que les mesures peuvent être bruitées. On considère dans cette méthode que l'état du système (toutes les mesures des capteurs) est un vecteur de variable aléatoire pour lesquelles on recherche un pdf. Un Estimateur récursif Bayesien a pour objectif de trouver ce pdf, minimisant la valeur espérée d'une fonction de cout. La fonction de cout la 15 plus connue est la fonction d'erreur quadratique moyenne ou MSE pour laquelle on cherche à minimiser l'erreur quadratique entre un estimateur et ses observations. Pour un capteur, on définit le l'erreur quadratique moyenne comme 11 e 2/ h) i a i k k 20 Avec dt est le vecteur de mesure résiduel et nt sa longueur, telle que uHét Lorsque toutes les mesures sont considérées comme indépendantes les unes des autres avec la covariance k 25 avec /s la matrice identité On obtient alors E tl e . t( t v7t Le problème de la fusion de capteur consiste à minimiser toutes les erreurs quadratiques moyennes des capteurs. Un exemple d'estimateur Bayesien est l'ajustement de faisceaux. Technique de l'ajustement de faisceaux La technique d'ajustement de faisceaux ou BA pour Bundle Adjustment est une technique d'optimisation basée sur une méthode de résolution des moindres carrés non-linéaires (par exemple Gauss-Newton, Levenberg-Marquardt,...). La fonction objective E à minimiser est généralement un terme de coût basé sur l'image (l'unité étant le pixel) et l'on utilise par exemple l'Erreur Quadratique Moyenne MSE: l'erreur de reprojection E(xr,zr )= E, (xr,zr) qui est la somme des distances élevées au carré entre les observations 2D, Zt de la caméra c à l'image t et les reprojections de la scène dans l'image. E? : erreur définie précédemment xt : = [psi la pose du dispositif et la scène Z : = les observations de la scène détectées dans l'image à l'instant t par la caméra c. La technique d'ajustement de faisceaux est utilisée pour différents propos, en fonction des variables d'entrée à optimiser. Habituellement, les variables peuvent être la dernière pose pt seulement et l'on parle de "raffinement de pose", de la scène seule s ou d'un certain nombre de poses et de la scène et l'on parlera alors d'ajustement de faisceaux local ou global (si on raffinement toutes les poses) [pt s]. Par exemple, afin de raffiner la scène et la pose du dispositif on résout l'équation : xr =argminE(xt,zr ), l'exposant c signifie que ce sont des observations effectuées par la caméra. L'exposant * indique une optimisation. Cette équation peut être résolue par une technique de l'art antérieur telle que Gauss-Newton ou Levenberg-Marquardt. Erreur de reprojection On définit ici l'erreur de reprojection : )= 1 n (0«1)2 11.zt -Y`(x,,s') Avec Y[(xr,s') la fonction de projection d'un ensemble de primitives 3D s' de la scène dans l'image opérée par la caméra c dont la pose pt est contenue dans xt. Cette fonction est propre au type de caméra et au type de primitives 3D utilisés dans le système. On peut, par exemple, prendre une caméra de type sténopé et des primitives formées de points 3D. La figure 2 représente une projection d'un point 3D Ez~x zt)- I 1 ,(x ) ra. )2 criJ l'écart type de l'observation zg si celui-ci est connu (sinon 1 et {zt = ql, } les observations de la camera c à l'instant t. D'autres types existent dans le domaine et sont connus de l'Homme du métier. Modules pour l'architecture système Les figures 3 et 4 décrivent deux exemples de variante de mise en oeuvre pour le procédé selon l'invention. Les modules et leurs fonctionnalités mis en oeuvre dans ces deux variantes vont être décrits. Partie localisation par vision [201,301] Module de localisation par vision situé au niveau du 25 dispositif du traitement des données Dans ce module les techniques de l'état de l'art sont utilisées pour estimer la pose du système à partir des images de la caméra. Ce module peut contenir divers algorithmes de l'état de l'art et peut donc varier suivant les implémentations. L'important pour le procédé selon l'invention est de disposer en sortie de la partie vision : - d'une estimée de la pose du système - d'un historique des dernières poses du système (ou une partie) - d'un historique des primitives 3D de la scène et leurs observations dans les images (ou une partie). [202,302] Acquisition des images On considère un module d'acquisition des images provenant d'une caméra de manière synchrone ou asynchrone. [204,304] Estimation de la pose par la vision On estime la pose (position et orientation) du dispositif dans le cas où les capteurs ne sont pas sur le même bâti que l'unité de traitement à l'aide des images définies précédemment. L'estimation de la pose s'opère au travers des méthodes suivantes Détection des éléments d'intérêt 2D Lorsqu'une image arrive au module [204,304], le procédé exécute une détection des éléments d'intérêt. Un élément d'intérêt est une observation d'une primitive 3D qui a une position bien définie dans une image et qui correspond à un point de l'espace détectable de manière robuste au changement de point de vue ou d'éclairage. Il existe de nombreux détecteurs connus de l'Homme du métier. Description des éléments d'intérêt Les éléments d'intérêts sont ensuite caractérisés par des descripteurs permettant de les différencier les uns des autres et de pouvoir les retrouver dans les images suivantes. Il permet de reconnaître le point grâce à son environnement proche. Il existe de nombreux descripteurs (ZNCC, abréviation anglo-saxonne de 30 Zero Mean Normalized Cross Correlation, etc.) connus de l'Homme du métier. Mise en correspondance des éléments d'intérêt Considérons maintenant que l'on dispose de deux images de la même scène prises selon des points de vue différents. Pour chacune d'elles, on suppose que l'on a détecté des éléments d'intérêt et calculé les descripteurs correspondants. La phase d'appariement proprement dite consiste à établir une correspondance entre les éléments ayant des caractérisations similaires entre les deux images. Il existe plusieurs méthodes d'appariement connues de l'Homme du métier. 1 o Calcul de la pose à partir des correspondances Une pose du dispositif est définie par la position Tt en 3 dimensions (x, y, z) dans un repère global ainsi que l'orientation, stockée par exemple sous la forme d'une matrice de rotation Rt. La méthode peut varier, par exemple on peut prendre l'algorithme des 5 points ou des 8 points 15 (méthodes classiques en vision par ordinateur) et calculer la pose à partir de N images. On peut aussi calculer la pose du dispositif en utilisant la scène déjà reconstruite (algorithme de resection connu de l'Homme du métier). Les paramètres décrivant la pose peuvent aussi varier (un quaternion au lieu d'une matrice de rotation, etc). 20 Estimation robuste de la pose La première estimée de la pose peut être perturbée par des erreurs dues aux méthodes précédentes et l'on propose ici d'estimer de manière plus précise et robuste la pose du dispositif. Pour cela l'Homme du métier utilise des méthodes connues : RANSAC, M-Estimateur, etc... 25 Sélection des poses clés Le mouvement entre deux images consécutives doit être suffisamment important pour calculer de manière précise la géométrie épipolaire, et ainsi estimer la pose des deux images. Ainsi, le procédé sélectionne des images relativement éloignées les unes des autres mais qui 30 ont suffisamment de points en commun. Pour cela, la première image 11 est toujours sélectionnée comme une image clé. La seconde image 12 sélectionnée est choisie aussi éloignée que possible de 11 dans la vidéo mais c'est une image qui doit posséder au moins M points de correspondance d'intérêt avec 11. Ensuite, pour 13, on choisit l'image la plus éloignée de 12 de façon telle que il y a au moins M points d'intérêts correspondant entre 13 et 12 et au moins M' points d'intérêts correspondant entre 13 et 11. Ainsi, il y a suffisamment de points d'intérêts communs entre les images pour déterminer le mouvement de la caméra. Pour les images ln suivantes, on procède de même en vérifiant que les images ln-2, ln-1 et ln possèdent au moins M points d'intérêts correspondant à la même primitive 3D. Une image 1 ne devient une image clé ln que si elle respecte ces règles. Cette méthode de détection des images clé peut varier et non proposons ici un exemple. Initialisation de la scène L'initialisation de la scène n'est opérée qu'une seule fois en début de séquence. Il s'agit ici de reconstruire une première fois l'environnement. Pour cela, on peut par exemple utiliser les 3 premières poses clés et trianguler les primitives 3D à partir de leurs observations (éléments d'intérêt) dans les 3 images (méthode de triangulation 3D). Elle permet notamment de constituer les bases de données des poses clés et des éléments de scène (3D). [212,312] Base de données des poses clés On considère que l'on possède une base de données qui contient les N (de 2 à toutes) dernières poses du système (avec les paramètres de calibrage courants de la caméra). [213,313] Base de données des éléments de scène On considère que l'on possède une base de données des primitives 3D de la scène reconstruite, ainsi que leurs correspondances (associations observations 2D (x,y) - primitive 3D) dans chaque image clé dans lesquelles l'élément aura été détecté (dont on connait la pose (position et orientation) grâce à la base de données [212]). Les primitives 3D sont par exemple des points 3D (x,y,z) mais peuvent être d'autres éléments (patch, segment, etc...). [209,309] Raffinement contraint de la pose (Extended Pose refinement) Les modules raffinement de la pose « pose refinement » et raffinement contraint de la pose « extended pose refinement » sont basés sur un algorithme très connu dans le domaine de la vision par ordinateur : l'ajustement de faisceaux. Plus précisément, dans cette partie, le procédé raffine juste la pose courante du dispositif en partant d'une première estimée de pose délivrée par le module [204,304] et en tenant compte des éléments de scène précédemment estimés. Dans le module [309], la fonction de coût est uniquement composée des erreurs de reprojections des primitives 3D de la scène sur la caméra dont la pose est actuellement raffinée. Dans le module [209], on ajoute au terme de coût, l'erreur de la prédiction (contrainte) délivrée par le module [208] et pondérée par un facteur lambda ou A' , appris dans le module de sélection automatique de pondération [208,308]. A la fin du raffinement, la pose courante du système est ajoutée à la base de données (historique) des poses [212,312]. Le raffinement de la pose peut être exécuté de la manière détaillée décrite ci-après. Dans un procédé de suivi incrémentai SfM, lorsqu'une nouvelle image ou une image clé est détectée un raffinement est exécuté sur la localisation de la caméra. Ainsi, il est possible de récupérer la pose courante pt du dispositif qui explique les mesures de capteurs Z, = [zrzr ] (ensemble de toutes les mesures pour tous les capteurs caméra c et des capteurs de mouvement k à un instant t). La fonction d'agrégation des coûts est ensuite construite avec le terme d'erreur standard visuel , ainsi que la contrainte sélectionnée E't( basée sur la prédiction de pose donnée par le capteur k ou de la caméra lorsque cette dernière comporte un module intégré de mesure de mouvement. Le second terme est pondéré avec un facteur de pondération sélectionné préalablement comme il sera détaillé plus loin dans la description. Le terme MSE (erreur quadratique moyenne) global est alors donné par la formule Ep(xt,Zt)= (xt,zr )+Î, 8k(xt,z,) pour un capteur de mouvement ou de position k, une camera c, un instant t. La pose courante pt est ensuite optimisée en utilisant l'algorithme de Levenberg-Marquardt avec la fonction de coût E p (xt, Zt) pour résoudre pt =argminEp(xt,Zt) Pt 1 o Le « * » signifie que l'on obtient la meilleure valeur de pt [210,310] Reconstruction de primitives 3D Ce module de reconstruction de primitives 3D est activé après le raffinement de la pose ([209,309]), soit juste après, soit quelques temps après. Il consiste en la triangulation des primitives 3D de la scène (point 3D 15 par exemple) à partir de leurs observations 2D effectuées dans les N dernières images (par ex N=3). Pour cela, on utilise les algorithmes de l'état de l'art de la vision par ordinateur. A la fin des calculs, les primitives 3D et leurs correspondances (associations observations 2D - primitive 3D) sont ajoutés dans la base de données 20 2D/3D. Partie estimation de la pose par l'intégration des données d'un capteur de position/déplacement [205,305] Acquisition des données du capteur de position/déplacement On considère un système (liaison capteur-module de traitement/ 25 driver) permettant de récupérer de manière asynchrone les données du capteur de mouvement et/ou de position. Lorsqu'une donnée est produite par le capteur de mouvement, celle-ci est récupérée par un programme pour être stockée dans une mémoire tampon soit située au niveau du capteur de mouvement et/ou de position, soit au niveau du module de traitement. [206,306] Prédiction de la pose à partir du capteur inertiel Lorsqu'une demande est faite au module de prédiction (206, 306) (par le module estimation de la pose par vision [204,304]), celui-ci récupère les observations du capteur antérieures à l'instant demandé par la synchronisation, les enlève de la pile ou mémoire tampon, et calcule ensuite la pose (ou une partie de celle-ci) du dispositif ou du système, à partir de ces mesures et d'un modèle de mouvement (optionnel mais la méthode est plus précise avec). Si l'on considère que le dispositif (composé de la caméra + et du capteur de mouvement) est séparé du moyen de traitement, par exemple, 1 o un PC, alors le procédé calcule la pose du dispositif et non du système comprenant le PC. Hypothèse modèle prédiction La technique d'ajustement de faisceaux ne fournissant pas de vitesses optimisées, ces quantités doivent être estimées en amont, ou être 15 fournies directement par le second capteur de mouvement et/ou de position. Il est aussi supposé, la présence du modèle d'observation inverse H ' qui offre une estimation de l'état du dispositif xr à un instant t' Dans la première version du système (figure 3), le procédé modifie les fonctions de coût des modules [209] et [214], alors que dans la deuxième version de mise en oeuvre du procédé représentée à la figure 4, ce terme est ajouté seulement dans la fonction de cout de l'ajustement de la pose [314]. Ce terme (fonction de cout) est généralement une distance entre la pose (ou une partie) courante du système ou du dispositif (et que l'on optimise) et la pose prédite par le module [206,306]. Le choix de l'erreur est à spécifier par le concepteur et dépend du type d'observations que le capteur de mouvement ou de position délivre. Par exemple, si l'on utilise un gyroscope 3D, celui-ci délivre une information de vitesse angulaire O. Cette information, alliée à un modèle de mouvement (vitesse angulaire constante par exemple), permet de construire une prédiction de l'orientation qk au même instant que l'image clé (dont l'orientation courante est qc). L'erreur peut par exemple être la distance entre les deux quaternions I Iqk - (Icl I. Le type de contrainte dépend de la nature des mesures effectuées par les capteurs. Par exemple, plusieurs contraintes pour des capteurs peuvent être proposées Contrainte de position £k (xr, zr) _ Vî,tk -T r Contrainte d'orientation : ER(xt,z, )= RY RY -13V avec R la matrice de rotation paramétrisant l'orientation, l la matrice Identité, Contrainte de facteur d'échelle (ou de la norme de la translation) ek(xr,z )= Tk -Tt-1 -T -T_1 Chaque contrainte aura un impact différent sur le système de localisation et tendra à corriger la dérive correspondante. Le procédé selon l'invention est générique, ainsi la contrainte de l'utilisateur définie pourra être tout terme de distance (en moindres carrés) qui contraint les variables optimisées dans la technique d'ajustement de faisceaux. [208] sélection automatique des pondérations: Estimation du ratio des covariances [210] Module de sélection de nouvelles primitives 3D L'erreur ou contrainte définie par le module de construction de contrainte doit être pondérée par un coefficient que l'on appellera lambda À, choisi par une technique spécifique dont certains exemples sont donnés ci-après. Dans l'exemple donné, plusieurs méthodes sont proposées pour calculer ce facteur de pondération. Chaque méthode peut être utilisée indépendamment les unes des autres pour résoudre ce problème. Pour cela, une fonction de coût globale est construite avec le terme d'erreur basé vision et avec le terme de fusion (la contrainte précédente) pondéré par le lambda. Plusieurs optimisations sont ensuite exécutées, soit appelant plusieurs fois le module [209] de la figure 3, soit appelant plusieurs fois le module [314] de la figure 4, pour un jeu de lambda différent (par exemple avec un jeu statique de 100 lambdas allant de 0.1 à 100). Comme détaillé ci-après, les critères de sélection du paramètre de pondération lambda varient. Soit on utilise la méthode de L-Curve et il s'agit de déterminer le maximum de courbure d'une certaine courbe, soit on utilise la méthode dite de la validation croisée et on cherche la meilleure valeur de lambda pour laquelle la prédiction d'une reprojection est la plus efficace. On peut aussi utiliser d'autres méthodes par critères (L-tangent Norm, Reginska, etc..) ou par apprentissage (comme la validation croisée, etc). Une fois que le meilleur lambda a été choisi, on procède à l'optimisation finale (soit [209], soit [314]) avec cette pondération et nous stockons de plus cette valeur ainsi que les paramètres de la contrainte dans la base de données prévue à cet effet [211,311]. Deux méthodes pour déterminer le paramètre de pondération vont être données à titre illustratif, sachant qu'il y en a d'autres connues de l'Homme du métier qui pourraient être mises en oeuvre. Méthode de sélection par critère Lcurve Le critère L-Curve à été à l'origine employée des les problèmes de régularisation dans lesquels on s'intéresse à estimer un paramètre de régularisation efficace permettant de minimiser aussi bien un premier terme de coût que le nombre de paramètres d'un modèle. La méthode à été de plus utilisée dans des problèmes d'optimisation 1 o moindres carrés non-linéaires à plusieurs objectifs, où il s'agissait de trouver un compromis entres les différents objectifs en estimant le facteur de pondération lambda de chaque terme de coût, et ainsi tenter de minimiser au mieux l'ensemble des objectifs. Le critère L-Curve représente donc le compromis entre les deux termes de 15 coût, en échelle log-log . Ce critère possède généralement une forme de L, dans laquelle un 'coin' sépare la partie horizontale de la partie verticale, correspondant à la prépondérance d'un des deux termes de coût à minimiser (objectifs). La courbure k(l) du critère L-Curve permet de trouver le coin recherché: où ' et " représentent les dérivées premières et secondes par rapport à lambda. Un "bon" paramètre de pondération lambda se situe près du coin du critère, là où la courbure k est au maximum. Dès lors on cherche : 25 Pour estimer la courbe L-Curve, nous devons calculer plusieurs couples représentant les deux objectifs (Ec,Ek) à minimiser pour différentes valeurs de lambda. Cela signifie que pour chaque valeur de lambda on optimise soit la 20 dernière pose du dispositif [209] soit les N dernières poses avec une partie de la scène [314]. Ensuite on construit la courbe L-Curve avec les couples et on estime le meilleur lambda situé au maximum de courbure (max k). Méthode de sélection par apprentissage Validation croisée. La méthode de validation croisée est un outil très populaire basée sur des considérations statistiques : un modèle adéquat (ici le facteur de pondération lambda) doit pouvoir prédire correctement des données manquantes. Plus précisément, si on enlève quelques mesures du jeu de données, alors la solution estimée (phase d'apprentissage : module [209 ou 314]) doit pouvoir prédire ces observations (lors de la phase de test). Il existe plusieurs variantes de la validation croisée, qui se différencient principalement par leur gestion des ensembles de test et d'apprentissage. Dans notre cas, le nombre limité d'observations de la caméra nous incite à utiliser la méthode du 'Ieave-one-out' (en recyclant les observations) mais l'on peut tout aussi bien utiliser une autre variante. Le score de validation croisée Cvloo(l) reflète la qualité de la prédiction pour un certain paramètre de pondération lambda. Il représente la différence moyenne entre l'observation réelle yj d'un point 3D j et la prédiction p 1, où la pose P't1j est optimisée sans le point j. avec , la solution du module [209 ou 314] sans le point j. L'objectif de la validation croisée est alors de maximiser la qualité de la prédiction que réalise le module [209 ou 314] pour un lambda, et ainsi minimiser les erreurs de prédiction. a = r n Clki 0 (A [211,311] Bases de données des contraintes pondérées Chaque contrainte pondérée est associée à une pose du dispositif pour chaque image si l'étape de sélection des images clés n'est pas mise en oeuvre ou pour chaque image clé et est ensuite stockée dans une base de données 211, 311. Cette contrainte (avec la pondération) sera réutilisée à chaque fois que la pose du dispositif associée sera raffinée dans le module d'ajustement [214,314]. Dans la base de données 211, 311, on stocke les éléments de la contrainte : la pondération Àk, et la prédiction pr de la pose 1 o du dispositif et non la contrainte entière, par exemple, 2 Ttk -T t , T étant une partie de p comme il est précisé dans la notation. [214,314] Ajustement de faisceaux contraint (Extended Bundle adjustement ) L'ajustement de faisceaux consiste en l'optimisation simultanée 15 d'un certain nombre de poses du dispositif (toute ou partie, récupérée depuis la base de données [212,312]) ainsi qu'un certain nombre de primitives 3D de scène (stockés dans la base [213,313]), par exemple l'ensemble des primitives vues par les poses que l'on optimise. La fonction de coût globale à minimiser est formée d'une part, avec 20 la vision : distance entre les reprojections des éléments de la scène dans les images et leurs vraies mesures effectuées dans les images, et d'autre part, de toutes les contraintes pondérées associées aux différentes poses clés du système (base de données [211,311]). Pour résoudre ce problème d'optimisation, on fait appel à un 25 algorithme d'optimisation nommé Levenberg-Marquardt. On peut aussi utiliser d'autres algorithmes (Descente de gradient avec ou sans préconditionnement, Gauss-Newton etc.) connus de l'Homme du métier. 24 25 Typiquement, le procédé optimise seulement les 3 dernières caméras clés ou les 3 dernières poses de la caméra, mais ce nombre peut varier. Ajustement de faisceaux local (LBA) Périodiquement, un raffinement est opéré sur la scène et la localisation du système afin de réduire les erreurs cumulées du SLAM incrémentai. De bons résultats sont obtenus avec une optimisation locale (c'est-à-dire en ne raffinant que les dernières poses). Il est ainsi possible d'optimiser, par exemple, au moins les poses des 2 dernières poses (éventuellement les poses clés). Une manière simple de faire est d'injecter les contraintes précédentes dans la fonction de coût LBA. On définit Xt = [pt-9 un vecteur comprenant des poses fixées (pt_9,....pt_3), des poses de la caméra que l'on souhaite raffiner (pt_2,pt_I,pt) et l'estimation courante d'une partie s' de la scène s, par exemple l'ensemble 15 des points 3D vus dans les N dernières poses. On pose, par exemple, N =3, M. 9 La fonction de coût d'ajustement des faisceaux avec les contraintes devient alors: r-- 1 t EBa (X t, Zt) _ EE, (L[~t'S 1~ zt )+ E Ep ([pt's ], Zt ) t'=t-(N-1) 20 Avec par exemple a= 3, b=9, d=2. La première partie de l'équation correspond aux erreurs de reprojection de la caméra dont les poses ne sont pas raffinées (donc fixes). Il n'est pas nécessaire de contraindre toutes les poses dans ce cas, seules les erreurs de reprojection originale sont prises en compte. 25 La seconde partie de l'équation est composée des erreurs de reprojections et des contraintes sur les N poses les plus récentes de la caméra. Le procédé optimise ensuite la localisation du système (les N dernières caméra) et une partie de la scène en utilisant l'algorithme de Levenberg-Marquardt, pour résoudre [Pt-ahz-Ipts']* = argmin EBA(Xt,Xz) ~Pr-2Pr-1Prs~ Le procédé et le système selon l'invention permettent donc de réaliser une fusion efficace, automatique et temps réel entre les données. Une telle fusion, qui tire ses performances de l'intégration de la fusion 1 o directement dans l'ajustement des faisceaux n'était pas possible par les méthodes de l'art antérieur qui ne permettent pas l'estimation optimisée d'un terme de pondération en temps réel. Elle offre une fusion précise et rapide par intégration directe des données au sein d'un processus d'ajustements de faisceau, et de fait offre une précision plus grande que les méthodes utilisant 15 le filtre de Kalman Etendu. Il n'est pas nécessaire par cette technique d'estimer les covariances initiales des mesures, ni de propager les covariances sur l'état du système, car le procédé selon l'invention s'adapte automatiquement: estimation en temps réel du paramètre de pondération lambda. 20 L'estimation du facteur de pondération est dynamique et temps-réel. Il est calculé pour chaque nouvelle optimisation, ce qui offre une méthode d'optimisation plus complète avec un historique des états précédents, c'est-à-dire que l'on raffine les N dernières poses du dispositif contrairement aux méthodes par filtre de Kalman selon l'art antérieur. REVENDICATIONS1 - Procédé pour fusionner des données issues d'un premier capteur images et d'un module de mesure de mouvement et/ou de position, ledit premier capteur et ledit module de mesure de mouvement et/ou de position étant solidaire et formant un dispositif, le procédé étant exécuté par un module de traitement afin de localiser ledit dispositif caractérisé en ce qu'il comporte au moins les étapes suivantes : - Une première étape consistant à acquérir plusieurs points sur les 10 images pour définir une pose pt du dispositif (caméra image et module de mesure mouvement et/ou position) pour un instant donné t - Une étape de prédiction de la pose pr du dispositif (caméra et capteur mouvement/position) pour un même instant donné t et en utilisant les mesures du module de mesure de mouvement et/ou de 15 position - Une étape où l'on détermine une contrainte Ek associée pour le dispositif (caméra et capteur) k, - Une étape où l'on détermine automatiquement un coefficient de pondération Àt de ladite contrainte, 20 - Une étape d'estimation de la pose courante pt dudit dispositif, - Une étape d'optimisation simultanée par la technique d'ajustement des faisceaux en utilisant la fonction de coût suivante : r-- 1 t EBA = (X t, Z) _ EE, (L[~t'S 1 zj+ E Ep (Lpt's ], Zt t'=t-(N-1) avec 25 Ep(xt,Zt)- ~c ~xt~zt )+%,t sk ~xt~zt : erreurs de reprojection de la scène dans les images o xt : = [psi la pose du dispositif et la scèneo Zt : = les observations de la scène détectées dans l'image de la caméra c à l'instant t Z2. z` I ensemble de toutes les mesures pour tous les o capteurs caméra c et des capteurs k ou module de mouvement et/ou de position à un instant t 2 - Procédé selon la revendication 1 caractérisé en ce qu'il comporte une étape d'initialisation des bases de données des poses clés et des éléments de scène (3D) en reconstruisant l'environnement du capteur images en 1 o utilisant un nombre donné de premières poses et en exécutant une triangulation des primitives 3D à partir des éléments d'intérêts les constituant pour les poses choisies. 3 - Procédé selon la revendication 1 caractérisé en ce que l'étape de 15 sélection automatique du coefficient de pondération utilise le résultat d'une étape de raffinement de la pose du dispositif en partant d'une première estimée de pose du dispositif délivrée par le module d'estimation de la pose par vision [204,304] et en tenant compte des éléments de scène précédemment estimés et [209], en ajoutant au terme de coût, l'erreur de la 20 prédiction (contrainte) délivrée par le module de sélection automatique de la pondération [208] et pondérée par un facteur lambda ). ou , appris dans le module de sélection automatique de pondération [208,308], à la fin de l'étape de raffinement, la pose courante du système est ajoutée à la base de données des poses [212]. 25 4 - Procédé selon la revendication 1 caractérisé en ce que l'étape de sélection automatique de la pondération comporte une étape (309) de raffinement de la pose du dispositif, dans le module [309], la fonction de coût est uniquement composée des erreurs de reprojections des primitives 3D de 30 la scène sur la caméra dont la pose est actuellement raffinée suivie d'uneétape de construction de la contrainte, puis de l'étape de sélection automatique de la pondération, à la fin de l'étape de raffinement, la pose courante du système est ajoutée à la base de données (historique) des poses [312]. 5 - Procédé selon la revendication 1 caractérisé en ce qu'il comporte une étape de sélection d'images clés comprenant les étapes suivantes : sélectionner une première image 11 comme une image clé, puis une seconde image 12 choisie aussi éloignée que possible de 11 dans la vidéo et 1 o possédant au moins M points de correspondance d'intérêt avec 11, puis choisir une troisième image 13, la plus éloignée de 12 de façon telle que il y a au moins M points d'intérêts correspondant entre 13 et 12 et au moins M' points d'intérêts correspondant entre 13 et 11, et ainsi de suite pour les images ln suivantes, procéder de même en vérifiant que les images ln-2, ln-1 15 et ln possèdent au moins M points d'intérêts correspondant à la même primitive 3D. 6 - Procédé selon la revendication 1 caractérisé en ce que la contrainte est choisie parmi la liste des contraintes suivante : 20 Contrainte de position £k «xr,z, )_ H Tk -T V Contrainte d'orientation : ER(xt,zR )= RY RY -I3 avec R la matrice de rotation paramétrisant l'orientation, l la matrice Identité, Contrainte de facteur d'échelle (ou de la norme de la translation) 25 sk(xr,z )= T 7 - Procédé selon la revendication 1 caractérisé en ce qu'il utilise pour prédire la pose Pf. du dispositif le modèle d'observation inverse H71 quioffre une estimation de l'état du dispositif xr à un instant t' 8 - Système pour fusionner des données issues d'un premier capteur images (101) et d'un module de mesure de mouvement et/ou de position, i o ledit premier capteur et ledit module de mesure de mouvement et/ou de position (102) étant solidaire et formant un dispositif, ledit système comportant : une unité de traitement (103) adaptée à exécuter les étapes du procédé selon l'une des revendications 1 à 7.