Feature/3Q19/addingppe

src/CliqStateMachine.jl

@@ -112,6 +112,11 @@ function doCliqDownSolve_StateMachine(csmc::CliqStateMachineContainer)
   csmc.dodownsolve = false
   infocsm(csmc, "11, doCliqDownSolve_StateMachine -- finished with downGibbsCliqueDensity, now update csmc")
 
+  # set MAP est
+  for sym in getCliqFrontalVarIds(csmc.cliq)
+    setVariablePosteriorEstimates!(csmc.cliqSubFg, sym)
+  end
+
   # store the cliqSubFg for later debugging
   if opts.dbg
     DFG.saveDFG(csmc.cliqSubFg, joinpath(opts.logpath,"logs/cliq$(csmc.cliq.index)/fg_afterdownsolve"))
@@ -180,7 +185,7 @@ function determineCliqIfDownSolve_StateMachine(csmc::CliqStateMachineContainer)
     if prntst != :downsolved
       infocsm(csmc, "10, determineCliqIfDownSolve_StateMachine, going around again.")
       return determineCliqIfDownSolve_StateMachine
-end
+  	end
   else
     # special case for down solve on root clique.  When using solveCliq! following an up pass.
 
@@ -190,7 +195,16 @@ end
     setDwnMsg!(csmc.cliq, dwnmsgs)
     setCliqStatus!(csmc.cliq, :downsolved)
 	csmc.dodownsolve = false
+
+	# Update estimates and transfer back to the graph
+	frsyms = getCliqFrontalVarIds(csmc.cliq)
+	# Calculate estimates
+	map(sym -> setVariablePosteriorEstimates!(csmc.cliqSubFg, sym), frsyms)
+	# Transfer to parent graph
+	transferUpdateSubGraph!(csmc.dfg, csmc.cliqSubFg, frsyms)
+
     notifyCliqDownInitStatus!(csmc.cliq, :downsolved, logger=csmc.logger)
+
     return IncrementalInference.exitStateMachine
   end
 

src/CliqStateMachineUtils.jl

@@ -636,6 +636,27 @@ function areSiblingsRemaingNeedDownOnly(tree::BayesTree,
 end
 
 
+function setVariablePosteriorEstimates!(subfg::G,
+                                        sym::Symbol )::Nothing where G <: AbstractDFG
+  #
+
+  var = getVariable(subfg, sym)
+  bel = getKDE(var)
+  ops = buildHybridManifoldCallbacks(getManifolds(var))
+
+  @show varMax = getKDEMax(bel, addop=ops[1], diffop=ops[2])
+  @show varMean = getKDEMean(bel)
+  # TODO: We need to populate PPE.
+  @show varPpe = deepcopy(varMax) #TODO
+
+  var.estimateDict[:default] = Dict{Symbol, VariableEstimate}(
+    :max => VariableEstimate(:default, :max, varMax),
+    :mean => VariableEstimate(:default, :mean, varMean),
+    :ppe => VariableEstimate(:default, :ppe, varPpe))
+
+  return nothing
+end
+
 """
     $SIGNATURES
 

src/SubGraphFunctions.jl

@@ -265,6 +265,8 @@ function transferUpdateSubGraph!(dest::G1,
   #
   with_logger(logger) do
     @info "transferUpdateSubGraph! -- syms=$syms"
+
+    # TODO add with DFG v0.4
     # DFG.updateGraphSolverData!(src, dest, syms)
     for sym in syms
       vari = DFG.getVariable(src, sym)

test/runtests.jl

@@ -61,16 +61,13 @@ end
     include("fourdoortest.jl")
 end
 
-@testset "saving to and loading from .jld2 file" begin
-
-# DFG.savedfg(fg)
-# DFG.loaddfg() # some convert problem on DFGVariable
-
-savejld(fg, file="tempfg.jld2" )
-@warn "not able to load a new DFG object yet"
-# fgu = loadjld( file="tempfg.jld2" )
-Base.rm("tempfg.jld2")
-
+@testset "saving to and loading from FileDFG" begin
+    saveFolder = "/tmp/dfg_test"
+    saveDFG(fg, saveFolder)
+    retDFG = GraphsDFG{SolverParams}(params=SolverParams())
+    retDFG = loadDFG(saveFolder, IncrementalInference, retDFG)
+    @test symdiff(ls(fg), ls(retDFG)) == []
+    @test symdiff(lsf(fg), lsf(retDFG)) == []
 end
 
 @warn "must return testExpandedJLD.jl to testing -- currently skipped since jld2 files cannot be loaded."