several test fixes using maniproduct w selective resampling

IncrementalInference/Project.toml

@@ -70,7 +70,7 @@ IncrInfrInterpolationsExt = "Interpolations"
 
 [compat]
 AMD = "0.5"
-ApproxManifoldProducts = "0.15.5"
+ApproxManifoldProducts = "0.15.7"
 BSON = "0.2, 0.3"
 CliqueTrees = "0.5, 1"
 Combinatorics = "1.0"

IncrementalInference/src/Deprecated.jl

@@ -1,5 +1,111 @@
 
 
+# function AMP.getBW(vnd::State)
+#   return ApproxManifoldProducts.getBW(vnd.belief)
+# end
+
+# # setVal! assumes you will update values to database separate, this used for local graph mods only
+# function getBWVal(v::VariableCompute; solveKey::Symbol = :default)
+#   return getBW(getState(v, solveKey))
+# end
+# function setBW!(vd::State, bw::Array{Float64, 2}; solveKey::Symbol = :default)
+#   DFG.refBandwidth(vd) .= bw # FIXME
+#   return nothing
+# end
+# function setBW!(v::VariableCompute, bw::Array{Float64, 2}; solveKey::Symbol = :default)
+#   setBW!(getState(v, solveKey), bw)
+#   return nothing
+# end
+
+# function setVal!(
+#   vd::State, 
+#   val::AbstractVector{P}; 
+#   observability::AbstractVector{<:Real} = [0.0;]
+# ) where {P}
+#   points = DFG.refPoints(vd)
+#   resize!(points, length(val))
+#   points .= val
+
+#   observability = DFG.refObservability(vd)
+#   resize!(observability, length(observability))
+#   observability .= observability
+#   return nothing
+# end
+# function setVal!(
+#   v::VariableCompute,
+#   val::AbstractVector{P};
+#   solveKey::Symbol = :default,
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   setVal!(getState(v, solveKey), val; observability)
+#   return nothing
+# end
+# function setVal!(
+#   vd::State,
+#   val::AbstractVector{P},
+#   bw::AbstractMatrix{Float64};
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   setVal!(vd, val; observability)
+#   setBW!(vd, bw)
+#   return nothing
+# end
+# function setVal!(
+#   v::VariableCompute,
+#   val::AbstractVector{P},
+#   bw::AbstractMatrix{Float64};
+#   solveKey::Symbol = :default,
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   setVal!(v, val; solveKey, observability)
+#   setBW!(v, bw; solveKey)
+#   return nothing
+# end
+# function setVal!(
+#   vd::State,
+#   val::AbstractVector{P},
+#   bw::AbstractVector{Float64};
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   setVal!(vd, val, reshape(bw, length(bw), 1); observability)
+#   return nothing
+# end
+# function setVal!(
+#   v::VariableCompute,
+#   val::AbstractVector{P},
+#   bw::AbstractVector{Float64};
+#   solveKey::Symbol = :default,
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   setVal!(getState(v, solveKey), val, bw; observability)
+#   return nothing
+# end
+# function setVal!(
+#   dfg::AbstractDFG,
+#   sym::Symbol,
+#   val::AbstractVector{P};
+#   solveKey::Symbol = :default,
+#   observability::AbstractVector{<:Real} = [0.0;],
+# ) where {P}
+#   return setVal!(getVariable(dfg, sym), val; solveKey, observability)
+# end
+
+
+# """
+#     $(SIGNATURES)
+
+# Fetch the variable marginal joint sampled points.  Use [`getBelief`](@ref) to retrieve the full Belief object.
+# """
+getVal(v::VariableCompute; solveKey::Symbol = :default) = getPoints(getState(v, solveKey).belief; permute=false)
+function getVal(v::VariableCompute, idx::Int; solveKey::Symbol = :default)
+  return getPoints(getState(v, solveKey).belief; permute=false)[idx]
+end
+getVal(vnd::State) = getPoints(vnd; permute=false)
+getVal(vnd::State, idx::Int) = getPoints(vnd; permute=false)[idx]
+function getVal(dfg::AbstractDFG, lbl::Symbol; solveKey::Symbol = :default)
+  return getVal(getVariable(dfg, lbl); solveKey)
+end
+
 # convert(
 #   ::Type{<:ApproxManifoldProducts.HomotopyDensity}, 
 #   src::TreeBelief,

