# factorGraph

## 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:

Create an empty

`factorGraph`

object.For each desired factor type.

Generate node IDs using the

`generateNodeID`

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

Add factors to the factor graph using the

`addFactor`

object function. If there is no node in the factor graph with the specified ID, a node with that ID is automatically created and added to the factor graph when this factor is added to the factor graph. If there is a node in the factor graph with the specified ID, ensure that adding the new factor does not cause a node type mismatch. For more information, see Tips.

Check if all the nodes in the factor graph are connected to at least one other node using the

`isConnected`

object function.Create a

`factorGraphSolverOptions`

object to specify factor graph solver options.Optimize the factor graph using the

`optimize`

object function with the desired factor graph solver options.Extract factor graph node data such as node IDs and node states using the

`nodeIDs`

and`nodeState`

object functions.

## Creation

### Syntax

### Description

creates an empty
`graph`

= factorGraph`factorGraph`

object.

## Properties

## 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

## 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);`

If you constructed a factor graph containing SE(2) robot poses, you can get the states of all the pose nodes by first using the

`nodeIDs`

function and specifying the node type as`"POSE_SE2"`

. 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. These are the node types that the

`NodeID`

property of each factor object specifies and connects to:Factor Object Expected Node Types of Specified Node IDs `factorGPS`

`["POSE_SE3"]`

`factorIMU`

`["POSE_SE3","VEL3","IMU_BIAS","POSE_SE3","VEL3","IMU_BIAS"]`

`factorCameraSE3AndPointXYZ`

`["POSE_SE3","POINT_XYZ"]`

`factorPoseSE2AndPointXY`

`["POSE_SE2","POINT_XY"]`

`factorPoseSE3AndPointXYZ`

`["POSE_SE3","POINT_XYZ"]`

`factorTwoPoseSE2`

`["POSE_SE2","POSE_SE2"]`

`factorTwoPoseSE3`

`["POSE_SE3","POSE_SE3"]`

`factorIMUBiasPrior`

`["IMU_BIAS"]`

`factorPoseSE3Prior`

`["POSE_SE3"]`

`factorVelocity3Prior`

`["VEL_3"]`

For example,

`factorPoseSE2AndPointXY([1 2])`

creates a 2-D landmark factor connecting to node IDs 1 and 2. If you try to add that factor to a factor graph that already contains nodes 1 and 2, the factor expects nodes 1 and 2 to be of types`"POSE_SE2"`

and`"POINT_XY"`

, respectively.

## References

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

## Extended Capabilities

## Version History

**Introduced in R2022a**