Use CalcFactorNormSq as functor (#1786)

* Use CalcFactorNormSq as functor
* Force svector for performance
* Fix deconv manifold and init

NOTE #1786 (comment)

Author: Affie

src/Factors/Mixture.jl

@@ -126,7 +126,8 @@ function sampleFactor(cf::CalcFactor{<:Mixture}, N::Int = 1)
     cf.cache,
     cf.fullvariables,
     cf.solvefor,
-    cf.manifold
+    cf.manifold,
+    cf.measurement
   )
   smpls = [getSample(cf_) for _ = 1:N]
   # smpls = Array{Float64,2}(undef,s.dims,N)

src/entities/CalcFactor.jl

@@ -35,15 +35,16 @@ struct CalcFactorNormSq{
   X, 
   C, 
   VT <: Tuple, 
-  M <: AbstractManifold
+  M <: AbstractManifold,
+  MEAS
 } <: CalcFactor{FT}
   """ the interface compliant user object functor containing the data and logic """
   factor::FT
   """ what is the sample (particle) id for which the residual is being calculated """
   _sampleIdx::Int
   """ legacy support for variable values old functor residual functions.
       TBD, this is still being used by DERelative factors. """
-  _legacyParams::X
+  _legacyParams::X #TODO rename to varValsHypo for consistent naming? and not not legacy any more
   """ allow threading for either sampling or residual calculations (workaround for thread yield issue) """
   _allowThreads::Bool
   """ user cache of arbitrary type, overload the [`preambleCache`](@ref) function. NOT YET THREADSAFE """
@@ -56,6 +57,7 @@ struct CalcFactorNormSq{
   # which index is being solved for?
   solvefor::Int
   manifold::M
+  measurement::MEAS
 end
 
 #TODO deprecate after CalcFactor is updated to CalcFactorNormSq

src/services/CalcFactor.jl

@@ -43,7 +43,8 @@ function CalcFactorNormSq(
     cache,
     tuple(fullvariables...),
     solvefor,
-    manifold
+    manifold,
+    ccwl.measurement,
   )
 end
 
@@ -395,8 +396,7 @@ function _createCCW(
   solvefor = length(Xi)
   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 = CalcFactorNormSq(
     usrfnc,
     1,
@@ -405,17 +405,30 @@ function _createCCW(
     userCache,
     fullvariables,
     solvefor,
-    manifold
+    manifold,
+    nothing,
   )
 
   # get a measurement sample
   meas_single = sampleFactor(_cf, 1)[1]
-
   elT = typeof(meas_single)
-
   #TODO preallocate measurement?
   measurement = Vector{elT}()
 
+  #FIXME chicken and egg problem for getting measurement type, so creating twice.
+  _cf = CalcFactorNormSq(
+    usrfnc,
+    1,
+    _varValsAll,
+    false,
+    userCache,
+    fullvariables,
+    solvefor,
+    manifold,
+    measurement,
+  )
+
+
   # partialDims are sensitive to both which solvefor variable index and whether the factor is partial
   partial = hasfield(T, :partial) # FIXME, use isPartial function instead
   partialDims = if partial

src/services/DeconvUtils.jl

@@ -71,9 +71,6 @@ function approxDeconv(
 
   islen1 = zDim == 1
 
-  # FIXME, is this still in use
-  destVarVals = Vector{Int}()
-
   for idx = 1:N
     # towards each particle in their own thread (not 100% ready yet, factors should be separate memory)
     target_smpl = makeTarget(idx)
@@ -82,7 +79,7 @@ function approxDeconv(
     resize!(ccw.hyporecipe.activehypo, length(hyporecipe.activehypo[2][2]))
     ccw.hyporecipe.activehypo[:] = hyporecipe.activehypo[2][2]
 
-    onehypo!, _ = _buildCalcFactorLambdaSample(ccw, idx, target_smpl, measurement)
+    onehypo! = _buildCalcFactorLambdaSample(ccw, idx, measurement)
     #
 
     # lambda with which to find best measurement values

src/services/EvalFactor.jl

@@ -82,7 +82,7 @@ function calcVariableDistanceExpectedFractional(
   # also check distance to certainidx for general scale reference (workaround heuristic)
   for cidx in certainidx
     count += 1
-    cerMeanPnt = mean(getManifold(varTypes[cidx]), ccwl.varValsAll[][cidx])
+    cerMeanPnt = mean(getManifold(varTypes[cidx]), ccwl.varValsAll[][cidx], GeodesicInterpolation())
     cerMean = getCoordinates(varTypes[cidx], cerMeanPnt)
     dists[count] = norm(refMean[1:dims] - cerMean[1:dims])
   end