edgeResidualErrors
Compute pose graph edge residual errors
Description
returns the residual errors for each edge in the pose graph with the current pose node
estimates. The residual errors order matches the order of edge IDs in
resErrorVec
= edgeResidualErrors(poseGraphObj
)poseGraph
.
Examples
Optimize and Trim Loop Closures For 2-D Pose Graphs
Optimize a pose graph based on the nodes and edge constraints. Trim loop closed based on their edge residual errors.
Load the data set that contains a 2-D pose graph. Inspect the poseGraph
object to view the number of nodes and loop closures.
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 ... ] LandmarkNodeIDs: [1x0 double]
Plot the pose graph with IDs off. Red lines indicate loop closures identified in the dataset. The poses in the graph should follow a grid pattern, but show evidence of drift over time.
show(pg,'IDs','off'); title('Original Pose Graph')
Optimize the pose grap using the optimizePoseGraph
function. By default, this function uses the "builtin-trust-region"
solver. Because the pose graph contains some bad loop closures, the resulting pose graph is actual not desirable.
pgOptim = optimizePoseGraph(pg); figure; show(pgOptim);
Look at the edge residual errors for the original pose graph. Large outlier error values at the end indicate bad loop closures.
resErrorVec = edgeResidualErrors(pg);
plot(resErrorVec);
title('Edge Residual Errors by Edge ID')
Certain loop closures should be trimmed from the pose graph based on their residual error. Use the trimLoopClosures
function to trim therse bad loop closures. Set the maximum and truncation threshold for the trimmer parameters. This threshold is set based on the measurement accuracy and should be tuned for your system.
trimParams.MaxIterations = 100; trimParams.TruncationThreshold = 25; solverOptions = poseGraphSolverOptions;
Use the trimLoopClosures
function with the trimmer parameters and solver options.
[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);
From the trimInfo
output, plot the loop closures removed from the optimized pose graph. By plotting with the residual errors plot before, you can see the large error loop closures were removed.
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
Show the new pose graph with the bad loop closures trimmed.
show(pgNew,"IDs","off");
Input Arguments
poseGraphObj
— Pose graph
poseGraph
object | poseGraph3D
object
Pose graph, specified as a poseGraph
or
poseGraph3D
object.
Output Arguments
resErrorVec
— Edge residual errors for pose graph
vector of positive scalars
Edge residual errors for pose graph, specified as a vector of positive scalars.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Use this syntax when constructing poseGraph
or
poseGraph3D
objects for code generation:
poseGraph = poseGraph('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)
specifies an upper bound on the number of edges and nodes allowed in the pose graph when
generating code. This limit is only required when generating code.
Version History
Introduced in R2020b
See Also
Functions
Objects
Ouvrir l'exemple
Vous possédez une version modifiée de cet exemple. Souhaitez-vous ouvrir cet exemple avec vos modifications ?
Commande MATLAB
Vous avez cliqué sur un lien qui correspond à cette commande MATLAB :
Pour exécuter la commande, saisissez-la dans la fenêtre de commande de MATLAB. Les navigateurs web ne supportent pas les commandes MATLAB.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)