Merge pull request #61 from JuliaRobotics/feature/cloudgraphdfg

CloudGraphs rewritten for DFG is now working

Author: GearsAD

Project.toml

@@ -9,6 +9,7 @@ Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
 JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Neo4j = "d2adbeaf-5838-5367-8a2f-e46d570981db"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"

src/CloudGraphsDFG/entities/CloudGraphsDFG.jl

@@ -14,6 +14,7 @@ mutable struct CloudGraphsDFG{T <: AbstractParams} <: AbstractDFG
     encodePackedTypeFunc
     getPackedTypeFunc
     decodePackedTypeFunc
+    rebuildFactorMetadata!
     labelDict::Dict{Symbol, Int64}
     variableCache::Dict{Symbol, DFGVariable}
     factorCache::Dict{Symbol, DFGFactor}
@@ -23,8 +24,8 @@ mutable struct CloudGraphsDFG{T <: AbstractParams} <: AbstractDFG
 end
 
 function show(io::IO, c::CloudGraphsDFG)
-    println("CloudGraphsDFG:")
-    println(" - Neo4J instance: $(c.neo4jInstance.connection.host)")
-    println(" - Session: $(c.userId):$(c.robotId):$(c.sessionId)")
-    println(" - Caching: $(c.useCache)")
+    println(io, "CloudGraphsDFG:")
+    println(io, " - Neo4J instance: $(c.neo4jInstance.connection.host)")
+    println(io, " - Session: $(c.userId):$(c.robotId):$(c.sessionId)")
+    println(io, " - Caching: $(c.useCache)")
 end

src/CloudGraphsDFG/services/CloudGraphsDFG.jl

@@ -41,18 +41,18 @@ end
     $(SIGNATURES)
 Create a new CloudGraphs-based DFG factor graph using a Neo4j.Connection.
 """
-function CloudGraphsDFG{T}(neo4jConnection::Neo4j.Connection, userId::String, robotId::String, sessionId::String, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc; description::String="CloudGraphs DFG", solverParams::T=NoSolverParams(), useCache::Bool=false) where T <: AbstractParams
+function CloudGraphsDFG{T}(neo4jConnection::Neo4j.Connection, userId::String, robotId::String, sessionId::String, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, rebuildFactorMetadata!; description::String="CloudGraphs DFG", solverParams::T=NoSolverParams(), useCache::Bool=false) where T <: AbstractParams
     graph = Neo4j.getgraph(neo4jConnection)
     neo4jInstance = Neo4jInstance(neo4jConnection, graph)
-    return CloudGraphsDFG{T}(neo4jInstance, description, userId, robotId, sessionId, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, Dict{Symbol, Int64}(), Dict{Symbol, DFGVariable}(), Dict{Symbol, DFGFactor}(), Symbol[], solverParams, useCache)
+    return CloudGraphsDFG{T}(neo4jInstance, description, userId, robotId, sessionId, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, rebuildFactorMetadata!, Dict{Symbol, Int64}(), Dict{Symbol, DFGVariable}(), Dict{Symbol, DFGFactor}(), Symbol[], solverParams, useCache)
 end
 """
     $(SIGNATURES)
 Create a new CloudGraphs-based DFG factor graph by specifying the Neo4j connection information.
 """
-function CloudGraphsDFG{T}(host::String, port::Int, dbUser::String, dbPassword::String, userId::String, robotId::String, sessionId::String, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc; description::String="CloudGraphs DFG", solverParams::T=NoSolverParams(), useCache::Bool=false) where T <: AbstractParams
+function CloudGraphsDFG{T}(host::String, port::Int, dbUser::String, dbPassword::String, userId::String, robotId::String, sessionId::String, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, rebuildFactorMetadata!; description::String="CloudGraphs DFG", solverParams::T=NoSolverParams(), useCache::Bool=false) where T <: AbstractParams
     neo4jConnection = Neo4j.Connection(host, port=port, user=dbUser, password=dbPassword);
-    return CloudGraphsDFG{T}(neo4jConnection, userId, robotId, sessionId, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, description=description, solverParams=solverParams, useCache=useCache)
+    return CloudGraphsDFG{T}(neo4jConnection, userId, robotId, sessionId, encodePackedTypeFunc, getPackedTypeFunc, decodePackedTypeFunc, rebuildFactorMetadata!, description=description, solverParams=solverParams, useCache=useCache)
 end
 
 """
