Merge pull request #508 from JuliaRobotics/feat/4Q19/fix499

add few tree tests (API)

Author: dehann

src/CanonicalGraphExamples.jl

@@ -0,0 +1,106 @@
+
+export loadCanonicalFG_Kaess, loadCanonicalFG_TestSymbolic, loadCanonicalFG_CaesarRing1D
+
+
+"""
+    $SIGNATURES
+
+Canonical example from literature, Kaess, et al.: ISAM2, IJRR, 2011.
+
+Notes
+- Paper variable ordering: p = [:l1;:l2;:x1;:x2;:x3]
+"""
+function loadCanonicalFG_Kaess()
+  fg = initfg()
+
+  addVariable!(fg,:x1, ContinuousScalar)
+  addFactor!(fg, [:x1;], Prior(Normal()))
+
+
+  addVariable!(fg,:x2, ContinuousScalar)
+  addFactor!(fg,[:x1, :x2], LinearConditional(Normal()))
+
+  addVariable!(fg, :x3, ContinuousScalar)
+  addFactor!(fg,[:x2,:x3],LinearConditional(Normal()))
+
+  addVariable!(fg, :l1, ContinuousScalar)
+  addFactor!(fg, [:x1,:l1], LinearConditional(Normal()) )
+  addFactor!(fg, [:x2,:l1], LinearConditional(Normal()) )
+
+  addVariable!(fg, :l2, ContinuousScalar)
+  addFactor!(fg, [:x3,:l2], LinearConditional(Normal()))
+
+  return fg
+end
+
+
+"""
+    $SIGNATURES
+
+Canonical example introduced by Borglab.
+
+Notes
+- Known variable ordering: p = [:x1; :l3; :l1; :x5; :x2; :l2; :x4; :x3]
+"""
+function loadCanonicalFG_TestSymbolic()
+  fg = initfg()
+
+  addVariable!(fg, :x1, ContinuousScalar)
+  addVariable!(fg, :x2, ContinuousScalar)
+  addVariable!(fg, :x3, ContinuousScalar)
+  addVariable!(fg, :x4, ContinuousScalar)
+  addVariable!(fg, :x5, ContinuousScalar)
+  addVariable!(fg, :l1, ContinuousScalar)
+  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()))
+
+  return fg
+end
+
+
+
+"""
+    $SIGNATURES
+
+Canonical example introduced originally as Caesar Hex Example.
+
+Notes
+- Paper variable ordering: p = [:x0;:x2;:x4;:x6;:x1;:l1;:x5;:x3;]
+"""
+function loadCanonicalFG_CaesarRing1D()
+
+  fg = initfg()
+
+  addVariable!(fg, :x0, ContinuousScalar)
+  addVariable!(fg, :x1, ContinuousScalar)
+  addVariable!(fg, :x2, ContinuousScalar)
+  addVariable!(fg, :x3, ContinuousScalar)
+  addVariable!(fg, :x4, ContinuousScalar)
+  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()))
+
+  addVariable!(fg, :l1, ContinuousScalar)
+  addFactor!(fg, [:x0;:l1], LinearConditional(Normal()))
+  addFactor!(fg, [:x6;:l1], LinearConditional(Normal()))
+
+  return fg
+end

src/FactorGraph01.jl

@@ -1095,6 +1095,7 @@ function getEliminationOrder(dfg::G; ordering::Symbol=:qr, solvable::Int=1) wher
   end
 
   # Return the variable ordering that we should use for the Bayes map
+  # reverse order checked in #475 and #499
   return permuteds[p] |> reverse
 end
 

src/IncrementalInference.jl

@@ -548,6 +548,7 @@ include("Factors/Sphere1D.jl")
 include("AdditionalUtils.jl")
 include("SolverAPI.jl")
 
+include("CanonicalGraphExamples.jl")
 include("Deprecated.jl")
 
 exportimg(pl) = error("Please do `using Gadfly` before IncrementalInference is used to allow image export.")

src/JunctionTree.jl

@@ -205,14 +205,16 @@ end
 
 Build the whole tree in batch format.
 """
-function buildTree!(tree::BayesTree, dfg::G, elimorder::Array{Symbol,1}) where G <: AbstractDFG
-  revorder = reverse(elimorder,dims=1) # flipdim(p, 1)
+function buildTree!(tree::BayesTree, dfg::AbstractDFG, elimorder::Array{Symbol,1})
+  revorder = reverse(elimorder,dims=1) # flipdim(p, 1), fixing #499
   # prevVar = 0
   for var in revorder
     @info "Adding $var to tree..."
     newPotential(tree, dfg, var, elimorder)
     prevVar = var
   end
+
+  return tree
 end
 
 """

