Main Content

trimLoopClosures

Optimize pose graph and remove bad loop closures

Since R2020b

collapse all in page

Syntax

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

Description

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions) 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 trimParams. Create the solverOptions input using the poseGraphSolverOptions function.

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 [1].

[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions) returns additional information related to the trimming process.

example

Examples

collapse all

Open Live Script

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')

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

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);

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

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')

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

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

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.

Show the new pose graph with the bad loop closures trimmed. 
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
Input Arguments
collapse all

          
Pose graph, specified as a poseGraph or
              poseGraph3D
            object.

        

          
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 are trimmed.
  • 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 edgeResidualErrors function for the pose
                  graph.



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

        
Data Types: struct

          
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:
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;

        
Output Arguments
collapse all

          
Pose graph with trimmed looped closures, specified as a poseGraph or
              poseGraph3D
            object.

        

          
Information from trimming process, returned as a structure with fields:


  • LoopClosuresToRemove — Loop closure edge IDs to remove from
                  the input poseGraphObj. These loop closures are removed in
                  the output poseGraphUpdated.
  • Iterations — Number of trimming iterations
                  performed.



        
References
[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.
Version History
Introduced in R2020b
See Also
Functions
Objects