Adding single call to push graphs solverDicts and estimateDicts

Author: GearsAD

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))

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

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

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.

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

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,30 +41,15 @@ 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)
     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
+# Push updates back to cloud.
+updateGraphSolverData!(localFg, cloudFg, ls(localFg))