Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

factorGraph

Graphique biparti de facteurs et de nœuds

Depuis R2022a

Description

Un objet factorGraph stocke un graphe biparti composé de facteurs connectés à des nœuds variables. Les nœuds représentent les variables aléatoires inconnues dans un problème d'estimation, tel que les poses de robot, et les facteurs représentent des contraintes probabilistes sur ces nœuds, dérivées de mesures ou de connaissances antérieures. Lors de l'optimisation, le graphique de facteurs utilise tous les facteurs et les états actuels des nœuds pour mettre à jour les états des nœuds.

Pour utiliser le graphique factoriel :

1. Créez un objet factorGraph vide.

2. Pour chaque type de facteur souhaité :

   1. Générez des ID de nœud à l'aide de la fonction d'objet generateNodeID .

   2. Définissez des facteurs avec les ID de nœud souhaités, à l'aide de l'un des objets facteurs pris en charge :

      • factorGPS

      • factorIMU

      • factorCameraSE3AndPointXYZ

      • factorPoseSE2AndPointXY

      • factorPoseSE3AndPointXYZ

      • factorTwoPoseSE2

      • factorTwoPoseSE3

      • factorIMUBiasPrior

      • factorPoseSE3Prior

      • factorVelocity3Prior

   3. Ajoutez des facteurs au graphique de facteurs à l'aide de la fonction objet addFactor . Si le graphique de facteurs ne contient pas de nœud avec l'ID spécifié, la fonction crée automatiquement un nœud avec cet ID et l'ajoute au graphique de facteurs lors de l'ajout du facteur au graphique de facteurs. Si le graphique de facteurs contient un nœud avec l'ID spécifié, assurez-vous que l'ajout du nouveau facteur n'entraîne pas une incompatibilité de type de nœud. Pour plus d'informations, consultez Conseils. Pour une liste des types de nœuds attendus pour chaque facteur, voir Types de nœuds attendus pour les objets facteurs.

3. Vérifiez si tous les nœuds du graphique factoriel sont connectés à au moins un autre nœud à l'aide de la fonction objet isConnected .

4. Créez un objet factorGraphSolverOptions pour spécifier les options du solveur de graphe factoriel.

5. Optimisez le graphique factoriel à l'aide de la fonction objet optimize avec les options de résolution de graphique factoriel souhaitées.

6. Extrayez les données de nœud du graphique factoriel, telles que les ID de nœud et les états de nœud, à l'aide des fonctions d'objet nodeIDs et nodeState .

Création

Syntaxe

graph = factorGraph

Description

graph = factorGraph crée un objet factorGraph vide.

Propriétés

NumNodes — Nombre de nœuds dans le graphique factoriel

entier non négatif

Ce propriété est en lecture seule.

Nombre de nœuds dans le graphique factoriel, spécifié sous la forme d'un entier positif. NumNodes a une valeur de 0 lorsque le graphique du facteur est vide et NumNodes augmente chaque fois que vous ajoutez un facteur qui spécifie de nouveaux ID de nœud. au graphique factoriel.

Les nœuds du graphique factoriel peuvent être de l’un des types suivants :

• "POSE_SE2" — Pose dans l'espace d'état SE(2)

• "POSE_SE3" — Pose dans l'espace d'état SE(3)

• "VEL3" — vitesse 3D

• "POINT_XY" — Point 2D

• "POINT_XYZ" — Point 3D

• "IMU_BIAS" — Biais du gyroscope et de l'accéléromètre IMU

Pour vérifier le type de nœud d'un nœud dans le graphique, utilisez la fonction nodeType .

Remarque

Le graphique de facteurs définit le type de nœud lorsque vous ajoutez l'objet facteur qui spécifie ce nœud au graphique de facteurs. Vous ne pouvez pas modifier le type de nœud d'un nœud après l'avoir ajouté au graphique.

NumFactors — Nombre de facteurs dans le graphique des facteurs

entier non négatif

Ce propriété est en lecture seule.

Nombre de facteurs dans le graphique de facteurs, spécifié sous la forme d'un entier positif. NumFactors a une valeur de 0 lorsque le graphique de facteurs est vide et NumFactors augmente à chaque fois que vous ajoutez un facteur au graphique de facteurs.

Vous pouvez utiliser addfactor pour ajouter l'un de ces objets facteurs au graphique factoriel :

Associer les poses aux mesures du capteur

• factorGPS — Connectez le nœud de pose SE(3) ("POSE_SE3") à une mesure GPS.

