JuliaRobotics · Affie · Aug 13, 2021 · Aug 13, 2021 · Aug 13, 2021 · Aug 13, 2021
diff --git a/NEWS.md b/NEWS.md
@@ -23,8 +23,7 @@ The list below highlights major breaking changes, and please note that significa
 - 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.
-
+- Removing obsolete `approxConvCircular`, use `approxConvBelief` instead.
 
 # Major changes in v0.24
 

diff --git a/src/AdditionalUtils.jl b/src/AdditionalUtils.jl
@@ -1,5 +1,4 @@
 
-export approxConvCircular
 
 """
     $SIGNATURES
@@ -132,91 +131,5 @@ end
 
 
 
-"""
-    $SIGNATURES
-
-Print basic summary of graph to `logger=ConsoleLogger()`.
-"""
-function printGraphSummary(dfg::G, logger=ConsoleLogger())::Nothing where {G <: AbstractDFG}
-    vars = ls(dfg)
-    fcts = lsf(dfg)
-
-    prio = lsfPriors(dfg)
-
-    isinit = map(x->isInitialized(dfg,x), vars)
-    infdim = map(x->getVariableInferredDim(dfg, x), vars)
-    numedges = map(v->length(ls(dfg, v)), vars)
-    numfed = map(fc->length(ls(dfg, fc)), fcts)
-    vardims = map(v->getDimension(getVariable(dfg, v)), vars)
-    fctdims = map(v->getDimension(getFactor(dfg, v)), fcts)
-    priodims = map(v->getDimension(getFactor(dfg, v)), prio)
-
-    with_logger(logger) do
-      @info "Distributed Factor Graph summary:"
-      @info "  num variables:    $(length(vars))"
-      @info "  num factors:      $(length(fcts)), w/ $(length(prio)) priors"
-      @info "  var initialized:  $(sum(isinit))"
-      @info ""
-      @info "  var num edges: min. $(minimum(numedges)) | mean $(round(Statistics.mean(numedges),digits=2)) | 90% $(round(quantile(numedges,0.9),digits=2)) | max. $(maximum(numedges))"
-      @info "  fct num edges: min. $(minimum(numfed)) | mean $(round(Statistics.mean(numfed),digits=2)) | 90% $(round(quantile(numfed,0.9),digits=2)) | max. $(maximum(numfed))"
-      @info "  Variable dims: min. $(minimum(vardims)) | mean $(round(Statistics.mean(vardims),digits=2)) | 90% $(round(quantile(vardims,0.9),digits=2)) | max. $(maximum(vardims))"
-      @info "  Factor dims:   min. $(minimum(fctdims)) | mean $(round(Statistics.mean(fctdims),digits=2)) | 90% $(round(quantile(fctdims,0.9),digits=2)) | max. $(maximum(fctdims))"
-      @info "  Prior dimens:  min. $(minimum(priodims)) | mean $(round(Statistics.mean(priodims),digits=2)) | 90% $(round(quantile(priodims,0.9),digits=2)) | max. $(maximum(priodims))"
-      @info "  var infr'dims: min. $(minimum(infdim)) | mean $(round(Statistics.mean(infdim),digits=2)) | 90% $(round(quantile(infdim,0.9),digits=2)) | max. $(maximum(infdim))"
-    end
-    nothing
-end
-
-"""
-    $SIGNATURES
-
-Print basic summary of graph to `logger=ConsoleLogger()`.
-"""
-function printSummary(dfg::G, logger=ConsoleLogger()) where G <: AbstractDFG
-    printGraphSummary(dfg, logger)
-end
-
-
-"""
-    $SIGNATURES
-
-Build an approximate density `[Y|X,DX,.]=[X|Y,DX][DX|.]` as proposed by the conditional convolution.
-
-Notes
-- Assume both are on circular manifold, `manikde!(pts, (:Circular,))`
-"""
-function approxConvCircular(pX::ManifoldKernelDensity, 
-                            pDX::ManifoldKernelDensity; N::Int=100)
-  #
-
-  # building basic factor graph
-  tfg = initfg()
-  addVariable!(tfg, :s1, Sphere1)
-  addVariable!(tfg, :s2, Sphere1)
-  addFactor!(tfg, [:s1;:s2], Sphere1Sphere1(pDX), graphinit=false)
-  initManual!(tfg,:s1, pX)
-
-  # solve for outgoing proposal value
-  approxConv(tfg,:s1s2f1,:s2)
-end
-
-function approxConvCircular(pX::ManifoldKernelDensity, 
-                            pDX::SamplableBelief; N::Int=100)
-  #
-  pts = reshape(rand(pDX, N), 1, :)
-  pC = manikde!(pts, Sphere1)
-  approxConvCircular(pX, pC)
-end
-
-
-function approxConvCircular(pX::SamplableBelief, 
-                            pDX::ManifoldKernelDensity; N::Int=100)
-  #
-    pts = reshape(rand(pX, N), 1, :)
-  pC = manikde!(pts, Sphere1)
-  approxConvCircular(pC, pDX)
-end
-
-
 
 #
diff --git a/src/ConsolidateParametricRelatives.jl b/src/ConsolidateParametricRelatives.jl
@@ -0,0 +1,73 @@
+# functions relating to parametric solutions of a single factor that is likely in need of consolidation
+
+
+
+"""
+    $SIGNATURES
+
+Helper function to propagate a parametric estimate along a factor chain.  
+This function takes and returns variable values as coordinates.
+
+Notes
+- Not used during MM-iSAM inference.
+- Expected uses are for user analysis of factors and estimates.
+- real-time dead reckoning chain prediction.
+- Parametric binary factor utility function, used by DRT
+
+DevNotes
+- TODO ensure type stability, likely returning types `Any` at this time.
+- TODO MeanMaxPPE currently stored as coordinates, complicating fast calculation.
+
+Related:
+
+[`getMeasurementParametric`](@ref), [`approxConv`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
+"""
+function solveFactorParameteric(dfg::AbstractDFG,
+                                fct::DFGFactor,
+                                # currval::P1,
+                                srcsym_vals::AbstractVector{Pair{Symbol, P}},
+                                trgsym::Symbol,
+                                solveKey::Symbol=:default;
+                                evaltmpkw...  ) where P
+  #
+
+  varLbls = getVariableOrder(fct)
+  varTypes = tuple((getVariableType.(dfg, varLbls))...)
+  sfidx = findfirst( varLbls .== trgsym )
+
+  # get the measurement point
+  fctTyp = getFactorType(fct)
+  # this is definitely in coordinates, see JuliaRobotics/RoME.jl#465
+  mea, _ = getMeasurementParametric(fctTyp)
+  # must change measT to be a tangent vector
+  M = getManifold(fctTyp)
+  e0 = identity_element(M)
+  mea_ = hat(M, e0, mea)
+  measT = (mea_,)
+
+  # get variable points
+  function _getParametric(vari::DFGVariable, 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))
+
+    getCoordinates(getVariableType(vari),pt)
+  end
+
+  # overwrite specific src values from user
+  coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
+  for (srcsym, currval) in srcsym_vals
+    coordVals[findfirst(varLbls .== srcsym)] = currval
+  end
+  crds = tuple(coordVals...)
+
+  pts = tuple(map(t->getPoint(t...), zip(varTypes,crds))...)
+
+  # do the calculation to find solvefor index using the factor, as manifold point
+  pt = _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )[1]
+
+  return getCoordinates(getVariableType(dfg, trgsym), pt)
+end
+
+
diff --git a/src/Deprecated.jl b/src/Deprecated.jl
@@ -26,6 +26,99 @@ end
 ##==============================================================================
 
 
+
+# """
+#     $SIGNATURES
+
+# Build an approximate density `[Y|X,DX,.]=[X|Y,DX][DX|.]` as proposed by the conditional convolution.
+
+# Notes
+# - Assume both are on circular manifold, `manikde!(pts, (:Circular,))`
+# """
+# function approxConvCircular(pX::ManifoldKernelDensity, 
+#                             pDX::ManifoldKernelDensity; N::Int=100)
+#   #
+
+#   # building basic factor graph
+#   tfg = initfg()
+#   addVariable!(tfg, :s1, Sphere1)
+#   addVariable!(tfg, :s2, Sphere1)
+#   addFactor!(tfg, [:s1;:s2], Sphere1Sphere1(pDX), graphinit=false)
+#   initManual!(tfg,:s1, pX)
+
+#   # solve for outgoing proposal value
+#   approxConv(tfg,:s1s2f1,:s2)
+# end
+
+# function approxConvCircular(pX::ManifoldKernelDensity, 
+#                             pDX::SamplableBelief; N::Int=100)
+#   #
+#   pts = reshape(rand(pDX, N), 1, :)
+#   pC = manikde!(pts, Sphere1)
+#   approxConvCircular(pX, pC)
+# end
+
+
+# function approxConvCircular(pX::SamplableBelief, 
+#                             pDX::ManifoldKernelDensity; N::Int=100)
+#   #
+#     pts = reshape(rand(pX, N), 1, :)
+#   pC = manikde!(pts, Sphere1)
+#   approxConvCircular(pC, pDX)
+# end
+
+
+
+@deprecate printGraphSummary(dfg::AbstractDFG, logger=ConsoleLogger()) show(logger.stream, MIME("text/plain"), dfg)
+@deprecate printSummary(dfg::AbstractDFG, logger=ConsoleLogger()) show(logger.stream, MIME("text/plain"), dfg)
+
+
+# """
+#     $SIGNATURES
+
+# Print basic summary of graph to `logger=ConsoleLogger()`.
+# """
+# function printGraphSummary(dfg::G, logger=ConsoleLogger())::Nothing where {G <: AbstractDFG}
+#     vars = ls(dfg)
+#     fcts = lsf(dfg)
+
+#     prio = lsfPriors(dfg)
+
+#     isinit = map(x->isInitialized(dfg,x), vars)
+#     infdim = map(x->getVariableInferredDim(dfg, x), vars)
+#     numedges = map(v->length(ls(dfg, v)), vars)
+#     numfed = map(fc->length(ls(dfg, fc)), fcts)
+#     vardims = map(v->getDimension(getVariable(dfg, v)), vars)
+#     fctdims = map(v->getDimension(getFactor(dfg, v)), fcts)
+#     priodims = map(v->getDimension(getFactor(dfg, v)), prio)
+
+#     with_logger(logger) do
+#       @info "Distributed Factor Graph summary:"
+#       @info "  num variables:    $(length(vars))"
+#       @info "  num factors:      $(length(fcts)), w/ $(length(prio)) priors"
+#       @info "  var initialized:  $(sum(isinit))"
+#       @info ""
+#       @info "  var num edges: min. $(minimum(numedges)) | mean $(round(Statistics.mean(numedges),digits=2)) | 90% $(round(quantile(numedges,0.9),digits=2)) | max. $(maximum(numedges))"
+#       @info "  fct num edges: min. $(minimum(numfed)) | mean $(round(Statistics.mean(numfed),digits=2)) | 90% $(round(quantile(numfed,0.9),digits=2)) | max. $(maximum(numfed))"
+#       @info "  Variable dims: min. $(minimum(vardims)) | mean $(round(Statistics.mean(vardims),digits=2)) | 90% $(round(quantile(vardims,0.9),digits=2)) | max. $(maximum(vardims))"
+#       @info "  Factor dims:   min. $(minimum(fctdims)) | mean $(round(Statistics.mean(fctdims),digits=2)) | 90% $(round(quantile(fctdims,0.9),digits=2)) | max. $(maximum(fctdims))"
+#       @info "  Prior dimens:  min. $(minimum(priodims)) | mean $(round(Statistics.mean(priodims),digits=2)) | 90% $(round(quantile(priodims,0.9),digits=2)) | max. $(maximum(priodims))"
+#       @info "  var infr'dims: min. $(minimum(infdim)) | mean $(round(Statistics.mean(infdim),digits=2)) | 90% $(round(quantile(infdim,0.9),digits=2)) | max. $(maximum(infdim))"
+#     end
+#     nothing
+# end
+
+# """
+#     $SIGNATURES
+
+# Print basic summary of graph to `logger=ConsoleLogger()`.
+# """
+# function printSummary(dfg::G, logger=ConsoleLogger()) where G <: AbstractDFG
+#     printGraphSummary(dfg, logger)
+# end
+
+
+
 # """
 #     $SIGNATURES
 

diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl
@@ -132,8 +132,6 @@ export *,
   # state machine methods
   StateMachine,
   exitStateMachine,
-  printGraphSummary,
-  printSummary,
   print,
   getGraphFromHistory,
   getCliqSubgraphFromHistory,
@@ -464,14 +462,12 @@ include("Factors/PartialPrior.jl")
 include("Factors/PartialPriorPassThrough.jl")
 include("DefaultNodeTypes.jl") # older file
 
-
 # Refactoring in progress
 include("services/CalcFactor.jl")
 # gradient tools
 include("services/FactorGradients.jl")
 include("services/CliqueTypes.jl")
 
-
 # solving graphs
 include("SolverUtilities.jl")
 include("NumericalCalculations.jl")
@@ -480,6 +476,9 @@ include("ExplicitDiscreteMarginalizations.jl")
 include("InferDimensionUtils.jl")
 include("services/EvalFactor.jl")
 include("services/ApproxConv.jl")
+
+include("ConsolidateParametricRelatives.jl") # FIXME CONSOLIDATE
+
 include("GraphProductOperations.jl")
 include("SolveTree.jl")
 include("TetherUtils.jl")

diff --git a/src/services/ApproxConv.jl b/src/services/ApproxConv.jl
@@ -241,75 +241,6 @@ end
 
 
 
-"""
-    $SIGNATURES
-
-Helper function to propagate a parametric estimate along a factor chain.  
-This function takes and returns variable values as coordinates.
-
-Notes
-- Not used during MM-iSAM inference.
-- Expected uses are for user analysis of factors and estimates.
-- real-time dead reckoning chain prediction.
-- Parametric binary factor utility function, used by DRT
-
-DevNotes
-- TODO ensure type stability, likely returning types `Any` at this time.
-- TODO MeanMaxPPE currently stored as coordinates, complicating fast calculation.
-
-Related:
-
-[`getMeasurementParametric`](@ref), [`approxConv`](@ref), [`MutablePose2Pose2Gaussian`](@ref)
-"""
-function solveFactorParameteric(dfg::AbstractDFG,
-                                fct::DFGFactor,
-                                # currval::P1,
-                                srcsym_vals::AbstractVector{Pair{Symbol, P}},
-                                trgsym::Symbol,
-                                solveKey::Symbol=:default;
-                                evaltmpkw...  ) where P
-  #
-
-  varLbls = getVariableOrder(fct)
-  varTypes = tuple((getVariableType.(dfg, varLbls))...)
-  sfidx = findfirst( varLbls .== trgsym )
-
-  # get the measurement point
-  fctTyp = getFactorType(fct)
-  # this is definitely in coordinates, see JuliaRobotics/RoME.jl#465
-  mea, _ = getMeasurementParametric(fctTyp)
-  # must change measT to be a tangent vector
-  M = getManifold(fctTyp)
-  e0 = identity_element(M)
-  mea_ = hat(M, e0, mea)
-  measT = (mea_,)
-
-  # get variable points
-  function _getParametric(vari::DFGVariable, 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))
-
-    getCoordinates(getVariableType(vari),pt)
-  end
-
-  # overwrite specific src values from user
-  coordVals = _getParametric.(getVariable.(dfg, varLbls), solveKey)
-  for (srcsym, currval) in srcsym_vals
-    coordVals[findfirst(varLbls .== srcsym)] = currval
-  end
-  crds = tuple(coordVals...)
-
-  pts = tuple(map(t->getPoint(t...), zip(varTypes,crds))...)
-
-  # do the calculation to find solvefor index using the factor, as manifold point
-  pt = _evalFactorTemporary!( fctTyp, varTypes, sfidx, measT, pts; solveKey=solveKey, evaltmpkw... )[1]
-
-  return getCoordinates(getVariableType(dfg, trgsym), pt)
-end
-
-
 
 
 #