Extra unit test to check that marginals from sequential and multifrontal are the same

inference/BayesTree-inl.h

@@ -342,7 +342,7 @@ namespace gtsam {
 		p_S_R.permuteWithInverse(toBack);
 
 		// return the parent shortcut P(Sp|R)
-		return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_S_R).eliminate();
+		return p_S_R;
 	}
 
 	/* ************************************************************************* */

tests/testGaussianISAM.cpp

@@ -23,11 +23,13 @@ using namespace boost::assign;
 
 #define GTSAM_MAGIC_KEY
 
+#include <gtsam/geometry/Rot2.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/inference/ISAM-inl.h>
 #include <gtsam/linear/GaussianISAM.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
 #include <gtsam/slam/smallExample.h>
 
 using namespace std;
@@ -339,6 +341,23 @@ TEST( BayesTree, balanced_smoother_joint )
 //	CHECK(assert_equal(expected4,actual4,tol));
 }
 
+/* ************************************************************************* */
+TEST(BayesTree, simpleMarginal)
+{
+  GaussianFactorGraph gfg;
+
+  Matrix A12 = Rot2::fromDegrees(45.0).matrix();
+
+  gfg.add(0, eye(2), zero(2), sharedSigma(2, 1.0));
+  gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0));
+  gfg.add(1, -eye(2), 2, A12, ones(2), sharedSigma(2, 1.0));
+
+  Matrix expected(GaussianSequentialSolver(gfg).marginalStandard(2).second);
+  Matrix actual(GaussianMultifrontalSolver(gfg).marginalStandard(2).second);
+
+  CHECK(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */