addRelativePose

Add relative pose to pose graph

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