From ea15ebc126f3272624e61b1fe2521398d4653fc9 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 24 Mar 2026 14:32:17 +0200 Subject: [PATCH 1/4] Refactor path functions to getPath[s] --- src/Deprecated.jl | 91 ++++++++++------ src/DistributedFactorGraphs.jl | 4 +- src/GraphsDFG/GraphsDFG.jl | 2 + src/GraphsDFG/services/GraphsDFG.jl | 154 +++++++++++++++++----------- src/services/AbstractDFG.jl | 35 ++++++- test/interfaceTests.jl | 4 + test/testBlocks.jl | 126 +++++++++++++++++++++++ 7 files changed, 321 insertions(+), 95 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index a0fdee71..5d94fdfc 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -634,40 +634,9 @@ end # - A better noun is maybe Path or simply listFactors with a fancy filter, something like: # - [list/get]Path(dfg, from, to; algorithm...) # the `search` verb can also come ito play, but it is more for knn search type functions. -""" - $SIGNATURES - -Relatively naive function counting linearly from-to - -DevNotes -- Convert to using Graphs shortest path methods instead. -""" -# -function findFactorsBetweenNaive( - dfg::AbstractDFG, - from::Symbol, - to::Symbol, - assertSingles::Bool = false, -) - # - @info "findFactorsBetweenNaive is naive linear number method -- improvements welcome" - SRT = getVariableLabelNumber(from) - STP = getVariableLabelNumber(to) - prefix = string(from)[1] - @assert prefix == string(to)[1] "from-to prefixes must match, one is $prefix, other $(string(to)[1])" - prev = from - fctlist = Symbol[] - for num = (SRT + 1):STP - next = Symbol(prefix, num) - fct = intersect(ls(dfg, prev), ls(dfg, next)) - if assertSingles - @assert length(fct) == 1 "assertSingles=true, won't return multiple factors joining variables at this time" - end - union!(fctlist, fct) - prev = next - end - return fctlist +function findFactorsBetweenNaive(args...) + return error("findFactorsBetweenNaive is obsolete, use DFG.getPath[s] instead.") end #TODO deprecate `is` is the correct verb, but rather isHomogeneous(path::Path) the form is isAdjective @@ -690,3 +659,59 @@ function isPathFactorsHomogeneous(dfg::AbstractDFG, from::Symbol, to::Symbol) utyp = unique(types) return (length(utyp) == 1), utyp end + +# deprecated use filter and path seperately. +function findShortestPathDijkstra( + dfg::GraphsDFG, + from::Symbol, + to::Symbol; + labelFilterVariables::Union{Function, Nothing} = nothing, + labelFilterFactors::Union{Function, Nothing} = nothing, + tagsFilterVariables::Union{Function, Nothing} = nothing, + tagsFilterFactors::Union{Function, Nothing} = nothing, + typeFilterVariables::Union{Function, Nothing} = nothing, + typeFilterFactors::Union{Function, Nothing} = nothing, + solvableFilter::Union{Function, Nothing} = nothing, + initialized::Union{Nothing, Bool} = nothing, +) + Base.depwarn( + "findShortestPathDijkstra is deprecated, use getPath with `variableLabels`/`factorLabels` kwargs instead.", + :findShortestPathDijkstra, + ) + any_active_filters = any( + .!isnothing.([labelFilterVariables, labelFilterFactors, tagsFilterVariables, tagsFilterFactors, typeFilterVariables, typeFilterFactors, initialized, solvableFilter]), + ) + + if any_active_filters + varList = listVariables( + dfg; + labelFilter = labelFilterVariables, + tagsFilter = tagsFilterVariables, + typeFilter = typeFilterVariables, + solvableFilter, + ) + fctList = listFactors( + dfg; + labelFilter = labelFilterFactors, + tagsFilter = tagsFilterFactors, + typeFilter = typeFilterFactors, + solvableFilter, + ) + + varList = if initialized !== nothing + initmask = isInitialized.(dfg, varList) .== initialized + varList[initmask] + else + varList + end + restrict_labels = vcat(varList, fctList) + subdfg = DFG.getSubgraph( + GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, + dfg, + restrict_labels, + ) + return getPath(subdfg, from, to).path + else + return getPath(dfg, from, to).path + end +end diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index a3852d05..cd8a4a8a 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -422,6 +422,8 @@ const unstable_functions::Vector{Symbol} = [ :FactorSummary, :listNeighborhood, :listNeighbors, + :getPath, + :getPaths, :InMemoryBlobstore, :exists, :compare, @@ -442,7 +444,7 @@ const unstable_functions::Vector{Symbol} = [ :findClosestTimestamp, :findVariablesNearTimestamp, :findShortestPathDijkstra, - :findFactorsBetweenNaive, + :findFactorsBetweenNaive, # TODO not really used :getAgentLabel, #TODO check and mark as public :getGraphLabel, #TODO check and mark as public :getDescription, diff --git a/src/GraphsDFG/GraphsDFG.jl b/src/GraphsDFG/GraphsDFG.jl index 8fcd6602..6394a792 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -54,6 +54,8 @@ import ...DistributedFactorGraphs: lsf, isConnected, listNeighbors, + getPaths, + getPath, buildSubgraph, getBiadjacencyMatrix, toDot, diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index bbe167e0..0e68d24c 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -358,74 +358,110 @@ function toDot(dfg::GraphsDFG) return String(data) end -function findShortestPathDijkstra( +#API design NOTE: +# Do not create new Verbs or Nouns for metric vs. topological pathfinding. getPaths is the universal router... getPaths(..., metric) +# for now we only look at topological paths. + +function getPaths(::typeof(all_simple_paths), dfg, from::Symbol, to::Symbol; kwargs...) + gpaths = Graphs.all_simple_paths(dfg.g, dfg.g.labels[from], dfg.g.labels[to]; kwargs...) + return map(p -> (path = map(i -> dfg.g.labels[i], p), dist = length(p) - 1), gpaths) +end + +function getPaths( + ::typeof(yen_k_shortest_paths), dfg::GraphsDFG, from::Symbol, - to::Symbol; - labelFilterVariables::Union{Function, Nothing} = nothing, - labelFilterFactors::Union{Function, Nothing} = nothing, - tagsFilterVariables::Union{Function, Nothing} = nothing, - tagsFilterFactors::Union{Function, Nothing} = nothing, - typeFilterVariables::Union{Function, Nothing} = nothing, - typeFilterFactors::Union{Function, Nothing} = nothing, - solvableFilter::Union{Function, Nothing} = nothing, - initialized::Union{Nothing, Bool} = nothing, + to::Symbol, + k::Int; + distmx = weights(dfg.g), + kwargs..., ) - duplicate = - !isnothing(labelFilterVariables) || - !isnothing(labelFilterFactors) || - !isnothing(tagsFilterVariables) || - !isnothing(tagsFilterFactors) || - !isnothing(typeFilterVariables) || - !isnothing(typeFilterFactors) || - !isnothing(initialized) || - !isnothing(solvableFilter) - - dfg_ = if duplicate - # use copy if filter is being applied - varList = ls( - dfg; - labelFilter = labelFilterVariables, - tagsFilter = tagsFilterVariables, - typeFilter = typeFilterVariables, - solvableFilter, - ) - fctList = lsf( - dfg; - labelFilter = labelFilterFactors, - tagsFilter = tagsFilterFactors, - typeFilter = typeFilterFactors, - solvableFilter, - ) + (; paths, dists) = Graphs.yen_k_shortest_paths( + dfg.g, + dfg.g.labels[from], + dfg.g.labels[to], + distmx, + k; + kwargs..., + ) + return map(zip(paths, dists)) do (path, dist) + return (path = map(i -> dfg.g.labels[i], path), dist = dist) + end +end - varList = if initialized !== nothing - initmask = isInitialized.(dfg, varList) .== initialized - varList[initmask] - else - varList - end - DFG.deepcopyGraph(typeof(dfg), dfg, varList, fctList) - else - # no filter can be used directly - dfg +# note with default heuristic this is just dijkstra's algorithm +function getPaths( + ::typeof(a_star), + dfg::GraphsDFG, + from::Symbol, + to::Symbol, + ::Int; + distmx::AbstractMatrix{T} = weights(dfg.g), + heuristic = nothing, +) where {T} + #TODO make it easier to use label in the heuristic + heuristic = something(heuristic, (n) -> zero(T)) + edgepath = Graphs.a_star(dfg.g, dfg.g.labels[from], dfg.g.labels[to], distmx, heuristic) + + if isempty(edgepath) + return @NamedTuple{path::Vector{Symbol}, dist::Int64}[] + end + + path = [dfg.g.labels[edgepath[1].src]] + dist = 0 + for (; dst, src) in edgepath + push!(path, dfg.g.labels[dst]) + dist += distmx[src, dst] end - if !(hasVariable(dfg_, from) || hasFactor(dfg_, from)) || - !(hasVariable(dfg_, to) || hasFactor(dfg_, to)) - # assume filters excluded either `to` or `from` and hence no shortest path - return Symbol[] + return [(path = path, dist = dist)] +end + +#TODO Move getPaths and getPath to AbstractDFG services as default implementations. +function getPaths( + dfg::AbstractDFG, + from::Symbol, + to::Symbol, + k::Int; + algorithm = k == 1 ? a_star : yen_k_shortest_paths, + variableLabels::Union{Nothing, Vector{Symbol}} = nothing, + factorLabels::Union{Nothing, Vector{Symbol}} = nothing, + kwargs..., +) + # If the user provided restricted lists, build the subgraph automatically + active_dfg = if variableLabels === nothing && factorLabels === nothing + dfg + else + vlabels = something(variableLabels, listVariables(dfg)) + flabels = something(factorLabels, listFactors(dfg)) + labels = vcat(vlabels, flabels) + DFG.getSubgraph( + GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, + dfg, + labels, + ) end - # GraphsDFG internally uses Integers - frI = dfg_.g.labels[from] - toI = dfg_.g.labels[to] + return getPaths(algorithm, active_dfg, from, to, k; kwargs...) +end - # get shortest path from graph provider - path_state = Graphs.dijkstra_shortest_paths(dfg_.g.graph, [frI;]) - path = Graphs.enumerate_paths(path_state, toI) - dijkpath = map(x -> dfg_.g.labels[x], path) +function getPath( + dfg::AbstractDFG, + from::Symbol, + to::Symbol; + variableLabels::Union{Nothing, Vector{Symbol}} = nothing, + factorLabels::Union{Nothing, Vector{Symbol}} = nothing, + kwargs..., +) + paths = getPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) + + # Adhere strictly to the "getSingular = Error" rule + if isempty(paths) + error( + "No path found between :$(from) and :$(to). If a disconnected graph is expected, use `getPaths` instead.", + ) + end - # return the list of symbols - return dijkpath + return first(paths) end export bfs_tree diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index b95b1373..f1d4ee2e 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -381,6 +381,37 @@ Implement `listNeighbors(dfg::AbstractDFG, label::Symbol; solvableFilter, tagsFi """ function listNeighbors end +""" + getPaths(dfg, from::Symbol, to::Symbol, k::Int; variableLabels, factorLabels, kwargs...) + +Return the `k` shortest paths between `from` and `to` in the factor graph. +Each result is a `(path = Vector{Symbol}, dist)` named tuple. + +Optional keyword arguments restrict which variables and/or factors may appear on +the path. When neither is given the full graph is used. When only one is +provided the other defaults to all labels of that kind in `dfg`. + +Typical usage with filters: +```julia +vars = listVariables(dfg; solvableFilter = >=(1)) +facs = listFactors(dfg; solvableFilter = >=(1)) +getPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) +``` + +See also: [`getPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) +""" +function getPaths end + +""" + getPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) + +Return the single shortest path between `from` and `to`. +Errors if no path exists (use `getPaths` for graphs that may be disconnected). + +Accepts the same restriction keywords as [`getPaths`](@ref). +""" +function getPath end + function listNeighbors(dfg::AbstractDFG, node::AbstractGraphNode; kwargs...) return listNeighbors(dfg, getLabel(node); kwargs...) end @@ -549,13 +580,13 @@ function getSubgraph( return destDFG end -function buildSubgraph( +function getSubgraph( dfg::AbstractDFG, variableFactorLabels::Vector{Symbol}, distance::Int = 0; kwargs..., ) - return buildSubgraph(LocalDFG, dfg, variableFactorLabels, distance; kwargs...) + return getSubgraph(LocalDFG, dfg, variableFactorLabels, distance; kwargs...) end """ diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 72678517..34502153 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -231,6 +231,10 @@ end end end +@testset "Path Finding" begin + PathFindingTests(testDFGAPI) +end + # FIXME this will likeley become obsolete with new pack/unpack system # @testset "Mixing Compute and DFG graph nodes" begin # com_fg = testDFGAPI() diff --git a/test/testBlocks.jl b/test/testBlocks.jl index c9978868..94466bfb 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -1847,3 +1847,129 @@ function FileDFGTestBlock(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDFG @test issetequal(ls(dfg), ls(skeletondfg)) @test issetequal(lsf(dfg), lsf(skeletondfg)) end + +function PathFindingTests(testDFGAPI) + # Graph layout (connectivityTestGraph): + # x1 - x1x2f1 - x2 - x2x3f1 - x3 - x3x4f1 - x4 - x4x5f1 - x5 + # - x5x6f1 - x6 - x6x7f1 - x7 - x7x8f1 - x8 - x8x9f1 - x9 - x9x10f1 - x10 + # Variable types: x1..x5 = TestVariableType1, x6..x10 = TestVariableType2 + # Solvable defaults: x8=0, x9=0, x7x8f1=0; rest = 1 + dfg, verts, facs = connectivityTestGraph(testDFGAPI, VariableDFG, FactorDFG) + + # Add cross-link factors to create loops for multi-path testing + # x1 -- x1x3f1 -- x3 (shortcut bypassing x2) + # x2 -- x2x4f1 -- x4 (shortcut bypassing x3) + addFactor!(dfg, FactorDFG(:x1x3f1, [:x1, :x3], TestFunctorInferenceType1())) + addFactor!(dfg, FactorDFG(:x2x4f1, [:x2, :x4], TestFunctorInferenceType1())) + + # --- Basic getPaths / getPath (no restrictions) --- + result = getPath(dfg, :x1, :x3) + # shortest path should be via the direct link x1-x1x3f1-x3 (dist=2) not via x2 (dist=4) + @test result.path == [:x1, :x1x3f1, :x3] + @test result.dist == 2 + + # Multiple paths from x1 to x4: + # 1) x1-x1x2f1-x2-x2x4f1-x4 (dist=4) + # 2) x1-x1x3f1-x3-x3x4f1-x4 (dist=4) + # 3) x1-x1x2f1-x2-x2x3f1-x3-x3x4f1-x4 (dist=6) + # 4) x1-x1x3f1-x3-x2x3f1-x2-x2x4f1-x4 (dist=6) + results = getPaths(dfg, :x1, :x4, 4) + @test length(results) >= 2 + @test results[1].dist <= results[end].dist # sorted by distance + + # path across the whole graph + full_path = getPath(dfg, :x1, :x10) + @test :x1 == first(full_path.path) + @test :x10 == last(full_path.path) + + # --- Restrict with variableLabels only (all factors kept) --- + # Restrict to x1..x5 variables. Factors connecting only those vars are auto-included. + vars_subset = listVariables(dfg; typeFilter = ==(TestVariableType1())) + result_restricted = getPath(dfg, :x1, :x5; variableLabels = vars_subset) + @test first(result_restricted.path) == :x1 + @test last(result_restricted.path) == :x5 + # x6..x10 should NOT appear on the path + @test isempty(intersect(result_restricted.path, [:x6, :x7, :x8, :x9, :x10])) + + # --- Restrict with factorLabels only (all variables kept) --- + # Only allow the first 4 factors, path x1→x5 should still work + facs_first4 = listFactors(dfg; labelFilter = contains(r"x[1-4]")) + result_fac = getPath(dfg, :x1, :x5; factorLabels = facs_first4) + @test first(result_fac.path) == :x1 + @test last(result_fac.path) == :x5 + + # --- Restrict with both variableLabels and factorLabels --- + vars_1to5 = listVariables(dfg; typeFilter = ==(TestVariableType1())) + facs_1to4 = listFactors(dfg; labelFilter = contains(r"x[1-4]")) + result_both = + getPath(dfg, :x1, :x5; variableLabels = vars_1to5, factorLabels = facs_1to4) + @test first(result_both.path) == :x1 + @test last(result_both.path) == :x5 + + # --- With solvableFilter --- + # Only solvable >= 1 variables (excludes x8, x9) + solvable_vars = listVariables(dfg; solvableFilter = >=(1)) + solvable_facs = listFactors(dfg; solvableFilter = >=(1)) + # Path from x1 to x7 should work (all solvable) + result_solvable = + getPath(dfg, :x1, :x7; variableLabels = solvable_vars, factorLabels = solvable_facs) + @test first(result_solvable.path) == :x1 + @test last(result_solvable.path) == :x7 + # x8 and x9 (unsolvable) should not appear + @test :x8 ∉ result_solvable.path + @test :x9 ∉ result_solvable.path + + # Path from x1 to x10 with solvable filter should fail (x8, x9, x7x8f1 block the way) + paths_blocked = getPaths( + dfg, + :x1, + :x10, + 1; + variableLabels = solvable_vars, + factorLabels = solvable_facs, + ) + @test isempty(paths_blocked) + + # And the singular getPath should throw on disconnect + @test_throws ErrorException getPath( + dfg, + :x1, + :x10; + variableLabels = solvable_vars, + factorLabels = solvable_facs, + ) + + # --- With tagsFilter --- + # By default all variables have :VARIABLE tag. Tag some for testing. + mergeTags!(dfg, :x3, Set([:LANDMARK])) + mergeTags!(dfg, :x4, Set([:LANDMARK])) + landmark_vars = listVariables(dfg; tagsFilter = ⊇([:LANDMARK])) + @test :x3 ∈ landmark_vars + @test :x4 ∈ landmark_vars + # Restrict to only LANDMARK variables - x1 is not a LANDMARK, so include it to enable the path + vars_with_x1 = union([:x1, :x2], landmark_vars) + result_tags = getPath(dfg, :x1, :x4; variableLabels = vars_with_x1) + @test first(result_tags.path) == :x1 + @test last(result_tags.path) == :x4 + # x5..x10 should not be on this path + @test isempty(intersect(result_tags.path, [:x5, :x6, :x7, :x8, :x9, :x10])) + + # --- getPaths with k > 1 on looped graph --- + # With cross-links x1x3f1 and x2x4f1. Multiple paths exist from x1 to x4. + results_multi = getPaths(dfg, :x1, :x4, 5) + @test length(results_multi) >= 2 + # All paths should start at x1 and end at x4 + for r in results_multi + @test first(r.path) == :x1 + @test last(r.path) == :x4 + end + # Distances should be non-decreasing + @test issorted([r.dist for r in results_multi]) + + # --- Error: no path exists --- + # Disconnect x10 by removing the factor + deleteFactor!(dfg, :x9x10f1) + @test_throws ErrorException getPath(dfg, :x1, :x10) + empty_paths = getPaths(dfg, :x1, :x10, 1) + @test isempty(empty_paths) +end From c87144210615d7bd8de500da06173361dbe267d5 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 24 Mar 2026 15:19:34 +0200 Subject: [PATCH 2/4] Changes from review and finish changing build -> getSubgraph (probably still wrong verbNoun) --- src/Deprecated.jl | 8 +++---- src/GraphsDFG/GraphsDFG.jl | 2 +- src/GraphsDFG/services/GraphsDFG.jl | 36 ++++++++++++++++++----------- src/services/AbstractDFG.jl | 2 +- test/iifCompareTests.jl | 2 +- test/testBlocks.jl | 22 +++++++++--------- 6 files changed, 40 insertions(+), 32 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 5d94fdfc..6c832678 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -436,7 +436,7 @@ end # - Orphaned factors (where the subgraph does not contain all the related variables) are not included. # Related: # - [`copyGraph!`](@ref) -# - [`buildSubgraph`](@ref) +# - [`getSubgraph`](@ref) # - [`listNeighborhood`](@ref) # - [`deepcopyGraph`](@ref) # """ @@ -455,7 +455,7 @@ NOTE: `copyGraphMetadata` is deprecated – use agent/graph Bloblets instead. Related: - [`deepcopyGraph`](@ref) - [`deepcopyGraph!`](@ref) -- [`buildSubgraph`](@ref) +- [`getSubgraph`](@ref) - [`listNeighborhood`](@ref) - [`mergeGraph!`](@ref) """ @@ -528,7 +528,7 @@ Copy nodes from one graph into another graph by making deepcopies. see [`copyGraph!`](@ref) for more detail. Related: - [`deepcopyGraph`](@ref) -- [`buildSubgraph`](@ref) +- [`getSubgraph`](@ref) - [`listNeighborhood`](@ref) - [`mergeGraph!`](@ref) """ @@ -556,7 +556,7 @@ Copy nodes from one graph into a new graph by making deepcopies. see [`copyGraph!`](@ref) for more detail. Related: - [`deepcopyGraph!`](@ref) -- [`buildSubgraph`](@ref) +- [`getSubgraph`](@ref) - [`listNeighborhood`](@ref) - [`mergeGraph!`](@ref) """ diff --git a/src/GraphsDFG/GraphsDFG.jl b/src/GraphsDFG/GraphsDFG.jl index 6394a792..9ddf88a3 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -56,7 +56,7 @@ import ...DistributedFactorGraphs: listNeighbors, getPaths, getPath, - buildSubgraph, + getSubgraph, getBiadjacencyMatrix, toDot, toDotFile, diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index 0e68d24c..3ab80977 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -404,11 +404,11 @@ function getPaths( edgepath = Graphs.a_star(dfg.g, dfg.g.labels[from], dfg.g.labels[to], distmx, heuristic) if isempty(edgepath) - return @NamedTuple{path::Vector{Symbol}, dist::Int64}[] + return @NamedTuple{path::Vector{Symbol}, dist::T}[] end path = [dfg.g.labels[edgepath[1].src]] - dist = 0 + dist = zero(T) for (; dst, src) in edgepath push!(path, dfg.g.labels[dst]) dist += distmx[src, dst] @@ -429,18 +429,26 @@ function getPaths( kwargs..., ) # If the user provided restricted lists, build the subgraph automatically - active_dfg = if variableLabels === nothing && factorLabels === nothing - dfg - else - vlabels = something(variableLabels, listVariables(dfg)) - flabels = something(factorLabels, listFactors(dfg)) - labels = vcat(vlabels, flabels) - DFG.getSubgraph( - GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, - dfg, - labels, - ) - end + active_dfg = + if isa(dfg, GraphsDFG) && isnothing(variableLabels) && isnothing(factorLabels) + dfg + else + vlabels = something(variableLabels, listVariables(dfg)) + flabels = something(factorLabels, listFactors(dfg)) + labels = vcat(vlabels, flabels) + DFG.getSubgraph( + GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, + dfg, + labels, + ) + end + !hasVariable(active_dfg, from) && + !hasFactor(active_dfg, from) && + throw(DFG.LabelNotFoundError(from)) + !hasVariable(active_dfg, to) && + !hasFactor(active_dfg, to) && + throw(DFG.LabelNotFoundError(to)) + return getPaths(algorithm, active_dfg, from, to, k; kwargs...) end diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index f1d4ee2e..3e5b2c99 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -565,7 +565,7 @@ function getSubgraph( if !isnothing(solvable) Base.depwarn( "solvable kwarg is deprecated, use kwarg `solvableFilter = (>=solvable)` instead", #v0.29 - :buildSubgraph, + :getSubgraph, ) !isnothing(solvableFilter) && error("Cannot use both solvable and solvableFilter kwargs.") diff --git a/test/iifCompareTests.jl b/test/iifCompareTests.jl index cbc381f7..d88d0543 100644 --- a/test/iifCompareTests.jl +++ b/test/iifCompareTests.jl @@ -107,7 +107,7 @@ end addVariable!(fg, :l1, ContinuousScalar) addFactor!(fg, [:x1; :l1], LinearRelative(Rayleigh())) - sfg = buildSubgraph(GraphsDFG, fg, [:x0; :x1]) + sfg = getSubgraph(GraphsDFG, fg, [:x0; :x1]) @warn "FIXME This is NOT supposed to pass" @test_skip compareFactorGraphs(fg, sfg, skip = [:labelDict; :addHistory; :logpath]) diff --git a/test/testBlocks.jl b/test/testBlocks.jl index 94466bfb..c24989e8 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -1518,21 +1518,21 @@ function BuildingSubgraphs(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDF # "Getting Subgraphs" dfg, verts, facs = connectivityTestGraph(testDFGAPI, VARTYPE, FACTYPE) # Subgraphs - dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [verts[1].label], 2) + dfgSubgraph = getSubgraph(testDFGAPI, dfg, [verts[1].label], 2) # Only returns x1 and x2 @test issetequal([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) # - dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [:x1, :x2, :x1x2f1]) + dfgSubgraph = getSubgraph(testDFGAPI, dfg, [:x1, :x2, :x1x2f1]) # Only returns x1 and x2 @test issetequal([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) - dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [:x1x2f1], 1) + dfgSubgraph = getSubgraph(testDFGAPI, dfg, [:x1x2f1], 1) # Only returns x1 and x2 @test issetequal([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) #TODO if not a GraphsDFG with and summary or skeleton if VARTYPE == VariableDFG - dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [:x8], 2; solvableFilter = >=(1)) + dfgSubgraph = getSubgraph(testDFGAPI, dfg, [:x8], 2; solvableFilter = >=(1)) @test issetequal([:x7], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) #end if not a GraphsDFG with and summary or skeleton end @@ -1540,25 +1540,25 @@ function BuildingSubgraphs(testDFGAPI; VARTYPE = VariableDFG, FACTYPE = FactorDF # REF: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/95 for fId in listVariables(dfg) # Get a subgraph of this and it's related factors+variables - dfgSubgraph = buildSubgraph(testDFGAPI, dfg, [fId], 2) + dfgSubgraph = getSubgraph(testDFGAPI, dfg, [fId], 2) # For each factor check that the order the copied graph == original for fact in getFactors(dfgSubgraph) @test fact.variableorder == getFactor(dfg, fact.label).variableorder end end - #TODO buildSubgraph default constructors for skeleton and summary + #TODO getSubgraph default constructors for skeleton and summary if VARTYPE == VariableDFG - dfgSubgraph = buildSubgraph(dfg, [:x1, :x2, :x1x2f1]) + dfgSubgraph = getSubgraph(dfg, [:x1, :x2, :x1x2f1]) @test issetequal([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) - dfgSubgraph = buildSubgraph(dfg, [:x2, :x3], 2) + dfgSubgraph = getSubgraph(dfg, [:x2, :x3], 2) @test issetequal( [:x2, :x3, :x1, :x4, :x3x4f1, :x1x2f1, :x2x3f1], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...], ) - dfgSubgraph = buildSubgraph(dfg, [:x1x2f1], 1) + dfgSubgraph = getSubgraph(dfg, [:x1x2f1], 1) @test issetequal([:x1, :x1x2f1, :x2], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) end end @@ -1893,14 +1893,14 @@ function PathFindingTests(testDFGAPI) # --- Restrict with factorLabels only (all variables kept) --- # Only allow the first 4 factors, path x1→x5 should still work - facs_first4 = listFactors(dfg; labelFilter = contains(r"x[1-4]")) + facs_first4 = listFactors(dfg; labelFilter = contains(r"x[1-4](?!\d)")) result_fac = getPath(dfg, :x1, :x5; factorLabels = facs_first4) @test first(result_fac.path) == :x1 @test last(result_fac.path) == :x5 # --- Restrict with both variableLabels and factorLabels --- vars_1to5 = listVariables(dfg; typeFilter = ==(TestVariableType1())) - facs_1to4 = listFactors(dfg; labelFilter = contains(r"x[1-4]")) + facs_1to4 = listFactors(dfg; labelFilter = contains(r"x[1-4](?!\d)")) result_both = getPath(dfg, :x1, :x5; variableLabels = vars_1to5, factorLabels = facs_1to4) @test first(result_both.path) == :x1 From b14a8bb8abce12d36d988c6405cb0435699f2dd9 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 24 Mar 2026 16:53:34 +0200 Subject: [PATCH 3/4] find is the better verb as the path is still an index to the graph --- src/Deprecated.jl | 10 +++--- src/DistributedFactorGraphs.jl | 4 +-- src/GraphsDFG/GraphsDFG.jl | 4 +-- src/GraphsDFG/services/GraphsDFG.jl | 36 ++++++++++---------- src/entities/DFGVariable.jl | 2 +- src/services/AbstractDFG.jl | 16 ++++----- test/testBlocks.jl | 53 ++++++++++++++++------------- 7 files changed, 66 insertions(+), 59 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 6c832678..becd2687 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -636,7 +636,7 @@ end # the `search` verb can also come ito play, but it is more for knn search type functions. function findFactorsBetweenNaive(args...) - return error("findFactorsBetweenNaive is obsolete, use DFG.getPath[s] instead.") + return error("findFactorsBetweenNaive is obsolete, use DFG.findPath[s] instead.") end #TODO deprecate `is` is the correct verb, but rather isHomogeneous(path::Path) the form is isAdjective @@ -660,7 +660,7 @@ function isPathFactorsHomogeneous(dfg::AbstractDFG, from::Symbol, to::Symbol) return (length(utyp) == 1), utyp end -# deprecated use filter and path seperately. +# deprecated use filter and path separately. function findShortestPathDijkstra( dfg::GraphsDFG, from::Symbol, @@ -675,7 +675,7 @@ function findShortestPathDijkstra( initialized::Union{Nothing, Bool} = nothing, ) Base.depwarn( - "findShortestPathDijkstra is deprecated, use getPath with `variableLabels`/`factorLabels` kwargs instead.", + "findShortestPathDijkstra is deprecated, use findPath with `variableLabels`/`factorLabels` kwargs instead.", :findShortestPathDijkstra, ) any_active_filters = any( @@ -710,8 +710,8 @@ function findShortestPathDijkstra( dfg, restrict_labels, ) - return getPath(subdfg, from, to).path + return findPath(subdfg, from, to).path else - return getPath(dfg, from, to).path + return findPath(dfg, from, to).path end end diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index cd8a4a8a..fa225ddb 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -422,8 +422,8 @@ const unstable_functions::Vector{Symbol} = [ :FactorSummary, :listNeighborhood, :listNeighbors, - :getPath, - :getPaths, + :findPath, + :findPaths, :InMemoryBlobstore, :exists, :compare, diff --git a/src/GraphsDFG/GraphsDFG.jl b/src/GraphsDFG/GraphsDFG.jl index 9ddf88a3..ad5a67ac 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -54,8 +54,8 @@ import ...DistributedFactorGraphs: lsf, isConnected, listNeighbors, - getPaths, - getPath, + findPaths, + findPath, getSubgraph, getBiadjacencyMatrix, toDot, diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index 3ab80977..4887f08f 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -359,15 +359,15 @@ function toDot(dfg::GraphsDFG) end #API design NOTE: -# Do not create new Verbs or Nouns for metric vs. topological pathfinding. getPaths is the universal router... getPaths(..., metric) +# Do not create new Verbs or Nouns for metric vs. topological pathfinding. findPaths is the universal router... findPaths(..., metric) # for now we only look at topological paths. -function getPaths(::typeof(all_simple_paths), dfg, from::Symbol, to::Symbol; kwargs...) +function findPaths(::typeof(all_simple_paths), dfg, from::Symbol, to::Symbol; kwargs...) gpaths = Graphs.all_simple_paths(dfg.g, dfg.g.labels[from], dfg.g.labels[to]; kwargs...) return map(p -> (path = map(i -> dfg.g.labels[i], p), dist = length(p) - 1), gpaths) end -function getPaths( +function findPaths( ::typeof(yen_k_shortest_paths), dfg::GraphsDFG, from::Symbol, @@ -390,12 +390,11 @@ function getPaths( end # note with default heuristic this is just dijkstra's algorithm -function getPaths( +function findPaths( ::typeof(a_star), dfg::GraphsDFG, from::Symbol, - to::Symbol, - ::Int; + to::Symbol; distmx::AbstractMatrix{T} = weights(dfg.g), heuristic = nothing, ) where {T} @@ -417,13 +416,12 @@ function getPaths( return [(path = path, dist = dist)] end -#TODO Move getPaths and getPath to AbstractDFG services as default implementations. -function getPaths( +#TODO Move findPaths and findPath to AbstractDFG services as default implementations. +function findPaths( dfg::AbstractDFG, from::Symbol, to::Symbol, k::Int; - algorithm = k == 1 ? a_star : yen_k_shortest_paths, variableLabels::Union{Nothing, Vector{Symbol}} = nothing, factorLabels::Union{Nothing, Vector{Symbol}} = nothing, kwargs..., @@ -449,10 +447,15 @@ function getPaths( !hasFactor(active_dfg, to) && throw(DFG.LabelNotFoundError(to)) - return getPaths(algorithm, active_dfg, from, to, k; kwargs...) + # optimization for k=1 since A* is more efficient than Yen's for single shortest path + if k == 1 + return findPaths(a_star, active_dfg, from, to; kwargs...) + else + return findPaths(yen_k_shortest_paths, active_dfg, from, to, k; kwargs...) + end end -function getPath( +function findPath( dfg::AbstractDFG, from::Symbol, to::Symbol; @@ -460,16 +463,13 @@ function getPath( factorLabels::Union{Nothing, Vector{Symbol}} = nothing, kwargs..., ) - paths = getPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) + paths = findPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) - # Adhere strictly to the "getSingular = Error" rule if isempty(paths) - error( - "No path found between :$(from) and :$(to). If a disconnected graph is expected, use `getPaths` instead.", - ) + return nothing + else + return first(paths) end - - return first(paths) end export bfs_tree diff --git a/src/entities/DFGVariable.jl b/src/entities/DFGVariable.jl index 6e185c74..d68031c8 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -27,7 +27,7 @@ end # TODO naming? Density, DensityRepresentation, BeliefRepresentation, BeliefState, etc? # TODO flatten in State? likeley not for easier serialization of points. @kwdef struct BeliefRepresentation{T <: StateType, P} - statekind::T = T()# NOTE duplication for serialization, TODO maybe only in State and therefore belief cannot deserialize seperately. + statekind::T = T()# NOTE duplication for serialization, TODO maybe only in State and therefore belief cannot deserialize separately. """Discriminator for which representation is active.""" densitykind::AbstractDensityKind = NonparametricDensityKind() diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index 3e5b2c99..b09b5edd 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -382,7 +382,7 @@ Implement `listNeighbors(dfg::AbstractDFG, label::Symbol; solvableFilter, tagsFi function listNeighbors end """ - getPaths(dfg, from::Symbol, to::Symbol, k::Int; variableLabels, factorLabels, kwargs...) + findPaths(dfg, from::Symbol, to::Symbol, k::Int; variableLabels, factorLabels, kwargs...) Return the `k` shortest paths between `from` and `to` in the factor graph. Each result is a `(path = Vector{Symbol}, dist)` named tuple. @@ -395,22 +395,22 @@ Typical usage with filters: ```julia vars = listVariables(dfg; solvableFilter = >=(1)) facs = listFactors(dfg; solvableFilter = >=(1)) -getPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) +findPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) ``` -See also: [`getPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) +See also: [`findPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) """ -function getPaths end +function findPaths end """ - getPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) + findPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) Return the single shortest path between `from` and `to`. -Errors if no path exists (use `getPaths` for graphs that may be disconnected). +Errors if no path exists (use `findPaths` for graphs that may be disconnected). -Accepts the same restriction keywords as [`getPaths`](@ref). +Accepts the same restriction keywords as [`findPaths`](@ref). """ -function getPath end +function findPath end function listNeighbors(dfg::AbstractDFG, node::AbstractGraphNode; kwargs...) return listNeighbors(dfg, getLabel(node); kwargs...) diff --git a/test/testBlocks.jl b/test/testBlocks.jl index c24989e8..5c42104c 100644 --- a/test/testBlocks.jl +++ b/test/testBlocks.jl @@ -1635,7 +1635,7 @@ function ProducingDotFiles( # │ caller = ProducingDotFiles(testDFGAPI::Type{GraphsDFG}, v1::Nothing, v2::Nothing, f1::Nothing; VARTYPE::Type{VariableDFG}, FACTYPE::Type{FactorDFG}) at testBlocks.jl:1440 # └ @ Main ~/.julia/dev/DistributedFactorGraphs/test/testBlocks.jl:1440 addFactor!(dotdfg, f1) - #NOTE hardcoded toDot will have different results so test Graphs seperately + #NOTE hardcoded toDot will have different results so test Graphs separately if testDFGAPI <: GraphsDFG || testDFGAPI <: GraphsDFG todotstr = DFG.toDot(dotdfg) todota = @@ -1862,8 +1862,8 @@ function PathFindingTests(testDFGAPI) addFactor!(dfg, FactorDFG(:x1x3f1, [:x1, :x3], TestFunctorInferenceType1())) addFactor!(dfg, FactorDFG(:x2x4f1, [:x2, :x4], TestFunctorInferenceType1())) - # --- Basic getPaths / getPath (no restrictions) --- - result = getPath(dfg, :x1, :x3) + # --- Basic findPaths / findPath (no restrictions) --- + result = findPath(dfg, :x1, :x3) # shortest path should be via the direct link x1-x1x3f1-x3 (dist=2) not via x2 (dist=4) @test result.path == [:x1, :x1x3f1, :x3] @test result.dist == 2 @@ -1873,19 +1873,19 @@ function PathFindingTests(testDFGAPI) # 2) x1-x1x3f1-x3-x3x4f1-x4 (dist=4) # 3) x1-x1x2f1-x2-x2x3f1-x3-x3x4f1-x4 (dist=6) # 4) x1-x1x3f1-x3-x2x3f1-x2-x2x4f1-x4 (dist=6) - results = getPaths(dfg, :x1, :x4, 4) + results = findPaths(dfg, :x1, :x4, 4) @test length(results) >= 2 @test results[1].dist <= results[end].dist # sorted by distance # path across the whole graph - full_path = getPath(dfg, :x1, :x10) + full_path = findPath(dfg, :x1, :x10) @test :x1 == first(full_path.path) @test :x10 == last(full_path.path) # --- Restrict with variableLabels only (all factors kept) --- # Restrict to x1..x5 variables. Factors connecting only those vars are auto-included. vars_subset = listVariables(dfg; typeFilter = ==(TestVariableType1())) - result_restricted = getPath(dfg, :x1, :x5; variableLabels = vars_subset) + result_restricted = findPath(dfg, :x1, :x5; variableLabels = vars_subset) @test first(result_restricted.path) == :x1 @test last(result_restricted.path) == :x5 # x6..x10 should NOT appear on the path @@ -1894,7 +1894,7 @@ function PathFindingTests(testDFGAPI) # --- Restrict with factorLabels only (all variables kept) --- # Only allow the first 4 factors, path x1→x5 should still work facs_first4 = listFactors(dfg; labelFilter = contains(r"x[1-4](?!\d)")) - result_fac = getPath(dfg, :x1, :x5; factorLabels = facs_first4) + result_fac = findPath(dfg, :x1, :x5; factorLabels = facs_first4) @test first(result_fac.path) == :x1 @test last(result_fac.path) == :x5 @@ -1902,7 +1902,7 @@ function PathFindingTests(testDFGAPI) vars_1to5 = listVariables(dfg; typeFilter = ==(TestVariableType1())) facs_1to4 = listFactors(dfg; labelFilter = contains(r"x[1-4](?!\d)")) result_both = - getPath(dfg, :x1, :x5; variableLabels = vars_1to5, factorLabels = facs_1to4) + findPath(dfg, :x1, :x5; variableLabels = vars_1to5, factorLabels = facs_1to4) @test first(result_both.path) == :x1 @test last(result_both.path) == :x5 @@ -1911,8 +1911,13 @@ function PathFindingTests(testDFGAPI) solvable_vars = listVariables(dfg; solvableFilter = >=(1)) solvable_facs = listFactors(dfg; solvableFilter = >=(1)) # Path from x1 to x7 should work (all solvable) - result_solvable = - getPath(dfg, :x1, :x7; variableLabels = solvable_vars, factorLabels = solvable_facs) + result_solvable = findPath( + dfg, + :x1, + :x7; + variableLabels = solvable_vars, + factorLabels = solvable_facs, + ) @test first(result_solvable.path) == :x1 @test last(result_solvable.path) == :x7 # x8 and x9 (unsolvable) should not appear @@ -1920,7 +1925,7 @@ function PathFindingTests(testDFGAPI) @test :x9 ∉ result_solvable.path # Path from x1 to x10 with solvable filter should fail (x8, x9, x7x8f1 block the way) - paths_blocked = getPaths( + paths_blocked = findPaths( dfg, :x1, :x10, @@ -1930,13 +1935,15 @@ function PathFindingTests(testDFGAPI) ) @test isempty(paths_blocked) - # And the singular getPath should throw on disconnect - @test_throws ErrorException getPath( - dfg, - :x1, - :x10; - variableLabels = solvable_vars, - factorLabels = solvable_facs, + # And the singular findPath should return nothing + @test isnothing( + findPath( + dfg, + :x1, + :x10; + variableLabels = solvable_vars, + factorLabels = solvable_facs, + ), ) # --- With tagsFilter --- @@ -1948,15 +1955,15 @@ function PathFindingTests(testDFGAPI) @test :x4 ∈ landmark_vars # Restrict to only LANDMARK variables - x1 is not a LANDMARK, so include it to enable the path vars_with_x1 = union([:x1, :x2], landmark_vars) - result_tags = getPath(dfg, :x1, :x4; variableLabels = vars_with_x1) + result_tags = findPath(dfg, :x1, :x4; variableLabels = vars_with_x1) @test first(result_tags.path) == :x1 @test last(result_tags.path) == :x4 # x5..x10 should not be on this path @test isempty(intersect(result_tags.path, [:x5, :x6, :x7, :x8, :x9, :x10])) - # --- getPaths with k > 1 on looped graph --- + # --- findPaths with k > 1 on looped graph --- # With cross-links x1x3f1 and x2x4f1. Multiple paths exist from x1 to x4. - results_multi = getPaths(dfg, :x1, :x4, 5) + results_multi = findPaths(dfg, :x1, :x4, 5) @test length(results_multi) >= 2 # All paths should start at x1 and end at x4 for r in results_multi @@ -1969,7 +1976,7 @@ function PathFindingTests(testDFGAPI) # --- Error: no path exists --- # Disconnect x10 by removing the factor deleteFactor!(dfg, :x9x10f1) - @test_throws ErrorException getPath(dfg, :x1, :x10) - empty_paths = getPaths(dfg, :x1, :x10, 1) + @test isnothing(findPath(dfg, :x1, :x10)) + empty_paths = findPaths(dfg, :x1, :x10, 1) @test isempty(empty_paths) end From 295e6c2848d517c02090c8c290a84dc90a1ec4a4 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 26 Mar 2026 17:22:53 +0200 Subject: [PATCH 4/4] missing functions TODO --- src/services/AbstractDFG.jl | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index b09b5edd..b6504f5e 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -251,11 +251,16 @@ Implement `getFactor(dfg::AbstractDFG, label::Symbol)` """ function getFactor end +#TODO implement +function getFactorSkeleton end +function getFactorSummary end """ $(SIGNATURES) Get the skeleton factors from a DFG as a Vector{FactorSkeleton}. """ function getFactorsSkeleton end +#TODO implement +function getFactorsSummary end function Base.getindex(dfg::AbstractDFG, lbl::Symbol) if isVariable(dfg, lbl)