Feature/skeleton dfg (#162)

* working skeleton dfg variables and factors.
label, tags and order

* addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, factor::F)

* SkeletonDFG tests

Author: Affie

src/DistributedFactorGraphs.jl

@@ -18,12 +18,15 @@ include("entities/AbstractDFGSummary.jl")
 
 export AbstractDFG
 export AbstractParams, NoSolverParams
-export DFGNode, DFGVariable, DFGFactor
+export DFGNode, DFGVariable, DFGFactor, AbstractDFGVariable, AbstractDFGFactor
 export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariable, ConvolutionObject
 export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize
 export label, timestamp, tags, estimates, estimate, data, softtype, solverData, getData, solverDataDict, setSolverData, internalId, smallData, bigData
 export DFGVariableSummary, DFGFactorSummary, AbstractDFGSummary
 
+#Skeleton types
+export SkeletonDFGVariable, SkeletonDFGFactor
+
 #graph small data
 export getUserData, setUserData, getRobotData, setRobotData, getSessionData, setSessionData
 export pushUserData!, pushRobotData!, pushSessionData!, popUserData!, popRobotData!, popSessionData!

src/LightDFG/services/LightDFG.jl

@@ -88,8 +88,9 @@ function addFactor!(dfg::LightDFG{<:AbstractParams, V, F}, variables::Vector{V},
 
 	variableLabels = map(v->v.label, variables)
 
-    factor._variableOrderSymbols = copy(variableLabels)
-
+	resize!(factor._variableOrderSymbols, length(variableLabels))
+	factor._variableOrderSymbols .= variableLabels
+    # factor._variableOrderSymbols = copy(variableLabels)
 
     return FactorGraphs.addFactor!(dfg.g, variableLabels, factor)
 end
@@ -104,12 +105,23 @@ function addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, v
         error("Factor '$(factor.label)' already exists in the factor graph")
     end
 
-    factor._variableOrderSymbols = variableLabels
+	resize!(factor._variableOrderSymbols, length(variableLabels))
+	factor._variableOrderSymbols .= variableLabels
+	# factor._variableOrderSymbols = copy(variableLabels)
 
     return FactorGraphs.addFactor!(dfg.g, variableLabels, factor)
 end
 
 
+function addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, factor::F)::Bool where F <: AbstractDFGFactor
+	#TODO should this be an error
+	if haskey(dfg.g.factors, factor.label)
+        error("Factor '$(factor.label)' already exists in the factor graph")
+    end
+
+    return FactorGraphs.addFactor!(dfg.g, variableLabels, factor)
+end
+
 """
     $(SIGNATURES)
 Get a DFGVariable from a DFG using its label.

src/entities/DFGFactor.jl

@@ -103,3 +103,19 @@ label(f::DFGFactorSummary) = f.label
 data(f::DFGFactorSummary) = f.data
 tags(f::DFGFactorSummary) = f.tags
 internalId(f::DFGFactorSummary) = f._internalId
+
+
+# SKELETON DFG
+"""
+	$(TYPEDEF)
+Skeleton variable with essentials.
+"""
+struct SkeletonDFGVariable <: AbstractDFGVariable
+	label::Symbol
+	tags::Vector{Symbol}
+end
+
+SkeletonDFGVariable(label::Symbol) = SkeletonDFGVariable(label, Symbol[])
+
+label(v::SkeletonDFGVariable) = v.label
+tags(v::SkeletonDFGVariable) = v.tags

src/entities/DFGVariable.jl

@@ -174,3 +174,22 @@ estimates(v::DFGVariableSummary) = v.estimateDict
 estimate(v::DFGVariableSummary, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing
 softtype(v::DFGVariableSummary)::Symbol = v.softtypename
 internalId(v::DFGVariableSummary) = v._internalId
+
+
+
+# SKELETON DFG
+"""
+	$(TYPEDEF)
+Skeleton factor with essentials.
+"""
+struct SkeletonDFGFactor <: AbstractDFGFactor
+    label::Symbol
+	tags::Vector{Symbol}
+	_variableOrderSymbols::Vector{Symbol}
+end
+
+#NOTE I feel like a want to force a variableOrderSymbols
+SkeletonDFGFactor(label::Symbol, variableOrderSymbols::Vector{Symbol} = Symbol[]) = SkeletonDFGFactor(label, Symbol[], variableOrderSymbols)
+
+label(f::SkeletonDFGFactor) = f.label
+tags(f::SkeletonDFGFactor) = f.tags

src/services/DFGVariable.jl

@@ -120,6 +120,10 @@ Equality check for AbstractPointParametricEst.
     mapreduce(n -> :(x.$n == y.$n), (a,b)->:($a && $b), fieldnames(x))
 end
 
+@generated function Base.:(==)(x::T, y::T) where T <: Union{DFGFactorSummary, DFGVariableSummary, SkeletonDFGVariable, SkeletonDFGFactor}
+    mapreduce(n -> :(x.$n == y.$n), (a,b)->:($a && $b), fieldnames(x))
+end
+
 """
     $(SIGNATURES)
 Equality check for DFGVariable.

