factorPoseSE3Prior

Full-state prior factor for SE(3) pose

Since R2022a

Description

The `factorPoseSE3Prior` object is a full-state prior factor for an SE(3) state space pose for a `factorGraph` object.

Creation

Syntax

``F = factorPoseSE3Prior(nodeID)``
``F = factorPoseSE3Prior(nodeID,Name=Value)``

Description

example

````F = factorPoseSE3Prior(nodeID)` creates a `factorPoseSE3Prior` object, `F`, with the node identification numbers property `NodeID` set to `nodeID`.```
````F = factorPoseSE3Prior(nodeID,Name=Value)` specifies properties using one or more name-value arguments. For example, `factorPoseSE3Prior(1,Measurement=[1 2 3 4 5 6 7])` sets the `Measurement` property of the `factorPoseSE3Prior` object to `[1 2 3 4 5 6 7]`.```

Properties

expand all

Node ID numbers, specified as an N-element vector of nonnegative integers, where N is the total number of desired factors. Each element represents a factor that connects to a node of type `POSE_SE3` in the factor graph using the specified node ID.

If a factor in the `factorPoseSE3Prior` object specifies ID that does not correspond to a node in the factor graph, the factor graph automatically creates an `POSE_SE3` type node with that ID and adds it to the factor graph when adding the factor to the factor graph.

You must specify this property at object creation.

For more information about the expected node types of all supported factors, see Expected Node Types of Factor Objects.

Measured absolute SE(3) prior pose in local coordinates, specified as an N-by-7 matrix, where each row is of the form [x y z qw qx qy qz]. N is the total number of factors. x, y, and z are the position measurements. qw, qx, qy, and qz are the quaternion rotation measurements.

This measurement provides an initial node state for the specified nodes during optimization.

The specified quaternion is expected to be normalized.

Information matrices associated with the measurements, specified as a 6-by-6 matrix or a 6-by-6-by-N array. N is the total number of factors specified by the `factorPoseSE3Prior` object. Each information matrix corresponds to the measurements of the corresponding node in `NodeID`.

If you specify this property as a 6-by-6 matrix when `NodeID` contains more than one element, the information matrix corresponds to all measurements in `Measurement`.

Object Functions

 `nodeType` Get node type of node in factor graph

Examples

collapse all

Create a prior SE(3) pose factor with a node ID of `1`.

`f = factorPoseSE3Prior(1);`

Create a default factor graph and add the factor to the graph using the `addFactor` function.

```g = factorGraph; addFactor(g,f);```