Deconv using CalcFactor dispatch (#1789)

Author: Affie

src/entities/CalcFactor.jl

@@ -58,7 +58,7 @@ struct CalcFactorNormSq{
   # which index is being solved for?
   solvefor::Int
   manifold::M
-  measurement::MEAS
+  measurement::MEAS #TBD make measurement only one sample per calc factor
   slack::S
 end
 

src/services/DeconvUtils.jl

@@ -92,17 +92,7 @@ function approxDeconv(
 
     # find solution via SubArray view pointing to original memory location
     if fcttype isa AbstractManifoldMinimize
-      sfidx = ccw.varidx[]
-      ts = _solveLambdaNumericMeas(
-        fcttype,
-        hypoObj,
-        res_,
-        measurement[idx],
-        getVariableType(ccw.fullvariables[sfidx]), # ccw.vartypes[sfidx](),
-        islen1,
-      )
-      # copyto!(target_smpl, ts)
-      measurement[idx] = ts
+      error("Fix dispatch on AbstractManifoldMinimize")
     else
       ts = _solveLambdaNumeric(fcttype, hypoObj, res_, measurement[idx], islen1)
       measurement[idx] = ts
@@ -115,6 +105,62 @@ function approxDeconv(
   return measurement, fctSmpls
 end
 
+# TBD deprecate use of xDim
+function approxDeconv(
+  fcto::DFGFactor{<:CommonConvWrapper{<:AbstractManifoldMinimize}},
+  ccw::CommonConvWrapper = _getCCW(fcto);
+  N::Int = 100,
+  measurement::AbstractVector = sampleFactor(ccw, N),
+  retries=nothing,
+)
+  if !isnothing(retries)
+    Base.depwarn(
+      "approxDeconv kwarg retries is not used",
+      :approxDeconv,
+    )
+  end
+  # but what if this is a partial factor -- is that important for general cases in deconv?
+  _setCCWDecisionDimsConv!(ccw, 0)
+
+  varsyms = getVariableOrder(fcto)
+  
+  # TODO assuming vector on only first container in measurement::Tuple  # TBD How should user dispatch fancy tuple measurements on deconv.
+  
+  # NOTE 
+  # build a lambda that incorporates the multihypo selections
+  # deconv has to solve for the best matching for particles
+  # FIXME This does not incorporate multihypo, Apply hyporecipe to full variable order list. But remember hyporecipe assignment must be found (NPhard)
+  hyporecipe = _prepareHypoRecipe!(nothing, N, 0, length(varsyms))
+  # only doing the current active hypo
+  @assert hyporecipe.activehypo[2][1] == 1 "deconv was expecting hypothesis nr == (1, 1:d)"
+
+  # get measurement dimension
+  zDim = _getZDim(fcto)
+  islen1 = zDim == 1
+
+  #make a copy of the original measurement before mutating it
+  sampled_meas = deepcopy(measurement)
+
+  fcttype = getFactorType(fcto)
+  
+  for idx = 1:N
+
+    # TODO must first resolve hypothesis selection before unrolling them -- deferred #1096
+    resize!(ccw.hyporecipe.activehypo, length(hyporecipe.activehypo[2][2]))
+    ccw.hyporecipe.activehypo[:] = hyporecipe.activehypo[2][2]
+    #TODO why is this resize in the loop?
+
+    # Create a CalcFactor functor of the correct hypo.
+    _hypoCalcFactor = _buildHypoCalcFactor(ccw, idx)
+
+    ts = _solveLambdaNumericMeas(fcttype, _hypoCalcFactor, measurement[idx], islen1)
+    measurement[idx] = ts
+
+  end
+
+  return measurement, sampled_meas
+end
+
 """
     $SIGNATURES
 

src/services/NumericalCalculations.jl

@@ -87,15 +87,6 @@ function (hypoCalcFactor::CalcFactorNormSq)(::Type{ManoptCalcConv}, M::AbstractM
   return hypoCalcFactor(CalcConv, p)
 end
 
-#TODO untested and unused 
-# for deconv with the measurement a tangent vector
-# function (hypoCalcFactor::CalcFactorNormSq)(M::AbstractManifold, Xc::AbstractVector)
-#   # M = hypoCalcFactor.manifold # calc factor has factor manifold in not variable that is needed here
-#   ϵ = getPointIdentity(M)
-#   X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
-#   return hypoCalcFactor(CalcDeconv, X)
-# end
-
 function _solveLambdaNumeric(
   fcttype::Union{F, <:Mixture{N_, F, S, T}},
   hypoCalcFactor,
@@ -109,7 +100,6 @@ function _solveLambdaNumeric(
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
   # 
   #TODO this is not general to all manifolds, should work for lie groups.
-  # ϵ = identity_element(M, u0)
   ϵ = getPointIdentity(variableType)
 
   X0c = zero(MVector{getDimension(M),Float64})
@@ -142,34 +132,43 @@ function _solveLambdaNumeric(
   return exp(M, ϵ, hat(M, ϵ, r.minimizer))
 end
 
+## deconvolution with calcfactor wip
+struct CalcDeconv end
+
+function (cf::CalcFactorNormSq)(::Type{CalcDeconv}, meas) 
+  res = cf(meas, map(vvh -> _getindex_anyn(vvh, cf._sampleIdx), cf._legacyParams)...)
+  return sum(x->x^2, res)
+end
+
+# for deconv with the measurement a tangent vector, can dispatch for other measurement types.
+function (hypoCalcFactor::CalcFactorNormSq)(::Type{CalcDeconv}, M::AbstractManifold, Xc::AbstractVector)
+  ϵ = getPointIdentity(M)
+  X = get_vector(M, ϵ, Xc, DefaultOrthogonalBasis())
+  return hypoCalcFactor(CalcDeconv, X)
+end
 
-#TODO Consolidate with _solveLambdaNumeric, see #1374
-#TODO _solveLambdaNumericMeas assumes a measurement is always a tangent vector, confirm.
+# NOTE Optim.jl version that assumes measurement is on the tangent
+# TODO test / dev for n-ary factor deconv
+# TODO Consolidate with _solveLambdaNumeric, see #1374
 function _solveLambdaNumericMeas(
   fcttype::Union{F, <:Mixture{N_, F, S, T}},
-  objResX::Function,
-  residual::AbstractVector{<:Real},
+  hypoCalcFactor,
   X0,#::AbstractVector{<:Real},
-  variableType::InferenceVariable,
   islen1::Bool = false,
 ) where {N_, F <: AbstractManifoldMinimize, S, T}
   #
-  # Assume measurement is on the tangent
   M = getManifold(fcttype)
-  # the variable is a manifold point, we are working on the tangent plane in optim for now.
   ϵ = getPointIdentity(M)
   X0c = zeros(manifold_dimension(M))
   X0c .= vee(M, ϵ, X0)
 
-  function cost(Xc)
-    X = hat(M, ϵ, Xc)
-    residual = objResX(X)
-    return sum(residual .^ 2)
-  end
-
   alg = islen1 ? Optim.BFGS() : Optim.NelderMead()
 
-  r = Optim.optimize(cost, X0c, alg)
+  r = Optim.optimize(
+    x->hypoCalcFactor(CalcDeconv, M, x),
+    X0c,
+    alg
+  )
   if !Optim.converged(r)
     @debug "Optim did not converge:" r
   end
@@ -374,7 +373,6 @@ end
 #
 
 struct CalcConv end
-struct CalcDeconv end
 
 _getindex_anyn(vec, n) = begin
   len = length(vec)
@@ -397,7 +395,7 @@ function (cf::CalcFactorNormSq)(::Type{CalcConv}, x)
   return sum(x->x^2, res)
 end
 
-function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer, _slack)
+function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer, _slack=nothing)
   # build a view to the decision variable memory
   varValsHypo = ccwl.varValsAll[][ccwl.hyporecipe.activehypo]
   # create calc factor selected hypo and samples

test/testSpecialEuclidean2Mani.jl

@@ -257,7 +257,6 @@ m_θ = map(x->x.x[2][2], meas)
 @test isapprox(mean(p_t), mean(m_t), atol=0.3)
 @test isapprox(std(p_t), std(m_t), atol=0.3)
 
-##
 end