Merge pull request #509 from JuliaRobotics/feat/4Q19/savetree

add save load Tree,

Author: dehann

src/CanonicalGraphExamples.jl

@@ -10,25 +10,25 @@ Canonical example from literature, Kaess, et al.: ISAM2, IJRR, 2011.
 Notes
 - Paper variable ordering: p = [:l1;:l2;:x1;:x2;:x3]
 """
-function loadCanonicalFG_Kaess()
+function loadCanonicalFG_Kaess(;graphinit::Bool=false)
   fg = initfg()
 
   addVariable!(fg,:x1, ContinuousScalar)
-  addFactor!(fg, [:x1;], Prior(Normal()))
+  addFactor!(fg, [:x1;], Prior(Normal()), autoinit=graphinit)
 
 
   addVariable!(fg,:x2, ContinuousScalar)
-  addFactor!(fg,[:x1, :x2], LinearConditional(Normal()))
+  addFactor!(fg,[:x1, :x2], LinearConditional(Normal()), autoinit=graphinit)
 
   addVariable!(fg, :x3, ContinuousScalar)
-  addFactor!(fg,[:x2,:x3],LinearConditional(Normal()))
+  addFactor!(fg,[:x2,:x3],LinearConditional(Normal()), autoinit=graphinit)
 
   addVariable!(fg, :l1, ContinuousScalar)
-  addFactor!(fg, [:x1,:l1], LinearConditional(Normal()) )
-  addFactor!(fg, [:x2,:l1], LinearConditional(Normal()) )
+  addFactor!(fg, [:x1,:l1], LinearConditional(Normal()) , autoinit=graphinit)
+  addFactor!(fg, [:x2,:l1], LinearConditional(Normal()) , autoinit=graphinit)
 
   addVariable!(fg, :l2, ContinuousScalar)
-  addFactor!(fg, [:x3,:l2], LinearConditional(Normal()))
+  addFactor!(fg, [:x3,:l2], LinearConditional(Normal()), autoinit=graphinit)
 
   return fg
 end
@@ -42,7 +42,7 @@ Canonical example introduced by Borglab.
 Notes
 - Known variable ordering: p = [:x1; :l3; :l1; :x5; :x2; :l2; :x4; :x3]
 """
-function loadCanonicalFG_TestSymbolic()
+function loadCanonicalFG_TestSymbolic(;graphinit::Bool=false)
   fg = initfg()
 
   addVariable!(fg, :x1, ContinuousScalar)
@@ -54,16 +54,16 @@ function loadCanonicalFG_TestSymbolic()
   addVariable!(fg, :l2, ContinuousScalar)
   addVariable!(fg, :l3, ContinuousScalar)
 
-  addFactor!(fg, [:x1;:l1], LinearConditional(Normal()))
-  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()))
-  addFactor!(fg, [:x2;:l1], LinearConditional(Normal()))
-  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()))
-  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()))
-  addFactor!(fg, [:x4;:l2], LinearConditional(Normal()))
-  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()))
-  addFactor!(fg, [:l2;:x5], LinearConditional(Normal()))
-  addFactor!(fg, [:x4;:l3], LinearConditional(Normal()))
-  addFactor!(fg, [:x5;:l3], LinearConditional(Normal()))
+  addFactor!(fg, [:x1;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:l2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:l2;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:l3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x5;:l3], LinearConditional(Normal()), autoinit=graphinit)
 
   return fg
 end
