Skip to content

Commit 18d962e

Browse files
dehanndehann
authored
Merge pull request #62 from JuliaRobotics/feature/filedfg
File importing and exporting for DFG
2 parents ddd4a99 + 21b17d9 commit 18d962e

File tree

8 files changed

+224
-16
lines changed

8 files changed

+224
-16
lines changed

‎src/CloudGraphsDFG/services/CloudGraphsDFG.jl

Lines changed: 2 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -4,20 +4,6 @@ export copySession!
44
# With great power comes great "Oh crap, I deleted everything..."
55
export clearSession!!, clearRobot!!, clearUser!!
66

7-
## Utility functions for getting type names and modules (from IncrementalInference)
8-
function _getmodule(t::T) where T
9-
T.name.module
10-
end
11-
function _getname(t::T) where T
12-
T.name.name
13-
end
14-
15-
# Simply for convenience - don't export
16-
const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString}
17-
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)
18-
const FunctionNodeData{T} = GenericFunctionNodeData{T, Symbol}
19-
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)
20-
217
function _getNodeType(dfg::CloudGraphsDFG, nodeLabel::Symbol)::Symbol
228
dfg.useCache && haskey(dfg.variableDict, nodeLabel) && return :VARIABLE
239
dfg.useCache && haskey(dfg.factorDict, nodeLabel) && return :FACTOR
@@ -343,8 +329,8 @@ function getFactor(dfg::CloudGraphsDFG, factorId::Int64)::DFGFactor
343329

344330
# Lastly, rebuild the metadata
345331
factor = dfg.rebuildFactorMetadata!(dfg, factor)
346-
# GUARANTEED never to bite us in the ass in the future...
347-
# ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
332+
# GUARANTEED never to bite us in the future...
333+
# ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
348334
getData(factor).fncargvID = _variableOrderSymbols
349335

350336
# Add to cache

‎src/DistributedFactorGraphs.jl

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,11 @@ include("services/DFGVariable.jl")
4141
# Include the Graphs.jl API.
4242
include("GraphsDFG/GraphsDFG.jl")
4343

44+
# Include the FilesDFG API.
45+
include("FileDFG/FileDFG.jl")
46+
47+
export saveDFG, loadDFG
48+
4449
function __init__()
4550
@require DataFrames="a93c6f00-e57d-5684-b7b6-d8193f3e46c0" begin
4651
if isdefined(Main, :DataFrames)

‎src/FileDFG/FileDFG.jl

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
2+
# Entities
3+
include("entities/FileDFG.jl")
4+
5+
# Services
6+
include("services/FileDFG.jl")

‎src/FileDFG/entities/FileDFG.jl

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
2+
# mutable struct FileDFG
3+
# folderName::String
4+
# FileDFG(folderName::String)::FileDFG = new(foldername)
5+
# end

