12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update python tests

release/4.3a0
Varun Agrawal 2024-09-17 14:53:31 -04:00
parent dbd0ae1f27
commit c1ebdb200c
2 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -20,7 +20,7 @@ import gtsam
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
HybridBayesNet, HybridGaussianConditional, HybridBayesNet, HybridGaussianConditional,
HybridGaussianFactor, HybridGaussianFactorGraph, HybridGaussianFactor, HybridGaussianFactorGraph,
HybridValues, JacobianFactor, Ordering, noiseModel) HybridValues, JacobianFactor, noiseModel)
DEBUG_MARGINALS = False DEBUG_MARGINALS = False
@ -31,13 +31,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_create(self): def test_create(self):
"""Test construction of hybrid factor graph.""" """Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3) model = noiseModel.Unit.Create(3)
dk = DiscreteKeys()
dk.push_back((C(0), 2))
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph() hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1) hfg.push_back(jf1)
@ -58,13 +56,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_optimize(self): def test_optimize(self):
"""Test construction of hybrid factor graph.""" """Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3) model = noiseModel.Unit.Create(3)
dk = DiscreteKeys()
dk.push_back((C(0), 2))
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model) jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model) jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)]) gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph() hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1) hfg.push_back(jf1)
@ -96,8 +92,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
# Create Gaussian mixture Z(0) = X(0) + noise for each measurement. # Create Gaussian mixture Z(0) = X(0) + noise for each measurement.
I_1x1 = np.eye(1) I_1x1 = np.eye(1)
keys = DiscreteKeys()
keys.push_back(mode)
for i in range(num_measurements): for i in range(num_measurements):
conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
I_1x1, I_1x1,
@ -107,8 +101,10 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
I_1x1, I_1x1,
X(0), [0], X(0), [0],
sigma=3) sigma=3)
discreteParents = DiscreteKeys()
discreteParents.push_back(mode)
bayesNet.push_back( bayesNet.push_back(
HybridGaussianConditional([Z(i)], [X(0)], keys, HybridGaussianConditional([Z(i)], [X(0)], discreteParents,
[conditional0, conditional1])) [conditional0, conditional1]))
# Create prior on X(0). # Create prior on X(0).

4
python/gtsam/tests/test_HybridNonlinearFactorGraph.py
View File

@ -27,8 +27,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_nonlinear_hybrid(self): def test_nonlinear_hybrid(self):
nlfg = gtsam.HybridNonlinearFactorGraph() nlfg = gtsam.HybridNonlinearFactorGraph()
dk = gtsam.DiscreteKeys()
dk.push_back((10, 2))
nlfg.push_back( nlfg.push_back(
BetweenFactorPoint3(1, 2, Point3(1, 2, 3), BetweenFactorPoint3(1, 2, Point3(1, 2, 3),
noiseModel.Diagonal.Variances([1, 1, 1]))) noiseModel.Diagonal.Variances([1, 1, 1])))
@ -40,7 +38,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
noiseModel.Unit.Create(3)), 0.0), noiseModel.Unit.Create(3)), 0.0),
(PriorFactorPoint3(1, Point3(1, 2, 1), (PriorFactorPoint3(1, Point3(1, 2, 1),
noiseModel.Unit.Create(3)), 0.0)] noiseModel.Unit.Create(3)), 0.0)]
nlfg.push_back(gtsam.HybridNonlinearFactor([1], dk, factors)) nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors))
nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3")) nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
values = gtsam.Values() values = gtsam.Values()
values.insert_point3(1, Point3(0, 0, 0)) values.insert_point3(1, Point3(0, 0, 0))