@@ -78,7 +78,7 @@ Canonical example introduced originally as Caesar Hex Example.
 Notes
 - Paper variable ordering: p = [:x0;:x2;:x4;:x6;:x1;:l1;:x5;:x3;]
 """
-function loadCanonicalFG_CaesarRing1D()
+function loadCanonicalFG_CaesarRing1D(;graphinit::Bool=false)
 
   fg = initfg()
 
@@ -90,17 +90,17 @@ function loadCanonicalFG_CaesarRing1D()
   addVariable!(fg, :x5, ContinuousScalar)
   addVariable!(fg, :x6, ContinuousScalar)
 
-  addFactor!(fg, [:x0], Prior(Normal()))
-  addFactor!(fg, [:x0;:x1], LinearConditional(Normal()))
-  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()))
-  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()))
-  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()))
-  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()))
-  addFactor!(fg, [:x5;:x6], LinearConditional(Normal()))
+  addFactor!(fg, [:x0], Prior(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x0;:x1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x5;:x6], LinearConditional(Normal()), autoinit=graphinit)
 
   addVariable!(fg, :l1, ContinuousScalar)
-  addFactor!(fg, [:x0;:l1], LinearConditional(Normal()))
-  addFactor!(fg, [:x6;:l1], LinearConditional(Normal()))
+  addFactor!(fg, [:x0;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x6;:l1], LinearConditional(Normal()), autoinit=graphinit)
 
   return fg
 end

src/IncrementalInference.jl

@@ -434,11 +434,13 @@ export
   extractdistribution,
 
   # factor graph operating system utils (fgos)
-  convert2packedfunctionnode,
-  encodefg,
-  decodefg,
-  savejld,
-  loadjld,
+  saveTree,
+  loadTree,
+  # convert2packedfunctionnode,
+  # encodefg,
+  # decodefg,
+  # savejld,
+  # loadjld,
   landmarks,
   setCliqDrawColor,
 

src/JunctionTree.jl

@@ -1490,3 +1490,80 @@ Return the variable order stored in a tree object.
 getVariableOrder(treel::BayesTree)::Vector{Symbol} = treel.variableOrder
 
 getEliminationOrder(treel::BayesTree) = treel.variableOrder
+
+
+"""
+    $SIGNATURES
+
+EXPERIMENTAL, Save a Bayes (Junction) tree object to file.
+
+Notes
+- Converts and saves to JLD2 format a set of `PackedBayesTreeNodeData` objects.
+- IIF issue #481
+
+Related
+
+IIF.loadTree, DFG.saveDFG, DFG.loadDFG, JLD2.@save, JLD2.@load
+"""
+function saveTree(treel::BayesTree,
+                  filepath=joinpath("/tmp","caesar","savetree.jld2") )
+  #
+  savetree = deepcopy(treel)
+  for i in 1:length(savetree.cliques)
+    if  savetree.cliques[i].attributes["data"] isa BayesTreeNodeData
+      savetree.cliques[i].attributes["data"] = convert(PackedBayesTreeNodeData, savetree.cliques[i].attributes["data"])
+    end
+  end
+
+  JLD2.@save filepath savetree
+  return filepath
+end
+
+function saveTree(treeArr::Vector{BayesTree},
+                  filepath=joinpath("/tmp","caesar","savetrees.jld2") )
+  #
+  savetree = deepcopy(treeArr)
+  for savtre in savetree, i in 1:length(savtre.cliques)
+    if savtre.cliques[i].attributes["data"] isa BayesTreeNodeData
+      savtre.cliques[i].attributes["data"] = convert(PackedBayesTreeNodeData, savtre.cliques[i].attributes["data"])
+    end
+  end
+
+  JLD2.@save filepath savetree
+  return filepath
+end
+
+"""
+    $SIGNATURES
+
+EXPERIMENTAL, Save a Bayes (Junction) tree object to file.
+
+Notes
+- Converts and saves to JLD2 format a set of `PackedBayesTreeNodeData` objects.
+- IIF issue #481
+
+Related
+
+IIF.saveTree, DFG.saveDFG, DFG.loadDFG, JLD2.@save, JLD2.@load
+"""
+function loadTree(filepath=joinpath("/tmp","caesar","savetree.jld2"))
+  data = @load filepath savetree
+
+  # convert back to a type that which could not be serialized by JLD2
+  if savetree isa Vector
+      for savtre in savetree, i in 1:length(savtre.cliques)
+        if savtre.cliques[i].attributes["data"] isa PackedBayesTreeNodeData
+          savtre.cliques[i].attributes["data"] = convert(BayesTreeNodeData, savtre.cliques[i].attributes["data"])
+        end
+      end
+  else
+    for i in 1:length(savetree.cliques)
+      if savetree.cliques[i].attributes["data"] isa PackedBayesTreeNodeData
+        savetree.cliques[i].attributes["data"] = convert(BayesTreeNodeData, savetree.cliques[i].attributes["data"])
+      end
+    end
+  end
+
+  # return loaded and converted tree
+  return savetree
+end

test/runtests.jl

@@ -23,6 +23,9 @@ include("testPartialFactors.jl")
 @testset "basic Bayes tree construction" begin
     include("testBayesTreeiSAM2Example.jl")
 end
+
+include("testTreeSaveLoad.jl")
+
 @testset "Ensure converter types can be run from extending namespaces..." begin
     include("saveconvertertypes.jl")
 end

test/testTreeSaveLoad.jl

@@ -0,0 +1,53 @@
+# test saving and loading of trees
+
+using Test
+using IncrementalInference
+
+@testset "Test loading and saving of Bayes (Junction) tree" begin
+
+    fg = loadCanonicalFG_Kaess(graphinit=false)
+    tree = wipeBuildNewTree!(fg)
+
+    # save and load tree
+    saveTree(tree)
+    tree2 = loadTree()
+
+    # perform a few spot checks to see that the trees are similar
+    @test length(tree.cliques) == length(tree2.cliques)
+    @test getVariableOrder(tree) == getVariableOrder(tree2)
+
+    for (clid,cl) in tree.cliques
+      fsyms = getFrontals(cl)
+      cl2 = getCliq(tree2, fsyms[1])
+      fsyms2 = getFrontals(cl2)
+      @test fsyms == fsyms2
+      @test getCliqSeparatorVarIds(cl) == getCliqSeparatorVarIds(cl2)
+      @test typeof(cl) == typeof(cl2)
+    end
+
+end
+
+
+@testset "Test loading and saving of Bayes (Junction) tree" begin
+
+    fg = loadCanonicalFG_Kaess(graphinit=false)
+    tree = wipeBuildNewTree!(fg)
+
+    # save and load tree as array
+    filepath = saveTree([tree;deepcopy(tree)])
+    trees = loadTree(filepath)
+
+    # perform a few spot checks to see that the trees are similar
+    @test length(tree.cliques) == length(trees[1].cliques)
+    @test getVariableOrder(tree) == getVariableOrder(trees[1])
+
+    for (clid,cl) in tree.cliques
+      fsyms = getFrontals(cl)
+      cl2 = getCliq(trees[1], fsyms[1])
+      fsyms2 = getFrontals(cl2)
+      @test fsyms == fsyms2
+      @test getCliqSeparatorVarIds(cl) == getCliqSeparatorVarIds(cl2)
+      @test typeof(cl) == typeof(cl2)
+    end
+
+end