‎src/FileDFG/services/FileDFG.jl

Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
2+
function _packVariable(dfg::G, v::DFGVariable)::Dict{String, Any} where G <: AbstractDFG
3+
props = Dict{String, Any}()
4+
props["label"] = string(v.label)
5+
props["timestamp"] = string(v.timestamp)
6+
props["tags"] = JSON2.write(v.tags)
7+
props["estimateDict"] = JSON2.write(v.estimateDict)
8+
props["solverDataDict"] = JSON2.write(Dict(keys(v.solverDataDict) .=> map(vnd -> pack(dfg, vnd), values(v.solverDataDict))))
9+
props["smallData"] = JSON2.write(v.smallData)
10+
props["ready"] = v.ready
11+
props["backendset"] = v.backendset
12+
return props
13+
end
14+
15+
function _unpackVariable(dfg::G, packedProps::Dict{String, Any})::DFGVariable where G <: AbstractDFG
16+
label = Symbol(packedProps["label"])
17+
timestamp = DateTime(packedProps["timestamp"])
18+
tags = JSON2.read(packedProps["tags"], Vector{Symbol})
19+
estimateDict = JSON2.read(packedProps["estimateDict"], Dict{Symbol, VariableEstimate})
20+
smallData = nothing
21+
smallData = JSON2.read(packedProps["smallData"], Dict{String, String})
22+
23+
packed = JSON2.read(packedProps["solverDataDict"], Dict{String, PackedVariableNodeData})
24+
solverData = Dict(Symbol.(keys(packed)) .=> map(p -> unpack(dfg, p), values(packed)))
25+
26+
# Rebuild DFGVariable
27+
variable = DFGVariable(Symbol(packedProps["label"]))
28+
variable.timestamp = timestamp
29+
variable.tags = tags
30+
variable.estimateDict = estimateDict
31+
variable.solverDataDict = solverData
32+
variable.smallData = smallData
33+
variable.ready = packedProps["ready"]
34+
variable.backendset = packedProps["backendset"]
35+
36+
return variable
37+
end
38+
39+
function _packFactor(dfg::G, f::DFGFactor)::Dict{String, Any} where G <: AbstractDFG
40+
# Construct the properties to save
41+
props = Dict{String, Any}()
42+
props["label"] = string(f.label)
43+
props["tags"] = JSON2.write(f.tags)
44+
# Pack the node data
45+
fnctype = f.data.fnc.usrfnc!
46+
packtype = getfield(_getmodule(fnctype), Symbol("Packed$(_getname(fnctype))"))
47+
packed = convert(PackedFunctionNodeData{packtype}, f.data)
48+
props["data"] = JSON2.write(packed)
49+
# Include the type
50+
props["fnctype"] = String(_getname(fnctype))
51+
props["_variableOrderSymbols"] = JSON2.write(f._variableOrderSymbols)
52+
props["backendset"] = f.backendset
53+
props["ready"] = f.ready
54+
55+
return props
56+
end
57+
58+
59+
function _unpackFactor(dfg::G, packedProps::Dict{String, Any}, iifModule)::DFGFactor where G <: AbstractDFG
60+
label = packedProps["label"]
61+
tags = JSON2.read(packedProps["tags"], Vector{Symbol})
62+
63+
data = packedProps["data"]
64+
datatype = packedProps["fnctype"]
65+
packtype = getfield(Main, Symbol("Packed"*datatype))
66+
packed = JSON2.read(data, GenericFunctionNodeData{packtype,String})
67+
fullFactor = iifModule.decodePackedType(dfg, packed)
68+
# fullFactor = dfg.decodePackedTypeFunc(dfg, packed)
69+
70+
# Include the type
71+
_variableOrderSymbols = JSON2.read(packedProps["_variableOrderSymbols"], Vector{Symbol})
72+
backendset = packedProps["backendset"]
73+
ready = packedProps["ready"]
74+
75+
# Rebuild DFGVariable
76+
factor = DFGFactor{typeof(fullFactor.fnc), Symbol}(Symbol(label))
77+
factor.tags = tags
78+
factor.data = fullFactor
79+
factor._variableOrderSymbols = _variableOrderSymbols
80+
factor.ready = ready
81+
factor.backendset = backendset
82+
83+
# GUARANTEED never to bite us in the ass in the future...
84+
# ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
85+
getData(factor).fncargvID = _variableOrderSymbols
86+
87+
# Note, once inserted, you still need to call IIF.rebuildFactorMetadata!
88+
return factor
89+
end
90+
91+
function saveDFG(dfg::G, folder::String) where G <: AbstractDFG
92+
variables = getVariables(dfg)
93+
factors = getFactors(dfg)
94+
varFolder = "$folder/variables"
95+
factorFolder = "$folder/factors"
96+
# Folder preparations
97+
if !isdir(folder)
98+
@info "Folder '$folder' doesn't exist, creating..."
99+
mkpath(folder)
100+
end
101+
!isdir(varFolder) && mkpath(varFolder)
102+
!isdir(factorFolder) && mkpath(factorFolder)
103+
# Clearing out the folders
104+
map(f -> rm("$varFolder/$f"), readdir(varFolder))
105+
map(f -> rm("$factorFolder/$f"), readdir(factorFolder))
106+
# Variables
107+
for v in variables
108+
vPacked = _packVariable(dfg, v)
109+
io = open("$varFolder/$(v.label).json", "w")
110+
JSON2.write(io, vPacked)
111+
close(io)
112+
end
113+
# Factors
114+
for f in factors
115+
fPacked = _packFactor(dfg, f)
116+
io = open("$folder/factors/$(f.label).json", "w")
117+
JSON2.write(io, fPacked)
118+
close(io)
119+
end
120+
end
121+
122+
function loadDFG(folder::String, iifModule, dfgLoadInto::G=GraphsDFG{NoSolverParams}()) where G <: AbstractDFG
123+
variables = DFGVariable[]
124+
factors = DFGFactor[]
125+
varFolder = "$folder/variables"
126+
factorFolder = "$folder/factors"
127+
# Folder preparations
128+
!isdir(folder) && error("Can't load DFG graph - folder '$folder' doesn't exist")
129+
!isdir(varFolder) && error("Can't load DFG graph - folder '$folder' doesn't exist")
130+
!isdir(factorFolder) && error("Can't load DFG graph - folder '$folder' doesn't exist")
131+
132+
varFiles = readdir(varFolder)
133+
factorFiles = readdir(factorFolder)
134+
for varFile in varFiles
135+
io = open("$varFolder/$varFile")
136+
packedData = JSON2.read(io, Dict{String, Any})
137+
push!(variables, _unpackVariable(dfgLoadInto, packedData))
138+
end
139+
@info "Loaded $(length(variables)) variables - $(map(v->v.label, variables))"
140+
@info "Inserting variables into graph..."
141+
# Adding variables
142+
map(v->addVariable!(dfgLoadInto, v), variables)
143+
144+
for factorFile in factorFiles
145+
io = open("$factorFolder/$factorFile")
146+
packedData = JSON2.read(io, Dict{String, Any})
147+
push!(factors, _unpackFactor(dfgLoadInto, packedData, iifModule))
148+
end
149+
@info "Loaded $(length(variables)) factors - $(map(f->f.label, factors))"
150+
@info "Inserting factors into graph..."
151+
# # Adding factors
152+
map(f->addFactor!(dfgLoadInto, f._variableOrderSymbols, f), factors)
153+
154+
# Finally, rebuild the CCW's for the factors to completely reinflate them
155+
@info "Rebuilding CCW's for the factors..."
156+
for factor in factors
157+
iifModule.rebuildFactorMetadata!(dfgLoadInto, factor)
158+
end
159+
160+
return dfgLoadInto
161+
end

