Skip to content

Commit 5ba94d2

Browse files
GearsADGearsAD
authored
Merge pull request #154 from JuliaRobotics/feature/152_genpackingunpacking
General variable+factor packing and unpacking functions for DFG
2 parents bca01e6 + 1b13334 commit 5ba94d2

5 files changed

Lines changed: 181 additions & 111 deletions

File tree

‎docs/src/variable_factor_serialization.md‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
# Serialization of Variables and Factors
2+
3+
If you are transferring variables and factors over a wire you need to serialize
4+
and deserialize variables and factors.
5+
6+
## Packing and Unpacking
7+
8+
Packing is done with the exposed functions `packVariable()::Dict{String, Any}` and
9+
`packFactor()::Dict{String, Any}`. You can then serialize this into a string or JSON
10+
as you would normally.
11+
12+
> Note: When you deserialize a factor and want to use it for solving, you must call IncrementalInference.rebuildFactorMetadata!(dfgLoadInto, factor) to reinflate it completely. Please review [FileDFG service](src/FileDFG/services/FileDFG.jl) for an example.
13+
14+
For example:
15+
```julia
16+
using DistributedFactorGraphs
17+
using IncrementalInference, RoME
18+
19+
# Make a variable and a factor:
20+
# Make a simple graph
21+
dfg = GraphsDFG{SolverParams}(params=SolverParams())
22+
# Add the first pose :x0
23+
x0 = addVariable!(dfg, :x0, Pose2)
24+
# Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
25+
prior = addFactor!(dfg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) )
26+
27+
# Now serialize them:
28+
pVariable = packVariable(dfg, x0)
29+
pFactor = packFactor(dfg, prior)
30+
31+
# And we can deserialize them
32+
upVariable = unpackVariable(dfg, pVariable)
33+
# FYI: The graph is used in unpackFactor to find the variables that the factor links to.
34+
upFactor = unpackFactor(dfg, pFactor, IncrementalInference)
35+
# Note, you need to call IncrementalInference.rebuildFactorMetadata!(dfgLoadInto, factor)
36+
# to make it useable. Please add an issue if this poses a problem or causes issues.
37+
```
38+
39+
As a more complex example, we can use JSON2 to stringify the data and write it to a folder of files as FileDFG does:
40+
41+
```julia
42+
using DistributedFactorGraphs
43+
using IncrementalInference, RoME
44+
45+
# Make a variable and a factor:
46+
# Make a simple graph
47+
dfg = GraphsDFG{SolverParams}(params=SolverParams())
48+
# Add the first pose :x0
49+
x0 = addVariable!(dfg, :x0, Pose2)
50+
# Add at a fixed location PriorPose2 to pin :x0 to a starting location (10,10, pi/4)
51+
prior = addFactor!(dfg, [:x0], PriorPose2( MvNormal([10; 10; 1.0/8.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) )
52+
53+
# Slightly fancier example: We can use JSON2, we can serialize to a string
54+
varFolder = "/tmp"
55+
for v in getVariables(dfg)
56+
vPacked = packVariable(dfg, v)
57+
io = open("$varFolder/$(v.label).json", "w")
58+
JSON2.write(io, vPacked)
59+
close(io)
60+
end
61+
# Factors
62+
for f in getFactors(dfg)
63+
fPacked = packFactor(dfg, f)
64+
io = open("$folder/factors/$(f.label).json", "w")
65+
JSON2.write(io, fPacked)
66+
close(io)
67+
end
68+
```

‎src/DistributedFactorGraphs.jl‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,9 @@ export VariableNodeData, PackedVariableNodeData, VariableEstimate
3333
export GenericFunctionNodeData#, FunctionNodeData
3434
export getSerializationModule, setSerializationModule!
3535
export pack, unpack
36+
# Resolve with above
37+
export packVariable, unpackVariable, packFactor, unpackFactor
38+
3639

3740
#Interfaces
3841
export getAdjacencyMatrixSparse

‎src/FileDFG/services/FileDFG.jl‎

