partials and antimarginal

Author: dehann

src/services/ApproxConv.jl

@@ -265,10 +265,23 @@ function calcProposalBelief(dfg::AbstractDFG,
   #
 
   # density passed through directly from PartialPriorPassThrough.Z
-  proposal = getFactorType(fct).Z.densityFnc
+  fctFnc = getFactorType(fct)
+  proposal = fctFnc.Z.densityFnc
+
+  # in case of partial, place the proposal into larger marginal/partial MKD
+  proposal_ = if isPartial(fctFnc)
+    # oldbel = getBelief(dfg, target, solveKey)
+    varType = getVariableType(dfg, target)
+    M = getManifold(varType)
+    u0 = getPointIdentity(varType)
+    # replace(oldbel, proposal)
+    antimarginal(M,u0,proposal,Int[fctFnc.partial...])
+  else
+    proposal
+  end
 
   # return the proposal belief and inferdim, NOTE likely to be changed
-  return proposal
+  return proposal_
 end
 
 """
@@ -302,7 +315,7 @@ function proposalbeliefs!(dfg::AbstractDFG,
     inferd = getFactorSolvableDim(dfg, fct, destlbl, solveKey)
     # convolve or passthrough to get a new proposal
     propBel_ = calcProposalBelief(dfg, fct, destlbl, measurement, N=N, dbg=dbg, solveKey=solveKey)
-    # @show propBel_.manifold
+    # @show propBel_.manifold, isPartial(propBel_)
     # partial density
     propBel = if isPartial(ccwl)
       pardims = _getDimensionsPartial(ccwl)

src/services/GraphInit.jl

@@ -123,7 +123,7 @@ function doautoinit!( dfg::AbstractDFG,
         end
         # FIXME ensure a product of only partial densities and returned pts are put to proper dimensions
         bel,inferdim = propagateBelief(dfg, getVariable(dfg,vsym), getFactor.(dfg,useinitfct), solveKey=solveKey, logger=logger)
-        # @info "MANIFOLD IS" bel.manifold string(getPoints(bel, false)[1]) 
+        # @info "MANIFOLD IS" bel.manifold isPartial(bel) string(bel._partial) string(getPoints(bel, false)[1]) 
         setValKDE!(xi, bel, true, inferdim, solveKey=solveKey) # getPoints(bel, false)
         # Update the estimates (longer DFG function used so cloud is also updated)
         setVariablePosteriorEstimates!(dfg, xi.label, solveKey)

test/testPartialPrior.jl

@@ -25,12 +25,16 @@ getSample(cfo::CalcFactor{<:PartialDim2}, N::Int=1) = ([rand(cfo.factor.Z, 1)[:]
 
 fg = initfg()
 
-addVariable!(fg, :x0, ContinuousEuclid{2})
+v0 = addVariable!(fg, :x0, ContinuousEuclid{2})
 
 f0 = addFactor!(fg, [:x0], PartialDim2(Normal()))
 
 @test IIF._getDimensionsPartial(f0) == [2]
 
+bel, infd = propagateBelief(fg, v0, [f0;])
+
+@test isPartial(bel)
+
 ##
 
 dens = Vector{ManifoldKernelDensity}()

test/testSpecialEuclidean2Mani.jl

@@ -1,8 +1,17 @@
 using DistributedFactorGraphs
 using IncrementalInference
+using Interpolations
 using Manifolds
 using StaticArrays
 using Test
+import IncrementalInference: HeatmapDensityRegular
+
+## define new local variable types for testing
+
+@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
+
+# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
+@defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
 
 ##
 
@@ -13,8 +22,6 @@ using Test
 Base.convert(::Type{<:Tuple}, M::SpecialEuclidean{2}) = (:Euclid, :Euclid, :Circular)
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{SpecialEuclidean{2}})  = (:Euclid, :Euclid, :Circular)
 
-# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
-@defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
 
 M = getManifold(SpecialEuclidean2)
 @test M == SpecialEuclidean(2)
@@ -179,8 +186,6 @@ end
 Base.convert(::Type{<:Tuple}, M::TranslationGroup{Tuple{2},ℝ}) = (:Euclid, :Euclid)
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{TranslationGroup{Tuple{2},ℝ}})  = (:Euclid, :Euclid)
 
-@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
-
 ##
 fg = initfg()
 
@@ -214,4 +219,46 @@ vnd = getVariableSolverData(fg, :x1)
 end
 
 
+@testset "test propagateBelief w HeatmapSampler and init for PartialPriorPassThrough" begin
+##
+
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+
+img_ = rand(10,10).+5.0
+x_,y_ = ([-9:2.0:9;],[-9:2.0:9;])
+
+hmd = HeatmapDensityRegular(img_, (x_,y_), 5.5, 0.1)
+pthru = PartialPriorPassThrough(hmd, (1,2))
+
+# test without nullhyp
+f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
+
+## test the inference functions
+
+bel, infd = propagateBelief(fg, v0, [f0])
+
+@test isPartial(bel)
+
+
+## repeat test with nullhypo
+
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, SpecialEuclidean2)
+# test with nullhypo
+f0 = addFactor!(fg, [:x0], pthru, graphinit=false, nullhypo=0.2)
+
+## test the inference functions
+
+bel, infd = propagateBelief(fg, v0, [f0])
+
+@test isPartial(bel)
+
+##
+end
+
+
+
 #