Merge pull request #88 from JuliaRobotics/jt/develop

Integrating @Affie's changes into master (v0.4 release)

Author: GearsAD

.gitignore

@@ -4,3 +4,4 @@
 deps/deps.jl
 docs/build
 Manifest.toml
+dev

Project.toml

@@ -1,8 +1,9 @@
 name = "DistributedFactorGraphs"
 uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
-version = "0.3.2"
+version = "0.4.0"
 
 [deps]
+Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
 DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
 Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
@@ -15,15 +16,16 @@ MetaGraphs = "626554b9-1ddb-594c-aa3c-2596fe9399a5"
 Neo4j = "d2adbeaf-5838-5367-8a2f-e46d570981db"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"
+SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 
 [compat]
 Distributions = "≥ 0.18"
 DocStringExtensions = "≥ 0.7"
 Graphs = "≥ 0.10.3"
+MetaGraphs = "≥ 0.6.4"
 Reexport = "≥ 0.2"
 Requires = "≥ 0.5"
 julia = "0.7, 1"
-MetaGraphs = "≥ 0.6.4"
 
 [extras]
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

src/CloudGraphsDFG/services/CloudGraphsDFG.jl

@@ -704,7 +704,7 @@ 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::CloudGraphsDFG, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::Union{Nothing, CloudGraphsDFG}=nothing)::CloudGraphsDFG where T <: DFGNode
+function getSubgraphAroundNode(dfg::CloudGraphsDFG, node::DFGNode, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::Union{Nothing, CloudGraphsDFG}=nothing)::CloudGraphsDFG
     distance < 1 && error("getSubgraphAroundNode() only works for distance > 0")
 
     # Making a copy session if not specified
