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

isNodeFixed

Vérifiez si le nœud est corrigé

Depuis R2022a

Syntaxe

isFixed = isNodeFixed(graph,nodeID)

Description

isFixed = isNodeFixed(graph,nodeID) renvoie un indicateur logique indiquant si le nœud avec l'ID de nœud spécifié dans le graphique de facteurs est corrigé ou non lors de l'optimisation. Lorsque vous corrigez un nœud, la fonction optimize ne modifie pas l'état de ce nœud.

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.

Arguments d'entrée

graph — Graphique factoriel

Objet factorGraph

Graphique factoriel, spécifié comme objet factorGraph .

nodeID — ID de nœud du nœud existant

entier non négatif

ID de nœud d'un nœud existant, spécifié sous forme d'entier non négatif.

Arguments de sortie

isFixed — Correction de l'état du nœud

true ou 1 | false ou 0

Correction de l'état du nœud, renvoyé sous la forme d'un 1 (true) ou 0 (false). La fonction renvoie true lorsque le nœud est fixe, et renvoie false lorsque le nœud est libre.

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

• factorGraph