Merge pull request #1147 from JuliaRobotics/feature/21Q1/residual_return

Return Residual in CalcFactor

NEWS.md

@@ -13,7 +13,7 @@ The list below highlights major breaking changes, and please note that significa
 
 # Major changes in IIF v0.21
 
-- `AbstractRelativeMinimize` should in-place populate `residual` not return scalar cost as previously done, see #1132.
+- `CalcResidual` no longer takes a `residual` as input parameter and should return `residual`, see #467 .
 
 # Major changes in IIF v0.20
 

examples/ApproximateConvolution.jl

@@ -24,13 +24,9 @@ function getSample(cf::CalcFactor{<:MultiModalConditional}, N::Int=1)
   return (ret, p)
 end
 
-function (cf::CalcFactor{<:MultiModalConditional})( res::AbstractVector{<:Real},
-                                                    meas,
-                                                    x1,
-                                                    x2  )
+function (cf::CalcFactor{<:MultiModalConditional})(meas, x1, x2)
   #
-  res[1] = meas[1] - (x2[1]-x1[1])
-  nothing
+  return meas[1] - (x2[1]-x1[1])
 end
 
 

examples/SquareRootTypes.jl

@@ -25,13 +25,11 @@ function getSample(cf::CalcFactor{<:AreEqual}, N::Int=1)
   return (reshape(rand(cf.factor.z,N),1,:), )
 end
 
