poseGraph3D

Create 3-D pose graph

expand all in page

Description

A poseGraph3D object stores information for a 3-D pose graph representation. A pose graph contains nodes connected by edges, with edge constraints that define the relative pose between nodes and the uncertainty on that measurement. The optimizePoseGraph function modifies the nodes to account for the uncertainty and improve the overall graph.

For 2-D pose graphs, see poseGraph.

To construct a pose graph iteratively, use addRelativePose to add poses and connect them to the existing graph. Specify the uncertainty associated using an information matrix. Specify loop closures by add extra edge constraints between existing nodes.

Creation

Syntax

poseGraph = poseGraph3D
poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)

Description

example

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

poseGraph = poseGraph3D('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.

Properties

expand all

This property is read-only.

Number of nodes in pose graph, specified as a positive integer. Each node represents a pose in the pose graph as an [x y z qw qx qy qz] vector with an xyz-position and quaternion orientation, [qw qx qy qz]. To specify relative poses between nodes, use addRelativePose. To get a list of all nodes, use nodes.

Note

The order of the quaternion [qw qx qy qz] uses the standard convention. Some vehicle coordinate systems instead specify the order as [qx qy qz qw]. Check the source of your pose graph data before adding nodes to your poseGraph3D object.

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

Object Functions

addRelativePoseAdd relative pose to pose graph
edgesEdges in pose graph
edgeConstraintsEdge constraints in pose graph
edgeResidualErrorsCompute pose graph edge residual errors
findEdgeIDFind edge ID of edge
nodesPoses of nodes in pose graph
optimizePoseGraphOptimize nodes in pose graph
removeEdgesRemove loop closure edges from graph
showPlot pose graph

Examples

collapse all

Open Live Script

Optimize a pose graph based on the nodes and edge constraints. The pose graph used in this example is taken from the MIT Dataset and was generated using information extracted from a parking garage.

Load the pose graph from the MIT dataset. Inspect the poseGraph3D object to view the number of nodes and loop closures.

load parking-garage-posegraph.mat pg
disp(pg);
  poseGraph3D with properties:

               NumNodes: 1661
               NumEdges: 6275
    NumLoopClosureEdges: 4615
     LoopClosureEdgeIDs: [1x4615 double]

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

title('Original Pose Graph')
show(pg,'IDs','off');
view(-30,45)

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
title('Updated Pose Graph')
show(updatedPG,'IDs','off');
view(-30,45)

References

[1] Carlone, Luca, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. "Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization." 2015 IEEE International Conference on Robotics and Automation (ICRA). 2015, pp. 4597–4604.

Extended Capabilities

See Also

Functions

Objects

Introduced in R2019b

Navigation Toolbox Documentation

Support