Remove PPEs - use calcMeanMaxSuggested

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,218 @@ 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
+# _checkVariableByReference used in ROME #TODO
+"""
+    $SIGNATURES
+
+Return `::Tuple` with matching variable ID symbols and `Suggested` PPE values.
+
+Related
+
+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
+
+
+"""
+    $SIGNATURES
 
-# TODO maybe upstream to DFG
-DFG.MeanMaxPPE(solveKey::Symbol, suggested::StaticArray, max::StaticArray, mean::StaticArray) =
-  DFG.MeanMaxPPE(solveKey, Vector(suggested), Vector(max), Vector(mean))
+Check if a variable might already be located at the test location, by means of a (default) `refKey=:simulated` PPE stored in the existing variables.
+
+Notes
+- Checks, using provided `factor` from `srcLabel` in `fg` to an assumed `dest` variable whcih may or may not yet exist.
+- This function was written to aid in building simulation code, 
+  - it's use in real world usage may have unexpected behaviour -- hence not exported.
+- Return `::Tuple{Bool, Vector{Float64}, Symbol}`, eg. already exists `(true, [refVal], :l17)`, or if a refernce variable does not yet `(false, [refVal], :l28)`.
+  - Vector contains the PPE reference location of the new variable as calculated.
+- Auto `destPrefix` is trying to parse `destRegex` labels like `l\\d+` or `tag\\d+`, won't work with weirder labels e.g. `:l_4_23`.
+  - User can overcome weird names by self defining `destPrefix` and `srcNumber`.
+  - User can also ignore and replace the generated new label `Symbol(destPrefix, srcNumber)`.
+- 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 [`solveFactorParametric`](@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).
+
+Example
+```julia
+# fg has :x5 and :l2 and PPEs :simulated exists in all variables
+# user wants to add a factor from :x5 to potential new :l5, but maybe a (simulated) variable, say :l2, is already there.
+
+newFactor = RoME.Pose2Point2BearingRange(Normal(), Normal(20,0.5))
+isAlready, simPPE, genLabel = IIF._checkVariableByReference(fg, :x5, r"l\\d+", Point2, newFactor)
+
+# maybe add new variable
+if !isAlready
+  @info "New variable with simPPE" genLabel simPPE 
+  newVar = addVariable!(fg, genLabel, Point2)
+  addFactor!(fg, [:x5; genLabel], newFactor)
+
+  # also set :simulated PPE for similar future usage
+  newPPE = DFG.MeanMaxPPE(:simulated, simPPE, simPPE, simPPE)
+  setPPE!(newVar, :simulated, typeof(newPPE), newPPE)   # TODO this API can be improved
+else
+  @info "Adding simulated loop closure with perfect data association" :x5 genLabel
+  addFactor!(fg, [:x5; genLabel], newFactor)
+end
+
+# the point is that only the (0,20) values in newFactor are needed, all calculations are abstracted away.
+```
+
+See also: [`RoME.generateGraph_Honeycomb!`](@ref), [`accumulateFactorMeans`](@ref)
+"""
+function _checkVariableByReference(
+  fg::AbstractDFG,
+  srcLabel::Symbol,
+  destRegex::Regex,
+  destType::Type{<:StateType},
+  factor::AbstractRelativeObservation;
+  srcType::Type{<:StateType} = getVariableType(fg, srcLabel) |> typeof,
+  doRef::Bool = true,
+  refKey::Symbol = :simulated,
+  prior = if !doRef
+    nothing
+  else
+    DFG._getPriorType(srcType)(
+    MvNormal(calcMeanMaxSuggested(fg, srcLabel, refKey).suggested, diagm(ones(getDimension(srcType)))),
+  )
+  end,
+  atol::Real = 1e-2,
+  destPrefix::Symbol = match(r"[a-zA-Z_]+", destRegex.pattern).match |> Symbol,
+  srcNumber = match(r"\d+", string(srcLabel)).match |> x -> parse(Int, x),
+  overridePPE = doRef ? nothing : zeros(getDimension(destType)),
+)
+  #
+
+  refVal = if overridePPE !== nothing
+    overridePPE
+  else
+    # calculate and add the reference value
+    # TODO refactor consolidation to use `_buildGraphByFactorAndTypes!`
+    tfg = initfg()
+    addVariable!(tfg, :x0, srcType)
+    addFactor!(tfg, [:x0], prior)
+    addVariable!(tfg, :l0, destType)
+    addFactor!(tfg, [:x0; :l0], factor; graphinit = false)
+
+    # calculate where the landmark reference position is
+    accumulateFactorMeans(tfg, [:x0f1; :x0l0f1])
+  end
+
+  varLms = ls(fg, destRegex) |> sortDFG
+  already = if doRef
+    ppeLms = calcMeanMaxSuggested.(getVariable.(fg, varLms), refKey) .|> x -> x.suggested
+    errmask = ppeLms .|> (x -> isapprox(x, refVal; atol = atol))
+    any(errmask)
+  else
+    false
+  end
+
+  if already
+    # does exist, ppe, variableLabel
+    alrLm = varLms[findfirst(errmask)]
+    # @info "Variable on :$refKey does exists at" srcLabel alrLm
+    return true, ppe, alrLm
+  end
+
+  # Nope does not exist, ppe, generated new variable label only
+  return false, ppe, Symbol(destPrefix, srcNumber)
+end
+
+function _checkVariableByReference(
+  fg::AbstractDFG,
+  srcLabel::Symbol,
+  destRegex::Regex,
+  destType::Type{<:StateType},
+  factor::AbstractPriorObservation;
+  srcType::Type{<:StateType} = getVariableType(fg, srcLabel) |> typeof,
+  doRef::Bool = true,
+  refKey::Symbol = :simulated,
+  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),
+  overridePPE = doRef ? nothing : zeros(getDimension(destType)),
+)
+  #
+
+  refVal = if overridePPE !== nothing
+    overridePPE
+  else
+    getMeasurementParametric(factor)[1]
+  end
+
+  ppe = nothing #FIXME
+
+  # Nope does not exist, ppe, generated new variable label only
+  return false, ppe, Symbol(destPrefix, srcNumber)
+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

@@ -110,7 +110,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, replace with homotopy? fits in future"
     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