Restore _slack in mumerical calcs (#1790)

JuliaRobotics · Oct 19, 2023 · 4bcc7b4 · 4bcc7b4
1 parent 0757330
commit 4bcc7b4
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 6 deletions.
diff --git a/src/Factors/Mixture.jl b/src/Factors/Mixture.jl
@@ -127,7 +127,8 @@ function sampleFactor(cf::CalcFactor{<:Mixture}, N::Int = 1)
     cf.fullvariables,
     cf.solvefor,
     cf.manifold,
-    cf.measurement
+    cf.measurement,
+    nothing,
   )
   smpls = [getSample(cf_) for _ = 1:N]
   # smpls = Array{Float64,2}(undef,s.dims,N)

diff --git a/src/entities/CalcFactor.jl b/src/entities/CalcFactor.jl
@@ -36,7 +36,8 @@ struct CalcFactorNormSq{
   C, 
   VT <: Tuple, 
   M <: AbstractManifold,
-  MEAS
+  MEAS,
+  S
 } <: CalcFactor{FT}
   """ the interface compliant user object functor containing the data and logic """
   factor::FT
@@ -58,6 +59,7 @@ struct CalcFactorNormSq{
   solvefor::Int
   manifold::M
   measurement::MEAS
+  slack::S
 end
 
 #TODO deprecate after CalcFactor is updated to CalcFactorNormSq

diff --git a/src/services/CalcFactor.jl b/src/services/CalcFactor.jl
@@ -31,7 +31,8 @@ function CalcFactorNormSq(
   cache = ccwl.dummyCache,
   fullvariables = ccwl.fullvariables,
   solvefor = ccwl.varidx[],
-  manifold = getManifold(ccwl)
+  manifold = getManifold(ccwl),
+  slack=nothing,
 )
   #
   # FIXME using ccwl.dummyCache is not thread-safe
@@ -45,6 +46,7 @@ function CalcFactorNormSq(
     solvefor,
     manifold,
     ccwl.measurement,
+    slack
   )
 end
 
@@ -407,6 +409,7 @@ function _createCCW(
     solvefor,
     manifold,
     nothing,
+    nothing,
   )
 
   # get a measurement sample
@@ -426,6 +429,7 @@ function _createCCW(
     solvefor,
     manifold,
     measurement,
+    nothing,
   )
 
 

diff --git a/src/services/NumericalCalculations.jl b/src/services/NumericalCalculations.jl
@@ -192,6 +192,7 @@ function _buildCalcFactor(
   smpid,
   varParams,
   activehypo,
+  _slack = nothing,
 )
   #
   # FIXME, make thread safe (cache)
@@ -210,6 +211,7 @@ function _buildCalcFactor(
     solveforidx,                #solvefor
     getManifold(ccwl),           #manifold
     ccwl.measurement,
+    _slack,
   )
 end
 
@@ -391,10 +393,11 @@ function (cf::CalcFactorNormSq)(::Type{CalcConv}, x)
   varValsHypo[cf.solvefor][sampleIdx] = x
 
   res = cf(cf.measurement[sampleIdx], map(vvh -> _getindex_anyn(vvh, sampleIdx), varValsHypo)...)
+  res = isnothing(cf.slack) ? res : res .- cf.slack
   return sum(x->x^2, res)
 end
 
-function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer)
+function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer, _slack)
   # build a view to the decision variable memory
   varValsHypo = ccwl.varValsAll[][ccwl.hyporecipe.activehypo]
   # create calc factor selected hypo and samples
@@ -403,7 +406,8 @@ function _buildHypoCalcFactor(ccwl::CommonConvWrapper, smpid::Integer)
     ccwl,                        #
     smpid,                       # ends in _sampleIdx
     varValsHypo,                 # ends in _legacyParams
-    ccwl.hyporecipe.activehypo   # ends in solvefor::Int
+    ccwl.hyporecipe.activehypo,   # ends in solvefor::Int
+    _slack,
   )
   return cf
 end
@@ -427,7 +431,7 @@ function _solveCCWNumeric!(
   # target = view(ccwl.varValsAll[][ccwl.varidx[]], smpid) 
 
   # SUPER IMPORTANT ON PARTIALS, RESIDUAL FUNCTION MUST DEAL WITH PARTIAL AND WILL GET FULL VARIABLE POINTS REGARDLESS
-  _hypoCalcFactor = _buildHypoCalcFactor(ccwl, smpid)
+  _hypoCalcFactor = _buildHypoCalcFactor(ccwl, smpid, _slack)
 
   # do the parameter search over defined decision variables using Minimization
   sfidx = ccwl.varidx[]