fix propagateBelief N, more testing, passThru fix

Author: dehann

src/Factors/PartialPrior.jl

@@ -4,19 +4,19 @@
 $(TYPEDEF)
 
 Partial prior belief (absolute data) on any variable, given `<:SamplableBelief` and which dimensions of the intended variable.
+
+Notes
+- If using [`AMP.ManifoldKernelDensity`](@ref), don't double partial.  Only define the partial in this `PartialPrior` container. 
+  - Future TBD, consider using `AMP.getManifoldPartial` for more general abstraction.
 """
 struct PartialPrior{T <: SamplableBelief,P <: Tuple} <: AbstractPrior
   Z::T
   partial::P
 end
 
-function getSample(cf::CalcFactor{<:PartialPrior}, N::Int=1)
-  ret = Vector{Vector{Float64}}(undef, N)
-  for i in 1:N
-    ret[i] = rand(cf.factor.Z,1)[:] 
-  end
-  (ret, )
-end
+getManifold(pp::PartialPrior{<:PackedManifoldKernelDensity}) = pp.Z.manifold
+
+getSample(cf::CalcFactor{<:PartialPrior}, N::Int=1) = (randToPoints(cf.factor.Z,N), )
 
 
 """

src/GraphProductOperations.jl

@@ -19,7 +19,7 @@ function propagateBelief( dfg::AbstractDFG,
                           factors::AbstractVector{<:DFGFactor};
                           solveKey::Symbol=:default,
                           dens = Vector{ManifoldKernelDensity}(),
-                          N::Int=maximum([length(getPoints(getBelief(destvar, solveKey))); getSolverParams(dfg).N]),
+                          N::Int=getSolverParams(dfg).N, #maximum([length(getPoints(getBelief(destvar, solveKey))); getSolverParams(dfg).N]),
                           needFreshMeasurements::Bool=true,
                           dbg::Bool=false,
                           logger=ConsoleLogger()  )

test/testBasicGraphs.jl

@@ -353,32 +353,6 @@ end
 
 
 
-@testset "test propagateBelief returning a partial" begin
-##
-
-fg = initfg()
-
-v0 = addVariable!(fg, :x0, ContinuousEuclid{2})
-
-pp = PartialPrior(Normal(), (2,))
-f0 = addFactor!(fg, [:x0;], pp, graphinit=false)
-
-##
-
-bel, infd = propagateBelief(fg, v0, [f0;])
-
-@test isPartial(bel)
-
-
-##
-end
-
-
-
-
-
-
-
 
 
 #

test/testPartialPrior.jl

@@ -2,6 +2,7 @@
 
 using Test
 using IncrementalInference
+using Manifolds
 
 ##
 
@@ -20,7 +21,6 @@ getSample(cfo::CalcFactor{<:PartialDim2}, N::Int=1) = ([rand(cfo.factor.Z, 1)[:]
 ##
 
 @testset "Test partial dimensions on prior are correct" begin
-
 ##
 
 fg = initfg()
@@ -48,5 +48,35 @@ predictbelief(fg, :x0, [:x0f1;])
 @test true
 
 ##
+end
+
+
+
+@testset "test propagateBelief returning a partial" begin
+##
+
+fg = initfg()
+
+v0 = addVariable!(fg, :x0, ContinuousEuclid{2})
+
+pts = [randn(1) for _ in 1:1000];
+mkd = manikde!(TranslationGroup(1), pts, bw=[0.1;])
+pp = PartialPrior(mkd, (2,))
+f0 = addFactor!(fg, [:x0;], pp, graphinit=false)
+
+##
+
+bel, infd = propagateBelief(fg, v0, [f0;])
+@test isPartial(bel)
 
-end
+##
+
+doautoinit!(fg, :x0)
+
+# check the number of points in the graph value store
+@show getSolverParams(fg).N
+@test length(getPoints(getBelief(fg, :x0))) == getSolverParams(fg).N
+@info "PassThrough factors currently work different and will pass the full N=1000 count through to the graph."
+
+##
+end

test/testSpecialEuclidean2Mani.jl

@@ -229,7 +229,7 @@ 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)
+hmd = HeatmapDensityRegular(img_, (x_,y_), 5.5, 0.1, N=1000)
 pthru = PartialPriorPassThrough(hmd, (1,2))
 
 # test without nullhyp
@@ -240,6 +240,7 @@ f0 = addFactor!(fg, [:x0], pthru, graphinit=false)
 bel, infd = propagateBelief(fg, v0, [f0])
 
 @test isPartial(bel)
+@test length(getPoints(bel)) == 1000
 
 
 ## repeat test with nullhypo
@@ -253,9 +254,46 @@ f0 = addFactor!(fg, [:x0], pthru, graphinit=false, nullhypo=0.2)
 ## test the inference functions
 
 bel, infd = propagateBelief(fg, v0, [f0])
-
 @test isPartial(bel)
 
+## 
+
+doautoinit!(fg, :x0)
+
+@test length(getPoints(getBelief(fg, :x0))) == 1000
+@info "PassThrough transfers the full point count to the graph, unless a product is calculated during the propagateBelief step."
+
+##
+
+solveGraph!(fg);
+
+@test 1000 == length(getPoints(fg, :x0))
+
+@warn "must still check if bandwidths are recalculated on many points (not necessary), or lifted from this case single prior"
+
+##
+
+mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
+f1 = addFactor!(fg, [:x0], mp, graphinit=false)
+
+@test length(ls(fg, :x0)) == 2
+
+##
+
+prp, infd = propagateBelief(fg, v0, [f0;f1])
+
+@test length(getPoints(prp)) == getSolverParams(fg).N
+
+## check that solve corrects the point count on graph variable
+
+@test 1000 == length(getPoints(fg, :x0))
+
+solveGraph!(fg);
+
+# this number should drop down to usual, 100 at time of writing
+@test getSolverParams(fg).N == length(getPoints(fg, :x0))
+
+
 ##
 end