@@ -720,6 +720,19 @@ function getSubgraphAroundNode(dfg::CloudGraphsDFG, node::T, distance::Int64=1,
     return addToDFG
 end
 
+function getSubgraphAroundNode(dfg::CloudGraphsDFG{<:AbstractParams}, node::DFGNode, distance::Int64, includeOrphanFactors::Bool, addToDFG::MetaGraphsDFG{AbstractParams})::AbstractDFG
+    distance < 1 && error("getSubgraphAroundNode() only works for distance > 0")
+
+
+    # Thank you Neo4j for 0..* awesomeness!!
+    neighborList = _getLabelsFromCyphonQuery(dfg.neo4jInstance, "(n:$(dfg.userId):$(dfg.robotId):$(dfg.sessionId):$(node.label))-[FACTORGRAPH*0..$distance]-(node:$(dfg.userId):$(dfg.robotId):$(dfg.sessionId))")
+
+    # Copy the section of graph we want
+    _copyIntoGraph!(dfg, addToDFG, neighborList, includeOrphanFactors)
+    return addToDFG
+end
+
+
 """
     $(SIGNATURES)
 Get a deep subgraph copy from the DFG given a list of variables and factors.

src/Common.jl

@@ -5,7 +5,7 @@ export getData
 export getVariableType, getSofttype
 export getFactorType, getfnctype
 export lsTypes, lsfTypes
-
+export lsWho, lsfWho
 
 """
     $(SIGNATURES)
@@ -230,3 +230,35 @@ function lsTypes(dfg::G)::Dict{Symbol, Vector{String}} where G <: AbstractDFG
   end
   return alltypes
 end
+
+
+"""
+    $(SIGNATURES)
+Gives back all factor labels that fit the bill:
+	lsWho(dfg, :Pose3)
+"""
+function lsWho(dfg::AbstractDFG, type::Symbol; solveKey::Symbol=:default)::Vector{Symbol}
+    vars = getVariables(dfg)
+	labels = Symbol[]
+    for v in vars
+		varType = typeof(getVariableType(v, solveKey=solveKey)).name |> Symbol
+		varType == type && push!(labels, v.label)
+	end
+	return labels
+end
+
+
+"""
+    $(SIGNATURES)
+Gives back all factor labels that fit the bill:
+	lsfWho(dfg, :Point2Point2)
+"""
+function lsfWho(dfg::AbstractDFG, type::Symbol)::Vector{Symbol}
+	facs = getFactors(dfg)
+	labels = Symbol[]
+    for f in facs
+		facType = typeof(getFactorType(f)).name |> Symbol
+		facType == type && push!(labels, f.label)
+	end
+	return labels
+end

src/DFGPlots/DFGPlots.jl

@@ -24,36 +24,58 @@ end
 DFGPlotProps() = DFGPlotProps(  (var=colorant"seagreen", fac=colorant"cyan3"),
                                 (var=1.0, fac=0.3),
                                 (var=:box, fac=:elipse),
-                                spring_layout,
+                                spectral_layout,
                                 true)
 
 
-function dfgplot(dfg::LightGraphsDFG)
+function dfgplot(dfg::LightDFG, p::DFGPlotProps = DFGPlotProps())
 
-    nodesize = [has_prop(dfg.g,i,:factor) ? 0.3 : 1.0 for i=vertices(dfg.g)]
-    nodelabel = [has_prop(dfg.g,i,:factor) ? "" : string(get_prop(dfg.g,i,:label)) for i=vertices(dfg.g)]
-    nodefillc = [has_prop(dfg.g,i,:factor) ? colorant"seagreen" : colorant"cyan3" for i=vertices(dfg.g)]
+    nodetypes = [haskey(dfg.g.variables, s) for s in dfg.g.labels]
 
-    gplot(dfg.g, nodelabel=nodelabel, nodesize=nodesize, nodefillc=nodefillc, layout=spectral_layout)
+    nodesize = [isVar ? p.nodesize.var : p.nodesize.fac for isVar in nodetypes]
+    # nodelabel = [isVar ? string(get_prop(dfg.g,i,:label)) : "" for (i,isVar) in enumerate(nodetypes)]
+    if p.drawlabels
+        nodelabel = [nodetypes[i] ? string(s) : "" for (i,s) in enumerate(dfg.g.labels)]
+    else
+        nodelabel = nothing
+    end
+
+    nodefillc = [isVar ? p.nodefillc.var : p.nodefillc.fac for isVar in nodetypes]
+
+    gplot(dfg.g, nodelabel=nodelabel, nodesize=nodesize, nodefillc=nodefillc, layout=p.layout)
 
 end
 
-function gplot(dfg::LightGraphsDFG; keyargs...)
-    gplot(dfg.g; keyargs...)
+function dfgplot(dfg::MetaGraphsDFG, p::DFGPlotProps = DFGPlotProps())
+
+    nodesize = [has_prop(dfg.g,i,:factor) ?  p.nodesize.fac : p.nodesize.var for i=vertices(dfg.g)]
+    if p.drawlabels
+        nodelabel = [has_prop(dfg.g,i,:factor) ? "" : string(get_prop(dfg.g,i,:label)) for i=vertices(dfg.g)]
+    else
+        nodelabel = nothing
+    end
+    nodefillc = [has_prop(dfg.g,i,:factor) ? p.nodefillc.fac : p.nodefillc.var for i=vertices(dfg.g)]
+
+    gplot(dfg.g, nodelabel=nodelabel, nodesize=nodesize, nodefillc=nodefillc, layout=p.layout)
+
 end
 
-function dfgplot(dfg::AbstractDFG)
+function dfgplot(dfg::AbstractDFG, p::DFGPlotProps = DFGPlotProps())
     # TODO implement convert functions
     @warn "TODO Implement convert"
-    ldfg = LightGraphsDFG{AbstractParams}()
+    ldfg = MetaGraphsDFG{AbstractParams}()
     DistributedFactorGraphs._copyIntoGraph!(dfg, ldfg, union(getVariableIds(dfg), getFactorIds(dfg)), true)
 
-    nodesize = [has_prop(ldfg.g,i,:factor) ? 0.3 : 1.0 for i=vertices(ldfg.g)]
-    nodelabel = [has_prop(ldfg.g,i,:factor) ? "" : string(get_prop(ldfg.g,i,:label)) for i=vertices(ldfg.g)]
-    nodefillc = [has_prop(ldfg.g,i,:factor) ? colorant"seagreen" : colorant"cyan3" for i=vertices(ldfg.g)]
+    dfgplot(ldfg, p)
 
-    gplot(ldfg.g, nodelabel=nodelabel, nodesize=nodesize, nodefillc=nodefillc, layout=spectral_layout)
+end
 
+function gplot(dfg::MetaGraphsDFG; keyargs...)
+    gplot(dfg.g; keyargs...)
+end
+
+function gplot(dfg::LightDFG; keyargs...)
+    gplot(dfg.g; keyargs...)
 end
 
 end

src/DistributedFactorGraphs.jl

@@ -8,6 +8,7 @@ using Distributions
 using Reexport
 using JSON2
 using LinearAlgebra
+using SparseArrays
 
 # Entities
 include("entities/AbstractTypes.jl")
@@ -34,6 +35,9 @@ export GenericFunctionNodeData#, FunctionNodeData
 export getSerializationModule, setSerializationModule!
 export pack, unpack
 
+#Interfaces
+export getAdjacencyMatrixSparse
+
 # Common includes
 include("services/AbstractDFG.jl")
 include("services/DFGVariable.jl")
@@ -45,7 +49,13 @@ include("GraphsDFG/GraphsDFG.jl")
 include("FileDFG/FileDFG.jl")
 
 # Include the LightGraphs.jl (MetaGraphs.jl) API.
-include("LightGraphsDFG/LightGraphsDFG.jl")
+include("MetaGraphsDFG/MetaGraphsDFG.jl")
+
+include("SymbolDFG/SymbolDFG.jl")
+@reexport using .SymbolDFGs
+
+include("LightDFG/LightDFG.jl")
+@reexport using .LightDFGs
 
 export saveDFG, loadDFG
 
@@ -58,7 +68,7 @@ function __init__()
             Rows are all factors, columns are all variables, and each cell contains either nothing or the symbol of the relating factor.
             The first column is the factor headings.
             """
-            function getAdjacencyMatrixDataFrame(dfg::Union{GraphsDFG, LightGraphsDFG})::Main.DataFrames.DataFrame
+            function getAdjacencyMatrixDataFrame(dfg::Union{GraphsDFG, MetaGraphsDFG, SymbolDFG, LightDFG})::Main.DataFrames.DataFrame
                 varLabels = sort(map(v->v.label, getVariables(dfg)))
                 factLabels = sort(map(f->f.label, getFactors(dfg)))
                 adjDf = DataFrames.DataFrame(:Factor => Union{Missing, Symbol}[])
@@ -75,18 +85,19 @@ function __init__()
         end
     end
 
-  @require Neo4j="d2adbeaf-5838-5367-8a2f-e46d570981db" begin
-    # Include the Cloudgraphs API
-    include("CloudGraphsDFG/CloudGraphsDFG.jl")
-  end
+    @require Neo4j="d2adbeaf-5838-5367-8a2f-e46d570981db" begin
+        # Include the Cloudgraphs API
+        include("CloudGraphsDFG/CloudGraphsDFG.jl")
+    end
 
-  @require GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231" begin
-      include("DFGPlots/DFGPlots.jl")
-      @reexport using .DFGPlots
-  end
+    @require GraphPlot = "a2cc645c-3eea-5389-862e-a155d0052231" begin
+        include("DFGPlots/DFGPlots.jl")
+        @reexport using .DFGPlots
+    end
 
 end
 
+
 # not sure where to put
 include("Common.jl")
 include("NeedsAHome.jl")

src/GraphsDFG/entities/GraphsDFG.jl

@@ -11,14 +11,21 @@ const FGType = Graphs.GenericIncidenceList{GraphsNode,Graphs.Edge{GraphsNode},Di
 mutable struct GraphsDFG{T <: AbstractParams} <: AbstractDFG
     g::FGType
     description::String
+    userId::String
+    robotId::String
+    sessionId::String
     nodeCounter::Int64
     labelDict::Dict{Symbol, Int64}
     addHistory::Vector{Symbol} #TODO: Discuss more - is this an audit trail?
     solverParams::T # Solver parameters
-    GraphsDFG{T}(g::FGType=Graphs.incdict(GraphsNode,is_directed=false),
-                 d::String="Graphs.jl implementation",
-                 n::Int64=0,
-                 l::Dict{Symbol, Int64}=Dict{Symbol, Int64}(),
-                 a::Vector{Symbol}=Symbol[];
-                 params::T=NoSolverParams()) where T <: AbstractParams = new{T}(g, d, n, l, a, params)
 end
+
+GraphsDFG{T}(   g::FGType=Graphs.incdict(GraphsNode,is_directed=false),
+                d::String="Graphs.jl implementation",
+                n::Int64=0,
+                l::Dict{Symbol, Int64}=Dict{Symbol, Int64}(),
+                a::Vector{Symbol}=Symbol[];
+                userId::String = "UserID",
+                robotId::String = "robotID",
+                sessionId::String = "sessionID",
+                params::T=NoSolverParams()) where T <: AbstractParams = GraphsDFG{T}(g, d, userId, robotId, sessionId, n, l, a, params)

src/GraphsDFG/services/GraphsDFG.jl

@@ -484,6 +484,21 @@ function getAdjacencyMatrix(dfg::GraphsDFG)::Matrix{Union{Nothing, Symbol}}
     return adjMat
 end
 
+function getAdjacencyMatrixSparse(dfg::GraphsDFG)::Tuple{LightGraphs.SparseMatrixCSC, Vector{Symbol}, Vector{Symbol}}
+	varLabels = map(v->v.label, getVariables(dfg))
+	factLabels = map(f->f.label, getFactors(dfg))
+
+	vDict = Dict(varLabels .=> [1:length(varLabels)...])
+
+	adjMat = spzeros(Int, length(factLabels), length(varLabels))
+
+	for (fIndex, factLabel) in enumerate(factLabels)
+		factVars = getNeighbors(dfg, getFactor(dfg, factLabel))
+	    map(vLabel -> adjMat[fIndex,vDict[vLabel]] = 1, factVars)
+	end
+	return adjMat, varLabels, factLabels
+end
+
 """
     $(SIGNATURES)
 Produces a dot-format of the graph for visualization.

src/LightDFG/FactorGraphs/BiMaps.jl

@@ -0,0 +1,53 @@
+# import Base: getindex, setindex!, firstindex, lastindex, iterate, keys, isempty
+struct BiDictMap{T <: Integer}
+    int_sym::Dict{T,Symbol}
+    sym_int::Dict{Symbol,T}
+end
+
+BiDictMap{T}(;sizehint=100) where T<:Integer = begin
+    int_sym = Dict{T,Symbol}()
+    sizehint!(int_sym, sizehint)
+    sym_int = Dict{Symbol,T}()
+    sizehint!(sym_int, sizehint)
+    BiDictMap{T}(int_sym, sym_int)
+end
+
+BiDictMap(;sizehint=100) = BiDictMap{Int}(;sizehint=sizehint)
+
+
+Base.getindex(b::BiDictMap, key::Int) = b.int_sym[key]
+Base.getindex(b::BiDictMap, key::Symbol) = b.sym_int[key]
+
+# setindex!(b, value, key) = b[key] = value
+function Base.setindex!(b::BiDictMap, s::Symbol, i::Int)
+    haskey(b.sym_int, s) && delete!(b.int_sym, b[s])
+    haskey(b.int_sym, i) && delete!(b.sym_int, b[i])
+
+    b.int_sym[i] = s
+    b.sym_int[s] = i
+end
+
+function Base.setindex!(b::BiDictMap, i::Int, s::Symbol)
+    haskey(b.int_sym, i) && delete!(b.sym_int, b[i])
+    haskey(b.sym_int, s) && delete!(b.int_sym, b[s])
+
+    b.int_sym[i] = s
+    b.sym_int[s] = i
+end
+
+function Base.delete!(b::BiDictMap, i::Int)
+    s = b[i]
+    delete!(b.int_sym, i)
+    delete!(b.sym_int, s)
+    return b
+end
+
+Base.haskey(b::BiDictMap, s::Symbol) = haskey(b.sym_int, s)
+Base.haskey(b::BiDictMap, i::Int) = haskey(b.int_sym, i)
+
+Base.length(b::BiDictMap) = length(b.int_sym)
+Base.firstindex(v::BiDictMap) = 1
+Base.lastindex(v::BiDictMap) = length(v.int_sym)
+Base.iterate(v::BiDictMap, i=1) = (length(v.int_sym) < i ? nothing : (v.int_sym[i], i + 1))
+Base.keys(v::BiDictMap) = Base.OneTo(length(v.int_sym))
+Base.isempty(v::BiDictMap) = (length(v.int_sym) == 0)