diff --git a/src/ApproxConv.jl b/src/ApproxConv.jl index 40b5bc5f2..c5245e2ab 100644 --- a/src/ApproxConv.jl +++ b/src/ApproxConv.jl @@ -252,7 +252,7 @@ function calcVariableDistanceExpectedFractional(ccwl::CommonConvWrapper, end # Add entrypy on a point in `points` on manifold M, only on dimIdx if in p -function addEntropyOnManifoldHack!( M::ManifoldsBase.AbstractManifold, +function addEntropyOnManifold!( M::ManifoldsBase.AbstractManifold, points::Union{<:AbstractVector{<:Real},SubArray}, dimIdx::AbstractVector, spreadDist::Real, @@ -281,16 +281,6 @@ function addEntropyOnManifoldHack!( M::ManifoldsBase.AbstractManifold, end # - # manis = convert(Tuple, M) # LEGACY, TODO REMOVE - # # TODO deprecate - # maniAddOps, _, _, _ = buildHybridManifoldCallbacks(manis) - # # add 1σ "noise" level to max distance as control - # # 1:size(addEntr, 1) - # for dim in dimIdx, idx in 1:length(addEntr) - # if (p === :) || dim in p - # addEntr[idx][dim] = maniAddOps[dim](addEntr[idx][dim], spreadDist*(rand()-0.5)) - # end - # end nothing end @@ -333,7 +323,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F}, # consider duplicate convolution approximations for inflation off-zero # ultimately set by dfg.params.inflateCycles for iflc in 1:inflateCycles - addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist, cpt_.p) + addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist, cpt_.p) # no calculate new proposal belief on kernels `allelements[count]` skipSolve ? @warn("skipping numerical solve operation") : approxConvOnElements!(ccwl, allelements[count]) end @@ -348,7 +338,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F}, addEntr = view(ccwl.params[sfidx], allelements[count]) # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo) spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=spreadNH) - addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist) + addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist) elseif hypoidx == 0 # basically do nothing since the factor is not active for these allelements[count] @@ -358,7 +348,7 @@ function computeAcrossHypothesis!(ccwl::Union{<:CommonConvWrapper{F}, # dynamic estimate with user requested speadNH of how much noise to inject (inflation or nullhypo) spreadDist = calcVariableDistanceExpectedFractional(ccwl, sfidx, certainidx, kappa=spreadNH) # # make spread (1σ) equal to mean distance of other fractionals - addEntropyOnManifoldHack!(mani, addEntr, 1:getDimension(mani), spreadDist) + addEntropyOnManifold!(mani, addEntr, 1:getDimension(mani), spreadDist) else error("computeAcrossHypothesis -- not dealing with multi-hypothesis case correctly") end @@ -518,7 +508,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable}, addEntr[m] .= ccwl.measurement[1][m] end # ongoing part of RoME.jl #244 - addEntropyOnManifoldHack!(mani, addEntrNH, 1:getDimension(mani), spreadDist) + addEntropyOnManifold!(mani, addEntrNH, 1:getDimension(mani), spreadDist) else i = 0 for dimnum in fnc.partial @@ -529,7 +519,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable}, # @show size(addEntr), dimnum, nhmask addEntrNHp = view(view(addEntr, (1:length(ahmask))[ahmask]), dimnum) # ongoing part of RoME.jl #244 - addEntropyOnManifoldHack!(mani, addEntrNHp, dimnum:dimnum, spreadDist) + addEntropyOnManifold!(mani, addEntrNHp, dimnum:dimnum, spreadDist) end end return addEntr diff --git a/src/CompareUtils.jl b/src/CompareUtils.jl index db9b2575b..e06a87828 100644 --- a/src/CompareUtils.jl +++ b/src/CompareUtils.jl @@ -25,7 +25,7 @@ function compareAllSpecial( A::T1, B::T2; skip=Symbol[], show::Bool=true) where {T1 <: CommonConvWrapper, T2 <: CommonConvWrapper} # if T1 != T2 - @warn "CCW types not equal" T1 T2 + @warn "CCW types T1 and T2 not equal=>" T1 T2 return false end # FIXME still issues with compare, skipping :vartypes https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/434 diff --git a/src/DispatchPackedConversions.jl b/src/DispatchPackedConversions.jl index fd688c056..315db8456 100644 --- a/src/DispatchPackedConversions.jl +++ b/src/DispatchPackedConversions.jl @@ -52,10 +52,12 @@ function convert( # reconstitute from stored data # FIXME, add threadmodel=threadmodel # FIXME https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-776838053 + # FIXME dont know what manifolds to use in ccw ccw = prepgenericconvolution(DFG.DFGVariable[], usrfnc, multihypo=mhcat, nullhypo=nh, inflation=packed.inflation) ccw.certainhypo = packed.certainhypo - ret = FunctionNodeData{CommonConvWrapper{typeof(usrfnc)}}(packed.eliminated, packed.potentialused, packed.edgeIDs, ccw, + # CommonConvWrapper{typeof(usrfnc)} + ret = FunctionNodeData{typeof(ccw)}(packed.eliminated, packed.potentialused, packed.edgeIDs, ccw, packed.multihypo, packed.certainhypo, packed.nullhypo, packed.solveInProgress, packed.inflation ) # diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl index 3fe965ebf..205509623 100644 --- a/src/FGOSUtils.jl +++ b/src/FGOSUtils.jl @@ -140,19 +140,7 @@ function manikde!(pts::AbstractVector{P}, return AMP.manikde!(M, pts) end -# manikde!( pts::AbstractVector{<:Real}, -# vartype::Type{<:ContinuousScalar}) = manikde!(reshape(pts,1,:), vartype) -# -# TEMPORARY legacy wrapper -# function manikde!(ptsArr::Vector{Vector{Float64}}, -# bw::Vector{Float64}, -# varType::Union{InstanceType{<:InferenceVariable}, InstanceType{<:AbstractFactor}} ) -# # -# arr = Matrix{Float64}(undef, length(ptsArr[1]), length(ptsArr)) -# @cast arr[i,j] = ptsArr[j][i] -# manikde!( arr, bw, varType ) -# end """ $SIGNATURES diff --git a/src/FactorGraph.jl b/src/FactorGraph.jl index 11fa530a5..dfd325008 100644 --- a/src/FactorGraph.jl +++ b/src/FactorGraph.jl @@ -594,20 +594,21 @@ Notes - Will not work in all situations, but good enough so far. - # TODO standardize via domain or manifold definition...?? """ -function calcZDim(cf::CalcFactor{T}) where {T <: FunctorInferenceType} +function calcZDim(cf::CalcFactor{T}) where {T <: AbstractFactor} # - # zdim = T != GenericMarginal ? size(getSample(usrfnc, 2)[1],1) : 0 - zdim = if T != GenericMarginal - # vnds = Xi # (x->getSolverData(x)).(Xi) - # NOTE try to make sure we get matrix back (not a vector) - smpls = sampleFactor(cf, 2)[1] - length(smpls[1]) - else - 0 - end - return zdim + + # NOTE try to make sure we get matrix back (not a vector) + smpls = sampleFactor(cf, 2)[1] + return length(smpls[1]) end +# FIXME THIS IS NEW REPLACEMENT FUNCTION +# function calcZDim(cf::CalcFactor{T}) where T <: AbstractFactor +# return manifold_dimension(getManifold(cf.factor)) +# end + +calcZDim(cf::CalcFactor{<:GenericMarginal}) = 0 + calcZDim(cf::CalcFactor{<:ManifoldPrior}) = manifold_dimension(cf.factor.M) @@ -689,7 +690,7 @@ function getDefaultFactorData(dfg::AbstractDFG, ccw = prepgenericconvolution(Xi, usrfnc, multihypo=mhcat, nullhypo=nh, threadmodel=threadmodel, inflation=inflation) # and the factor data itself - return FunctionNodeData{CommonConvWrapper{T}}(eliminated, potentialused, edgeIDs, ccw, multihypo, ccw.certainhypo, nullhypo, solveInProgress, inflation) + return FunctionNodeData{typeof(ccw)}(eliminated, potentialused, edgeIDs, ccw, multihypo, ccw.certainhypo, nullhypo, solveInProgress, inflation) end @@ -1024,10 +1025,10 @@ Related initManual!, graphinit (keyword) """ function resetInitialValues!(dest::AbstractDFG, - src::AbstractDFG=dest, - initKey::Symbol=:graphinit, - solveKey::Symbol=:default; - varList::AbstractVector{Symbol}=ls(dest)) + src::AbstractDFG=dest, + initKey::Symbol=:graphinit, + solveKey::Symbol=:default; + varList::AbstractVector{Symbol}=ls(dest)) # for vs in varList vnd = getSolverData(getVariable(src, vs), initKey) @@ -1145,24 +1146,23 @@ variables are related to data association uncertainty. Experimental - `inflation`, to better disperse kernels before convolution solve, see IIF #1051. """ -function addFactor!(dfg::AbstractDFG, - Xi::Vector{<:DFGVariable}, - usrfnc::R; - multihypo::Vector{Float64}=Float64[], - nullhypo::Float64=0.0, - solvable::Int=1, - tags::Vector{Symbol}=Symbol[], - timestamp::Union{DateTime,ZonedDateTime}=now(localzone()), - graphinit::Bool=getSolverParams(dfg).graphinit, - threadmodel=SingleThreaded, - suppressChecks::Bool=false, - inflation::Real=getSolverParams(dfg).inflation ) where - {R <: FunctorInferenceType} +function DFG.addFactor!(dfg::AbstractDFG, + Xi::Vector{<:DFGVariable}, + usrfnc::AbstractFactor; + multihypo::Vector{Float64}=Float64[], + nullhypo::Float64=0.0, + solvable::Int=1, + tags::Vector{Symbol}=Symbol[], + timestamp::Union{DateTime,ZonedDateTime}=now(localzone()), + graphinit::Bool=getSolverParams(dfg).graphinit, + threadmodel=SingleThreaded, + suppressChecks::Bool=false, + inflation::Real=getSolverParams(dfg).inflation, + namestring::Symbol = assembleFactorName(dfg, Xi) ) # # depcrecation varOrderLabels = Symbol[v.label for v=Xi] - namestring = assembleFactorName(dfg, Xi) solverData = getDefaultFactorData(dfg, Xi, deepcopy(usrfnc), @@ -1188,16 +1188,17 @@ end function DFG.addFactor!(dfg::AbstractDFG, xisyms::Vector{Symbol}, - usrfnc::FunctorInferenceType; - multihypo::Vector{<:Real}=Float64[], - nullhypo::Float64=0.0, - solvable::Int=1, - timestamp::Union{DateTime,ZonedDateTime}=now(localzone()), - tags::Vector{Symbol}=Symbol[], - graphinit::Bool=getSolverParams(dfg).graphinit, - threadmodel=SingleThreaded, - inflation::Real=getSolverParams(dfg).inflation, - suppressChecks::Bool=false ) + usrfnc::AbstractFactor; + suppressChecks::Bool=false, + kw... ) + # multihypo::Vector{<:Real}=Float64[], + # nullhypo::Float64=0.0, + # solvable::Int=1, + # timestamp::Union{DateTime,ZonedDateTime}=now(localzone()), + # tags::Vector{Symbol}=Symbol[], + # graphinit::Bool=getSolverParams(dfg).graphinit, + # threadmodel=SingleThreaded, + # inflation::Real=getSolverParams(dfg).inflation, # # depcrecation @@ -1208,7 +1209,7 @@ function DFG.addFactor!(dfg::AbstractDFG, variables = getVariable.(dfg, xisyms) # verts = map(vid -> DFG.getVariable(dfg, vid), xisyms) - addFactor!(dfg, variables, usrfnc, multihypo=multihypo, nullhypo=nullhypo, solvable=solvable, tags=tags, graphinit=graphinit, threadmodel=threadmodel, timestamp=timestamp, inflation=inflation ) + addFactor!(dfg, variables, usrfnc; suppressChecks=suppressChecks, kw... ) # multihypo=multihypo, nullhypo=nullhypo, solvable=solvable, tags=tags, graphinit=graphinit, threadmodel=threadmodel, timestamp=timestamp, inflation=inflation ) end diff --git a/src/GraphProductOperations.jl b/src/GraphProductOperations.jl index 6cf6072a2..08575e4c3 100644 --- a/src/GraphProductOperations.jl +++ b/src/GraphProductOperations.jl @@ -72,7 +72,7 @@ function localProduct(dfg::AbstractDFG, # make manifold belief from product vari = getVariable(dfg, sym) - pp = AMP.manikde!(pGM, getVariableType(vari) |> getManifold ) + pp = AMP.manikde!(getManifold(getVariableType(vari)), pGM ) return pp, dens, lb, sinfd end diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index 8fa1abe10..d4a8cbeab 100644 --- a/src/IncrementalInference.jl +++ b/src/IncrementalInference.jl @@ -422,6 +422,7 @@ include("Factors/GenericFunctions.jl") # Refactoring in progress include("Factors/MsgLikelihoods.jl") +include("entities/GraphConstraintTypes.jl") include("CliqueTypes.jl") include("JunctionTreeTypes.jl") @@ -444,7 +445,6 @@ include("TreeBasedInitialization.jl") include("HeatmapSampler.jl") # special variables and factors, see RoME.jl for more examples -include("GraphConstraintTypes.jl") include("Factors/Mixture.jl") include("Factors/DefaultPrior.jl") include("Factors/LinearRelative.jl") diff --git a/src/GraphConstraintTypes.jl b/src/entities/GraphConstraintTypes.jl similarity index 100% rename from src/GraphConstraintTypes.jl rename to src/entities/GraphConstraintTypes.jl