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

fixes based on previous PR

release/4.3a0
Varun Agrawal 2022-12-30 14:23:40 +05:30
parent 3ab91b27d5
commit 5ea63be8c5
3 changed files with 41 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -138,8 +138,6 @@ void HybridBayesNet::updateDiscreteConditionals(
HybridConditional::shared_ptr conditional = this->at(i); HybridConditional::shared_ptr conditional = this->at(i);
if (conditional->isDiscrete()) { if (conditional->isDiscrete()) {
auto discrete = conditional->asDiscrete(); auto discrete = conditional->asDiscrete();
KeyVector frontals(discrete->frontals().begin(),
discrete->frontals().end());
// Apply prunerFunc to the underlying AlgebraicDecisionTree // Apply prunerFunc to the underlying AlgebraicDecisionTree
auto discreteTree = auto discreteTree =

2
gtsam/hybrid/HybridValues.h
View File

@ -104,7 +104,7 @@ class GTSAM_EXPORT HybridValues {
* @param j The index with which the value will be associated. */ * @param j The index with which the value will be associated. */
void insert(Key j, const Vector& value) { continuous_.insert(j, value); } void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
// TODO(Shangjie)- update() and insert_or_assign() , similar to Values.h // TODO(Shangjie)- insert_or_assign() , similar to Values.h
/** /**
* Read/write access to the discrete value with key \c j, throws * Read/write access to the discrete value with key \c j, throws

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

@ -11,30 +11,20 @@ Author: Fan Jiang
# pylint: disable=invalid-name, no-name-in-module, no-member # pylint: disable=invalid-name, no-name-in-module, no-member
import unittest import unittest
import math
import numpy as np import numpy as np
from gtsam.symbol_shorthand import C, M, X, Z from gtsam.symbol_shorthand import C, M, X, Z
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
import gtsam import gtsam
from gtsam import ( from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
DecisionTreeFactor, GaussianMixture, GaussianMixtureFactor,
DiscreteConditional, HybridGaussianFactorGraph, JacobianFactor, Ordering,
DiscreteKeys, noiseModel)
GaussianConditional,
GaussianMixture,
GaussianMixtureFactor,
HybridGaussianFactorGraph,
JacobianFactor,
Ordering,
noiseModel,
)
class TestHybridGaussianFactorGraph(GtsamTestCase): class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph.""" """Unit tests for HybridGaussianFactorGraph."""
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)
@ -52,8 +42,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
hfg.push_back(gmf) hfg.push_back(gmf)
hbn = hfg.eliminateSequential( hbn = hfg.eliminateSequential(
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
) hfg, [C(0)]))
self.assertEqual(hbn.size(), 2) self.assertEqual(hbn.size(), 2)
@ -84,36 +74,39 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
hfg.push_back(dtf) hfg.push_back(dtf)
hbn = hfg.eliminateSequential( hbn = hfg.eliminateSequential(
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
) hfg, [C(0)]))
hv = hbn.optimize() hv = hbn.optimize()
self.assertEqual(hv.atDiscrete(C(0)), 1) self.assertEqual(hv.atDiscrete(C(0)), 1)
@staticmethod @staticmethod
def tiny(num_measurements: int = 1): def tiny(num_measurements: int = 1) -> gtsam.HybridBayesNet:
"""Create a tiny two variable hybrid model.""" """
Create a tiny two variable hybrid model which represents
the generative probability P(z, x, n) = P(z | x, n)P(x)P(n).
"""
# Create hybrid Bayes net. # Create hybrid Bayes net.
bayesNet = gtsam.HybridBayesNet() bayesNet = gtsam.HybridBayesNet()
# Create mode key: 0 is low-noise, 1 is high-noise. # Create mode key: 0 is low-noise, 1 is high-noise.
modeKey = M(0) mode = (M(0), 2)
mode = (modeKey, 2)
# Create Gaussian mixture Z(0) = X(0) + noise for each measurement. # Create Gaussian mixture Z(0) = X(0) + noise for each measurement.
I = np.eye(1) I = np.eye(1)
keys = DiscreteKeys() keys = DiscreteKeys()
keys.push_back(mode) keys.push_back(mode)
for i in range(num_measurements): for i in range(num_measurements):
conditional0 = GaussianConditional.FromMeanAndStddev( conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
Z(i), I, X(0), [0], sigma=0.5 I,
) X(0), [0],
conditional1 = GaussianConditional.FromMeanAndStddev( sigma=0.5)
Z(i), I, X(0), [0], sigma=3 conditional1 = GaussianConditional.FromMeanAndStddev(Z(i),
) I,
bayesNet.emplaceMixture( X(0), [0],
[Z(i)], [X(0)], keys, [conditional0, conditional1] sigma=3)
) bayesNet.emplaceMixture([Z(i)], [X(0)], keys,
[conditional0, conditional1])
# Create prior on X(0). # Create prior on X(0).
prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0) prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0)
@ -142,14 +135,22 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
self.assertEqual(fg.size(), 3) self.assertEqual(fg.size(), 3)
def test_tiny2(self): def test_ratio(self):
"""Test a tiny two variable hybrid model, with 2 measurements.""" """
# Create the Bayes net and sample from it. Given a tiny two variable hybrid model, with 2 measurements,
test the ratio of the bayes net model representing P(z, x, n)=P(z|x, n)P(x)P(n)
and the factor graph P(x, n | z)=P(x | n, z)P(n|z),
both of which represent the same posterior.
"""
# Create the Bayes net representing the generative model P(z, x, n)=P(z|x, n)P(x)P(n)
bayesNet = self.tiny(num_measurements=2) bayesNet = self.tiny(num_measurements=2)
sample = bayesNet.sample() # Sample from the Bayes net.
sample: gtsam.HybridValues = bayesNet.sample()
# print(sample) # print(sample)
# Create a factor graph from the Bayes net with sampled measurements. # Create a factor graph from the Bayes net with sampled measurements.
# The factor graph is `P(x)P(n) ϕ(x, n; z1) ϕ(x, n; z2)`
# and thus represents the same joint probability as the Bayes net.
fg = HybridGaussianFactorGraph() fg = HybridGaussianFactorGraph()
for i in range(2): for i in range(2):
conditional = bayesNet.atMixture(i) conditional = bayesNet.atMixture(i)
@ -161,13 +162,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
fg.push_back(bayesNet.atDiscrete(3)) fg.push_back(bayesNet.atDiscrete(3))
self.assertEqual(fg.size(), 4) self.assertEqual(fg.size(), 4)
# Calculate ratio between Bayes net probability and the factor graph:
# Calculate ratio between Bayes net probability and the factor graph:
continuousValues = gtsam.VectorValues() continuousValues = gtsam.VectorValues()
continuousValues.insert(X(0), sample.at(X(0))) continuousValues.insert(X(0), sample.at(X(0)))
discreteValues = sample.discrete() discreteValues = sample.discrete()
expected_ratio = bayesNet.evaluate(sample) / fg.probPrime( expected_ratio = bayesNet.evaluate(sample) / fg.probPrime(
continuousValues, discreteValues continuousValues, discreteValues)
) #TODO(Varun) This should be 1. Adding the normalizing factor should fix fg.probPrime
print(expected_ratio) print(expected_ratio)
# TODO(dellaert): Change the mode to 0 and calculate the ratio again. # TODO(dellaert): Change the mode to 0 and calculate the ratio again.