Main Content

# addFactor

Add factor to factor graph

Since R2022a

## Syntax

``factorIDs = addFactor(fg,factor)``
``factorIDs = addFactor(fg,factor,groupID)``

## Description

The `addFactor` function adds one or more factors to a factor graph and can group the factors and nodes that are specified by the added factors.

example

````factorIDs = addFactor(fg,factor)` adds one or more factors to the specified factor graph and returns the IDs 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. ```

example

````factorIDs = addFactor(fg,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. You can use group IDs to represent timestamps or frames.```

## Examples

collapse all

Create a factor graph.

`fg = factorGraph;`

Define two approximate pose states of the robot.

```rstate = [0 0 0; 1 1 pi/2];```

Define the relative pose measurement between two nodes from the odometry as the pose difference between the states with some noise. The relative measurement must be in the reference frame of the second node so you must rotate the difference in position to be in the reference frame of the second node.

```posediff = diff(rstate); rotdiffso2 = so2(posediff(3),"theta"); transformedPos = transform(inv(rotdiffso2),posediff(1:2)); odomNoise = 0.1*rand; measure = [transformedPos posediff(3)] + odomNoise;```

Create a SE(2) two-pose factor with the relative measurement. Then add the factor to the factor graph to create two nodes.

```ids = generateNodeID(fg,1,"factorTwoPoseSE2"); f = factorTwoPoseSE2(ids,Measurement=measure); addFactor(fg,f);```

Get the state of both pose nodes.

`stateDefault = nodeState(fg,ids)`
```stateDefault = 2×3 0 0 0 0 0 0 ```

Because these nodes are new, they have default state values. Ideally before optimizing, you should assign an approximate guess of the absolute pose. This increases the possibility of the `optimize` function finding the global minimum. Otherwise `optimize` may become trapped in the local minimum, producing a suboptimal solution.

Keep the first node state at the origin and set the second node state to an approximate xy-position at `[0.9 0.95]` and a theta rotation of `pi/3` radians. In practical applications you could use sensor measurements from your odometry to determine the approximate state of each pose node.

`nodeState(fg,ids(2),rstate(2,:))`
```ans = 1×3 1.0000 1.0000 1.5708 ```

Before optimizing, save the node state so you can reoptimize as needed.

`statePriorOpt1 = nodeState(fg,ids);`

Optimize the nodes and check the node states.

```optimize(fg); stateOpt1 = nodeState(fg,ids)```
```stateOpt1 = 2×3 -0.1038 0.8725 0.1512 1.1038 0.1275 1.8035 ```

Note that after optimization the first node did not stay at the origin because although the graph does have the initial guess for the state, the graph does not have any constraint on the absolute position. The graph has only the relative pose measurement, which acts as a constraint for the relative pose between the two nodes. So the graph attempts to reduce the cost related to the relative pose, but not the absolute pose. To provide more information to the graph, you can fix the state of nodes or add an absolute prior measurement factor.

Reset the states and then fix the first node. Then verify that the first node is fixed.

```nodeState(fg,ids,statePriorOpt1); fixNode(fg,ids(1)) isNodeFixed(fg,ids(1))```
```ans = logical 1 ```

Reoptimize the factor graph and get the node states.

`optimize(fg)`
```ans = struct with fields: InitialCost: 1.8470 FinalCost: 1.8470e-16 NumSuccessfulSteps: 2 NumUnsuccessfulSteps: 0 TotalTime: 1.4591e-04 TerminationType: 0 IsSolutionUsable: 1 OptimizedNodeIDs: 1 FixedNodeIDs: 0 ```
`stateOpt2 = nodeState(fg,ids)`
```stateOpt2 = 2×3 0 0 0 1.0815 -0.9185 1.6523 ```

Note that after optimizing this time, the first node state remained at the origin.

Create a factor graph, generate node IDs, and create two `factorTwoPoseSE2` factors.

```fg1 = factorGraph; ids = generateNodeID(fg1,[2 2])```
```ids = 2×2 0 1 2 3 ```
`f = factorTwoPoseSE2(ids);`

Group All Nodes

Add all nodes of the factors to group 1.

```addFactor(fg1,f,1); fg1Group1 = nodeIDs(fg1,GroupID=1)```
```fg1Group1 = 1×4 0 1 2 3 ```

Group Nodes by Column

Specify the group ID as a row vector to add the nodes of the first column of the node IDs of the factors to group 1 and add the nodes of the second column to group 2.

```fg2 = factorGraph; addFactor(fg2,f,[1 2]); fg2Group1 = nodeIDs(fg2,GroupID=1)```
```fg2Group1 = 1×2 0 2 ```
`fg2Group2 = nodeIDs(fg2,GroupID=2)`
```fg2Group2 = 1×2 1 3 ```

Group Nodes by Row

Specify the group ID as a column vector to add the nodes of the first row in the node IDs of the factors to group 1 and add the nodes of the second row to group 2.

```fg3 = factorGraph; addFactor(fg3,f,[1; 2]); fg3Group1 = nodeIDs(fg3,GroupID=1)```
```fg3Group1 = 1×2 0 1 ```
`fg3Group2 = nodeIDs(fg3,GroupID=2)`
```fg3Group2 = 1×2 2 3 ```

Group Nodes by Matrix

You can also specify the group ID as a matrix of the same size as the node IDs of the factors to assign each node to a specific group. Add the first and fourth nodes to group 1 and the second and third nodes to groups 2 and 3, respectively.

```fg4 = factorGraph; groups = [1 2; 3 1]; addFactor(fg4,f,groups); fg4Group1 = nodeIDs(fg4,GroupID=1)```
```fg4Group1 = 1×2 0 3 ```
`fg4Group2 = nodeIDs(fg4,GroupID=2)`
```fg4Group2 = 1 ```
`fg4Group3 = nodeIDs(fg4,GroupID=3)`
```fg4Group3 = 2 ```

## Input Arguments

collapse all

Factor graph to add factor to, specified as a `factorGraph` 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:

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.

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

`groupID` SizeGrouping 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

collapse all

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.

## Version History

Introduced in R2022a

expand all