Merge pull request #1669 from JuliaRobotics/23Q1/refac/enhccwdims

towards better partial handling

NEWS.md

@@ -19,6 +19,8 @@ The list below highlights breaking changes according to normal semver workflow -
 - Now have `CalcFactor.manifold` to reduce new allocation load inside hot-loop for solving.
 - Fixed tesing issues in `testSpecialEuclidean2Mani.jl`.
 - Refactored, consolidated, and added more in-place operations in surrounding `ccw.measurement`.
+- Refactor `CommonConvWrapper` to a not non-mutable struct, with several cleanups and some updated compat requirements.
+- Refactor interal hard type `HypoRecipe`.
 
 # Changes in v0.31
 - `FactorMetaData` is deprecated and replaced by `CalcFactor`.

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.32.0"
+version = "0.32.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -50,7 +50,7 @@ ApproxManifoldProducts = "0.6"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
-DistributedFactorGraphs = "0.18.4"
+DistributedFactorGraphs = "0.18.10"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"

src/entities/FactorOperationalMemory.jl

@@ -80,21 +80,21 @@ Related
 
 [`CalcFactor`](@ref), [`CalcFactorMahalanobis`](@ref)
 """
-mutable struct CommonConvWrapper{
+struct CommonConvWrapper{
   T <: AbstractFactor, 
+  VT <: Tuple,
   NTP <: Tuple, 
-  G, 
-  MT, 
   CT,
+  AM <: AbstractManifold,
   HP <: Union{Nothing, <:Distributions.Categorical{Float64, Vector{Float64}}},
   CH <: Union{Nothing, Vector{Int}},
-  VT <: Tuple,
-  AM <: AbstractManifold
+  MT, 
+  G
 } <: FactorOperationalMemory
   # Basic factor topological info
   """ Values consistent across all threads during approx convolution """
   usrfnc!::T # user factor / function
-  """ Consolidation from FMD, ordered tuple of all variables connected to this factor """
+  """ Ordered tuple of all variables connected to this factor """
   fullvariables::VT
   # shortcuts to numerical containers
   """ Numerical containers for all connected variables.  Hypo selection needs to be passed 
