-
Notifications
You must be signed in to change notification settings - Fork 21
/
DispatchPackedConversions.jl
120 lines (87 loc) · 4.65 KB
/
DispatchPackedConversions.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
struct PackedManifoldKernelDensity <: PackedSamplableBelief
json::String
end
Base.convert(::Type{<:SamplableBelief}, ::Type{<:PackedManifoldKernelDensity}) = ManifoldKernelDensity
Base.convert(::Type{<:PackedSamplableBelief}, ::Type{<:ManifoldKernelDensity}) = PackedManifoldKernelDensity
Base.convert(::Type{<:PackedSamplableBelief}, mkd::ManifoldKernelDensity) = convert(String, mkd)
Base.convert(::Type{<:SamplableBelief}, mkd::PackedManifoldKernelDensity) = convert(ManifoldKernelDensity, mkd.json)
function packmultihypo(fnc::CommonConvWrapper{T}) where {T<:AbstractFactor}
@warn "packmultihypo is deprecated in favor of Vector only operations"
fnc.hypotheses !== nothing ? string(fnc.hypotheses) : ""
end
function parsemultihypostr(str::AS) where {AS <: AbstractString}
@warn "parsemultihypostr is deprecated in favor of Vector only operations"
mhcat=nothing
if length(str) > 0
mhcat = convert(SamplableBelief, str)
end
return mhcat
end
## packing converters-----------------------------------------------------------
# heavy use of multiple dispatch for converting between packed and original data types during DB usage
function convert(::Type{PackedFunctionNodeData{P}}, d::FunctionNodeData{T}) where {P <: PackedInferenceType, T <: FactorOperationalMemory}
return PackedFunctionNodeData(d.eliminated, d.potentialused, d.edgeIDs,
convert(P, _getCCW(d).usrfnc!),
d.multihypo, _getCCW(d).certainhypo, d.nullhypo,
d.solveInProgress, d.inflation) # extract two values from ccw for storage -- ccw thrown away
end
## unpack converters------------------------------------------------------------
function convert(
::Type{GenericFunctionNodeData{CommonConvWrapper{F}}},
packed::GenericFunctionNodeData{P} ) where {F <: AbstractFactor, P <: PackedInferenceType}
#
# TODO store threadmodel=MutliThreaded,SingleThreaded in persistence layer
usrfnc = convert(F, packed.fnc)
mhcat, nh = parseusermultihypo(packed.multihypo, packed.nullhypo)
# TODO -- improve prepgenericconvolution for hypotheses and certainhypo field recovery when deserializing
# reconstitute from stored data
# FIXME, add threadmodel=threadmodel
# FIXME https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-776838053
ccw = prepgenericconvolution(DFG.DFGVariable[], usrfnc, multihypo=mhcat, nullhypo=nh, inflation=packed.inflation)
ccw.certainhypo = packed.certainhypo
ret = FunctionNodeData{CommonConvWrapper{typeof(usrfnc)}}(packed.eliminated, packed.potentialused, packed.edgeIDs, ccw,
packed.multihypo, packed.certainhypo, packed.nullhypo,
packed.solveInProgress, packed.inflation )
#
return ret
end
## Variables
"""
$(SIGNATURES)
After deserializing a factor using decodePackedType, use this to
completely rebuild the factor's CCW and user data.
"""
function rebuildFactorMetadata!(dfg::AbstractDFG{SolverParams}, factor::DFGFactor)
# Set up the neighbor data
neighbors = map(vId->getVariable(dfg, vId), getNeighbors(dfg, factor))
neighborUserData = map(v->getVariableType(v), neighbors)
# Rebuilding the CCW
fsd = getSolverData(factor)
ccw_new = getDefaultFactorData( dfg, neighbors, getFactorType(factor),
multihypo=fsd.multihypo,
nullhypo=fsd.nullhypo,
# special inflation override
inflation=fsd.inflation,
eliminated=fsd.eliminated,
potentialused=fsd.potentialused,
edgeIDs=fsd.edgeIDs,
solveInProgress=fsd.solveInProgress)
setSolverData!(factor, ccw_new)
#... Copying neighbor data into the factor?
# JT TODO it looks like this is already updated in getDefaultFactorData -> prepgenericconvolution
# factormetadata.variableuserdata is deprecated, remove when removing deprecation
# for i in 1:Threads.nthreads()
# ccw_new.fnc.cpt[i].factormetadata.variableuserdata = deepcopy(neighborUserData)
# end
return factor
end
# import IncrementalInference: decodefg, loadjld
veeCategorical(val::Categorical) = val.p
veeCategorical(val::Union{Nothing, Vector{Float64}}) = val
function convert( ::Type{Tuple{ManifoldKernelDensity,Float64}},
p::TreeBelief )
# @show size(p.val), size(p.bw), p.manifolds
# (AMP.manikde!(p.val, p.bw[:,1], p.manifolds), p.inferdim)
(convert(ManifoldKernelDensity, p), p.inferdim)
end
#