Merge pull request #31 from JuliaRobotics/feature/templatedsolverparams

Feature/templatedsolverparams

Author: dehann

src/NeedsAHome.jl

@@ -1,5 +1,5 @@
 
-export hasFactor, hasVariable
+export hasFactor, hasVariable, isInitialized
 
 """
     $SIGNATURES
@@ -18,3 +18,25 @@ Return `::Bool` on whether `dfg` contains the variable `lbl::Symbol`.
 function hasVariable(dfg::G, label::Symbol)::Bool where {G <: AbstractDFG}
   return haskey(dfg.labelDict, label) # haskey(vertices(dfg.g), label)
 end
+
+
+"""
+    $SIGNATURES
+
+Returns state of vertex data `.initialized` flag.
+
+Notes:
+- used by both factor graph variable and Bayes tree clique logic.
+TODO: Refactor
+"""
+function isInitialized(var::DFGVariable; key::Symbol=:default)::Bool
+  return var.solverDataDict[key].initialized
+end
+function isInitialized(fct::DFGFactor; key::Symbol=:default)::Bool
+  return fct.solverDataDict[key].initialized
+end
+function isInitialized(dfg::G, label::Symbol; key::Symbol=:default)::Bool where G <: AbstractDFG
+  # nothing to do for factors
+  # return dfg.g.vertices[dfg.labelDict[label]].dfgNode.solverDataDict[key].initialized
+  return true
+end

src/services/GraphsDFG.jl

@@ -27,7 +27,7 @@ end
 vertex_index(v::GraphsNode) = v.index
 
 # Exports
-export GraphsDFG
+export GraphsDFG, NoSolverParams, AbstractParams
 export exists
 export getLabelDict, getDescription, setDescription, getInnerGraph, getAddHistory, getSolverParams, setSolverParams
 
@@ -45,20 +45,32 @@ export getSubgraph
 export isFullyConnected, hasOrphans
 export toDot, toDotFile
 
-mutable struct GraphsDFG <: AbstractDFG
+abstract type AbstractParams end
+
+mutable struct NoSolverParams <: AbstractParams
+end
+
+mutable struct GraphsDFG{T<:AbstractParams} <: AbstractDFG
     g::FGType
     description::String
     nodeCounter::Int64
     labelDict::Dict{Symbol, Int64}
     addHistory::Vector{Symbol} #TODO: Discuss more - is this an audit trail?
-    solverParams::Any # Solver parameters
+    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
 
-"""
-    $(SIGNATURES)
-Create a new in-memory Graphs.jl-based DFG factor graph.
-"""
-GraphsDFG() = GraphsDFG(Graphs.incdict(GraphsNode,is_directed=false), "Graphs.jl implementation", 0, Dict{Symbol, Int64}(), Symbol[], nothing)
+
+# """
+#     $(SIGNATURES)
+# Create a new in-memory Graphs.jl-based DFG factor graph.
+# """
+# GraphsDFG() = GraphsDFG(Graphs.incdict(GraphsNode,is_directed=false), "Graphs.jl implementation", 0, Dict{Symbol, Int64}(), Symbol[], NoSolverParams())
 
 # Accessors
 getLabelDict(dfg::GraphsDFG) = dfg.labelDict
@@ -67,7 +79,11 @@ setDescription(dfg::GraphsDFG, description::String) = dfg.description = descript
 getInnerGraph(dfg::GraphsDFG) = dfg.g
 getAddHistory(dfg::GraphsDFG) = dfg.addHistory
 getSolverParams(dfg::GraphsDFG) = dfg.solverParams
-setSolverParams(dfg::GraphsDFG, solverParams::Any) = dfg.solverParams = solverParams
+
+# setSolverParams(dfg::GraphsDFG, solverParams) = dfg.solverParams = solverParams
+function setSolverParams(dfg::GraphsDFG, solverParams::P) where P <: AbstractParams
+  dfg.solverParams = solverParams
+end
 
 """
     $(SIGNATURES)
@@ -125,7 +141,7 @@ function addFactor!(dfg::GraphsDFG, variables::Vector{DFGVariable}, factor::DFGF
         Graphs.add_edge!(dfg.g, edge)
     end
     # Track insertion
-    push!(dfg.addHistory, factor.label)
+    # push!(dfg.addHistory, factor.label)
 
     return true
 end
@@ -428,7 +444,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::GraphsDFG, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG())::GraphsDFG where T <: DFGNode
+function getSubgraphAroundNode(dfg::GraphsDFG{P}, node::T, distance::Int64=1, includeOrphanFactors::Bool=false, addToDFG::GraphsDFG=GraphsDFG{P}())::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

test/interfaceTests.jl

@@ -1,4 +1,4 @@
-dfg = testDFGAPI()
+dfg = testDFGAPI{NoSolverParams}()
 v1 = DFGVariable(:a)
 v2 = DFGVariable(:b)
 f1 = DFGFactor{Int, :Symbol}(:f1)
@@ -21,7 +21,7 @@ end
     @test ls(dfg, r"a") == [v1]
     @test lsf(dfg, r"f*") == [f1]
     # Accessors
-    @test getAddHistory(dfg) == [:a, :b, :f1]
+    @test getAddHistory(dfg) == [:a, :b] #, :f1
     @test getDescription(dfg) != nothing
     @test getLabelDict(dfg) != nothing
     # Existence
@@ -96,7 +96,7 @@ end
 
 # Now make a complex graph for connectivity tests
 numNodes = 10
-dfg = testDFGAPI()
+dfg = testDFGAPI{NoSolverParams}()
 verts = map(n -> DFGVariable(Symbol("x$n")), 1:numNodes)
 map(v -> addVariable!(dfg, v), verts)
 map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactor{Int, :Symbol}(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
@@ -132,4 +132,5 @@ end
     @test toDot(dfg) == "graph graphname {\n18 [\"label\"=\"x8x9f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n2 [\"label\"=\"x2\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n2 -- 11\n2 -- 12\n16 [\"label\"=\"x6x7f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n11 [\"label\"=\"x1x2f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n7 [\"label\"=\"x7\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n7 -- 16\n7 -- 17\n9 [\"label\"=\"x9\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n9 -- 18\n9 -- 19\n10 [\"label\"=\"x10\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n10 -- 19\n19 [\"label\"=\"x9x10f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n17 [\"label\"=\"x7x8f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n8 [\"label\"=\"x8\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n8 -- 17\n8 -- 18\n6 [\"label\"=\"x6\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n6 -- 15\n6 -- 16\n4 [\"label\"=\"x4\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n4 -- 13\n4 -- 14\n3 [\"label\"=\"x3\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n3 -- 12\n3 -- 13\n5 [\"label\"=\"x5\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n5 -- 14\n5 -- 15\n13 [\"label\"=\"x3x4f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n14 [\"label\"=\"x4x5f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n15 [\"label\"=\"x5x6f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n12 [\"label\"=\"x2x3f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n1 [\"label\"=\"x1\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n1 -- 11\n}\n"
 
     @test toDotFile(dfg, "something.dot") == nothing
+    Base.rm("something.dot")
 end