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

addFactor

Ajouter un facteur au graphique des facteurs

Depuis R2022a

Syntaxe

factorIDs = addFactor(graph,factor)

factorIDs = addFactor(graph,factor,groupID)

Description

La fonction addFactor ajoute un ou plusieurs facteurs à un graphique de facteurs et peut regrouper les facteurs et les nœuds spécifiés par les facteurs ajoutés.

factorIDs = addFactor(graph,factor) ajoute un ou plusieurs facteurs au graphique de facteurs spécifié et renvoie les ID des facteurs ajoutés.

Si l'ajout des facteurs entraîne un nœud non valide, alors addFactor renvoie une erreur et indique les nœuds non valides.

Remarque

addFactor ne prend en charge que l'addition à un seul facteur pour les objets factorIMU et factorGPS .

factorIDs = addFactor(graph,factor,groupID) ajoute un facteur au graphique de facteurs avec l'ID de groupe groupID. Les ID de nœud du même groupe peuvent être récupérés par ID de groupe à l'aide de la fonction nodeIDs . Vous pouvez utiliser des ID de groupe pour représenter des horodatages ou des images.

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.

Ajouter des nœuds aux groupes

Créez un graphique de facteurs, générez des ID de nœud et créez deux facteurs factorTwoPoseSE2 .

fg1 = factorGraph;
ids = generateNodeID(fg1,[2 2])

ids = 2×2

     0     1
     2     3

f = factorTwoPoseSE2(ids);

Regrouper tous les nœuds

Ajoutez tous les nœuds des facteurs au groupe 1.

addFactor(fg1,f,1);
fg1Group1 = nodeIDs(fg1,GroupID=1)

fg1Group1 = 1×4

     0     1     2     3

Regrouper les nœuds par colonne

Spécifiez l'ID de groupe en tant que vecteur de ligne pour ajouter les nœuds de la première colonne des ID de nœud des facteurs au groupe 1 et ajoutez les nœuds de la deuxième colonne au groupe 2.

fg2 = factorGraph;
addFactor(fg2,f,[1 2]);
fg2Group1 = nodeIDs(fg2,GroupID=1)

fg2Group1 = 1×2

     0     2

fg2Group2 = nodeIDs(fg2,GroupID=2)

fg2Group2 = 1×2

     1     3

Regrouper les nœuds par ligne

Spécifiez l'ID de groupe en tant que vecteur de colonne pour ajouter les nœuds de la première ligne dans les ID de nœud des facteurs au groupe 1 et ajoutez les nœuds de la deuxième ligne au groupe 2.

fg3 = factorGraph;
addFactor(fg3,f,[1; 2]);
fg3Group1 = nodeIDs(fg3,GroupID=1)

fg3Group1 = 1×2

     0     1

fg3Group2 = nodeIDs(fg3,GroupID=2)

fg3Group2 = 1×2

     2     3

Regrouper les nœuds par matrice

Vous pouvez également spécifier l'ID de groupe sous forme de matrice de la même taille que les ID de nœud des facteurs pour attribuer chaque nœud à un groupe spécifique. Ajoutez les premier et quatrième nœuds au groupe 1 et les deuxième et troisième nœuds aux groupes 2 et 3, respectivement.

fg4 = factorGraph;
groups = [1 2;
          3 1];
addFactor(fg4,f,groups);
fg4Group1 = nodeIDs(fg4,GroupID=1)

fg4Group1 = 1×2

     0     3

fg4Group2 = nodeIDs(fg4,GroupID=2)

fg4Group2 = 1

fg4Group3 = nodeIDs(fg4,GroupID=3)

fg4Group3 = 2

Arguments d'entrée

graph — Graphique de facteur auquel ajouter un facteur

Objet factorGraph

Graphique de facteurs auquel ajouter un facteur, spécifié comme objet factorGraph .

factor — Facteurs à ajouter au graphique des facteurs

objet facteur valide

Facteurs à ajouter au graphique de facteurs, spécifiés comme objet facteur valide.

Un objet facteur valide doit être l'un de ces objets, et l'objet ne doit pas créer de nœuds non valides lorsqu'il est ajouté au graphique de facteurs :

• factorGPS

• factorIMU

• factorCameraSE3AndPointXYZ

• factorPoseSE2AndPointXY

• factorPoseSE3AndPointXYZ

• factorTwoPoseSE2

• factorTwoPoseSE3

• factorIMUBiasPrior

• factorPoseSE3Prior

• factorVelocity3Prior

