Merge pull request #1716 from JuliaRobotics/23Q2/twig/varValsAll_ref

rework ccw.varValsAll memory for static arrays

src/Deprecated.jl

@@ -119,11 +119,64 @@ function solveGraphParametric2(
   return d, result, flatvar.idx, Σ
 end
 
+##==============================================================================
+## Deprecate code below before v0.35
+##==============================================================================
+
+
+@deprecate _prepCCW(w...;kw...) _createCCW(w...;kw...)
+
+predictbelief(w...;asPartial::Bool=false,kw...) = begin 
+  @warn("predictbelief is deprecated, use propagateBelief instead")
+  bel,ipc = propagateBelief(w...;asPartial,kw...)
+  getPoints(bel), ipc
+end
+
+# """
+#     $SIGNATURES
+
+# This is an old function name that will be replaced by [`propagateBelief`](@ref).
+# """
+# function predictbelief(
+#   dfg::AbstractDFG,
+#   destvert::DFGVariable,
+#   factors::AbstractVector; #{<:DFGFactor};
+#   asPartial::Bool = false,
+#   kw...,
+# )
+#   #
+#   # new
+#   mkd, ifd = propagateBelief(dfg, destvert, factors; kw...)
+
+#   # legacy interface
+#   return getPoints(mkd, asPartial), ifd
+# end
+
+# function predictbelief(
+#   dfg::AbstractDFG,
+#   destlbl::Symbol,
+#   fctlbls::AbstractVector{Symbol};
+#   kw...,
+# )
+#   return predictbelief(
+#     dfg,
+#     getVariable(dfg, destlbl),
+#     map(x -> getFactor(dfg, x), fctlbls);
+#     kw...,
+#   )
+# end
+# #
+
+# function predictbelief(dfg::AbstractDFG, destlbl::Symbol, ::Colon; kw...)
+#   return predictbelief(dfg, destlbl, getNeighbors(dfg, destlbl); kw...)
+# end
+#
 
 ##==============================================================================
 ## Deprecate code below before v0.34
 ##==============================================================================
 
+
 # function CommonConvWrapper(
 #   usrfnc::T,
 #   fullvariables, #::Tuple ::Vector{<:DFGVariable};
@@ -204,15 +257,27 @@ function Base.getproperty(ccw::CommonConvWrapper, f::Symbol)
     # return SingleThreaded
   elseif f == :params
     error("CommonConvWrapper.params is deprecated, use .varValsAll instead")
-    return ccw.varValsAll
+    return ccw.varValsAll[]
   elseif f == :vartypes
     @warn "CommonConvWrapper.vartypes is deprecated, use typeof.(getVariableType.(ccw.fullvariables) instead" maxlog=3
     return typeof.(getVariableType.(ccw.fullvariables))
+  elseif f == :hypotheses
+    @warn "CommonConvWrapper.hypotheses is now under ccw.hyporecipe.hypotheses" maxlog=5
+    return ccw.hyporecipe.hypotheses
+  elseif f == :certainhypo
+    @warn "CommonConvWrapper.certainhypo is now under ccw.hyporecipe.certainhypo" maxlog=5
+    return ccw.hyporecipe.certainhypo
+  elseif f == :activehypo
+    @warn "CommonConvWrapper.activehypo is now under ccw.hyporecipe.activehypo" maxlog=5
+    return ccw.hyporecipe.activehypo
   else
     return getfield(ccw, f)
   end
 end
 
+
+
+
 ##==============================================================================
 ## Deprecate code below before v0.35
 ##==============================================================================

src/ExportAPI.jl

@@ -153,6 +153,7 @@ export CSMHistory,
   getNumPts,
   getBWVal,
   setBW!,
+  setBelief!,
   setValKDE!,
   buildCliqSubgraph,
 
@@ -240,7 +241,6 @@ export CSMHistory,
   getCliqNumAssocFactorsPerVar,
 
   # user functions
-  predictbelief,
   propagateBelief,
   getCliqMat,
   getCliqAssocMat,

src/Factors/LinearRelative.jl

@@ -10,7 +10,7 @@ Default linear offset between two scalar variables.
 X_2 = X_1 + η_Z
 ```
 """
-struct LinearRelative{N, T <: SamplableBelief} <: AbstractRelativeRoots
+struct LinearRelative{N, T <: SamplableBelief} <: AbstractRelativeMinimize
   Z::T
 end
 
@@ -41,6 +41,10 @@ getDimension(::InstanceType{LinearRelative{N}}) where {N} = N
 # new and simplified interface for both nonparametric and parametric
 function (s::CalcFactor{<:LinearRelative})(z, x1, x2)
   # TODO convert to distance(distance(x2,x1),z) # or use dispatch on `-` -- what to do about `.-`
+  # if s._sampleIdx < 5
+  #   @info "LinearRelative" s._sampleIdx "$z" "$x1" "$x2" s.solvefor getLabel.(s.fullvariables)
+  #   @info "in variables" pointer(getVal(s.fullvariables[s.solvefor])) getVal(s.fullvariables[s.solvefor])[1]
+  # end
   return z .- (x2 .- x1)
 end
 

src/IncrementalInference.jl

@@ -125,6 +125,7 @@ include("ManifoldsExtentions.jl")
 # regular
 include("entities/SolverParams.jl")
 
+include("entities/HypoRecipe.jl")
 include("entities/FactorOperationalMemory.jl")
 
 include("Factors/GenericMarginal.jl")

src/Serialization/services/DispatchPackedConversions.jl

@@ -37,12 +37,12 @@ function reconstFactorData(
   vars = map(f -> getVariable(dfg, f), varOrder)
   userCache = preambleCache(dfg, vars, usrfnc)
 
-  # TODO -- improve _prepCCW for hypotheses and certainhypo field recovery when deserializing
+  # TODO -- improve _createCCW for hypotheses and certainhypo field recovery when deserializing
   # reconstitute from stored data
   # FIXME, add threadmodel=threadmodel
   # FIXME https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-776838053
   # FIXME dont know what manifolds to use in ccw
-  ccw = _prepCCW(
+  ccw = _createCCW(
     vars,
     usrfnc;
     multihypo,
@@ -145,7 +145,7 @@ function rebuildFactorMetadata!(
   end
 
   #... Copying neighbor data into the factor?
-  # JT TODO it looks like this is already updated in getDefaultFactorData -> _prepCCW
+  # JT TODO it looks like this is already updated in getDefaultFactorData -> _createCCW
   # factormetadata.variableuserdata is deprecated, remove when removing deprecation
   # for i in 1:Threads.nthreads()
   #   ccw_new.fnc.cpt[i].factormetadata.variableuserdata = deepcopy(neighborUserData)

src/entities/FactorOperationalMemory.jl

@@ -63,6 +63,8 @@ $(TYPEDEF)
 """
 struct MultiThreaded <: _AbstractThreadModel end
 
+
+
 """
 $(TYPEDEF)
 
@@ -83,11 +85,10 @@ Related
 Base.@kwdef struct CommonConvWrapper{
   T <: AbstractFactor, 
   VT <: Tuple,
-  NTP <: Tuple, 
+  TP <: Base.RefValue{<:Tuple},
   CT,
   AM <: AbstractManifold,
-  HP <: Union{Nothing, <:Distributions.Categorical{Float64, Vector{Float64}}},
-  CH <: Union{Nothing, Vector{Int}},
+  HR <: HypoRecipeCompute,
   MT, 
   G
 } <: FactorOperationalMemory
@@ -98,8 +99,9 @@ Base.@kwdef struct CommonConvWrapper{
   fullvariables::VT
   # shortcuts to numerical containers
   """ Numerical containers for all connected variables.  Hypo selection needs to be passed 
-      to each hypothesis evaluation event on user function via CalcFactor, #1321 """
-  varValsAll::NTP
+      to each hypothesis evaluation event on user function via CalcFactor, #1321.
+      Points directly at the variable VND.val (not a deepcopy). """
+  varValsAll::TP
   """ dummy cache value to be deep copied later for each of the CalcFactor instances """
   dummyCache::CT = nothing
   # derived config parameters for this factor
@@ -113,13 +115,8 @@ Base.@kwdef struct CommonConvWrapper{
   nullhypo::Float64 = 0.0
   """ inflationSpread particular to this factor (by how much to dispurse the belief initial values before numerical optimization is run).  Analogous to stochastic search """
   inflation::Float64 = SolverParams().inflation
-  # multihypo specific field containers for recipe of hypotheses to compute
-  """ multi hypothesis settings #NOTE no need for a parameter as type is known from `parseusermultihypo` """
-  hypotheses::HP = nothing
-  """ categorical to select which hypothesis is being considered during convolution operation """
-  certainhypo::CH = nothing
-  """ subsection indices to select which params should be used for this hypothesis evaluation """
-  activehypo::Vector{Int} = collect(1:length(varValsAll))
+  """ multihypo specific field containers for recipe of hypotheses to compute """
+  hyporecipe::HR = HypoRecipeCompute(;activehypo=collect(1:length(varValsAll)))
   # buffers and indices to point numerical computations to specific memory locations
   """ user defined measurement values for each approxConv operation
       FIXME make type stable, JT should now be type stable if rest works.

src/entities/HypoRecipe.jl

@@ -0,0 +1,21 @@
+
+
+
+Base.@kwdef struct HypoRecipe
+  certainidx::Vector{Int}
+  allelements::Vector{Vector{Int}}
+  activehypo::Vector{Tuple{Int,Vector{Int}}}
+  mhidx::Vector{Int}
+end
+
+Base.@kwdef struct HypoRecipeCompute{
+  HP <: Union{Nothing, <:Distributions.Categorical{Float64, <:AbstractVector{Float64}}},
+  CH <: Union{Nothing, <:AbstractVector{<:Integer}},
+}
+  """ multi hypothesis settings #NOTE no need for a parameter as type is known from `parseusermultihypo` """
+  hypotheses::HP = nothing
+  """ categorical to select which hypothesis is being considered during convolution operation """
+  certainhypo::CH = nothing
+  """ subsection indices to select which params should be used for this hypothesis evaluation """
+  activehypo::Vector{Int} = Int[]
+end

src/services/ApproxConv.jl

@@ -12,23 +12,36 @@ function approxConvBelief(
   skipSolve::Bool = false,
 )
   #
-  v1 = getVariable(dfg, target)
-  N = N == 0 ? getNumPts(v1; solveKey = solveKey) : N
+  v_trg = getVariable(dfg, target)
+  N = N == 0 ? getNumPts(v_trg; solveKey = 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
-  pts, ipc = evalFactor(dfg, fc, v1.label, measurement; solveKey, N, skipSolve, nullSurplus)
+
+  pts, ipc = evalFactor(
+    dfg, 
+    fc, 
+    v_trg.label, 
+    measurement; 
+    solveKey, 
+    N, 
+    skipSolve, 
+    nullSurplus
+  )
 
   len = length(ipc)
   mask = 1e-14 .< abs.(ipc)
   partl = collect(1:len)[mask]
 
   # is the convolution infoPerCoord full or partial
-  if sum(mask) == len
+  res = if sum(mask) == len
     # not partial
-    return manikde!(getManifold(getVariable(dfg, target)), pts; partial = nothing)
+    manikde!(getManifold(getVariable(dfg, target)), pts; partial = nothing)
   else
     # is partial
-    return manikde!(getManifold(getVariable(dfg, target)), pts; partial = partl)
+    manikde!(getManifold(getVariable(dfg, target)), pts; partial = partl)
   end
+
+  return res
 end
 
 approxConv(w...; kw...) = getPoints(approxConvBelief(w...; kw...), false)
@@ -66,7 +79,7 @@ function approxConvBelief(
   measurement::AbstractVector = Tuple[];
   solveKey::Symbol = :default,
   N::Int = length(measurement),
-  tfg::AbstractDFG = initfg(),
+  tfg::AbstractDFG = LocalDFG(;solverParams=getSolverParams(dfg)),
   setPPEmethod::Union{Nothing, Type{<:AbstractPointParametricEst}} = nothing,
   setPPE::Bool = setPPEmethod !== nothing,
   path::AbstractVector{Symbol} = Symbol[],
@@ -96,7 +109,9 @@ function approxConvBelief(
     neMsk = exists.(tfg, varLbls) .|> x -> xor(x, true)
     # put the non-existing variables into the temporary graph `tfg`
     # bring all the solveKeys too
-    addVariable!.(tfg, getVariable.(dfg, varLbls[neMsk]))
+    for v in getVariable.(dfg, varLbls[neMsk])
+      addVariable!(tfg, v.label, getVariableType(v))
+    end
     # variables adjacent to the shortest path should be initialized from dfg
     setdiff(varLbls, path[xor.(fctMsk, true)]) .|>
     x -> initVariable!(tfg, x, getBelief(dfg, x))