test/runtests.jl

@@ -40,9 +40,10 @@ end
 
 include("testBasicForwardConvolve.jl")
 
-
 include("testFactorMetadata.jl")
 
+include("testJunctionTreeConstruction.jl")
+
 include("testBasicCSM.jl")
 
 include("testCliqueFactors.jl")

test/testBasicGraphs.jl

@@ -106,7 +106,7 @@ tree, smt, hist = solveTree!(fg)
 # check mean and covariance -- should be zero
 @test abs((getKDE(fg, :x0) |> getKDEMean)[1] + 1000) < 0.6
 # should be sqrt(1/2) = 0.707 -- computation results nearer 0.7.
-@test 0.3 < Statistics.cov( getPoints(getKDE(fg, :x0))[1,:] ) < 1
+@test 0.2 < Statistics.cov( getPoints(getKDE(fg, :x0))[1,:] ) < 1
 
 end
 

test/testJunctionTree2.jl

@@ -0,0 +1,101 @@
+# another tree test
+
+using IncrementalInference
+using Test
+
+
+@testset "Variable ordering Bayes tree member check." begin
+
+  fg = loadCanonicalFG_Kaess()
+  # Choose specific variable ordering and perform check.
+  vo = [:l1, :l2, :x1, :x2, :x3]
+  tree = buildTreeFromOrdering!(fg, vo)
+  @test vo == tree.variableOrder
+  @test vo == getVariableOrder(tree)
+  @test vo == getEliminationOrder(tree)
+
+end
+
+@testset "Test Caesar Ring 1D symbolic tree construction" begin
+
+fg = loadCanonicalFG_CaesarRing1D()
+# drawGraph(fg, show=true)
+
+eo = [:x0;:x2;:x4;:x6;:x1;:l1;:x5;:x3;]
+
+tree = buildTreeFromOrdering!(fg,eo)
+drawTree(tree, show=true)
+
+@test length(tree.cliques) == 6
+
+C0 = getCliq(tree, :x3)
+@test intersect( C0 |> getFrontals, [:x3; :x5; :l1]) |> length == 3
+@test C0 |> getCliqSeparatorVarIds |> length == 0
+
+cC0 = getChildren(tree, C0)
+@test cC0 |> length == 3
+
+C1 = getCliq(tree, :x1)
+@test C1 in cC0
+@test C1 |> getFrontals == [:x1;]
+@test intersect(C1 |> getCliqSeparatorVarIds, [:x3;:l1]) |> length == 2
+
+cC1 = getChildren(tree, C1)
+@test cC1 |> length == 2
+
+C4 = getCliq(tree, :x2)
+@test C4 in cC1
+@test C4 |> getFrontals == [:x2;]
+@test intersect(C4 |> getCliqSeparatorVarIds, [:x3;:x1]) |> length == 2
+
+C5 = getCliq(tree, :x0)
+@test C5 in cC1
+@test C5 |> getFrontals == [:x0;]
+@test intersect(C5 |> getCliqSeparatorVarIds, [:l1;:x1]) |> length == 2
+
+C2 = getCliq(tree, :x6)
+@test C2 in cC0
+@test C2 |> getFrontals == [:x6;]
+@test intersect(C2 |> getCliqSeparatorVarIds, [:l1;:x5]) |> length == 2
+
+C3 = getCliq(tree, :x4)
+@test C3 in cC0
+@test C3 |> getFrontals == [:x4;]
+@test intersect(C3 |> getCliqSeparatorVarIds, [:x3;:x5]) |> length == 2
+
+end
+
+
+@testset "Test tree formation and consistent APIs" begin
+
+  fg = loadCanonicalFG_TestSymbolic()
+
+  #writeGraphPdf(fg, show=true)
+
+  eo = [:x1; :l3; :l1; :x5; :x2; :l2; :x4; :x3]
+
+  tree = buildTreeFromOrdering!(fg,eo)
+  # drawTree(tree, show=true)
+
+  @warn "TODO, complete further testing on tree formation"
+
+
+  ## test variable order APIs consistent, see issue 499
+
+  vo = getEliminationOrder(fg)
+  tree1 = resetBuildTreeFromOrder!(fg, vo)
+  # drawTree(tree1, show=true)
+
+  tree2 = wipeBuildNewTree!(fg)
+  # drawTree(tree2, show=true)
+
+  @test getVariableOrder(tree1) == getVariableOrder(tree1)
+
+end
+
+
+
+
+
+
+#