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.
For more information about factor graph and how to use it, see Factor Graph for SLAM.
Creation
Syntax
Description
creates an empty
fg
= factorGraphfactorGraph
object.
Properties
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"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.
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 likematchScans
.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 likepcregistericp
(Computer Vision Toolbox) andestrelpose
(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 theestgeotform3d
(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, or velocity 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.
Object Functions
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 |
addFactor | Add factor to factor graph |
removeFactor | Remove factor from factor graph |
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 |
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 |
Examples
Create a matrix of positions of the landmarks to use for localization, and the real poses of the robot to compare your factor graph estimate against. Use the exampleHelperPlotGroundTruth
helper function to visualize the landmark points and the real path of the robot.
gndtruth = [0 0 0; 2 0 pi/2; 2 2 pi; 0 2 pi]; landmarks = [3 0; 0 3]; exampleHelperPlotGroundTruth(gndtruth,landmarks)
Use the exampleHelperSimpleFourPoseGraph
helper function to create a factor graph contains four poses related by three 2-D two-pose factors. For more details, see the factorTwoPoseSE2
object page.
fg = exampleHelperSimpleFourPoseGraph(gndtruth);
Create Landmark Factors
Generate node IDs to create two node IDs for two landmarks. The second and third pose nodes observe the first landmark point so they should connect to that landmark with a factor. The third and fourth pose nodes observe the second landmark.
lmIDs = generateNodeID(fg,2); lmFIDs = [1 lmIDs(1); % Pose Node 1 <-> Landmark 1 2 lmIDs(1); % Pose Node 2 <-> Landmark 1 2 lmIDs(2); % Pose Node 2 <-> Landmark 2 3 lmIDs(2)]; % Pose Node 3 <-> Landmark 2
Define the relative position measurements between the position of the poses and their landmarks in the reference frame of the pose node. Then add some noise.
lmFMeasure = [0 -1; % Landmark 1 in pose node 1 reference frame -1 2; % Landmark 1 in pose node 2 reference frame 2 -1; % Landmark 2 in pose node 2 reference frame 0 -1]; % Landmark 2 in pose node 3 reference frame lmFMeasure = lmFMeasure + 0.1*rand(4,2);
Create the landmark factors with those relative measurements and add it to the factor graph.
lmFactor = factorPoseSE2AndPointXY(lmFIDs,Measurement=lmFMeasure); addFactor(fg,lmFactor);
Set the initial state of the landmark nodes to the real position of the landmarks with some noise.
nodeState(fg,lmIDs,landmarks+0.1*rand(2));
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.
rng default
optimize(fg)
ans = struct with fields:
InitialCost: 0.0538
FinalCost: 6.2053e-04
NumSuccessfulSteps: 4
NumUnsuccessfulSteps: 0
TotalTime: 0.0025
TerminationType: 0
IsSolutionUsable: 1
OptimizedNodeIDs: [1 2 3 4 5]
FixedNodeIDs: 0
Visualize and Compare Results
Get and store the updated node states for the robot and landmarks. Then 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_SE2")
poseIDs = 1×4
0 1 2 3
poseStatesOpt = nodeState(fg,poseIDs)
poseStatesOpt = 4×3
0 0 0
2.0815 0.0913 1.5986
1.9509 2.1910 -3.0651
-0.0457 2.0354 -2.9792
landmarkStatesOpt = nodeState(fg,lmIDs)
landmarkStatesOpt = 2×2
3.0031 0.1844
-0.1893 2.9547
handle = show(fg,Orientation="on",OrientationFrameSize=0.5,Legend="on"); grid on; hold on; exampleHelperPlotGroundTruth(gndtruth,landmarks,handle);
More About
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"] |
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 thegenerateNodeID
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 thenodeState
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
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.
Version History
Introduced in R2022aUse the marginalizeFactor
and marginalizeNode
functions to marginalize factors and nodes in a factor graph. This enables you to optimize
larger factor graphs more efficiently.
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.
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.
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.
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.
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.
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.
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:
These factor objects now support specifying multiple factors at a time by specifying more than one set of node IDs:
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.
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.
See Also
Objects
Functions
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Website auswählen
Wählen Sie eine Website aus, um übersetzte Inhalte (sofern verfügbar) sowie lokale Veranstaltungen und Angebote anzuzeigen. Auf der Grundlage Ihres Standorts empfehlen wir Ihnen die folgende Auswahl: .
Sie können auch eine Website aus der folgenden Liste auswählen:
So erhalten Sie die bestmögliche Leistung auf der Website
Wählen Sie für die bestmögliche Website-Leistung die Website für China (auf Chinesisch oder Englisch). Andere landesspezifische Websites von MathWorks sind für Besuche von Ihrem Standort aus nicht optimiert.
Amerika
- América Latina (Español)
- Canada (English)
- United States (English)
Europa
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)