Merge pull request #1343 from JuliaRobotics/21Q3/refac/consolsfparam

consolidate solveFactorParametric, and deprs

Author: dehann

NEWS.md

@@ -20,6 +20,11 @@ The list below highlights major breaking changes, and please note that significa
 - New helper function `randToPoints(::SamplableBelief, N=1)::Vector{P}` to help with `getSample` for cases with new `ManifoldKernelDensity` beliefs for manifolds containing points of type `P`.
 - Upstream `calcHelix_T` canonical generator utility from RoME.jl.
 - Deserialization of factors with DFG needs new API and change of solverData and CCW type in factor.
+- Deprecate use of `getParametricMeasurement` and use `getMeasurementParametric` instead, and add `<:AbstractManifold` to API.
+- Deprecate use of `solveBinaryFactorParameteric`, instead use `solveFactorParameteric`.
+- Deprecating `approxConvBinary`, use `approxConvBelief` instead.
+- Deprecating `accumulateFactorChain`, use `approxConvBelief` instead.
+
 
 # Major changes in v0.24
 

src/Deprecated.jl

@@ -26,6 +26,91 @@ end
 ##==============================================================================
 
 
+# """
+#     $SIGNATURES
+
+# Calculate both measured and predicted relative variable values, starting with `from` at zeros up to `to::Symbol`.
+
+# Notes
+# - assume single variable separators only.
+
+# DevNotes
+# - TODO better consolidate with [`approxConvBelief`](@ref) which can now also work with factor chains.
+# """
+# function accumulateFactorChain( dfg::AbstractDFG,
+#                                 from::Symbol,
+#                                 to::Symbol,
+#                                 fsyms::Vector{Symbol}=findFactorsBetweenNaive(dfg, from, to);
+#                                 initval=zeros(size(getVal(dfg, from))))
+
+#   # get associated variables
+#   svars = union(ls.(dfg, fsyms)...)
+
+#   # use subgraph copys to do calculations
+#   tfg_meas = buildSubgraph(dfg, [svars;fsyms])
+#   tfg_pred = buildSubgraph(dfg, [svars;fsyms])
+
+#   # drive variable values manually to ensure no additional stochastics are introduced.
+#   nextvar = from
+#   initManual!(tfg_meas, nextvar, initval)
+#   initManual!(tfg_pred, nextvar, initval)
+
+#   # nextfct = fsyms[1] # for debugging
+#   for nextfct in fsyms
+#     nextvars = setdiff(ls(tfg_meas,nextfct),[nextvar])
+#     @assert length(nextvars) == 1 "accumulateFactorChain requires each factor pair to separated by a single variable"
+#     nextvar = nextvars[1]
+#     meas, pred = approxDeconv(dfg, nextfct) # solveFactorMeasurements
+#     pts_meas = approxConv(tfg_meas, nextfct, nextvar, (meas,ones(Int,100),collect(1:100)))
+#     pts_pred = approxConv(tfg_pred, nextfct, nextvar, (pred,ones(Int,100),collect(1:100)))
+#     initManual!(tfg_meas, nextvar, pts_meas)
+#     initManual!(tfg_pred, nextvar, pts_pred)
+#   end
+#   return getVal(tfg_meas,nextvar), getVal(tfg_pred,nextvar)
+# end
+
+
+# # TODO should this be consolidated with regular approxConv?
+# # TODO, perhaps pass Xi::Vector{DFGVariable} instead?
+# function approxConvBinary(arr::Vector{Vector{Float64}},
+#                           meas::AbstractFactor,
+#                           outdims::Int,
+#                           fmd::FactorMetadata,
+#                           measurement::Tuple=(Vector{Vector{Float64}}(),);
+#                           varidx::Int=2,
+#                           N::Int=length(arr),
+#                           vnds=DFGVariable[],
+#                           _slack=nothing )
+#   #
+#   # N = N == 0 ? size(arr,2) : N
+#   pts = [zeros(outdims) for _ in 1:N];
+#   ptsArr = Vector{Vector{Vector{Float64}}}()
+#   push!(ptsArr,arr)
+#   push!(ptsArr,pts)
+
+#   fmd.arrRef = ptsArr
+
+#   # TODO consolidate with ccwl??
+#   # FIXME do not divert Mixture for sampling
+#   # cf = _buildCalcFactorMixture(ccwl, fmd, 1, ccwl.measurement, ARR) # TODO perhaps 0 is safer
+#   # FIXME 0, 0, ()
+#   cf = CalcFactor( meas, fmd, 0, 0, (), ptsArr)
+
+#   measurement = length(measurement[1]) == 0 ? sampleFactor(cf, N) : measurement
+#   # measurement = size(measurement[1],2) == 0 ? sampleFactor(meas, N, fmd, vnds) : measurement
+
+#   zDim = length(measurement[1][1])
+#   ccw = CommonConvWrapper(meas, ptsArr[varidx], zDim, ptsArr, fmd, varidx=varidx, measurement=measurement)  # N=> size(measurement[1],2)
+
+#   for n in 1:N
+#     ccw.cpt[Threads.threadid()].particleidx = n
+#     _solveCCWNumeric!( ccw, _slack=_slack )
+#   end
+#   return pts
+# end
+
+@deprecate getParametricMeasurement(w...;kw...) getMeasurementParametric(w...;kw...)
+
 # function prtslperr(s)
 #   println(s)
 #   sleep(0.1)
