diff --git a/NEWS.md b/NEWS.md index 51b6ba58e..8a7cb7fd1 100644 --- 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 index f916e0e0e..5aa5f6774 100644 --- 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 new file mode 100644 index 000000000..050edca9b --- /dev/null +++ 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 index b5838de56..338566472 100644 --- 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 index b59f7a5cc..fd1cf484c 100644 --- 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 index 6ec9a5152..99995b8e1 100644 --- 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 - - #