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

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.

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 boucle 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

poseGraph objet | poseGraph3D objet

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

edgeIDs — ID de bord

vecteur d'entiers positifs

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

Arguments de sortie

measurements — Mesures entre nœuds

n-matrice par 3 | n-matrice par 7

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 .

infoMats — Matrices d'informations

n-matrice par 6 | n-matrice par 21

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

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.

Historique des versions

Introduit dans R2019b

Voir aussi

Fonctions

• edgeNodePairs | optimizePoseGraph | addRelativePose | findEdgeID | 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