@@ -323,10 +323,10 @@ function getFactor(dfg::CloudGraphsDFG, factorId::Int64)::DFGFactor
 
     data = props["data"]
     datatype = props["fnctype"]
-    fulltype = getfield(Main, Symbol(datatype))
+    # fulltype = getfield(Main, Symbol(datatype))
     packtype = getfield(Main, Symbol("Packed"*datatype))
     packed = JSON2.read(data, GenericFunctionNodeData{packtype,String})
-    fullFactor = dfg.decodePackedTypeFunc(packed, "")
+    fullFactor = dfg.decodePackedTypeFunc(dfg, packed)
 
     # Include the type
     _variableOrderSymbols = JSON2.read(props["_variableOrderSymbols"], Vector{Symbol})
@@ -341,6 +341,12 @@ function getFactor(dfg::CloudGraphsDFG, factorId::Int64)::DFGFactor
     factor.ready = ready
     factor.backendset = backendset
 
+    # Lastly, rebuild the metadata
+    factor = dfg.rebuildFactorMetadata!(dfg, factor)
+    # GUARANTEED never to bite us in the ass in the future...
+    # ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350 
+    getData(factor).fncargvID = _variableOrderSymbols
+
     # Add to cache
     push!(dfg.factorCache, factor.label=>factor)
 

src/DistributedFactorGraphs.jl

@@ -7,6 +7,7 @@ using Dates
 using Distributions
 using Reexport
 using JSON2
+using LinearAlgebra
 
 # Entities
 include("entities/AbstractTypes.jl")
@@ -33,6 +34,7 @@ export GenericFunctionNodeData#, FunctionNodeData
 export getSerializationModule, setSerializationModule!
 export pack, unpack
 
+# Common includes
 include("services/AbstractDFG.jl")
 include("services/DFGVariable.jl")
 

src/GraphsDFG/services/GraphsDFG.jl

@@ -89,6 +89,15 @@ function addFactor!(dfg::GraphsDFG, variables::Vector{DFGVariable}, factor::DFGF
     return true
 end
 
+"""
+    $(SIGNATURES)
+Add a DFGFactor to a DFG.
+"""
+function addFactor!(dfg::GraphsDFG, variableIds::Vector{Symbol}, factor::DFGFactor)::Bool
+    variables = map(vId -> getVariable(dfg, vId), variableIds)
+    return addFactor!(dfg, variables, factor)
+end
+
 """
     $(SIGNATURES)
 Get a DFGVariable from a DFG using its underlying integer ID.

src/entities/DFGFactor.jl

@@ -47,9 +47,6 @@ mutable struct DFGFactor{T, S} <: DFGNode
     DFGFactor{T, S}(label::Symbol, _internalId::Int64) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, 0, _internalId, Symbol[])
 end
 
-# 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)
-
 label(f::F) where F <: DFGFactor = f.label
 data(f::F) where F <: DFGFactor = f.data
 id(f::F) where F <: DFGFactor = f._internalId

test/HexagonalCloud.jl

@@ -0,0 +1,117 @@
+using Revise
+using Neo4j # So that DFG initializes the database driver.
+using RoME
+using DistributedFactorGraphs
+using Test
+
+# start with an empty factor graph object
+# fg = initfg()
+cloudFg = CloudGraphsDFG{SolverParams}("localhost", 7474, "neo4j", "test",
+    "testUser", "testRobot", "testSession",
+    nothing,
+    nothing,
+    IncrementalInference.decodePackedType,
+    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
+    psym = Symbol("x$i")
+    nsym = Symbol("x$(i+1)")
+    addVariable!(cloudFg, nsym, Pose2)
+    pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+    addFactor!(cloudFg, [psym;nsym], pp )
+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)
+# Some checks
+@test symdiff(getVariableIds(localFg), getVariableIds(cloudFg)) == []
+@test symdiff(getFactorIds(localFg), getFactorIds(cloudFg)) == []
+@test isFullyConnected(localFg)
+# 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(...)
+
+
+# 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(...)
+
+
+
+
+#