diff --git a/src/BeliefTypes.jl b/src/BeliefTypes.jl index 053a87ccd..654343819 100644 --- a/src/BeliefTypes.jl +++ b/src/BeliefTypes.jl @@ -2,9 +2,9 @@ """ CliqStatus Clique status message enumerated type with status: -initialized, upsolved, marginalized, downsolved, uprecycled +NULL, INITIALIZED, UPSOLVED, MARGINALIZED, DOWNSOLVED, UPRECYCLED, ERROR_STATUS """ -@enum CliqStatus NULL initialized upsolved marginalized downsolved uprecycled error_status +@enum CliqStatus NULL INITIALIZED UPSOLVED MARGINALIZED DOWNSOLVED UPRECYCLED ERROR_STATUS """ @@ -51,37 +51,36 @@ getManifolds(treeb::TreeBelief) = getManifolds(treeb.softtype) Belief message for message passing on the tree. Notes: -- belief -> common mode -- cobelief -> differential mode +- belief -> Dictionary of [`TreeBelief`](@ref) +- variableOrder -> Ordered variable id list of the seperators in cliqueLikelihood +- cliqueLikelihood -> marginal distribution (<: `SamplableBelief`) over clique seperators. DevNotes: - Objective for parametric: `MvNormal(μ=[:x0;:x2;:l5], Σ=[+ * *; * + *; * * +])` - TODO confirm why <: Singleton - +- #459 $(TYPEDFIELDS) """ mutable struct LikelihoodMessage <: Singleton status::CliqStatus belief::Dict{Symbol, TreeBelief} - cobelief::NamedTuple{(:varlbl, :μ, :Σ),Tuple{Vector{Symbol}, Vector{Float64}, Matrix{Float64}}} #TODO name something mathier + variableOrder::Vector{Symbol} + cliqueLikelihood::Union{Nothing,SamplableBelief} end # EARLIER NAMES INCLUDE: productFactor, Fnew, MsgPrior, LikelihoodMessage -#struct LikelihoodMessage{T <: SamplableBelief} #<: Singleton - # status::CliqStatus - # variableOrder::Vector{Symbol} - # cliqueLikelihood::{T} # MvNormal for parametric -#end + LikelihoodMessage(status::CliqStatus) = - LikelihoodMessage(status, Dict{Symbol, TreeBelief}(), (varlbl=Symbol[], μ=Float64[], Σ=Matrix{Float64}(undef,0,0))) + LikelihoodMessage(status, Dict{Symbol, TreeBelief}(), Symbol[], nothing) -LikelihoodMessage(status::CliqStatus, cobelief) = - LikelihoodMessage(status, Dict{Symbol, TreeBelief}(), cobelief) +LikelihoodMessage(status::CliqStatus, cliqueLikelihood::SamplableBelief) = + LikelihoodMessage(status, Dict{Symbol, TreeBelief}(), Symbol[], cliqueLikelihood) LikelihoodMessage(;status::CliqStatus=NULL, beliefDict::Dict=Dict{Symbol, TreeBelief}(), - cobelief=(varlbl=Symbol[], μ=Float64[], Σ=Matrix{Float64}(undef,0,0)) ) = - LikelihoodMessage(status, beliefDict, cobelief) + variableOrder=Symbol[], + cliqueLikelihood=nothing ) = + LikelihoodMessage(status, beliefDict, variableOrder, cliqueLikelihood) # @@ -95,7 +94,7 @@ const IntermediateMultiSiblingMessages = Dict{Symbol, IntermediateSiblingMessage # TODO this is casing problems between nonparametric and parametric -const BeliefMessage = LikelihoodMessage +# const BeliefMessage = LikelihoodMessage # Deprecated, replaced by LikelihoodMessage diff --git a/src/JunctionTree.jl b/src/JunctionTree.jl index 35f5a1926..e9250d025 100644 --- a/src/JunctionTree.jl +++ b/src/JunctionTree.jl @@ -641,15 +641,15 @@ Experimental create and initialize tree message channels """ function initTreeMessageChannels!(tree::BayesTree) for e = 1:tree.bt.nedges - push!(tree.messages, e=>(upMsg=Channel{BeliefMessage}(0),downMsg=Channel{BeliefMessage}(0))) + push!(tree.messages, e=>(upMsg=Channel{LikelihoodMessage}(0),downMsg=Channel{LikelihoodMessage}(0))) end return nothing end function initTreeMessageChannels!(tree::MetaBayesTree) for e = MetaGraphs.edges(tree.bt) - set_props!(tree.bt, e, Dict{Symbol,Any}(:upMsg=>Channel{BeliefMessage}(0),:downMsg=>Channel{BeliefMessage}(0))) - # push!(tree.messages, e=>(upMsg=Channel{BeliefMessage}(0),downMsg=Channel{BeliefMessage}(0))) + set_props!(tree.bt, e, Dict{Symbol,Any}(:upMsg=>Channel{LikelihoodMessage}(0),:downMsg=>Channel{LikelihoodMessage}(0))) + # push!(tree.messages, e=>(upMsg=Channel{LikelihoodMessage}(0),downMsg=Channel{LikelihoodMessage}(0))) end return nothing end diff --git a/src/JunctionTreeTypes.jl b/src/JunctionTreeTypes.jl index 092303449..cb17d18b7 100644 --- a/src/JunctionTreeTypes.jl +++ b/src/JunctionTreeTypes.jl @@ -52,7 +52,7 @@ mutable struct BayesTree <: AbstractBayesTree cliques::Dict{Int,TreeClique} frontals::Dict{Symbol,Int} #TEMP JT for evaluation, store message channels associated with edges between nodes Int -> edge id. TODO rather store in graph - messages::Dict{Int, NamedTuple{(:upMsg, :downMsg),Tuple{Channel{BeliefMessage},Channel{BeliefMessage}}}} + messages::Dict{Int, NamedTuple{(:upMsg, :downMsg),Tuple{Channel{LikelihoodMessage},Channel{LikelihoodMessage}}}} variableOrder::Vector{Symbol} buildTime::Float64 end @@ -61,7 +61,7 @@ BayesTree() = BayesTree(Graphs.inclist(TreeClique,is_directed=true), 0, Dict{Int,TreeClique}(), Dict{AbstractString, Int}(), - Dict{Int, NamedTuple{(:upMsg, :downMsg),Tuple{Channel{BeliefMessage},Channel{BeliefMessage}}}}(), + Dict{Int, NamedTuple{(:upMsg, :downMsg),Tuple{Channel{LikelihoodMessage},Channel{LikelihoodMessage}}}}(), Symbol[], 0.0 ) @@ -232,8 +232,8 @@ mutable struct CliqStateMachineContainer{BTND, T <: AbstractDFG, InMemG <: InMem refactoring::Dict{Symbol, String} oldcliqdata::BTND logger::SimpleLogger - msgsUp::Vector{BeliefMessage} #TODO towards consolidated messages - msgsDown::Vector{BeliefMessage} + msgsUp::Vector{LikelihoodMessage} #TODO towards consolidated messages + msgsDown::Vector{LikelihoodMessage} end const CSMHistory = Vector{Tuple{DateTime, Int, Function, CliqStateMachineContainer}} @@ -255,7 +255,7 @@ function CliqStateMachineContainer(x1::G, x13::SimpleLogger=SimpleLogger(Base.stdout); x4i::Int = x4.index) where {BTND, G <: AbstractDFG} # - CliqStateMachineContainer{BTND, G, typeof(x2), typeof(x3)}(x1,x2,x3,x4,x4i,x5,x6,x7,x8,x9,x10,x10aa,x10aaa,x10b,x11,x13, BeliefMessage[], BeliefMessage[]) + CliqStateMachineContainer{BTND, G, typeof(x2), typeof(x3)}(x1,x2,x3,x4,x4i,x5,x6,x7,x8,x9,x10,x10aa,x10aaa,x10b,x11,x13, LikelihoodMessage[], LikelihoodMessage[]) end @@ -287,7 +287,7 @@ mutable struct BayesTreeNodeData debugDwn # future might concentrate these four fields down to two - # these should become specialized BeliefMessage type + # these should become specialized LikelihoodMessage type upMsg::LikelihoodMessage dwnMsg::LikelihoodMessage upInitMsgs::Dict{Int, LikelihoodMessage} diff --git a/src/ParametricCliqStateMachine.jl b/src/ParametricCliqStateMachine.jl index 55ca4805b..7873bafe0 100644 --- a/src/ParametricCliqStateMachine.jl +++ b/src/ParametricCliqStateMachine.jl @@ -1,4 +1,4 @@ -# STATUS messages [:initialized;:upsolved;:marginalized;:downsolved;:uprecycled] + """ $SIGNATURES @@ -31,7 +31,7 @@ function initStartCliqStateMachineParametric!(dfg::G, prnt, children, false, incremental, drawtree, downsolve, delay, getSolverParams(dfg), Dict{Symbol,String}(), oldcliqdata, logger, - BeliefMessage[], BeliefMessage[]) + LikelihoodMessage[], LikelihoodMessage[]) # nxt = upsolve ? testCliqCanRecycled_ParametricStateMachine : (downsolve ? testCliqCanRecycled_ParametricStateMachine : error("must attempt either up or down solve")) nxt = buildCliqSubgraph_ParametricStateMachine @@ -91,7 +91,7 @@ function waitForUp_ParametricStateMachine(csmc::CliqStateMachineContainer) csmc.drawtree ? drawTree(csmc.tree, show=false, filepath=joinpath(getSolverParams(csmc.dfg).logpath,"bt.pdf")) : nothing childrenOk = true - beliefMessages = BeliefMessage[] + beliefMessages = LikelihoodMessage[] @sync for e in getEdgesChildren(csmc.tree, csmc.cliq) @async begin @@ -106,7 +106,7 @@ function waitForUp_ParametricStateMachine(csmc::CliqStateMachineContainer) for beliefMsg in beliefMessages #save up message (and add priors to cliqSubFg) #kies csmc vir boodskappe vir debugging, dis 'n vector een per kind knoop - if beliefMsg.status == upsolved + if beliefMsg.status == UPSOLVED push!(csmc.msgsUp, beliefMsg) else @@ -115,14 +115,14 @@ function waitForUp_ParametricStateMachine(csmc::CliqStateMachineContainer) for e in getEdgesParent(csmc.tree, csmc.cliq) @info "Par-2, $(csmc.cliq.index): propagate up error on edge $(isa(e,Graphs.Edge) ? e.index : e)" - putBeliefMessageUp!(csmc.tree, e, BeliefMessage(error_status))#put!(csmc.tree.messages[e.index].upMsg, BeliefMessage(error_status)) + putBeliefMessageUp!(csmc.tree, e, LikelihoodMessage(ERROR_STATUS))#put!(csmc.tree.messages[e.index].upMsg, LikelihoodMessage(ERROR_STATUS)) end #if its the root, propagate error down #FIXME rather check if no parents with function (hasParents or isRoot) if length(getParent(csmc.tree, csmc.cliq)) == 0 @sync for e in getEdgesChildren(csmc.tree, csmc.cliq) @info "Par-2 Root $(csmc.cliq.index): propagate down error on edge $(isa(e,Graphs.Edge) ? e.index : e)" - @async putBeliefMessageDown!(csmc.tree, e, BeliefMessage(error_status))#put!(csmc.tree.messages[e.index].downMsg, BeliefMessage(error_status)) + @async putBeliefMessageDown!(csmc.tree, e, LikelihoodMessage(ERROR_STATUS))#put!(csmc.tree.messages[e.index].downMsg, LikelihoodMessage(ERROR_STATUS)) end @error "Par-2 $(csmc.cliq.index): Exit with error state" return IncrementalInference.exitStateMachine @@ -180,7 +180,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer) vardict, result, varIds, Σ = solveFactorGraphParametric(csmc.cliqSubFg) - @info "$(csmc.cliq.index) vars $(keys(varIds.idx))" + @info "$(csmc.cliq.index) vars $(keys(varIds))" # @info "$(csmc.cliq.index) Σ $(Σ)" # Pack all results in variables # FIXME test f_converged, ls_success, confirm convergence check @@ -203,7 +203,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer) @error "Par-3, clique $(csmc.cliq.index) failed to converge in upsolve" result # propagate error to cleanly exit all cliques? - beliefMsg = BeliefMessage(error_status) + beliefMsg = LikelihoodMessage(ERROR_STATUS) for e in getEdgesParent(csmc.tree, csmc.cliq) @info "Par-3, $(csmc.cliq.index): generate up error on edge $(isa(e,Graphs.Edge) ? e.index : e)" putBeliefMessageUp!(csmc.tree, e, beliefMsg)# put!(csmc.tree.messages[e.index].upMsg, beliefMsg) @@ -212,7 +212,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer) if length(getParent(csmc.tree, csmc.cliq)) == 0 @sync for e in getEdgesChildren(csmc.tree, csmc.cliq) @info "Par-3 Root $(csmc.cliq.index): generate down error on edge $(isa(e,Graphs.Edge) ? e.index : e)" - @async putBeliefMessageDown!(csmc.tree, e, BeliefMessage(error_status))#put!(csmc.tree.messages[e.index].downMsg, BeliefMessage(error_status)) + @async putBeliefMessageDown!(csmc.tree, e, LikelihoodMessage(ERROR_STATUS))#put!(csmc.tree.messages[e.index].downMsg, LikelihoodMessage(ERROR_STATUS)) end @error "Par-3 $(csmc.cliq.index): Exit with error state" return IncrementalInference.exitStateMachine @@ -233,7 +233,7 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer) #fill in belief #TODO createBeliefMessageParametric(csmc.cliqSubFg, csmc.cliq, solvekey=opts.solvekey) cliqSeparatorVarIds = getCliqSeparatorVarIds(csmc.cliq) - beliefMsg = BeliefMessage(upsolved) + beliefMsg = LikelihoodMessage(UPSOLVED) #FIXME this is a bit of a hack to only send messages if there are priors or for now more than one seperator if length(lsfPriors(csmc.cliqSubFg)) > 0 || length(cliqSeparatorVarIds) > 1 @@ -274,7 +274,7 @@ function waitForDown_ParametricStateMachine(csmc::CliqStateMachineContainer) #save down messages in msgsDown - if beliefMsg.status == downsolved + if beliefMsg.status == DOWNSOLVED push!(csmc.msgsDown, beliefMsg) else @@ -283,7 +283,7 @@ function waitForDown_ParametricStateMachine(csmc::CliqStateMachineContainer) @sync for e in getEdgesChildren(csmc.tree, csmc.cliq) @info "Par-4, $(csmc.cliq.index): put! error on edge $(isa(e,Graphs.Edge) ? e.index : e)" - @async putBeliefMessageDown!(csmc.tree, e, BeliefMessage(error_status))#put!(csmc.tree.messages[e.index].downMsg, BeliefMessage(error_status)) + @async putBeliefMessageDown!(csmc.tree, e, LikelihoodMessage(ERROR_STATUS))#put!(csmc.tree.messages[e.index].downMsg, LikelihoodMessage(ERROR_STATUS)) end @error "Par-4, $(csmc.cliq.index): Exit with error state" return IncrementalInference.exitStateMachine @@ -357,7 +357,7 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer) @error "Par-5, clique $(csmc.cliq.index) failed to converge in down solve" result #propagate error to cleanly exit all cliques? - beliefMsg = BeliefMessage(error_status) + beliefMsg = LikelihoodMessage(ERROR_STATUS) @sync for e in getEdgesChildren(csmc.tree, csmc.cliq) @info "Par-5, $(csmc.cliq.index): put! error on edge $(isa(e,Graphs.Edge) ? e.index : e)" @async putBeliefMessageDown!(csmc.tree, e, beliefMsg)#put!(csmc.tree.messages[e.index].downMsg, beliefMsg) @@ -372,7 +372,7 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer) cliqFrontalVarIds = getCliqFrontalVarIds(csmc.cliq) #TODO createBeliefMessageParametric # beliefMsg = createBeliefMessageParametric(csmc.cliqSubFg, cliqFrontalVarIds, solvekey=opts.solvekey) - beliefMsg = BeliefMessage(downsolved) + beliefMsg = LikelihoodMessage(DOWNSOLVED) for fi in cliqFrontalVarIds vnd = getSolverData(getVariable(csmc.cliqSubFg, fi), :parametric) beliefMsg.belief[fi] = TreeBelief(vnd) diff --git a/src/ParametricMessageUtils.jl b/src/ParametricMessageUtils.jl index cbc729ce4..66b2496d2 100644 --- a/src/ParametricMessageUtils.jl +++ b/src/ParametricMessageUtils.jl @@ -41,13 +41,13 @@ end Put a belief message on the down tree message channel edge. Blocks until a take! is performed by a different task. """ -function putBeliefMessageDown!(tree::BayesTree, edge, beliefMsg::BeliefMessage) +function putBeliefMessageDown!(tree::BayesTree, edge, beliefMsg::LikelihoodMessage) # Blocks until data is available. put!(tree.messages[edge.index].downMsg, beliefMsg) return beliefMsg end -function putBeliefMessageDown!(tree::MetaBayesTree, edge, beliefMsg::BeliefMessage) +function putBeliefMessageDown!(tree::MetaBayesTree, edge, beliefMsg::LikelihoodMessage) # Blocks until data is available. put!(MetaGraphs.get_prop(tree.bt, edge, :downMsg), beliefMsg) return beliefMsg @@ -59,13 +59,13 @@ end Put a belief message on the up tree message channel `edge`. Blocks until a take! is performed by a different task. """ -function putBeliefMessageUp!(tree::BayesTree, edge, beliefMsg::BeliefMessage) +function putBeliefMessageUp!(tree::BayesTree, edge, beliefMsg::LikelihoodMessage) # Blocks until data is available. put!(tree.messages[edge.index].upMsg, beliefMsg) return beliefMsg end -function putBeliefMessageUp!(tree::MetaBayesTree, edge, beliefMsg::BeliefMessage) +function putBeliefMessageUp!(tree::MetaBayesTree, edge, beliefMsg::LikelihoodMessage) # Blocks until data is available. put!(MetaGraphs.get_prop(tree.bt, edge, :upMsg), beliefMsg) return beliefMsg diff --git a/src/ParametricUtils.jl b/src/ParametricUtils.jl index b300e2659..8666efcb3 100644 --- a/src/ParametricUtils.jl +++ b/src/ParametricUtils.jl @@ -104,7 +104,7 @@ function solveFactorGraphParametric(fg::AbstractDFG; push!(d,key=>(val=rv[r],cov=Σ[r,r])) end - return d, result, flatvar, Σ + return d, result, flatvar.idx, Σ end #TODO maybe consolidate with solveFactorGraphParametric @@ -188,17 +188,17 @@ function solveConditionalsParametric(fg::AbstractDFG, push!(d,key=>(val=rv[r],cov=Σ[r,r])) end - return d, result, flatvar, Σ + return d, result, flatvar.idx, Σ end """ $SIGNATURES Get the indexes for labels in FlatVariables """ -function collectIdx(flatvars, labels) +function collectIdx(varinds, labels) idx = Int[] for lbl in labels - append!(idx, flatvars.idx[lbl]) + append!(idx, varinds[lbl]) end return idx end diff --git a/src/TreeMessageUtils.jl b/src/TreeMessageUtils.jl index ba4143f6b..7bbbe06db 100644 --- a/src/TreeMessageUtils.jl +++ b/src/TreeMessageUtils.jl @@ -70,7 +70,7 @@ end # Consolidate with nonparametric addMsgFactors! ? function addMsgFactors_Parametric!(subfg::AbstractDFG, - msgs::BeliefMessage)::Vector{DFGFactor} + msgs::LikelihoodMessage)::Vector{DFGFactor} # add messages as priors to this sub factor graph msgfcts = DFGFactor[] svars = DFG.listVariables(subfg) diff --git a/test/testBasicParametric.jl b/test/testBasicParametric.jl index 4a47d4c23..0fbb47109 100644 --- a/test/testBasicParametric.jl +++ b/test/testBasicParametric.jl @@ -189,6 +189,8 @@ end ############################################################################### #Test error prop if not converged. +#TODO Update test, it now converges + fg = generateCanonicalFG_lineStep(20, vardims=2, poseEvery=1, landmarkEvery=3, posePriorsAt=Int[0,5,10], sightDistance=3, params=SolverParams(algorithms=[:default, :parametric])) #do not initialize to force failure