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

addRelativePose

Ajouter une pose relative au graphique de pose

Depuis R2019b

Syntaxe

addRelativePose(poseGraph,measurement)

addRelativePose(poseGraph,measurement,infoMat)

addRelativePose(poseGraph,measurement,infoMat,fromNodeID)

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID)

[nodePair,edgeID] = addRelativePose(___)

Description

addRelativePose(poseGraph,measurement) crée un nœud basé sur l'entrée measurement qui se connecte au dernier nœud de pose dans le graphique de pose. Pour ajouter des nœuds de repère, consultez la fonction addPointLandmark .

addRelativePose(poseGraph,measurement,infoMat) spécifie également la matrice d'informations dans le cadre de la contrainte de bord, qui représente l'incertitude de la mesure de pose.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID) crée un nouveau nœud de pose et le connecte au nœud spécifique spécifié par fromNodeID.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID) crée un bord en spécifiant une mesure de pose relative entre les nœuds existants spécifiés par fromNodeID et toNodeID. Cette arête est appelée fermeture de boucle. Si une fermeture de boucle existe déjà, la fonction ajoute la nouvelle mesure. L’appel de la fonction optimizePoseGraph combine plusieurs mesures ajoutées en une seule arête. Cette syntaxe ne prend pas en charge l'ajout d'arêtes à un nœud de repère.

[nodePair,edgeID] = addRelativePose(___) renvoie le bord et l'ID de bord nouvellement ajoutés en utilisant l'une des syntaxes précédentes.

Exemples

Identifier et supprimer les fermetures de boucles parasites du graphique de pose

Cet exemple montre comment identifier et supprimer les fermetures de boucles parasites du graphique de pose. Pour ce faire, vous pouvez modifier la pose relative d'un bord de fermeture de boucle et essayer d'optimiser le graphique de pose avec et sans supprimer la fermeture de boucle parasite automatique et comparer les résultats.

Chargez le Intel Research Lab Dataset qui contient un graphique de pose 2D. Optimisez le graphique de pose. Tracez le graphique de pose sans les identifiants. Les lignes rouges indiquent les fermetures de boucles identifiées dans l'ensemble de données.

load intel-2d-posegraph.mat pg
optimizedPG = optimizePoseGraph(pg);
show(optimizedPG,IDs="off");
title("Optimized Pose Graph")

Modifiez la pose relative du bord de fermeture de boucle 1386 en quelques valeurs aléatoires.

loopclosureId = 1386;
nodePair = edgeNodePairs(optimizedPG,loopclosureId);
[relPose,infoMat] = edgeConstraints(optimizedPG,loopclosureId);
relPose(2) = -5;
relPose(3) = 1.5;
addRelativePose(optimizedPG,relPose,infoMat,nodePair(1),nodePair(2));

Optimisez le graphique de pose sans découpage automatique de la fermeture de boucle. Tracez le graphique de pose optimisé pour voir le mauvais ajustement des nœuds avec des fermetures de boucle.

[updatedPG,solutionInfo] = optimizePoseGraph(optimizedPG);
show(updatedPG,IDs="off");
title("Updated Pose Graph")

Certaines fermetures de boucles doivent être supprimées du graphique de pose. Utilisez la fonction trimLoopClosures pour supprimer ces mauvaises fermetures de boucle. Définissez le seuil de troncature et les itérations maximales pour les paramètres de découpage.

trimParams = struct("TruncationThreshold",0.5,"MaxIterations",100);

Générez des options de solveur.

solverOptions = poseGraphSolverOptions("g2o-levenberg-marquardt");

Utilisez la fonction trimLoopClosures avec les paramètres de découpage et les options du solveur. Tracez le nouveau graphique de pose pour voir que les mauvaises fermetures de boucles ont été supprimées.

[newPG,trimInfo] = trimLoopClosures(updatedPG,trimParams,solverOptions);
show(newPG,IDs="off");
title("New Pose Graph")

Arguments d'entrée