@@ -203,7 +288,7 @@ end
 
 @deprecate ensureAllInitialized!(w...;kw...) initAll!(w...;kw...)
 
-@deprecate getFactorMean(w...) IIF.getParametricMeasurement(w...)[1]
+@deprecate getFactorMean(w...) IIF.getMeasurementParametric(w...)[1]
 
 # """
 #     $SIGNATURES

src/FGOSUtils.jl

@@ -251,7 +251,7 @@ Related
 """
 function calcPPE( var::DFGVariable,
                   varType::InferenceVariable=getVariableType(var);
-                  ppeType::Type{MeanMaxPPE}=MeanMaxPPE,
+                  ppeType::Type{<:MeanMaxPPE}=MeanMaxPPE,
                   solveKey::Symbol=:default,
                   ppeKey::Symbol=solveKey,
                   timestamp=now()  )
@@ -265,7 +265,7 @@ function calcPPE( var::DFGVariable,
   # returns coordinates at identify
   Pma = getKDEMax(P, addop=ops[1], diffop=ops[2])
   # calculate point
- 
+
   ## TODO make PPE only use getCoordinates for now (IIF v0.25)
   Pme_ = getCoordinates(typeof(varType),Pme)
   # Pma_ = getCoordinates(M,Pme)

src/FactorGraph.jl

@@ -752,7 +752,7 @@ function DFG.addFactor!(dfg::AbstractDFG,
                         suppressChecks::Bool=false,
                         inflation::Real=getSolverParams(dfg).inflation,
                         namestring::Symbol = assembleFactorName(dfg, Xi),
-                        _blockRecursion::Bool=false  )
+                        _blockRecursion::Bool=!getSolverParams(dfg).attemptGradients  )
   #
 
   varOrderLabels = Symbol[v.label for v=Xi]

src/IncrementalInference.jl

@@ -392,8 +392,7 @@ export *,
   getPointType, 
   getPointIdentity,
   setVariableRefence!,
-  reshapeVec2Mat,
-  accumulateFactorChain
+  reshapeVec2Mat
 
 # more optional exports
 export HeatmapDensityRegular

src/NumericalCalculations.jl

@@ -103,10 +103,13 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
     return sum(residual.^2)
   end
 
-  alg = islen1 ? Optim.BFGS() : Optim.NelderMead() 
+  # separate statements to try improve type-stability 
+  r = if islen1
+    Optim.optimize(cost, X0c, Optim.BFGS())
+  else
+    Optim.optimize(cost, X0c, Optim.NelderMead())
+  end
 
-  r = Optim.optimize(cost, X0c, alg)
-  
   return exp(M, ϵ, hat(M, ϵ, r.minimizer)) 
 
 end

src/ODE/DERelative.jl

@@ -5,7 +5,7 @@ using .DifferentialEquations
 
 import .DifferentialEquations: solve
 
-import IncrementalInference: getSample
+import IncrementalInference: getSample, getManifold
 
 export DERelative
 
@@ -38,6 +38,7 @@ struct DERelative{T <:InferenceVariable, P, D} <: AbstractRelativeRoots
   specialSampler::Function
 end
 
+getManifold(de::DERelative{T}) where T = getManifold(de.domain)
 
 """
 $SIGNATURES

