Optimize pose graph and remove bad loop closures
optimizes the pose graph to best satisfy the edge constrains and removes any bad loop
closure edges based on the residual error parameters specified in
poseGraphUpdated = trimLoopClosures(
trimParams. Create the
solverOptions input using
The function implements the graduated non-convexity (GNC) method with truncated least squares (TLS) robust cost in combination with the non-minimal pose graph solver .
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;
trimLoopClosures function with the trimmer parameters and solver options.
[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);
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.
trimParams — Residual error parameters for trimming
Residual error parameters for trimming loop closures, specified as a structure with fields:
MaxIterations— Maximum number of iterations allowed for loop closure trimming, specified as a positive integer. In one trimming iteration, the pose graph is optimized based on the solver options and any edges outside the
TruncationThreshold— Maximum allowed residual error for an edge. This value depends heavily on the pose graph you specify in
poseGraphObj. To find a proper threshold based on all the errors, use the
edgeResidualErrorsfunction for the pose graph.
solverOptions — Pose graph solver options
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
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'
Modify the options to tune the solver parameters using dot notation.
pgSolverG2o.MaxIterations = 200;
trimInfo — Information from trimming process
 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.