@@ -109,10 +109,6 @@ mutable struct CommonConvWrapper{
   partialDims::Vector{<:Integer}
   """ is this a partial constraint as defined by the existance of factor field `.partial::Tuple` """
   partial::Bool
-  """ coordinate dimension size of current target variable (see .fullvariables[.varidx]), TODO remove once only use under AbstractRelativeRoots is deprecated or resolved """
-  xDim::Int
-  """ coordinate dimension size of factor, same as manifold_dimension(.manifold) """
-  zDim::Int
   """ probability that this factor is wholly incorrect and should be ignored during solving """
   nullhypo::Float64
   """ inflationSpread particular to this factor (by how much to dispurse the belief initial values before numerical optimization is run).  Analogous to stochastic search """
@@ -131,9 +127,9 @@ mutable struct CommonConvWrapper{
       can be a Vector{<:Tuple} or more direct Vector{<: pointortangenttype} """
   measurement::Vector{MT}
   """ which index is being solved for in params? """
-  varidx::Int
+  varidx::Base.RefValue{Int}
   """ Consolidation from CPT, the actual particle being solved at this moment """
-  particleidx::Int
+  particleidx::Base.RefValue{Int}
   """ working memory to store residual for optimization routines """
   res::Vector{Float64}
   """ experimental feature to embed gradient calcs with ccw """

src/services/CalcFactor.jl

@@ -7,6 +7,8 @@ getFactorOperationalMemoryType(dfg::SolverParams) = CommonConvWrapper
 # difficult type piracy case needing both types NoSolverParams and CommonConvWrapper.
 getFactorOperationalMemoryType(dfg::NoSolverParams) = CommonConvWrapper
 
+getManifold(fct::DFGFactor{<:CommonConvWrapper}) = getManifold(_getCCW(fct))
+
 function _getDimensionsPartial(ccw::CommonConvWrapper)
   # @warn "_getDimensionsPartial not ready for use yet"
   return ccw.partialDims
@@ -28,7 +30,7 @@ function CalcFactor(
   _allowThreads = true,
   cache = ccwl.dummyCache,
   fullvariables = ccwl.fullvariables,
-  solvefor = ccwl.varidx,
+  solvefor = ccwl.varidx[],
   manifold = getManifold(ccwl)
 )
   #
@@ -76,7 +78,7 @@ Notes
 """
 function calcZDim(cf::CalcFactor{T}) where {T <: AbstractFactor}
   #
-  M = cf.manifold # getManifold(T)
+  M = getManifold(cf) # getManifold(T)
   try
     return manifold_dimension(M)
   catch
@@ -191,9 +193,8 @@ function CommonConvWrapper(
   usrfnc::T,
   fullvariables, #::Tuple ::Vector{<:DFGVariable};
   varValsAll::Tuple,
-  X::AbstractVector{P}, #TODO remove X completely
-  zDim::Int;
-  xDim::Int = size(X, 1),
+  X::AbstractVector{P}; #TODO remove X completely
+  # xDim::Int = size(X, 1),
   userCache::CT = nothing,
   manifold = getManifold(usrfnc),
   partialDims::AbstractVector{<:Integer} = 1:length(X),
@@ -206,7 +207,7 @@ function CommonConvWrapper(
   measurement::AbstractVector = Vector(Vector{Float64}()),
   varidx::Int = 1,
   particleidx::Int = 1,
-  res::AbstractVector{<:Real} = zeros(zDim),
+  res::AbstractVector{<:Real} = zeros(manifold_dimension(manifold)), # zDim
   gradients = nothing,
 ) where {T <: AbstractFactor, P, H, CT}
   #
@@ -218,22 +219,23 @@ function CommonConvWrapper(
     manifold,
     partialDims,
     partial,
-    xDim,
-    zDim,
+    # xDim,
     Float64(nullhypo),
     inflation,
     hypotheses,
     certainhypo,
     activehypo,
     measurement,
-    varidx,
-    particleidx,
+    Ref(varidx),
+    Ref(particleidx),
     res,
     gradients,
   )
 end
 
-getManifold(ccwl::CommonConvWrapper) = ccwl.manifold # getManifold(ccwl.usrfnc!)
+# the same as legacy, getManifold(ccwl.usrfnc!)
+getManifold(ccwl::CommonConvWrapper) = ccwl.manifold
+getManifold(cf::CalcFactor) = cf.manifold
 
 function _resizePointsVector!(
   vecP::AbstractVector{P},
@@ -279,10 +281,13 @@ function _prepParamVec(
   #
   # FIXME refactor to new NamedTuple instead
   varParamsAll = getVal.(Xi; solveKey)
-  Xi_labels = getLabel.(Xi)
-  sfidx = findfirst(==(solvefor), Xi_labels)
-
-  sfidx = isnothing(sfidx) ? 0 : sfidx
+  # Xi_labels = getLabel.(Xi)
+  sfidx = if isnothing(solvefor)
+    0
+  else
+    findfirst(==(solvefor), getLabel.(Xi))
+  end
+  # sfidx = isnothing(sfidx) ? 0 : sfidx
 
   # this line does nothing...
   # maxlen = N # FIXME see #105
@@ -292,7 +297,7 @@ function _prepParamVec(
 
   # resample variables with too few kernels (manifolds points)
   SAMP = LEN .< maxlen
-  for i = 1:length(Xi_labels)
+  for i = 1:length(Xi)
     if SAMP[i]
       Pr = getBelief(Xi[i], solveKey)
       _resizePointsVector!(varParamsAll[i], Pr, maxlen)
@@ -317,6 +322,7 @@ Internal method to set which dimensions should be used as the decision variables
 """
 function _setCCWDecisionDimsConv!(
   ccwl::Union{CommonConvWrapper{F}, CommonConvWrapper{Mixture{N_, F, S, T}}},
+  xDim::Int
 ) where {
   N_,
   F <: Union{
@@ -331,12 +337,14 @@ function _setCCWDecisionDimsConv!(
   #
 
   # NOTE should only be done in the constructor
-  ccwl.partialDims = if ccwl.partial
+  newval = if ccwl.partial
     Int[ccwl.usrfnc!.partial...]
   else
     # NOTE this is the target variable dimension (not factor manifold dimension) 
-    Int[1:(ccwl.xDim)...]
+    Int[1:xDim...] # ccwl.xDim
   end
+  resize!(ccwl.partialDims, length(newval))
+  ccwl.partialDims[:] = newval
 
   return nothing
 end
@@ -411,22 +419,19 @@ function _prepCCW(
   end
 
   # TODO check no Anys, see #1321
-  _varValsQuick, maxlen, sfidx = _prepParamVec(Xi, nothing, 0; solveKey) # Nothing for init.
+  _varValsAll, maxlen, sfidx = _prepParamVec(Xi, nothing, 0; solveKey) # Nothing for init.
 
   manifold = getManifold(usrfnc)
   # standard factor metadata
   solvefor = length(Xi)
-  # sflbl = 0 == length(Xi) ? :null : getLabel(Xi[end])
-  # lbs = getLabel.(Xi)
-  # fmd = FactorMetadata(Xi, lbs, _varValsQuick, sflbl, nothing)
   fullvariables = tuple(Xi...) # convert(Vector{DFGVariable}, Xi)
   # create a temporary CalcFactor object for extracting the first sample
   # TODO, deprecate this:  guess measurement points type
   # MeasType = Vector{Float64} # FIXME use `usrfnc` to get this information instead
   _cf = CalcFactor(
     usrfnc,
     0,
-    _varValsQuick,
+    _varValsAll,
     false,
     userCache,
     fullvariables,
@@ -457,7 +462,7 @@ function _prepCCW(
   gradients = attemptGradientPrep(
     varTypes,
     usrfnc,
-    _varValsQuick,
+    _varValsAll,
     multihypo,
     meas_single,
     _blockRecursion,
@@ -466,15 +471,15 @@ function _prepCCW(
   # variable Types
   pttypes = getVariableType.(Xi) .|> getPointType
   PointType = 0 < length(pttypes) ? pttypes[1] : Vector{Float64}
-
-  # @info "CCW" typeof(measurement)
+  if !isconcretetype(PointType)
+    @warn "_prepCCW PointType is not concrete $PointType" maxlog=50
+  end
 
   return CommonConvWrapper(
     usrfnc,
     fullvariables,
-    _varValsQuick,
-    PointType[],
-    calcZDim(_cf);
+    _varValsAll,
+    PointType[];
     userCache, # should be higher in args list
     manifold,  # should be higher in args list
     partialDims,
@@ -501,9 +506,6 @@ function updateMeasurement!(
   if needFreshMeasurements
     # TODO this is only one thread, make this a for loop for multithreaded sampling
     sampleFactor!(ccwl, N; _allowThreads)
-    # TODO use common sampleFactor! call instead
-    # cf = CalcFactor(ccwl; _allowThreads)
-    # ccwl.measurement = sampleFactor(cf, N)
   elseif 0 < length(measurement) 
     resize!(ccwl.measurement, length(measurement))
     ccwl.measurement[:] = measurement
@@ -541,40 +543,38 @@ function _updateCCW!(
 
   # FIXME, order of fmd ccwl cf are a little weird and should be revised.
   # FIXME maxlen should parrot N (barring multi-/nullhypo issues)
-  _varValsQuick, maxlen, sfidx = _prepParamVec(Xi, solvefor, N; solveKey)
+  _varValsAll, maxlen, sfidx = _prepParamVec(Xi, solvefor, N; solveKey)
+
+  # TODO, ensure all values (not just multihypothesis) is correctly used from here
+  for (i,varVal) in enumerate(_varValsAll)
+    resize!(ccwl.varValsAll[i],length(varVal))
+    ccwl.varValsAll[i][:] = varVal
+  end
+
+  # set the 'solvefor' variable index -- i.e. which connected variable of the factor is being computed in this convolution. 
+  ccwl.varidx[] = sfidx
 
-  # NOTE should be selecting for the correct multihypothesis mode
-  ccwl.varValsAll = _varValsQuick
   # TODO better consolidation still possible
   # FIXME ON FIRE, what happens if this is a partial dimension factor?  See #1246
   # FIXME, confirm this is hypo sensitive selection from Xi, better to use double indexing for clarity getDimension(ccw.fullvariables[hypoidx[sfidx]])
-  ccwl.xDim = getDimension(getVariableType(Xi[sfidx]))
-  # TODO maybe refactor new type higher up?
+  xDim = getDimension(getVariableType(Xi[sfidx])) # ccwl.varidx[]
+  # ccwl.xDim = xDim
+  # TODO maybe refactor different type or api call?
 
   # 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
   # TODO, should this not be part of `prepareCommonConvWrapper` -- only here do we look for .partial
-  _setCCWDecisionDimsConv!(ccwl)
-
-  # set the 'solvefor' variable index -- i.e. which connected variable of the factor is being computed in this convolution. 
-  ccwl.varidx = sfidx
+  _setCCWDecisionDimsConv!(ccwl, xDim)
 
   # FIXME do not divert Mixture for sampling
 
-    # TODO remove ccwl.zDim updating
-    # cache the measurement dimension
-    cf = CalcFactor(ccwl; _allowThreads = true)
-    @assert ccwl.zDim == calcZDim(cf) "refactoring in progress, cannot drop assignment ccwl.zDim:$(ccwl.zDim) = calcZDim( cf ):$(calcZDim( cf ))"
-    # ccwl.zDim = calcZDim( cf )  # CalcFactor(ccwl) )
-
   updateMeasurement!(ccwl, maxlen; needFreshMeasurements, measurement, _allowThreads=true)
 
-  # set each CPT
   # used in ccw functor for AbstractRelativeMinimize
-  resize!(ccwl.res, ccwl.zDim)
+  resize!(ccwl.res, _getZDim(ccwl))
   fill!(ccwl.res, 0.0)
 
-  # calculate new gradients perhaps
+  # calculate new gradients
   # J = ccwl.gradients(measurement..., pts...)
 
   return sfidx, maxlen
@@ -590,14 +590,16 @@ function _updateCCW!(
   needFreshMeasurements::Bool = true,
   solveKey::Symbol = :default,
 ) where {F <: AbstractFactor} # F might be Mixture
-  # 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)
-
   # FIXME, NEEDS TO BE CLEANED UP AND WORK ON MANIFOLDS PROPER
   # fnc = ccwl.usrfnc!
   sfidx = findfirst(getLabel.(Xi) .== solvefor)
+  @assert sfidx == 1 "Solving on Prior with CCW should have sfidx=1, priors are unary factors."
   # sfidx = 1 #  why hardcoded to 1, maybe for only the AbstractPrior case here
+
+  # 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(getVariableType(Xi[sfidx])))
+
   solveForPts = getVal(Xi[sfidx]; solveKey)
   maxlen = maximum([N; length(solveForPts); length(ccwl.varValsAll[sfidx])])  # calcZDim(ccwl); length(measurement[1])
 

src/services/DeconvUtils.jl

@@ -37,8 +37,10 @@ function approxDeconv(
   retries::Int = 3,
 )
   #
+  # FIXME needs xDim for all variables at once? xDim = 0 likely to break?
+
   # but what if this is a partial factor -- is that important for general cases in deconv?
-  _setCCWDecisionDimsConv!(ccw)
+  _setCCWDecisionDimsConv!(ccw, 0) # ccwl.xDim used to hold the last forward solve getDimension(getVariableType(Xi[sfidx]))
 
   # FIXME This does not incorporate multihypo??
   varsyms = getVariableOrder(fcto)
@@ -74,7 +76,8 @@ function approxDeconv(
     targeti_ = makeTarget(idx)
 
     # TODO must first resolve hypothesis selection before unrolling them -- deferred #1096
-    ccw.activehypo = hyporecipe.activehypo[2][2]
+    resize!(ccw.activehypo, length(hyporecipe.activehypo[2][2]))
+    ccw.activehypo[:] = hyporecipe.activehypo[2][2]
 
     onehypo!, _ = _buildCalcFactorLambdaSample(ccw, idx, targeti_, measurement)
     #
@@ -84,7 +87,7 @@ function approxDeconv(
 
     # find solution via SubArray view pointing to original memory location
     if fcttype isa AbstractManifoldMinimize
-      sfidx = ccw.varidx
+      sfidx = ccw.varidx[]
       targeti_ .= _solveLambdaNumericMeas(
         fcttype,
         hypoObj,