À l'exception de factorGPS et factorIMU, vous pouvez ajouter simultanément plusieurs facteurs au graphique de facteurs en utilisant l'un des objets facteurs répertoriés. factorGPS et factorIMU ne prennent en charge que l'addition à un seul facteur.

Si l'objet facteur spécifié crée des nœuds non valides, alors addFactor n'ajoute aucun des facteurs de l'objet facteur.

groupID — ID de groupe pour les nœuds du facteur ajouté

entier non négatif | vecteur ligne à deux éléments d'entiers non négatifs | N-élément colonne vecteur d'entiers non négatifs | N-matrice par 2 d'entiers non négatifs

ID de groupe pour les nœuds du facteur ajouté, spécifiés comme l'une de ces options :

groupID Taille	Comportement de regroupement
entier non négatif	Attribue tous les nœuds à un groupe. Par exemple, si vous ajoutez un objet facteur qui a une valeur NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ avec une valeur groupID de 1, addFactor ajoute des nœuds 1, 2, 3 et 4 au groupe 1.
vecteur ligne à deux éléments d'entiers non négatifs	Spécifiez des groupes pour chaque colonne de nœuds. Par exemple, si vous ajoutez un objet facteur qui a une valeur NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ avec une valeur groupID de $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor ajoute les nœuds 1 et 3 au groupe 1 et ajoute les nœuds 2 et 4 au groupe 2. Le comportement des facteurs IMU est différent. Si vous ajoutez un facteur IMU avec une valeur NodeID de $$\left[\begin{array}{cccccc}1& 2& 3& 4& 5& 6\end{array}\right]$$ et groupID défini sur $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor ajoute les nœuds 1, 2 et 3 au groupe 1 et les nœuds 4, 5 et 6 au groupe 2.
N-élément colonne vecteur d'entiers non négatifs	Regroupez les nœuds par facteur, où N est le nombre total de facteurs spécifiés par la propriété NodeID de factor. Par exemple, si vous ajoutez un objet facteur qui a une valeur NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ avec une valeur groupID de $$\left[\begin{array}{c}1\\ 2\end{array}\right]$$, addFactor ajoute les nœuds 1 et 2 au groupe 1 et ajoute les nœuds 3 et 4 au groupe 2.
N-matrice par 2 d'entiers non négatifs	Ajoutez des nœuds dans NodeID au groupe spécifié à son index correspondant dans groupID, où N est le nombre total de lignes du NodeID propriété de factor. Par exemple, si vous ajoutez un objet facteur qui a une valeur NodeID de $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ avec une valeur groupID de $$\left[\begin{array}{cc}1& 2\\ 3& 1\end{array}\right]$$, addFactor ajoutent les nœuds 1 et 4 dans le groupe 1, ajoute le nœud 2 au groupe 2, et ajoute le nœud 3 au groupe 3.

Remarque

Lors de l'ajout d'un factorIMU ou factorGPS à un graphique factoriel, groupID accepte uniquement ces valeurs :

• factorIMU — Entier non négatif ou vecteur de ligne à deux éléments d'entiers non négatifs.

• factorGPS — Entier non négatif

L'ajout de nœuds à des groupes vous permet d'interroger les ID de nœud par groupe en spécifiant l'argument nom-valeur GroupID de la fonction nodeIDs .

Arguments de sortie

factorIDs — ID de facteur des facteurs ajoutés

N - vecteur de ligne d'éléments d'entiers non négatifs

ID de facteur des facteurs ajoutés, renvoyés sous la forme d'un vecteur de ligne d'éléments N d'entiers non négatifs. N est le nombre total de facteurs ajoutés.

La fonction renvoie cet argument uniquement lorsqu'elle ajoute avec succès les facteurs au graphique de facteurs. Si l'ajout des facteurs entraîne un nœud non valide, alors addFactor n'ajoute aucun des facteurs de l'objet facteur.

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

R2023a: Spécifier les ID de groupe pour les facteurs ajoutés

addFactor prend désormais en charge la spécification de groupes auxquels ajouter des nœuds de facteurs ajoutés par ID de groupe.

Voir aussi

Objets

• factorGraph | factorTwoPoseSE2 | factorTwoPoseSE3 | factorPoseSE3Prior | factorVelocity3Prior | factorIMUBiasPrior | factorGPS | factorIMU

Fonctions

• nodeIDs

Rubriques

• Factor Graph-Based Pedestrian Localization with IMU and GPS Sensors
• Landmark SLAM Using AprilTag Markers