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`

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

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

`addRelativePose` | Add relative pose to pose graph |

`edges` | Edges in pose graph |

`edgeConstraints` | Edge constraints in pose graph |

`edgeResidualErrors` | Compute pose graph edge residual errors |

`findEdgeID` | Find edge ID of edge |

`nodes` | Poses of nodes in pose graph |

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