Main Content

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

trimLoopClosures

Optimisez le graphique de pose et supprimez les mauvaises fermetures de boucles

Depuis R2020b

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

Syntaxe

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions)
[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions)

Description

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions) optimise le graphique de pose pour satisfaire au mieux les contraintes de bord et supprime tous les bords de fermeture de boucle défectueux en fonction des paramètres d'erreur résiduels spécifiés dans trimParams. Créez l'entrée solverOptions à l'aide de la fonction poseGraphSolverOptions .

La fonction implémente la méthode de non-convexité graduée (GNC) avec un coût robuste des moindres carrés tronqués (TLS) en combinaison avec le solveur de graphe de pose non minimal [1].

exemple

[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions) renvoie des informations supplémentaires liées au processus de découpage.

Exemples

réduire tout

Ouvrir le live script

Optimisez un graphique de pose en fonction des nœuds et des contraintes de bord. Boucle de coupe fermée en fonction de leurs erreurs résiduelles de bord.

Chargez l'ensemble de données contenant un graphique de pose 2D. Inspectez l'objet poseGraph pour afficher le nombre de nœuds et de fermetures de boucle.

load grid-2d-posegraph.mat pg
disp(pg)
  poseGraph with properties:

               NumNodes: 120
               NumEdges: 193
    NumLoopClosureEdges: 74
     LoopClosureEdgeIDs: [120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 ... ] (1x74 double)
        LandmarkNodeIDs: [1x0 double]

Tracez le graphique de pose sans les identifiants. Les lignes rouges indiquent les fermetures de boucles identifiées dans l'ensemble de données. Les poses dans le graphique doivent suivre un quadrillage, mais montrer des signes de dérive au fil du temps.

show(pg,'IDs','off');
title('Original Pose Graph')

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

Optimisez le graphique de pose à l'aide de la fonction optimizePoseGraph . Par défaut, cette fonction utilise le solveur "builtin-trust-region" . Étant donné que le graphique de pose contient des fermetures de boucle incorrectes, le graphique de pose résultant n'est en réalité pas souhaitable.

pgOptim = optimizePoseGraph(pg);
figure;
show(pgOptim);

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

Examinez les erreurs résiduelles de bord pour le graphique de pose d'origine. De grandes valeurs d’erreur aberrantes à la fin indiquent de mauvaises fermetures de boucle.

resErrorVec = edgeResidualErrors(pg);
plot(resErrorVec);
title('Edge Residual Errors by Edge ID')

Figure contains an axes object. The axes object with title Edge Residual Errors by Edge ID contains an object of type line.

Certaines fermetures de boucles doivent être supprimées du graphique de pose en fonction de leur erreur résiduelle. Utilisez la fonction trimLoopClosures pour supprimer ces mauvaises fermetures de boucle. Définissez le seuil maximum et de troncature pour les paramètres de découpage. Ce seuil est défini en fonction de la précision des mesures et doit être adapté à votre système.

trimParams.MaxIterations = 100;
trimParams.TruncationThreshold = 25;

solverOptions = poseGraphSolverOptions;

Utilisez la fonction trimLoopClosures avec les paramètres de découpage et les options du solveur.

[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);

À partir de la sortie trimInfo , tracez les fermetures de boucle supprimées du graphique de pose optimisé. En traçant avec le tracé des erreurs résiduelles auparavant, vous pouvez voir que les grandes fermetures de boucles d'erreur ont été supprimées.

removedLCs = trimInfo.LoopClosuresToRemove;

hold on
plot(removedLCs,zeros(length(removedLCs)),'or')
title('Edge Residual Errors and Removed Loop Closures')
legend('Residual Errors', 'Removed Loop Closures')
xlabel('Edge IDs')
ylabel('Edge Residual Error')
hold off

Figure contains an axes object. The axes object with title Edge Residual Errors and Removed Loop Closures, xlabel Edge IDs, ylabel Edge Residual Error contains 45 objects of type line. One or more of the lines displays its values using only markers These objects represent Residual Errors, Removed Loop Closures.

Affichez le nouveau graphique de pose avec les mauvaises fermetures de boucles supprimées. 

show(pgNew,"IDs","off");

Figure contains an axes object. The axes object with 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 .

Paramètres d'erreur résiduelle pour couper les fermetures de boucles, spécifiés sous forme de structure avec des champs :

  • MaxIterations — Nombre maximum d'itérations autorisées pour le découpage de fermeture de boucle, spécifié sous la forme d'un entier positif. En une itération de découpage, le graphe de pose est optimisé en fonction des options du solveur et toutes les arêtes en dehors du TruncationThreshold sont tronquées.

  • TruncationThreshold — Erreur résiduelle maximale autorisée pour un bord. Cette valeur dépend fortement du graphique de pose que vous spécifiez dans poseGraphObj. Pour trouver un seuil approprié basé sur toutes les erreurs, utilisez la fonction edgeResidualErrors pour le graphique de pose.

Exemple : struct('MaxIterations',10,'TruncationThreshold',20)

Types de données : struct

Options du solveur de graphique de pose, spécifiées sous la forme d'un ensemble de paramètres générés en appelant la fonction poseGraphSolverOptions . La fonction génère un ensemble d'options de solveur avec des valeurs par défaut pour le type de solveur de graphique de pose spécifié :

pgSolverTrustRegion = poseGraphSolverOptions('builtin-trust-region')
pgSolverTrustRegion = 

TrustRegion (builtin-trust-region-dogleg) options:

               MaxIterations: 300
                     MaxTime: 10
           FunctionTolerance: 1.0000e-08
           GradientTolerance: 5.0000e-09
               StepTolerance: 1.0000e-12
    InitialTrustRegionRadius: 100
               VerboseOutput: 'off'
pgSolverG2o = poseGraphSolverOptions('g2o-levenberg-marquardt')
pgSolverG2o = 

G2oLevenbergMarquardt (g2o-levenberg-marquardt) options:

        MaxIterations: 300
              MaxTime: 10
    FunctionTolerance: 1.0000e-09
        VerboseOutput: 'off'

Modifiez les options pour ajuster les paramètres du solveur à l'aide de la notation par points.

pgSolverG2o.MaxIterations = 200;

Arguments de sortie

réduire tout

Graphique de pose avec fermetures en boucle découpées, spécifié comme objet poseGraph ou poseGraph3D .

Informations issues du processus de découpage, renvoyées sous forme de structure avec des champs :

  • LoopClosuresToRemove — ID de bord de fermeture de boucle à supprimer de l'entrée poseGraphObj. Ces fermetures de boucle sont supprimées dans la sortie poseGraphUpdated.

  • Iterations — Nombre d'itérations de découpage effectuées.

Références

[1] Yang, Heng, et al. “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection.” IEEE Robotics and Automation Letters, vol. 5, no. 2, Apr. 2020, pp. 1127–34. DOI.org (Crossref), doi:10.1109/LRA.2020.2965893.

Historique des versions

Introduit dans R2020b

Voir aussi

Fonctions

Objets