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

factorPoseSE3AndPointXYZ

Facteur reliant la position SE (3) et le point 3D

Depuis R2022b

Description

L'objet factorPoseSE3AndPointXYZ contient des facteurs qui décrivent chacun la relation entre une position dans l'espace d'état SE(3) et un point de repère 3D. Vous pouvez utiliser cet objet pour ajouter un ou plusieurs facteurs à un objet factorGraph .

Création

Syntaxe

F = factorPoseSE3AndPointXYZ(nodeID)

F = factorPoseSE3AndPointXYZ(___,Name=Value)

Description

F = factorPoseSE3AndPointXYZ(nodeID) crée un objet factorPoseSE3AndPointXYZ , F, avec la propriété des numéros d'identification de nœud, NodeID, définie à nodeID.

F = factorPoseSE3AndPointXYZ(___,Name=Value) spécifie les propriétés en utilisant un ou plusieurs arguments nom-valeur en plus de l'argument de la syntaxe précédente. Par exemple, factorPoseSE3AndPointXYZ([1 2],Measurement=[1 2 3]) définit la propriété Measurement de l'objet factorPoseSE3AndPointXYZ sur [1 2 3].

Propriétés

NodeID — Numéros d'identification des nœuds

N-matrice par 2 d'entiers non négatifs

Ce propriété est en lecture seule.

Numéros d'identification de nœud, spécifiés sous la forme d'une matrice N-par 2 d'entiers non négatifs, où N est le nombre total de facteurs souhaités. Chaque ligne représente un facteur reliant un nœud de type POSE_SE3 à un nœud de type POINT_XYZ sous la forme [PoseID PointID], où PoseID est l'ID du nœud POSE_SE3 et PointID est l'ID du Nœud $POINT_XYZ dans le graphique factoriel.

Si un facteur dans l'objet factorPoseSE3AndPointXYZ spécifie un ID qui ne correspond pas à un nœud dans le graphe de facteurs, le graphe de facteurs crée automatiquement un nœud du type requis avec cet ID et l'ajoute au facteur. graphique lors de l’ajout du facteur au graphique des facteurs.

Vous devez spécifier cette propriété lors de la création de l'objet.

Pour plus d'informations sur les types de nœuds attendus de tous les facteurs pris en charge, consultez Types de nœuds attendus pour les objets facteurs.

Measurement — Position relative mesurée

zeros(N,3) (par défaut) | N-matrice par 3

Position relative mesurée entre la position actuelle et le point de repère, spécifiée sous la forme d'une matrice N-par 3 où chaque ligne est de la forme [dx dy dz], en mètres. N est le nombre total de facteurs, et dx, dy et dz sont le changement en position dans x, y et z, respectivement.

Information — Matrice d'informations associée à l'incertitude des mesures

eye(3) (par défaut) | Matrice 3x3 | Tableau 3 par 3 par N

Matrice d'informations associée à l'incertitude des mesures, spécifiée sous la forme d'une matrice 3 par 3 ou d'un tableau 3 par 3 par N . N est le nombre total de facteurs spécifiés par l'objet factorPoseSE3AndPointXYZ . Chaque matrice d'information correspond aux mesures du nœud correspondant dans NodeID.

Si vous spécifiez cette propriété sous forme de matrice 3 par 3 lorsque NodeID contient plus d'une ligne, la matrice d'informations correspond à toutes les mesures dans Measurement.

Cette matrice d'information est l'inverse de la matrice de covariance, où la matrice de covariance est de la forme :

$$\left[\begin{array}{ccc}\sigma (x,x)& \sigma (x,y)& \sigma (x,z)\\ \sigma (y,x)& \sigma (y,y)& \sigma (y,z)\\ \sigma (z,x)& \sigma (y,x)& \sigma (z,z)\end{array}\right]$$

Chaque élément indique la covariance entre deux variables. Par exemple, σ(x,y) est la covariance entre x et y.

Fonctions d'objet

nodeType

Obtenir le type de nœud dans le graphique de facteurs

Exemples

Estimer les poses à l'aide de facteurs marquants

Créez une matrice de positions des points de repère à utiliser pour la localisation et des poses réelles du robot avec lesquelles comparer votre estimation du graphique factoriel. Utilisez la fonction d'assistance exampleHelperPlotPositionsAndLandmarks pour visualiser les points de repère et le chemin réel du robot.

