Merge pull request #263 from JuliaRobotics/feature/20Q1/getSubgraphAroundNode

Abstract getSubgraphAroundNode

Author: GearsAD

src/DFGPlots/DFGPlots.jl

@@ -88,7 +88,7 @@ More information at [GraphPlot.jl](https://github.com/JuliaGraphs/GraphPlot.jl)
 function dfgplot(dfg::AbstractDFG, p::DFGPlotProps = DFGPlotProps())
     # TODO implement convert functions
     @warn "TODO Implement convert"
-    ldfg = LightDFG{AbstractParams}()
+    ldfg = LightDFG{NoSolverParams}()
     DistributedFactorGraphs._copyIntoGraph!(dfg, ldfg, union(getVariableIds(dfg), getFactorIds(dfg)), true, copyGraphMetadata=false)
 
     dfgplot(ldfg, p)

src/DistributedFactorGraphs.jl

@@ -26,6 +26,7 @@ export timestamp # DEPRECATED
 export label, getTimestamp, setTimestamp!, tags, setTags!, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, setSolverData!, internalId, smallData, setSmallData!, bigData
 export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
 export addBigDataEntry!, getBigDataEntry, updateBigDataEntry!, deleteBigDataEntry!, getBigDataEntries, getBigDataKeys
+export getNeighborhood, getSubgraph, getSubgraphAroundNode
 
 #Skeleton types
 export SkeletonDFGVariable, SkeletonDFGFactor

src/GraphsDFG/services/GraphsDFG.jl

@@ -327,40 +327,41 @@ function getNeighbors(dfg::GraphsDFG, label::Symbol; solvable::Int=0)::Vector{Sy
     return map(n -> n.dfgNode.label, neighbors)
 end
 
-"""
-    $(SIGNATURES)
-Retrieve a deep subgraph copy around a given variable or factor.
-Optionally provide a distance to specify the number of edges should be followed.
-Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
-Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
-"""
-function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{P}(); solvable::Int=0)::GraphsDFG where {P <: AbstractParams, T <: DFGNode}
-    if !haskey(dfg.labelDict, node.label)
-        error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
-    end
-
-    # Build a list of all unique neighbors inside 'distance'
-    neighborList = Dict{Symbol, Any}()
-    push!(neighborList, node.label => dfg.g.vertices[dfg.labelDict[node.label]])
-    curList = Dict{Symbol, Any}(node.label => dfg.g.vertices[dfg.labelDict[node.label]])
-    for dist in 1:distance
-        newNeighbors = Dict{Symbol, Any}()
-        for (key, node) in curList
-            neighbors = in_neighbors(node, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
-            for neighbor in neighbors
-                if !haskey(neighborList, neighbor.dfgNode.label) && (isSolvable(neighbor.dfgNode) >= solvable)
-                    push!(neighborList, neighbor.dfgNode.label => neighbor)
-                    push!(newNeighbors, neighbor.dfgNode.label => neighbor)
-                end
-            end
-        end
-        curList = newNeighbors
-    end
-
-    # Copy the section of graph we want
-    _copyIntoGraph!(dfg, addToDFG, collect(keys(neighborList)), includeOrphanFactors)
-    return addToDFG
-end
+#NOTE Replaced by abstract function in services/AbstractDFG.jl 
+# """
+#     $(SIGNATURES)
+# Retrieve a deep subgraph copy around a given variable or factor.
+# Optionally provide a distance to specify the number of edges should be followed.
+# Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
+# Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
+# """
+# function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{P}(); solvable::Int=0)::GraphsDFG where {P <: AbstractParams, T <: DFGNode}
+#     if !haskey(dfg.labelDict, node.label)
+#         error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
+#     end
+#
+#     # Build a list of all unique neighbors inside 'distance'
+#     neighborList = Dict{Symbol, Any}()
+#     push!(neighborList, node.label => dfg.g.vertices[dfg.labelDict[node.label]])
+#     curList = Dict{Symbol, Any}(node.label => dfg.g.vertices[dfg.labelDict[node.label]])
+#     for dist in 1:distance
+#         newNeighbors = Dict{Symbol, Any}()
+#         for (key, node) in curList
+#             neighbors = in_neighbors(node, dfg.g) #Don't use out_neighbors! It enforces directiveness even if we don't want it
+#             for neighbor in neighbors
+#                 if !haskey(neighborList, neighbor.dfgNode.label) && (isSolvable(neighbor.dfgNode) >= solvable)
+#                     push!(neighborList, neighbor.dfgNode.label => neighbor)
+#                     push!(newNeighbors, neighbor.dfgNode.label => neighbor)
+#                 end
+#             end
+#         end
+#         curList = newNeighbors
+#     end
+#
+#     # Copy the section of graph we want
+#     _copyIntoGraph!(dfg, addToDFG, collect(keys(neighborList)), includeOrphanFactors)
+#     return addToDFG
+# end
 
 """
     $(SIGNATURES)

src/LightDFG/entities/LightDFG.jl

@@ -36,7 +36,7 @@ function LightDFG{T,V,F}(g::FactorGraph{Int,V,F}=FactorGraph{Int,V,F}();
                            userData::Dict{Symbol, String} = Dict{Symbol, String}(),
                            robotData::Dict{Symbol, String} = Dict{Symbol, String}(),
                            sessionData::Dict{Symbol, String} = Dict{Symbol, String}(),
-                           params::T=NoSolverParams()) where {T <: AbstractParams, V <:AbstractDFGVariable, F<:AbstractDFGFactor}
+                           params::T=T()) where {T <: AbstractParams, V <:AbstractDFGVariable, F<:AbstractDFGFactor}
 
     LightDFG{T,V,F}(g, description, userId, robotId, sessionId, userData, robotData, sessionData, Symbol[], params)
 end

src/SymbolDFG/services/SymbolDFG.jl

@@ -360,44 +360,44 @@ function _copyIntoGraph!(sourceDFG::SymbolDFG, destDFG::SymbolDFG, labels::Vecto
     return nothing
 end
 
-
-"""
-    $(SIGNATURES)
-Retrieve a deep subgraph copy around a given variable or factor.
-Optionally provide a distance to specify the number of edges should be followed.
-Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
-Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
-"""
-function getSubgraphAroundNode(dfg::SymbolDFG, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::SymbolDFG=SymbolDFG{AbstractParams}(); solvable::Int=0)::SymbolDFG
-    if !exists(dfg,node.label)
-        error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
-    end
-
-    nodelabel = node.label
-    # Build a list of all unique neighbors inside 'distance'
-    neighborList = Symbol[nodelabel]
-
-    curList = Symbol[nodelabel]
-
-    for dist in 1:distance
-        newNeighbors = Symbol[]
-        for cl in curList
-            neighbors = outneighbors(dfg.g, cl)
-            for neighbor in neighbors
-                if !(neighbor in neighborList) && _isSolvable(dfg, neighbor, solvable)
-                    push!(neighborList, neighbor)
-                    push!(newNeighbors, neighbor)
-                end
-            end
-        end
-        curList = newNeighbors
-    end
-
-
-    # _copyIntoGraph!(dfg, addToDFG, map(n->get_prop(dfg.g, n, :label), ns), includeOrphanFactors)
-    _copyIntoGraph!(dfg, addToDFG, neighborList, includeOrphanFactors)
-    return addToDFG
-end
+# moved to abstract 
+# """
+#     $(SIGNATURES)
+# Retrieve a deep subgraph copy around a given variable or factor.
+# Optionally provide a distance to specify the number of edges should be followed.
+# Optionally provide an existing subgraph addToDFG, the extracted nodes will be copied into this graph. By default a new subgraph will be created.
+# Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
+# """
+# function getSubgraphAroundNode(dfg::SymbolDFG, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::SymbolDFG=SymbolDFG{AbstractParams}(); solvable::Int=0)::SymbolDFG
+#     if !exists(dfg,node.label)
+#         error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
+#     end
+#
+#     nodelabel = node.label
+#     # Build a list of all unique neighbors inside 'distance'
+#     neighborList = Symbol[nodelabel]
+#
+#     curList = Symbol[nodelabel]
+#
+#     for dist in 1:distance
+#         newNeighbors = Symbol[]
+#         for cl in curList
+#             neighbors = outneighbors(dfg.g, cl)
+#             for neighbor in neighbors
+#                 if !(neighbor in neighborList) && _isSolvable(dfg, neighbor, solvable)
+#                     push!(neighborList, neighbor)
+#                     push!(newNeighbors, neighbor)
+#                 end
+#             end
+#         end
+#         curList = newNeighbors
+#     end
+#
+#
+#     # _copyIntoGraph!(dfg, addToDFG, map(n->get_prop(dfg.g, n, :label), ns), includeOrphanFactors)
+#     _copyIntoGraph!(dfg, addToDFG, neighborList, includeOrphanFactors)
+#     return addToDFG
+# end
 
 
 """

src/needsahome.jl

@@ -25,7 +25,7 @@ function buildSubgraphFromLabels!(dfg::G,
                                   syms::Vector{Symbol};
                                   subfg::AbstractDFG=(G <: InMemoryDFGTypes ? G : GraphsDFG)(params=getSolverParams(dfg)),
                                   solvable::Int=0,
-                                  allowedFactors::Union{Nothing, Vector{Symbol}}=nothing  )::G where G <: AbstractDFG
+                                  allowedFactors::Union{Nothing, Vector{Symbol}}=nothing  )::AbstractDFG where G <: AbstractDFG
   #
 
   # add a little too many variables (since we need the factors)

src/services/AbstractDFG.jl

@@ -418,6 +418,30 @@ end
 
 # TODO: NEED TO FIGURE OUT SIGNATURE FOR DEFAULT ARGS
 
+
+# TODO export, test and overwrite in LightGraphs and CloudGraphsDFG
+"""
+    $(SIGNATURES)
+Build a list of all unique neighbors inside 'distance'
+"""
+function getNeighborhood(dfg::AbstractDFG, label::Symbol, distance::Int)::Vector{Symbol}
+    neighborList = Set{Symbol}([label])
+    curList = Set{Symbol}([label])
+
+    for dist in 1:distance
+        newNeighbors = Set{Symbol}()
+        for node in curList
+            neighbors = getNeighbors(dfg, node)
+            for neighbor in neighbors
+                push!(neighborList, neighbor)
+                push!(newNeighbors, neighbor)
+            end
+        end
+        curList = newNeighbors
+    end
+    return collect(neighborList)
+end
+
 """
     $(SIGNATURES)
 Retrieve a deep subgraph copy around a given variable or factor.
@@ -426,8 +450,20 @@ Optionally provide an existing subgraph addToDFG, the extracted nodes will be co
 Note: By default orphaned factors (where the subgraph does not contain all the related variables) are not returned. Set includeOrphanFactors to return the orphans irrespective of whether the subgraph contains all the variables.
 Note: Always returns the node at the center, but filters around it if solvable is set.
 """
-function getSubgraphAroundNode(dfg::G, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::H=_getDuplicatedEmptyDFG(dfg); solvable::Int=0)::G where {G <: AbstractDFG, H <: AbstractDFG, P <: AbstractParams, T <: DFGNode}
-    error("getSubgraphAroundNode not implemented for $(typeof(dfg))")
+function getSubgraphAroundNode(dfg::AbstractDFG, node::DFGNode, distance::Int=1, includeOrphanFactors::Bool=false, addToDFG::AbstractDFG=_getDuplicatedEmptyDFG(dfg); solvable::Int=0)::AbstractDFG
+
+    if !exists(dfg, node.label)
+        error("Variable/factor with label '$(node.label)' does not exist in the factor graph")
+    end
+
+    neighbors = getNeighborhood(dfg, node.label, distance)
+
+    # for some reason: always returns the node at the center with  || (nlbl == node.label)
+    solvable != 0 && filter!(nlbl -> (getSolvable(dfg, nlbl) >= solvable) || (nlbl == node.label), neighbors)
+
+    # Copy the section of graph we want
+    _copyIntoGraph!(dfg, addToDFG, neighbors, includeOrphanFactors)
+    return addToDFG
 end
 
 """

test/interfaceTests.jl

@@ -416,6 +416,10 @@ end
     # Filter - always returns the node you start at but filters around that.
     dfgSubgraph = getSubgraphAroundNode(dfg, getFactor(dfg, :x7x8f1), 1, true, solvable=1)
     @test symdiff([:x7x8f1, :x7], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == []
+    # Test for distance = 2, should return orphans
+    #:x7x8f1 is not solvable
+    dfgSubgraph = getSubgraphAroundNode(dfg, getVariable(dfg, :x8), 2, true, solvable=1)
+    @test symdiff([:x8, :x7], [ls(dfgSubgraph)..., lsf(dfgSubgraph)...]) == []
 
     # DFG issue #95 - confirming that getSubgraphAroundNode retains order
     # REF: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/95