addFactor

Add factor to factor graph

Since R2022a

Syntax

factorIDs = addFactor(graph,factor)

factorIDs = addFactor(graph,factor,groupID)

Description

factorIDs = addFactor(graph,factor) adds one or more factors, factor, to the specified factorGraph object graph and returns the IDs factorIDs of the added factors.

If adding the factors results in an invalid node, then addFactor returns an error, and indicates the invalid nodes.

Note

addFactor supports only single-factor addition for the factorIMU and factorGPS objects.

factorIDs = addFactor(graph,factor,groupID) adds a factor to the factor graph with group ID groupID. Node IDs of the same group can be retrieved by group ID using nodeIDs function.

Input Arguments

graph — Factor graph to add factor to

factorGraph object

Factor graph to add factor to, specified as a factorGraph object.

factor — Factors to add to factor graph

valid factor object

Factors to add to the factor graph, specified as a valid factor object.

A valid factor object must be one of these objects, and the object must not create any invalid nodes when added to the factor graph:

• factorGPS

• factorIMU

• factorCameraSE3AndPointXYZ

• factorPoseSE2AndPointXY

• factorPoseSE3AndPointXYZ

• factorTwoPoseSE2

• factorTwoPoseSE3

• factorIMUBiasPrior

• factorPoseSE3Prior

• factorVelocity3Prior

With the exception of factorGPS and factorIMU, you can simultaneously add multiple factors to the factor graph using any one of the listed factor objects. factorGPS and factorIMU support only single-factor addition.

If the specified factor object creates any invalid nodes, then addFactor adds none of the factors from the factor object.

groupID — Group IDs for nodes of added factor

nonnegative integer | two-element row vector of nonnegative integers | N-element column vector of nonnegative integers | N-by-2 matrix of nonnegative integers

Group IDs for nodes of the added factor, specified as any of these options:

groupID Size	Grouping Behavior
nonnegative integer	Assigns all nodes to one group. For example, if you add a factor object that has a NodeID value of $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ with a groupID value of 1, addFactor adds nodes 1, 2, 3, and 4 to group 1.
two-element row vector of nonnegative integers	Specify groups for each column of nodes. For example, if you add a factor object that has a NodeID value of $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ with a groupID value of $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor adds nodes 1 and 3 to group 1 and adds nodes 2 and 4 to group 2. The behavior for IMU factors is different. If you add an IMU factor with a NodeID value of $$\left[\begin{array}{cccccc}1& 2& 3& 4& 5& 6\end{array}\right]$$ and groupID set to $$\left[\begin{array}{cc}1& 2\end{array}\right]$$, addFactor adds nodes 1, 2, and 3 to group 1 and nodes 4, 5, and 6 to group 2.
N-element column vector of nonnegative integers	Group nodes by factor, where N is the total number of factors specified by the NodeID property of factor. For example, if you add a factor object that has a NodeID value of $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ with a groupID value of $$\left[\begin{array}{c}1\\ 2\end{array}\right]$$, addFactor adds nodes 1 and 2 to group 1 and adds nodes 3 and 4 to group 2.
N-by-2 matrix of nonnegative integers	Add nodes in NodeID to the group specified at its corresponding index in groupID, where N is the total number of rows of the NodeID property of factor. For example, if you add a factor object that has a NodeID value of $$\left[\begin{array}{cc}1& 2\\ 3& 4\end{array}\right]$$ with a groupID value of $$\left[\begin{array}{cc}1& 2\\ 3& 1\end{array}\right]$$, addFactor add nodes 1 and 4 into group 1, adds node 2 to group 2, and adds node 3 to group 3.

Note

When adding a factorIMU or factorGPS object to a factor graph, groupID accepts only these values:

• factorIMU — Nonnegative integer or a two-element row vector of nonnegative integers.

• factorGPS — Nonnegative integer

Adding nodes to groups enables you to query node IDs by group by specifying the GroupID name-value argument of the nodeIDs function.

Output Arguments

factorIDs — Factor IDs of added factors

N-element row vector of nonnegative integers

Factor IDs of the added factors, returned as an N-element row vector of nonnegative integers. N is the total number of factors added.

The function returns this argument only when it successfully adds the factors to the factor graph. If adding the factors results in an invalid node, then addFactor adds none of the factors from the factor object.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a

R2023a: Specify group IDs for added factors

addFactor now supports specifying groups to add nodes of added factors to by group ID.