• factorIMU — Connectez deux nœuds de pose SE(3) ("POSE_SE3"), deux nœuds de vitesse 3D ("VEL3") et deux Nœuds de biais IMU ("IMU_BIAS") utilisant une mesure IMU.

Associer les poses aux positions marquantes

• factorCameraSE3AndPointXYZ — Connectez le nœud de pose SE(3) d'une caméra sténopé ("POSE_SE3") à des nœuds de repère 3D ("Point_XYZ") en utilisant des mesures de pose relative.

• factorPoseSE2AndPointXY — Connectez un nœud de pose SE(2) ("POSE_SE2") à des nœuds de repère 2D ("Point_XY") à l'aide de mesures de pose relatives .

• factorPoseSE3AndPointXYZ — Connectez un nœud de pose SE(3) ("POSE_SE3") à des nœuds de repère 3D ("Point_XYZ") à l'aide de mesures de pose relatives .

Relier les poses les unes aux autres

• factorTwoPoseSE2 — Connectez des paires de nœuds de pose SE(2) ("POSE_SE2") avec des poses relatives à l'aide de mesures de pose relative.

• factorTwoPoseSE3 — Connectez des paires de nœuds de pose SE(3) ("POSE_SE3") avec des poses relatives à l'aide de mesures de pose relative.

Relier les poses ou les vitesses à des mesures connues antérieurement

• factorIMUBiasPrior — Connectez les nœuds de pose SE(3) ("POSE_SE3"), les nœuds de vitesse 3D ("VEL3") et les nœuds de biais IMU ("IMU_BIAS") aux mesures IMU connues antérieurement.

• factorPoseSE3Prior — Connectez les nœuds de pose SE(3) ("POSE_SE3") aux mesures de pose SE(3) connues auparavant.

• factorVelocity3Prior — Connectez le nœud de vitesse 3D ("VEL_3") aux mesures de vitesse SE(3) connues auparavant.

Fonctions d'objet

addFactor	Ajouter un facteur au graphique des facteurs
fixNode	Fix or free nodes in factor graph
generateNodeID	Générer de nouveaux ID de nœud
hasNode	Check if node ID exists in factor graph
isConnected	Vérifiez si le graphique des facteurs est connecté
isNodeFixed	Vérifiez si le nœud est corrigé
nodeIDs	Obtenir les ID de nœud dans le graphique de facteurs
nodeState	Get or set node state in factor graph
nodeType	Obtenir le type de nœud dans le graphique de facteurs
optimize	Optimiser le graphique du facteur

Exemples

Optimiser le graphique à facteurs simples

Créez un graphique factoriel.

fg = factorGraph;

Définissez deux états de pose approximatifs du robot.

rstate = [0 0 0;
          1 1 pi/2];

Définissez la mesure de pose relative entre deux nœuds de l'odométrie comme la différence de pose entre les états avec du bruit. La mesure relative doit être dans le référentiel du deuxième nœud, vous devez donc faire pivoter la différence de position pour être dans le référentiel du deuxième nœud.

posediff = diff(rstate);
rotdiffso2 = so2(posediff(3),"theta");
transformedPos = transform(inv(rotdiffso2),posediff(1:2));
odomNoise = 0.1*rand;
measure = [transformedPos posediff(3)] + odomNoise;

Créez un facteur à deux poses SE(2) avec la mesure relative. Ajoutez ensuite le facteur au graphique des facteurs pour créer deux nœuds.

ids = generateNodeID(fg,1,"factorTwoPoseSE2");
f = factorTwoPoseSE2(ids,Measurement=measure);
addFactor(fg,f);

Obtenez l'état des deux nœuds de pose.

stateDefault = nodeState(fg,ids)

stateDefault = 2×3

     0     0     0
     0     0     0

Ces nœuds étant nouveaux, ils ont des valeurs d’état par défaut. Idéalement, avant d'optimiser, vous devez attribuer une estimation approximative de la pose absolue. Cela augmente la possibilité pour la fonction optimize de trouver le minimum global. Sinon, optimize pourrait se retrouver piégé dans le minimum local, produisant une solution sous-optimale.

Conservez le premier état du nœud à l'origine et définissez le deuxième état du nœud sur une position xy approximative à [0.9 0.95] et une rotation thêta de pi/3 radians. Dans des applications pratiques, vous pouvez utiliser les mesures des capteurs de votre odométrie pour déterminer l'état approximatif de chaque nœud de pose.

nodeState(fg,ids(2),rstate(2,:))

ans = 1×3

    1.0000    1.0000    1.5708

