Refactor path functions to findPath[s] (#1214)

* Refactor path functions to getPath[s]

* Changes from review and finish changing build -> getSubgraph (probably still wrong verbNoun)

* find is the better verb as the path is still an index to the graph

* missing functions TODO

---------

Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>

Author: Affie

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

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,

src/GraphsDFG/GraphsDFG.jl

@@ -54,7 +54,9 @@ import ...DistributedFactorGraphs:
     lsf,
     isConnected,
     listNeighbors,
-    buildSubgraph,
+    findPaths,
+    findPath,
+    getSubgraph,
     getBiadjacencyMatrix,
     toDot,
     toDotFile,

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

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()