diff --git a/src/Factors/Mixture.jl b/src/Factors/Mixture.jl index 0941f311..0351011a 100644 --- 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 index 192d4f49..a821f279 100644 --- 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 index 45cada7e..8cccca2c 100644 --- 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 index 56eacca0..88d535ae 100644 --- 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[]