gndtruth = [0 0 0; 
            2 0 pi/2; 
            2 2 pi; 
            0 2 pi];
landmarks = [3 0; 0 3];
exampleHelperPlotPositionsAndLandmarks(gndtruth,landmarks)

Utilisez la fonction d'assistance exampleHelperSimpleFourPoseGraph pour créer un graphique de facteurs contenant quatre poses liées par trois facteurs 2D à deux poses. Pour plus de détails, voir la page objet factorTwoPoseSE2 .

fg = exampleHelperSimpleFourPoseGraph(gndtruth);

Créer des facteurs marquants

Générez des ID de nœud pour créer deux ID de nœud pour deux points de repère. Les deuxième et troisième nœuds de pose observent le premier point de repère et doivent donc se connecter à ce point de repère avec un facteur. Les troisième et quatrième nœuds de pose observent le deuxième point de repère.

lmIDs = generateNodeID(fg,2);
lmFIDs = [1 lmIDs(1);  % Pose Node 1 <-> Landmark 1 
          2 lmIDs(1);  % Pose Node 2 <-> Landmark 1
          2 lmIDs(2);  % Pose Node 2 <-> Landmark 2
          3 lmIDs(2)]; % Pose Node 3 <-> Landmark 2

Définir les mesures de position relative entre la position des poses et leurs repères dans le référentiel du nœud de pose. Ajoutez ensuite du bruit.

lmFMeasure = [0  -1; % Landmark 1 in pose node 1 reference frame 
             -1   2; % Landmark 1 in pose node 2 reference frame
              2  -1; % Landmark 2 in pose node 2 reference frame
              0  -1]; % Landmark 2 in pose node 3 reference frame
lmFMeasure = lmFMeasure + 0.1*rand(4,2);

Créez les facteurs de repère avec ces mesures relatives et ajoutez-les au graphique des facteurs.

lmFactor = factorPoseSE2AndPointXY(lmFIDs,Measurement=lmFMeasure);
addFactor(fg,lmFactor);

Définissez l'état initial des nœuds de points de repère sur la position réelle des points de repère avec un peu de bruit.

nodeState(fg,lmIDs,landmarks+0.1*rand(2));

Optimiser le graphique des facteurs

Optimisez le graphique factoriel avec les options du solveur par défaut. L'optimisation met à jour les états de tous les nœuds dans le graphique de facteurs, donc les positions du véhicule et les points de repère sont mis à jour.

rng default
optimize(fg)

ans = struct with fields:
             InitialCost: 0.0538
               FinalCost: 6.2053e-04
      NumSuccessfulSteps: 4
    NumUnsuccessfulSteps: 0
               TotalTime: 3.1304e-04
         TerminationType: 0
        IsSolutionUsable: 1
        OptimizedNodeIDs: [1 2 3 4 5]
            FixedNodeIDs: 0

Visualisez et comparez les résultats

Obtenez et stockez les états de nœuds mis à jour pour le robot et les points de repère. Tracez ensuite les résultats, en comparant l'estimation du graphique factoriel de la trajectoire du robot à la vérité terrain connue du robot.

poseIDs = nodeIDs(fg,NodeType="POSE_SE2")

poseIDs = 1×4

     0     1     2     3

poseStatesOpt = nodeState(fg,poseIDs)

poseStatesOpt = 4×3

         0         0         0
    2.0815    0.0913    1.5986
    1.9509    2.1910   -3.0651
   -0.0457    2.0354   -2.9792

landmarkStatesOpt = nodeState(fg,lmIDs)

landmarkStatesOpt = 2×2

    3.0031    0.1844
   -0.1893    2.9547

exampleHelperPlotPositionsAndLandmarks(gndtruth,landmarks,poseStatesOpt,landmarkStatesOpt);

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.

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 R2022b

R2023a: Spécifiez plusieurs facteurs

Les propriétés NodeID, Measurement et Information acceptent désormais des lignes supplémentaires pour spécifier plusieurs facteurs.

Voir aussi

Objets

• factorGraph | factorTwoPoseSE3 | factorPoseSE2AndPointXY | factorCameraSE3AndPointXYZ

Rubriques

• Landmark SLAM Using AprilTag Markers