WIP getVariableSolverData, Cleanup, API updates on user/robot/session data, toDot for plotting,

Affie · Affie · commit 6d49afcc0c58 · 2020-01-20T17:05:09.000+02:00
diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl
@@ -87,8 +87,11 @@ export MeanMaxPPE
 export setSerializationModule!, getSerializationModule
 export getLabelDict, getDescription, setDescription, getAddHistory, getSolverParams, setSolverParams
 export getUserData, setUserData, getRobotData, setRobotData, getSessionData, setSessionData
-# Not sure these are going to work everywhere
-export pushUserData!, pushRobotData!, pushSessionData!, popUserData!, popRobotData!, popSessionData!
+
+# Not sure these are going to work everywhere, TODO implement in cloud?
+export updateUserData!, updateRobotData!, updateSessionData!, deleteUserData!, deleteRobotData!, deleteSessionData!
+export emptyUserData!, emptyRobotData!, emptySessionData!
+
 
 export exists, addVariable!, addFactor!, getVariable, getFactor, updateVariable!, updateFactor!, deleteVariable!, deleteFactor!
 export getVariables, getVariableIds, getFactors, getFactorIds, ls, lsf
diff --git a/src/LightDFG/LightDFG.jl b/src/LightDFG/LightDFG.jl
@@ -42,7 +42,9 @@ import ...DistributedFactorGraphs:  setSolverParams,
                                     getSubgraph,
                                     getIncidenceMatrix,
                                     getIncidenceMatrixSparse,
-                                    _getDuplicatedEmptyDFG
+                                    _getDuplicatedEmptyDFG,
+                                    toDot,
+                                    toDotFile
 
 include("FactorGraphs/FactorGraphs.jl")
 using .FactorGraphs
diff --git a/src/LightDFG/services/LightDFG.jl b/src/LightDFG/services/LightDFG.jl
@@ -42,11 +42,11 @@ exists(dfg::LightDFG, nId::Symbol) = haskey(dfg.g.labels, nId)
 exists(dfg::LightDFG, node::DFGNode) = exists(dfg, node.label)
 
 function isVariable(dfg::LightDFG{P,V,F}, sym::Symbol) where {P <: AbstractParams, V <: AbstractDFGVariable, F <: AbstractDFGFactor}
-	return haskey(dfg.g.variables, sym)
+    return haskey(dfg.g.variables, sym)
 end
 
 function isFactor(dfg::LightDFG{P,V,F}, sym::Symbol) where {P <: AbstractParams, V <: AbstractDFGVariable, F <: AbstractDFGFactor}
-	return haskey(dfg.g.factors, sym)
+    return haskey(dfg.g.factors, sym)
 end
 
 
@@ -113,17 +113,34 @@ function addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, f
     return FactorGraphs.addFactor!(dfg.g, variableLabels, factor)
 end
 
+#TODO Sam, I would say if we handle the update vsd corectly this function is only needed in the cloud.
+#   In memory types are just passing pointers along so no perfomrance issue.
 function getVariable(dfg::LightDFG, label::Symbol; solveKey::Union{Nothing, Symbol})::Union{Nothing, AbstractDFGVariable}
-	!(label in dfg.g.variables) && return nothing
-    solveKey == nothing && return dfg.g.variables[label]
-	# Requesting a single solvekey
-	if solveKey in dfg.g.variables[label]
-		v = copy(dfg.g.variables[label]) # Shallow copy
-		v.solverDataDict = [solveKey=>v.solverDataDict[solveKey]]
-		return v
-	else
-		return nothing
-	end
+
+    if !haskey(dfg.g.variables, label)
+        @warn "Variable '$label' does not exists in the factor graph"
+        return nothing
+    end
+
+    var = dfg.g.variables[label]
+
+    if solveKey != nothing && !haskey(var.solverDataDict, solveKey)
+        @warn "Solvekey '$solveKey' does not exists in the variable"
+        return nothing
+    end
+
+    return var
+
+    # !(label in dfg.g.variables) && return nothing
+    # solveKey == nothing && return dfg.g.variables[label]
+    # # Requesting a single solvekey
+    # if solveKey in dfg.g.variables[label]
+    #     v = copy(dfg.g.variables[label]) # Shallow copy
+    #     v.solverDataDict = [solveKey=>v.solverDataDict[solveKey]]
+    #     return v
+    # else
+    #     return nothing
+    # end
 end
 
 function getFactor(dfg::LightDFG, label::Symbol)::AbstractDFGFactor
