diff --git a/src/Deprecated.jl b/src/Deprecated.jl index a0fdee71..becd2687 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) """ @@ -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.findPath[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 separately. +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 findPath 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 findPath(subdfg, from, to).path + else + return findPath(dfg, from, to).path + end +end diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index a3852d05..fa225ddb 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -422,6 +422,8 @@ const unstable_functions::Vector{Symbol} = [ :FactorSummary, :listNeighborhood, :listNeighbors, + :findPath, + :findPaths, :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..ad5a67ac 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -54,7 +54,9 @@ import ...DistributedFactorGraphs: lsf, isConnected, listNeighbors, - buildSubgraph, + findPaths, + findPath, + getSubgraph, getBiadjacencyMatrix, toDot, toDotFile, diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index bbe167e0..4887f08f 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -358,74 +358,118 @@ 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. findPaths is the universal router... findPaths(..., metric) +# for now we only look at topological paths. + +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 findPaths( + ::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] +# note with default heuristic this is just dijkstra's algorithm +function findPaths( + ::typeof(a_star), + dfg::GraphsDFG, + from::Symbol, + to::Symbol; + 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::T}[] + end + + path = [dfg.g.labels[edgepath[1].src]] + dist = zero(T) + for (; dst, src) in edgepath + push!(path, dfg.g.labels[dst]) + dist += distmx[src, dst] + end + + return [(path = path, dist = dist)] +end + +#TODO Move findPaths and findPath to AbstractDFG services as default implementations. +function findPaths( + dfg::AbstractDFG, + from::Symbol, + to::Symbol, + k::Int; + 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 isa(dfg, GraphsDFG) && isnothing(variableLabels) && isnothing(factorLabels) + dfg else - varList + vlabels = something(variableLabels, listVariables(dfg)) + flabels = something(factorLabels, listFactors(dfg)) + labels = vcat(vlabels, flabels) + DFG.getSubgraph( + GraphsDFG{NoSolverParams, VariableSkeleton, FactorSkeleton}, + dfg, + labels, + ) end - DFG.deepcopyGraph(typeof(dfg), dfg, varList, fctList) + !hasVariable(active_dfg, from) && + !hasFactor(active_dfg, from) && + throw(DFG.LabelNotFoundError(from)) + !hasVariable(active_dfg, to) && + !hasFactor(active_dfg, to) && + throw(DFG.LabelNotFoundError(to)) + + # 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 - # no filter can be used directly - dfg - 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 findPaths(yen_k_shortest_paths, active_dfg, from, to, k; kwargs...) end - # GraphsDFG internally uses Integers - frI = dfg_.g.labels[from] - toI = dfg_.g.labels[to] +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 findPath( + dfg::AbstractDFG, + from::Symbol, + to::Symbol; + variableLabels::Union{Nothing, Vector{Symbol}} = nothing, + factorLabels::Union{Nothing, Vector{Symbol}} = nothing, + kwargs..., +) + paths = findPaths(dfg, from, to, 1; variableLabels, factorLabels, kwargs...) - # return the list of symbols - return dijkpath + if isempty(paths) + return nothing + else + return first(paths) + end 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 b95b1373..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) @@ -381,6 +386,37 @@ Implement `listNeighbors(dfg::AbstractDFG, label::Symbol; solvableFilter, tagsFi """ function listNeighbors end +""" + 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. + +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)) +findPaths(dfg, :x1, :x5, 3; variableLabels = vars, factorLabels = facs) +``` + +See also: [`findPath`](@ref), [`listVariables`](@ref), [`listFactors`](@ref) +""" +function findPaths end + +""" + findPath(dfg, from::Symbol, to::Symbol; variableLabels, factorLabels, kwargs...) + +Return the single shortest path between `from` and `to`. +Errors if no path exists (use `findPaths` for graphs that may be disconnected). + +Accepts the same restriction keywords as [`findPaths`](@ref). +""" +function findPath end + function listNeighbors(dfg::AbstractDFG, node::AbstractGraphNode; kwargs...) return listNeighbors(dfg, getLabel(node); kwargs...) end @@ -534,7 +570,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.") @@ -549,13 +585,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/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/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..5c42104c 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 @@ -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 = @@ -1847,3 +1847,136 @@ 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 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 + + # 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 = 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 = 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 = 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 + @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](?!\d)")) + result_fac = findPath(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](?!\d)")) + result_both = + findPath(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 = 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 + @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 = findPaths( + dfg, + :x1, + :x10, + 1; + variableLabels = solvable_vars, + factorLabels = solvable_facs, + ) + @test isempty(paths_blocked) + + # And the singular findPath should return nothing + @test isnothing( + findPath( + 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 = 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])) + + # --- findPaths with k > 1 on looped graph --- + # With cross-links x1x3f1 and x2x4f1. Multiple paths exist from x1 to x4. + 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 + @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 isnothing(findPath(dfg, :x1, :x10)) + empty_paths = findPaths(dfg, :x1, :x10, 1) + @test isempty(empty_paths) +end