Skip to content

Commit e4227c1

Browse files
committed
partials and antimarginal
1 parent a00df5d commit e4227c1

File tree

4 files changed

+73
-9
lines changed

4 files changed

+73
-9
lines changed

src/services/ApproxConv.jl

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -265,10 +265,23 @@ function calcProposalBelief(dfg::AbstractDFG,
265265
#
266266

267267
# density passed through directly from PartialPriorPassThrough.Z
268-
proposal = getFactorType(fct).Z.densityFnc
268+
fctFnc = getFactorType(fct)
269+
proposal = fctFnc.Z.densityFnc
270+
271+
# in case of partial, place the proposal into larger marginal/partial MKD
272+
proposal_ = if isPartial(fctFnc)
273+
# oldbel = getBelief(dfg, target, solveKey)
274+
varType = getVariableType(dfg, target)
275+
M = getManifold(varType)
276+
u0 = getPointIdentity(varType)
277+
# replace(oldbel, proposal)
278+
antimarginal(M,u0,proposal,Int[fctFnc.partial...])
279+
else
280+
proposal
281+
end
269282

270283
# return the proposal belief and inferdim, NOTE likely to be changed
271-
return proposal
284+
return proposal_
272285
end
273286

274287
"""
@@ -302,7 +315,7 @@ function proposalbeliefs!(dfg::AbstractDFG,
302315
inferd = getFactorSolvableDim(dfg, fct, destlbl, solveKey)
303316
# convolve or passthrough to get a new proposal
304317
propBel_ = calcProposalBelief(dfg, fct, destlbl, measurement, N=N, dbg=dbg, solveKey=solveKey)
305-
# @show propBel_.manifold
318+
# @show propBel_.manifold, isPartial(propBel_)
306319
# partial density
307320
propBel = if isPartial(ccwl)
308321
pardims = _getDimensionsPartial(ccwl)

src/services/GraphInit.jl

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

test/testPartialPrior.jl

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,16 @@ getSample(cfo::CalcFactor{<:PartialDim2}, N::Int=1) = ([rand(cfo.factor.Z, 1)[:]
2525

2626
fg = initfg()
2727

28-
addVariable!(fg, :x0, ContinuousEuclid{2})
28+
v0 = addVariable!(fg, :x0, ContinuousEuclid{2})
2929

3030
f0 = addFactor!(fg, [:x0], PartialDim2(Normal()))
3131

3232
@test IIF._getDimensionsPartial(f0) == [2]
3333

34+
bel, infd = propagateBelief(fg, v0, [f0;])
35+
36+
@test isPartial(bel)
37+
3438
##
3539

3640
dens = Vector{ManifoldKernelDensity}()

test/testSpecialEuclidean2Mani.jl

Lines changed: 51 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,17 @@
11
using DistributedFactorGraphs
22
using IncrementalInference
3+
using Interpolations
34
using Manifolds
45
using StaticArrays
56
using Test
7+
import IncrementalInference: HeatmapDensityRegular
8+
9+
## define new local variable types for testing
10+
11+
@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
12+
13+
# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
14+
@defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
615

716
##
817

@@ -13,8 +22,6 @@ using Test
1322
Base.convert(::Type{<:Tuple}, M::SpecialEuclidean{2}) = (:Euclid, :Euclid, :Circular)
1423
Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{SpecialEuclidean{2}}) = (:Euclid, :Euclid, :Circular)
1524

16-
# @defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
17-
@defVariable SpecialEuclidean2 SpecialEuclidean(2) ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
1825

1926
M = getManifold(SpecialEuclidean2)
2027
@test M == SpecialEuclidean(2)
@@ -179,8 +186,6 @@ end
179186
Base.convert(::Type{<:Tuple}, M::TranslationGroup{Tuple{2},ℝ}) = (:Euclid, :Euclid)
180187
Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{TranslationGroup{Tuple{2},ℝ}}) = (:Euclid, :Euclid)
181188

182-
@defVariable Point2 TranslationGroup(2) [0.0, 0.0]
183-
184189
##
185190
fg = initfg()
186191

@@ -214,4 +219,46 @@ vnd = getVariableSolverData(fg, :x1)
214219
end
215220

216221

222+
@testset "test propagateBelief w HeatmapSampler and init for PartialPriorPassThrough" begin
223+
##
224+
225+
fg = initfg()
226+
227+
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
228+
229+
img_ = rand(10,10).+5.0
230+
x_,y_ = ([-9:2.0:9;],[-9:2.0:9;])
231+
232+
hmd = HeatmapDensityRegular(img_, (x_,y_), 5.5, 0.1)
233+
pthru = PartialPriorPassThrough(hmd, (1,2))
234+
235+
# test without nullhyp
236+
f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
237+
238+
## test the inference functions
239+
240+
bel, infd = propagateBelief(fg, v0, [f0])
241+
242+
@test isPartial(bel)
243+
244+
245+
## repeat test with nullhypo
246+
247+
fg = initfg()
248+
249+
v0 = addVariable!(fg, :x0, SpecialEuclidean2)
250+
# test with nullhypo
251+
f0 = addFactor!(fg, [:x0], pthru, graphinit=false, nullhypo=0.2)
252+
253+
## test the inference functions
254+
255+
bel, infd = propagateBelief(fg, v0, [f0])
256+
257+
@test isPartial(bel)
258+
259+
##
260+
end
261+
262+
263+
217264
#

0 commit comments

Comments
 (0)