Merge pull request #653 from JuliaRobotics/feature/20q1/par/likelihoo…

…dmsg

Add cliqueLikelihood to LikelihoodMessage and remove BeliefMessage

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

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

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}

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)

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

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

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)

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