Use CalcFactorNormSq as functor

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/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