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 04064c40..ac4594c6 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}) @@ -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 '$(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)) + 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/src/Common.jl b/src/Common.jl index c16286eb..276cd707 100644 --- a/src/Common.jl +++ b/src/Common.jl @@ -1,6 +1,5 @@ export sortVarNested export isPrior, lsfPriors -export getData export getVariableType, getSofttype export getFactorType, getfnctype export lsTypes, lsfTypes @@ -89,14 +88,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 fe4e8a63..ae68f6ca 100644 --- a/src/DistributedFactorGraphs.jl +++ b/src/DistributedFactorGraphs.jl @@ -25,10 +25,14 @@ 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, internalId, smallData, bigData export setSolverData export label, data, id +# Services/AbstractDFG Exports +export hasFactor, hasVariable, isInitialized, getFactorFunction, isVariable, isFactor +export updateGraphSolverData! + # Solver (IIF) Exports export VariableNodeData, PackedVariableNodeData, VariableEstimate export GenericFunctionNodeData#, FunctionNodeData @@ -72,9 +76,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/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/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/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..084e4a4b 100644 --- a/src/entities/DFGFactor.jl +++ b/src/entities/DFGFactor.jl @@ -49,10 +49,17 @@ 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} 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..a7c85585 100644 --- a/src/entities/DFGVariable.jl +++ b/src/entities/DFGVariable.jl @@ -72,9 +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 """ @@ -85,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 @@ -102,11 +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/src/services/AbstractDFG.jl b/src/services/AbstractDFG.jl index a94567a8..4c9f452d 100644 --- a/src/services/AbstractDFG.jl +++ b/src/services/AbstractDFG.jl @@ -337,29 +337,6 @@ function getAdjacencyMatrix(dfg::G)::Matrix{Union{Nothing, Symbol}} where G <: A error("getAdjacencyMatrix not implemented for $(typeof(dfg))") end -""" - $(SIGNATURES) -Produces a dot-format of the graph for visualization. -""" -function toDot(dfg::G)::String where G <: AbstractDFG - error("toDot not implemented for $(typeof(dfg))") -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::G, fileName::String="/tmp/dfg.dot")::Nothing where G <: AbstractDFG - error("toDotFile not implemented for $(typeof(dfg))") -end - - """ $(SIGNATURES) Common function for copying nodes from one graph into another graph. @@ -402,6 +379,36 @@ 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). +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) + # 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 + +""" + $(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 + """ $(SIGNATURES) Get an adjacency matrix for the DFG, returned as a tuple: adjmat::SparseMatrixCSC{Int}, var_labels::Vector{Symbol) fac_labels::Vector{Symbol). @@ -410,3 +417,111 @@ Rows are the factors, columns are the variables, with the corresponding labels i function getAdjacencyMatrixSparse(dfg::G) where G <: AbstractDFG error("getAdjacencyMatrixSparse not implemented for $(typeof(dfg))") end + +""" + $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) + + +""" + $(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 diff --git a/test/interfaceTests.jl b/test/interfaceTests.jl index c1d8e06f..18c1297e 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) @@ -87,16 +87,56 @@ 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 @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] = Dict{Symbol, VariableEstimate}( + :max => VariableEstimate(:default, :max, [100.0]), + :mean => VariableEstimate(:default, :mean, [50.0]), + :ppe => VariableEstimate(:default, :ppe, [75.0])) + #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 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] = 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) + # 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 @testset "Connectivity Test" begin global dfg,v1,v2,f1 diff --git a/test/obsolete_nowinRoME/FileDFG.jl b/test/obsolete_nowinRoME/FileDFG.jl index f83dae16..52b5692c 100644 --- a/test/obsolete_nowinRoME/FileDFG.jl +++ b/test/obsolete_nowinRoME/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/obsolete_nowinRoME/HexagonalCloud.jl b/test/obsolete_nowinRoME/HexagonalCloud.jl index 86d69516..a35f16ef 100644 --- a/test/obsolete_nowinRoME/HexagonalCloud.jl +++ b/test/obsolete_nowinRoME/HexagonalCloud.jl @@ -2,8 +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() cloudFg = CloudGraphsDFG{SolverParams}("localhost", 7474, "neo4j", "test", @@ -14,27 +13,13 @@ 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))) ) ) -# 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 @@ -48,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)) == [] @@ -55,63 +43,24 @@ 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 ) - -#### 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(...) +tree, smtasks = solveTree!(localFg) +# solveTree!(cloudFg) -# 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(...) +# Checking estimates +for variable in getVariables(localFg) + @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))