edgeNodePairs

Edge node pairs in pose graph

Syntax

nodePairs = edgeNodePairs(poseGraph)

nodePairs = edgeNodePairs(poseGraph,edgeIDs)

Description

nodePairs = edgeNodePairs(poseGraph) returns all edges in the specified pose graph as a list of node ID pairs. Each row of the edges output is a pair of nodes that form an edge. Multiple edges may exist between the same pair of nodes.

nodePairs = edgeNodePairs(poseGraph,edgeIDs) returns edges corresponding to the specified edge IDs. Each edge in the pose graph has a unique ID even if the node pairs are the same.

Examples

Identify and Remove Spurious Loop Closures from Pose Graph

This example shows how to identify and remove spurious loop closures from pose graph. To do this, you can modify the relative pose of a loop closure edge and try optimizing the pose graph with and without removing the auto spurious loop closure and compare the results.

Load the Intel Research Lab Dataset that contains a 2-D pose graph. Optimize the pose graph. Plot the pose graph with IDs off. Red lines indicate loop closures identified in the dataset.

load intel-2d-posegraph.mat pg
optimizedPG = optimizePoseGraph(pg);
show(optimizedPG,IDs="off");
title("Optimized Pose Graph")

Modify the relative pose of the loop closure edge 1386 to some random values.

loopclosureId = 1386;
nodePair = edgeNodePairs(optimizedPG,loopclosureId);
[relPose,infoMat] = edgeConstraints(optimizedPG,loopclosureId);
relPose(2) = -5;
relPose(3) = 1.5;
addRelativePose(optimizedPG,relPose,infoMat,nodePair(1),nodePair(2));

Optimize the pose graph without auto loop closure trimming. Plot the optimized pose graph to see the poor adjustment of the nodes with loop closures.

[updatedPG,solutionInfo] = optimizePoseGraph(optimizedPG);
show(updatedPG,IDs="off");
title("Updated Pose Graph")

Certain loop closures should be trimmed from the pose graph. Use the trimLoopClosures function to trim these bad loop closures. Set the truncation threshold and maximum iterations for the trimmer parameters.

trimParams = struct("TruncationThreshold",0.5,"MaxIterations",100);

Generate solver options.

solverOptions = poseGraphSolverOptions("g2o-levenberg-marquardt");

Use the trimLoopClosures function with the trimmer parameters and solver options. Plot the new pose graph to see the bad loop closures were removed.

[newPG,trimInfo] = trimLoopClosures(updatedPG,trimParams,solverOptions);
show(newPG,IDs="off");
title("New Pose Graph")

Input Arguments

poseGraph — Pose graph

poseGraph object | poseGraph3D object

Pose graph, specified as a poseGraph or poseGraph3D object.

edgeIDs — Edge IDs

vector of positive integers

Edge IDs, specified as a vector of positive integers.

Output Arguments

nodePairs — Edge node pairs in pose graph

n-by-2 matrix

Edge node pairs in pose graph, returned as n-by-2 matrix that lists the IDs of the two nodes that each edge connects. Each row is a pair of nodes that form an edge. Multiple edges may exist between the same pair of nodes, so the matrix may contain duplicate entries.

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 R2019b

R2019b: edgeNodePairs was renamed

The edgeNodePairs object function was renamed from edges. Use edgeNodePairs when calling the function.