Add relative pose to pose graph
creates an edge by specifying a relative pose measurement between existing nodes
edge is called a loop closure. If a loop closure already
exists, the function appends the new measurement. Calling the
optimizePoseGraph function combines multiple appended measurements
into a single edge. This syntax does not support adding edges to a landmark
measurement — Relative pose between nodes
[x y theta] vector |
[x y z qw qx qy qz] vector
Relative pose between nodes, specified as one of the following:
poseGraph (2-D), the pose is a
[x y theta]
vector, which defines a xy-position and orientation
poseGraph3D, the pose is a
[x y z qw qx qy
qz] vector, which defines by an
xyz-position and quaternion orientation,
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
infoMat — Information matrix
6-element vector | 21-element vector
Information matrices, specified in compact form as a 6-element vector or 21-element vector.
Each row is the upper triangle of the square information matrix. An
information matrix represents the uncertainty of the measurement. The matrix
is calculated as the inverse of the covariance. If the measurement is an
[x y theta] vector, the covariance matrix is a 3-by-3
of pairwise covariance calculations. Typically, the uncertainty is
determined by the sensor model.
poseGraph (2-D), each information matrix is a six-element
vector. The default is
[1 0 0 1 0 1]. For landmark nodes,
the last three elements are returned as
poseGraph3D, each 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
nodePair — Edge node pair in pose graph
Edge node pairs in pose graph, returned as two-element vector that lists the IDs of the two nodes that each edge connects. Multiple edges may exist between the same pair of nodes.
edgeID — ID of added edge
ID of added edge, returned as a positive integer.
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Use this syntax when constructing
poseGraph3D objects for code generation:
poseGraph('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.