Merge pull request #859 from JuliaRobotics/feat/3Q20/locprodsolvekey_ppe

solveKey and PPE cleanup and mmd

src/AnalysisTools.jl

@@ -158,3 +158,31 @@ function getTreeCost_02(tree::BayesTree; alpha::Float64=1.0)
 
   return getTreeCost_01(tree, alpha=alpha)/(totalNumChildren/numParents)
 end
+
+
+## ============================================================================
+# Tools for checking the numerical performance of the solve
+
+
+function mmdSolveKey(vari::DFGVariable,
+                    refKey::Symbol,
+                    tstKey::Symbol;
+                    bw::AbstractVector{<:Real}=[0.001;] )
+  #
+  refVal = getBelief(vari, refKey)
+  tstVal = getBelief(vari, tstKey)
+
+  # calc mmd distance
+  mmd(refVal, tstVal, getSofttype(vari), bw=bw)
+end
+
+
+
+# vari = getVariable(fg, :x1)
+# kys = filter(x->!(x in [:graphinit;:default]), listSolveKeys(fg) |> collect |>sortDFG)
+# X1_dist_0 = kys .|> x->mmdSolveKey(vari, :default_0, x)
+
+# kyD = [(kys[i],kys[i+1]) for i in 1:length(kys)-1]
+# X1_dist_D = kyD .|> x->mmdSolveKey(vari, x[1], x[2])
+
+# 

src/ApproxConv.jl

@@ -75,11 +75,12 @@ Prepare a common functor computation object `prepareCommonConvWrapper{T}` contai
 function prepareCommonConvWrapper!(ccwl::CommonConvWrapper{T},
                                    Xi::Vector{DFGVariable},
                                    solvefor::Symbol,
-                                   N::Int  ) where {T <: FunctorInferenceType}
+                                   N::Int;
+                                   solveKey::Symbol=:default  ) where {T <: FunctorInferenceType}
   #
   ARR = Array{Array{Float64,2},1}()
   # FIXME maxlen should parrot N (barring multi-/nullhypo issues)
-  maxlen, sfidx, manis = prepareparamsarray!(ARR, Xi, solvefor, N)
+  maxlen, sfidx, manis = prepareparamsarray!(ARR, Xi, solvefor, N, solveKey=solveKey)
   # should be selecting for the correct multihypothesis mode here with `gwp.params=ARR[??]`
   ccwl.params = ARR
   # get factor metadata -- TODO, populate, also see #784
