Merge pull request #1318 from JuliaRobotics/21Q3/fix/serializemkd

move MKD serialization downstream to IIF

Project.toml

@@ -43,7 +43,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.4.7"
+ApproxManifoldProducts = "0.4.8"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"

src/DispatchPackedConversions.jl

@@ -1,14 +1,4 @@
 
-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"

src/IncrementalInference.jl

@@ -427,6 +427,7 @@ include("CliqueTypes.jl")
 include("JunctionTreeTypes.jl")
 include("FactorGraph.jl")
 include("SerializingDistributions.jl")
+include("SerializationMKD.jl")
 include("DispatchPackedConversions.jl")
 
 include("Variables/DefaultVariables.jl")

src/SerializationMKD.jl

@@ -0,0 +1,76 @@
+
+## ================================================================================================================================
+# Serialization
+## ================================================================================================================================
+
+# abstract type JSONManifoldKernelDensity end
+
+import DistributedFactorGraphs: getVariableType
+
+export PackedManifoldKernelDensity
+
+# NOTE, user variables and manifolds will require the same definitions, TODO perhaps add into `@defVariable`
+# unusual definitions, but all they need to do is pack and unpack as one-to-one
+# this step actually occurs separate from the actual variables or factors (with their own manifolds) 
+# relies on later use of getManifold to give back the same <:AbstractManifold
+getVariableType(M::Euclidean{Tuple{N}}) where N = ContinuousEuclid(N)
+getVariableType(M::TranslationGroup{Tuple{N}}) where N = ContinuousEuclid(N)
+getVariableType(M::Circle) = Circular()
+
+
+
+##
+
+struct PackedManifoldKernelDensity <: PackedSamplableBelief
+  _type::String
+  varType::String
+  pts::Vector{Vector{Float64}}
+  bw::Vector{Float64}
+  partial::Vector{Int}
+  infoPerCoord::Vector{Float64}
+end
+
+# Type converters for MKD
+Base.convert(::Type{<:SamplableBelief}, ::Type{<:PackedManifoldKernelDensity}) = ManifoldKernelDensity
+Base.convert(::Type{<:PackedSamplableBelief}, ::Type{<:ManifoldKernelDensity}) = PackedManifoldKernelDensity
+
+# Data converters for MKD
+function Base.convert(::Type{<:AbstractString}, 
+                      mkd::ManifoldKernelDensity )
+  #
+  pts = getPoints(mkd)
+
+  packedMKD = PackedManifoldKernelDensity(
+    "PackedManifoldKernelDensity",
+    # piggy back on InferenceVariable serialization rather than try serialize anything Manifolds.jl
+    DFG.typeModuleName(getVariableType(mkd.manifold)),
+    [AMP.makeCoordsFromPoint(mkd.manifold, pt) for pt in pts],
+    getBW(mkd.belief)[:,1],
+    mkd._partial isa Nothing ? collect(1:manifold_dimension(mkd.manifold)) : mkd._partial ,
+    mkd.infoPerCoord
+  )
+
+  JSON2.write(packedMKD)
+end
+
+Base.convert(::Type{<:PackedSamplableBelief}, mkd::ManifoldKernelDensity) = convert(String, mkd)
+
+
+# make module specific
+# good references: 
+#  https://discourse.julialang.org/t/converting-string-to-datatype-with-meta-parse/33024/2
+#  https://discourse.julialang.org/t/is-there-a-way-to-import-modules-with-a-string/15723/6
+function Base.convert(::Type{<:ManifoldKernelDensity}, str::AbstractString)
+  data = JSON2.read(str, PackedManifoldKernelDensity)
+
+  # piggy back on serialization of InferenceVariable rather than try serialize anything Manifolds.jl
+  M = DFG.getTypeFromSerializationModule(data.varType) |> getManifold
+  vecP = [AMP.makePointFromCoords(M, pt) for pt in data.pts]
+  partial = length(data.partial) == manifold_dimension(M) ? nothing : data.partial
+
+  return manikde!(M, vecP, bw=data.bw, partial=partial, infoPerCoord=data.infoPerCoord)
+end
+
+
+
+#

test/testMixtureLinearConditional.jl

@@ -93,6 +93,8 @@ fg_ = initfg();
 addVariable!(fg_, :x0, ContinuousScalar)
 addVariable!(fg_, :x1, ContinuousScalar)
 
+##
+
 f0_ = DFG.unpackFactor(fg_, pf0)
 f1_ = DFG.unpackFactor(fg_, pf1)
 
@@ -104,8 +106,12 @@ f1_ = DFG.unpackFactor(fg_, pf1)
 
 @test IIF._getCCW(f1).usrfnc!.components.naive == IIF._getCCW(f1).usrfnc!.components.naive
 
-A = ManifoldBelief(Euclid, IIF._getCCW(f1).usrfnc!.components.fancy )
-B = ManifoldBelief(Euclid, IIF._getCCW(f1_).usrfnc!.components.fancy )
+# already ManifoldKernelDensity
+A = IIF._getCCW(f1).usrfnc!.components.fancy
+B = IIF._getCCW(f1_).usrfnc!.components.fancy
+
+# A = ManifoldBelief(Euclid, IIF._getCCW(f1).usrfnc!.components.fancy )
+# B = ManifoldBelief(Euclid, IIF._getCCW(f1_).usrfnc!.components.fancy )
 
 @test mmd(A,B) < 1e-6
 

test/testgraphpackingconverters.jl

@@ -93,6 +93,26 @@ unpckd = unpackVariableNodeData(dfg, pd)
 end
 
 
+@testset "test serialization of ManifoldKernelDensity" begin
+##
+
+# create a basic manifoldkerneldensity
+mkd = manikde!(TranslationGroup(2), [randn(2) for _ in 1:100])
+
+# convert up and down
+st = convert(PackedSamplableBelief, mkd)
+upk = convert(SamplableBelief, st)
+
+# and check the basics
+@test isapprox( getPoints(mkd)[1], getPoints(upk)[1])
+@test isapprox( getPoints(mkd)[end], getPoints(upk)[end])
+
+@test mkd.manifold == upk.manifold
+@test mkd._partial == upk._partial
+@test mkd.infoPerCoord == upk.infoPerCoord
+
+##
+end