Lines changed: 4 additions & 111 deletions
Original file line numberDiff line numberDiff line change
@@ -1,111 +1,4 @@
11

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, 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-
try
47-
packtype = getfield(_getmodule(fnctype), Symbol("Packed$(_getname(fnctype))"))
48-
packed = convert(PackedFunctionNodeData{packtype}, f.data)
49-
props["data"] = JSON2.write(packed)
50-
catch ex
51-
io = IOBuffer()
52-
showerror(io, ex, catch_backtrace())
53-
err = String(take!(io))
54-
msg = "Error while packing '$(f.label)' as '$fnctype', please check the unpacking/packing converters for this factor - \r\n$err"
55-
error(msg)
56-
end
57-
# Include the type
58-
props["fnctype"] = String(_getname(fnctype))
59-
props["_variableOrderSymbols"] = JSON2.write(f._variableOrderSymbols)
60-
props["backendset"] = f.backendset
61-
props["ready"] = f.ready
62-
63-
return props
64-
end
65-
66-
67-
function _unpackFactor(dfg::G, packedProps::Dict{String, Any}, iifModule)::DFGFactor where G <: AbstractDFG
68-
label = packedProps["label"]
69-
tags = JSON2.read(packedProps["tags"], Vector{Symbol})
70-
71-
data = packedProps["data"]
72-
@debug "Decoding $label..."
73-
datatype = packedProps["fnctype"]
74-
packtype = getfield(Main, Symbol("Packed"*datatype))
75-
packed = nothing
76-
fullFactor = nothing
77-
try
78-
packed = JSON2.read(data, GenericFunctionNodeData{packtype,String})
79-
fullFactor = iifModule.decodePackedType(dfg, packed)
80-
catch ex
81-
io = IOBuffer()
82-
showerror(io, ex, catch_backtrace())
83-
err = String(take!(io))
84-
msg = "Error while unpacking '$label' as '$datatype', please check the unpacking/packing converters for this factor - \r\n$err"
85-
error(msg)
86-
end
87-
88-
# Include the type
89-
_variableOrderSymbols = JSON2.read(packedProps["_variableOrderSymbols"], Vector{Symbol})
90-
backendset = packedProps["backendset"]
91-
ready = packedProps["ready"]
92-
93-
# Rebuild DFGVariable
94-
factor = DFGFactor{typeof(fullFactor.fnc), Symbol}(Symbol(label))
95-
factor.tags = tags
96-
factor.data = fullFactor
97-
factor._variableOrderSymbols = _variableOrderSymbols
98-
factor.ready = ready
99-
factor.backendset = backendset
100-
101-
# GUARANTEED never to bite us in the ass in the future...
102-
# ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
103-
factor.data.fncargvID = deepcopy(_variableOrderSymbols)
104-
105-
# Note, once inserted, you still need to call IIF.rebuildFactorMetadata!
106-
return factor
107-
end
108-
1092
function saveDFG(dfg::G, folder::String) where G <: AbstractDFG
1103
variables = getVariables(dfg)
1114
factors = getFactors(dfg)
@@ -123,14 +16,14 @@ function saveDFG(dfg::G, folder::String) where G <: AbstractDFG
12316
map(f -> rm("$factorFolder/$f"), readdir(factorFolder))
12417
# Variables
12518
for v in variables
126-
vPacked = _packVariable(dfg, v)
19+
vPacked = packVariable(dfg, v)
12720
io = open("$varFolder/$(v.label).json", "w")
12821
JSON2.write(io, vPacked)
12922
close(io)
13023
end
13124
# Factors
13225
for f in factors
133-
fPacked = _packFactor(dfg, f)
26+
fPacked = packFactor(dfg, f)
13427
io = open("$folder/factors/$(f.label).json", "w")
13528
JSON2.write(io, fPacked)
13629
close(io)
@@ -152,7 +45,7 @@ function loadDFG(folder::String, iifModule, dfgLoadInto::G=GraphsDFG{NoSolverPar
15245
for varFile in varFiles
15346
io = open("$varFolder/$varFile")
15447
packedData = JSON2.read(io, Dict{String, Any})
155-
push!(variables, _unpackVariable(dfgLoadInto, packedData))
48+
push!(variables, unpackVariable(dfgLoadInto, packedData))
15649
end
15750
@info "Loaded $(length(variables)) variables - $(map(v->v.label, variables))"
15851
@info "Inserting variables into graph..."
@@ -162,7 +55,7 @@ function loadDFG(folder::String, iifModule, dfgLoadInto::G=GraphsDFG{NoSolverPar
16255
for factorFile in factorFiles
16356
io = open("$factorFolder/$factorFile")
16457
packedData = JSON2.read(io, Dict{String, Any})
165-
push!(factors, _unpackFactor(dfgLoadInto, packedData, iifModule))
58+
push!(factors, unpackFactor(dfgLoadInto, packedData, iifModule))
16659
end
16760
@info "Loaded $(length(variables)) factors - $(map(f->f.label, factors))"
16861
@info "Inserting factors into graph..."

‎src/services/DFGFactor.jl‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,3 +3,72 @@ import Base: convert
33
function convert(::Type{DFGFactorSummary}, f::DFGFactor)
44
return DFGFactorSummary(f.label, deepcopy(f.tags), f._internalId, deepcopy(f._variableOrderSymbols))
55
end
6+
7+
function packFactor(dfg::G, f::DFGFactor)::Dict{String, Any} where G <: AbstractDFG
8+
# Construct the properties to save
9+
props = Dict{String, Any}()
10+
props["label"] = string(f.label)
11+
props["tags"] = JSON2.write(f.tags)
12+
# Pack the node data
13+
fnctype = f.data.fnc.usrfnc!
14+
try
15+
packtype = getfield(_getmodule(fnctype), Symbol("Packed$(_getname(fnctype))"))
16+
packed = convert(PackedFunctionNodeData{packtype}, f.data)
17+
props["data"] = JSON2.write(packed)
18+
catch ex
19+
io = IOBuffer()
20+
showerror(io, ex, catch_backtrace())
21+
err = String(take!(io))
22+
msg = "Error while packing '$(f.label)' as '$fnctype', please check the unpacking/packing converters for this factor - \r\n$err"
23+
error(msg)
24+
end
25+
# Include the type
26+
props["fnctype"] = String(_getname(fnctype))
27+
props["_variableOrderSymbols"] = JSON2.write(f._variableOrderSymbols)
28+
props["backendset"] = f.backendset
29+
props["ready"] = f.ready
30+
31+
return props
32+
end
33+
34+
function unpackFactor(dfg::G, packedProps::Dict{String, Any}, iifModule)::DFGFactor where G <: AbstractDFG
35+
label = packedProps["label"]
36+
tags = JSON2.read(packedProps["tags"], Vector{Symbol})
37+
38+
data = packedProps["data"]
39+
@debug "Decoding $label..."
40+
datatype = packedProps["fnctype"]
41+
packtype = getfield(Main, Symbol("Packed"*datatype))
42+
packed = nothing
43+
fullFactor = nothing
44+
try
45+
packed = JSON2.read(data, GenericFunctionNodeData{packtype,String})
46+
fullFactor = iifModule.decodePackedType(dfg, packed)
47+
catch ex
48+
io = IOBuffer()
49+
showerror(io, ex, catch_backtrace())
50+
err = String(take!(io))
51+
msg = "Error while unpacking '$label' as '$datatype', please check the unpacking/packing converters for this factor - \r\n$err"
52+
error(msg)
53+
end
54+
55+
# Include the type
56+
_variableOrderSymbols = JSON2.read(packedProps["_variableOrderSymbols"], Vector{Symbol})
57+
backendset = packedProps["backendset"]
58+
ready = packedProps["ready"]
59+
60+
# Rebuild DFGVariable
61+
factor = DFGFactor{typeof(fullFactor.fnc), Symbol}(Symbol(label))
62+
factor.tags = tags
63+
factor.data = fullFactor
64+
factor._variableOrderSymbols = _variableOrderSymbols
65+
factor.ready = ready
66+
factor.backendset = backendset
67+
68+
# GUARANTEED never to bite us in the ass in the future...
69+
# ... TODO: refactor if changed: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/350
70+
factor.data.fncargvID = deepcopy(_variableOrderSymbols)
71+
72+
# Note, once inserted, you still need to call IIF.rebuildFactorMetadata!
73+
return factor
74+
end

‎src/services/DFGVariable.jl‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,42 @@
11
import Base: ==, convert
22

3+
function packVariable(dfg::G, v::DFGVariable)::Dict{String, Any} where G <: AbstractDFG
4+
props = Dict{String, Any}()
5+
props["label"] = string(v.label)
6+
props["timestamp"] = string(v.timestamp)
7+
props["tags"] = JSON2.write(v.tags)
8+
props["estimateDict"] = JSON2.write(v.estimateDict)
9+
props["solverDataDict"] = JSON2.write(Dict(keys(v.solverDataDict) .=> map(vnd -> pack(dfg, vnd), values(v.solverDataDict))))
10+
props["smallData"] = JSON2.write(v.smallData)
11+
props["ready"] = v.ready
12+
props["backendset"] = v.backendset
13+
return props
14+
end
15+
16+
function unpackVariable(dfg::G, packedProps::Dict{String, Any})::DFGVariable where G <: AbstractDFG
17+
label = Symbol(packedProps["label"])
18+
timestamp = DateTime(packedProps["timestamp"])
19+
tags = JSON2.read(packedProps["tags"], Vector{Symbol})
20+
estimateDict = JSON2.read(packedProps["estimateDict"], Dict{Symbol, Dict{Symbol, VariableEstimate}})
21+
smallData = nothing
22+
smallData = JSON2.read(packedProps["smallData"], Dict{String, String})
23+
24+
packed = JSON2.read(packedProps["solverDataDict"], Dict{String, PackedVariableNodeData})
25+
solverData = Dict(Symbol.(keys(packed)) .=> map(p -> unpack(dfg, p), values(packed)))
26+
27+
# Rebuild DFGVariable
28+
variable = DFGVariable(Symbol(packedProps["label"]))
29+
variable.timestamp = timestamp
30+
variable.tags = tags
31+
variable.estimateDict = estimateDict
32+
variable.solverDataDict = solverData
33+
variable.smallData = smallData
34+
variable.ready = packedProps["ready"]
35+
variable.backendset = packedProps["backendset"]
36+
37+
return variable
38+
end
39+
340
function pack(dfg::G, d::VariableNodeData)::PackedVariableNodeData where G <: AbstractDFG
441
@debug "Dispatching conversion variable -> packed variable for type $(string(d.softtype))"
542
return PackedVariableNodeData(d.val[:],size(d.val,1),

0 commit comments

Comments
 (0)