test_GaussianFactorGraph.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Unit tests for Linear Factor Graphs.
9 Author: Frank Dellaert & Gerry Chen
10 """
11 # pylint: disable=invalid-name, no-name-in-module, no-member
12 
13 from __future__ import print_function
14 
15 import unittest
16 
17 import numpy as np
18 from gtsam.symbol_shorthand import X
19 from gtsam.utils.test_case import GtsamTestCase
20 
21 import gtsam
22 
23 
25  """Create a basic linear factor graph for testing"""
27 
28  x0 = X(0)
29  x1 = X(1)
30  x2 = X(2)
31 
32  BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
33  PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1))
34 
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)
38 
39  return graph, (x0, x1, x2)
40 
41 
43  """Tests for Gaussian Factor Graphs."""
44 
45  def test_fg(self):
46  """Test solving a linear factor graph"""
47  graph, X = create_graph()
48  result = graph.optimize()
49 
50  EXPECTEDX = [0, 1, 3]
51 
52  # check solutions
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)
56 
58  """Test converting a linear factor graph to a nonlinear one"""
59  graph, X = create_graph()
60 
61  EXPECTEDM = [1, 2, 3]
62 
63  # create nonlinear factor graph for marginalization
66  nlresult = optimizer.optimizeSafely()
67 
68  # marginalize
69  marginals = gtsam.Marginals(nfg, nlresult)
70  m = [marginals.marginalCovariance(x) for x in X]
71 
72  # check linear marginalizations
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)
76 
78  """Marginalize a linear factor graph"""
79  graph, X = create_graph()
80  result = graph.optimize()
81 
82  EXPECTEDM = [1, 2, 3]
83 
84  # linear factor graph marginalize
85  marginals = gtsam.Marginals(graph, result)
86  m = [marginals.marginalCovariance(x) for x in X]
87 
88  # check linear marginalizations
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)
92 
93  def test_ordering(self):
94  """Test ordering"""
95  gfg, keys = create_graph()
96  ordering = gtsam.Ordering()
97  for key in keys[::-1]:
98  ordering.push_back(key)
99 
100  bn = gfg.eliminateSequential(ordering)
101  self.assertEqual(bn.size(), 3)
102 
103  keyVector = [keys[2]]
104  ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
105  gfg, keyVector)
106  bn = gfg.eliminateSequential(ordering)
107  self.assertEqual(bn.size(), 3)
108 
109 
110 if __name__ == '__main__':
111  unittest.main()
test_GaussianFactorGraph.TestGaussianFactorGraph.test_linearMarginalization
def test_linearMarginalization(self)
Definition: test_GaussianFactorGraph.py:77
test_GaussianFactorGraph.TestGaussianFactorGraph.test_fg
def test_fg(self)
Definition: test_GaussianFactorGraph.py:45
gtsam::Marginals
Definition: Marginals.h:32
test_GaussianFactorGraph.TestGaussianFactorGraph.test_ordering
def test_ordering(self)
Definition: test_GaussianFactorGraph.py:93
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
X
#define X
Definition: icosphere.cpp:20
test_GaussianFactorGraph.TestGaussianFactorGraph.test_convertNonlinear
def test_convertNonlinear(self)
Definition: test_GaussianFactorGraph.py:57
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
test_GaussianFactorGraph.TestGaussianFactorGraph
Definition: test_GaussianFactorGraph.py:42
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::utils.test_case
Definition: test_case.py:1
test_GaussianFactorGraph.create_graph
def create_graph()
Definition: test_GaussianFactorGraph.py:24
gtsam::Values
Definition: Values.h:65
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::LinearContainerFactor::ConvertLinearGraph
static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph &linear_graph, const Values &linearizationPoint=Values())
Definition: LinearContainerFactor.cpp:210


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:05