# factorGraph

Bipartite graph of factors and nodes

Since R2022a

## Description

A `factorGraph` object stores a bipartite graph consisting of factors connected to variable nodes. The nodes represent the unknown random variables in an estimation problem, such as robot poses, and the factors represent probabilistic constraints on those nodes, derived from measurements or prior knowledge. During optimization, the factor graph uses all the factors and current node states to update the node states.

To use the factor graph:

1. Create an empty `factorGraph` object.

2. For each desired factor type:

1. Generate node IDs using the `generateNodeID` object function.

2. Define factors with the desired node IDs, using any of the supported factor objects:

3. Add factors to the factor graph using the `addFactor` object function. If the factor graph does not contain a node with the specified ID, the function automatically creates a node with that ID and adds it to the factor graph when adding the factor to the factor graph. If the factor graph contains a node with the specified ID, ensure that adding the new factor does not cause a node type mismatch. For more information, see Tips. For a list of expected node types for each factor, see Expected Node Types of Factor Objects.

3. Check if all the nodes in the factor graph are connected to at least one other node using the `isConnected` object function.

4. Create a `factorGraphSolverOptions` object to specify factor graph solver options.

5. Optimize the factor graph using the `optimize` object function with the desired factor graph solver options.

6. Extract factor graph node data, such as node IDs and node states, using the `nodeIDs` and `nodeState` object functions.

## Creation

### Syntax

``graph = factorGraph``

### Description

example

````graph = factorGraph` creates an empty `factorGraph` object.```

## Properties

expand all

Number of nodes in the factor graph, specified as a positive integer. `NumNodes` has a value of `0` when the factor graph is empty and `NumNodes` increases each time you add a factor that specifies new node IDs to the factor graph.

The nodes in the factor graph can be any of these types:

• `"POSE_SE2"` — Pose in SE(2) state space

• `"POSE_SE3"` — Pose in SE(3) state space

• `"VEL3"` — 3-D velocity

• `"POINT_XY"` — 2-D point

• `"POINT_XYZ"` — 3-D point

• `"IMU_BIAS"` — IMU gyroscope and accelerometer bias

To check the node type of a node in the graph, use the `nodeType` function.

Note

The factor graph sets the node type when you add the factor object that specifies that node to the factor graph. You cannot change the node type of a node after you add it to the graph.

Number of factors in the factor graph, specified as a positive integer. `NumFactors` has a value of `0` when the factor graph is empty and `NumFactors` increases each time you add a factor to the factor graph.

You can use `addfactor` to add any of these factor objects to the factor graph:

Relate Poses to Sensor Measurements

• `factorGPS` — Connect SE(3) pose node (`"POSE_SE3"`) to a GPS measurement.

• `factorIMU` — Connect two SE(3) pose nodes (`"POSE_SE3"`), two 3-D velocity nodes (`"VEL3"`), and two IMU bias nodes (`"IMU_BIAS"`) using an IMU measurement.

Relate Poses to Landmark Positions

• `factorCameraSE3AndPointXYZ` — Connect the SE(3) pose node of a pinhole camera (`"POSE_SE3"`) to 3-D landmark nodes (`"Point_XYZ"`) using relative pose measurements.

• `factorPoseSE2AndPointXY` — Connect a SE(2) pose node (`"POSE_SE2"`) to 2-D landmark nodes (`"Point_XY"`) using relative pose measurements.

• `factorPoseSE3AndPointXYZ` — Connect a SE(3) pose node (`"POSE_SE3"`) to 3-D landmark nodes (`"Point_XYZ"`) using relative pose measurements.

Relate Poses to Each Other

• `factorTwoPoseSE2` — Connect pairs of SE(2) pose nodes (`"POSE_SE2"`) with relative poses using relative pose measurements.

• `factorTwoPoseSE3` — Connect pairs of SE(3) pose nodes (`"POSE_SE3"`) with relative poses using relative pose measurements.

Relate Poses or Velocities to Prior-Known Measurements

• `factorIMUBiasPrior` — Connect SE(3) pose nodes (`"POSE_SE3"`), 3-D velocity nodes (`"VEL3"`), and IMU bias nodes (`"IMU_BIAS"`) to prior-known IMU measurements.

• `factorPoseSE3Prior` — Connect SE(3) pose nodes (`"POSE_SE3"`) to prior-known SE(3) pose measurements.

• `factorVelocity3Prior` — Connect 3-D velocity node (`"VEL_3"`) to prior-known SE(3) velocity measurements.

## Object Functions

 `addFactor` Add factor to factor graph `fixNode` Fix or free nodes in factor graph `generateNodeID` Generate new node IDs `hasNode` Check if node ID exists in factor graph `isConnected` Check if factor graph is connected `isNodeFixed` Check if node is fixed `nodeIDs` Get node IDs in factor graph `nodeState` Get or set node state in factor graph `nodeType` Get node type of node in factor graph `optimize` Optimize factor graph

## 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.3900e-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.

expand all

## Tips

• To specify multiple factors and nodes at once for a specific factor type, use the `generateNodeID` function and specify the number of factors and the factor type. See the `generateNodeID` function for more details.

```poseIDPairs = generateNodeID(fg,3,"factorTwoPoseSE2"); ftpse2 = factorTwoPoseSE2(poseIDPairs);```
• You can get the states of all the pose nodes by first using the `nodeIDs` function and specifying the node type as `"POSE_SE2"` for SE(2) robot poses and `"POSE_SE3"` for SE(3) robot poses. Then, use the `nodeState` function with those node IDs to get the node states of the robot pose nodes.

```poseIDs = nodeIDs(fg,NodeType="POSE_SE2"); poseStates = nodeState(fg,poseIDs);```
• Check the types of nodes that each factor creates or connects to before adding factors to the factor graph to avoid node type mismatch errors. For a list of expected node types for each factor, see Expected Node Types of Factor Objects.

 Dellaert, Frank. Factor graphs and GTSAM: A Hands-On Introduction. Georgia: Georgia Tech, September, 2012.