test_HybridNonlinearFactorGraph.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 Hybrid Nonlinear Factor Graphs.
9 Author: Fan Jiang
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 from gtsam.utils.test_case import GtsamTestCase
18 
19 import gtsam
20 from gtsam import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel
21 
22 
24  """Unit tests for HybridGaussianFactorGraph."""
25 
28  nlfg.push_back(
29  BetweenFactorPoint3(1, 2, Point3(1, 2, 3),
30  noiseModel.Diagonal.Variances([1, 1, 1])))
31  nlfg.push_back(
32  PriorFactorPoint3(2, Point3(1, 2, 3),
33  noiseModel.Diagonal.Variances([0.5, 0.5, 0.5])))
34 
35  factors = [(PriorFactorPoint3(1, Point3(0, 0, 0),
36  noiseModel.Unit.Create(3)), 0.0),
37  (PriorFactorPoint3(1, Point3(1, 2, 1),
38  noiseModel.Unit.Create(3)), 0.0)]
39  mode = (10, 2)
40  nlfg.push_back(gtsam.HybridNonlinearFactor(mode, factors))
41  nlfg.push_back(gtsam.DecisionTreeFactor(mode, "1 3"))
42  values = gtsam.Values()
43  values.insert_point3(1, Point3(0, 0, 0))
44  values.insert_point3(2, Point3(2, 3, 1))
45  hfg = nlfg.linearize(values)
46  hbn = hfg.eliminateSequential()
47  hbv = hbn.optimize()
48  self.assertEqual(hbv.atDiscrete(10), 0)
49 
50 
51 if __name__ == "__main__":
52  unittest.main()
gtsam::DecisionTreeFactor
Definition: DecisionTreeFactor.h:44
gtsam::HybridNonlinearFactorGraph
Definition: HybridNonlinearFactorGraph.h:33
test_HybridNonlinearFactorGraph.TestHybridGaussianFactorGraph.test_nonlinear_hybrid
def test_nonlinear_hybrid(self)
Definition: test_HybridNonlinearFactorGraph.py:26
BetweenFactorPoint3
BetweenFactor< Point3 > BetweenFactorPoint3
Definition: serialization.cpp:46
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Values
Definition: Values.h:65
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
test_HybridNonlinearFactorGraph.TestHybridGaussianFactorGraph
Definition: test_HybridNonlinearFactorGraph.py:23
PriorFactorPoint3
PriorFactor< Point3 > PriorFactorPoint3
Definition: serialization.cpp:34
gtsam::HybridNonlinearFactor
Implementation of a discrete-conditioned hybrid factor.
Definition: HybridNonlinearFactor.h:58


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27