‎src/entities/DFGFactor.jl

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,3 +50,9 @@ end
5050
label(f::F) where F <: DFGFactor = f.label
5151
data(f::F) where F <: DFGFactor = f.data
5252
id(f::F) where F <: DFGFactor = f._internalId
53+
54+
# Simply for convenience - don't export
55+
const PackedFunctionNodeData{T} = GenericFunctionNodeData{T, <: AbstractString}
56+
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)
57+
const FunctionNodeData{T} = GenericFunctionNodeData{T, Symbol}
58+
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

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -62,3 +62,11 @@ function _copyIntoGraph!(sourceDFG::G, destDFG::H, variableFactorLabels::Vector{
6262
end
6363
return nothing
6464
end
65+
66+
## Utility functions for getting type names and modules (from IncrementalInference)
67+
function _getmodule(t::T) where T
68+
T.name.module
69+
end
70+
function _getname(t::T) where T
71+
T.name.name
72+
end

‎test/FileDFG.jl

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
using Revise
2+
using Test
3+
using DistributedFactorGraphs
4+
using IncrementalInference, RoME
5+
6+
# Make a simple graph
7+
dfg = GraphsDFG{SolverParams}(params=SolverParams())
8+
# Add the first pose :x0
9+
x0 = addVariable!(dfg, :x0, Pose2)
10+
# Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
11+
prior = addFactor!(dfg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) )
12+
# Drive around in a hexagon
13+
for i in 0:5
14+
psym = Symbol("x$i")
15+
nsym = Symbol("x$(i+1)")
16+
addVariable!(dfg, nsym, Pose2)
17+
pp = Pose2Pose2(MvNormal([10.0;0;pi/3], Matrix(Diagonal([0.1;0.1;0.1].^2))))
18+
addFactor!(dfg, [psym;nsym], pp )
19+
end
20+
21+
# Save it
22+
saveFolder = "/tmp/fileDFG"
23+
saveDFG(dfg, saveFolder)
24+
@test readdir("$saveFolder/variables") == ["x0.json", "x1.json", "x2.json", "x3.json", "x4.json", "x5.json", "x6.json"]
25+
@test readdir("$saveFolder/factors") == ["x0f1.json", "x0x1f1.json", "x1x2f1.json", "x2x3f1.json", "x3x4f1.json", "x4x5f1.json", "x5x6f1.json"]
26+
27+
retDFG = loadDFG(saveFolder, IncrementalInference)
28+
@test symdiff(ls(dfg), ls(dfg)) == []
29+
@test symdiff(lsf(dfg), lsf(retDFG)) == []
30+
31+
# Success!

0 commit comments

Comments
 (0)