-function (cf::CalcFactor{<:AreEqual})( res::AbstractVector{<:Real},
-                                      meas,
+function (cf::CalcFactor{<:AreEqual})(meas,
                                       X,
                                       Y  )
   #
-  res[1] = X[1]-Y[1] + meas[1]
-  nothing
+  return X[1]-Y[1] + meas[1]
 end
 
 
@@ -41,13 +39,11 @@ struct Square <: AbstractRelativeRoots
 end
 getSample(cf::CalcFactor{<:Square}, N::Int=1) = (reshape(rand(cf.factor.z,N),1,:), )
 
-function (cf::CalcFactor{<:Square})( res::AbstractVector{<:Real},
-                                    meas,
+function (cf::CalcFactor{<:Square})(meas,
                                     X,
                                     XX  )
   #
-  res[1] = XX[1] - X[1]*X[1] + meas[1]
-  nothing
+  return XX[1] - X[1]*X[1] + meas[1]
 end
 
 

src/CalcFactor.jl

@@ -53,9 +53,10 @@ Example
 # TBD
 ```
 """
-function (cf::CalcFactor)( res, measparams... ) #where {T<:FunctorInferenceType,M,P<:Tuple,X<:AbstractVector} {T,M,P,X}
+function (cf::CalcFactor)(measparams... ) #where {T<:FunctorInferenceType,M,P<:Tuple,X<:AbstractVector} {T,M,P,X}
   #
   # NOTE this is a legacy interface
+  res = zeros(size(cf._legacyMeas,1))
   cf.factor(res, cf.metadata, cf._sampleIdx, cf._legacyMeas, cf._legacyParams...)
 end
 
@@ -132,7 +133,7 @@ function testFactorResidualBinary(fct,
   res = zeros(zdim)
 
   # calc the residual
-  @time cfo(res, meas..., param1, param2)
+  @time res = cfo(meas..., param1, param2)
 
   return res
 end

src/DeconvUtils.jl

@@ -90,7 +90,7 @@ function approxDeconv(fcto::DFGFactor,
     #
 
     # lambda with which to find best measurement values
-    hypoObj = (res, tgt) -> (targeti_.=tgt; onehypo!(res) )
+    hypoObj = (tgt) -> (targeti_.=tgt; onehypo!() )
 
     # find solution via SubArray view pointing to original memory location
     targeti_ .= _solveLambdaNumeric(fcttype, hypoObj, res_, measurement[1][:,idx], islen1)

src/Factors/DefaultPrior.jl

@@ -14,15 +14,8 @@ Prior(::UniformScaling) = Prior(Normal())
 
 getSample(cf::CalcFactor{<:Prior}, N::Int=1) = (reshape(rand(cf.factor.Z,N),:,N), )
 
-
-function (s::CalcFactor{<:Prior,M,P,X})(res::AbstractVector{<:Real},
-                                        z,
-                                        x1  ) where {M<:FactorMetadata,P<:Tuple,X<:AbstractVector}
-  #
-  res .= z .- x1
-  nothing
-end
-
+# basic default
+(s::CalcFactor{<:Prior})(z, x1) = z .- x1
 
 ## packed types are still developed by hand.  Future versions would likely use a @packable macro to write Protobuf safe versions of factors
 

src/Factors/EuclidDistance.jl

@@ -25,15 +25,9 @@ getSample(cf::CalcFactor{<:EuclidDistance}, N::Int=1) = (reshape(rand(cf.factor.
 
 
 # new and simplified interface for both nonparametric and parametric
-function (s::CalcFactor{<:EuclidDistance})( residual::AbstractVector{<:Real},
-                                            z,
-                                            x1,
-                                            x2 )
-  #
-  residual .= z .- norm(x2 .- x1)
-
-  # v0.21+, should not return cost, but rather populate residual
-  nothing
+function (s::CalcFactor{<:EuclidDistance})(z, x1, x2)
+  # v0.21+, should return residual and not have residual parameter
+  return z .- norm(x2 .- x1)
 end
 
 

src/Factors/LinearRelative.jl

@@ -40,14 +40,10 @@ getSample(cf::CalcFactor{<:LinearRelative}, N::Int=1) = (reshape(rand(cf.factor.
 
 
 # new and simplified interface for both nonparametric and parametric
-function (s::CalcFactor{<:LinearRelative})( res::AbstractVector{<:Real},
-                                                  z,
-                                                  x1,
-                                                  x2  ) # where {M<:FactorMetadata,P<:Tuple,X<:AbstractVector}
-  #
+function (s::CalcFactor{<:LinearRelative})(z, x1, x2) 
   # TODO convert to distance(distance(x2,x1),z) # or use dispatch on `-` -- what to do about `.-`
-  res .= z - (x2 - x1)
-  nothing
+   # v0.21+, should return residual
+  return z .- (x2 .- x1)
 end
 
 

src/Factors/MsgLikelihoods.jl

@@ -28,12 +28,8 @@ const MsgRelativeType = Vector{NamedTuple{(:variables, :likelihood), Tuple{Vecto
 const MsgPriorType = Dict{Symbol, MsgPrior{BallTreeDensity}}
 
 
-function (cfo::CalcFactor{<:MsgPrior})( res::AbstractVector{<:Real},
-                                        z,
-                                        x1  )
-#
-  res .= z .- x1
-  nothing
+function (cfo::CalcFactor{<:MsgPrior})(z, x1)
+  return  z .- x1
 end
 
 

src/Factors/Sphere1D.jl

@@ -21,14 +21,13 @@ Sphere1Sphere1(::UniformScaling) = Sphere1Sphere1()
 getSample(cf::CalcFactor{<:Sphere1Sphere1}, N::Int=1) = (reshape(rand(cf.factor.Z,N),:,N), )
 
 
-function (cf::CalcFactor{<:Sphere1Sphere1})(res::AbstractVector{<:Real},
-                                            meas,
+function (cf::CalcFactor{<:Sphere1Sphere1})(meas,
                                             wxi,
                                             wxj  ) # where {M<:FactorMetadata,P<:Tuple,X<:AbstractVector}
   #
   wXjhat = addtheta(wxi[1], meas[1])
-  res[1] = difftheta(wxj[1], wXjhat)  # jXjhat =
-  nothing
+  res = difftheta(wxj[1], wXjhat)  # jXjhat =
+  return res
 end
 
 

src/NumericalCalculations.jl

@@ -43,7 +43,7 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   #
 
   #
-  r = nlsolve(objResX, u0, inplace=true)
+  r = nlsolve( (res, x) -> res .= objResX(x), u0, inplace=true)
 
   #
   return r.zero
@@ -58,9 +58,9 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   #
   # wrt #467 allow residual to be standardize for Roots and Minimize and Parametric cases.
   r = if islen1
-    optimize((x) -> (objResX(residual, x); sum(residual.^2)), u0, BFGS() )
+    optimize((x) -> (residual = objResX(x); sum(residual.^2)), u0, BFGS() )
   else
-    optimize((x) -> (objResX(residual, x); sum(residual.^2)), u0)
+    optimize((x) -> (residual = objResX(x); sum(residual.^2)), u0)
   end
 
   # 
@@ -148,7 +148,7 @@ function _buildCalcFactorLambdaSample(ccwl::CommonConvWrapper,
   fill!(cpt_.res, 0.0) # Roots->xDim | Minimize->zDim
 
   # build static lambda
-  unrollHypo! = (res) -> cf( res, (_viewdim1or2.(measurement_, :, smpid))..., (view.(varParams, :, smpid))... )
+  unrollHypo! =  ()->cf( (_viewdim1or2.(measurement_, :, smpid))..., (view.(varParams, :, smpid))... )
 
   return unrollHypo!, target
 end
@@ -195,7 +195,7 @@ function _solveCCWNumeric!( ccwl::Union{CommonConvWrapper{F},
 
   # broadcast updates original view memory location
   ## using CalcFactor legacy path inside (::CalcFactor)
-  _hypoObj = (res, x) -> (target.=x; unrollHypo!(res) )
+  _hypoObj = (x) -> (target.=x; unrollHypo!())
 
   # TODO small off-manifold perturbation is a numerical workaround only, make on-manifold requires RoME.jl #244
   # use all element dimensions : ==> 1:ccwl.xDim

src/ODE/DERelative.jl

@@ -159,8 +159,7 @@ end
 
 
 # NOTE see #1025, CalcFactor should fix `multihypo=` in `cf.metadata` fields
-function (cf::CalcFactor{<:DERelative})(res::AbstractVector{<:Real},
-                                        meas1,
+function (cf::CalcFactor{<:DERelative})(meas1,
                                         diffOp,
                                         X...)
   #
@@ -189,6 +188,9 @@ function (cf::CalcFactor{<:DERelative})(res::AbstractVector{<:Real},
   ## assuming the ODE integrated from current X1 through to predicted X2 (ie `meas1[:,idx]`)
   ## FIXME, obviously this is not going to work for more compilcated groups/manifolds -- must fix this soon!
   # @show cf._sampleIdx, solveforIdx, meas1
+
+  #FIXME 
+  res = zeros(size(X[2],1))
   for i in 1:size(X[2],1)
     # diffop( test, reference )   <===>   ΔX = test \ reference
     res[i] = diffOp[i]( X[solveforIdx][i], meas1[i] )

src/ParametricUtils.jl

@@ -40,8 +40,8 @@ end
     $SIGNATURES
 
 Returns the parametric measurement for a factor as a tuple (measurement, inverse covariance) for parametric inference (assumign Gaussian).
-Defaults to find the parametric measurement at field `Z` followed by `z`.  
-  
+Defaults to find the parametric measurement at field `Z` followed by `z`.
+
 Notes
 - Users should overload this method should their factor not default to `.Z<:ParametricType` or `.z<:ParametricType`
 """
@@ -83,7 +83,7 @@ end
 """
     $TYPEDEF
 
-Internal parametric extension to [`CalcFactor`](@ref) used for buffering measurement and calculating Mahalanobis distance 
+Internal parametric extension to [`CalcFactor`](@ref) used for buffering measurement and calculating Mahalanobis distance
 """
 struct CalcFactorMahalanobis{CF}
   calcfactor!::CF
@@ -102,13 +102,10 @@ end
 
 # This is where the actual parametric calculation happens, CalcFactor equivalent for parametric
 function (cfp::CalcFactorMahalanobis)(variables...)
-  #TODO res might be removed see https://github.com/JuliaRobotics/IncrementalInference.jl/issues/467#issuecomment-772987921
-  res = Vector{eltype(variables[1])}(undef, length(cfp.meas))
-  iΣ = cfp.iΣ
-  cfp.calcfactor!(res, cfp.meas, variables...)
-
+  # call the user function (be careful to call the new CalcFactor version only!!!)
+  res = cfp.calcfactor!(cfp.meas, variables...)
   # 1/2*log(1/(  sqrt(det(Σ)*(2pi)^k) ))  ## k = dim(μ)
-  return 0.5 * (res' * iΣ * res)
+  return res' * cfp.iΣ * res
 end
 
 function calcFactorMahalanobisDict(fg)
@@ -119,45 +116,45 @@ function calcFactorMahalanobisDict(fg)
   return calcFactors
 end
 
-function _totalCost(cfdict::Dict{Symbol, CalcFactorMahalanobis}, 
-                    flatvar, 
+function _totalCost(cfdict::Dict{Symbol, CalcFactorMahalanobis},
+                    flatvar,
                     X )
   #
   obj = 0
   for (fid, cfp) in cfdict
 
     varOrder = cfp.varOrder
-    
+
     Xparams = [view(X, flatvar.idx[varId]) for varId in varOrder]
 
     # call the user function
-    retval = cfp(Xparams...) 
+    retval = cfp(Xparams...)
 
     # 1/2*log(1/(  sqrt(det(Σ)*(2pi)^k) ))  ## k = dim(μ)
-    obj += 1/2*retval 
+    obj += 1/2*retval
   end
 
   return obj
 end
 
 
 # build the cost function
-function _totalCost(fg::AbstractDFG, 
-                    flatvar, 
+function _totalCost(fg::AbstractDFG,
+                    flatvar,
                     X )
   #
   obj = 0
   for fct in getFactors(fg)
 
     cf = getFactorType(fct)
     varOrder = getVariableOrder(fct)
-    
+
     Xparams = [view(X, flatvar.idx[varId]) for varId in varOrder]
 
     retval = cf(Xparams...)
 
     # 1/2*log(1/(  sqrt(det(Σ)*(2pi)^k) ))  ## k = dim(μ)
-    obj += 1/2*retval 
+    obj += 1/2*retval
   end
 
   return obj
@@ -168,12 +165,12 @@ export solveFactorGraphParametric
 """
     $SIGNATURES
 
-Batch solve a Gaussian factor graph using Optim.jl. Parameters can be passed directly to optim.  
-Notes:  
+Batch solve a Gaussian factor graph using Optim.jl. Parameters can be passed directly to optim.
+Notes:
   - Only :Euclid and :Circular manifolds are currently supported, own manifold are supported with `algorithmkwargs` (code may need updating though)
 """
 function solveFactorGraphParametric(fg::AbstractDFG;
-                                    useCalcFactor::Bool=false, #TODO dev param will be removed
+                                    useCalcFactor::Bool=true, #TODO dev param will be removed
                                     solvekey::Symbol=:parametric,
                                     autodiff = :forward,
                                     algorithm=BFGS,
@@ -428,7 +425,7 @@ function solveFactorGraphParametric!(fg::AbstractDFG; init::Bool=true, kwargs...
   vardict, result, varIds, Σ = solveFactorGraphParametric(fg; kwargs...)
 
   updateParametricSolution!(fg, vardict)
-  
+
   return vardict, result, varIds, Σ
 end
 
@@ -496,7 +493,7 @@ function updateParametricSolution!(sfg, vardict)
       #calculate and fill in covariance
       vnd.bw = val.cov
       #fill in ppe as mean
-      ppe = MeanMaxPPE(:parametric, val.val, val.val, val.val) 
+      ppe = MeanMaxPPE(:parametric, val.val, val.val, val.val)
       getPPEDict(getVariable(sfg, v))[:parametric] = ppe
   end
 end

test/testCSMMonitor.jl

@@ -8,13 +8,11 @@ end
 
 IncrementalInference.getSample(cf::CalcFactor{<:BrokenFactor}, N::Int=1) = (reshape(rand(cf.factor.Z, N),:,N), )
 
-function (s::CalcFactor{<:BrokenFactor})(res::AbstractVector{<:Real},
-                                        z,
-                                        wxi,
-                                        wxj  )
+function (s::CalcFactor{<:BrokenFactor})(z,
+                                         wxi,
+                                         wxj)
     #
     error("User factor has a bug.")
-    nothing
 end
 
 # FIXME consolidate with CalcFactor according to #467

test/testCommonConvWrapper.jl

@@ -77,13 +77,11 @@ getSample(cf::CalcFactor{<:Pose1Pose1Test}, N::Int=1) = (reshape(rand(cf.factor.
 
 
 #proposed standardized parameter list, does not have to be functor
-function (cf::CalcFactor{<:Pose1Pose1Test})(res::AbstractVector{<:Real},
-                                            Dx,
+function (cf::CalcFactor{<:Pose1Pose1Test})(Dx,
                                             p1,
                                             p2 )
   #
-  res[1] = Dx[1] - (p2[1] - p1[1])
-  nothing
+  return Dx[1] - (p2[1] - p1[1])
 end
 
 ##