marginalizeFactor

Marginalize factors from factor graph into marginal factor

Since R2025a

collapse all in page

    Syntax

    factorID = marginalizeFactor(fg,factorIDs)
    [factorID,removedNodeIDs] = marginalizeFactor(fg,factorIDs)

    Description

    factorID = marginalizeFactor(fg,factorIDs) generates a marginal factor using the information from the specified factors and adds it to the factor graph. The function also removes the specified factors and any nodes that, as a result of the marginalization process, become disconnected from any factor in the factor graph.

    example

    [factorID,removedNodeIDs] = marginalizeFactor(fg,factorIDs) optionally returns the IDs of any nodes that were removed by marginalizing the specified factors.

    Examples

    collapse all

    Open Live Script

    Create a factor graph and load initial state data.

    fg = factorGraph;
load exampleSimple3DFGStateData.mat

    For simplicity, create one relative measurement to use for all pose factors.

    relMeasure = [0.75 0 0 eul2quat([pi/8 -pi/12 0],"ZYX")];

    Create a four-pose factor graph using three pose factors.

    nIDs = generateNodeID(fg,3,"factorTwoPoseSE3") + 1;
fRelPose = factorTwoPoseSE3(nIDs,Measurement=repmat(relMeasure,3,1));
fRelPoseID = addFactor(fg,fRelPose);
nIDs = nodeIDs(fg,NodeType="POSE_SE3");  % Get node IDs out of pose pair format
nodeState(fg,nIDs,state(1:4,:));         % Set initial guess states of pose nodes

    Add a prior factor to the first node, optimize, and then show the factor graph.

    fPrior = factorPoseSE3Prior(nIDs(1),Measurement=[0 0 0 1 0 0 0]);
fPriorPoseID = addFactor(fg,fPrior);
optimize(fg);
ax = show(fg,Orientation="on");
title("Factor Graph Poses Before Marginalization")
axis([-0.05 2.0 -0.05 1.5 0 1.5])
view(-15,45)

    Figure contains an axes object. The axes object with title Factor Graph Poses Before Marginalization contains 18 objects of type patch, line. One or more of the lines displays its values using only markers

    Add a new pose at the end of the factor graph.

    fRelPose2 = factorTwoPoseSE3([nIDs(end) nIDs(end)+1],Measurement=relMeasure);
fRelPoseID = [fRelPoseID addFactor(fg,fRelPose2)];
nodeState(fg,nIDs(end),state(end,:)); % Set the initial state of the new pose node

    Marginalize the first relative pose factor and the prior pose factor, which provides absolute pose information. The marginalization process removes the first pose node from the factor graph. This process reduces the size of the factor graph, but retains the information from the removed factors and nodes by replacing them with a marginal factor.

    [fMarginalID,removedNodeID] = marginalizeFactor(fg,[fPriorPoseID fRelPoseID(1)]);

    Optimize and show the factor graph. Note that the states of the second, third, fourth, and fifth nodes retain their positions because the graph optimization incorporates the new marginal factor, which includes the absolute pose information from the marginalized node and factors.

    optimize(fg);
show(fg,Parent=ax,Orientation="on");
title(["Optimization After Adding New Node and","Marginalizing Relative Pose Factor and Prior Pose Factor"])
axis([-0.05 2.0 -0.05 1.5 0 1.5])
view(-15,45)

    Figure contains an axes object. The axes object with title Optimization After Adding New Node and Marginalizing Relative Pose Factor and Prior Pose Factor contains 18 objects of type patch, line. One or more of the lines displays its values using only markers

    Input Arguments

    collapse all

    Factor graph, specified as a factorGraph object.

    ID of factors to marginalize, specified as an N-element array of nonnegative integers. N is the total number of factors to marginalize.

    Note

    Ensure that nodes that will be marginalized as a result of marginalizeFactor are not fixed. Any fixed nodes that would remain in the graph after marginalization can stay fixed.

    If you must marginalize a node that is currently fixed, first free the node using the fixNode function. If the node state of the fixed node is accurate and the marginalization must incorporate this information, create a prior factor, such as SE(3) prior pose factor, and connect it to the node.

    Output Arguments

    collapse all

    ID of marginal factor added to the factor graph, returned as a nonnegative integer.

    IDs of removed nodes that became disconnected after marginalizing the specified factors, returned as an M-element vector of nonnegative integers. M is the number of nodes that would become completely disconnected as a result of marginalizing related factors.

    Extended Capabilities

    expand all

    C/C++ Code Generation
    Generate C and C++ code using MATLAB® Coder™.

    Version History

    Introduced in R2025a

    See Also

    | |