isConnected

Check if factor graph is connected

Since R2022a

Syntax

connected = isConnected(fg)

connected = isConnected(fg,poseNodeIDs)

Description

The isConnected function returns a logical flag that indicates whether the factor graph, or a partial factor graph built from specified pose nodes, contains a path between every pair of nodes.

connected = isConnected(fg) returns a logical flag indicating whether the specified factor graph contains a path between every pair of nodes associated with it.

connected = isConnected(fg,poseNodeIDs) returns a logical flag indicating whether a partial factor graph comprised of the specified pose nodes IDs poseNodeIDs, and related factors and non-pose nodes, contains a path between every pair of nodes. For more information, see Factor Graph Connectivity.

Examples

Checking Factor Graph Connectivity

Create a factor graph.

fg = factorGraph;
poseIDs1 = generateNodeID(fg,2,"factorTwoPoseSE3")

poseIDs1 = 2×2

     0     1
     1     2

poseFactors1 = factorTwoPoseSE3(poseIDs1);
addFactor(fg,poseFactors1);

Check the connectivity.

isConnected(fg)

ans = logical
   1

The graph is connected because there is a path between every node pair of graph. For example, you can reach node 2 from node 0 by going through node 1.

Next try to add a disconnected node. Generate a node ID for a GPS factor.

gpsID = generateNodeID(fg,1,"factorGPS")

gpsID = 
3

Create the GPS factor and add it to the factor graph.

gpsFactor = factorGPS(gpsID);
addFactor(fg,gpsFactor);

Check the connectivity. Note that because the new node specified by the GPS factor is not connected to any of the previous nodes, there is a disconnect.

isConnected(fg)

ans = logical
   0

Add another factor between node 2 and node 3 to resolve this disconnect.

poseFactors2 = factorTwoPoseSE3([2 3]);
addFactor(fg,poseFactors2);

Check the connectivity to verify the graph is connected again.

isConnected(fg)

ans = logical
   1

Input Arguments

fg — Factor graph

factorGraph object

Factor graph, specified as a factorGraph object.

poseNodeIDs — IDs of pose nodes to check for connection

N-element row vector of nonnegative integers

IDs of pose nodes to check for connection within the factor graph, specified as an N-element row vector of nonnegative integers. N is the total number of nodes to check.

The pose nodes specified by poseNodeIDs must all be of type "POSE_SE2", or must all be of type "POSE_SE3". The specified pose nodes must also be unique. For example, poseNodeIDs cannot be [1 2 1] because node ID 1 not unique in this vector.

The specified pose nodes in the factor graph must form a connected factor graph. For more information, see Factor Graph Connectivity.

Output Arguments

connected — Graph is connected in factor graph or in partial factor graph

false or 0 | true or 1

Graph is connected in factor graph or in the partial factor graph, returned as 1 (true) if the factor graph contains a path between every pair of specified nodes and 0 (false) if it does not contain a path between every pair of specified nodes.

More About

Factor Graph Connectivity

A factor graph is considered connected if there is a path between every pair of nodes. For example, for a factor graph containing four pose nodes, connected consecutively by three factors, there are paths in the factor graph from one node in the graph to any other node in the graph.

connected = isConnected(fg,[1 2 3 4])

connected =

     1

If the graph does not contain node 3, although there is still a path from node 1 to node 2, there is no path from node 1 or node 2 to node 4.

connected = isConnected(fg,[1 2 4])

connected =

     0

A fully connected factor graph is important for optimization. If the factor graph is not fully connected, then the optimization occurs separately for each of the disconnected graphs, which may produce undesired results. The connectivity of graphs can become more complex when you specify certain subsets of pose node IDs to optimize. This is because the optimize function optimizes parts of the factor graph by using the specified IDs to identify which factors to use to create a partial factor graph. optimize adds a factor to the partial factor graph if that factor connects to any of the specified pose nodes and does not connect to any unspecified pose nodes. The function also adds any non-pose nodes that the added factors connect to, but does not add other factors connected to those nodes. For example, for this factor graph there are three pose nodes, two non-pose nodes, and the factors that connect the nodes.

If you specify nodes 1 and 2, then factors 1, 3, 4, and 5 form a factor graph for the optimization because they connect to pose nodes 1 and 2. The optimization includes nodes 4 and 5 because they connect to factors that relate to the specified pose node IDs.

If you specify poseNodeIDs as [1 3], then the optimize function optimizes each separated graph separately because the formed factor graph does not contain a path between nodes 1 and 3.

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 non-minimal headers. For more information on packaging options, see the packNGo (MATLAB Coder) function.

Version History

Introduced in R2022a

R2023b: Check connection of specified pose nodes

The isConnected supports checking the connection of specified pose nodes by specifying node IDs.