From 22e17e1c16e2e01b03a6c0b26d8cdd77a89b23f3 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 18 Mar 2021 21:24:24 -0400 Subject: [PATCH 1/3] add dep Manifolds.jl, towards new defs --- Project.toml | 2 ++ src/Deprecated.jl | 53 +++++++++++++++++-------------- src/Factors/Circular.jl | 13 +++++--- src/Factors/EuclidDistance.jl | 1 + src/Factors/LinearRelative.jl | 8 ++--- src/IncrementalInference.jl | 2 ++ src/SolverAPI.jl | 4 +-- src/Variables/Circular.jl | 4 +++ src/Variables/DefaultVariables.jl | 5 +++ 9 files changed, 58 insertions(+), 34 deletions(-) diff --git a/Project.toml b/Project.toml index bd31b5f29..d81d8ccc6 100644 --- a/Project.toml +++ b/Project.toml @@ -23,6 +23,7 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" +Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" MetaGraphs = "626554b9-1ddb-594c-aa3c-2596fe9399a5" NLSolversBase = "d41bc354-129a-5804-8e4c-c37616107c6c" @@ -52,6 +53,7 @@ FunctionalStateMachine = "0.2.9" JLD2 = "0.2, 0.3, 0.4" JSON2 = "0.3, 0.4, 0.5, 0.6, 0.7, 1" KernelDensityEstimate = "0.5.1, 0.6" +Manifolds = "0.4" ManifoldsBase = "0.10" MetaGraphs = "0.6.4" NLSolversBase = "7.6" diff --git a/src/Deprecated.jl b/src/Deprecated.jl index bc4d06b8e..85a168e9d 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -46,19 +46,6 @@ mutable struct CliqGibbsMC CliqGibbsMC(a,b) = new(a,b) end -""" -$(TYPEDEF) - -TODO TO BE DEPRECATED -""" -mutable struct DebugCliqMCMC - mcmc::Union{Nothing, Array{CliqGibbsMC,1}} - outmsg::LikelihoodMessage - outmsglbls::Dict{Symbol, Symbol} # Int - priorprods::Vector{CliqGibbsMC} - DebugCliqMCMC() = new() - DebugCliqMCMC(a,b,c,d) = new(a,b,c,d) -end ##============================================================================== @@ -66,17 +53,10 @@ end ##============================================================================== -# FIXME, much consolidation required here -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar}) = AMP.Euclid -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{1}}) = AMP.Euclid -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{2}}) = AMP.Euclid2 -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{3}}) = AMP.Euclid3 -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{4}}) = AMP.Euclid4 -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = AMP.Circle1 +# Legacy support +getManifolds(::InstanceType{Manifolds.Euclidean{Tuple{N}, ℝ}}) where N = tuple([:Euclid for i in 1:N]...) +getManifolds(::InstanceType{Manifolds.Circle{ℝ}}) = (:Circular,) -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = AMP.Circle1 -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = AMP.Euclid -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = convert(Manifold, ContinuousEuclid{N}) @@ -84,6 +64,18 @@ Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}} ## Deprecate code below before v0.23 ##============================================================================== + +# FIXME, much consolidation required here +# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{1}}) = AMP.Euclid +# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{2}}) = AMP.Euclid2 +# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{3}}) = AMP.Euclid3 +# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{4}}) = AMP.Euclid4 + +# Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = AMP.Circle1 + +# convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{1}}) = AMP.Euclid +# convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{2}}) = AMP.Euclid2 + export Sphere1 @warn "Deprecating old use of Sphere1, being replaced by Cicular instead" @@ -96,4 +88,19 @@ const Sphere1 = Circular @deprecate PackedSphere1Sphere1(w...;kw...) PackedCircularCircular(w...;kw...) +# """ +# $(TYPEDEF) + +# TODO TO BE DEPRECATED +# """ +# mutable struct DebugCliqMCMC +# mcmc::Union{Nothing, Array{CliqGibbsMC,1}} +# outmsg::LikelihoodMessage +# outmsglbls::Dict{Symbol, Symbol} # Int +# priorprods::Vector{CliqGibbsMC} +# DebugCliqMCMC() = new() +# DebugCliqMCMC(a,b,c,d) = new(a,b,c,d) +# end + + # diff --git a/src/Factors/Circular.jl b/src/Factors/Circular.jl index e98ffe1b6..a5ea453b1 100644 --- a/src/Factors/Circular.jl +++ b/src/Factors/Circular.jl @@ -19,7 +19,6 @@ end const Sphere1Sphere1 = CircularCircular CircularCircular(::UniformScaling) = CircularCircular(Normal()) -# Sphere1Sphere1() getSample(cf::CalcFactor{<:CircularCircular}, N::Int=1) = (reshape(rand(cf.factor.Z,N),:,N), ) @@ -35,6 +34,9 @@ function (cf::CalcFactor{<:CircularCircular})(meas, end +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle{ℝ} + + """ $(TYPEDEF) @@ -62,6 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular}, N::Int=1) end +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle{ℝ} @@ -72,8 +75,8 @@ Serialized object for storing PriorCircular. """ mutable struct PackedPriorCircular <: IncrementalInference.PackedInferenceType datastr::String - PackedPriorCircular() = new() - PackedPriorCircular(x::String) = new(x) + # PackedPriorCircular() = new() + # PackedPriorCircular(x::String) = new(x) end function convert(::Type{PackedPriorCircular}, d::PriorCircular) return PackedPriorCircular(convert(PackedSamplableBelief, d.Z)) @@ -97,8 +100,8 @@ Serialized object for storing CircularCircular. """ mutable struct PackedCircularCircular <: IncrementalInference.PackedInferenceType datastr::String - PackedCircularCircular() = new() - PackedCircularCircular(x::String) = new(x) + # PackedCircularCircular() = new() + # PackedCircularCircular(x::String) = new(x) end function convert(::Type{CircularCircular}, d::PackedCircularCircular) return CircularCircular(convert(SamplableBelief, d.datastr)) diff --git a/src/Factors/EuclidDistance.jl b/src/Factors/EuclidDistance.jl index b1ce5ba00..900bf1f43 100644 --- a/src/Factors/EuclidDistance.jl +++ b/src/Factors/EuclidDistance.jl @@ -31,6 +31,7 @@ function (s::CalcFactor{<:EuclidDistance})(z, x1, x2) end +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = Manifolds.Euclidean{Tuple{1}, ℝ} """ diff --git a/src/Factors/LinearRelative.jl b/src/Factors/LinearRelative.jl index 32c21bf5d..821c9bd02 100644 --- a/src/Factors/LinearRelative.jl +++ b/src/Factors/LinearRelative.jl @@ -48,8 +48,8 @@ end -convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{1}}) = AMP.Euclid -convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{2}}) = AMP.Euclid2 +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ} +# convert(Manifold, ContinuousEuclid{N}) @@ -59,8 +59,8 @@ Serialization type for `LinearRelative` binary factor. """ mutable struct PackedLinearRelative <: PackedInferenceType Z::String - PackedLinearRelative() = new() - PackedLinearRelative(z::AS) where {AS <: AbstractString} = new(z) + # PackedLinearRelative() = new() + # PackedLinearRelative(z::AS) where {AS <: AbstractString} = new(z) end function convert(::Type{PackedLinearRelative}, d::LinearRelative) PackedLinearRelative(convert(PackedSamplableBelief, d.Z)) diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index 3dcfa08fe..327f5175c 100644 --- a/src/IncrementalInference.jl +++ b/src/IncrementalInference.jl @@ -12,6 +12,8 @@ using Reexport # @reexport using Graphs @reexport using LinearAlgebra +using Manifolds + import NLsolve import NLSolversBase import Optim diff --git a/src/SolverAPI.jl b/src/SolverAPI.jl index 3694ae11b..f68b89cd6 100644 --- a/src/SolverAPI.jl +++ b/src/SolverAPI.jl @@ -345,7 +345,7 @@ function solveTree!(dfgl::AbstractDFG, # if desired, drawtree in a loop treetask, _dotreedraw = drawTreeAsyncLoop(tree, opt; dotreedraw = dotreedraw) - @info "Do tree based init-inference on tree" + @info "Do tree based init-ference on tree" # choose algorithm if algorithm == :parametric @@ -373,7 +373,7 @@ function solveTree!(dfgl::AbstractDFG, verbose=verbose, verbosefid=verbosefid, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs) - @info "Finished tree based init-inference" + @info "Finished tree based init-ference" end end diff --git a/src/Variables/Circular.jl b/src/Variables/Circular.jl index b271e6e58..395d33d5f 100644 --- a/src/Variables/Circular.jl +++ b/src/Variables/Circular.jl @@ -8,3 +8,7 @@ Sphere1 is a S1 mechanization of one Circular rotation, with `theta in [-pi,pi)` """ @defVariable Circular 1 (:Circular,) + +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = Manifolds.Circle{ℝ} + +# \ No newline at end of file diff --git a/src/Variables/DefaultVariables.jl b/src/Variables/DefaultVariables.jl index 06a032e49..99b8f3c20 100644 --- a/src/Variables/DefaultVariables.jl +++ b/src/Variables/DefaultVariables.jl @@ -42,3 +42,8 @@ ContinuousEuclid(x::Int) = ContinuousEuclid{x}() getDimension(::ContinuousEuclid{N}) where N = N::Int getManifolds(::ContinuousEuclid{N}) where N = ntuple(i -> :Euclid, N) + + +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar}) = Manifolds.Euclidean{Tuple{1}, ℝ} # AMP.Euclid + From 5fa50103d89c8b0c7352b6f6a377ddc78d75b375 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 18 Mar 2021 22:00:43 -0400 Subject: [PATCH 2/3] more deprecations --- src/Deprecated.jl | 52 ++++++++++++++++++++++--------------------- src/SolveTree.jl | 24 ++++++++++---------- src/TreeDebugTools.jl | 12 ++++------ 3 files changed, 43 insertions(+), 45 deletions(-) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 85a168e9d..b0804b59c 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -21,31 +21,6 @@ function _evalType(pt::String)::Type end -""" -$(TYPEDEF) - -TODO TO BE DEPRECATED -""" -mutable struct PotProd - Xi::Symbol # Int - prev::Array{Float64,2} - product::Array{Float64,2} - potentials::Array{BallTreeDensity,1} - potentialfac::Vector{Symbol} -end - -""" -$(TYPEDEF) - -TODO TO BE DEPRECATED -""" -mutable struct CliqGibbsMC - prods::Array{PotProd,1} - lbls::Vector{Symbol} - CliqGibbsMC() = new() - CliqGibbsMC(a,b) = new(a,b) -end - ##============================================================================== @@ -88,6 +63,33 @@ const Sphere1 = Circular @deprecate PackedSphere1Sphere1(w...;kw...) PackedCircularCircular(w...;kw...) + +# """ +# $(TYPEDEF) + +# TODO TO BE DEPRECATED +# """ +# mutable struct PotProd +# Xi::Symbol # Int +# prev::Array{Float64,2} +# product::Array{Float64,2} +# potentials::Array{BallTreeDensity,1} +# potentialfac::Vector{Symbol} +# end + +# """ +# $(TYPEDEF) + +# TODO TO BE DEPRECATED +# """ +# mutable struct CliqGibbsMC +# prods::Array{PotProd,1} +# lbls::Vector{Symbol} +# CliqGibbsMC() = new() +# CliqGibbsMC(a,b) = new(a,b) +# end + + # """ # $(TYPEDEF) diff --git a/src/SolveTree.jl b/src/SolveTree.jl index 80325dc56..dfd29174d 100644 --- a/src/SolveTree.jl +++ b/src/SolveTree.jl @@ -65,7 +65,7 @@ function doFMCIteration(fgl::AbstractDFG, vert = DFG.getVariable(fgl, vsym) if !getSolverData(vert).ismargin - potprod = nothing + # potprod = nothing densPts, inferdim = predictbelief(fgl, vsym, :, needFreshMeasurements=needFreshMeasurements, N=N, dbg=dbg, logger=logger) if 0 < size(densPts,1) @@ -102,7 +102,7 @@ function fmcmc!(fgl::AbstractDFG, if length(lbls) == 1 MCMCIter=1 end - mcmcdbg = Array{CliqGibbsMC,1}() + # mcmcdbg = Array{CliqGibbsMC,1}() # burn-in loop for outer Gibbs for iter in 1:MCMCIter @@ -110,7 +110,7 @@ function fmcmc!(fgl::AbstractDFG, with_logger(logger) do @info "#$(iter)\t -- " end - dbgvals = !dbg ? nothing : CliqGibbsMC([], Symbol[]) + # dbgvals = !dbg ? nothing : CliqGibbsMC([], Symbol[]) needFreshMeasurements = iter == 1 || getSolverParams(fgl).alwaysFreshMeasurements @@ -118,13 +118,13 @@ function fmcmc!(fgl::AbstractDFG, for vsym in lbls doFMCIteration(fgl, vsym, cliq, fmsgs, N, dbg, needFreshMeasurements, logger) end - !dbg ? nothing : push!(mcmcdbg, dbgvals) + # !dbg ? nothing : push!(mcmcdbg, dbgvals) end # populate dictionary for return NBPMessage in multiple dispatch msgdict = compileFMCMessages(fgl, lbls, logger) - return mcmcdbg, msgdict + return msgdict end @@ -162,10 +162,10 @@ function upGibbsCliqueDensity(dfg::AbstractDFG, cliq::TreeClique, @info "up w $(length(inmsgs)) msgs" end # TODO -- some weirdness with: d,. = d = ., nothing - mcmcdbg = Array{CliqGibbsMC,1}() + # mcmcdbg = Array{CliqGibbsMC,1}() d = Dict{Symbol,TreeBelief}() - priorprods = Vector{CliqGibbsMC}() + # priorprods = Vector{CliqGibbsMC}() cliqdata = getCliqueData(cliq) @@ -173,23 +173,23 @@ function upGibbsCliqueDensity(dfg::AbstractDFG, cliq::TreeClique, # use nested structure for more efficient Chapman-Kolmogorov solution approximation if false IDS = [cliqdata.frontalIDs;cliqdata.separatorIDs] #inp.cliq.attributes["frontalIDs"] - mcmcdbg, d = fmcmc!(dfg, cliq, inmsgs, IDS, N, iters, dbg, logger) + d = fmcmc!(dfg, cliq, inmsgs, IDS, N, iters, dbg, logger) else # NOTE -- previous mistake, must iterate over directsvarIDs also (or incorporate once at the right time) # NOTE -- double up on directs to allow inflation to take proper affect, see #1051 - dummy, d = fmcmc!(dfg, cliq, inmsgs, cliqdata.directFrtlMsgIDs, N, 1, dbg, logger, true) + d = fmcmc!(dfg, cliq, inmsgs, cliqdata.directFrtlMsgIDs, N, 1, dbg, logger, true) if length(cliqdata.msgskipIDs) > 0 - dummy, dd = fmcmc!(dfg, cliq, inmsgs, cliqdata.msgskipIDs, N, 1, dbg, logger, true) + dd = fmcmc!(dfg, cliq, inmsgs, cliqdata.msgskipIDs, N, 1, dbg, logger, true) for md in dd d[md[1]] = md[2]; end end if length(cliqdata.itervarIDs) > 0 - mcmcdbg, ddd = fmcmc!(dfg, cliq, inmsgs, cliqdata.itervarIDs, N, iters, dbg, logger, false) + ddd = fmcmc!(dfg, cliq, inmsgs, cliqdata.itervarIDs, N, iters, dbg, logger, false) for md in ddd d[md[1]] = md[2]; end end if length(cliqdata.directPriorMsgIDs) > 0 doids = setdiff(cliqdata.directPriorMsgIDs, cliqdata.msgskipIDs) - priorprods, dddd = fmcmc!(dfg, cliq, inmsgs, doids, N, 1, dbg, logger, true) + dddd = fmcmc!(dfg, cliq, inmsgs, doids, N, 1, dbg, logger, true) for md in dddd d[md[1]] = md[2]; end end end diff --git a/src/TreeDebugTools.jl b/src/TreeDebugTools.jl index a75e4d1e8..3d3c703be 100644 --- a/src/TreeDebugTools.jl +++ b/src/TreeDebugTools.jl @@ -71,15 +71,13 @@ function treeProductUp(fg::AbstractDFG, # end # push!( upmsgssym, LikelihoodMessage(beliefDict=dict) ) # end - upmsgssym = fetchMsgsUpChildren(tree, cliq, TreeBelief) + + # upmsgssym = fetchMsgsUpChildren(tree, cliq, TreeBelief) # perform the actual computation - potprod = nothing pGM, fulldim = predictbelief(fg, sym, :, N=N, dbg=dbg ) - # manis = getVariableType(getVariable(fg, sym)) |> getManifolds - # pGM, potprod, fulldim = cliqGibbs( fg, cliq, sym, upmsgssym, N, dbg, manis ) - return pGM, potprod + return pGM, nothing end @@ -115,11 +113,9 @@ function treeProductDwn(fg::G, dwnmsgssym = LikelihoodMessage[LikelihoodMessage(dict);] # perform the actual computation - potprod = nothing pGM, fulldim = predictbelief(fg, sym, :, N=N, dbg=dbg) - # pGM, potprod, fulldim = cliqGibbs( fg, cliq, sym, dwnmsgssym, N, dbg ) #vertid - return pGM, potprod, sym, dwnmsgssym + return pGM, nothing, sym, dwnmsgssym end From f9a5bab78e938d63f8a40d1bc739fa7c52a88518 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 25 Mar 2021 23:13:57 -0400 Subject: [PATCH 3/3] mkd to Manifold objects, AMP v0.3.1 --- Project.toml | 2 +- src/Deprecated.jl | 3 +- src/FGOSUtils.jl | 43 +++++++++--------- src/Factors/Circular.jl | 4 +- src/Factors/EuclidDistance.jl | 2 +- src/Factors/LinearRelative.jl | 2 +- src/IncrementalInference.jl | 2 + src/JunctionTree.jl | 67 +++++++++++++++------------- src/Variables/Circular.jl | 4 +- src/Variables/DefaultVariables.jl | 4 +- test/testCliqSolveDbgUtils.jl | 8 ++++ test/testMixtureLinearConditional.jl | 1 + test/testSphere1.jl | 8 ++-- 13 files changed, 81 insertions(+), 69 deletions(-) diff --git a/Project.toml b/Project.toml index d81d8ccc6..6a04ee7a7 100644 --- a/Project.toml +++ b/Project.toml @@ -41,7 +41,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53" UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" [compat] -ApproxManifoldProducts = "0.3" +ApproxManifoldProducts = "0.3.1" BSON = "0.2, 0.3" Combinatorics = "1.0" DataStructures = "0.16, 0.17, 0.18" diff --git a/src/Deprecated.jl b/src/Deprecated.jl index b0804b59c..55940f42e 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -34,7 +34,6 @@ getManifolds(::InstanceType{Manifolds.Circle{ℝ}}) = (:Circular,) - ##============================================================================== ## Deprecate code below before v0.23 ##============================================================================== @@ -53,7 +52,7 @@ getManifolds(::InstanceType{Manifolds.Circle{ℝ}}) = (:Circular,) export Sphere1 -@warn "Deprecating old use of Sphere1, being replaced by Cicular instead" +@warn "Deprecating old use of Sphere1, being replaced by Circular instead" const Sphere1 = Circular # @deprecate Sphere1(w...;kw...) Circular(w...;kw...) diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl index c854e4ef5..fd8f8b307 100644 --- a/src/FGOSUtils.jl +++ b/src/FGOSUtils.jl @@ -81,7 +81,7 @@ end # extend convenience function function manikde!(pts::AbstractArray{Float64,2}, bws::Vector{Float64}, - variableType::Union{InstanceType{InferenceVariable}, InstanceType{FunctorInferenceType}} ) + variableType::Union{<:InstanceType{InferenceVariable}, <:InstanceType{FunctorInferenceType}} ) # addopT, diffopT, getManiMu, getManiLam = buildHybridManifoldCallbacks(getManifolds(variableType)) bel = KernelDensityEstimate.kde!(pts, bws, addopT, diffopT) @@ -186,12 +186,6 @@ end -function calcMean(mkd::ManifoldKernelDensity) - error("not implemented yet") - -end - - """ $SIGNATURES @@ -199,6 +193,7 @@ Get the ParametricPointEstimates---based on full marginal belief estimates---of DevNotes - TODO update for manifold subgroups. +- TODO standardize after AMP3D Related @@ -210,24 +205,26 @@ function calcPPE( var::DFGVariable, solveKey::Symbol=:default ) # P = getBelief(var, solveKey) - manis = getManifolds(varType) # getManifolds(vnd) + maniDef = convert(Manifold, varType) + manis = getManifolds(maniDef) # varType # getManifolds(vnd) ops = buildHybridManifoldCallbacks(manis) - Pme = getKDEMean(P) #, addop=ops[1], diffop=ops[2] + Pme = calcMean(P) # getKDEMean(P) #, addop=ops[1], diffop=ops[2] Pma = getKDEMax(P, addop=ops[1], diffop=ops[2]) - suggested = zeros(getDimension(var)) - # TODO standardize after AMP3D - @assert length(manis) == getDimension(var) - for i in 1:length(manis) - mani = manis[i] - if mani == :Euclid - suggested[i] = Pme[i] - elseif mani == :Circular - suggested[i] = Pma[i] - else - error("Unknown manifold to find PPE, $varType, $mani") - end - end - MeanMaxPPE(solveKey, suggested, Pma, Pme, now()) + # suggested = zeros(getDimension(var)) + # @assert length(manis) == getDimension(var) + # for i in 1:length(manis) + # mani = manis[i] + # if mani == :Euclid + # suggested[i] = Pme[i] + # elseif mani == :Circular + # suggested[i] = Pma[i] + # else + # error("Unknown manifold to find PPE, $varType, $mani") + # end + # end + + # suggested, max, mean, current time + MeanMaxPPE(solveKey, Pme, Pma, Pme, now()) end diff --git a/src/Factors/Circular.jl b/src/Factors/Circular.jl index a5ea453b1..6876a9b74 100644 --- a/src/Factors/Circular.jl +++ b/src/Factors/Circular.jl @@ -34,7 +34,7 @@ function (cf::CalcFactor{<:CircularCircular})(meas, end -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle{ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{CircularCircular}) = Manifolds.Circle() """ @@ -64,7 +64,7 @@ function getSample(cf::CalcFactor{<:PriorCircular}, N::Int=1) end -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle{ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{PriorCircular}) = Manifolds.Circle() diff --git a/src/Factors/EuclidDistance.jl b/src/Factors/EuclidDistance.jl index 900bf1f43..8110bcb9b 100644 --- a/src/Factors/EuclidDistance.jl +++ b/src/Factors/EuclidDistance.jl @@ -31,7 +31,7 @@ function (s::CalcFactor{<:EuclidDistance})(z, x1, x2) end -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = Manifolds.Euclidean{Tuple{1}, ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{EuclidDistance}) = Manifolds.Euclidean(1) """ diff --git a/src/Factors/LinearRelative.jl b/src/Factors/LinearRelative.jl index 821c9bd02..e660d9182 100644 --- a/src/Factors/LinearRelative.jl +++ b/src/Factors/LinearRelative.jl @@ -48,7 +48,7 @@ end -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{LinearRelative{N}}) where N = Manifolds.Euclidean(N) # convert(Manifold, ContinuousEuclid{N}) diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index 327f5175c..af89e08e4 100644 --- a/src/IncrementalInference.jl +++ b/src/IncrementalInference.jl @@ -14,6 +14,8 @@ using Reexport using Manifolds +export ℝ, Manifold, Euclidean, Circle + import NLsolve import NLSolversBase import Optim diff --git a/src/JunctionTree.jl b/src/JunctionTree.jl index 947fc2c64..21ba7bb8f 100644 --- a/src/JunctionTree.jl +++ b/src/JunctionTree.jl @@ -381,39 +381,39 @@ Kaess et al.: iSAM2, IJRR, 2011, [Alg. 3] Fourie, D.: mmisam, PhD thesis, 2017. [Chpt. 5] """ function newPotential(tree::AbstractBayesTree, dfg::G, var::Symbol, elimorder::Array{Symbol,1}) where G <: AbstractDFG - firvert = DFG.getVariable(dfg,var) - # no parent - if (length(getSolverData(firvert).separator) == 0) - # if (length(getCliques(tree)) == 0) - # create new root - addClique!(tree, dfg, var) - # else - # # add to root - # @warn "root append clique is happening" - # appendClique!(tree, 1, dfg, var) - # end + firvert = DFG.getVariable(dfg,var) + # no parent + if (length(getSolverData(firvert).separator) == 0) + # if (length(getCliques(tree)) == 0) + # create new root + addClique!(tree, dfg, var) + # else + # # add to root + # @warn "root append clique is happening" + # appendClique!(tree, 1, dfg, var) + # end + else + # find parent clique Cp that containts the first eliminated variable of Sj as frontal + Sj = getSolverData(firvert).separator + felbl = identifyFirstEliminatedSeparator(dfg, elimorder, firvert, Sj).label + # get clique id of first eliminated frontal + CpID = tree.frontals[felbl] + # look to add this conditional to the tree + cliq = getClique(tree, CpID) + # clique of the first eliminated frontal + unFC = union(getCliqFrontalVarIds(cliq), getCliqSeparatorVarIds(cliq)) + # if the separator of this new variable is identical to the (entire) clique of the firstly eliminated frontal. + if (sort(unFC) == sort(Sj)) + # just add new variable as frontal to this clique + # insert conditional (p(var|sepr)) into clique CpID -- i.e. just adding a frontal + # @info "adding new frontal $var to existing clique $CpID" + appendClique!(tree, CpID, dfg, var) else - # find parent clique Cp that containts the first eliminated variable of Sj as frontal - Sj = getSolverData(firvert).separator - felbl = identifyFirstEliminatedSeparator(dfg, elimorder, firvert, Sj).label - # get clique id of first eliminated frontal - CpID = tree.frontals[felbl] - # look to add this conditional to the tree - cliq = getClique(tree, CpID) - # clique of the first eliminated frontal - unFC = union(getCliqFrontalVarIds(cliq), getCliqSeparatorVarIds(cliq)) - # if the separator of this new variable is identical to the (entire) clique of the firstly eliminated frontal. - if (sort(unFC) == sort(Sj)) - # just add new variable as frontal to this clique - # insert conditional (p(var|sepr)) into clique CpID -- i.e. just adding a frontal - # @info "adding new frontal $var to existing clique $CpID" - appendClique!(tree, CpID, dfg, var) - else - # a new child clique is required here (this becomes parent) - # @info "adding new child clique with parent separator." - newChildClique!(tree, dfg, CpID, var, Sj) - end + # a new child clique is required here (this becomes parent) + # @info "adding new child clique with parent separator." + newChildClique!(tree, dfg, CpID, var, Sj) end + end end """ @@ -1260,6 +1260,10 @@ function compCliqAssocMatrices!(dfg::G, bt::AbstractBayesTree, cliq::TreeClique) cols = [frtl;cond] getCliqueData(cliq).inmsgIDs = inmsgIDs getCliqueData(cliq).potIDs = potIDs + + @debug "Building cliqAssocMat" cliq + @debug "Building cliqAssocMat" cliq.id string(inmsgIDs) string(potIDs) + cliqAssocMat = Array{Bool,2}(undef, length(potIDs), length(cols)) cliqMsgMat = Array{Bool,2}(undef, length(inmsgIDs), length(cols)) fill!(cliqAssocMat, false) @@ -1284,6 +1288,7 @@ function compCliqAssocMatrices!(dfg::G, bt::AbstractBayesTree, cliq::TreeClique) end end end + @debug "Final cliqAssocMat" cliq.id cliqAssocMat getCliqueData(cliq).cliqAssocMat = cliqAssocMat getCliqueData(cliq).cliqMsgMat = cliqMsgMat nothing diff --git a/src/Variables/Circular.jl b/src/Variables/Circular.jl index 395d33d5f..acbbe1a4c 100644 --- a/src/Variables/Circular.jl +++ b/src/Variables/Circular.jl @@ -1,5 +1,5 @@ -export Circular +export Circular, Circle """ $(TYPEDEF) @@ -9,6 +9,6 @@ Sphere1 is a S1 mechanization of one Circular rotation, with `theta in [-pi,pi)` @defVariable Circular 1 (:Circular,) -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = Manifolds.Circle{ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Circular}) = Manifolds.Circle() # \ No newline at end of file diff --git a/src/Variables/DefaultVariables.jl b/src/Variables/DefaultVariables.jl index 99b8f3c20..9c257adf8 100644 --- a/src/Variables/DefaultVariables.jl +++ b/src/Variables/DefaultVariables.jl @@ -44,6 +44,6 @@ getDimension(::ContinuousEuclid{N}) where N = N::Int getManifolds(::ContinuousEuclid{N}) where N = ntuple(i -> :Euclid, N) -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{N}}) where N = Manifolds.Euclidean{Tuple{N}, ℝ} -Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar}) = Manifolds.Euclidean{Tuple{1}, ℝ} # AMP.Euclid +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousEuclid{N}}) where N = Manifolds.Euclidean(N) # {Tuple{N}, ℝ} +Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{ContinuousScalar}) = Manifolds.Euclidean(1) # {Tuple{1}, ℝ} # AMP.Euclid diff --git a/test/testCliqSolveDbgUtils.jl b/test/testCliqSolveDbgUtils.jl index 5da6589cf..6c65af1be 100644 --- a/test/testCliqSolveDbgUtils.jl +++ b/test/testCliqSolveDbgUtils.jl @@ -1,8 +1,12 @@ using IncrementalInference using Test +## + @testset "Test solveCliqueUp! and solveCliqDown!" begin + ## + N=8 fg = generateCanonicalFG_lineStep(N; graphinit=false, @@ -11,9 +15,11 @@ fg = generateCanonicalFG_lineStep(N; posePriorsAt=[0], landmarkPriorsAt=[], sightDistance=N+1) +# deleteFactor!.(fg, [Symbol("x$(i)lm0f1") for i=1:(N-1)]) +# test the ensureAllInitialized! separately anyway ensureAllInitialized!(fg) tree = buildTreeReset!(fg) @@ -30,7 +36,9 @@ a,b = solveCliqUp!(fg, tree, 2; recordcliq = true) a,b = solveCliqDown!(fg, tree, 2) a,b = solveCliqDown!(fg, tree, 2; recordcliq = true) @test length(a) > 0 + ## + end diff --git a/test/testMixtureLinearConditional.jl b/test/testMixtureLinearConditional.jl index e911f61bc..e7fff3de4 100644 --- a/test/testMixtureLinearConditional.jl +++ b/test/testMixtureLinearConditional.jl @@ -3,6 +3,7 @@ using IncrementalInference using Test +# using Manifolds # should be done within regular exports ## diff --git a/test/testSphere1.jl b/test/testSphere1.jl index d4c39210b..47a43ee02 100644 --- a/test/testSphere1.jl +++ b/test/testSphere1.jl @@ -4,16 +4,16 @@ using Test ## -@testset "test Sphere1D" begin +@testset "test Circular" begin ## fg = initfg() getSolverParams(fg).useMsgLikelihoods = true -addVariable!.(fg, [Symbol("x$i") for i=0:4], Sphere1) -addFactor!(fg, [:x0], PriorSphere1(Normal(0.0,0.1))) -map(i->addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], Sphere1Sphere1(Normal(1.0, 0.1))), 0:3) +addVariable!.(fg, [Symbol("x$i") for i=0:4], Circular) +addFactor!(fg, [:x0], PriorCircular(Normal(0.0,0.1))) +map(i->addFactor!(fg, [Symbol("x$i"),Symbol("x$(i+1)")], CircularCircular(Normal(1.0, 0.1))), 0:3) solveTree!(fg);