Merge pull request #1342 from JuliaRobotics/21Q3/enh/passthrufinal

fix propagateBelief N, more testing, passThru fix

Author: dehann

Project.toml

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

src/FactorGraph.jl

@@ -221,16 +221,18 @@ function setValKDE!(vnd::VariableNodeData,
                     setinit::Bool=true,
                     inferdim::Union{Float32, Float64, Int32, Int64}=0 ) where {M,B,L<:AbstractVector}
   #
-  oldbel = getBelief(vnd)
+  oldBel = getBelief(vnd)
+
+  # New infomation might be partial
+  newBel = replace(oldBel, mkd)
+
   # Set partial dims as Manifold points
-  ptsArr = AMP.getPoints(mkd, false)
-  oldPts = getVal(vnd)
-  # get partial coord dims
-  pvec = mkd._partial
+  ptsArr = AMP.getPoints(newBel, false)
 
-  # also set the bandwidth
-  bws = getBW(mkd)[:,1]
-  bw_ = getBW
+  # also get the bandwidth
+  bws = getBandwidth(newBel, false)
+
+  # update values in graph
   setValKDE!(vnd,ptsArr,bws,setinit,inferdim )
   nothing
 end

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()  )
@@ -45,7 +45,7 @@ function propagateBelief( dfg::AbstractDFG,
   # @info "BUILDING MKD" varType M
 
   # take the product
-  mkd = AMP.manifoldProduct(dens, M, Niter=1, oldPoints=oldpts)
+  mkd = AMP.manifoldProduct(dens, M, Niter=1, oldPoints=oldpts, N=N)
 
   # @info "GOT" mkd.manifold
 
@@ -95,7 +95,7 @@ Return: product belief, full proposals, partial dimension proposals, labels
 function localProduct(dfg::AbstractDFG,
                       sym::Symbol;
                       solveKey::Symbol=:default,
-                      N::Int=maximum([length(getPoints(getBelief(dfg, sym, solveKey))); getSolverParams(dfg).N]),
+                      N::Int=getSolverParams(dfg).N, #maximum([length(getPoints(getBelief(dfg, sym, solveKey))); getSolverParams(dfg).N]),
                       dbg::Bool=false,
                       logger=ConsoleLogger() )
   #

src/IncrementalInference.jl

@@ -395,6 +395,8 @@ export *,
   reshapeVec2Mat,
   accumulateFactorChain
 
+# more optional exports
+export HeatmapDensityRegular
 
 
 export  buildCliqSubgraph_StateMachine

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
 
 """
@@ -287,7 +300,7 @@ function proposalbeliefs!(dfg::AbstractDFG,
                           # partials::Dict{Any, Vector{ManifoldKernelDensity}}, # TODO change this structure
                           measurement::Tuple=(Vector{Vector{Float64}}(),);
                           solveKey::Symbol=:default,
-                          N::Int=maximum([length(getPoints(getBelief(dfg, destlbl, solveKey))); getSolverParams(dfg).N]),
+                          N::Int=getSolverParams(dfg).N, #maximum([length(getPoints(getBelief(dfg, destlbl, solveKey))); getSolverParams(dfg).N]),
                           dbg::Bool=false  )
   #
 
@@ -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

@@ -89,7 +89,7 @@ function doautoinit!( dfg::AbstractDFG,
                       xi::DFGVariable;
                       solveKey::Symbol=:default,
                       singles::Bool=true,
-                      N::Int=maximum([length(getPoints(getBelief(xi, solveKey))); getSolverParams(dfg).N]),
+                      N::Int=getSolverParams(dfg).N, #maximum([length(getPoints(getBelief(xi, solveKey))); getSolverParams(dfg).N]),
                       logger=ConsoleLogger() )
   #
   didinit = false
@@ -122,9 +122,11 @@ function doautoinit!( dfg::AbstractDFG,
           @info "do init of $vsym"
         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]) 
-        setValKDE!(xi, bel, true, inferdim, solveKey=solveKey) # getPoints(bel, false)
+        bel,inferdim = propagateBelief(dfg, getVariable(dfg,vsym), getFactor.(dfg,useinitfct), solveKey=solveKey, logger=logger, N=N)
+        # while the propagate step might allow large point counts, the graph should stay restricted to N
+        bel_ = Npts(bel) == getSolverParams(dfg).N ? bel : resample(bel, getSolverParams(dfg).N)
+        # @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)
         # Update the data in the event that it's not local

src/services/HeatmapSampler.jl

@@ -2,7 +2,7 @@
 
 @info "Including Interpolations related functions in IncrementalInference."
 
-# using Interpolations 
+using .Interpolations 
 
 # only export on Requires.jl
 export HeatmapDensityRegular
@@ -72,7 +72,7 @@ function HeatmapDensityRegular( data::AbstractMatrix{<:Real},
   # constuct a pre-density from which to draw intermediate samples
   density_ = fitKDE(support_, weights_, domain...; bw_factor=bw_factor)
   pts_preIS, = sample(density_, N)
-  @show size(pts_preIS)
+  # @show size(pts_preIS)
 
   @cast vec_preIS[j][i] := pts_preIS[i,j]
 

test/testBasicGraphs.jl

@@ -355,6 +355,4 @@ end
 
 
 
-
-
 #

test/testPartialPrior.jl

@@ -2,6 +2,7 @@
 
 using Test
 using IncrementalInference
+using Manifolds
 
 ##
 
@@ -20,17 +21,20 @@ getSample(cfo::CalcFactor{<:PartialDim2}, N::Int=1) = ([rand(cfo.factor.Z, 1)[:]
 ##
 
 @testset "Test partial dimensions on prior are correct" begin
-
 ##
 
 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}()
@@ -44,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})
 
-end
+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)
+
+##
+
+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