IncrementalInference/src/services/ApproxConv.jl

@@ -14,17 +14,23 @@ function approxConvBelief(
 )
   #
   v_trg = getVariable(dfg, target)
-  N = N == 0 ? getNumPts(v_trg; solveKey) : N
-  # approxConv should push its result into duplicate memory destination, NOT the variable.VND.val itself.  ccw.varValsAll always points directly to variable.VND.val
-  # points and infoPerCoord
+  N_ = if N != 0
+    N 
+  elseif hasState(v_trg, solveKey)
+    getNumPts(v_trg; solveKey)
+  else
+    getSolverParams(dfg).N
+  end
+  # N = N == 0 ? getNumPts(v_trg; solveKey) : N
 
-  pts, ipc = evalFactor(
+  # NOTE approxConv results happen in  duplicate memory destination,  ccw.varValsAll always points directly to variable.VND.val
+  pts, observability = evalFactor(
     dfg, 
     fc, 
     v_trg.label, 
     measurement; 
     solveKey, 
-    N, 
+    N = N_, 
     skipSolve, 
     nullSurplus,
     keepCalcFactor
@@ -33,19 +39,19 @@ function approxConvBelief(
   ## FIXME, bad way to find partial info!!!!
   # Not sufficient to use only observability to determine partial, but is necessary
   # original need is if observability on some coords are zero after a convolution
-  len = length(ipc)
-  mask = 1e-14 .< abs.(ipc)
+  len = length(observability)
+  mask = 1e-14 .< abs.(observability)
   partl = collect(1:len)[mask] 
 
   # is the convolution infoPerCoord full or partial
   statekind = getStateKind(v_trg)
   # FIXME, this if induces type instability via partial
   res = if sum(mask) == getDimension(v_trg)
     # not partial
-    HomotopyDensity_legacy(statekind, pts; partial = nothing)
+    HomotopyDensity_legacy(statekind, pts; observability, partial = nothing)
   else
     # is partial
-    HomotopyDensity_legacy(statekind, pts; partial = partl)
+    HomotopyDensity_legacy(statekind, pts; observability, partial = partl)
   end
 
   return res
@@ -273,10 +279,7 @@ function proposalbeliefs!(
   vardim = getDimension(getVariable(dfg, destlbl))
   # get a proposal belief from each factor connected to destlbl
   for (count, fct) in enumerate(factors)
-    # need way to convey partial information
-    # determine if evaluation is "dimension-deficient" solvable dimension
-    # FIXME, update to infoPerCoord
-    fct_ipc = ones(vardim) # getFactorSolvableDim(dfg, fct, destlbl, solveKey)
+
     # convolve or passthrough to get a new proposal
     propBel_ = calcProposalBelief(
       dfg,
@@ -288,15 +291,22 @@ function proposalbeliefs!(
       solveKey,
       nullSurplus = nullSrp[count],
     )
-    # partial density
+
+    # need way to convey partial density information
     obs = DFG.getObservation(fct)
+    ipcs[count] = propBel_.observability # TODO, deprecate ipcs duplication, use hode.observability directly
+      # # determine if evaluation is "dimension-deficient" solvable dimension
+      # fct_ipc = zeros(vardim)  
+      # ApproxManifoldProducts._viewprl(fct_ipc, obs.partial) .+= 1
+      # ipcs[count] = fct_ipc
+
     propBel = if isPartial(obs)
+      # @show obs.partial
       AMP.marginal(propBel_, Int[obs.partial...])
     else
       propBel_
     end
     push!(dens, propBel)
-    ipcs[count] = fct_ipc
   end
   # len = maximum(length.(ipcs))
   ipc = zeros(vardim)

IncrementalInference/src/services/CalcFactor.jl

@@ -523,17 +523,25 @@ function _beforeSolveCCW!(
     @debug("cannot prep ccw.param list with length(variables)==0, see DFG #590")
   end
 
+  function _consolgetval(v::VariableCompute)
+    return if hasState(v, solveKey) && isInitialized(v, solveKey)
+      getVal(v; solveKey)
+    else
+      getPoints(defaultBelief(v, NPBPSolver(); num_kernels=N); permute=false)
+    end
+  end
+
   # in forward solve case, important to set which variable is being solved early in this sequence
   # set the 'solvefor' variable index -- i.e. which connected variable of the factor is being computed in this convolution. 
   ccwl.varidx[] = sfidx
   # ccwl.varidx[] = findfirst(==(solvefor), getLabel.(variables))
 
-  # splice, type stable
+  # splice, TBD type stability
   # make deepcopy of destination variable since multiple approxConv type computations should happen from different factors to the same variable
   tvarv = tuple(
-    map(s->getVal(s; solveKey), variables[1:ccwl.varidx[]-1])..., 
-    deepcopy(getVal(variables[ccwl.varidx[]]; solveKey)), # deepcopy(ccwl.varValsAll[][sfidx]),
-    map(s->getVal(s; solveKey), variables[ccwl.varidx[]+1:end])...,
+    map(s->_consolgetval(s), variables[1:ccwl.varidx[]-1])..., 
+    deepcopy(_consolgetval(variables[ccwl.varidx[]])), # deepcopy(ccwl.varValsAll[][sfidx]),
+    map(s->_consolgetval(s), variables[ccwl.varidx[]+1:end])...,
   )
   ccwl.varValsAll[] = tvarv
 
@@ -590,14 +598,22 @@ function _beforeSolveCCW!(
 ) where {F <: AbstractObservation} # F might be Mixture
   # FIXME, NEEDS TO BE CLEANED UP AND WORK ON MANIFOLDS PROPER
 
+  function _consolgetval(v::VariableCompute)
+    return if hasState(v, solveKey) && isInitialized(v, solveKey)
+      getVal(v; solveKey)
+    else
+      getPoints(defaultBelief(v, NPBPSolver(); num_kernels=N); permute=false)
+    end
+  end
+
   ccwl.varidx[] = sfidx
   @assert ccwl.varidx[] == 1 "Solving on Prior with CCW should have sfidx=1, priors are unary factors."
 
   # setup the partial or complete decision variable dimensions for this ccwl object
   # NOTE perhaps deconv has changed the decision variable list, so placed here during consolidation phase
   _setCCWDecisionDimsConv!(ccwl, getDimension(getStateKind(variables[ccwl.varidx[]])))
 
-  solveForPts = getVal(variables[ccwl.varidx[]]; solveKey)
+  solveForPts = _consolgetval(variables[ccwl.varidx[]])
   maxlen = maximum([N; length(solveForPts); length(ccwl.varValsAll[][ccwl.varidx[]])])  # calcZDim(ccwl); length(measurement[1])
 
   # FIXME do not divert Mixture for sampling

IncrementalInference/src/services/DeconvUtils.jl

@@ -181,11 +181,10 @@ function approxDeconv(
   dfg::AbstractDFG,
   fctsym::Symbol,
   solveKey::Symbol = :default;
+  N::Int = 100,
 )
   # which factor
   fct = getFactor(dfg, fctsym)
-  pts = getPoints(getBelief(dfg, getVariableOrder(fct)[1], solveKey))
-  N = length(pts)
   pts = approxDeconv(fct, _getCCW(dfg, fct); N = N)
   return pts
 end