factorGraph

Graph-based framework for multisensor state estimation

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.

For more information about factor graph and how to use it, see Factor Graph for SLAM.

Creation

Syntax

fg = factorGraph

Description

fg = factorGraph creates an empty factorGraph object.

Properties

NumNodes — Number of nodes in factor graph

Read-only: nonnegative integer

This property is read-only.

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

• "POSE_SE3_SCALE" — Pose scale in SE(3) state space

• "TRANSFORM_SE3" — Sensor transform in SE(3) state space

• "GRAVITY_ROTATION" — Gravity rotation represented as quaternion

• "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.

NumFactors — Number of factors in factor graph

Read-only: nonnegative integer

This property is read-only.

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 Each Other

• factorTwoPoseSE2 — Connect pairs of SE(2) pose nodes ("POSE_SE2") using relative pose measurements. You can calculate the relative pose using 2-D lidar scan registration using functions like matchScans.

• factorTwoPoseSE3 — Connect pairs of SE(3) pose nodes ("POSE_SE3") using relative pose measurements. You can calculate the relative pose using 3-D lidar scan registration using functions like pcregistericp (Computer Vision Toolbox) and estrelpose (Computer Vision Toolbox).

• factorTwoPoseSIM3 — Connect pairs of SIM(3) pose nodes using relative pose measurements. A SIM(3) pose node is represented using a combination of an SE(3) pose node ("POSE_SE3") and an SE(3) pose scale node ("POSE_SE3_SCALE"). You can use the estgeotform3d (Computer Vision Toolbox) function to calculate the relative pose measurement for this factor.

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"), or the SE(3) pose node of a pinhole camera ("POSE_SE3"), 3-D landmark nodes ("Point_XYZ") , and an SE(3) sensor transform node ("TRANSFORM_SE3") using the 2-D image point measurements from the camera.

• factorPoseSE2AndPointXY — Connect an SE(2) pose node ("POSE_SE2") to 2-D landmark nodes ("POINT_XY") using 2-D position measurements in the reference frame of the pose node.

• factorPoseSE3AndPointXYZ — Connect an SE(3) pose node ("POSE_SE3") to 3-D landmark nodes ("POINT_XYZ") using 3-D position measurements in the reference frame of the pose node.

Relate Poses to Sensor Measurements

• factorGPS — Connect an 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 IMU measurements. These measurements consist of accelerometer and gyroscope data that capture changes in pose and velocity.

Relate Poses or Velocities to Prior-Known Measurements

If a pose, IMU bias, velocity or SE(3) state space transform is a known quantity, add one of these prior factors to softly fix the node states, enabling slight variations only if doing so significantly reduces the overall cost:

• 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 a 3-D velocity node ("VEL_3") to prior-known SE(3) velocity measurements.

• factorTransformSE3Prior — Connect SE(3) Sensor Transform node ("TRANSFORM_SE3") to prior-known SE(3) Sensor Transform measurements.

Object Functions

Node Management

generateNodeID	Generate new node IDs
hasNode	Check if node ID exists in factor graph
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
fixNode	Fix or free nodes in factor graph
isNodeFixed	Check if node is fixed
removeNode	Remove node from factor graph

Factor Management

addFactor	Add factor to factor graph
removeFactor	Remove factor from factor graph

Optimization and Marginalization

optimize	Optimize factor graph
marginalizeFactor	Marginalize factors from factor graph into marginal factor
marginalizeNode	Marginalize node and related factors from factor graph into marginal factor

Graph Analysis

isConnected	Check if factor graph is connected
nodeCovariance	Get state covariance of nodes in factor graph
show	Plot pose nodes, pose node edges, and landmark nodes of factor graph

Utilities

copy	Copy factor graph

Examples

More About

Expected Node Types of Factor Objects

The NodeID property of each factor object specifies and connects to these node types:

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"] or ["POSE_SE3","POINT_XYZ","TRANSFORM_SE3"]
factorPoseSE2AndPointXY	["POSE_SE2","POINT_XY"]
factorPoseSE3AndPointXYZ	["POSE_SE3","POINT_XYZ"]
factorTwoPoseSE2	["POSE_SE2","POSE_SE2"]
factorTwoPoseSE3	["POSE_SE3","POSE_SE3"]
factorTwoPoseSIM3	["POSE_SE3","POSE_SE3_SCALE","POSE_SE3","POSE_SE3_SCALE"]
factorIMUBiasPrior	["IMU_BIAS"]
factorPoseSE3Prior	["POSE_SE3"]
factorVelocity3Prior	["VEL_3"]
factorTransformSE3Prior	["TRANSFORM_SE3"]

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 with the IDs 1 and 2, the factor expects nodes 1 and 2 to be of types "POSE_SE2" and "POINT_XY", respectively.

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.

References

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

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• When generating portable C code with a C++ compiler, you must specify hierarchical packing with non-minimal headers. For more information on packaging options, see the packNGo (MATLAB Coder) function.

