Start of importing/exporting

Author: GearsAD

src/CloudGraphsDFG/services/CloudGraphsDFG.jl

@@ -4,20 +4,6 @@ export copySession!
 # With great power comes great "Oh crap, I deleted everything..."
 export clearSession!!, clearRobot!!, clearUser!!
 
-## Utility functions for getting type names and modules (from IncrementalInference)
-function _getmodule(t::T) where T
-  T.name.module
-end
-function _getname(t::T) where T
-  T.name.name
-end
-
-# 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)
-
 function _getNodeType(dfg::CloudGraphsDFG, nodeLabel::Symbol)::Symbol
     dfg.useCache && haskey(dfg.variableDict, nodeLabel) && return :VARIABLE
     dfg.useCache && haskey(dfg.factorDict, nodeLabel) && return :FACTOR
@@ -344,7 +330,7 @@ function getFactor(dfg::CloudGraphsDFG, factorId::Int64)::DFGFactor
     # 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 
+    # ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
     getData(factor).fncargvID = _variableOrderSymbols
 
     # Add to cache

src/DistributedFactorGraphs.jl

@@ -41,6 +41,11 @@ include("services/DFGVariable.jl")
 # Include the Graphs.jl API.
 include("GraphsDFG/GraphsDFG.jl")
 
+# Include the FilesDFG API.
+include("FileDFG/FileDFG.jl")
+
+export saveDFG, loadDFG
+
 function __init__()
     @require DataFrames="a93c6f00-e57d-5684-b7b6-d8193f3e46c0" begin
         if isdefined(Main, :DataFrames)

src/FileDFG/FileDFG.jl

@@ -0,0 +1,6 @@
+
+# Entities
+include("entities/FileDFG.jl")
+
+# Services
+include("services/FileDFG.jl")

src/FileDFG/entities/FileDFG.jl

@@ -0,0 +1,5 @@
+
+# mutable struct FileDFG
+#     folderName::String
+#     FileDFG(folderName::String)::FileDFG = new(foldername)
+# end

src/FileDFG/services/FileDFG.jl

@@ -0,0 +1,67 @@
+
+function _packVariable(dfg::G, v::DFGVariable)::Dict{String, Any} where G <: AbstractDFG
+    props = Dict{String, Any}()
+    props["label"] = string(v.label)
+    props["timestamp"] = string(v.timestamp)
+    props["tags"] = JSON2.write(v.tags)
+    props["estimateDict"] = JSON2.write(v.estimateDict)
+    props["solverDataDict"] = JSON2.write(Dict(keys(v.solverDataDict) .=> map(vnd -> pack(dfg, vnd), values(v.solverDataDict))))
+    props["smallData"] = JSON2.write(v.smallData)
+    props["ready"] = v.ready
+    props["backendset"] = v.backendset
+    return props
+end
+
+function _packFactor(dfg::G, f::DFGFactor)::Dict{String, Any} where G <: AbstractDFG
+    # Construct the properties to save
+    props = Dict{String, Any}()
+    props["label"] = string(f.label)
+    props["tags"] = JSON2.write(f.tags)
+    # Pack the node data
+    fnctype = f.data.fnc.usrfnc!
+    packtype = getfield(_getmodule(fnctype), Symbol("Packed$(_getname(fnctype))"))
+    packed = convert(PackedFunctionNodeData{packtype}, f.data)
+    props["data"] = JSON2.write(packed)
+    # Include the type
+    props["fnctype"] = String(_getname(fnctype))
+    props["_variableOrderSymbols"] = JSON2.write(f._variableOrderSymbols)
+    props["backendset"] = f.backendset
+    props["ready"] = f.ready
+
+    return props
+end
+
+function saveDFG(dfg::G, folder::String) where G <: AbstractDFG
+    variables = getVariables(dfg)
+    factors = getFactors(dfg)
+    varFolder = "$folder/variables"
+    factorFolder = "$folder/factors"
+    # Folder preparations
+    if !isdir(folder)
+        @info "Folder '$folder' doesn't exist, creating..."
+        mkpath(folder)
+    end
+    !isdir(varFolder) && mkpath(varFolder)
+    !isdir(factorFolder) && mkpath(factorFolder)
+    # Clearing out the folders
+    map(f -> rm("$varFolder/$f"), readdir(varFolder))
+    map(f -> rm("$factorFolder/$f"), readdir(factorFolder))
+    # Variables
+    for v in variables
+        vPacked = _packVariable(dfg, v)
+        io = open("$varFolder/$(v.label).json", "w")
+        JSON2.write(io, vPacked)
+        close(io)
+    end
+    # Factors
+    for f in factors
+        fPacked = _packFactor(dfg, f)
+        io = open("$folder/factors/$(f.label).json", "w")
+        JSON2.write(io, fPacked)
+        close(io)
+    end
+end
+
+function loadDFG(folderName::String, dfgLoadInto::G=GraphsDFG{NoSolverParams}()) where G <: AbstractDFG
+    return dfgLoadInto
+end

src/entities/DFGFactor.jl

@@ -50,3 +50,9 @@ 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
+
+# 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)

src/services/AbstractDFG.jl

@@ -62,3 +62,11 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{
     end
     return nothing
 end
+
+## Utility functions for getting type names and modules (from IncrementalInference)
+function _getmodule(t::T) where T
+  T.name.module
+end
+function _getname(t::T) where T
+  T.name.name
+end

test/FileDFG.jl

@@ -0,0 +1,25 @@
+using Revise
+using Test
+using DistributedFactorGraphs
+using IncrementalInference, RoME
+
+# Make a simple graph
+dfg = GraphsDFG{SolverParams}(params=SolverParams())
+
+# Add the first pose :x0
+x0 = addVariable!(dfg, :x0, Pose2)
+IncrementalInference.compareVariable(x0, getVariable(dfg, :x0))
+
+# Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
+prior = addFactor!(dfg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) )
+
+# Drive around in a hexagon
+for i in 0:5
+    psym = Symbol("x$i")
+    nsym = Symbol("x$(i+1)")
+    addVariable!(dfg, nsym, Pose2)
+    pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
+    addFactor!(dfg, [psym;nsym], pp )
+end
+
+saveDFG(dfg "/tmp/fileDFG")