poseGraphSolverOptions
Description
returns the set of solver options with default values for the specified pose graph solver
type.solverOptions
= poseGraphSolverOptions(solverType
)
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 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]
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 graph 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 these 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
solverType
— Pose graph solver type
'builtin-trust-region'
(default) | 'g2o-levenberg-marquardt'
Pose graph solver type, specified as 'builtin-trust-region'
or
'g2o-levenberg-marquardt'
.
The function generates a set of solver options with default values for the specified pose graph solver type:
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'
Data Types: char
| string
Output Arguments
solverOptions
— Pose graph solver options
poseGraphSolverOptions
parameters
Pose graph solver options, specified as a set of parameters generated by calling the
poseGraphSolverOptions
function. The function generates a set of solver
options with default values for the specified pose graph solver type.
If the solverType
input is set to
"builtin-trust-region"
:
Default | Description | |
---|---|---|
MaxIterations | 300 | Maximum number of iterations, specified as a positive integer. The optimizer exits after it exceeds this number of iterations. |
MaxTime | 500 | Maximum time allowed, specified as a positive numeric scalar in seconds. The optimizer exits after it exceeds this time. |
FunctionTolerance | 1e-8 | Lower bound on the change in the cost function, specified as a scalar. If the cost function change falls below this value between optimization steps, the optimizer exits. |
GradientTolerance | 0.5e-8 | Lower bound on the norm of the gradient, specified as a scalar. The norm of the gradient is calculated based on the cost function of the optimization. If the norm falls below this value, the optimizer exits. |
StepTolerance | 1e-12 | Lower bound on the step size, specified as a scalar. If the norm of the optimization step falls below this value, the optimizer exits. |
InitialTrustRegionRadius | 100 | Initial trust region radius, specified as a scalar. |
VerboseOutput | 'off' or 'on' | Display intermediate iteration information on the MATLAB® command line. |
If the solver
input is set to
"g2o-levenberg-marquardt"
:
Default | Description | |
---|---|---|
MaxIterations | 300 | Maximum number of iterations, specified as a positive integer. The optimizer exits after it exceeds this number of iterations. |
MaxTime | 500 | Maximum time allowed, specified as a positive numeric scalar in seconds. The optimizer exits after it exceeds this time. |
FunctionTolerance | 1e-8 | Lower bound on the change in the cost function, specified as a scalar. If the cost function change falls below this value between optimization steps, the optimizer exits. |
VerboseOutput | 'off' or 'on' | Display intermediate iteration information on the MATLAB command line. |
Version History
Introduced in R2020b
See Also
Functions
trimLoopClosures
|edgeResidualErrors
|edgeResidualErrors
|removeEdges
|edgeNodePairs
|edgeConstraints
Objects
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
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)