2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for Linear Factor Graphs.
9 Author: Frank Dellaert & Gerry Chen
13 from __future__
import print_function
25 """Create a basic linear factor graph for testing"""
35 graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
36 graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE)
37 graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
39 return graph, (x0, x1, x2)
43 """Tests for Gaussian Factor Graphs."""
46 """Test solving a linear factor graph"""
48 result = graph.optimize()
53 self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
54 self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
55 self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
58 """Test converting a linear factor graph to a nonlinear one"""
66 nlresult = optimizer.optimizeSafely()
70 m = [marginals.marginalCovariance(x)
for x
in X]
73 self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
74 self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
75 self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
78 """Marginalize a linear factor graph"""
80 result = graph.optimize()
86 m = [marginals.marginalCovariance(x)
for x
in X]
89 self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
90 self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
91 self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
97 for key
in keys[::-1]:
98 ordering.push_back(key)
100 bn = gfg.eliminateSequential(ordering)
101 self.assertEqual(bn.size(), 3)
103 keyVector = [keys[2]]
104 ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
106 bn = gfg.eliminateSequential(ordering)
107 self.assertEqual(bn.size(), 3)
110 if __name__ ==
'__main__':