Avant d'optimiser, enregistrez l'état du nœud afin de pouvoir le réoptimiser si nécessaire.

statePriorOpt1 = nodeState(fg,ids);

Optimisez les nœuds et vérifiez les états des nœuds.

optimize(fg);
stateOpt1 = nodeState(fg,ids)

stateOpt1 = 2×3

   -0.1038    0.8725    0.1512
    1.1038    0.1275    1.8035

Notez qu'après optimisation, le premier nœud n'est pas resté à l'origine car bien que le graphe ait l'estimation initiale de l'état, le graphe n'a aucune contrainte sur la position absolue. Le graphique n'a que la mesure de pose relative, qui agit comme une contrainte pour la pose relative entre les deux nœuds. Le graphique tente donc de réduire le coût lié à la pose relative, mais pas à la pose absolue. Pour fournir plus d'informations au graphique, vous pouvez fixer l'état des nœuds ou ajouter un facteur de mesure préalable absolu.

Réinitialisez les états, puis corrigez le premier nœud. Vérifiez ensuite que le premier nœud est fixe.

nodeState(fg,ids,statePriorOpt1);
fixNode(fg,ids(1))
isNodeFixed(fg,ids(1))

ans = logical
   1

Réoptimisez le graphique des facteurs et obtenez les états des nœuds.

optimize(fg)

ans = struct with fields:
             InitialCost: 1.8470
               FinalCost: 1.8470e-16
      NumSuccessfulSteps: 2
    NumUnsuccessfulSteps: 0
               TotalTime: 8.6069e-05
         TerminationType: 0
        IsSolutionUsable: 1
        OptimizedNodeIDs: 1
            FixedNodeIDs: 0

stateOpt2 = nodeState(fg,ids)

stateOpt2 = 2×3

         0         0         0
    1.0815   -0.9185    1.6523

Notez qu'après optimisation de ce temps, l'état du premier nœud est resté à l'origine.

En savoir plus

Types de nœuds attendus pour les objets facteurs

Voici les types de nœuds que la propriété NodeID de chaque objet facteur spécifie et auxquels elle se connecte :

Objet facteur	Types de nœuds attendus des ID de nœud spécifiés
factorGPS	["POSE_SE3"]
factorIMU	["POSE_SE3","VEL3","IMU_BIAS","POSE_SE3","VEL3","IMU_BIAS"]
factorCameraSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]

Par exemple, factorPoseSE2AndPointXY([1 2]) crée un facteur de repère 2D se connectant aux ID de nœud 1 et 2. Si vous essayez d'ajouter ce facteur à un graphique de facteurs qui contient déjà les nœuds 1 et 2, le facteur s'attend à ce que les nœuds 1 et 2 soient de types "POSE_SE2" et "POINT_XY", respectivement.

Conseils

• Pour spécifier plusieurs facteurs et nœuds à la fois pour un type de facteur spécifique, utilisez la fonction generateNodeID et spécifiez le nombre de facteurs et le type de facteur. Voir la fonction generateNodeID pour plus de détails.

  poseIDPairs = generateNodeID(fg,3,"factorTwoPoseSE2");
  ftpse2 = factorTwoPoseSE2(poseIDPairs);

• Vous pouvez obtenir les états de tous les nœuds de pose en utilisant d'abord la fonction nodeIDs et en spécifiant le type de nœud comme "POSE_SE2" pour les poses de robot SE(2) et "POSE_SE3" pour les poses du robot SE(3). Ensuite, utilisez la fonction nodeState avec ces ID de nœud pour obtenir les états des nœuds de pose du robot.

  poseIDs = nodeIDs(fg,NodeType="POSE_SE2");
  poseStates = nodeState(fg,poseIDs);

• Vérifiez les types de nœuds créés ou auxquels chaque facteur se connecte avant d'ajouter des facteurs au graphique de facteurs afin d'éviter les erreurs d'incompatibilité des types de nœuds. Pour une liste des types de nœuds attendus pour chaque facteur, voir Types de nœuds attendus pour les objets facteurs.

Références

[1] Dellaert, Frank. Factor graphs and GTSAM: A Hands-On Introduction. Georgia: Georgia Tech, September, 2012.

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2022a

Voir aussi

Objets

• factorGraphSolverOptions

Fonctions

• importFactorGraph | estimateGravityRotation | estimateGravityRotationAndPoseScale

Rubriques

• Factor Graph-Based Pedestrian Localization with IMU and GPS Sensors
• Landmark SLAM Using AprilTag Markers
• Implement Visual SLAM in MATLAB (Computer Vision Toolbox)