@@ -250,17 +251,18 @@ Multiple dispatch wrapper for `<:AbstractRelativeFactor` types, to prepare and e
 
 Planned changes will fold null hypothesis in as a standard feature and no longer appear as a separate `InferenceType`.
 """
-function evalPotentialSpecific(Xi::Vector{DFGVariable},
-                               ccwl::CommonConvWrapper{T},
-                               solvefor::Symbol,
-                               measurement::Tuple=(zeros(0,100),);
-                               N::Int=size(measurement[1],2),
-                               spreadNH::Real=3.0,
-                               dbg::Bool=false  ) where {T <: Union{AbstractRelativeFactor, AbstractRelativeFactorMinimize}}
+function evalPotentialSpecific( Xi::Vector{DFGVariable},
+                                ccwl::CommonConvWrapper{T},
+                                solvefor::Symbol,
+                                measurement::Tuple=(zeros(0,100),);
+                                solveKey::Symbol=:default,
+                                N::Int=size(measurement[1],2),
+                                spreadNH::Real=3.0,
+                                dbg::Bool=false  ) where {T <: Union{AbstractRelativeFactor, AbstractRelativeFactorMinimize}}
   #
 
   # Prep computation variables
-  sfidx, maxlen, manis = prepareCommonConvWrapper!(ccwl, Xi, solvefor, N)
+  sfidx, maxlen, manis = prepareCommonConvWrapper!(ccwl, Xi, solvefor, N, solveKey=solveKey)
   # check for user desired measurement values
   if 0 < size(measurement[1],1)
     ccwl.measurement = measurement
@@ -284,20 +286,23 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
   return ccwl.params[ccwl.varidx]
 end
 
-function evalPotentialSpecific(Xi::Vector{DFGVariable},
-                               ccwl::CommonConvWrapper{T},
-                               solvefor::Symbol,
-                               measurement::Tuple=(zeros(0,0),);
-                               N::Int=size(measurement[1],2),
-                               dbg::Bool=false,
-                               spreadNH::Float64=3.0 ) where {T <: AbstractPrior}
+function evalPotentialSpecific( Xi::Vector{DFGVariable},
+                                ccwl::CommonConvWrapper{T},
+                                solvefor::Symbol,
+                                measurement::Tuple=(zeros(0,0),);
+                                solveKey::Symbol=:default,
+                                N::Int=size(measurement[1],2),
+                                dbg::Bool=false,
+                                spreadNH::Float64=3.0 ) where {T <: AbstractPrior}
   #
-  # FIXME, NEEDS TO BE CLEANED UP AND ADD MEAN ON MANIFOLDS PROPER
+  # FIXME, NEEDS TO BE CLEANED UP AND WORK ON MANIFOLDS PROPER
   fnc = ccwl.usrfnc!
   sfidx = 1
-  oldVal = getVal(Xi[sfidx])
+  oldVal = getVal(Xi[sfidx], solveKey=solveKey)
   nn = maximum([N; size(measurement[1],2); size(oldVal,2); size(ccwl.params[sfidx],2)]) # (N <= 0 ? size(getVal(Xi[1]),2) : N)
   vnds = Xi # (x->getSolverData(x)).(Xi)
+  # FIXME better standardize in-place operations (considering solveKey)
+  # FIXME FMD is not always just ()
   freshSamples!(ccwl, nn, FactorMetadata(), vnds)
   # Check which variables have been initialized
   isinit = map(x->isInitialized(x), Xi)
@@ -311,11 +316,11 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
   # generate nullhypo samples
   # inject lots of entropy in nullhypo case
   # make spread (1σ) equal to mean distance of other fractionals
+  # FIXME better standardize in-place operations (considering solveKey)
   addEntr = if size(oldVal,2) == nn
     deepcopy(oldVal)  #ccwl.params[sfidx])
   else
     ret = zeros(size(oldVal,1),nn)
-    # @show nn, size(ccwl.params[sfidx],2), size(ret)
     ret[:,1:size(oldVal,2)] .= oldVal #ccwl.params[sfidx]
     ret
   end
@@ -325,6 +330,7 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
   # ENT = generateNullhypoEntropy(addEntr, nn, spreadDist)
   if !ccwl.partial
       addEntr[:,ahmask] = ccwl.measurement[1][:,ahmask]
+      # ongoing part of RoME.jl #244
       addEntropyOnManifoldHack!(addEntrNH, addOps, spreadDist)
     # return ccwl.measurement[1]
   else
@@ -333,6 +339,7 @@ function evalPotentialSpecific(Xi::Vector{DFGVariable},
       i += 1
       addEntr[dimnum,ahmask] = ccwl.measurement[1][i,ahmask]
       addEntrNHp = view(addEntr, dimnum, nhmask)
+      # ongoing part of RoME.jl #244
       addEntropyOnManifoldHack!(addEntrNHp, addOps[dimnum:dimnum], spreadDist)
     end
   end
@@ -348,6 +355,7 @@ function evalFactor2(dfg::AbstractDFG,
                      fct::DFGFactor,
                      solvefor::Symbol,
                      measurement::Tuple=(zeros(0,100),);
+                     solveKey::Symbol=:default,
                      N::Int=size(measurement[1],2),
                      dbg::Bool=false  )
   #
@@ -375,7 +383,7 @@ function evalFactor2(dfg::AbstractDFG,
   for i in 1:Threads.nthreads()
     ccw.cpt[i].factormetadata.variablelist = variablelist
   end
-  return evalPotentialSpecific(Xi, ccw, solvefor, measurement, N=N, dbg=dbg, spreadNH=getSolverParams(dfg).spreadNH)
+  return evalPotentialSpecific(Xi, ccw, solvefor, measurement, solveKey=solveKey, N=N, dbg=dbg, spreadNH=getSolverParams(dfg).spreadNH)
 end
 
 # import IncrementalInference: evalFactor2, approxConv
@@ -444,13 +452,14 @@ The remaining dimensions will keep pre-existing variable values.
 Notes
 - fulldim is true when "rank-deficient" -- TODO swap to false (or even float)
 """
