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

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 node type is set by the factor graph when the factor object that specifies the node is added to the factor graph. You can not change the node type of a node after it has been added 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:

PurposeFactor Objects
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 two 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 a 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 matrix of positions of the landmarks to use for localization, and the real positions of the robot to compare your factor graph estimate against. Use the `exampleHelperPlotPositionsAndLandmarks` helper function to visualize the landmark points and the real path of the robot..

```landmarks = [0 -3 0; 3 4 0; 7 1 0]; realpos = [0 0 0; 2 -2 0; 5 3 0; 10 2 0]; exampleHelperPlotPositionsAndLandmarks(realpos,landmarks)``` Create Robot Pose Nodes

Create a factor graph, and add a prior factor to loosely fix the start pose of the robot by providing an estimate pose.

```fg = factorGraph; rng(1) pf = factorPoseSE3Prior(0);```

Generate node IDs to use to create three `factorTwoPoseSE3` relative pose factors that relate four robot poses. To simulate sensor readings for the measurements of each factor, take the difference between a consecutive pair of ground truth positions, add noise, and append a quaternion of zero to provide a rotation of zero. Then add the prior factor and the pose factors to the factor graph.

```zeroQuat = [1 0 0 0]; rpfIDs = generateNodeID(fg,3,"factorTwoPoseSE3")```
```rpfIDs = 3×2 0 1 1 2 2 3 ```
```rpfmeasure = [(diff(realpos) + 0.1*rand(3)) repmat(zeroQuat,3,1)]; rpf = factorTwoPoseSE3(rpfIDs,Measurement=rpfmeasure); addFactor(fg,pf); addFactor(fg,rpf);```

Create Landmark Factors

Generate node IDs to create three `factorPoseSE3AndXYZ` landmark factor objects that relate to the pose nodes. The first and second pose nodes observe the first landmark point so they should connect to that landmark with a factor. The second and third pose nodes observe the second landmark. The third and fourth pose nodes observe the third landmark.

`landmarkIDs = generateNodeID(fg,3)'`
```landmarkIDs = 3×1 4 5 6 ```

The landmark factors used here are for 3-D state space but the process is identical for landmark factors for 2-D state space. Add some random number to the relative position between the landmark and the ground truth position to simulate real sensor measurements. Then create the landmark factors and add them to the factor graph.

```lmf1measure = [landmarks(1,:) - realpos(1:2,:)] + 0.5*rand(1,3); lmf2measure = [landmarks(2,:) - realpos(2:3,:)] + 0.5*rand(1,3); lmf3measure = [landmarks(3,:) - realpos(3:4,:)] + 0.5*rand(1,3); lmf1 = factorPoseSE3AndPointXYZ([[0 1]' repmat(landmarkIDs(1),2,1)],Measurement=lmf1measure); lmf2 = factorPoseSE3AndPointXYZ([[1 2]' repmat(landmarkIDs(2),2,1)],Measurement=lmf2measure); lmf3 = factorPoseSE3AndPointXYZ([[2 3]' repmat(landmarkIDs(3),2,1)],Measurement=lmf3measure); addFactor(fg,lmf1); addFactor(fg,lmf2); addFactor(fg,lmf3);```

Optimize Factor Graph

Optimize the factor graph with the default solver options. The optimization updates the states of all nodes in the factor graph, so the positions of vehicle and the landmarks update.

```fgso = factorGraphSolverOptions; optimize(fg,fgso)```
```ans = struct with fields: InitialCost: 72.6129 FinalCost: 0.0011 NumSuccessfulSteps: 4 NumUnsuccessfulSteps: 0 TotalTime: 4.2391e-04 TerminationType: 0 IsSolutionUsable: 1 ```

Visualize and Compare Results

Get and store the updated node states for the vehicle and landmarks and plot the results, comparing the factor graph estimate of the robot path to the known ground truth of the robot.

```poseIDs = nodeIDs(fg,NodeType="POSE_SE3"); fgposopt = nodeState(fg,poseIDs)```
```fgposopt = 4×7 0.0000 0.0000 0.0000 1.0000 0.0000 -0.0000 0.0000 2.0278 -1.9778 0.0173 1.0000 0.0018 -0.0034 0.0014 5.0684 3.0500 0.0871 0.9999 -0.0010 -0.0072 0.0089 10.0844 2.1475 0.1972 0.9999 0.0006 -0.0121 0.0100 ```
```fglmopt = nodeState(fg,landmarkIDs); exampleHelperPlotPositionsAndLandmarks(realpos,landmarks,fgposopt,fglmopt)``` ## 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 ObjectExpected 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.

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