addFactor
Description
The addFactor
function adds one or more factors to a factor
graph and can group the factors and nodes that are specified by the added
factors.
Examples
Optimize Simple Factor Graph
Create a factor graph.
fg = factorGraph;
Define two pose states of the robot as the ground truth.
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 factor connecting two SE(2) pose with the relative measurment between the poses. 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 xyposition 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),[0.9 0.95 pi/3])
ans = 1×3
0.9000 0.9500 1.0472
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.1161 0.9026 0.0571
1.0161 0.0474 1.7094
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.9452
FinalCost: 1.9452e16
NumSuccessfulSteps: 2
NumUnsuccessfulSteps: 0
TotalTime: 8.1062e05
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.
Add Nodes to Groups
Create a factor graph, generate node IDs, and create two factorTwoPoseSE2
factors.
fg1 = factorGraph; ids = generateNodeID(fg1,[2 2])
ids = 2×2
0 1
2 3
f = factorTwoPoseSE2(ids);
Group All Nodes
Add all nodes of the factors to group 1.
addFactor(fg1,f,1); fg1Group1 = nodeIDs(fg1,GroupID=1)
fg1Group1 = 1×4
0 1 2 3
Group Nodes by Column
Specify the group ID as a row vector to add the nodes of the first column of the node IDs of the factors to group 1 and add the nodes of the second column to group 2.
fg2 = factorGraph; addFactor(fg2,f,[1 2]); fg2Group1 = nodeIDs(fg2,GroupID=1)
fg2Group1 = 1×2
0 2
fg2Group2 = nodeIDs(fg2,GroupID=2)
fg2Group2 = 1×2
1 3
Group Nodes by Row
Specify the group ID as a column vector to add the nodes of the first row in the node IDs of the factors to group 1 and add the nodes of the second row to group 2.
fg3 = factorGraph; addFactor(fg3,f,[1; 2]); fg3Group1 = nodeIDs(fg3,GroupID=1)
fg3Group1 = 1×2
0 1
fg3Group2 = nodeIDs(fg3,GroupID=2)
fg3Group2 = 1×2
2 3
Group Nodes by Matrix
You can also specify the group ID as a matrix of the same size as the node IDs of the factors to assign each node to a specific group. Add the first and fourth nodes to group 1 and the second and third nodes to groups 2 and 3, respectively.
fg4 = factorGraph; groups = [1 2; 3 1]; addFactor(fg4,f,groups); fg4Group1 = nodeIDs(fg4,GroupID=1)
fg4Group1 = 1×2
0 3
fg4Group2 = nodeIDs(fg4,GroupID=2)
fg4Group2 = 1
fg4Group3 = nodeIDs(fg4,GroupID=3)
fg4Group3 = 2
Input Arguments
fg
— Factor graph to add factor to
factorGraph
object
Factor graph to add factor to, specified as a factorGraph
object.
factor
— Factors to add to factor graph
valid factor object
Factors to add to the factor graph, specified as a valid factor object.
A valid factor object must be one of these objects, and the object must not create any invalid nodes when added to the factor graph:
With the exception of factorGPS
and
factorIMU
, you can simultaneously add multiple factors to the factor
graph using any one of the listed factor objects. factorGPS
and
factorIMU
support only singlefactor addition.
If the specified factor object creates any invalid nodes, then
addFactor
adds none of the factors from the factor
object.
groupID
— Group IDs for nodes of added factor
nonnegative integer  twoelement row vector of nonnegative integers  Nelement column vector of nonnegative integers  Nby2 matrix of nonnegative integers
Group IDs for nodes of the added factor, specified as any of these options:
groupID Size  Grouping Behavior 

nonnegative integer  Assigns all nodes to one group. For example, if you add
a factor object that has a 
twoelement row vector of nonnegative integers  Specify groups for each column of nodes. For example,
if you add a factor object that has a The
behavior for IMU factors is different. If you add an IMU factor with a

Nelement column vector of nonnegative integers  Group nodes by factor, where N is the total
number of factors specified by the For example, if you add a factor
object that has a 
Nby2 matrix of nonnegative integers  Add nodes in For example, if you add a factor
object that has a 
Note
When adding a factorIMU
or factorGPS
object to a
factor graph, groupID
accepts only these values:
factorIMU
— Nonnegative integer or a twoelement row vector of nonnegative integers.factorGPS
— Nonnegative integer
Adding nodes to groups enables you to query node IDs by group by specifying the
GroupID
namevalue argument of the nodeIDs
function.
Output Arguments
factorIDs
— Factor IDs of added factors
Nelement row vector of nonnegative integers
Factor IDs of the added factors, returned as an Nelement row vector of nonnegative integers. N is the total number of factors added.
The function returns this argument only when it successfully adds the factors to the
factor graph. If adding the factors results in an invalid node, then
addFactor
adds none of the factors from the factor
object.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
When generating portable C code with a C++ compiler, you must specify hierarchical
packing with nonminimal headers. For more information on packaging options, see the
packNGo
(MATLAB Coder) function.
Version History
Introduced in R2022aR2023a: Specify group IDs for added factors
addFactor
now supports specifying groups to add nodes of added
factors to by group ID.
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.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
 América Latina (Español)
 Canada (English)
 United States (English)
Europe
 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)