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

edgeConstraints

Contraintes de bord dans le graphique de pose

réduire tous les éléments de la page

Syntaxe

measurements = edgeConstraints(poseGraph)
[measurements,infoMats] = edgeConstraints(poseGraph)
[measurements,infoMats] = edgeConstraints(poseGraph,edgeIDs)

Description

measurements = edgeConstraints(poseGraph) répertorie toutes les contraintes de bord dans le graphique de pose spécifié sous forme de pose relative.

[measurements,infoMats] = edgeConstraints(poseGraph) renvoie également les matrices d'informations pour chaque arête. La matrice d'informations est l'inverse de la covariance de la mesure de pose.

[measurements,infoMats] = edgeConstraints(poseGraph,edgeIDs) renvoie les contraintes de bord pour les ID de bord spécifiés.

exemple

Exemples

réduire tout

Ouvrir le live script

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")

Figure contains an axes object. The axes object with title Optimized Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

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")

Figure contains an axes object. The axes object with title Updated Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

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 boucle ont été supprimées.

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

Figure contains an axes object. The axes object with title New Pose Graph, xlabel X, ylabel Y contains 3 objects of type line. One or more of the lines displays its values using only markers

Arguments d'entrée

réduire tout

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

ID de bord, spécifiés comme vecteur d’entiers positifs.

Arguments de sortie

réduire tout

Mesures entre nœuds, renvoyées sous forme de matrice n-par-3 ou n-par-7.

Pour poseGraph (2-D), chaque ligne est un vecteur [x y theta] , qui définit la position relative et l'angle d'orientation du xy, theta, d'une pose dans le graphique. Pour les positions marquantes, theta est renvoyé sous la forme NaN.

Pour poseGraph3D, chaque ligne est un vecteur [x y z qw qx qy qz] , qui définit la position relative xyz et l'orientation du quaternion, [qw qx qy qz], d'une pose dans le graphique.

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 .

Matrices d'informations, spécifiées sous forme compacte sous la forme d'une matrice n-par-6 ou n-par-21, où n est le nombre de poses dans le graphique de poses.

Chaque ligne correspond au 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].

Capacités étendues

développer tout

Historique des versions

Introduit dans R2019b

Voir aussi

Fonctions

Objets

Rubriques