Merge pull request #209 from JuliaRobotics/feature/4Q19/solvablesearch

adding solvable for tree work

Author: GearsAD

src/GraphsDFG/services/GraphsDFG.jl

@@ -238,9 +238,9 @@ Optionally specify a label regular expression to retrieves a subset of the varia
 function getVariables(dfg::GraphsDFG,
                       regexFilter::Union{Nothing, Regex}=nothing;
                       tags::Vector{Symbol}=Symbol[],
-                      solvable::Int=0  )::Vector{DFGVariable}
+                      solvable::Int=0)::Vector{DFGVariable}
     #
-    variables = map(v -> v.dfgNode, filter(n -> (n.dfgNode isa DFGVariable) && (solvable <= isSolvable(n.dfgNode)), Graphs.vertices(dfg.g)))
+    variables = map(v -> v.dfgNode, filter(n -> (n.dfgNode isa DFGVariable) && (solvable != 0 ? solvable <= isSolvable(n.dfgNode) : true), Graphs.vertices(dfg.g)))
     # filter on solvable
 
     # filter on regex
@@ -261,8 +261,9 @@ end
 List the DFGFactors in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the factors.
 """
-function getFactors(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{DFGFactor}
-	factors = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGFactor, Graphs.vertices(dfg.g)))
+function getFactors(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{DFGFactor}
+	factors = map(v -> v.dfgNode, filter(n -> (n.dfgNode isa DFGFactor) && (solvable != 0 ? solvable <= isSolvable(n.dfgNode) : true), Graphs.vertices(dfg.g)))
+
 	if regexFilter != nothing
 		factors = filter(f -> occursin(regexFilter, String(f.label)), factors)
 	end
@@ -281,14 +282,14 @@ end
     $(SIGNATURES)
 Retrieve a list of labels of the immediate neighbors around a given variable or factor.
 """
