Remove PPEs - use calcMeanMaxSuggested (#1885)

Author: Affie

IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl

@@ -928,11 +928,9 @@ Notes
 function updateFromSubgraph_StateMachine(csmc::CliqStateMachineContainer)
   isParametricSolve = csmc.algorithm == :parametric
 
-  # set PPE and solved for all frontals
+  # set solved for all frontals
   if !isParametricSolve
     for sym in getCliqFrontalVarIds(csmc.cliq)
-      # set PPE in cliqSubFg
-      setVariablePosteriorEstimates!(csmc.cliqSubFg, sym)
       # set solved flag
       vari = getVariable(csmc.cliqSubFg, sym, csmc.solveKey)
       setSolvedCount!(vari, getSolvedCount(vari, csmc.solveKey) + 1, csmc.solveKey)
@@ -951,7 +949,6 @@ function updateFromSubgraph_StateMachine(csmc::CliqStateMachineContainer)
     frsyms,
     csmc.logger;
     solveKey = csmc.solveKey,
-    updatePPE = !isParametricSolve,
   )
 
   #solve finished change color

IncrementalInference/src/Deprecated.jl

@@ -155,12 +155,81 @@ function sampleTangent(x::ManifoldKernelDensity, p = mean(x))
   error("sampleTangent(x::ManifoldKernelDensity, p) should be replaced by sampleTangent(M<:AbstractManifold, x::ManifoldKernelDensity, p)")
 end
 
