wip changing to AMP.replace

Author: dehann

src/FactorGraph.jl

@@ -227,16 +227,36 @@ setVariableInferDim!(vari::DFGVariable, val::Real) = setVariableInferDim!(getSol
 ## ==============================================================================================
 
 
-function setValKDE!(vd::VariableNodeData,
-                    p::ManifoldKernelDensity,
+function setValKDE!(vnd::VariableNodeData,
+                    p::ManifoldKernelDensity{M,B,Nothing},
                     setinit::Bool=true,
-                    inferdim::Union{Float32, Float64, Int32, Int64}=0 )
+                    inferdim::Union{Float32, Float64, Int32, Int64}=0 ) where {M,B}
   #
-  ptsArr = AMP.getPoints(p)
-  # @show typeof(ptsArr)
-  # @cast ptsArr[j][i] := pts[i,j]
+  # L==Nothing means no partials
+  ptsArr = AMP.getPoints(p) # , false) # for not partial
+  # also set the bandwidth
   bws = getBW(p)[:,1]
-  setValKDE!(vd,ptsArr,bws,setinit,inferdim )
+  setValKDE!(vnd,ptsArr,bws,setinit,inferdim )
+  nothing
+end
+
+
+function setValKDE!(vnd::VariableNodeData,
+                    mkd::ManifoldKernelDensity{M,B,L},
+                    setinit::Bool=true,
+                    inferdim::Union{Float32, Float64, Int32, Int64}=0 ) where {M,B,L<:AbstractVector}
+  #
+  oldbel = getBelief(vnd)
+  # Set partial dims as Manifold points
+  ptsArr = AMP.getPoints(mkd, false)
+  oldPts = getVal(vnd)
+  # get partial coord dims
+  pvec = mkd._partial
+
+  # also set the bandwidth
+  bws = getBW(mkd)[:,1]
+  bw_ = getBW
+  setValKDE!(vnd,ptsArr,bws,setinit,inferdim )
   nothing
 end
 
@@ -809,7 +829,7 @@ 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
-        pts,inferdim = predictbelief(dfg, vsym, useinitfct, solveKey=solveKey, logger=logger)
+        bel,inferdim = proposalBelief(dfg, vsym, useinitfct, solveKey=solveKey, logger=logger)
         setValKDE!(xi, pts, true, inferdim, solveKey=solveKey)
         # Update the estimates (longer DFG function used so cloud is also updated)
         setVariablePosteriorEstimates!(dfg, xi.label, solveKey)
@@ -1379,7 +1399,7 @@ end
 
 Get KernelDensityEstimate kde estimate stored in variable node.
 """
-getBelief(vnd::VariableNodeData) = manikde!(getVal(vnd), getBW(vnd)[:,1], getVariableType(vnd) )
+getBelief(vnd::VariableNodeData) = manikde!(getManifold(getVariableType(vnd)), getVal(vnd), bw=getBW(vnd)[:,1] )
 
 getBelief(v::DFGVariable, solvekey::Symbol=:default) = getBelief(getSolverData(v, solvekey))
 getBelief(dfg::AbstractDFG, lbl::Symbol, solvekey::Symbol=:default) = getBelief(getVariable(dfg, lbl), solvekey)

src/Factors/PartialPriorPassThrough.jl

@@ -9,4 +9,8 @@ struct PartialPriorPassThrough{B <: HeatmapDensityRegular, T <:Tuple} <: Abstrac
 end
 
 # this step is skipped during main inference process
-getSample(cf::CalcFactor{<:PartialPriorPassThrough}, N::Int=1) = ([rand(cf.factor.Z.densityFnc,1) for _ in 1:N], )
+getSample(cf::CalcFactor{<:PartialPriorPassThrough}, N::Int=1) = ([rand(cf.factor.Z.densityFnc,1) for _ in 1:N], )
+
+getManifold(pppt::PartialPriorPassThrough{<:HeatmapDensityRegular{T,H,<:ManifoldKernelDensity}}) where {T,H} = (pppt.Z.densityFnc.manifold)
+
+#

src/GraphProductOperations.jl

@@ -9,35 +9,60 @@ Notes
 - Returns tuple of product and whether full dimensional (=true) or partial (=false).
 - `N` determines the number of samples to draw from the marginal.
 - `dens` can contain mixed full and partial dimension `ManifoldKernelDensity` beliefs
+
+Related
+
+[`approxConv`](@ref), [`proposalbeliefs!`](@ref), [`AMP.manifoldProduct`](@ref)
 """
-function predictbelief( dfg::AbstractDFG,
-                        destvert::DFGVariable,
-                        factors::Vector{<:DFGFactor};
-                        solveKey::Symbol=:default,
-                        dens = Vector{ManifoldKernelDensity}(),
-                        N::Int=maximum([length(getPoints(getBelief(destvert, solveKey))); getSolverParams(dfg).N]),
-                        needFreshMeasurements::Bool=true,
-                        dbg::Bool=false,
-                        logger=ConsoleLogger()  )
-  #
+function propagateBelief( dfg::AbstractDFG,
+                          destvar::DFGVariable,
+                          factors::AbstractVector{<:DFGFactor};
+                          solveKey::Symbol=:default,
+                          dens = Vector{ManifoldKernelDensity}(),
+                          N::Int=maximum([length(getPoints(getBelief(destvar, solveKey))); getSolverParams(dfg).N]),
+                          needFreshMeasurements::Bool=true,
+                          dbg::Bool=false,
+                          logger=ConsoleLogger()  )
+    #
 
   # get proposal beliefs
-  destlbl = getLabel(destvert)
+  destlbl = getLabel(destvar)
   inferdim = proposalbeliefs!(dfg, destlbl, factors, dens, solveKey=solveKey, N=N, dbg=dbg)
 
-  # take the product
-  # TODO, make sure oldpts has right number of points!
+  # make sure oldpts has right number of points
   oldBel = getBelief(dfg, destlbl, solveKey)
   oldpts = if Npts(oldBel) == N
     getPoints(oldBel)
   else
     sample(oldBel, N)[1]
   end
 
-  varType = getVariableType(dfg, destlbl)
-  pGM = AMP.productbelief(oldpts, getManifold(varType), dens, N, dbg=dbg, logger=logger, asPartial=false )
+  # few more data requirements
+  varType = getVariableType(destvar)
+  M = getManifold(varType)
 
-  return pGM, sum(inferdim)
+  # take the product
+  mkd = AMP.manifoldProduct(dens, M, Niter=1, oldPoints=oldpts)
+  
+  return mkd, sum(inferdim)
+end
+
+"""
+    $SIGNATURES
+
+This is an old function that will be replace by [`propagateBelief`](@ref).
+"""
+function predictbelief( dfg::AbstractDFG,
+                        destvert::DFGVariable,
+                        factors::Vector{<:DFGFactor};
+                        asPartial::Bool=false,
+                        kw... )
+  #
+  # new
+  mkd, ifd = propagateBelief(dfg,destvert,factors;kw...)
+
+  # legacy interface
+  return getPoints(mkd, asPartial), ifd
 end
 
 predictbelief(dfg::AbstractDFG,
@@ -53,6 +78,7 @@ predictbelief(dfg::AbstractDFG,
 #
 
 
+
 """
     $(SIGNATURES)
 

src/HeatmapSampler.jl

@@ -96,7 +96,8 @@ function HeatmapDensityRegular( data::AbstractMatrix{<:Real},
   # final samplable density object
   bw = getBW(density_)[:,1]
   @cast pts[i,j] := vec_preIS[j][i]
-  density = kde!(collect(pts), bw, weights)
+  bel = kde!(collect(pts), bw, weights)
+  density = ManifoldKernelDensity(TranslationGroup(Ndim(bel)), bel)
 
   # return `<:SamplableBelief` object
   HeatmapDensityRegular(data, domain, hist_callback, level, sigma, float(sigma_scale), bw_factor, density)

src/IncrementalInference.jl

@@ -345,6 +345,7 @@ export *,
 
   # user functions
   predictbelief,
+  propagateBelief,
   getCliqMat,
   getCliqAssocMat,
   getCliqMsgMat,