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);
if (conditional->isDiscrete()) {
auto discrete = conditional->asDiscrete();
KeyVector frontals(discrete->frontals().begin(),
discrete->frontals().end());
// Apply prunerFunc to the underlying AlgebraicDecisionTree
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. */
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

76
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
import unittest
import math
import numpy as np
from gtsam.symbol_shorthand import C, M, X, Z
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import (
DecisionTreeFactor,
DiscreteConditional,
DiscreteKeys,
GaussianConditional,
GaussianMixture,
GaussianMixtureFactor,
HybridGaussianFactorGraph,
JacobianFactor,
Ordering,
noiseModel,
)
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
GaussianMixture, GaussianMixtureFactor,
HybridGaussianFactorGraph, JacobianFactor, Ordering,
noiseModel)
class TestHybridGaussianFactorGraph(GtsamTestCase):
"""Unit tests for HybridGaussianFactorGraph."""
def test_create(self):
"""Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3)
@ -52,8 +42,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
hfg.push_back(gmf)
hbn = hfg.eliminateSequential(
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
)
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
hfg, [C(0)]))
self.assertEqual(hbn.size(), 2)
@ -84,36 +74,39 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
hfg.push_back(dtf)
hbn = hfg.eliminateSequential(
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
)
Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
hfg, [C(0)]))
hv = hbn.optimize()
self.assertEqual(hv.atDiscrete(C(0)), 1)
@staticmethod
def tiny(num_measurements: int = 1):
"""Create a tiny two variable hybrid model."""
def tiny(num_measurements: int = 1) -> gtsam.HybridBayesNet:
"""
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.
bayesNet = gtsam.HybridBayesNet()
# Create mode key: 0 is low-noise, 1 is high-noise.
modeKey = M(0)
mode = (modeKey, 2)
mode = (M(0), 2)
# Create Gaussian mixture Z(0) = X(0) + noise for each measurement.
I = np.eye(1)
keys = DiscreteKeys()
keys.push_back(mode)
for i in range(num_measurements):
conditional0 = GaussianConditional.FromMeanAndStddev(
Z(i), I, X(0), [0], sigma=0.5
)
conditional1 = GaussianConditional.FromMeanAndStddev(
Z(i), I, X(0), [0], sigma=3
)
bayesNet.emplaceMixture(
[Z(i)], [X(0)], keys, [conditional0, conditional1]
)
conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
I,
X(0), [0],
sigma=0.5)
conditional1 = GaussianConditional.FromMeanAndStddev(Z(i),
I,
X(0), [0],
sigma=3)
bayesNet.emplaceMixture([Z(i)], [X(0)], keys,
[conditional0, conditional1])
# Create prior on X(0).
prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0)
@ -142,14 +135,22 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
self.assertEqual(fg.size(), 3)
def test_tiny2(self):
"""Test a tiny two variable hybrid model, with 2 measurements."""
# Create the Bayes net and sample from it.
def test_ratio(self):
"""
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)
sample = bayesNet.sample()
# Sample from the Bayes net.
sample: gtsam.HybridValues = bayesNet.sample()
# print(sample)
# 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()
for i in range(2):
conditional = bayesNet.atMixture(i)
@ -161,13 +162,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
fg.push_back(bayesNet.atDiscrete(3))
self.assertEqual(fg.size(), 4)
# Calculate ratio between Bayes net probability and the factor graph:
continuousValues = gtsam.VectorValues()
continuousValues.insert(X(0), sample.at(X(0)))
discreteValues = sample.discrete()
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)
# TODO(dellaert): Change the mode to 0 and calculate the ratio again.