consolidate ccw.meas factor sampling

NEWS.md

@@ -17,6 +17,8 @@ The list below highlights breaking changes according to normal semver workflow -
 - Internal refactoring removing several legacy fields from `CalcFactor`.
 - All factors now require a `getManifold` definition.
 - 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`.
 
 # Changes in v0.31
 - `FactorMetaData` is deprecated and replaced by `CalcFactor`.

src/services/CalcFactor.jl

@@ -488,6 +488,30 @@ function _prepCCW(
   )
 end
 
+function updateMeasurement!(
+  ccwl::CommonConvWrapper,
+  N::Int=1;
+  measurement::AbstractVector = Vector{Tuple{}}(),
+  needFreshMeasurements::Bool=true,
+  _allowThreads::Bool = true
+)
+  # FIXME do not divert Mixture for sampling
+
+  # option to disable fresh samples or user provided
+  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
+  end
+
+  nothing
+end
+
 """
     $(SIGNATURES)
 
@@ -504,10 +528,10 @@ function _updateCCW!(
   Xi::AbstractVector{<:DFGVariable},
   solvefor::Symbol,
   N::Integer;
-  measurement = [],
+  measurement = Vector{Tuple{}}(),
   needFreshMeasurements::Bool = true,
   solveKey::Symbol = :default,
-) where {F <: AbstractFactor}
+) where {F <: AbstractFactor} # F might be Mixture
   #
   if length(Xi) !== 0
     nothing
@@ -521,8 +545,7 @@ function _updateCCW!(
 
   # NOTE should be selecting for the correct multihypothesis mode
   ccwl.varValsAll = _varValsQuick
-  # some better consolidate is needed
-  # ccwl.vartypes = varTypes
+  # 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]))
@@ -536,22 +559,15 @@ function _updateCCW!(
   # set the 'solvefor' variable index -- i.e. which connected variable of the factor is being computed in this convolution. 
   ccwl.varidx = sfidx
 
-  # get factor metadata -- TODO, populate, also see #784
-  # TODO consolidate with ccwl??
   # FIXME do not divert Mixture for sampling
-  cf = CalcFactor(ccwl; _allowThreads = true)
 
-  # cache the measurement dimension
-  @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) )
+    # 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) )
 
-  # option to disable fresh samples
-  if needFreshMeasurements
-    # TODO refactor
-    ccwl.measurement = sampleFactor(cf, maxlen)
-  elseif 0 < length(measurement) 
-    ccwl.measurement = measurement
-  end
+  updateMeasurement!(ccwl, maxlen; needFreshMeasurements, measurement, _allowThreads=true)
 
   # set each CPT
   # used in ccw functor for AbstractRelativeMinimize
@@ -564,6 +580,35 @@ function _updateCCW!(
   return sfidx, maxlen
 end
 
+function _updateCCW!(
+  F_::Type{<:AbstractPrior},
+  ccwl::CommonConvWrapper{F},
+  Xi::AbstractVector{<:DFGVariable},
+  solvefor::Symbol,
+  N::Integer;
+  measurement = Vector{Tuple{}}(),
+  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)
+  # sfidx = 1 #  why hardcoded to 1, maybe for only the AbstractPrior case here
+  solveForPts = getVal(Xi[sfidx]; solveKey)
+  maxlen = maximum([N; length(solveForPts); length(ccwl.varValsAll[sfidx])])  # calcZDim(ccwl); length(measurement[1])
+
+  # FIXME do not divert Mixture for sampling
+  # update ccwl.measurement values
+  updateMeasurement!(ccwl, maxlen; needFreshMeasurements, measurement, _allowThreads=true)
+
+  return sfidx, maxlen
+end
+
+# TODO, can likely deprecate this
 function _updateCCW!(
   ccwl::Union{CommonConvWrapper{F}, CommonConvWrapper{Mixture{N_, F, S, T}}},
   Xi::AbstractVector{<:DFGVariable},
@@ -575,4 +620,16 @@ function _updateCCW!(
   return _updateCCW!(F, ccwl, Xi, solvefor, N; kw...)
 end
 
+function _updateCCW!(
+  ccwl::Union{CommonConvWrapper{F}, CommonConvWrapper{Mixture{N_, F, S, T}}},
+  Xi::AbstractVector{<:DFGVariable},
+  solvefor::Symbol,
+  N::Integer;
+  kw...,
+) where {N_, F <: AbstractPrior, S, T}
+  #
+  return _updateCCW!(F, ccwl, Xi, solvefor, N; kw...)
+end
+
+
 #

src/services/EvalFactor.jl

@@ -333,7 +333,7 @@ function evalPotentialSpecific(
   Xi::AbstractVector{<:DFGVariable},
   ccwl::CommonConvWrapper{T},
   solvefor::Symbol,
-  T_::Type{<:AbstractRelative},
+  T_::Type{<:AbstractRelative},          # NOTE Relative
   measurement::AbstractVector = Tuple[]; # TODO make this a concrete type
   needFreshMeasurements::Bool = true,    # superceeds over measurement
   solveKey::Symbol = :default,
@@ -348,13 +348,8 @@ function evalPotentialSpecific(
   #
 
   # Prep computation variables
-  # NOTE #1025, should FMD be built here...
   # add user desired measurement values if 0 < length
   sfidx, maxlen = _updateCCW!(ccwl, Xi, solvefor, N; solveKey, needFreshMeasurements, measurement)
-  # if 0 < length(measurement)
-  #   # @info "HERE" typeof(ccwl.measurement) typeof(measurement)
-  #   ccwl.measurement = measurement
-  # end
 
   # Check which variables have been initialized
   isinit = map(x -> isInitialized(x), Xi)
@@ -368,7 +363,7 @@ function evalPotentialSpecific(
   # get manifold add operations
   # TODO, make better use of dispatch, see JuliaRobotics/RoME.jl#244
   # addOps, d1, d2, d3 = buildHybridManifoldCallbacks(manis)
-  mani = getManifold(getVariableType(Xi[sfidx]))
+  mani = getManifold(Xi[sfidx])
 
   # perform the numeric solutions on the indicated elements
   # FIXME consider repeat solve as workaround for inflation off-zero 
@@ -407,7 +402,7 @@ function evalPotentialSpecific(
   Xi::AbstractVector{<:DFGVariable},
   ccwl::CommonConvWrapper{T},
   solvefor::Symbol,
-  T_::Type{<:AbstractPrior},
+  T_::Type{<:AbstractPrior},             # NOTE Prior
   measurement::AbstractVector = Tuple[];
   needFreshMeasurements::Bool = true,
   solveKey::Symbol = :default,
@@ -420,32 +415,21 @@ function evalPotentialSpecific(
   _slack = nothing,
 ) where {T <: AbstractFactor}
   #
-  # 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)
+
+  # Prep computation variables
+  sfidx, maxlen = _updateCCW!(ccwl, Xi, solvefor, N; solveKey, needFreshMeasurements, measurement)
 
-  # FIXME, NEEDS TO BE CLEANED UP AND WORK ON MANIFOLDS PROPER
+  # # FIXME, NEEDS TO BE CLEANED UP AND WORK ON MANIFOLDS PROPER
   fnc = ccwl.usrfnc!
-  sfidx = findfirst(getLabel.(Xi) .== solvefor)
-  # sfidx = 1 #  WHY HARDCODED TO 1??
-  solveForPts = getVal(Xi[sfidx]; solveKey = solveKey)
-  nn = maximum([N; calcZDim(ccwl); length(solveForPts); length(ccwl.varValsAll[sfidx])])  # length(measurement[1])
-
-  # FIXME better standardize in-place operations (considering solveKey)
-  if needFreshMeasurements
-    cf = CalcFactor(ccwl)
-    # NOTE, sample factor is expected to return tangents=>relative or points=>prior
-    newMeas = sampleFactor(cf, nn)
-    ccwl.measurement = newMeas
-  end
+  solveForPts = getVal(Xi[sfidx]; solveKey)
 
   # Check which variables have been initialized
   # TODO not sure why forcing to Bool vs BitVector
   isinit::Vector{Bool} = Xi .|> isInitialized .|> Bool
   # nullSurplus see #1517
   runnullhypo = maximum((ccwl.nullhypo, nullSurplus))
   hyporecipe =
-    _prepareHypoRecipe!(ccwl.hypotheses, nn, sfidx, length(Xi), isinit, runnullhypo)
+    _prepareHypoRecipe!(ccwl.hypotheses, maxlen, sfidx, length(Xi), isinit, runnullhypo)
 
   # get solvefor manifolds, FIXME ON FIRE, upgrade to new Manifolds.jl
   mani = getManifold(Xi[sfidx])
@@ -456,14 +440,14 @@ function evalPotentialSpecific(
   # inject lots of entropy in nullhypo case
   # make spread (1σ) equal to mean distance of other fractionals
   # FIXME better standardize in-place operations (considering solveKey)
-  addEntr = if length(solveForPts) == nn
+  addEntr = if length(solveForPts) == maxlen
     deepcopy(solveForPts)
   else
-    ret = typeof(solveForPts)(undef, nn)
+    ret = typeof(solveForPts)(undef, maxlen)
     for i = 1:length(solveForPts)
       ret[i] = solveForPts[i]
     end
-    for i = (length(solveForPts) + 1):nn
+    for i = (length(solveForPts) + 1):maxlen
       ret[i] = getPointIdentity(getVariableType(Xi[sfidx]))
     end
     ret

src/services/FactorGradients.jl

@@ -159,7 +159,6 @@ function _setFGCSlack!(
 end
 
 function (fgc::FactorGradientsCached!)(meas_pts...)
-  # @warn "YELLING TIMBER1"
 
   # separate the measurements (forst) from the variable points (rest)
   lenm = 1# fgc.measurement is a single measurement so length is always 1
@@ -171,9 +170,8 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
   # for (m, tup_m) in enumerate(fgc.measurement)
   # setPointsMani!(tup_m, meas_pts[m])
   # end
-  fgc.measurement = meas_pts[1]
+  fgc.measurement = meas_pts[1] # why not 1:1 since ccwl.measurement::Vector{typeof(z)}
 
-  # @warn "YELLING TIMBER2"
   # update the residual _slack in preparation for new gradient calculation
   fct = getFactorType(fgc.dfgfct)
   measurement = meas_pts[1]
@@ -190,7 +188,6 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
     # update the local memory in fgc to take the values of incoming `pts`
     setPointsMani!(fgc.currentPoints[s], pt)
   end
-  # @warn "YELLING TIMBER3" fgc.measurement meas_pts
   println.(fgc.currentPoints)
 
   # update the gradients at new values contained in fgc
@@ -209,7 +206,6 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
     # recalculate the off diagonals
     λ()
   end
-  # @warn "YELLING TIMBER4"
 
   # return newly calculated gradients
   return fgc.cached_gradients

src/services/SolverUtilities.jl

@@ -46,29 +46,31 @@ function mmd(
 end
 
 # part of consolidation, see #927
-function sampleFactor!(ccwl::CommonConvWrapper, N::Int)
+function sampleFactor!(ccwl::CommonConvWrapper, N::Int; _allowThreads::Bool=true)
   #
 
-  # # TODO make this an in-place operation as far possible
-  # # build a CalcFactor object and get fresh samples.
-  # cf = CalcFactor(ccwl) 
-  # ccwl.measurement = sampleFactor(cf, N)    
-  ccwl.measurement = sampleFactor(ccwl, N)
-
-  return nothing
+  # FIXME get allocations here down to 0
+    # TODO make this an in-place operation as far possible
+  # TODO make this a multithreaded sampling function
+  # build a CalcFactor object and get fresh samples.
+  # cf = CalcFactor(ccwl; _allowThreads) 
+  resize!(ccwl.measurement, N)
+  ccwl.measurement[:] = sampleFactor(ccwl, N; _allowThreads)
+
+  return ccwl.measurement
 end
 
-function sampleFactor(ccwl::CommonConvWrapper, N::Int)
+function sampleFactor(ccwl::CommonConvWrapper, N::Int; _allowThreads::Bool=true)
   #
-  cf = CalcFactor(ccwl) 
+  cf = CalcFactor(ccwl; _allowThreads) 
   return sampleFactor(cf, N)
 end
 
-sampleFactor(fct::DFGFactor, N::Int = 1) = sampleFactor(_getCCW(fct), N)
+sampleFactor(fct::DFGFactor, N::Int = 1; _allowThreads::Bool=true) = sampleFactor(_getCCW(fct), N; _allowThreads)
 
-function sampleFactor(dfg::AbstractDFG, sym::Symbol, N::Int = 1)
+function sampleFactor(dfg::AbstractDFG, sym::Symbol, N::Int = 1; _allowThreads::Bool=true)
   #
-  return sampleFactor(getFactor(dfg, sym), N)
+  return sampleFactor(getFactor(dfg, sym), N; _allowThreads)
 end
 
 """
@@ -152,7 +154,7 @@ function _buildGraphByFactorAndTypes!(
   end
   # if newFactor then add the factor on vars, else assume only one existing factor between vars
   _dfgfct = if newFactor
-    addFactor!(dfg, vars, fct; graphinit = graphinit, _blockRecursion = _blockRecursion)
+    addFactor!(dfg, vars, fct; graphinit, _blockRecursion)
   else
     getFactor(dfg, intersect((ls.(dfg, vars))...)[1])
   end

test/runtests.jl

@@ -5,8 +5,7 @@ TEST_GROUP = get(ENV, "IIF_TEST_GROUP", "all")
 # temporarily moved to start (for debugging)
 #...
 if TEST_GROUP in ["all", "tmp_debug_group"]
-@error("Must restore testMultiHypo3Door.jl")
-# include("testMultiHypo3Door.jl")
+include("testMultiHypo3Door.jl")
 include("priorusetest.jl")
 end
 

test/testMixturePrior.jl

@@ -9,7 +9,6 @@ using TensorCast
 ##
 
 @testset "test mixture prior" begin
-
 ##
 
 # init graph
@@ -64,12 +63,10 @@ marginalPts_ = getBelief(fg, :x0) |> getPoints
 @test sum(marginalPts .< -2.5) - sum(-2.5 .< marginalPts) |> abs < 0.35*N
 
 ##
-
 end
 
 
 @testset "Serialization of Mixture(Prior,..) including a AliasingScalarSampler" begin
-
 ##
 
 fg_ = loadDFG("/tmp/test_fg_bss")
@@ -92,7 +89,6 @@ marginalPts_ = getBelief(fg_, :x0) |> getPoints
 Base.rm("/tmp/test_fg_bss.tar.gz")
 
 ##
-
 end