@@ -360,7 +377,9 @@ function getIncidenceMatrix(dfg::LightDFG; solvable::Int=0)::Matrix{Union{Nothin
     return adjMat
 end
 
-function getIncidenceMatrixSparse(dfg::LightDFG; solvable::Int=0)::Tuple{LightGraphs.SparseMatrixCSC, Vector{Symbol}, Vector{Symbol}}
+#TODO This is just way too strange to call a function  getIncidenceMatrix that calls adjacency_matrix internally,
+# So I'm going with Biadjacency Matrix https://en.wikipedia.org/wiki/Adjacency_matrix#Of_a_bipartite_graph
+function getBiadjacencyMatrixSparse(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]
@@ -372,6 +391,16 @@ function getIncidenceMatrixSparse(dfg::LightDFG; solvable::Int=0)::Tuple{LightGr
     return adjvf, varLabels, factLabels
 end
 
+function getAdjacencyMatrixSparse(dfg::LightDFG; solvable::Int=0)
+    @warn "Deprecated function, please use getBiadjacencyMatrixSparse as this will be removed in v0.6.1"
+    return getBiadjacencyMatrixSparse(dfg, solvable)
+end
+
+# this would be an incidence matrix
+function getIncidenceMatrixSparse(dfg::LightDFG)
+    return incidence_matrix(dfg.g)
+end
+
 """
     $(SIGNATURES)
 Gets an empty and unique LightDFG derived from an existing DFG.
@@ -383,3 +412,40 @@ function _getDuplicatedEmptyDFG(dfg::LightDFG{P,V,F})::LightDFG where {P <: Abst
     newDfg.description ="(Copy of) $(dfg.description)"
     return newDfg
 end
+
+
+#TODO JT test.
+"""
+    $(SIGNATURES)
+A replacement for to_dot that saves only hardcoded factor graph plotting attributes.
+"""
+function savedot_attributes(io::IO, dfg::LightDFG)
+    write(io, "graph G {\n")
+
+    for vl in getVariableIds(dfg)
+        write(io, "$vl [color=red, shape=ellipse];\n")
+    end
+    for fl in getFactorIds(dfg)
+        write(io, "$fl [color=blue, shape=box];\n")
+    end
+
+    for e in edges(dfg.g)
+        write(io, "$(dfg.g.labels[src(e)]) -- $(dfg.g.labels[dst(e)])\n")
+    end
+    write(io, "}\n")
+end
+
+function toDotFile(dfg::LightDFG, fileName::String="/tmp/dfg.dot")::Nothing
+    open(fileName, "w") do fid
+        savedot_attributes(fid, dfg)
+    end
+    return nothing
+end
+
+function toDot(dfg::LightDFG)::String
+    m = PipeBuffer()
+    savedot_attributes(m, dfg)
+    data = take!(m)
+    close(m)
+    return String(data)
+end
diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl
@@ -54,37 +54,34 @@ mutable struct DFGFactor{T, S} <: AbstractDFGFactor
     """Solvable flag for the factor.
     Accessors: `getSolvable`, `setSolvable!`
     TODO: Switch to `DFGNodeParams`"""
-    solvable::Int
+    solvable::Int #TODO we can go with DFGNodeParams or Reference the solvable field with getproperty overload
     """Mutable parameters for the variable. We suggest using accessors to get to this data.
     Accessors: `getSolvable`, `setSolvable!`"""
     _dfgNodeParams::DFGNodeParams
     """Internal cache of the ordering of the neighbor variables. Rather use getNeighbors to get the list as this is an internal value.
     Accessors: `getVariableOrder`"""
     _variableOrderSymbols::Vector{Symbol}
-    DFGFactor{T, S}(label::Symbol, _internalId::Int64=0, timestamp::DateTime=now()) where {T, S} = new{T, S}(label, timestamp, Symbol[], GenericFunctionNodeData{T, S}(), 0, 0, Symbol[])
-    DFGFactor{T, S}(label::Symbol,
-        timestamp::DateTime,
-        tags::Vector{Symbol},
-        solverData::GenericFunctionNodeData{T, S},
-        solvable::Int,
-        _dfgNodeParams::DFGNodeParams,
-        _variableOrderSymbols::Vector{Symbol}) where {T, S} = new(label, timestamp, tags, solverData, solvable, _dfgNodeParams, _variableOrderSymbols)
 end
 
 """
 $(SIGNATURES)
 
 Construct a DFG factor given a label.
 """
+#TODO _internalId?
+DFGFactor{T, S}(label::Symbol, _internalId::Int64=0, timestamp::DateTime=now()) where {T, S}
+            = DFGFactor(label, timestamp, Symbol[], GenericFunctionNodeData{T, S}(), 0, DFGNodeParams(0, _internalId), Symbol[])
+
 DFGFactor(label::Symbol;
+    timestamp::DateTime=now(),
     tags::Vector{Symbol}=Symbol[],
     data::GenericFunctionNodeData{T, S}=GenericFunctionNodeData{T, S}(),
     solvable::Int=0,
-    timestamp::DateTime=now(),
     _internalId::Int64=0,
     _variableOrderSymbols::Vector{Symbol}=Symbol[]) where {T, S} =
         DFGFactor{T,S}(label,timestamp,tags,data,solvable,DFGNodeParams(solvable, _internalId),_variableOrderSymbols)
 
+
 # Simply for convenience - don't export
 const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString}
 PackedFunctionNodeData(x1, x2, x3, x4, x5::S, x6::T, x7::String="", x8::Vector{Int}=Int[], x9::Int=0) where {T <: PackedInferenceType, S <: AbstractString} = GenericFunctionNodeData(x1, x2, x3, x4, x5, x6, x7, x8, x9)
diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl
@@ -94,13 +94,23 @@ function setSessionData(dfg::AbstractDFG, data::Dict{Symbol, String})::Bool
     return true
 end
 
-pushUserData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
-pushRobotData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
-pushSessionData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
+#NOTE with API standardization this should become something like:
+# JT, I however do not feel we should force it, as I prever dot notation
+getUserData(dfg::AbstractDFG, key::Symbol)::Dict{Symbol, String} = dfg.UserData[key]
+getRobotData(dfg::AbstractDFG, key::Symbol)::Dict{Symbol, String} = dfg.RobotData[key]
+getSessionData(dfg::AbstractDFG, key::Symbol)::Dict{Symbol, String} = dfg.SessionData[key]
 
-popUserData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
-popRobotData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
-popSessionData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
+updateUserData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
+updateRobotData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
+updateSessionData!(dfg::AbstractDFG, pair::Pair{Symbol,String}) = push!(dfg.userData, pair)
+
+deleteUserData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
+deleteRobotData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
+deleteSessionData!(dfg::AbstractDFG, key::Symbol) = pop!(dfg.userData, key)
+
+emptyUserData!(dfg::AbstractDFG) = empty!(dfg.userData)
+emptyRobotData!(dfg::AbstractDFG) = empty!(dfg.userData)
+emptySessionData!(dfg::AbstractDFG) = empty!(dfg.userData)
 
 """
     $(SIGNATURES)
@@ -432,6 +442,88 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{
     return nothing
 end
 
+#TODO UNTESTED and NOT FILLED IN, just to define function signatures
+"""
+    $(SIGNATURES)
+"""
+function getVariableSolverData(dfg::AbstractDFG, variablekey::Symbol, solvekey::Symbol=:default)
+    return solverData(getVariable(dfg, variablekey), solvekey)
+end
+
+"""
+    $(SIGNATURES)
+
+"""
+function addVariableSolverData!(dfg::AbstractDFG, variablekey::Symbol, vnd::VariableNodeData, solvekey::Symbol=:default)
+    #for InMemoryDFGTypes, cloud would update here
+    error("not implemented")
+
+    var = getVariable(dfg, variablekey)
+
+    if haskey(var.solverDataDict, solvekey)
+        error("VariableNodeData '$(solvekey)' already exists")
+    end
+
+    var.solverDataDict[solvekey] = vnd
+    # setSolverData!(var, vnd, solvekey)
+
+end
+
+"""
+    $(SIGNATURES)
+Add a new solver data  entry from a deepcopy of the source variable solver data.
+"""
+addVariableSolverData!(dfg::AbstractDFG, sourceVariable::DFGVariable, solvekey::Symbol=:default) =
+    addVariableSolverData!(dfg, sourceVariable.label, deepcopy(solverData(sourceVariable, solvekey)), solvekey)
+
+
+"""
+    $(SIGNATURES)
+"""
+function updateVariableSolverData!(dfg::AbstractDFG, variablekey::Symbol, vnd::VariableNodeData, solvekey::Symbol=:default)
+
+    #This is basically just setSolverData
+    var = getVariable(dfg, variablekey)
+
+    #for InMemoryDFGTypes, cloud would update here
+    # var.solverDataDict[solvekey] = deepcopy(vnd)
+    setSolverData!(var, vnd, solvekey)
+end
+
+"""
+    $(SIGNATURES)
+Update the destination variable solver data with a deepcopy of the source variable solver data
+"""
+updateVariableSolverData!(dfg::AbstractDFG, sourceVariable::DFGVariable, solvekey::Symbol=:default) =
+    updateVariableSolverData!(dfg, sourceVariable.label, deepcopy(solverData(sourceVariable, solvekey)), solvekey)
+
+function updateVariableSolverData!(dfg::AbstractDFG, sourceVariables::Vector{DFGVariable}, solvekey::Symbol=:default)
+    #I think cloud would do this in bulk for speed
+    for var in sourceVariables
+        updateVariableSolverData!(dfg, solverData(var, solvekey), var.label, solvekey)
+    end
+end
+
+"""
+    $(SIGNATURES)
+"""
+function deleteVariableSolverData!(dfg::AbstractDFG, variablekey::Symbol, solvekey::Symbol=:default)
+
+    var = getVariable(dfg, variablekey)
+
+    if !haskey(var.solverDataDict, solvekey)
+        error("VariableNodeData '$(solvekey)' does not exist")
+    end
+
+    vnd = pop!(var.solverDataDict, solvekey)
+
+    return vnd
+end
+
+# deleteVariableSolverData!(dfg::AbstractDFG, sourceVariable::DFGVariable, solvekey::Symbol=:default) =
+#     deleteVariableSolverData!(dfg, sourceVariable.label, solvekey)
+
+
 """
     $(SIGNATURES)
 Merges and updates solver and estimate data for a variable (variable can be from another graph).
@@ -642,11 +734,11 @@ Checks whether it both exists in the graph and is a variable.
 (If you rather want a quick for type, just do node isa DFGVariable)
 """
 function isVariable(dfg::G, sym::Symbol) where G <: AbstractDFG
-	error("isVariable not implemented for $(typeof(dfg))")
+    error("isVariable not implemented for $(typeof(dfg))")
 end
 # Alias - bit ridiculous but know it'll come up at some point. Does existential and type check.
 function isVariable(dfg::G, node::N)::Bool where {G <: AbstractDFG, N <: DFGNode}
-	return isVariable(dfg, node.label)
+    return isVariable(dfg, node.label)
 end
 
 """
@@ -657,11 +749,11 @@ Checks whether it both exists in the graph and is a factor.
 (If you rather want a quicker for type, just do node isa DFGFactor)
 """
 function isFactor(dfg::G, sym::Symbol) where G <: AbstractDFG
-	error("isFactor not implemented for $(typeof(dfg))")
+    error("isFactor not implemented for $(typeof(dfg))")
 end
 # Alias - bit ridiculous but know it'll come up at some point. Does existential and type check.
 function isFactor(dfg::G, node::N)::Bool where {G <: AbstractDFG, N <: DFGNode}
-	return isFactor(dfg, node.label)
+    return isFactor(dfg, node.label)
 end
 
 """