-## ================================================================================================
-## ================================================================================================
+export setPPE!, setVariablePosteriorEstimates!
+setPPE!(args...; kw...) = error("PPEs are obsolete (use `calcMeanMaxSuggested` provisionally), see DFG #1133")
+setVariablePosteriorEstimates!(args...; kw...) = error("PPEs are obsolete (use `calcMeanMaxSuggested` provisionally), see DFG #1133")
+
+@deprecate calcPPE(
+  var::VariableCompute,
+  varType::StateType = getVariableType(var);
+  solveKey::Symbol = :default,
+  kwargs...,
+) calcMeanMaxSuggested(var, solveKey)
+
+@deprecate calcPPE(
+  dfg::AbstractDFG,
+  label::Symbol;
+  solveKey::Symbol = :default,
+  kwargs...,
+) calcMeanMaxSuggested(dfg, label, solveKey)
+
+export calcVariablePPE
+const calcVariablePPE = calcPPE
+
+#FIXME The next functions use PPEs and should be updated or deprecated
+# getPPESuggestedAll no external use
+# findVariablesNear used in 1 rome example
+"""
+    $SIGNATURES
+
+Return `::Tuple` with matching variable ID symbols and `Suggested` PPE values.
+
+Related
 
-# TODO maybe upstream to DFG
-DFG.MeanMaxPPE(solveKey::Symbol, suggested::StaticArray, max::StaticArray, mean::StaticArray) =
-  DFG.MeanMaxPPE(solveKey, Vector(suggested), Vector(max), Vector(mean))
+getVariablePPE
+"""
+function getPPESuggestedAll(dfg::AbstractDFG, regexFilter::Union{Nothing, Regex} = nothing)
+  #
+  # get values
+  vsyms = listVariables(dfg, regexFilter) |> sortDFG
+  slamPPE = map(x -> getVariablePPE(dfg, x).suggested, vsyms)
+  # sizes to convert to matrix
+  rumax = zeros(Int, 2)
+  for ppe in slamPPE
+    rumax[2] = length(ppe)
+    rumax[1] = maximum(rumax)
+  end
+
+  # populate with values
+  XYT = zeros(length(slamPPE), rumax[1])
+  for i = 1:length(slamPPE)
+    XYT[i, 1:length(slamPPE[i])] = slamPPE[i]
+  end
+  return (vsyms, XYT)
+end
+
+"""
+    $SIGNATURES
+
+Find and return a `::Tuple` of variables and distances to `loc::Vector{<:Real}`.
+
+Related
+
+findVariablesNearTimestamp
+"""
+function findVariablesNear(
+  dfg::AbstractDFG,
+  loc::Vector{<:Real},
+  regexFilter::Union{Nothing, Regex} = nothing;
+  number::Int = 3,
+)
+  #
+
+  xy = getPPESuggestedAll(dfg, regexFilter)
+  dist = sum((xy[2][:, 1:length(loc)] .- loc') .^ 2; dims = 2) |> vec
+  prm = (dist |> sortperm)[1:number]
+  return (xy[1][prm], sqrt.(dist[prm]))
+end
 
 
 ## ================================================================================================

IncrementalInference/src/ExportAPI.jl

@@ -93,9 +93,6 @@ export CSMHistory,
   getLabel,
   getVariables,
   getVariableOrder,
-  getPPE,
-  getPPEDict,
-  getVariablePPE,
   isVariable,
   isFactor,
   getFactorType,
@@ -292,12 +289,7 @@ export CSMHistory,
   reshapeVec2Mat
 
 export incrSuffix
-
-export calcPPE, calcVariablePPE
-export setPPE!, setVariablePosteriorEstimates!
-export getPPEDict
-export getPPESuggested, getPPEMean, getPPEMax
-export getPPESuggestedAll
+export calcMeanMaxSuggested
 export loadDFG
 export findVariablesNear, defaultFixedLagOnTree!
 export fetchDataJSON

IncrementalInference/src/IncrementalInference.jl

@@ -109,7 +109,6 @@ import DistributedFactorGraphs: addVariable!, addFactor!, ls, lsf, isInitialized
 import DistributedFactorGraphs: compare
 import DistributedFactorGraphs: rebuildFactorCache!
 import DistributedFactorGraphs: getDimension, getManifold, getPointType, getPointIdentity
-import DistributedFactorGraphs: getPPE, getPPEDict
 import DistributedFactorGraphs: getPoint, getCoordinates
 import DistributedFactorGraphs: getVariableType
 import DistributedFactorGraphs: AbstractPointParametricEst, loadDFG

IncrementalInference/src/parametric/services/ConsolidateParametricRelatives.jl

@@ -14,7 +14,7 @@ Notes
 
 DevNotes
 - TODO ensure type stability, likely returning types `Any` at this time.
-- TODO MeanMaxPPE currently stored as coordinates, complicating fast calculation.
+- TODO parametric estimates currently stored as coordinates, complicating fast calculation.
 
 Related: [`getMeasurementParametric`](@ref), [`approxConvBelief`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
 """
@@ -45,9 +45,6 @@ function solveFactorParametric(
 
   # get variable points
   function _getParametric(vari::VariableCompute, key = :default)
-    # hasp = haskey(getPPEDict(vari), key)
-    # FIXME use PPE via Manifold points currently in coordinates
-    # hasp ? getPPE(vari, key).suggested : calcMean(getBelief(vari, key))
     pt = calcMean(getBelief(vari, key))
 
     return collect(getCoordinates(getVariableType(vari), pt))

IncrementalInference/src/parametric/services/ParametricManopt.jl

@@ -575,8 +575,6 @@ function autoinitParametric!(
     vnd.initialized = true
     #fill in ppe as mean
     Xc::Vector{Float64} = collect(getCoordinates(getVariableType(xi), val))
-    ppe = DFG.MeanMaxPPE(solveKey, Xc, Xc, Xc)
-    getPPEDict(xi)[solveKey] = ppe
 
     result = true
 

IncrementalInference/src/parametric/services/ParametricUtils.jl

@@ -923,17 +923,13 @@ end
 
 """
     $SIGNATURES
-Update the fg from solution in vardict and add MeanMaxPPE (all just mean). Usefull for plotting
+Update the fg from solution in vardict. Usefull for plotting
 """
 function updateParametricSolution!(sfg, vardict::AbstractDict; solveKey::Symbol = :parametric)
   for (v, val) in vardict
     vnd = getState(getVariable(sfg, v), solveKey)
     # Update the variable node data value and covariance
     updateSolverDataParametric!(vnd, val.val, val.cov)
-    #fill in ppe as mean
-    Xc = collect(getCoordinates(getVariableType(sfg, v), val.val))
-    ppe = DFG.MeanMaxPPE(solveKey, Xc, Xc, Xc)
-    getPPEDict(getVariable(sfg, v))[solveKey] = ppe
   end
 end
 
@@ -948,10 +944,6 @@ function updateParametricSolution!(fg, M, labels::AbstractArray{Symbol}, vals, 
     covar = isnothing(Σ) ? vnd.bw : covars[i]
     # Update the variable node data value and covariance
     updateSolverDataParametric!(vnd, val, covar)#FIXME add cov
-    #fill in ppe as mean
-    Xc = collect(getCoordinates(getVariableType(fg, v), val))
-    ppe = DFG.MeanMaxPPE(solveKey, Xc, Xc, Xc)
-    getPPEDict(getVariable(fg, v))[solveKey] = ppe
   end
 
 end
@@ -973,7 +965,7 @@ function createMvNormal(v::VariableCompute, key = :parametric)
     dims = vnd.dims
     return createMvNormal(vnd.val[1:dims, 1], vnd.bw[1:dims, 1:dims])
   else
-    @warn "Trying MvNormal Fit, replace with PPE fits in future"
+    @warn "Trying MvNormal Fit"
     return fit(MvNormal, getState(v, key).val)
   end
 end
@@ -1035,10 +1027,6 @@ function autoinitParametricOptim!(
     updateSolverDataParametric!(vnd, val, cov)
 
     vnd.initialized = true
-    #fill in ppe as mean
-    Xc = collect(getCoordinates(getVariableType(xi), val))
-    ppe = DFG.MeanMaxPPE(:parametric, Xc, Xc, Xc)
-    getPPEDict(xi)[:parametric] = ppe
 
     # updateVariableSolverData!(dfg, xi, solveKey, true; warn_if_absent=false)    
     # updateVariableSolverData!(dfg, xi.label, getState(xi, solveKey), :graphinit, true, Symbol[]; warn_if_absent=false)

IncrementalInference/src/services/ApproxConv.jl

@@ -60,14 +60,12 @@ Notes
 - Fresh starting point will be used if first element in `fctLabels` is a unary `<:AbstractPriorObservation`.
 - This function will not change any values in `dfg`, and might have slightly less speed performance to meet this requirement.
 - pass in `tfg` to get a recoverable result of all convolutions in the chain.
-- `setPPE` and `setPPEmethod` can be used to store PPE information in temporary `tfg`
 
 DevNotes
 - TODO strong requirement that this function is super efficient on single factor/variable case!
 - FIXME must consolidate with `accumulateFactorMeans`
 - TODO `solveKey` not fully wired up everywhere yet
   - tfg gets all the solveKeys inside the source `dfg` variables
-- TODO add a approxConv on PPE option
   - Consolidate with [`accumulateFactorMeans`](@ref), `approxConvBinary`
 
 Related
@@ -82,8 +80,6 @@ function approxConvBelief(
   solveKey::Symbol = :default,
   N::Int = length(measurement),
   tfg::AbstractDFG = LocalDFG(;solverParams=getSolverParams(dfg)),
-  setPPEmethod::Union{Nothing, Type{<:AbstractPointParametricEst}} = nothing,
-  setPPE::Bool = setPPEmethod !== nothing,
   path::AbstractVector{Symbol} = Symbol[],
   skipSolve::Bool = false,
   nullSurplus::Real = 0,
@@ -149,9 +145,6 @@ function approxConvBelief(
   end
   # didn't return early so shift focus to using `tfg` more intensely
   initVariable!(tfg, varLbls[1], pts)
-  # use in combination with setPPE and setPPEmethod keyword arguments
-  ppemethod = setPPEmethod === nothing ? DFG.MeanMaxPPE : setPPEmethod
-  !setPPE ? nothing : setPPE!(tfg, varLbls[1], solveKey, ppemethod)
 
   # do chain of convolutions
   for idx = idxS:length(path)
@@ -161,7 +154,6 @@ function approxConvBelief(
       addFactor!(tfg, fct)
       ptsBel = approxConvBelief(tfg, fct, path[idx + 1]; solveKey, N, skipSolve, keepCalcFactor)
       initVariable!(tfg, path[idx + 1], ptsBel)
-      !setPPE ? nothing : setPPE!(tfg, path[idx + 1], solveKey, ppemethod)
     end
   end