Main Content

generateNodeID

Generate new node IDs

Since R2023a

    Description

    The generateNodeID function generates new node IDs for creating new factors for a factor graph.

    example

    IDs = generateNodeID(fg,numNodes) generates the specified number numNodes new node IDs IDs to use when creating new factors to add to the factor graph fg.

    The first node ID in IDs is 0 if there are no nodes in fg, otherwise the first node ID in IDs is equal to graph.NumNodes + 1.

    example

    IDs = generateNodeID(fg,nodeMatrixSize) specifies the number of new node IDs to generate as the number of elements in a matrix with the specified dimensions nodeMatrixSize. The IDs increment in row-major order.

    For example, generateNodeID(graph,[3 2]) creates new node IDs as a matrix in this format.

    [ID1ID2ID3ID4ID5ID6]

    example

    IDs = generateNodeID(fg,numFactors,factorType) generates new node IDs for the specified number of factors numFactors. The size of the generated node IDs and how the IDs increment depends on the number of factors and the factor type factorType

    Note

    The generateNodeID can only generate node IDs for creating one factor using this syntax for the factorIMU and factorGPS objects. To use this syntax to generate node IDs for the factorIMU and factorGPS objects, numFactors must be 1.

    Examples

    collapse all

    Create a factor graph and generate ten node IDs.

    fg = factorGraph;
    nids = generateNodeID(fg,10)
    nids = 1×10
    
         0     1     2     3     4     5     6     7     8     9
    
    

    Create a factor graph and generate a 4-by-2 node ID matrix.

    fg = factorGraph;
    nids = generateNodeID(fg,[4 2])
    nids = 4×2
    
         0     1
         2     3
         4     5
         6     7
    
    

    Create a factor graph and generate node IDs to specify four factors in a factorPoseSE3AndPointXYZ object.

    fg = factorGraph;
    poseAndLandmarkIDs = generateNodeID(fg,4,"factorPoseSE3AndPointXYZ")
    poseAndLandmarkIDs = 4×2
    
         0     1
         0     2
         0     3
         0     4
    
    

    Note that for landmark factors, the goal is to connect one pose to multiple landmarks. So for "factorPoseSE3AndPointXYZ" factor type, the first column is constant because it represents the single pose node, and the second column increments because it represents multiple landmarks nodes.

    Create and add the factor object to the factor graph to create the factors.

    poseAndLandmarkFactors = factorPoseSE3AndPointXYZ(poseAndLandmarkIDs);
    addFactor(fg,poseAndLandmarkFactors);

    Generate node IDs to create a factorTwoPoseSE2 object with three factors. Because the last node ID of the factor graph is 4, the new node IDs start at five.

    poseIDs = generateNodeID(fg,3,"factorTwoPoseSE2")
    poseIDs = 3×2
    
         5     6
         6     7
         7     8
    
    

    Note that "factorTwoPoseSE2" factor type, the goal is create a chain of connected poses, meaning that each row should be connected to the row before and after it. So for every row after the first row, the first element is the second node ID of the previous row. This ensures that each node connects to the nodes immediately before and after it in sequence.

    Create and add the factorTwoPoseSE2 object to the factor graph.

    twoPoseFactors = factorTwoPoseSE2(poseIDs);
    addFactor(fg,twoPoseFactors);

    Input Arguments

    collapse all

    Factor graph to generate nodes for, specified as a factorGraph object.

    The NumNodes property of fg determines what the first generated node ID of the IDs output is. The first node ID in IDs is 0 if there are no nodes in fg, otherwise the first node ID in IDs is equal to graph.NumNodes + 1.

    Number of node IDs to generate, specified as a nonnegative integer.

    Example: generateNodeID(fg,3) generates three node IDs as a row vector in the form [NodeID1 NodeID2 NodeID3]

    Generated node ID matrix size, specified as a two-element vector of nonnegative integers. The first element specifies the number of rows and the second element specifies the number of columns in the node ID matrix.

    Example: generateNodeID(fg,[3 2]) generates two node IDs for three factors in the form [NodeID1 NodeID2; NodeID3 NodeID4; NodeID5 NodeID6]

    Number of factors to generate node IDs for, specified as a nonnegative integer.

    If factorType is either "factorGPS" or "factorIMU", then numFactors must be specified as 1.

    Example: generateNodeID(fg,1,"factorPoseSE2AndPointXY") creates a set of two node IDs in the form [SE2PoseNodeID LandmarkNodeID] for one factor.

    Factor type for which to generate node IDs, specified as one of these options:

    The generateNodeID function supports only generating node IDs to create a single factor for these options:

    If factorType is either "factorGPS" or "factorIMU", then the numFactors argument must be specified as 1.

    The format of the node IDs corresponds to the format of the NodeID property for that factor object. For example, if you specify "factorTwoPoseSE2", the function outputs node IDs of the form [SE2Pose1NodeID SE2Pose2NodeID].

    Example: generateNodeID(fg,1,"factorPoseSE2AndPointXY") creates a set of two node IDs in the form [SE2PoseNodeID LandmarkNodeID]

    Data Types: char | string

    Output Arguments

    collapse all

    Generated node IDs, returned as one of these options depending on which input arguments you specify:

    1. numNodesN-element row vector, where N is equal to numNodes, and increments with each new node ID.

    2. nodeMatrixSizeM-by-N matrix, where M and N are equal to the first and second elements of nodeMatrixSize, respectively. The node IDs increment in row-major order.

      For example, if you specify generateNodeID(fg,[3 2]) with no existing nodes in the factor graph fg, the function generates a node ID matrix in this format.

      [ID1ID2ID3ID4ID5ID6]

    3. numFactors and factorTypeM-by-N matrix, where M is equal to numFactors, and N is the number of node IDs generated per factor depending on the value of factorType:

    factorTypeIDs Format
    • "factorCameraAndPointXYZ"

    • "factorPoseSE2AndPointXY"

    • "factorPoseSE3AndPointXYZ"

    N-by-2 matrix of node IDs, where N is equal to numFactors. The first column contains the first node ID for every row, and the second column contains unique node IDs that increment down the second column.

    [ID1ID2ID1ID3ID1IDN]

    • "factorTwoPoseSE2"

    • "factorTwoPoseSE3"

    N-by-2 matrix, where N is equal to numFactors. Each row represents a pair of consecutive nodes. For every row after the first row, the first element is the second node ID of the previous row. This ensures that each node connects to the nodes immediately before and after it in sequence.

    [ID1ID2ID2ID3IDNIDN+1]

    • "factorIMUBiasPrior"

    • "factorPoseSE3Prior"

    • "factorVelocity3Prior"

    N-element column vector, where N is equal to numFactors.

    [ID1ID2IDN]

    The first node ID in IDs is 0 if there are no nodes in fg, otherwise the first node ID in IDs is equal to graph.NumNodes + 1.

    Extended Capabilities

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

    Version History

    Introduced in R2023a