From f9a5bab78e938d63f8a40d1bc739fa7c52a88518 Mon Sep 17 00:00:00 2001 From: dehann Date: Thu, 25 Mar 2021 23:13:57 -0400 Subject: [PATCH] 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);