Merge pull request #717 from JuliaRobotics/feat/4Q20/filtershortestpath

add filters for shortest path

Author: dehann

src/LightDFG/services/LightDFG.jl

@@ -366,6 +366,9 @@ end
 
 Speciallized function available to only LightDFG at this time.
 
+Notes
+- Has option for various types of filters (increases memory usage)
+
 Example
 ```julia
 using IncrementalInference
@@ -379,24 +382,63 @@ fg = generateCanonicalFG_Kaess()
 ```
 
 DevNotes
-- # TODO expand to other AbstractDFG entities.
+- TODO expand to other AbstractDFG entities.
+- TODO use of filter resource consumption can be improved.
 
 Related
 
 [`findFactorsBetweenNaive`](@ref), `LightGraphs.dijkstra_shortest_paths`
 """
 function findShortestPathDijkstra(  dfg::LightDFG, 
                                     from::Symbol,
-                                    to::Symbol  )
+                                    to::Symbol;
+                                    regexVariables::Union{Nothing, Regex}=nothing,
+                                    regexFactors::Union{Nothing, Regex}=nothing,
+                                    tagsVariables::Vector{Symbol}=Symbol[],
+                                    tagsFactors::Vector{Symbol}=Symbol[],
+                                    typeVariables::Union{Nothing, <:AbstractVector}=nothing,
+                                    typeFactors::Union{Nothing, <:AbstractVector}=nothing,
+                                    solvable::Int=0  )
+    #
+    # helper function to filter on vector of types
+    function _filterTypeList(thelist::Vector{Symbol}, typeList, listfnc=x->ls(dfg, x) )
+        thelist_ = Symbol[]
+        for type_ in typeList
+            union!(thelist_, listfnc(type_))
+        end
+        @show thelist_
+        intersect( thelist, thelist_ )
+    end
+
     #
+    duplicate = regexVariables !== nothing ||
+                regexFactors !== nothing   ||
+                0 < length(tagsVariables)  ||
+                0 < length(tagsFactors)    ||
+                typeVariables !== nothing  ||
+                typeFactors !== nothing    ||
+                solvable != 0
+    #
+    dfg_ = if duplicate
+        # use copy if filter is being applied
+        varList = ls(dfg, regexVariables, tags=tagsVariables, solvable=solvable)
+        fctList = lsf(dfg, regexFactors, tags=tagsFactors, solvable=solvable)
+        varList = typeVariables !== nothing ? _filterTypeList(varList, typeVariables) : varList
+        fctList = typeFactors !== nothing ? _filterTypeList(fctList, typeFactors, x->lsf(dfg, x)) : fctList
+        deepcopyGraph(typeof(dfg), dfg, varList, fctList)
+    else
+        # no filter can be used directly
+        dfg
+    end
+
     # LightDFG internally uses Integers 
-    frI = dfg.g.labels[from]
-    toI = dfg.g.labels[to]
+    frI = dfg_.g.labels[from]
+    toI = dfg_.g.labels[to]
 
     # get shortest path from graph provider
-    path_state = LightGraphs.dijkstra_shortest_paths(dfg.g.graph, [frI;])
-    path = LightGraphs.enumerate_paths(path_state, toI)
-    dijkpath = map(x->dfg.g.labels[x], path)
+    path_state = LightGraphs.dijkstra_shortest_paths( dfg_.g.graph, [frI;] )
+    path = LightGraphs.enumerate_paths( path_state, toI )
+    dijkpath = map( x->dfg_.g.labels[x], path )
 
     # return the list of symbols
     return dijkpath

src/entities/DFGFactor.jl

@@ -2,21 +2,22 @@
 ## Abstract Types
 ##==============================================================================
 
+# TODO consider changing this to AbstractFactor
 abstract type FunctorInferenceType <: Function end
 abstract type PackedInferenceType end
 
+abstract type AbstractPrior <: FunctorInferenceType end
+abstract type AbstractRelative <: FunctorInferenceType end
+abstract type AbstractRelativeRoots <: AbstractRelative end
+abstract type AbstractRelativeMinimize <: AbstractRelative end
+
 # NOTE DF, Convolution is IIF idea, but DFG should know about "FactorOperationalMemory"
 # DF, IIF.CommonConvWrapper <: FactorOperationalMemory #
 abstract type FactorOperationalMemory <: Function end
 # TODO to be removed from DFG,
 # we can add to IIF or have IIF.CommonConvWrapper <: FactorOperationalMemory directly
 # abstract type ConvolutionObject <: FactorOperationalMemory end
 
-abstract type AbstractPrior <: FunctorInferenceType end
-abstract type AbstractRelative <: FunctorInferenceType end
-abstract type AbstractRelativeRoots <: AbstractRelative end
-abstract type AbstractRelativeMinimize <: AbstractRelative end
-
 ##==============================================================================
 ## GenericFunctionNodeData
 ##==============================================================================

src/services/AbstractDFG.jl

@@ -1027,12 +1027,12 @@ Related:
 - [`getNeighborhood`](@ref)
 - [`mergeGraph!`](@ref)
 """
-function deepcopyGraph(::Type{T},
-                       sourceDFG::AbstractDFG,
-                       variableLabels::Vector{Symbol} = ls(sourceDFG),
-                       factorLabels::Vector{Symbol} = lsf(sourceDFG);
-                       sessionId::String = "",
-                       kwargs...) where T <: AbstractDFG
+function deepcopyGraph( ::Type{T},
+                        sourceDFG::AbstractDFG,
+                        variableLabels::Vector{Symbol} = ls(sourceDFG),
+                        factorLabels::Vector{Symbol} = lsf(sourceDFG);
+                        sessionId::String = "",
+                        kwargs...) where T <: AbstractDFG
 
     ginfo = [getDFGInfo(sourceDFG)...]
     if sessionId == ""
@@ -1089,17 +1089,24 @@ end
     $SIGNATURES
 Return (::Bool,::Vector{TypeName}) of types between two nodes in the factor graph 
 
+DevNotes
+- Only works on LigthDFG at the moment.
+
 Related
 
 [`LightDFG.findShortestPathDijkstra`](@ref)
 """
 function isPathFactorsHomogeneous(dfg::AbstractDFG, from::Symbol, to::Symbol)
+    # FIXME, must consider all paths, not just shortest...
     pth = intersect(findShortestPathDijkstra(dfg, from, to), lsf(dfg))
     types = getFactorType.(dfg, pth) .|> typeof .|> x->(x).name
     utyp = unique(types)
     (length(utyp) == 1), utyp
 end
 
+function existsPathOfFactorsType(dfg::AbstractDFG, from::Symbol, to::Symbol, ftype::FunctorInferenceType)
+  error("WIP")
+end
 
 ##==============================================================================
 ## Subgraphs and Neighborhoods