Add relative pose to pose graph

## Syntax

``addRelativePose(poseGraph,relPose)``
``addRelativePose(poseGraph,relPose,infoMatrix)``
``addRelativePose(poseGraph,relPose,infoMatrix,fromNodeID)``
``addRelativePose(poseGraph,relPose,infoMatrix,fromNodeID,toNodeID)``
``[edge,edgeID] = addRelativePose(___)``

## Description

``` `addRelativePose(poseGraph,relPose)` creates a pose node and uses an edge specified by `relPose` to connect it to the last node in the pose graph.```
``` `addRelativePose(poseGraph,relPose,infoMatrix)` also specifies the information matrix as part of the edge constraint, which represents the uncertainty of the pose measurement.```
``` `addRelativePose(poseGraph,relPose,infoMatrix,fromNodeID)` creates a new pose node and connects it to the specific node specified by `fromNodeID`.```
``` `addRelativePose(poseGraph,relPose,infoMatrix,fromNodeID,toNodeID)` creates an edge by specifying a relative pose between existing nodes specified by `fromNodeID` and `toNodeID`. This edge is called a loop closure.```
``` `[edge,edgeID] = addRelativePose(___)` returns the newly added edge and edge ID using any of the previous syntaxes. ```

## Input Arguments

collapse all

Pose graph, specified as a `poseGraph` or `poseGraph3D` object.

Relative pose between nodes, specified as one of the following:

For `poseGraph` (2-D), the pose is a `[x y theta]` vector, which defines a xy-position and orientation angle, `theta`.

For `poseGraph3D`, the pose is a ```[x y z qw qx qy qz]``` vector, which defines by an xyz-position and quaternion orientation, ```[qw qx qy qz]```

Note

Many other sources for 3-D pose graphs, including `.g2o` formats, specify the quaternion orientation in a different order, for example, `[qx qy qz qw]`. Check the source of your pose graph data before adding nodes to your `poseGraph3D` object.

Information matrix, specified as a 6-element or 21-element vector. This vector contains the elements of the upper triangle of the square information matrix (compact form). The information matrix is the inverse of the covariance of the pose and represents the uncertainty of the measurement. If the pose vector is `[x y theta]`, the covariance is a 3-by-3 matrix of pairwise covariance calculations. Typically, the uncertainty is determined by the sensor model.

For `poseGraph` (2-D), the information matrix is a six-element vector. The default is `[1 0 0 1 0 1]`.

For `poseGraph3D`, the information matrix is a 21-element vector. The default is ```[1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1]```.

Node to attach from, specified as a positive integer. This integer corresponds to the node ID of a node in `poseGraph`. When specified without `toNodeID`, `addRelativePose` creates a new node and adds an edge between the new node and the `fromNodeID` node.

Node to attach to, specified as a positive integer. This integer corresponds to the node ID of a node in `poseGraph`. `addRelativePose` adds an edge between this node and the `fromNodeID` node.

## Output Arguments

collapse all

Added edge, returned as a two-element vector. An edge is defined by the IDs of the two nodes that it connects with a relative pose.

ID of added edge, returned as a positive integer.

## Extended Capabilities

Introduced in R2019b