test/LightDFGSummaryTypes.jl

@@ -1,36 +1,35 @@
-dfg = LightDFG{NoSolverParams, DFGVariableSummary, DFGFactorSummary}()
 
+dfg = LightDFG{NoSolverParams, VARTYPE, FACTYPE}()
 DistributedFactorGraphs.DFGVariableSummary(label::Symbol) = DFGVariableSummary(label, DistributedFactorGraphs.now(), Symbol[], Dict{Symbol, MeanMaxPPE}(), :NA, 0)
-
 DistributedFactorGraphs.DFGFactorSummary(label::Symbol) = DFGFactorSummary(label, Symbol[], 0, Symbol[])
 
-v1 = DFGVariableSummary(:a)
-v2 = DFGVariableSummary(:b)
-f1 = DFGFactorSummary(:f1)
+v1 = VARTYPE(:a)
+v2 = VARTYPE(:b)
+f1 = FACTYPE(:f1)
 
 #add tags for filters
 append!(v1.tags, [:VARIABLE, :POSE])
 append!(v2.tags, [:VARIABLE, :LANDMARK])
 append!(f1.tags, [:FACTOR])
 
 #Force softtypename
-v1.softtypename = :Pose2
+isa(v1, DFGVariableSummary) && (v1.softtypename = :Pose2)
 
 # @testset "Creating Graphs" begin
 global dfg,v1,v2,f1
 addVariable!(dfg, v1)
 addVariable!(dfg, v2)
 addFactor!(dfg, [v1, v2], f1)
-@test_throws Exception addFactor!(dfg, DFGFactorSummary("f2"), [v1, DFGVariableSummary("Nope")])
+@test_throws Exception addFactor!(dfg, FACTYPE("f2"), [v1, VARTYPE("Nope")])
 # end
 
 @testset "Adding Removing Nodes" begin
-    dfg2 = LightDFG{NoSolverParams, DFGVariableSummary, DFGFactorSummary}()
-    v1 = DFGVariableSummary(:a)
-    v2 = DFGVariableSummary(:b)
-    v3 = DFGVariableSummary(:c)
-    f1 = DFGFactorSummary(:f1)
-    f2 = DFGFactorSummary(:f2)
+    dfg2 = LightDFG{NoSolverParams, VARTYPE, FACTYPE}()
+    v1 = VARTYPE(:a)
+    v2 = VARTYPE(:b)
+    v3 = VARTYPE(:c)
+    f1 = FACTYPE(:f1)
+    f2 = FACTYPE(:f2)
     # @testset "Creating Graphs" begin
     @test addVariable!(dfg2, v1)
     @test addVariable!(dfg2, v2)
