marginalizeNode

Marginalize node and related factors from factor graph into marginal factor

Since R2025a

collapse all in page

    Syntax

    factorID = marginalizeNode(fg,nodeID)
    [___,removedNodeIDs] = marginalizeNode(fg,nodeID)
    [___,removedFactorIDs] = marginalizeNode(fg,nodeID)

    Description

    factorID = marginalizeNode(fg,nodeID) generates a marginal factor using the information from the specified node and its related factors, and then adds the marginal factor 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 factors in the factor graph.

    example

    [___,removedNodeIDs] = marginalizeNode(fg,nodeID) additionally returns the node IDs of nodes that were removed along with the specified node.

    [___,removedFactorIDs] = marginalizeNode(fg,nodeID) additionally returns the factor IDs of nodes that were removed along with the specified node.

    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));
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 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]);
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 pose at the end of the factor graph.

    fRelPose2 = factorTwoPoseSE3([nIDs(end) nIDs(end)+1],Measurement=relMeasure);
addFactor(fg,fRelPose2);

    Marginalize the first node, which also marginalizes the two connected factors, a relative pose factor, and a prior pose factor. Note that the prior pose factor gives the factor graph absolute pose information.

    [fMarginalID,removedNodeID,removedFactorIDs] = marginalizeNode(fg,nIDs(1));
nodeState(fg,nIDs(end),state(end,:)); % Set the initial guess state of the new pose node

    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 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 First Node"])
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 First Node 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 the node to marginalize, specified as a nonnegative integer.

    Note

    The specified node must be not be fixed before marginalization. To marginalize a fixed node, free the node using the fixNode function prior to marginalization. If the node state is accurate and the marginalization must incorporate this information, create a prior factor, such as factorPoseSE3Prior, 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 node, 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 the specified node and its related factors.

    IDs of marginalized factors, returned as a P-element vector of nonnegative integers. P is the total number of factors marginalized during the node marginalization process.

    Extended Capabilities

    expand all

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

    Version History

    Introduced in R2025a

    See Also

    | |