Main Content

Create 3-D pose graph

A `poseGraph3D`

object stores information for a 3-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.

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.

To add landmark point nodes, use the `addPointLandmark`

function. This function specifies nodes as
*xyz*-points without orientation estimates. Landmarks must be
specified relative to a pose node.

For 2-D pose graphs, see `poseGraph`

.

For an example that builds and optimizes a 3-D pose graph from real-world sensor data, see Landmark SLAM Using AprilTag Markers.

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

`addPointLandmark` | Add landmark point node to pose graph |

`addRelativePose` | Add relative pose to pose graph |

`copy` | Create copy of pose graph |

`edgeNodePairs` | Edge node pairs in pose graph |

`edgeConstraints` | Edge constraints in pose graph |

`edgeResidualErrors` | Compute pose graph edge residual errors |

`findEdgeID` | Find edge ID of edge |

`nodeEstimates` | Poses of nodes in pose graph |

`removeEdges` | Remove loop closure edges from graph |

`show` | Plot pose graph |

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