poseGraph — Graphique de pose

Objet poseGraph | poseGraph3D objet

Graphique de pose, spécifié comme objet poseGraph ou poseGraph3D .

measurement — Pose relative entre les nœuds

[x y theta] vecteur | [x y z qw qx qy qz] vecteur

Pose relative entre les nœuds, spécifiée comme l'un des éléments suivants :

Pour poseGraph (2-D), la pose est un vecteur [x y theta] , qui définit une xy-position et angle d'orientation, theta.

Pour poseGraph3D, la pose est un vecteur [x y z qw qx qy qz] , qui définit par une xyz-position et orientation du quaternion, [qw qx qy qz]

Remarque

De nombreuses autres sources de graphiques de pose 3D, y compris les formats .g2o , spécifient l'orientation du quaternion dans un ordre différent, par exemple [qx qy qz qw]. Vérifiez la source de vos données de graphique de pose avant d'ajouter des nœuds à votre objet poseGraph3D .

infoMat — Matrice d'informations

vecteur à six éléments | vecteur de 21 éléments

Matrices d'informations, spécifiées sous forme compacte sous forme de vecteur à six éléments ou de vecteur à 21 éléments.

Chaque vecteur est la forme compacte du triangle supérieur de la matrice d'informations carrée. Une matrice d'information représente l'incertitude de la mesure. La matrice est calculée comme l'inverse de la covariance. Si la mesure est un vecteur [x y theta] , la matrice de covariance est un 3 par 3 de calculs de covariance par paires. Généralement, l'incertitude est déterminée par le modèle du capteur.

Pour poseGraph (2-D), chaque matrice d'informations est un vecteur à six éléments. La valeur par défaut est [1 0 0 1 0 1]. Pour les nœuds de repère, les trois derniers éléments sont renvoyés sous la forme NaN.

Pour poseGraph3D, chaque matrice d'informations est un vecteur de 21 éléments. La valeur par défaut est [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1].

fromNodeID — Nœud à partir duquel attacher

entier positif

Nœud à partir duquel s’attacher, spécifié sous la forme d’un entier positif. Cet entier correspond à l'ID d'un nœud dans poseGraph. Lorsqu'il est spécifié sans toNodeID, addRelativePose crée un nouveau nœud et ajoute une arête entre le nouveau nœud et le nœud fromNodeID .

toNodeID — Nœud auquel s'attacher

entier positif

Nœud auquel s'attacher, spécifié sous la forme d'un entier positif. Cet entier correspond à l'ID d'un nœud dans poseGraph. addRelativePose ajoute un bord entre ce nœud et le nœud fromNodeID .

Arguments de sortie

nodePair — Paire de nœuds Edge dans le graphique de pose

vecteur à deux éléments

Paires de nœuds de bord dans le graphique de pose, renvoyées sous forme de vecteur à deux éléments qui répertorie les ID des deux nœuds connectés par chaque bord. Plusieurs arêtes peuvent exister entre la même paire de nœuds.

edgeID — ID du bord ajouté

entier positif

ID du bord ajouté, renvoyé sous forme d’entier positif.

Capacités étendues

Génération de code C/C++

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

Utilisez cette syntaxe lors de la construction d'objets poseGraph ou poseGraph3D pour la génération de code :

poseGraph = poseGraph('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) spécifie une limite supérieure sur le nombre d'arêtes et de nœuds autorisés dans le graphe de pose lors de la génération du code. Cette limite n'est requise que lors de la génération de code.

poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)

Historique des versions

Introduit dans R2019b

Voir aussi

Fonctions

• optimizePoseGraph | findEdgeID | edgeNodePairs | edgeConstraints | nodeEstimates | removeEdges

Objets

• poseGraph | poseGraph3D | lidarSLAM

Rubriques

• Implement Simultaneous Localization And Mapping (SLAM) with Lidar Scans
• Implement Online Simultaneous Localization And Mapping (SLAM) with Lidar Scans
• Landmark SLAM Using AprilTag Markers