Main Content

poseGraph

Create 2-D pose graph

Description

A poseGraph object stores information for a 2-D pose graph representation. A pose graph contains nodes connected by edges. Each node estimate is connected to the graph by edge constraints that define the relative pose between nodes and the uncertainty on that measurement.

To construct a pose graph iteratively, use the addRelativePose function to add relative pose estimates and connect them to an existing node with specified edge constraints. Pose nodes must be specified relative to a pose node. Specify the uncertainty of the measurement using an information matrix.

Figure showing relative poses between node poses. Each sequential pose is connected by an edge. Poses are specified as x,y, theta relative to the previous node.

Adding an edge between two nonsequential nodes creates a loop closure in the graph. Multiple edges or multiedges between node pairs are also supported, which includes loop closures. To add additional edge constraints or loop closures, specify the node IDs using the addRelativePose function. When optimizing the pose graph, the optimizePoseGraph function finds a solution to satisfy all these edge constraints.

Figure showing loop closures and multiedges as constraints between nodes. Loop closures connect to previous non-sequential nodes. Multiedges are multiple constraints between the same node pair.

To add landmark point nodes, use the addPointLandmark function. This function specifies nodes as xy-points without orientation estimates. Landmarks must be specified relative to a pose node.

Figure showing a landmark position as an xy point relative to two nodes with an edge between each node. to.

The lidarSLAM object performs lidar-based simultaneous localization and mapping, which is based around the optimization of a 2-D pose graph.

For 3-D pose graphs, see the poseGraph3D object or the Landmark SLAM Using AprilTag Markers example.

Creation

Description

example

poseGraph = poseGraph creates a 2-D pose graph object. Add poses using addRelativePose to construct a pose graph iteratively.

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 syntax is only required when generating code.

Properties

expand all

This property is read-only.

Number of nodes in pose graph, specified as a positive integer. Each node represents a pose measurement or a point landmark measurement. To specify relative poses between nodes, use addRelativePose. To specify a landmark pose, use addLandmarkPose. To get a list of all nodes, use edgeNodePairs.

This property is read-only.

Number of edges in pose graph, specified as a nonnegative integer. Each edge connects two nodes in the pose graph. Loop closure edges and landmark edges are included.

This property is read-only.

Number of loop closures in pose graph, specified as a nonnegative integer. To get the edge IDs of the loop closures, use the LoopClosureEdgeIDs property.

This property is read-only.

Loop closure edges IDs, specified as a vector of edge IDs.

This property is read-only.

Landmark node IDs, specified as a vector of IDs for each node.

Object Functions

addPointLandmarkAdd landmark point node to pose graph
addRelativePoseAdd relative pose to pose graph
copyCreate copy of pose graph
edgeNodePairsEdge node pairs in pose graph
edgeConstraintsEdge constraints in pose graph
edgeResidualErrorsCompute pose graph edge residual errors
findEdgeIDFind edge ID of edge
nodeEstimatesPoses of nodes in pose graph
removeEdgesRemove loop closure edges from graph
showPlot pose graph

Examples

collapse all

Optimize a pose graph based on the nodes and edge constraints. The pose graph used in this example is from the Intel Research Lab Dataset and was generated from collecting wheel odometry and a laser range finder sensor information in an indoor lab.

Load the Intel data set that contains a 2-D pose graph. Inspect the poseGraph object to view the number of nodes and loop closures.

load intel-2d-posegraph.mat pg
disp(pg)
  poseGraph with properties:

               NumNodes: 1228
               NumEdges: 1483
    NumLoopClosureEdges: 256
     LoopClosureEdgeIDs: [1228 1229 1230 1231 1232 1233 1234 1235 1236 ... ]
        LandmarkNodeIDs: [1x0 double]

Plot the pose graph with IDs off. Red lines indicate loop closures identified in the dataset.

show(pg,'IDs','off');
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 graph. Nodes are adjusted based on the edge constraints and loop closures. Plot the optimized pose graph to see the adjustment of the nodes with loop closures.

updatedPG = optimizePoseGraph(pg);
figure
show(updatedPG,'IDs','off');
title('Updated Pose Graph')

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

References

[1] Grisetti, G., R. Kummerle, C. Stachniss, and W. Burgard. "A Tutorial on Graph-Based SLAM." IEEE Intelligent Transportation Systems Magazine. Vol. 2, No. 4, 2010, pp. 31–43. doi:10.1109/mits.2010.939925.

Extended Capabilities

Introduced in R2019b