Skip to content

Commit 985220e

Browse files
dehanndehann
committed
more code from IncrementalInference
1 parent 0df8b59 commit 985220e

File tree

2 files changed

+352
-0
lines changed

2 files changed

+352
-0
lines changed

‎src/DistributedFactorGraphs.jl

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ using
55
DocStringExtensions
66

77

8+
include("FactorGraphTypes.jl")
89
include("FGOSUtils.jl")
910

1011
end

‎src/FactorGraphTypes.jl

Lines changed: 351 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,351 @@
1+
2+
const FGG = Graphs.GenericIncidenceList{Graphs.ExVertex,Graphs.Edge{Graphs.ExVertex},Array{Graphs.ExVertex,1},Array{Array{Graphs.Edge{Graphs.ExVertex},1},1}}
3+
const FGGdict = Graphs.GenericIncidenceList{Graphs.ExVertex,Graphs.Edge{Graphs.ExVertex},Dict{Int,Graphs.ExVertex},Dict{Int,Array{Graphs.Edge{Graphs.ExVertex},1}}}
4+
5+
6+
7+
mutable struct FactorGraph
8+
g::FGGdict
9+
bn
10+
IDs::Dict{Symbol,Int}
11+
fIDs::Dict{Symbol,Int}
12+
id::Int
13+
nodeIDs::Array{Int,1} # TODO -- ordering seems improved to use adj permutation -- pending merge JuliaArchive/Graphs.jl/#225
14+
factorIDs::Array{Int,1}
15+
bnverts::Dict{Int,Graphs.ExVertex} # TODO -- not sure if this is still used, remove
16+
bnid::Int # TODO -- not sure if this is still used
17+
dimID::Int
18+
cg
19+
cgIDs::Dict{Int,Int} # cgIDs[exvid] = neoid
20+
sessionname::String
21+
robotname::String
22+
registeredModuleFunctions::VoidUnion{Dict{Symbol, Function}}
23+
reference::VoidUnion{Dict{Symbol, Tuple{Symbol, Vector{Float64}}}}
24+
stateless::Bool
25+
FactorGraph() = new()
26+
FactorGraph(
27+
x1,
28+
x2,
29+
x3,
30+
x4,
31+
x5,
32+
x6,
33+
x7,
34+
x8,
35+
x9,
36+
x10,
37+
x11,
38+
x12,
39+
x13,
40+
x14,
41+
x15,
42+
x16
43+
) = new(
44+
x1,
45+
x2,
46+
x3,
47+
x4,
48+
x5,
49+
x6,
50+
x7,
51+
x8,
52+
x9,
53+
x10,
54+
x11,
55+
x12,
56+
x13,
57+
x14,
58+
x15,
59+
x16,
60+
false )
61+
end
62+
63+
"""
64+
$(SIGNATURES)
65+
66+
Construct an empty FactorGraph object with the minimum amount of information / memory populated.
67+
"""
68+
function emptyFactorGraph(;reference::VoidUnion{Dict{Symbol, Tuple{Symbol, Vector{Float64}}}}=nothing)
69+
fg = FactorGraph(Graphs.incdict(Graphs.ExVertex,is_directed=false),
70+
Graphs.incdict(Graphs.ExVertex,is_directed=true),
71+
# Dict{Int,Graphs.ExVertex}(),
72+
# Dict{Int,Graphs.ExVertex}(),
73+
Dict{Symbol,Int}(),
74+
Dict{Symbol,Int}(),
75+
0,
76+
[],
77+
[],
78+
Dict{Int,Graphs.ExVertex}(),
79+
0,
80+
0,
81+
nothing,
82+
Dict{Int,Int}(),
83+
"",
84+
"",
85+
Dict{Symbol, Function}(:IncrementalInference=>IncrementalInference.getSample), # TODO likely to be removed
86+
reference ) #evalPotential
87+
return fg
88+
end
89+
90+
mutable struct VariableNodeData
91+
initval::Array{Float64,2}
92+
initstdev::Array{Float64,2}
93+
val::Array{Float64,2}
94+
bw::Array{Float64,2}
95+
BayesNetOutVertIDs::Array{Int,1}
96+
dimIDs::Array{Int,1}
97+
dims::Int
98+
eliminated::Bool
99+
BayesNetVertID::Int
100+
separator::Array{Int,1}
101+
groundtruth::VoidUnion{ Dict{ Tuple{Symbol, Vector{Float64}} } } # not packed yet
102+
softtype
103+
initialized::Bool
104+
VariableNodeData() = new()
105+
function VariableNodeData(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11)
106+
warn("Deprecated use of VariableNodeData(11 param), use 13 parameters instead")
107+
new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11, nothing, true) # TODO ensure this is initialized true is working for most cases
108+
end
109+
VariableNodeData(x1::Array{Float64,2},
110+
x2::Array{Float64,2},
111+
x3::Array{Float64,2},
112+
x4::Array{Float64,2},
113+
x5::Vector{Int},
114+
x6::Vector{Int},
115+
x7::Int,
116+
x8::Bool,
117+
x9::Int,
118+
x10::Vector{Int},
119+
x11::VoidUnion{ Dict{ Tuple{Symbol, Vector{Float64}} } },
120+
x12,
121+
x13::Bool) =
122+
new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13)
123+
end
124+
125+
mutable struct PackedVariableNodeData
126+
vecinitval::Array{Float64,1}
127+
diminitval::Int
128+
vecinitstdev::Array{Float64,1}
129+
diminitdev::Int
130+
vecval::Array{Float64,1}
131+
dimval::Int
132+
vecbw::Array{Float64,1}
133+
dimbw::Int
134+
BayesNetOutVertIDs::Array{Int,1}
135+
dimIDs::Array{Int,1}
136+
dims::Int
137+
eliminated::Bool
138+
BayesNetVertID::Int
139+
separator::Array{Int,1}
140+
# groundtruth::VoidUnion{ Dict{ Tuple{Symbol, Vector{Float64}} } }
141+
softtype::String
142+
initialized::Bool
143+
PackedVariableNodeData() = new()
144+
PackedVariableNodeData(x1::Vector{Float64},
145+
x2::Int,
146+
x3::Vector{Float64},
147+
x4::Int,
148+
x5::Vector{Float64},
149+
x6::Int,
150+
x7::Vector{Float64},
151+
x8::Int,
152+
x9::Vector{Int},
153+
x10::Vector{Int},
154+
x11::Int,
155+
x12::Bool,
156+
x13::Int,
157+
x14::Vector{Int},
158+
x15::String,
159+
x16::Bool) = new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16)
160+
end
161+
162+
163+
164+
mutable struct GenericFunctionNodeData{T, S}
165+
fncargvID::Array{Int,1}
166+
eliminated::Bool
167+
potentialused::Bool
168+
edgeIDs::Array{Int,1}
169+
frommodule::S #Union{Symbol, AbstractString}
170+
fnc::T
171+
GenericFunctionNodeData{T, S}() where {T, S} = new()
172+
GenericFunctionNodeData{T, S}(x1, x2, x3, x4, x5, x6) where {T, S} = new(x1, x2, x3, x4, x5, x6)
173+
end
174+
175+
FunctionNodeData{T <: Union{InferenceType, FunctorInferenceType}} = GenericFunctionNodeData{T, Symbol}
176+
FunctionNodeData() = GenericFunctionNodeData{T, Symbol}()
177+
FunctionNodeData(x1, x2, x3, x4, x5, x6) = GenericFunctionNodeData{T, Symbol}(x1, x2, x3, x4, x5, x6)
178+
179+
# typealias PackedFunctionNodeData{T <: PackedInferenceType} GenericFunctionNodeData{T, AbstractString}
180+
PackedFunctionNodeData{T <: PackedInferenceType} = GenericFunctionNodeData{T, AbstractString}
181+
PackedFunctionNodeData() = GenericFunctionNodeData{T, AbstractString}()
182+
PackedFunctionNodeData(x1, x2, x3, x4, x5, x6) = GenericFunctionNodeData{T, AbstractString}(x1, x2, x3, x4, x5, x6)
183+
184+
185+
function convert(::Type{PackedVariableNodeData}, d::VariableNodeData)
186+
return PackedVariableNodeData(d.initval[:],size(d.initval,1),
187+
d.initstdev[:],size(d.initstdev,1),
188+
d.val[:],size(d.val,1),
189+
d.bw[:], size(d.bw,1),
190+
d.BayesNetOutVertIDs,
191+
d.dimIDs, d.dims, d.eliminated,
192+
d.BayesNetVertID, d.separator,
193+
string(d.softtype), d.initialized)
194+
end
195+
function convert(::Type{VariableNodeData}, d::PackedVariableNodeData)
196+
197+
r1 = d.diminitval
198+
c1 = r1 > 0 ? floor(Int,length(d.vecinitval)/r1) : 0
199+
M1 = reshape(d.vecinitval,r1,c1)
200+
201+
r2 = d.diminitdev
202+
c2 = r2 > 0 ? floor(Int,length(d.vecinitstdev)/r2) : 0
203+
M2 = reshape(d.vecinitstdev,r2,c2)
204+
205+
r3 = d.dimval
206+
c3 = r3 > 0 ? floor(Int,length(d.vecval)/r3) : 0
207+
M3 = reshape(d.vecval,r3,c3)
208+
209+
r4 = d.dimbw
210+
c4 = r4 > 0 ? floor(Int,length(d.vecbw)/r4) : 0
211+
M4 = reshape(d.vecbw,r4,c4)
212+
213+
# TODO -- allow out of module type allocation (future feature, not currently in use)
214+
st = IncrementalInference.ContinuousMultivariate # eval(parse(d.softtype))
215+
216+
return VariableNodeData(M1,M2,M3,M4, d.BayesNetOutVertIDs,
217+
d.dimIDs, d.dims, d.eliminated, d.BayesNetVertID, d.separator,
218+
nothing, st, d.initialized )
219+
end
220+
function VNDencoder(P::Type{PackedVariableNodeData}, d::VariableNodeData)
221+
return convert(P, d) #PackedVariableNodeData
222+
end
223+
function VNDdecoder(T::Type{VariableNodeData}, d::PackedVariableNodeData)
224+
return convert(T, d) #VariableNodeData
225+
end
226+
227+
228+
function compare(a::VariableNodeData,b::VariableNodeData)
229+
TP = true
230+
TP = TP && a.initval == b.initval
231+
TP = TP && a.initstdev == b.initstdev
232+
TP = TP && a.val == b.val
233+
TP = TP && a.bw == b.bw
234+
TP = TP && a.BayesNetOutVertIDs == b.BayesNetOutVertIDs
235+
TP = TP && a.dimIDs == b.dimIDs
236+
TP = TP && a.dims == b.dims
237+
TP = TP && a.eliminated == b.eliminated
238+
TP = TP && a.BayesNetVertID == b.BayesNetVertID
239+
TP = TP && a.separator == b.separator
240+
return TP
241+
end
242+
243+
function ==(a::VariableNodeData,b::VariableNodeData, nt::Symbol=:var)
244+
return IncrementalInference.compare(a,b)
245+
end
246+
247+
248+
# heavy use of multiple dispatch for converting between packed and original data types during DB usage
249+
function convert{T <: InferenceType, P <: PackedInferenceType}(::Type{FunctionNodeData{T}}, d::PackedFunctionNodeData{P})
250+
return FunctionNodeData{T}(d.fncargvID, d.eliminated, d.potentialused, d.edgeIDs,
251+
Symbol(d.frommodule), convert(T, d.fnc))
252+
end
253+
function convert{P <: PackedInferenceType, T <: InferenceType}(::Type{PackedFunctionNodeData{P}}, d::FunctionNodeData{T})
254+
return PackedFunctionNodeData{P}(d.fncargvID, d.eliminated, d.potentialused, d.edgeIDs,
255+
string(d.frommodule), convert(P, d.fnc))
256+
end
257+
258+
259+
# Functor version -- TODO, abstraction can be improved here
260+
function convert{T <: FunctorInferenceType, P <: PackedInferenceType}(::Type{FunctionNodeData{GenericWrapParam{T}}}, d::PackedFunctionNodeData{P})
261+
usrfnc = convert(T, d.fnc)
262+
gwpf = prepgenericwrapper(Graphs.ExVertex[], usrfnc, getSample)
263+
return FunctionNodeData{GenericWrapParam{T}}(d.fncargvID, d.eliminated, d.potentialused, d.edgeIDs,
264+
Symbol(d.frommodule), gwpf) #{T}
265+
end
266+
function convert{P <: PackedInferenceType, T <: FunctorInferenceType}(::Type{PackedFunctionNodeData{P}}, d::FunctionNodeData{T})
267+
return PackedFunctionNodeData{P}(d.fncargvID, d.eliminated, d.potentialused, d.edgeIDs,
268+
string(d.frommodule), convert(P, d.fnc.usrfnc!))
269+
end
270+
271+
function FNDencode{T <: FunctorInferenceType, P <: PackedInferenceType}(::Type{PackedFunctionNodeData{P}}, d::FunctionNodeData{T})
272+
return convert(PackedFunctionNodeData{P}, d) #PackedFunctionNodeData{P}
273+
end
274+
function FNDdecode{T <: FunctorInferenceType, P <: PackedInferenceType}(::Type{FunctionNodeData{T}}, d::PackedFunctionNodeData{P})
275+
return convert(FunctionNodeData{T}, d) #FunctionNodeData{T}
276+
end
277+
278+
function FNDencode{T <: InferenceType, P <: PackedInferenceType}(::Type{PackedFunctionNodeData{P}}, d::FunctionNodeData{T})
279+
return convert(PackedFunctionNodeData{P}, d) #PackedFunctionNodeData{P}
280+
end
281+
function FNDdecode{T <: InferenceType, P <: PackedInferenceType}(::Type{FunctionNodeData{T}}, d::PackedFunctionNodeData{P})
282+
return convert(FunctionNodeData{T}, d) #FunctionNodeData{T}
283+
end
284+
285+
286+
# Compare FunctionNodeData
287+
function compare{T,S}(a::GenericFunctionNodeData{T,S},b::GenericFunctionNodeData{T,S})
288+
# TODO -- beef up this comparison to include the gwp
289+
TP = true
290+
TP = TP && a.fncargvID == b.fncargvID
291+
TP = TP && a.eliminated == b.eliminated
292+
TP = TP && a.potentialused == b.potentialused
293+
TP = TP && a.edgeIDs == b.edgeIDs
294+
TP = TP && a.frommodule == b.frommodule
295+
TP = TP && typeof(a.fnc) == typeof(b.fnc)
296+
return TP
297+
end
298+
299+
300+
function addGraphsVert!(fgl::FactorGraph,
301+
exvert::Graphs.ExVertex;
302+
labels::Vector{<:AbstractString}=String[])
303+
#
304+
Graphs.add_vertex!(fgl.g, exvert)
305+
end
306+
307+
function getVertNode(fgl::FactorGraph, id::Int; nt::Symbol=:var, bigData::Bool=false)
308+
return fgl.g.vertices[id] # check equivalence between fgl.v/f[i] and fgl.g.vertices[i]
309+
# return nt == :var ? fgl.v[id] : fgl.f[id]
310+
end
311+
function getVertNode(fgl::FactorGraph, lbl::Symbol; nt::Symbol=:var, bigData::Bool=false)
312+
return getVertNode(fgl, (nt == :var ? fgl.IDs[lbl] : fgl.fIDs[lbl]), nt=nt, bigData=bigData)
313+
end
314+
getVertNode{T <: AbstractString}(fgl::FactorGraph, lbl::T; nt::Symbol=:var, bigData::Bool=false) = getVertNode(fgl, Symbol(lbl), nt=nt, bigData=bigData)
315+
316+
317+
318+
# excessive function, needs refactoring
319+
function updateFullVertData!(fgl::FactorGraph,
320+
nv::Graphs.ExVertex;
321+
updateMAPest::Bool=false )
322+
#
323+
324+
# not required, since we using reference -- placeholder function CloudGraphs interface
325+
# getVertNode(fgl, nv.index).attributes["data"] = nv.attributes["data"]
326+
nothing
327+
end
328+
329+
330+
function makeAddEdge!(fgl::FactorGraph, v1::Graphs.ExVertex, v2::Graphs.ExVertex; saveedgeID::Bool=true)
331+
edge = Graphs.make_edge(fgl.g, v1, v2)
332+
Graphs.add_edge!(fgl.g, edge)
333+
if saveedgeID push!(getData(v2).edgeIDs,edge.index) end #.attributes["data"]
334+
edge
335+
end
336+
337+
function graphsOutNeighbors(fgl::FactorGraph, vert::Graphs.ExVertex; ready::Int=1,backendset::Int=1, needdata::Bool=false)
338+
Graphs.out_neighbors(vert, fgl.g)
339+
end
340+
function graphsOutNeighbors(fgl::FactorGraph, exVertId::Int; ready::Int=1,backendset::Int=1, needdata::Bool=false)
341+
graphsOutNeighbors(fgl.g, getVert(fgl,exVertId), ready=ready, backendset=backendset, needdata=needdata)
342+
end
343+
344+
function graphsGetEdge(fgl::FactorGraph, id::Int)
345+
nothing
346+
end
347+
348+
function graphsDeleteVertex!(fgl::FactorGraph, vert::Graphs.ExVertex)
349+
warn("graphsDeleteVertex! -- not deleting Graphs.jl vertex id=$(vert.index)")
350+
nothing
351+
end

0 commit comments

Comments
 (0)