-function getNeighbors(dfg::GraphsDFG, node::T; ready::Union{Nothing, Int}=nothing, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}  where T <: DFGNode
+function getNeighbors(dfg::GraphsDFG, node::T; solvable::Int=0, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}  where T <: DFGNode
     if !haskey(dfg.labelDict, node.label)
         error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
     end
     vert = dfg.g.vertices[dfg.labelDict[node.label]]
     neighbors = in_neighbors(vert, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
     # Additional filtering
-	neighbors = ready != nothing ? filter(v -> isSolvable(v.dfgNode) >= ready, neighbors) : neighbors
+	neighbors = solvable != 0 ? filter(v -> solvable <= isSolvable(v.dfgNode), neighbors) : neighbors
     neighbors = backendset != nothing ? filter(v -> isSolveInProgress(v.dfgNode) == backendset, neighbors) : neighbors
     # Variable sorting (order is important)
     if node isa DFGFactor
@@ -302,14 +303,14 @@ end
     $(SIGNATURES)
 Retrieve a list of labels of the immediate neighbors around a given variable or factor specified by its label.
 """
-function getNeighbors(dfg::GraphsDFG, label::Symbol; ready::Union{Nothing, Int}=nothing, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}  where T <: DFGNode
+function getNeighbors(dfg::GraphsDFG, label::Symbol; solvable::Int=0, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}  where T <: DFGNode
     if !haskey(dfg.labelDict, label)
         error("Variable/factor with label '$(label)' does not exist in the factor graph")
     end
     vert = dfg.g.vertices[dfg.labelDict[label]]
     neighbors = in_neighbors(vert, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
     # Additional filtering
-    neighbors = ready != nothing ? filter(v -> isSolvable(v.dfgNode) >= ready, neighbors) : neighbors
+    neighbors = solvable != 0 ? filter(v -> isSolvable(v.dfgNode) >= solvable, neighbors) : neighbors
     neighbors = backendset != nothing ? filter(v -> isSolveInProgress(v.dfgNode) == backendset, neighbors) : neighbors
     # Variable sorting when using a factor (function order is important)
     if vert.dfgNode isa DFGFactor

src/LightDFG/services/LightDFG.jl

@@ -194,13 +194,16 @@ end
 List the DFGVariables in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the variables.
 """
-function getVariables(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[])::Vector{AbstractDFGVariable}
+function getVariables(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[], solvable::Int=0)::Vector{AbstractDFGVariable}
 
 	# variables = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGVariable, vertices(dfg.g)))
 	variables = collect(values(dfg.g.variables))
     if regexFilter != nothing
         variables = filter(v -> occursin(regexFilter, String(v.label)), variables)
     end
+	if solvable != 0
+		variables = filter(v -> _isSolvable(dfg, v.label, solvable), variables)
+	end
 	if length(tags) > 0
         mask = map(v -> length(intersect(v.tags, tags)) > 0, variables )
         return variables[mask]
@@ -223,14 +226,15 @@ Related
 ls
 """
 
-function getVariableIds(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[])::Vector{Symbol}
+function getVariableIds(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; tags::Vector{Symbol}=Symbol[], solvable::Int=0)::Vector{Symbol}
 
 	# variables = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGVariable, vertices(dfg.g)))
 	if length(tags) > 0
-		return map(v -> v.label, getVariables(dfg, regexFilter, tags=tags))
+		return map(v -> v.label, getVariables(dfg, regexFilter, tags=tags, solvable=solvable))
 	else
 		variables = collect(keys(dfg.g.variables))
 		regexFilter != nothing && (variables = filter(v -> occursin(regexFilter, String(v)), variables))
+		solvable != 0 && (variables = filter(vId -> _isSolvable(dfg, vId, solvable), variables))
 		return variables
     end
 end
@@ -240,12 +244,15 @@ end
 List the DFGFactors in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the factors.
 """
-function getFactors(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{AbstractDFGFactor}
+function getFactors(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{AbstractDFGFactor}
 	# factors = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGFactor, vertices(dfg.g)))
 	factors = collect(values(dfg.g.factors))
 	if regexFilter != nothing
 		factors = filter(f -> occursin(regexFilter, String(f.label)), factors)
 	end
+	if solvable != 0
+		factors = filter(f -> _isSolvable(dfg, f.label, solvable), factors)
+	end
 	return factors
 end
 
@@ -254,12 +261,15 @@ end
 Get a list of the IDs of the DFGFactors in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the factors.
 """
-function getFactorIds(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{Symbol}
+function getFactorIds(dfg::LightDFG, regexFilter::Union{Nothing, Regex}=nothing; solvable::Int=0)::Vector{Symbol}
 	# factors = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGFactor, vertices(dfg.g)))
 	factors = collect(keys(dfg.g.factors))
 	if regexFilter != nothing
 		factors = filter(f -> occursin(regexFilter, String(f)), factors)
 	end
+	if solvable != 0
+		factors = filter(fId -> _isSolvable(dfg, fId, solvable), factors)
+	end
 	return factors
 end
 
@@ -271,7 +281,7 @@ function isFullyConnected(dfg::LightDFG)::Bool
     return length(LightGraphs.connected_components(dfg.g)) == 1
 end
 
-function _isready(dfg::LightDFG, label::Symbol, ready::Int)::Bool
+function _isSolvable(dfg::LightDFG, label::Symbol, ready::Int)::Bool
 
 	haskey(dfg.g.variables, label) && (return dfg.g.variables[label].solvable >= ready)
 	haskey(dfg.g.factors, label) && (return dfg.g.factors[label].solvable >= ready)
@@ -294,7 +304,7 @@ end
     $(SIGNATURES)
 Retrieve a list of labels of the immediate neighbors around a given variable or factor.
 """
-function getNeighbors(dfg::LightDFG, node::DFGNode; ready::Union{Nothing, Int}=nothing, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}
+function getNeighbors(dfg::LightDFG, node::DFGNode; solvable::Int=0, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}
 	label = node.label
     if !exists(dfg, label)
         error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
@@ -303,7 +313,7 @@ function getNeighbors(dfg::LightDFG, node::DFGNode; ready::Union{Nothing, Int}=n
 	neighbors_il =  FactorGraphs.outneighbors(dfg.g, dfg.g.labels[label])
 	neighbors_ll = [dfg.g.labels[i] for i in neighbors_il]
     # Additional filtering
-    ready != nothing && filter!(lbl -> _isready(dfg, lbl, ready), neighbors_ll)
+    solvable != 0 && filter!(lbl -> _isSolvable(dfg, lbl, solvable), neighbors_ll)
 	backendset != nothing && filter!(lbl -> _isbackendset(dfg, lbl, backendset), neighbors_ll)
 
     # Variable sorting (order is important)
@@ -320,15 +330,15 @@ end
     $(SIGNATURES)
 Retrieve a list of labels of the immediate neighbors around a given variable or factor specified by its label.
 """
-function getNeighbors(dfg::LightDFG, label::Symbol; ready::Union{Nothing, Int}=nothing, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}
+function getNeighbors(dfg::LightDFG, label::Symbol; solvable::Int=0, backendset::Union{Nothing, Int}=nothing)::Vector{Symbol}
 	if !exists(dfg, label)
         error("Variable/factor with label '$(label)' does not exist in the factor graph")
     end
 
 	neighbors_il =  FactorGraphs.outneighbors(dfg.g, dfg.g.labels[label])
 	neighbors_ll = [dfg.g.labels[i] for i in neighbors_il]
     # Additional filtering
-    ready != nothing && filter!(lbl -> _isready(dfg, lbl, ready), neighbors_ll)
+    solvable != 0 && filter!(lbl -> _isSolvable(dfg, lbl, solvable), neighbors_ll)
 	backendset != nothing && filter!(lbl -> _isbackendset(dfg, lbl, backendset), neighbors_ll)
 
     # Variable sorting (order is important)
@@ -426,10 +436,10 @@ Get an adjacency matrix for the DFG, returned as a Matrix{Union{Nothing, Symbol}
 Rows are all factors, columns are all variables, and each cell contains either nothing or the symbol of the relating factor.
 The first row and first column are factor and variable headings respectively.
 """
-function getAdjacencyMatrix(dfg::LightDFG)::Matrix{Union{Nothing, Symbol}}
+function getAdjacencyMatrix(dfg::LightDFG; solvable::Int=0)::Matrix{Union{Nothing, Symbol}}
 	#TODO Why does it need to be sorted?
-	varLabels = sort(collect(keys(dfg.g.variables)))#ort(map(v->v.label, getVariables(dfg)))
-    factLabels = sort(collect(keys(dfg.g.factors)))#sort(map(f->f.label, getFactors(dfg)))
+	varLabels = sort(getVariableIds(dfg, solvable=solvable))#ort(map(v->v.label, getVariables(dfg)))
+    factLabels = sort(getFactorIds(dfg, solvable=solvable))#sort(map(f->f.label, getFactors(dfg)))
     vDict = Dict(varLabels .=> [1:length(varLabels)...].+1)
 
     adjMat = Matrix{Union{Nothing, Symbol}}(nothing, length(factLabels)+1, length(varLabels)+1)
@@ -443,9 +453,9 @@ function getAdjacencyMatrix(dfg::LightDFG)::Matrix{Union{Nothing, Symbol}}
     return adjMat
 end
 
-function getAdjacencyMatrixSparse(dfg::LightDFG)::Tuple{LightGraphs.SparseMatrixCSC, Vector{Symbol}, Vector{Symbol}}
-	varLabels = collect(keys(dfg.g.variables))
-    factLabels = collect(keys(dfg.g.factors))
+function getAdjacencyMatrixSparse(dfg::LightDFG; solvable::Int=0)::Tuple{LightGraphs.SparseMatrixCSC, Vector{Symbol}, Vector{Symbol}}
+	varLabels = getVariableIds(dfg, solvable=solvable)
+    factLabels = getFactorIds(dfg, solvable=solvable)
 	varIndex = [dfg.g.labels[s] for s in varLabels]
 	factIndex = [dfg.g.labels[s] for s in factLabels]
 

src/SymbolDFG/SymbolDFG.jl

@@ -5,7 +5,7 @@ using SparseArrays
 using DocStringExtensions
 
 import ...DistributedFactorGraphs: AbstractDFG, DFGNode, AbstractParams, NoSolverParams, DFGVariable, DFGFactor
-# import DFG functions to exstend
+# import DFG functions to extend
 import ...DistributedFactorGraphs:  setSolverParams,
                                     getFactor,
                                     setDescription,