From 79a2df24b6bffe272b18ef91ca4cecf1e54b08d2 Mon Sep 17 00:00:00 2001 From: gearsad Date: Sun, 8 Sep 2019 23:06:36 -0500 Subject: [PATCH 01/10] WIP --- src/Common.jl | 9 ----- src/DistributedFactorGraphs.jl | 9 +++-- src/NeedsAHome.jl | 74 ---------------------------------- src/entities/DFGFactor.jl | 7 ++++ src/entities/DFGVariable.jl | 2 + src/services/AbstractDFG.jl | 73 +++++++++++++++++++++++++++++++++ 6 files changed, 87 insertions(+), 87 deletions(-) delete mode 100644 src/NeedsAHome.jl diff --git a/src/Common.jl b/src/Common.jl index 09541721..a0c9acc3 100644 --- a/src/Common.jl +++ b/src/Common.jl @@ -1,7 +1,6 @@ export sortVarNested export isPrior, lsfPriors -export getData export getVariableType, getSofttype export getFactorType, getfnctype export lsTypes, lsfTypes @@ -82,14 +81,6 @@ function sortVarNested(vars::Vector{Symbol})::Vector{Symbol} return retvars end -""" - $SIGNATURES - -Retrieve data structure stored in a node. -""" -getData(v::DFGFactor)::GenericFunctionNodeData = v.data -getData(v::DFGVariable; solveKey::Symbol=:default)::VariableNodeData = v.solverDataDict[solveKey] - """ $SIGNATURES diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 4c487a5c..11df6889 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -25,10 +25,13 @@ export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariab export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize export DFGVariable -export label, timestamp, tags, estimates, estimate, solverData, solverDataDict, id, smallData, bigData +export label, timestamp, tags, estimates, estimate, solverData, getData, solverDataDict, id, smallData, bigData export setSolverData export label, data, id +# Services/AbstractDFG Exports +export hasFactor, hasVariable, isInitialized, getFactorFunction, isVariable, isFactor + # Solver (IIF) Exports export VariableNodeData, PackedVariableNodeData, VariableEstimate export GenericFunctionNodeData#, FunctionNodeData @@ -97,9 +100,7 @@ function __init__() end - -# not sure where to put +# To be moved as necessary. include("Common.jl") -include("NeedsAHome.jl") end diff --git a/src/NeedsAHome.jl b/src/NeedsAHome.jl deleted file mode 100644 index b52e1035..00000000 --- a/src/NeedsAHome.jl +++ /dev/null @@ -1,74 +0,0 @@ - -export hasFactor, hasVariable, isInitialized, getFactorFunction, isVariable, isFactor - -""" - $SIGNATURES - -Return boolean whether a factor `label` is present in `<:AbstractDFG`. -""" -function hasFactor(dfg::G, label::Symbol)::Bool where {G <: AbstractDFG} - return haskey(dfg.labelDict, label) -end - -""" - $(SIGNATURES) - -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 - return isInitialized(getVariable(dfg, label), key=key) -end - - -""" - $SIGNATURES - -Return whether `sym::Symbol` represents a variable vertex in the graph. -""" -isVariable(dfg::G, sym::Symbol) where G <: AbstractDFG = hasVariable(dfg, sym) - -""" - $SIGNATURES - -Return whether `sym::Symbol` represents a factor vertex in the graph. -""" -isFactor(dfg::G, sym::Symbol) where G <: AbstractDFG = hasFactor(dfg, sym) - -""" - $SIGNATURES - -Return reference to the user factor in `<:AbstractDFG` identified by `::Symbol`. -""" -getFactorFunction(fcd::GenericFunctionNodeData) = fcd.fnc.usrfnc! -getFactorFunction(fc::DFGFactor) = getFactorFunction(getData(fc)) -function getFactorFunction(dfg::G, fsym::Symbol) where G <: AbstractDFG - getFactorFunction(getFactor(dfg, fsym)) -end - - -""" - $SIGNATURES - -Display and return to console the user factor identified by tag name. -""" -showFactor(fgl::G, fsym::Symbol) where G <: AbstractDFG = @show getFactor(fgl,fsym) diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index 01db5eb4..d9343bb2 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -56,3 +56,10 @@ const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString} PackedFunctionNodeData(x1, x2, x3, x4, x5::S, x6::T, x7::String="", x8::Vector{Int}=Int[]) where {T <: PackedInferenceType, S <: AbstractString} = GenericFunctionNodeData(x1, x2, x3, x4, x5, x6, x7, x8) const FunctionNodeData{T} = GenericFunctionNodeData{T, Symbol} FunctionNodeData(x1, x2, x3, x4, x5::Symbol, x6::T, x7::String="", x8::Vector{Int}=Int[]) where {T <: Union{FunctorInferenceType, ConvolutionObject}}= GenericFunctionNodeData{T, Symbol}(x1, x2, x3, x4, x5, x6, x7, x8) + +""" + $SIGNATURES + +Retrieve data structure stored in a node. +""" +getData(v::DFGFactor)::GenericFunctionNodeData = v.data diff --git a/src/entities/DFGVariable.jl b/src/entities/DFGVariable.jl index 5509567c..706ac068 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -75,6 +75,7 @@ struct VariableEstimate estimate::Vector{Float64} type::Symbol key::Symbol + lastUpdatedTimestamp::DateTime end """ @@ -104,6 +105,7 @@ estimates(v::DFGVariable) = v.estimateDict estimate(v::DFGVariable, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing #solverData(v::DFGVariable) = haskey(v.solverDataDict, :default) ? v.solverDataDict[:default] : nothing solverData(v::DFGVariable, key::Symbol=:default) = haskey(v.solverDataDict, key) ? v.solverDataDict[key] : nothing +getData(v::DFGVariable; solveKey::Symbol=:default)::VariableNodeData = v.solverDataDict[solveKey] setSolverData(v::DFGVariable, data::VariableNodeData, key::Symbol=:default) = v.solverDataDict[key] = data solverDataDict(v::DFGVariable) = v.solverDataDict id(v::DFGVariable) = v._internalId diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index c12e1ca0..31015ca6 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -82,3 +82,76 @@ Get an adjacency matrix for the DFG, returned as a tuple: adjmat::SparseMatrixCS Rows are the factors, columns are the variables, with the corresponding labels in fac_labels,var_labels. """ getAdjacencyMatrixSparse(dfg::AbstractDFG) = error("getAdjacencyMatrixSparse not implemented for $(typeof(dfg))") + + +""" + $SIGNATURES + +Return boolean whether a factor `label` is present in `<:AbstractDFG`. +""" +function hasFactor(dfg::G, label::Symbol)::Bool where {G <: AbstractDFG} + return haskey(dfg.labelDict, label) +end + +""" + $(SIGNATURES) + +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 + return isInitialized(getVariable(dfg, label), key=key) +end + + +""" + $SIGNATURES + +Return whether `sym::Symbol` represents a variable vertex in the graph. +""" +isVariable(dfg::G, sym::Symbol) where G <: AbstractDFG = hasVariable(dfg, sym) + +""" + $SIGNATURES + +Return whether `sym::Symbol` represents a factor vertex in the graph. +""" +isFactor(dfg::G, sym::Symbol) where G <: AbstractDFG = hasFactor(dfg, sym) + +""" + $SIGNATURES + +Return reference to the user factor in `<:AbstractDFG` identified by `::Symbol`. +""" +getFactorFunction(fcd::GenericFunctionNodeData) = fcd.fnc.usrfnc! +getFactorFunction(fc::DFGFactor) = getFactorFunction(getData(fc)) +function getFactorFunction(dfg::G, fsym::Symbol) where G <: AbstractDFG + getFactorFunction(getFactor(dfg, fsym)) +end + + +""" + $SIGNATURES + +Display and return to console the user factor identified by tag name. +""" +showFactor(fgl::G, fsym::Symbol) where G <: AbstractDFG = @show getFactor(fgl,fsym) From 2e90242a1c74172a32d1fc564324be277251c5eb Mon Sep 17 00:00:00 2001 From: gearsad Date: Mon, 9 Sep 2019 21:06:31 -0500 Subject: [PATCH 02/10] Renaming id(...) to internalId(...) so it's not used. Cleaning up estimateDict. --- src/CloudGraphsDFG/services/CloudGraphsDFG.jl | 2 +- src/DistributedFactorGraphs.jl | 2 +- src/FileDFG/services/FileDFG.jl | 7 ++--- src/entities/DFGFactor.jl | 2 +- src/entities/DFGVariable.jl | 12 ++++----- test/FileDFG.jl | 26 ++++++++++++++++++- test/interfaceTests.jl | 4 +-- 7 files changed, 38 insertions(+), 17 deletions(-) diff --git a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl index 48662c54..f09e5a63 100644 --- a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl +++ b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl @@ -255,7 +255,7 @@ function getVariable(dfg::CloudGraphsDFG, variableId::Int64)::DFGVariable # props["label"] = Symbol(variable.label) timestamp = DateTime(props["timestamp"]) tags = JSON2.read(props["tags"], Vector{Symbol}) - estimateDict = JSON2.read(props["estimateDict"], Dict{Symbol, VariableEstimate}) + estimateDict = JSON2.read(props["estimateDict"], Dict{Symbol, Dict{Symbol, VariableEstimate}}) smallData = nothing smallData = JSON2.read(props["smallData"], Dict{String, String}) diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index b58b8ce9..7861d927 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -25,7 +25,7 @@ export InferenceType, PackedInferenceType, FunctorInferenceType, InferenceVariab export FunctorSingleton, FunctorPairwise, FunctorPairwiseMinimize export DFGVariable -export label, timestamp, tags, estimates, estimate, solverData, getData, solverDataDict, id, smallData, bigData +export label, timestamp, tags, estimates, estimate, solverData, getData, solverDataDict, internalId, smallData, bigData export setSolverData export label, data, id diff --git a/src/FileDFG/services/FileDFG.jl b/src/FileDFG/services/FileDFG.jl index b118a4be..f67c4d70 100644 --- a/src/FileDFG/services/FileDFG.jl +++ b/src/FileDFG/services/FileDFG.jl @@ -16,7 +16,7 @@ function _unpackVariable(dfg::G, packedProps::Dict{String, Any})::DFGVariable wh label = Symbol(packedProps["label"]) timestamp = DateTime(packedProps["timestamp"]) tags = JSON2.read(packedProps["tags"], Vector{Symbol}) - estimateDict = JSON2.read(packedProps["estimateDict"], Dict{Symbol, VariableEstimate}) + estimateDict = JSON2.read(packedProps["estimateDict"], Dict{Symbol, Dict{Symbol, VariableEstimate}}) smallData = nothing smallData = JSON2.read(packedProps["smallData"], Dict{String, String}) @@ -137,10 +137,7 @@ function saveDFG(dfg::G, folder::String) where G <: AbstractDFG end end -function loadDFG(folder::String, - iifModule, - dfgLoadInto::G=GraphsDFG{NoSolverParams}()) where G <: AbstractDFG - # +function loadDFG(folder::String, iifModule, dfgLoadInto::G=GraphsDFG{NoSolverParams}()) where G <: AbstractDFG variables = DFGVariable[] factors = DFGFactor[] varFolder = "$folder/variables" diff --git a/src/entities/DFGFactor.jl b/src/entities/DFGFactor.jl index d9343bb2..084e4a4b 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -49,7 +49,7 @@ end label(f::F) where F <: DFGFactor = f.label data(f::F) where F <: DFGFactor = f.data -id(f::F) where F <: DFGFactor = f._internalId +internalId(f::F) where F <: DFGFactor = f._internalId # Simply for convenience - don't export const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString} diff --git a/src/entities/DFGVariable.jl b/src/entities/DFGVariable.jl index 706ac068..a7c85585 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -72,10 +72,11 @@ mutable struct PackedVariableNodeData end struct VariableEstimate - estimate::Vector{Float64} + solverKey::Symbol type::Symbol - key::Symbol + estimate::Vector{Float64} lastUpdatedTimestamp::DateTime + VariableEstimate(solverKey::Symbol, type::Symbol, estimate::Vector{Float64}, lastUpdatedTimestamp::DateTime=now()) = new(solverKey, type, estimate, lastUpdatedTimestamp) end """ @@ -86,14 +87,14 @@ mutable struct DFGVariable <: DFGNode label::Symbol timestamp::DateTime tags::Vector{Symbol} - estimateDict::Dict{Symbol, VariableEstimate} + estimateDict::Dict{Symbol, Dict{Symbol, VariableEstimate}} solverDataDict::Dict{Symbol, VariableNodeData} smallData::Dict{String, String} bigData::Any ready::Int backendset::Int _internalId::Int64 - DFGVariable(label::Symbol, _internalId::Int64) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), Dict{String, String}(), nothing, 0, 0, _internalId) + DFGVariable(label::Symbol, _internalId::Int64) = new(label, now(), Symbol[], Dict{Symbol, Dict{Symbol, VariableEstimate}}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), Dict{String, String}(), nothing, 0, 0, _internalId) DFGVariable(label::Symbol) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), Dict{String, String}(), nothing, 0, 0, 0) end @@ -103,12 +104,11 @@ timestamp(v::DFGVariable) = v.timestamp tags(v::DFGVariable) = v.tags estimates(v::DFGVariable) = v.estimateDict estimate(v::DFGVariable, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing -#solverData(v::DFGVariable) = haskey(v.solverDataDict, :default) ? v.solverDataDict[:default] : nothing solverData(v::DFGVariable, key::Symbol=:default) = haskey(v.solverDataDict, key) ? v.solverDataDict[key] : nothing getData(v::DFGVariable; solveKey::Symbol=:default)::VariableNodeData = v.solverDataDict[solveKey] setSolverData(v::DFGVariable, data::VariableNodeData, key::Symbol=:default) = v.solverDataDict[key] = data solverDataDict(v::DFGVariable) = v.solverDataDict -id(v::DFGVariable) = v._internalId +internalId(v::DFGVariable) = v._internalId # Todo: Complete this. smallData(v::DFGVariable) = v.smallData bigData(v::DFGVariable) = v.bigData diff --git a/test/FileDFG.jl b/test/FileDFG.jl index f83dae16..52b5692c 100644 --- a/test/FileDFG.jl +++ b/test/FileDFG.jl @@ -1,6 +1,7 @@ using Test using DistributedFactorGraphs using IncrementalInference, RoME +using Dates # Make a simple graph dfg = GraphsDFG{SolverParams}(params=SolverParams()) @@ -17,7 +18,7 @@ for i in 0:5 addFactor!(dfg, [psym;nsym], pp ) end -# Save it +# Save with no solution saveFolder = "/tmp/fileDFG" saveDFG(dfg, saveFolder) @test readdir("$saveFolder/variables") == ["x0.json", "x1.json", "x2.json", "x3.json", "x4.json", "x5.json", "x6.json"] @@ -27,4 +28,27 @@ retDFG = loadDFG(saveFolder, IncrementalInference) @test symdiff(ls(dfg), ls(dfg)) == [] @test symdiff(lsf(dfg), lsf(retDFG)) == [] +# Now solve the graph and update the solver results +# TODO: When PPE estimates are available, make the update happen here +for variable in getVariables(dfg) + variable.estimateDict[:default] = Dict{Symbol, VariableEstimate}(:MAP => VariableEstimate(:default, :MAP, round.(rand(3)*1000), now())) +end +saveDFG(dfg, saveFolder) +retDFG = loadDFG(saveFolder, IncrementalInference) +for retVar in getVariables(retDFG) + origVar = getVariable(dfg, retVar.label).estimateDict[:default][:MAP] + @test retVar.estimateDict[:default][:MAP].estimate == origVar.estimate + @test retVar.estimateDict[:default][:MAP].type == origVar.type + @test retVar.estimateDict[:default][:MAP].solverKey == origVar.solverKey +end + +# Now saving solverDataDict +tree, smtasks = batchSolve!(dfg, treeinit=true, drawpdf=false, show=false, + returntasks=true, limititers=50, + upsolve=true, downsolve=true ) +saveDFG(dfg, saveFolder) +retDFG = loadDFG(saveFolder, IncrementalInference) +x0 = getVariable(dfg, :x0) +x0ret = getVariable(retDFG, :x0) +solverData(x0) == solverData(x0ret) # Success! diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 67bd089e..4d9fe285 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -87,11 +87,11 @@ end @test solverData(v1) === v1.solverDataDict[:default] @test solverData(v1, :default) === v1.solverDataDict[:default] @test solverDataDict(v1) == v1.solverDataDict - @test id(v1) == v1._internalId + @test internalId(v1) == v1._internalId @test label(f1) == f1.label @test data(f1) == f1.data - @test id(f1) == f1._internalId + @test internalId(f1) == f1._internalId end # Connectivity test From 6474d51a097884c9d5e14c8acf262e27b35cb23b Mon Sep 17 00:00:00 2001 From: gearsad Date: Tue, 10 Sep 2019 00:13:01 -0500 Subject: [PATCH 03/10] Function to update only solverDicts and estimates back to cloud. --- src/CloudGraphsDFG/CloudGraphsDFG.jl | 2 +- src/CloudGraphsDFG/services/CloudGraphsDFG.jl | 14 ++++ test/HexagonalCloud.jl | 80 +++++-------------- 3 files changed, 33 insertions(+), 63 deletions(-) diff --git a/src/CloudGraphsDFG/CloudGraphsDFG.jl b/src/CloudGraphsDFG/CloudGraphsDFG.jl index c766f439..1ce60bc3 100644 --- a/src/CloudGraphsDFG/CloudGraphsDFG.jl +++ b/src/CloudGraphsDFG/CloudGraphsDFG.jl @@ -17,7 +17,7 @@ export getAddHistory, getDescription, getLabelDict export addVariable!, addFactor! export ls, lsf, getVariables, getFactors, getVariableIds, getFactorIds export getVariable, getFactor -export updateVariable!, updateFactor! +export updateVariable!, updateFactor!, updateVariableSolverData! export deleteVariable!, deleteFactor! export getAdjacencyMatrix export getNeighbors diff --git a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl index f09e5a63..d3b5ac3f 100644 --- a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl +++ b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl @@ -389,6 +389,20 @@ function updateVariable!(dfg::CloudGraphsDFG, variable::DFGVariable)::DFGVariabl return variable end +""" + $(SIGNATURES) +Update solver and estimate data for a variable (variable can be from another graph). +""" +function updateVariableSolverData!(dfg::CloudGraphsDFG, sourceVariable::DFGVariable)::DFGVariable + if !exists(dfg, sourceVariable) + error("Source variable '$(variable.label)' doesn't exist in the graph.") + end + nodeId = _tryGetNeoNodeIdFromNodeLabel(dfg.neo4jInstance, dfg.userId, dfg.robotId, dfg.sessionId, sourceVariable.label) + Neo4j.setnodeproperty(dfg.neo4jInstance.graph, nodeId, "estimateDict", JSON2.write(sourceVariable.estimateDict)) + Neo4j.setnodeproperty(dfg.neo4jInstance.graph, nodeId, "solverDataDict", JSON2.write(Dict(keys(sourceVariable.solverDataDict) .=> map(vnd -> pack(dfg, vnd), values(sourceVariable.solverDataDict))))) + return sourceVariable +end + """ $(SIGNATURES) Update a complete DFGFactor in the DFG. diff --git a/test/HexagonalCloud.jl b/test/HexagonalCloud.jl index 86d69516..5c6e8901 100644 --- a/test/HexagonalCloud.jl +++ b/test/HexagonalCloud.jl @@ -2,7 +2,7 @@ using Revise using Neo4j # So that DFG initializes the database driver. using RoME using DistributedFactorGraphs -using Test +using Test, Dates # start with an empty factor graph object # fg = initfg() @@ -24,17 +24,6 @@ IncrementalInference.compareVariable(x0, getVariable(cloudFg, :x0)) # Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4) prior = addFactor!(cloudFg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) ) -# retPrior = getFactor(cloudFg, :x0f1) -# Do the check -# IncrementalInference.compareFactor(prior, retPrior) -# Testing - -# retPrior.data.fnc.cpt = prior.data.fnc.cpt -# # This one -# prior.data.fnc.cpt[1].factormetadata -# deserialized: Any[Pose2(3, String[], (:Euclid, :Euclid, :Circular))] -# vs. -# original: Pose2[Pose2(3, String[], (:Euclid, :Euclid, :Circular))] # Drive around in a hexagon in the cloud for i in 0:5 @@ -58,60 +47,27 @@ toDotFile(localFg, "/tmp/localfg.dot") # Alrighty! At this point, we should be able to solve locally... # perform inference, and remember first runs are slower owing to Julia's just-in-time compiling # Can do with graph too! -tree, smt, hist = solveTree!(localFg) +# tree, smt, hist = solveTree!(localFg) -wipeBuildNewTree!(localFg) +# wipeBuildNewTree!(localFg) tree, smt, hist = solveTree!(localFg, tree) # Recycle # batchSolve!(localFg, drawpdf=true, show=true) # Erm, whut? Error = mcmcIterationIDs -- unaccounted variables # Trying new method. -tree, smtasks = batchSolve!(localFg, treeinit=true, drawpdf=true, show=true, - returntasks=true, limititers=50, - upsolve=true, downsolve=true ) - -#### WIP and general debugging - -# Testing with GenericMarginal -# This will not work because GenericMarginal *shouldn't* really be persisted. -# That would mean we're decomposing the cloud graph... -# genmarg = GenericMarginal() -# Xi = [getVariable(fg, :x0)] -# addFactor!(fg, Xi, genmarg, autoinit=false) - -# For Juno/Jupyter style use -pl = drawPoses(localFg, meanmax=:mean) -plotPose(fg, :x6) -# For scripting use-cases you can export the image -Gadfly.draw(Gadfly.PDF("/tmp/test1.pdf", 20cm, 10cm),pl) # or PNG(...) - - -# Add landmarks with Bearing range measurements -addVariable!(fg, :l1, Point2, labels=["LANDMARK"]) -p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) -addFactor!(fg, [:x0; :l1], p2br ) - - -# Initialize :l1 numerical values but do not rerun solver -ensureAllInitialized!(fg) -pl = drawPosesLandms(fg) -Gadfly.draw(Gadfly.PDF("/tmp/test2.pdf", 20cm, 10cm),pl) # or PNG(...) - - -# Add landmarks with Bearing range measurements -p2br2 = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) -addFactor!(fg, [:x6; :l1], p2br2 ) - - -# solve -batchSolve!(fg, drawpdf=true) - - -# redraw -pl = drawPosesLandms(fg, meanmax=:mean) -Gadfly.draw(Gadfly.PDF("/tmp/test3.pdf", 20cm, 10cm),pl) # or PNG(...) - - - +# tree, smtasks = batchSolve!(localFg, treeinit=true, drawpdf=true, show=true, + # returntasks=true, limititers=50, + # upsolve=true, downsolve=true ) + +# Testing writing estimates +for variable in getVariables(localFg) + means = mean(getData(variable).val, dims=2)[:] + variable.estimateDict[:default] = Dict{Symbol, VariableEstimate}(:Mean => VariableEstimate(:default, :Mean, means, now())) +end -# +x0 = getVariable(localFg, :x0) +data = getData(x0) +# Update back to cloud. +for variable in getVariables(localFg) + updateVariableSolverData!(cloudFg, variable) +end From 8591d3a1ad860686f0961915c3c05502324e44da Mon Sep 17 00:00:00 2001 From: gearsad Date: Tue, 10 Sep 2019 22:51:44 -0500 Subject: [PATCH 04/10] Adding single call to push graphs solverDicts and estimateDicts --- src/CloudGraphsDFG/services/CloudGraphsDFG.jl | 2 +- src/DistributedFactorGraphs.jl | 1 + src/GraphsDFG/GraphsDFG.jl | 2 +- src/GraphsDFG/services/GraphsDFG.jl | 13 ++++++++ src/services/AbstractDFG.jl | 13 ++++++++ test/HexagonalCloud.jl | 30 ++++--------------- 6 files changed, 35 insertions(+), 26 deletions(-) diff --git a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl index d3b5ac3f..59d3383e 100644 --- a/src/CloudGraphsDFG/services/CloudGraphsDFG.jl +++ b/src/CloudGraphsDFG/services/CloudGraphsDFG.jl @@ -395,7 +395,7 @@ Update solver and estimate data for a variable (variable can be from another gra """ function updateVariableSolverData!(dfg::CloudGraphsDFG, sourceVariable::DFGVariable)::DFGVariable if !exists(dfg, sourceVariable) - error("Source variable '$(variable.label)' doesn't exist in the graph.") + error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") end nodeId = _tryGetNeoNodeIdFromNodeLabel(dfg.neo4jInstance, dfg.userId, dfg.robotId, dfg.sessionId, sourceVariable.label) Neo4j.setnodeproperty(dfg.neo4jInstance.graph, nodeId, "estimateDict", JSON2.write(sourceVariable.estimateDict)) diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index 7861d927..ae68f6ca 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -31,6 +31,7 @@ export label, data, id # Services/AbstractDFG Exports export hasFactor, hasVariable, isInitialized, getFactorFunction, isVariable, isFactor +export updateGraphSolverData! # Solver (IIF) Exports export VariableNodeData, PackedVariableNodeData, VariableEstimate diff --git a/src/GraphsDFG/GraphsDFG.jl b/src/GraphsDFG/GraphsDFG.jl index c85c4c47..896b23cb 100644 --- a/src/GraphsDFG/GraphsDFG.jl +++ b/src/GraphsDFG/GraphsDFG.jl @@ -13,7 +13,7 @@ export getAddHistory, getDescription, getLabelDict export addVariable!, addFactor! export ls, lsf, getVariables, getFactors, getVariableIds, getFactorIds export getVariable, getFactor -export updateVariable!, updateFactor! +export updateVariable!, updateFactor!, updateVariableSolverData! export deleteVariable!, deleteFactor! export getAdjacencyMatrix export getNeighbors diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index 37e88de6..9c7ae3fa 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -164,6 +164,19 @@ function updateVariable!(dfg::GraphsDFG, variable::DFGVariable)::DFGVariable return variable end +""" + $(SIGNATURES) +Update solver and estimate data for a variable (variable can be from another graph). +""" +function updateVariableSolverData!(dfg::GraphsDFG, sourceVariable::DFGVariable)::DFGVariable + if !haskey(dfg.labelDict, sourceVariable.label) + error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") + end + dfg.g.vertices[dfg.labelDict[variable.label]].dfgNode.estimateDict = deepcopy(variable.estimateDict) + dfg.g.vertices[dfg.labelDict[variable.label]].dfgNode.solverDataDict = deepcopy(variable.solverDataDict) + return sourceVariable +end + """ $(SIGNATURES) Update a complete DFGFactor in the DFG. diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index 31015ca6..e3ff06ed 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -67,6 +67,19 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{ return nothing end +""" + $(SIGNATURES) +Common function to update all solver data and estimates from one graph to another. +This should be used to push local solve data back into a cloud graph, for example. +""" +function updateGraphSolverData!(sourceDFG::G, destDFG::H, varSyms::Vector{Symbol})::Nothing where {G <: AbstractDFG, H <: AbstractDFG} + # Update all variables in the destination + # (For now... we may change this soon) + for variableId in varSyms + updateVariableSolverData!(destDFG, getVariable(sourceDFG, variableId)) + end +end + ## Utility functions for getting type names and modules (from IncrementalInference) function _getmodule(t::T) where T T.name.module diff --git a/test/HexagonalCloud.jl b/test/HexagonalCloud.jl index 5c6e8901..0ea8fab1 100644 --- a/test/HexagonalCloud.jl +++ b/test/HexagonalCloud.jl @@ -1,4 +1,4 @@ -using Revise +# using Revise using Neo4j # So that DFG initializes the database driver. using RoME using DistributedFactorGraphs @@ -14,13 +14,10 @@ cloudFg = CloudGraphsDFG{SolverParams}("localhost", 7474, "neo4j", "test", IncrementalInference.rebuildFactorMetadata!, solverParams=SolverParams()) # cloudFg = GraphsDFG{SolverParams}(params=SolverParams()) -# cloudFg = GraphsDFG{SolverParams}(params=SolverParams()) clearSession!!(cloudFg) -# cloudFg = initfg() # Add the first pose :x0 x0 = addVariable!(cloudFg, :x0, Pose2) -IncrementalInference.compareVariable(x0, getVariable(cloudFg, :x0)) # Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4) prior = addFactor!(cloudFg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) ) @@ -44,20 +41,9 @@ DistributedFactorGraphs._copyIntoGraph!(cloudFg, localFg, union(getVariableIds(c # Show it toDotFile(localFg, "/tmp/localfg.dot") -# Alrighty! At this point, we should be able to solve locally... -# perform inference, and remember first runs are slower owing to Julia's just-in-time compiling -# Can do with graph too! -# tree, smt, hist = solveTree!(localFg) - -# wipeBuildNewTree!(localFg) -tree, smt, hist = solveTree!(localFg, tree) # Recycle -# batchSolve!(localFg, drawpdf=true, show=true) -# Erm, whut? Error = mcmcIterationIDs -- unaccounted variables - -# Trying new method. -# tree, smtasks = batchSolve!(localFg, treeinit=true, drawpdf=true, show=true, - # returntasks=true, limititers=50, - # upsolve=true, downsolve=true ) +tree, smtasks = batchSolve!(localFg, treeinit=true, drawpdf=false, show=false, + returntasks=true, limititers=50, + upsolve=true, downsolve=true ) # Testing writing estimates for variable in getVariables(localFg) @@ -65,9 +51,5 @@ for variable in getVariables(localFg) variable.estimateDict[:default] = Dict{Symbol, VariableEstimate}(:Mean => VariableEstimate(:default, :Mean, means, now())) end -x0 = getVariable(localFg, :x0) -data = getData(x0) -# Update back to cloud. -for variable in getVariables(localFg) - updateVariableSolverData!(cloudFg, variable) -end +# Push updates back to cloud. +updateGraphSolverData!(localFg, cloudFg, ls(localFg)) From bff33dd516b8af89b9cf73a7b05da2bc8deb0b0f Mon Sep 17 00:00:00 2001 From: gearsad Date: Wed, 11 Sep 2019 23:58:04 -0500 Subject: [PATCH 05/10] Fix for `updateVariabeSolverData` and changes to HexagonalCloud example. --- src/GraphsDFG/services/GraphsDFG.jl | 4 ++-- test/HexagonalCloud.jl | 27 +++++++++++++++++++-------- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index 9c7ae3fa..62de6d74 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -172,8 +172,8 @@ function updateVariableSolverData!(dfg::GraphsDFG, sourceVariable::DFGVariable): if !haskey(dfg.labelDict, sourceVariable.label) error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") end - dfg.g.vertices[dfg.labelDict[variable.label]].dfgNode.estimateDict = deepcopy(variable.estimateDict) - dfg.g.vertices[dfg.labelDict[variable.label]].dfgNode.solverDataDict = deepcopy(variable.solverDataDict) + dfg.g.vertices[dfg.labelDict[sourceVariable.label]].dfgNode.estimateDict = deepcopy(sourceVariable.estimateDict) + dfg.g.vertices[dfg.labelDict[sourceVariable.label]].dfgNode.solverDataDict = deepcopy(sourceVariable.solverDataDict) return sourceVariable end diff --git a/test/HexagonalCloud.jl b/test/HexagonalCloud.jl index 0ea8fab1..a35f16ef 100644 --- a/test/HexagonalCloud.jl +++ b/test/HexagonalCloud.jl @@ -1,9 +1,8 @@ -# using Revise +using Revise using Neo4j # So that DFG initializes the database driver. using RoME using DistributedFactorGraphs using Test, Dates - # start with an empty factor graph object # fg = initfg() cloudFg = CloudGraphsDFG{SolverParams}("localhost", 7474, "neo4j", "test", @@ -34,6 +33,9 @@ end # Right, let's copy it into local memory for solving... localFg = GraphsDFG{SolverParams}(params=SolverParams()) DistributedFactorGraphs._copyIntoGraph!(cloudFg, localFg, union(getVariableIds(cloudFg), getFactorIds(cloudFg)), true) +# Duplicate for later +localFgCopy = deepcopy(localFg) + # Some checks @test symdiff(getVariableIds(localFg), getVariableIds(cloudFg)) == [] @test symdiff(getFactorIds(localFg), getFactorIds(cloudFg)) == [] @@ -41,15 +43,24 @@ DistributedFactorGraphs._copyIntoGraph!(cloudFg, localFg, union(getVariableIds(c # Show it toDotFile(localFg, "/tmp/localfg.dot") -tree, smtasks = batchSolve!(localFg, treeinit=true, drawpdf=false, show=false, - returntasks=true, limititers=50, - upsolve=true, downsolve=true ) +tree, smtasks = solveTree!(localFg) + +# solveTree!(cloudFg) -# Testing writing estimates +# Checking estimates for variable in getVariables(localFg) - means = mean(getData(variable).val, dims=2)[:] - variable.estimateDict[:default] = Dict{Symbol, VariableEstimate}(:Mean => VariableEstimate(:default, :Mean, means, now())) + @show variable.label + @show variable.estimateDict + + # means = mean(getData(variable).val, dims=2)[:] + # variable.estimateDict[:default] = Dict{Symbol, VariableEstimate}(:Mean => VariableEstimate(:default, :Mean, means, now())) end +bel = getKDE(getVariable(localFg, :x0)) +bel + # Push updates back to cloud. updateGraphSolverData!(localFg, cloudFg, ls(localFg)) + +# Pull back to local +updateGraphSolverData!(cloudFg, localFgCopy, ls(cloudFg)) From 8d866f4d491ad127bf64630189353131497ca7d7 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Sep 2019 12:01:54 +0200 Subject: [PATCH 06/10] InMemoryDFGTypes UpdateVariableSolverData! --- src/DistributedFactorGraphs.jl | 1 + src/services/CommonInMemoryDFG.jl | 23 +++++++++++++++++++++++ test/interfaceTests.jl | 14 ++++++++++++++ 3 files changed, 38 insertions(+) create mode 100644 src/services/CommonInMemoryDFG.jl diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index ae68f6ca..f9c686ac 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -78,5 +78,6 @@ end # To be moved as necessary. include("Common.jl") +include("services/CommonInMemoryDFG.jl") end diff --git a/src/services/CommonInMemoryDFG.jl b/src/services/CommonInMemoryDFG.jl new file mode 100644 index 00000000..23e8e874 --- /dev/null +++ b/src/services/CommonInMemoryDFG.jl @@ -0,0 +1,23 @@ +## Common funtions to all InMemoryDFGTypes + + +#TODO consider moving IIF InMemoryDFGTypes here and exporting, +# but what IIF supports and what DFG support is different? +# define and don't export yet +const InMemoryDFGTypes = Union{SymbolDFG, LightDFG, GraphsDFG, MetaGraphsDFG} +#Union{SymbolDFG, LightDFG} + +""" + $(SIGNATURES) +Update solver and estimate data for a variable (variable can be from another graph). +""" +function updateVariableSolverData!(dfg::InMemoryDFGTypes, sourceVariable::DFGVariable)::DFGVariable + #TODO test graphs that pass var/factor by reference + if !exists(dfg, sourceVariable) + error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") + end + var = getVariable(dfg, sourceVariable.label) + merge!(var.estimateDict, sourceVariable.estimateDict) + merge!(var.solverDataDict, sourceVariable.solverDataDict) + return sourceVariable +end diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 4d9fe285..640d39bd 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -94,6 +94,20 @@ end @test internalId(f1) == f1._internalId end +@testset "Updating Nodes" begin + global dfg + v1 = getVariable(dfg, :a) + newv1 = deepcopy(v1) + newv1.estimateDict[:default] = Dict{Symbol, VariableEstimate}( + :max => VariableEstimate(:default, :max, [100.0]), + :mean => VariableEstimate(:default, :mean, [50.0]), + :ppe => VariableEstimate(:default, :ppe, [75.0])) + updateVariableSolverData!(dfg, newv1) + #TODO maybe implement ==; @test newv1==v1 + #For now spot check + @test newv1.solverDataDict == v1.solverDataDict +end + # Connectivity test @testset "Connectivity Test" begin global dfg,v1,v2,f1 From ad6917d7ef31b52e345521f7dfafcff4e8642dd9 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Sep 2019 13:40:50 +0200 Subject: [PATCH 07/10] Fails on GraphsDFG, skipping for now Don't know if i have a flaw in how I would like it to work. --- test/interfaceTests.jl | 40 ++++++++++++++++++++++++++++++++++------ 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 640d39bd..964c2697 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -96,16 +96,44 @@ end @testset "Updating Nodes" begin global dfg - v1 = getVariable(dfg, :a) - newv1 = deepcopy(v1) - newv1.estimateDict[:default] = Dict{Symbol, VariableEstimate}( + #get the variable + var = getVariable(dfg, :a) + #make a copy and simulate external changes + newvar = deepcopy(var) + newvar.estimateDict[:default] = Dict{Symbol, VariableEstimate}( :max => VariableEstimate(:default, :max, [100.0]), :mean => VariableEstimate(:default, :mean, [50.0]), :ppe => VariableEstimate(:default, :ppe, [75.0])) - updateVariableSolverData!(dfg, newv1) - #TODO maybe implement ==; @test newv1==v1 + #update + updateVariableSolverData!(dfg, newvar) + #TODO maybe implement ==; @test newvar==var + Base.:(==)(varest1::VariableEstimate, varest2::VariableEstimate) = begin + varest1.lastUpdatedTimestamp == varest2.lastUpdatedTimestamp || return false + varest1.type == varest2.type || return false + varest1.solverKey == varest2.solverKey || return false + varest1.estimate == varest2.estimate || return false + return true + end #For now spot check - @test newv1.solverDataDict == v1.solverDataDict + @test newvar.solverDataDict == var.solverDataDict + @test newvar.estimateDict == var.estimateDict + + #delete :default and replace to see if new ones can be added + delete!(newvar.estimateDict,:default) + newvar.estimateDict[:second] = Dict{Symbol, VariableEstimate}( + :max => VariableEstimate(:default, :max, [10.0]), + :mean => VariableEstimate(:default, :mean, [5.0]), + :ppe => VariableEstimate(:default, :ppe, [7.0])) + + updateVariableSolverData!(dfg, newvar) + + #FIXME + if testDFGAPI != GraphsDFG + @test symdiff(collect(keys(var.estimateDict)),Symbol[:default, :second]) == Symbol[] + else + @error "FIXME: $(keys(var.estimateDict)) should be [:default, :second]" + @test_skip symdiff(collect(keys(var.estimateDict)),Symbol[:default, :second]) == Symbol[] + end end # Connectivity test From af197353210d5d9b87959d95e56b542353514b64 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Sep 2019 16:43:43 +0200 Subject: [PATCH 08/10] Moved to AbstractDFG and removed InMemoryTypes --- src/DistributedFactorGraphs.jl | 1 - src/services/AbstractDFG.jl | 14 ++++++++++++++ src/services/CommonInMemoryDFG.jl | 23 ----------------------- 3 files changed, 14 insertions(+), 24 deletions(-) delete mode 100644 src/services/CommonInMemoryDFG.jl diff --git a/src/DistributedFactorGraphs.jl b/src/DistributedFactorGraphs.jl index f9c686ac..ae68f6ca 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -78,6 +78,5 @@ end # To be moved as necessary. include("Common.jl") -include("services/CommonInMemoryDFG.jl") end diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index e3ff06ed..78fd3b72 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -67,6 +67,20 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{ return nothing end +""" + $(SIGNATURES) +Update solver and estimate data for a variable (variable can be from another graph). +""" +function updateVariableSolverData!(dfg::AbstractDFG, sourceVariable::DFGVariable)::DFGVariable + if !exists(dfg, sourceVariable) + error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") + end + var = getVariable(dfg, sourceVariable.label) + merge!(var.estimateDict, sourceVariable.estimateDict) + merge!(var.solverDataDict, sourceVariable.solverDataDict) + return sourceVariable +end + """ $(SIGNATURES) Common function to update all solver data and estimates from one graph to another. diff --git a/src/services/CommonInMemoryDFG.jl b/src/services/CommonInMemoryDFG.jl deleted file mode 100644 index 23e8e874..00000000 --- a/src/services/CommonInMemoryDFG.jl +++ /dev/null @@ -1,23 +0,0 @@ -## Common funtions to all InMemoryDFGTypes - - -#TODO consider moving IIF InMemoryDFGTypes here and exporting, -# but what IIF supports and what DFG support is different? -# define and don't export yet -const InMemoryDFGTypes = Union{SymbolDFG, LightDFG, GraphsDFG, MetaGraphsDFG} -#Union{SymbolDFG, LightDFG} - -""" - $(SIGNATURES) -Update solver and estimate data for a variable (variable can be from another graph). -""" -function updateVariableSolverData!(dfg::InMemoryDFGTypes, sourceVariable::DFGVariable)::DFGVariable - #TODO test graphs that pass var/factor by reference - if !exists(dfg, sourceVariable) - error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") - end - var = getVariable(dfg, sourceVariable.label) - merge!(var.estimateDict, sourceVariable.estimateDict) - merge!(var.solverDataDict, sourceVariable.solverDataDict) - return sourceVariable -end From 6f76b680d0646081fe629fe943d7733e72491af8 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Sep 2019 17:28:51 +0200 Subject: [PATCH 09/10] Abstract interface for todot. Tests left as broken since order is changed --- src/services/AbstractDFG.jl | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index 78fd3b72..1e8f7055 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -182,3 +182,39 @@ end Display and return to console the user factor identified by tag name. """ showFactor(fgl::G, fsym::Symbol) where G <: AbstractDFG = @show getFactor(fgl,fsym) + + +""" + $(SIGNATURES) +Produces a dot-format of the graph for visualization. +""" +function toDot(dfg::AbstractDFG)::String + @warn "Falling Back to convert to GraphsDFG" + #TODO implement convert + graphsdfg = GraphsDFG{AbstractParams}() + DistributedFactorGraphs._copyIntoGraph!(dfg, graphsdfg, union(getVariableIds(dfg), getFactorIds(dfg)), true) + + return toDot(graphsdfg) +end + +""" + $(SIGNATURES) +Produces a dot file of the graph for visualization. +Download XDot to see the data + +Note +- Default location "/tmp/dfg.dot" -- MIGHT BE REMOVED +- Can be viewed with the `xdot` system application. +- Based on graphviz.org +""" +function toDotFile(dfg::AbstractDFG, fileName::String="/tmp/dfg.dot")::Nothing + @warn "Falling Back to convert to GraphsDFG" + #TODO implement convert + graphsdfg = GraphsDFG{AbstractParams}() + DistributedFactorGraphs._copyIntoGraph!(dfg, graphsdfg, union(getVariableIds(dfg), getFactorIds(dfg)), true) + + open(fileName, "w") do fid + write(fid,Graphs.to_dot(graphsdfg.g)) + end + return nothing +end From 91f40465b5de8c8c43513de0cdb7804daad10781 Mon Sep 17 00:00:00 2001 From: gearsad Date: Sun, 22 Sep 2019 23:58:19 -0500 Subject: [PATCH 10/10] Fixing tests --- src/GraphsDFG/services/GraphsDFG.jl | 13 ------------- src/services/AbstractDFG.jl | 7 +++++-- test/interfaceTests.jl | 28 +++++++++++++--------------- 3 files changed, 18 insertions(+), 30 deletions(-) diff --git a/src/GraphsDFG/services/GraphsDFG.jl b/src/GraphsDFG/services/GraphsDFG.jl index 62de6d74..37e88de6 100644 --- a/src/GraphsDFG/services/GraphsDFG.jl +++ b/src/GraphsDFG/services/GraphsDFG.jl @@ -164,19 +164,6 @@ function updateVariable!(dfg::GraphsDFG, variable::DFGVariable)::DFGVariable return variable end -""" - $(SIGNATURES) -Update solver and estimate data for a variable (variable can be from another graph). -""" -function updateVariableSolverData!(dfg::GraphsDFG, sourceVariable::DFGVariable)::DFGVariable - if !haskey(dfg.labelDict, sourceVariable.label) - error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") - end - dfg.g.vertices[dfg.labelDict[sourceVariable.label]].dfgNode.estimateDict = deepcopy(sourceVariable.estimateDict) - dfg.g.vertices[dfg.labelDict[sourceVariable.label]].dfgNode.solverDataDict = deepcopy(sourceVariable.solverDataDict) - return sourceVariable -end - """ $(SIGNATURES) Update a complete DFGFactor in the DFG. diff --git a/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index 1e8f7055..4a121c66 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -70,14 +70,17 @@ end """ $(SIGNATURES) Update solver and estimate data for a variable (variable can be from another graph). +Note: Makes a copy of the estimates and solver data so that there is no coupling +between graphs. """ function updateVariableSolverData!(dfg::AbstractDFG, sourceVariable::DFGVariable)::DFGVariable if !exists(dfg, sourceVariable) error("Source variable '$(sourceVariable.label)' doesn't exist in the graph.") end var = getVariable(dfg, sourceVariable.label) - merge!(var.estimateDict, sourceVariable.estimateDict) - merge!(var.solverDataDict, sourceVariable.solverDataDict) + # We don't know which graph this came from, must be copied! + merge!(var.estimateDict, deepcopy(sourceVariable.estimateDict)) + merge!(var.solverDataDict, deepcopy(sourceVariable.solverDataDict)) return sourceVariable end diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index 964c2697..08cb7f2b 100644 --- a/test/interfaceTests.jl +++ b/test/interfaceTests.jl @@ -17,7 +17,7 @@ addFactor!(dfg, [v1, v2], f1) # end @testset "Adding Removing Nodes" begin - dfg2 = testDFGAPI{NoSolverParams}() + dfg2 = GraphsDFG{NoSolverParams}() v1 = DFGVariable(:a) v2 = DFGVariable(:b) v3 = DFGVariable(:c) @@ -100,7 +100,7 @@ end var = getVariable(dfg, :a) #make a copy and simulate external changes newvar = deepcopy(var) - newvar.estimateDict[:default] = Dict{Symbol, VariableEstimate}( + estimates(newvar)[:default] = Dict{Symbol, VariableEstimate}( :max => VariableEstimate(:default, :max, [100.0]), :mean => VariableEstimate(:default, :mean, [50.0]), :ppe => VariableEstimate(:default, :ppe, [75.0])) @@ -115,25 +115,23 @@ end return true end #For now spot check - @test newvar.solverDataDict == var.solverDataDict - @test newvar.estimateDict == var.estimateDict + @test solverDataDict(newvar) == solverDataDict(var) + @test estimates(newvar) == estimates(var) - #delete :default and replace to see if new ones can be added - delete!(newvar.estimateDict,:default) - newvar.estimateDict[:second] = Dict{Symbol, VariableEstimate}( + # Delete :default and replace to see if new ones can be added + delete!(estimates(newvar), :default) + estimates(newvar)[:second] = Dict{Symbol, VariableEstimate}( :max => VariableEstimate(:default, :max, [10.0]), :mean => VariableEstimate(:default, :mean, [5.0]), :ppe => VariableEstimate(:default, :ppe, [7.0])) + # Persist to the original variable. updateVariableSolverData!(dfg, newvar) - - #FIXME - if testDFGAPI != GraphsDFG - @test symdiff(collect(keys(var.estimateDict)),Symbol[:default, :second]) == Symbol[] - else - @error "FIXME: $(keys(var.estimateDict)) should be [:default, :second]" - @test_skip symdiff(collect(keys(var.estimateDict)),Symbol[:default, :second]) == Symbol[] - end + # 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 # Connectivity test