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 Hybrid Nonlinear Factor Graphs.
13 from __future__
import print_function
20 from gtsam
import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel
24 """Unit tests for HybridGaussianFactorGraph."""
30 noiseModel.Diagonal.Variances([1, 1, 1])))
33 noiseModel.Diagonal.Variances([0.5, 0.5, 0.5])))
36 noiseModel.Unit.Create(3)), 0.0),
38 noiseModel.Unit.Create(3)), 0.0)]
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()
48 self.assertEqual(hbv.atDiscrete(10), 0)
51 if __name__ ==
"__main__":