@@ -79,6 +78,8 @@ end
 # Gets
 @testset "Gets, Sets, and Accessors" begin
     global dfg,v1,v2,f1
+
+
     @test getVariable(dfg, v1.label) == v1
     @test getFactor(dfg, f1.label) == f1
     @test_throws Exception getVariable(dfg, :nope)
@@ -88,22 +89,25 @@ end
 
     # Sets
     v1Prime = deepcopy(v1)
-    @test updateVariable!(dfg, v1Prime) != v1
+    @test updateVariable!(dfg, v1Prime) == v1
     f1Prime = deepcopy(f1)
-    @test updateFactor!(dfg, f1Prime) != f1
+    @test updateFactor!(dfg, f1Prime) == f1
 
     # Accessors
     @test label(v1) == v1.label
     @test tags(v1) == v1.tags
-    @test timestamp(v1) == v1.timestamp
-    @test estimates(v1) == v1.estimateDict
-    @test estimate(v1, :notfound) == nothing
-    @test softtype(v1) == :Pose2
+
+    if VARTYPE == DFGVariableSummary
+        @test timestamp(v1) == v1.timestamp
+        @test estimates(v1) == v1.estimateDict
+        @test estimate(v1, :notfound) == nothing
+        @test softtype(v1) == :Pose2
+        @test internalId(v1) == v1._internalId
+    end
     # @test solverData(v1) === v1.solverDataDict[:default]
     # @test getData(v1) === v1.solverDataDict[:default]
     # @test solverData(v1, :default) === v1.solverDataDict[:default]
     # @test solverDataDict(v1) == v1.solverDataDict
-    @test internalId(v1) == v1._internalId
 
     @test label(f1) == f1.label
     @test tags(f1) == f1.tags
@@ -112,44 +116,46 @@ end
     # @test data(f1) == f1.data
     # @test getData(f1) == f1.data
     # Internal function
-    @test internalId(f1) == f1._internalId
+    if FACTYPE == DFGFactorSummary
+        @test internalId(f1) == f1._internalId
+    end
 
-    @test getSolverParams(dfg) != nothing
-    @test setSolverParams(dfg, getSolverParams(dfg)) == getSolverParams(dfg)
 end
 
 @testset "Updating Nodes" begin
-    global dfg
-    #get the variable
-    var = getVariable(dfg, :a)
-    #make a copy and simulate external changes
-    newvar = deepcopy(var)
-    estimates(newvar)[:default] = MeanMaxPPE(:default, [100.0], [50.0])
-    #update
-    mergeUpdateVariableSolverData!(dfg, newvar)
-    #For now spot check
-    # @test solverDataDict(newvar) == solverDataDict(var)
-    @test estimates(newvar) == estimates(var)
+    if VARTYPE == DFGVariableSummary
+            global dfg
+        #get the variable
+        var = getVariable(dfg, :a)
+        #make a copy and simulate external changes
+        newvar = deepcopy(var)
+        estimates(newvar)[:default] = MeanMaxPPE(:default, [100.0], [50.0])
+        #update
+        mergeUpdateVariableSolverData!(dfg, newvar)
+        #For now spot check
+        # @test solverDataDict(newvar) == solverDataDict(var)
+        @test estimates(newvar) == estimates(var)
 
-    # Delete :default and replace to see if new ones can be added
-    delete!(estimates(newvar), :default)
-    estimates(newvar)[:second] = MeanMaxPPE(:second, [10.0], [5.0])
+        # Delete :default and replace to see if new ones can be added
+        delete!(estimates(newvar), :default)
+        estimates(newvar)[:second] = MeanMaxPPE(:second, [10.0], [5.0])
 