• When building the code generated from a factor graph object, you must specify C++17 language standard or newer, using coder configuration parameters and the mex C++ compiler settings.

  • For information about specifying the configuration parameters of MATLAB^® Coder™, see Change Language Standard Used for Code Generation (MATLAB Coder).

  • For specifying the language and language standard in Simulink^® Coder, you can use the Language and Language standard parameters, respectively.

  • For information about specifying the mex C++ compiler, see Change Default Compiler. The specified mex compiler is used by the build processes of MATLAB Coder and Simulink Coder, unless the build approach configuration selects a specific compiler.

  • After generating code, if you relocate and build it in a different environment (for example, by using packNGo (MATLAB Coder) function), the toolchain in that environment must use C++ 17 or newer language standard.

Version History

Introduced in R2022a

R2026a: Copy, save, and load factor graphs

The factorGraph object now supports the copy, save, and load functions.

R2026a: Estimate and refine sensor transform for multi-sensor extrinsic calibration

You can now perform multi-sensor calibration using factor graph by estimating and refining sensor transform (the relative pose between sensors) between more sensors. The factorIMU, factorTwoPoseSE3, and factorPoseSE3AndPointXYZ objects now allow you to connect factors created from sensor measurements to an optimizable sensor transform node.

R2026a: Support new factor factorTransformSE3Prior and new node GRAVITY_ROTATION

You can now connect SE(3) sensor transform node ("TRANSFORM_SE3") to prior-known SE(3) sensor transform measurements using the prior factor object factorTransformSE3Prior.

You can now use the "GRAVITY_ROTATION" node to represent gravity rotation as a quaternion.

R2026a: Check existence and get types of multiple nodes

You can now use the hasNode and nodeType functions to check the existence or retrieve the types of multiple nodes in a factor graph by specifying a vector of node IDs as input.

R2025a: Marginalize factor graph for more efficient optimization of large factor graphs

Use the marginalizeFactor and marginalizeNode functions to marginalize factors and nodes in a factor graph. This enables you to optimize larger factor graphs more efficiently.

R2025a: Add 3-D similarity transformation factor

You can now add a 3-D similarity transformation factor, which connects two SIM(3) pose nodes, to the factor graph using the factorTwoPoseSIM3 object. A SIM(3) pose node is represented using a combination of an SE(3) pose node (POSE_SE3) and an SE(3) pose scale node (POSE_SE3_SCALE).

You can use the factorTwoPoseSIM3 object and POSE_SE3_SCALE nodes to perform similarity graph optimization. Use similarity graph optimization in a factor graph for scale drift reduction, scale refinement, and utilization of landmark factors and camera factors for optimization within the same graph.

R2025a: Estimate sensor transform for transforming poses to initial camera reference frame

Estimate or refine the SensorTransform property of the factorCameraSE3AndPointXYZ camera factor object using factor graph optimization, by connecting the sensor transform to the SE(3) camera pose and a 3-D landmark point.

R2024b: Retrieve node state covariance post-optimization

The factorGraph object can now estimate and store node state covariance during factor graph optimization. To enable this functionality, specify custom factor graph solver options and set the StateCovarianceType property to one or more node types for which to estimate and store the calculated state covariance. After optimizing with custom factor graph solver options, use the nodeCovariance function to get the stored state covariances.

R2024a: Visualize factor graphs

Use the show function to visualize pose and landmark nodes in a factor graph. This function is useful for visualizing the robot poses, the connections between robot poses, and the landmark positions after optimizing the factor graph.

R2024a: Remove nodes and factors

Use the removeNode and removeFactor functions to remove nodes and factors from a factor graph. You can use these functions to manage the size of larger factor graphs for applications where memory management is crucial. You can also use these functions to remove inaccurate measurements from a factor graph.

R2023b: Incrementally optimize factor graph

The optimize object function now accepts node IDs, enabling you to incrementally optimize a factor graph. This is useful for applications such as visual SLAM, which can require a factor graph containing hundreds of factors to relate poses to feature points. By limiting the factor graph optimization to a subset of the most recent pose nodes within a sliding window in the factor graph, you can maintain approximately constant solving time with a relatively high accuracy.

R2023a: Generate node IDs to simplify factor graph construction

Use the generateNodeID function to generate new sets of node IDs depending on your desired node ID format by giving set number and factor type.

These factor graph object functions also have additional functionality that enables you to more easily construct and modify factor graphs:

• addFactor — You can now specify group IDs to organize newly created nodes.

• nodeIDs — You can now get node IDs that have the same node type, group ID, or are associated with the same factor type.

• nodeState — You can now get or set the states of multiple nodes at the same time.

These factor objects now support specifying multiple factors at a time by specifying more than one set of node IDs:

• factorTwoPoseSE2

• factorTwoPoseSE3

• factorCameraSE3AndPointXYZ

• factorPoseSE2AndPointXY

• factorPoseSE3AndPointXYZ

• factorPoseSE3Prior

• factorIMUBiasPrior

• factorVelocity3Prior

• factorPoseSE3Prior

R2023a: Add camera factor to capture camera observations

Use the factorCameraSE3AndPointXYZ object to describe the visual projection factor relating the poses of the pinhole camera in the SE(3) state space and 3-D landmark points. You can add this object as a factor to the factorGraph object.

R2022b: Add landmark factors to relate landmark observations with poses

Use the factorPoseSE2AndPointXY and factorPoseSE3AndPointXYZ objects to relate poses to points in the SE(2) and SE(3) state spaces. These objects make it easier to localize using landmarks in factor graph SLAM workflows.