src/ParametricUtils.jl

@@ -45,100 +45,52 @@ 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`
 """
-function getParametricMeasurement end
+function getMeasurementParametric end
 
-function getParametricMeasurement(Z)
+function getMeasurementParametric(Z)
   error("$(typeof(Z)) is not supported, please use non-parametric or open an issue if it should be")
 end
 
-function getParametricMeasurement(Z::Normal)
+function getMeasurementParametric(M::AbstractManifold, Z::Normal)
   meas = mean(Z)
   iσ = 1/std(Z)^2
   return [meas], reshape([iσ],1,1)
 end
 
-function getParametricMeasurement(Z::MvNormal)
+function getMeasurementParametric(M::AbstractManifold, Z::MvNormal)
   meas = mean(Z)
   iΣ = invcov(Z)
-  return meas, iΣ
+  p_μ = exp(M, identity_element(M), hat(M, identity_element(M), meas))
+  return p_μ, iΣ
 end
 
+getMeasurementParametric(Z::Normal)   = getMeasurementParametric(TranslationGroup(1), Z)
+getMeasurementParametric(Z::MvNormal) = getMeasurementParametric(TranslationGroup(length(mean(Z))), Z)
+
 # the point `p` on the manifold is the mean
-function getParametricMeasurement(s::ManifoldPrior)
+function getMeasurementParametric(s::ManifoldPrior)
   meas = s.p
   iΣ = invcov(s.Z)
   return meas, iΣ
 end
- 
-function getParametricMeasurement(s::FunctorInferenceType)
+
+function getMeasurementParametric(s::AbstractFactor)
   if hasfield(typeof(s), :Zij)
     Z = s.Zij
-    @info "getParametricMeasurement falls back to using field `.Zij` by default. Extend it for more complex factors." maxlog=1
+    @info "getMeasurementParametric falls back to using field `.Zij` by default. Extend it for more complex factors." maxlog=1
   elseif hasfield(typeof(s), :Z)
     Z = s.Z
-    @info "getParametricMeasurement falls back to using field `.Z` by default. Extend it for more complex factors." maxlog=1
+    @info "getMeasurementParametric falls back to using field `.Z` by default. Extend it for more complex factors." maxlog=1
   elseif hasfield(typeof(s), :z)
     Z = s.z
-    @info "getParametricMeasurement falls back to using field `.z` by default. Extend it for more complex factors." maxlog=1
+    @info "getMeasurementParametric falls back to using field `.z` by default. Extend it for more complex factors." maxlog=1
   else
     error("$(typeof(s)) not supported, please use non-parametric or open an issue if it should be")
   end
-  return getParametricMeasurement(Z)
+  return getMeasurementParametric(Z)
 end
 
 
-## ================================================================================================
-## Parametric binary factor utility function, used by DRT
-## ================================================================================================
-
-"""
-    $SIGNATURES
-
-Helper function to propagate a parametric estimate along a factor chain.
-
-Notes
-- Not used during mmisam inference.
-- Expected uses are for user analysis of factors and estimates.
-- real-time dead reckoning chain prediction.
-
-DevNotes
-- FIXME consolidate with `approxConv`
-
-Related:
-
-[`getParametricMeasurement`](@ref), [`approxConv`](@ref), [`accumulateFactorMeans`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
-"""
-function solveBinaryFactorParameteric(dfg::AbstractDFG,
-                                      fct::DFGFactor,
-                                      currval::Vector{Float64},
-                                      srcsym::Symbol,
-                                      trgsym::Symbol  )::Vector{Float64}
-  #
-  outdims = getVariableDim(getVariable(dfg, trgsym))
-  meas = getFactorType(fct)
-  mea, = getParametricMeasurement(meas)
-  # mea = getFactorMean(fct)
-  mea_ = Vector{Vector{Float64}}()
-  push!(mea_, mea)
-  measT = (mea_,)
-
-  # upgrade part of #639
-  varSyms = getVariableOrder(fct)
-  Xi = getVariable.(dfg, varSyms)  # (v->getVariable(dfg, v)).(varSyms)
-
-  # calculate the projection
-  varmask = (1:2)[varSyms .== trgsym][1]
-
-  fmd = FactorMetadata(Xi, getLabel.(Xi), Vector{Vector{Vector{Float64}}}(), :null, nothing)
-  currval_ = Vector{Vector{Float64}}()
-  push!(currval_, currval)
-  pts_ = approxConvBinary( currval_, meas, outdims, fmd, measT, varidx=varmask )
-
-  # return the result
-  @assert length(pts_[1]) == outdims
-  return pts_[1]
-end
-
 
 ## ================================================================================================
 ## Parametric solve with Mahalanobis distance - CalcFactor
@@ -153,7 +105,7 @@ function CalcFactorMahalanobis(fct::DFGFactor)
   cf = getFactorType(fct)
   varOrder = getVariableOrder(fct)
 
-  _meas, _iΣ = getParametricMeasurement(cf)
+  _meas, _iΣ = getMeasurementParametric(cf)
   meas = typeof(_meas) <: Tuple ? _meas : (_meas,)
   iΣ = typeof(_iΣ) <: Tuple ? _iΣ : (_iΣ,)
 
@@ -694,9 +646,9 @@ struct MaxMixture
   choice::Base.RefValue{Int}
 end
 
-function getParametricMeasurement(s::Mixture{N,F,S,T}) where {N,F,S,T}
-  meas = map(c->getParametricMeasurement(c)[1], values(s.components))
-  iΣ = map(c->getParametricMeasurement(c)[2], values(s.components))
+function getMeasurementParametric(s::Mixture{N,F,S,T}) where {N,F,S,T}
+  meas = map(c->getMeasurementParametric(c)[1], values(s.components))
+  iΣ = map(c->getMeasurementParametric(c)[2], values(s.components))
   return meas, iΣ
 end
 

src/SolverUtilities.jl

@@ -182,7 +182,7 @@ Notes
 - This function does not add new variables or factors to `fg`, user must do that themselves after.
   - Useful to use in combination with `setPPE!` on new variable.
 - At time of writing `accumulateFactorMeans` could only incorporate priors or binary relative factors.
-  - internal info, see [`solveBinaryFactorParameteric`](@ref),
+  - internal info, see [`solveFactorParameteric`](@ref),
   - This means at time of writing `factor` must be a binary factor.
 - Tip, if simulations are inducing odometry bias, think of using two factors from caller (e.g. simPerfect and simBias).
 
@@ -273,7 +273,7 @@ function _checkVariableByReference( fg::AbstractDFG,
                                     factor::AbstractPrior;
                                     srcType::Type{<:InferenceVariable} = getVariableType(fg, srcLabel) |> typeof,
                                     refKey::Symbol=:simulated,
-                                    prior = typeof(factor)( MvNormal(getParametricMeasurement(factor)...) ),
+                                    prior = typeof(factor)( MvNormal(getMeasurementParametric(factor)...) ),
                                     atol::Real = 1e-3,
                                     destPrefix::Symbol = match(r"[a-zA-Z_]+", destRegex.pattern).match |> Symbol,
                                     srcNumber = match(r"\d+", string(srcLabel)).match |> x->parse(Int,x),
@@ -283,7 +283,7 @@ function _checkVariableByReference( fg::AbstractDFG,
   refVal = if overridePPE !== nothing
     overridePPE
   else
-    getParametricMeasurement(factor)[1]
+    getMeasurementParametric(factor)[1]
   end
 
   ppe = DFG.MeanMaxPPE(refKey, refVal, refVal, refVal)