-function findRelatedFromPotential(dfg::G,
+function findRelatedFromPotential(dfg::AbstractDFG,
                                   fct::DFGFactor,
                                   varid::Symbol,
                                   N::Int,
-                                  dbg::Bool=false  )::Tuple{BallTreeDensity,Float64} where G <: AbstractDFG
+                                  dbg::Bool=false;
+                                  solveKey::Symbol=:default )
   # assuming it is properly initialized TODO
-  ptsbw = evalFactor2(dfg, fct, varid, N=N, dbg=dbg);
+  ptsbw = evalFactor2(dfg, fct, varid, solveKey=solveKey, N=N, dbg=dbg);
   # determine if evaluation is "dimension-deficient"
 
   # solvable dimension
@@ -468,7 +477,7 @@ function findRelatedFromPotential(dfg::G,
   if Npoints != N # this is where we control the overall particle set size
       p = resample(p,N)
   end
-  return p, inferdim
+  return (p, inferdim)::Tuple{BallTreeDensity,Float64}
 end
 
 

src/Deprecated.jl

@@ -26,6 +26,44 @@ end
 ## Delete at end v0.15.x
 ##==============================================================================
 
+export getIdx
+
+"""
+    $SIGNATURES
+
+Return interger index of desired variable element.
+
+Example
+-------
+```julia
+pp = RoME.Point2()
+getIdx(pp, :posY) # = 2
+```
+
+Internal Notes
+--------------
+- uses number i < 100 for index number, and
+- uses +100 offsets to track the minibatch number of the requested dimension
+"""
+function getIdx(pp::Tuple,
+                sym::Symbol,
+                i::Int=0)
+  #
+  error("getIdx is obsolete, use DistributedFactorGraphs objects/methods instead.")
+  i-=100
+  for p in pp
+    i,j = getIdx(p, sym, i)
+    if i > 0
+      return i, j
+    end
+  end
+  return i,-1
+end
+getIdx(pp::Symbol, sym::Symbol, i::Int=0) = pp==sym ? (abs(i)%100+1, div(abs(i)-100,100)) : (i-1, div(abs(i)-100,100))
+function getIdx(pp::InferenceVariable, sym::Symbol, i::Int=0)
+  return getIdx(pp.dimtype, sym)
+end
+
 # """
 #     $SIGNATURES
 

src/FGOSUtils.jl

@@ -4,7 +4,7 @@
 
 import DistributedFactorGraphs: AbstractPointParametricEst, loadDFG
 
-
+export calcPPE, calcVariablePPE
 export getPPESuggestedAll, findVariablesNear, defaultFixedLagOnTree!
 export loadDFG
 export fetchDataJSON
@@ -122,13 +122,13 @@ Related
 
 getVariablePPE, setVariablePosteriorEstimates!, getVariablePPE!
 """
-function calcVariablePPE(var::DFGVariable,
-                         softt::InferenceVariable;
-                         solveKey::Symbol=:default,
-                         method::Type{MeanMaxPPE}=MeanMaxPPE  )
+function calcPPE(var::DFGVariable,
+                  varType::InferenceVariable;
+                  solveKey::Symbol=:default,
+                  method::Type{MeanMaxPPE}=MeanMaxPPE  )
   #
-  P = getKDE(var, solveKey)
-  manis = getManifolds(softt) # getManifolds(vnd)
+  P = getBelief(var, solveKey)
+  manis = getManifolds(varType) # getManifolds(vnd)
   ops = buildHybridManifoldCallbacks(manis)
   Pme = getKDEMean(P) #, addop=ops[1], diffop=ops[2]
   Pma = getKDEMax(P, addop=ops[1], diffop=ops[2])
@@ -142,59 +142,25 @@ function calcVariablePPE(var::DFGVariable,
     elseif mani == :Circular
       suggested[i] = Pma[i]
     else
-      error("Unknown manifold to find PPE, $softt, $mani")
+      error("Unknown manifold to find PPE, $varType, $mani")
     end
   end
   MeanMaxPPE(solveKey, suggested, Pma, Pme, now())
 end
 
 
-calcVariablePPE(var::DFGVariable; method::Type{<:AbstractPointParametricEst}=MeanMaxPPE, solveKey::Symbol=:default) = calcVariablePPE(var, getSofttype(var), method=method, solveKey=solveKey)
+calcPPE(var::DFGVariable; method::Type{<:AbstractPointParametricEst}=MeanMaxPPE, solveKey::Symbol=:default) = calcPPE(var, getSofttype(var), method=method, solveKey=solveKey)
 
-function calcVariablePPE(dfg::AbstractDFG,
-                         sym::Symbol;
-                         method::Type{<:AbstractPointParametricEst}=MeanMaxPPE,
-                         solveKey::Symbol=:default )
+function calcPPE(dfg::AbstractDFG,
+                  sym::Symbol;
+                  method::Type{<:AbstractPointParametricEst}=MeanMaxPPE,
+                  solveKey::Symbol=:default )
   #
   var = getVariable(dfg, sym)
-  calcVariablePPE(var, getSofttype(var), method=method, solveKey=solveKey)
+  calcPPE(var, getSofttype(var), method=method, solveKey=solveKey)
 end
 
-
-"""
-    $SIGNATURES
-
-Return interger index of desired variable element.
-
-Example
--------
-```julia
-pp = RoME.Point2()
-getIdx(pp, :posY) # = 2
-```
-
-Internal Notes
---------------
-- uses number i < 100 for index number, and
-- uses +100 offsets to track the minibatch number of the requested dimension
-"""
-function getIdx(pp::Tuple,
-                sym::Symbol,
-                i::Int=0)
-  #
-  i-=100
-  for p in pp
-    i,j = getIdx(p, sym, i)
-    if i > 0
-      return i, j
-    end
-  end
-  return i,-1
-end
-getIdx(pp::Symbol, sym::Symbol, i::Int=0) = pp==sym ? (abs(i)%100+1, div(abs(i)-100,100)) : (i-1, div(abs(i)-100,100))
-function getIdx(pp::InferenceVariable, sym::Symbol, i::Int=0)
-  return getIdx(pp.dimtype, sym)
-end
+const calcVariablePPE = calcPPE
 
 
 
@@ -265,6 +231,13 @@ function unfreezeVariablesAll!(fgl::AbstractDFG)
   dontMarginalizeVariablesAll!(fgl)
 end
 
+# WIP
+# function resetSolvableAllExcept!(dfg::AbstractDFG,
+#                                   fltr::NothingUnion{Regex}=nothing)
+#   #
+#   unfreezeVariablesAll!(dfg)
+# end
+
 """
     $SIGNATURES
 

src/FactorGraph.jl

@@ -450,7 +450,7 @@ end
 
 Fetch the variable marginal sample points without the KDE bandwidth parameter.  Use getVertKDE to retrieve the full KDE object.
 """
-function getVal(vA::Vector{<:DFGVariable}, solveKey::Symbol=:default)::Array{Float64, 2}
+function getVal(vA::Vector{<:DFGVariable}, solveKey::Symbol=:default)
   @warn "getVal(::Vector{DFGVariable}) is obsolete, use getVal.(DFGVariable) instead."
   len = length(vA)
   vals = Array{Array{Float64,2},1}()
@@ -472,7 +472,7 @@ function getVal(vA::Vector{<:DFGVariable}, solveKey::Symbol=:default)::Array{Flo
       val[:,(cols[i]+1):cols[i+1]] = vals[i]
   end
   val[:,(cols[len]+1):cols[len+1]] = vals[len] # and the last one
-  return val
+  return val::Array{Float64, 2}
 end
 
 
@@ -491,15 +491,16 @@ Note for initialization, solveFor = Nothing.
 function prepareparamsarray!(ARR::Array{Array{Float64,2},1},
                              Xi::Vector{<:DFGVariable},
                              solvefor::Union{Nothing, Symbol},
-                             N::Int=0  )
+                             N::Int=0;
+                             solveKey::Symbol=:default  )
   #
   LEN = Int[]
   maxlen = N # FIXME see #105
   count = 0
   sfidx = 0
 
   for xi in Xi
-    push!(ARR, getVal(xi))
+    push!(ARR, getVal(xi, solveKey=solveKey))
     len = size(ARR[end], 2)
     push!(LEN, len)
     if len > maxlen
@@ -513,7 +514,7 @@ function prepareparamsarray!(ARR::Array{Array{Float64,2},1},
   SAMP=LEN.<maxlen
   for i in 1:count
     if SAMP[i]
-      ARR[i] = KDE.sample(getKDE(Xi[i]), maxlen)[1]
+      ARR[i] = KDE.sample(getKDE(Xi[i], solveKey), maxlen)[1]
     end
   end