Main Content


Compute pose graph edge residual errors



    resErrorVec = edgeResidualErrors(poseGraphObj) 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 poseGraph.


    collapse all

    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
      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.

    title('Original Pose Graph')

    Figure contains an axes object. The axes object with title Original Pose Graph contains 3 objects of type line.

    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 contains an axes object. The axes object contains 225 objects of type line, text.

    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);
    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 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
    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 contains 45 objects of type line. These objects represent Residual Errors, Removed Loop Closures.

    Show the new pose graph with the bad loop closures trimmed.


    Figure contains an axes object. The axes object contains 3 objects of type line.

    Input Arguments

    collapse all

    Pose graph, specified as a poseGraph or poseGraph3D object.

    Output Arguments

    collapse all

    Edge residual errors for pose graph, specified as a vector of positive scalars.

    Extended Capabilities

    Version History

    Introduced in R2020b