From 52019d85f37105fff63ed16508c6c837648006fd Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 5 Feb 2023 17:36:53 -0800
Subject: [PATCH] Added tests for BayesTree variants

---
 .../tests/testSymbolicFactorGraph.cpp         | 49 ++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index e1294157b..f114bd250 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -140,7 +140,7 @@ TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVector) {
   EXPECT(assert_equal(expectedBayesNet, actual));
 }
 
-TEST_UNSAFE(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrderingPlus) {
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrderingPlus) {
   auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional{0, 3})({3});
 
   const Ordering orderedVariables{0, 3},
@@ -160,6 +160,53 @@ TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVectorPlus) {
   EXPECT(assert_equal(expectedBayesNet, actual));
 }
 
+/* ************************************************************************* */
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeOrdering) {
+  auto expectedBayesTree =
+      *simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5})
+           .second->eliminateMultifrontal(Ordering{0, 1, 2, 3});
+
+  SymbolicBayesTree actual =
+      *simpleTestGraph2.marginalMultifrontalBayesTree(Ordering{0, 1, 2, 3});
+  EXPECT(assert_equal(expectedBayesTree, actual));
+}
+
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeKeyVector) {
+  auto expectedBayesTree =
+      *simpleTestGraph2.eliminatePartialMultifrontal(Ordering{4, 5})
+           .second->eliminateMultifrontal(Ordering::OrderingType::COLAMD);
+
+  SymbolicBayesTree actual =
+      *simpleTestGraph2.marginalMultifrontalBayesTree(KeyVector{0, 1, 2, 3});
+  EXPECT(assert_equal(expectedBayesTree, actual));
+}
+
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeOrderingPlus) {
+  const Ordering orderedVariables{0, 3},
+      marginalizedVariableOrdering{1, 2, 4, 5};
+  auto expectedBayesTree =
+      *simpleTestGraph2
+           .eliminatePartialMultifrontal(marginalizedVariableOrdering)
+           .second->eliminateMultifrontal(orderedVariables);
+
+  SymbolicBayesTree actual = *simpleTestGraph2.marginalMultifrontalBayesTree(
+      orderedVariables, marginalizedVariableOrdering);
+  EXPECT(assert_equal(expectedBayesTree, actual));
+}
+
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesTreeKeyVectorPlus) {
+  const Ordering marginalizedVariableOrdering{2, 4, 5};
+  auto expectedBayesTree =
+      *simpleTestGraph2
+           .eliminatePartialMultifrontal(marginalizedVariableOrdering)
+           .second->eliminateMultifrontal(Ordering::OrderingType::COLAMD);
+
+  const KeyVector variables{0, 1, 3};
+  SymbolicBayesTree actual = *simpleTestGraph2.marginalMultifrontalBayesTree(
+      variables, marginalizedVariableOrdering);
+  EXPECT(assert_equal(expectedBayesTree, actual));
+}
+
 /* ************************************************************************* */
 TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
   SymbolicFactorGraph fg;