From ebc75e3a8dac9bf9218a2e4ca0240a95082260e6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 11 Mar 2022 08:42:40 -0500 Subject: [PATCH] added test for ordering in Python --- .../gtsam/tests/test_GaussianFactorGraph.py | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index a29b0f263..26a8be1a6 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -23,23 +23,23 @@ from gtsam.utils.test_case import GtsamTestCase def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - + x0 = X(0) x1 = X(1) x2 = X(2) - + BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) - graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) + graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE) graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) return graph, (x0, x1, x2) + class TestGaussianFactorGraph(GtsamTestCase): """Tests for Gaussian Factor Graphs.""" - def test_fg(self): """Test solving a linear factor graph""" graph, X = create_graph() @@ -71,7 +71,7 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) - + def test_linearMarginalization(self): """Marginalize a linear factor graph""" graph, X = create_graph() @@ -88,5 +88,22 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + def test_ordering(self): + """Test ordering""" + gfg, keys = create_graph() + ordering = gtsam.Ordering() + for key in keys[::-1]: + ordering.push_back(key) + + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + keyVector = gtsam.KeyVector() + keyVector.append(keys[2]) + ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + if __name__ == '__main__': unittest.main()