-    # Persist to the original variable.
-    mergeUpdateVariableSolverData!(dfg, newvar)
-    # At this point newvar will have only :second, and var should have both (it is the reference)
-    @test symdiff(collect(keys(estimates(var))), [:default, :second]) == Symbol[]
-    @test symdiff(collect(keys(estimates(newvar))), [:second]) == Symbol[]
-    # Get the source too.
-    @test symdiff(collect(keys(estimates(getVariable(dfg, :a)))), [:default, :second]) == Symbol[]
+        # Persist to the original variable.
+        mergeUpdateVariableSolverData!(dfg, newvar)
+        # At this point newvar will have only :second, and var should have both (it is the reference)
+        @test symdiff(collect(keys(estimates(var))), [:default, :second]) == Symbol[]
+        @test symdiff(collect(keys(estimates(newvar))), [:second]) == Symbol[]
+        # Get the source too.
+        @test symdiff(collect(keys(estimates(getVariable(dfg, :a)))), [:default, :second]) == Symbol[]
+    end
 end
 
 # Connectivity test
 @testset "Connectivity Test" begin
     global dfg,v1,v2,f1
     @test isFullyConnected(dfg) == true
     @test hasOrphans(dfg) == false
-    addVariable!(dfg, DFGVariableSummary(:orphan))
+    addVariable!(dfg, VARTYPE(:orphan))
     @test isFullyConnected(dfg) == false
     @test hasOrphans(dfg) == true
 end
@@ -191,13 +197,13 @@ end
 
 # Now make a complex graph for connectivity tests
 numNodes = 10
-dfg = LightDFG{NoSolverParams, DFGVariableSummary, DFGFactorSummary}()
-verts = map(n -> DFGVariableSummary(Symbol("x$n")), 1:numNodes)
+dfg = LightDFG{NoSolverParams, VARTYPE, FACTYPE}()
+verts = map(n -> VARTYPE(Symbol("x$n")), 1:numNodes)
 #change ready and backendset for x7,x8 for improved tests on x7x8f1
 # verts[7].ready = 1
 # verts[8].backendset = 1
 map(v -> addVariable!(dfg, v), verts)
-map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactorSummary(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
+map(n -> addFactor!(dfg, [verts[n], verts[n+1]], FACTYPE(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
 
 @testset "Getting Neighbors" begin
     global dfg,verts
@@ -211,20 +217,6 @@ map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactorSummary(Symbol("x$(n)x
     @test getNeighbors(dfg, getFactor(dfg, :x1x2f1)) == ls(dfg, getFactor(dfg, :x1x2f1))
     @test getNeighbors(dfg, :x1x2f1) == ls(dfg, :x1x2f1)
 
-    # ready and backendset
-    # @test getNeighbors(dfg, :x5, ready=1) == Symbol[]
-    # @test getNeighbors(dfg, :x5, ready=0) == [:x4x5f1,:x5x6f1]
-    # @test getNeighbors(dfg, :x5, backendset=1) == Symbol[]
-    # @test getNeighbors(dfg, :x5, backendset=0) == [:x4x5f1,:x5x6f1]
-    # @test getNeighbors(dfg, :x7x8f1, ready=0) == [:x8]
-    # @test getNeighbors(dfg, :x7x8f1, backendset=0) == [:x7]
-    # @test getNeighbors(dfg, :x7x8f1, ready=1) == [:x7]
-    # @test getNeighbors(dfg, :x7x8f1, backendset=1) == [:x8]
-    # @test getNeighbors(dfg, verts[1], ready=0) == [:x1x2f1]
-    # @test getNeighbors(dfg, verts[1], ready=1) == Symbol[]
-    # @test getNeighbors(dfg, verts[1], backendset=0) == [:x1x2f1]
-    # @test getNeighbors(dfg, verts[1], backendset=1) == Symbol[]
-
 end
 
 @testset "Getting Subgraphs" begin
@@ -256,41 +248,27 @@ end
 end
 
 @testset "Summaries and Summary Graphs" begin
-    factorFields = fieldnames(DFGFactorSummary)
-    variableFields = fieldnames(DFGVariableSummary)
+    if VARTYPE == DFGVariableSummary
+        factorFields = fieldnames(FACTYPE)
+        variableFields = fieldnames(VARTYPE)
 
-    summary = getSummary(dfg)
-    @test symdiff(collect(keys(summary.variables)), ls(dfg)) == Symbol[]
-    @test symdiff(collect(keys(summary.factors)), lsf(dfg)) == Symbol[]
+        summary = getSummary(dfg)
+        @test symdiff(collect(keys(summary.variables)), ls(dfg)) == Symbol[]
+        @test symdiff(collect(keys(summary.factors)), lsf(dfg)) == Symbol[]
 
-    summaryGraph = getSummaryGraph(dfg)
-    @test symdiff(ls(summaryGraph), ls(dfg)) == Symbol[]
-    @test symdiff(lsf(summaryGraph), lsf(dfg)) == Symbol[]
-    # Check all fields are equal for all variables
-    for v in ls(summaryGraph)
-        for field in variableFields
-            @test getfield(getVariable(dfg, v), field) == getfield(getVariable(summaryGraph, v), field)
+        summaryGraph = getSummaryGraph(dfg)
+        @test symdiff(ls(summaryGraph), ls(dfg)) == Symbol[]
+        @test symdiff(lsf(summaryGraph), lsf(dfg)) == Symbol[]
+        # Check all fields are equal for all variables
+        for v in ls(summaryGraph)
+            for field in variableFields
+                @test getfield(getVariable(dfg, v), field) == getfield(getVariable(summaryGraph, v), field)
+            end
         end
-    end
-    for f in lsf(summaryGraph)
-        for field in factorFields
-            @test getfield(getFactor(dfg, f), field) == getfield(getFactor(summaryGraph, f), field)
+        for f in lsf(summaryGraph)
+            for field in factorFields
+                @test getfield(getFactor(dfg, f), field) == getfield(getFactor(summaryGraph, f), field)
+            end
         end
     end
 end
-
-# @testset "Producing Dot Files" begin
-#     # create a simpler graph for dot testing
-#     dotdfg = LightDFG{NoSolverParams, DFGVariableSummary, DFGFactorSummary}()
-#     v1 = DFGVariableSummary(:a)
-#     v2 = DFGVariableSummary(:b)
-#     f1 = DFGFactorSummary(:f1)
-#     addVariable!(dotdfg, v1)
-#     addVariable!(dotdfg, v2)
-#     addFactor!(dotdfg, [v1, v2], f1)
-#
-#     @test toDot(dotdfg) == "graph graphname {\n2 [\"label\"=\"b\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n2 -- 3\n3 [\"label\"=\"f1\",\"shape\"=\"ellipse\",\"fillcolor\"=\"blue\",\"color\"=\"blue\"]\n1 [\"label\"=\"a\",\"shape\"=\"box\",\"fillcolor\"=\"red\",\"color\"=\"red\"]\n1 -- 3\n}\n"
-#     @test toDotFile(dotdfg, "something.dot") == nothing
-#     Base.rm("something.dot")
-#
-# end

test/runtests.jl

@@ -37,12 +37,17 @@ end
     include("plottingTest.jl")
 end
 
-@testset "SummaryDFG test" begin
-    @info "Testing LightDFG Variable and Factor Subtypes"
-    include("LightDFGSummaryTypes.jl")
+@testset "LightDFG subtype tests" begin
+    for type in [(var=DFGVariableSummary, fac=DFGFactorSummary), (var=SkeletonDFGVariable,fac=SkeletonDFGFactor)]
+        @testset "$(type.var) and $(type.fac) tests" begin
+            @info "Testing $(type.var) and $(type.fac)"
+            global VARTYPE = type.var
+            global FACTYPE = type.fac
+            include("LightDFGSummaryTypes.jl")
+        end
+    end
 end
 
-
 if get(ENV, "IIF_TEST", "") == "true"
 
     # Pkg.add("IncrementalInference")