From 41a96473b577d1b08bbbc89f4ca26deaf20a856a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 25 Dec 2022 23:53:22 -0500
Subject: [PATCH 01/10] Pass reference
---
gtsam/hybrid/HybridValues.h | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h
index 4928f9384f..944fe17e61 100644
--- a/gtsam/hybrid/HybridValues.h
+++ b/gtsam/hybrid/HybridValues.h
@@ -78,10 +78,10 @@ class GTSAM_EXPORT HybridValues {
/// @{
/// Return the discrete MPE assignment
- DiscreteValues discrete() const { return discrete_; }
+ const DiscreteValues& discrete() const { return discrete_; }
/// Return the delta update for the continuous vectors
- VectorValues continuous() const { return continuous_; }
+ const VectorValues& continuous() const { return continuous_; }
/// Check whether a variable with key \c j exists in DiscreteValue.
bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
From b04f2f8582426857024ed54159cb55ace7739669 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 08:18:00 -0500
Subject: [PATCH 02/10] HBN::evaluate
# Conflicts:
# gtsam/hybrid/HybridBayesNet.cpp
# gtsam/hybrid/tests/testHybridBayesNet.cpp
---
gtsam/hybrid/HybridBayesNet.cpp | 44 ++++++++++--
gtsam/hybrid/HybridBayesNet.h | 8 +++
gtsam/hybrid/tests/testHybridBayesNet.cpp | 83 ++++++++++++++++++-----
3 files changed, 113 insertions(+), 22 deletions(-)
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 7a46d7832d..12b56ece88 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -150,9 +150,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
// Go through all the conditionals in the
// Bayes Net and prune them as per decisionTree.
- for (size_t i = 0; i < this->size(); i++) {
- HybridConditional::shared_ptr conditional = this->at(i);
-
+ for (auto &&conditional : *this) {
if (conditional->isHybrid()) {
GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture();
@@ -225,16 +223,50 @@ HybridValues HybridBayesNet::optimize() const {
DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize();
// Given the MPE, compute the optimal continuous values.
- GaussianBayesNet gbn = this->choose(mpe);
+ GaussianBayesNet gbn = choose(mpe);
return HybridValues(mpe, gbn.optimize());
}
/* ************************************************************************* */
VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
- GaussianBayesNet gbn = this->choose(assignment);
+ GaussianBayesNet gbn = choose(assignment);
return gbn.optimize();
}
+/* ************************************************************************* */
+double HybridBayesNet::evaluate(const HybridValues &values) const {
+ const DiscreteValues& discreteValues = values.discrete();
+ const VectorValues& continuosValues = values.continuous();
+
+ double probability = 1.0;
+
+ // Iterate over each conditional.
+ for (auto &&conditional : *this) {
+ if (conditional->isHybrid()) {
+ // If conditional is hybrid, select based on assignment and compute error.
+ // GaussianMixture::shared_ptr gm = conditional->asMixture();
+ // AlgebraicDecisionTree<Key> conditional_error =
+ // gm->error(continuousValues);
+
+ // error_tree = error_tree + conditional_error;
+
+ } else if (conditional->isContinuous()) {
+ // If continuous only, get the (double) error
+ // and add it to the error_tree
+ // double error = conditional->asGaussian()->error(continuousValues);
+ // // Add the computed error to every leaf of the error tree.
+ // error_tree = error_tree.apply(
+ // [error](double leaf_value) { return leaf_value + error; });
+ } else if (conditional->isDiscrete()) {
+ // Conditional is discrete-only, we skip.
+ probability *=
+ conditional->asDiscreteConditional()->operator()(discreteValues);
+ }
+ }
+
+ return probability;
+}
+
/* ************************************************************************* */
HybridValues HybridBayesNet::sample(const HybridValues &given,
std::mt19937_64 *rng) const {
@@ -273,7 +305,7 @@ HybridValues HybridBayesNet::sample() const {
/* ************************************************************************* */
double HybridBayesNet::error(const VectorValues &continuousValues,
const DiscreteValues &discreteValues) const {
- GaussianBayesNet gbn = this->choose(discreteValues);
+ GaussianBayesNet gbn = choose(discreteValues);
return gbn.error(continuousValues);
}
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 1e6bebf1a8..ff13ca1b73 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -95,6 +95,14 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
*/
GaussianBayesNet choose(const DiscreteValues &assignment) const;
+ //** evaluate for given HybridValues */
+ double evaluate(const HybridValues &values) const;
+
+ //** (Preferred) sugar for the above for given DiscreteValues */
+ double operator()(const HybridValues &values) const {
+ return evaluate(values);
+ }
+
/**
* @brief Solve the HybridBayesNet by first computing the MPE of all the
* discrete variables and then optimizing the continuous variables based on
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 3e3fab3760..8f9632ba35 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -36,36 +36,87 @@ using noiseModel::Isotropic;
using symbol_shorthand::M;
using symbol_shorthand::X;
-static const DiscreteKey Asia(0, 2);
+static const Key asiaKey = 0;
+static const DiscreteKey Asia(asiaKey, 2);
/* ****************************************************************************/
-// Test creation
+// Test creation of a pure discrete Bayes net.
TEST(HybridBayesNet, Creation) {
HybridBayesNet bayesNet;
-
bayesNet.add(Asia, "99/1");
DiscreteConditional expected(Asia, "99/1");
-
CHECK(bayesNet.atDiscrete(0));
- auto& df = *bayesNet.atDiscrete(0);
- EXPECT(df.equals(expected));
+ EXPECT(assert_equal(expected, *bayesNet.atDiscrete(0)));
}
/* ****************************************************************************/
-// Test adding a bayes net to another one.
+// Test adding a Bayes net to another one.
TEST(HybridBayesNet, Add) {
HybridBayesNet bayesNet;
-
bayesNet.add(Asia, "99/1");
- DiscreteConditional expected(Asia, "99/1");
-
HybridBayesNet other;
other.push_back(bayesNet);
EXPECT(bayesNet.equals(other));
}
+/* ****************************************************************************/
+// Test evaluate for a pure discrete Bayes net P(Asia).
+TEST(HybridBayesNet, evaluatePureDiscrete) {
+ HybridBayesNet bayesNet;
+ bayesNet.add(Asia, "99/1");
+ HybridValues values;
+ values.insert(asiaKey, 0);
+ EXPECT_DOUBLES_EQUAL(0.99, bayesNet.evaluate(values), 1e-9);
+}
+
+/* ****************************************************************************/
+// Test evaluate for a hybrid Bayes net P(X1|Asia) P(Asia).
+TEST(HybridBayesNet, evaluateHybrid) {
+ HybridBayesNet bayesNet;
+
+ SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+
+ const Vector2 d0(1, 2);
+ Matrix22 R0 = Matrix22::Ones();
+ const auto conditional0 =
+ boost::make_shared<GaussianConditional>(X(1), d0, R0, model);
+
+ const Vector2 d1(2, 1);
+ Matrix22 R1 = Matrix22::Ones();
+ const auto conditional1 =
+ boost::make_shared<GaussianConditional>(X(1), d1, R1, model);
+
+ // TODO(dellaert): creating and adding mixture is clumsy.
+ std::vector<GaussianConditional::shared_ptr> conditionals{conditional0,
+ conditional1};
+ const auto mixture =
+ GaussianMixture::FromConditionals({X(1)}, {}, {Asia}, conditionals);
+ bayesNet.push_back(
+ HybridConditional(boost::make_shared<GaussianMixture>(mixture)));
+
+ // Add component probabilities.
+ bayesNet.add(Asia, "99/1");
+
+ // Create values at which to evaluate.
+ HybridValues values;
+ values.insert(asiaKey, 0);
+ values.insert(X(1), Vector2(1, 2));
+
+ // TODO(dellaert): we need real probabilities!
+ const double conditionalProbability =
+ conditional0->error(values.continuous());
+ EXPECT_DOUBLES_EQUAL(conditionalProbability * 0.99, bayesNet.evaluate(values), 1e-9);
+}
+
+ // const Vector2 d1(2, 1);
+ // Matrix22 R1 = Matrix22::Ones();
+ // Matrix22 S1 = Matrix22::Identity() * 2;
+ // const auto conditional1 =
+ // boost::make_shared<GaussianConditional>(X(1), d1, R1, X(2), S1, model);
+
+
/* ****************************************************************************/
// Test choosing an assignment of conditionals
TEST(HybridBayesNet, Choose) {
@@ -105,7 +156,7 @@ TEST(HybridBayesNet, Choose) {
}
/* ****************************************************************************/
-// Test bayes net optimize
+// Test Bayes net optimize
TEST(HybridBayesNet, OptimizeAssignment) {
Switching s(4);
@@ -139,7 +190,7 @@ TEST(HybridBayesNet, OptimizeAssignment) {
}
/* ****************************************************************************/
-// Test bayes net optimize
+// Test Bayes net optimize
TEST(HybridBayesNet, Optimize) {
Switching s(4);
@@ -203,7 +254,7 @@ TEST(HybridBayesNet, Error) {
// regression
EXPECT(assert_equal(expected_error, error_tree, 1e-9));
- // Error on pruned bayes net
+ // Error on pruned Bayes net
auto prunedBayesNet = hybridBayesNet->prune(2);
auto pruned_error_tree = prunedBayesNet.error(delta.continuous());
@@ -238,7 +289,7 @@ TEST(HybridBayesNet, Error) {
}
/* ****************************************************************************/
-// Test bayes net pruning
+// Test Bayes net pruning
TEST(HybridBayesNet, Prune) {
Switching s(4);
@@ -256,7 +307,7 @@ TEST(HybridBayesNet, Prune) {
}
/* ****************************************************************************/
-// Test bayes net updateDiscreteConditionals
+// Test Bayes net updateDiscreteConditionals
TEST(HybridBayesNet, UpdateDiscreteConditionals) {
Switching s(4);
@@ -358,7 +409,7 @@ TEST(HybridBayesNet, Sampling) {
// Sample
HybridValues sample = bn->sample(&gen);
- discrete_samples.push_back(sample.discrete()[M(0)]);
+ discrete_samples.push_back(sample.discrete().at(M(0)));
if (i == 0) {
average_continuous.insert(sample.continuous());
From 8d4dc3d880776382b7ce1a4b086a58a658bbd4bf Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 10:19:30 -0500
Subject: [PATCH 03/10] GBN::evaluate prototype code works
---
gtsam/linear/tests/testGaussianBayesNet.cpp | 65 ++++++++++++++++++++-
1 file changed, 64 insertions(+), 1 deletion(-)
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 2b125265f7..b8534a02b3 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@@ -67,6 +67,69 @@ TEST( GaussianBayesNet, Matrix )
EXPECT(assert_equal(d,d1));
}
+/* ************************************************************************* */
+/**
+ * Calculate log-density for given values `x`:
+ * -0.5*(error + n*log(2*pi) + log det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+double logDensity(const GaussianConditional::shared_ptr& gc,
+ const VectorValues& x) {
+ constexpr double log2pi = 1.8378770664093454835606594728112;
+ size_t n = gc->d().size();
+ // log det(Sigma)) = - 2 * gc->logDeterminant()
+ double sum = gc->error(x) + n * log2pi - 2 * gc->logDeterminant();
+ return -0.5 * sum;
+}
+
+/**
+ * Calculate probability density for given values `x`:
+ * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+double evaluate(const GaussianConditional::shared_ptr& gc,
+ const VectorValues& x) {
+ return exp(logDensity(gc, x));
+}
+
+/** Calculate probability for given values `x` */
+double evaluate(const GaussianBayesNet& gbn, const VectorValues& x) {
+ double density = 1.0;
+ for (const auto& conditional : gbn) {
+ if (conditional) density *= evaluate(conditional, x);
+ }
+ return density;
+}
+
+// Check that the evaluate function matches direct calculation with R.
+TEST(GaussianBayesNet, Evaluate1) {
+ // Let's evaluate at the mean
+ const VectorValues mean = smallBayesNet.optimize();
+
+ // We get the matrix, which has noise model applied!
+ const Matrix R = smallBayesNet.matrix().first;
+ const Matrix invSigma = R.transpose() * R;
+
+ // The Bayes net is a Gaussian density ~ exp (-0.5*(Rx-d)'*(Rx-d))
+ // which at the mean is 1.0! So, the only thing we need to calculate is
+ // the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
+ // The covariance matrix inv(Sigma) = R'*R, so the determinant is
+ const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
+ const double actual = evaluate(smallBayesNet, mean);
+ EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+}
+
+// Check the evaluate with non-unit noise.
+TEST(GaussianBayesNet, Evaluate2) {
+ // See comments in test above.
+ const VectorValues mean = noisyBayesNet.optimize();
+ const Matrix R = noisyBayesNet.matrix().first;
+ const Matrix invSigma = R.transpose() * R;
+ const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
+ const double actual = evaluate(noisyBayesNet, mean);
+ EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+}
+
/* ************************************************************************* */
TEST( GaussianBayesNet, NoisyMatrix )
{
From 1d3a7d47530f70d1ad61f141ee93f1f85ddb7ad2 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 10:59:01 -0500
Subject: [PATCH 04/10] Added logDensity and evaluate to Gaussian conditional
and Bayes net
---
gtsam/linear/GaussianBayesNet.cpp | 14 +++++++
gtsam/linear/GaussianBayesNet.h | 14 +++++++
gtsam/linear/GaussianConditional.cpp | 14 +++++++
gtsam/linear/GaussianConditional.h | 21 +++++++---
gtsam/linear/tests/testGaussianBayesNet.cpp | 37 +-----------------
.../linear/tests/testGaussianConditional.cpp | 38 +++++++++++++++++++
6 files changed, 98 insertions(+), 40 deletions(-)
diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 229d4a9327..4c13384356 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -224,5 +224,19 @@ namespace gtsam {
}
/* ************************************************************************* */
+ double GaussianBayesNet::logDensity(const VectorValues& x) const {
+ double sum = 0.0;
+ for (const auto& conditional : *this) {
+ if (conditional) sum += conditional->logDensity(x);
+ }
+ return sum;
+ }
+
+ /* ************************************************************************* */
+ double GaussianBayesNet::evaluate(const VectorValues& x) const {
+ return exp(logDensity(x));
+ }
+
+ /* ************************************************************************* */
} // namespace gtsam
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index e81d6af332..381b8ad3a3 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -88,6 +88,20 @@ namespace gtsam {
/// @name Standard Interface
/// @{
+ /**
+ * Calculate log-density for given values `x`:
+ * -0.5*(error + n*log(2*pi) + log det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+ double logDensity(const VectorValues& x) const;
+
+ /**
+ * Calculate probability density for given values `x`:
+ * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+ double evaluate(const VectorValues& x) const;
+
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
/// back-substitution
VectorValues optimize() const;
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 4597156bc1..ec895b8e75 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -169,6 +169,20 @@ double GaussianConditional::logDeterminant() const {
return logDet;
}
+ /* ************************************************************************* */
+double GaussianConditional::logDensity(const VectorValues& x) const {
+ constexpr double log2pi = 1.8378770664093454835606594728112;
+ size_t n = d().size();
+ // log det(Sigma)) = - 2 * logDeterminant()
+ double sum = error(x) + n * log2pi - 2 * logDeterminant();
+ return -0.5 * sum;
+}
+
+/* ************************************************************************* */
+double GaussianConditional::evaluate(const VectorValues& x) const {
+ return exp(logDensity(x));
+}
+
/* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const {
// Concatenate all vector values that correspond to parent variables
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 8af7f66029..cf8cada828 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -121,6 +121,20 @@ namespace gtsam {
/// @name Standard Interface
/// @{
+ /**
+ * Calculate log-density for given values `x`:
+ * -0.5*(error + n*log(2*pi) + log det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+ double logDensity(const VectorValues& x) const;
+
+ /**
+ * Calculate probability density for given values `x`:
+ * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
+ * where x is the vector of values, and Sigma is the covariance matrix.
+ */
+ double evaluate(const VectorValues& x) const;
+
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
@@ -134,9 +148,7 @@ namespace gtsam {
const constBVector d() const { return BaseFactor::getb(); }
/**
- * @brief Compute the log determinant of the Gaussian conditional.
- * The determinant is computed using the R matrix, which is upper
- * triangular.
+ * @brief Compute the log determinant of the R matrix.
* For numerical stability, the determinant is computed in log
* form, so it is a summation rather than a multiplication.
*
@@ -145,8 +157,7 @@ namespace gtsam {
double logDeterminant() const;
/**
- * @brief Compute the determinant of the conditional from the
- * upper-triangular R matrix.
+ * @brief Compute the determinant of the R matrix.
*
* The determinant is computed in log form (hence summation) for numerical
* stability and then exponentiated.
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index b8534a02b3..e5e8840c06 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -68,39 +68,6 @@ TEST( GaussianBayesNet, Matrix )
}
/* ************************************************************************* */
-/**
- * Calculate log-density for given values `x`:
- * -0.5*(error + n*log(2*pi) + log det(Sigma))
- * where x is the vector of values, and Sigma is the covariance matrix.
- */
-double logDensity(const GaussianConditional::shared_ptr& gc,
- const VectorValues& x) {
- constexpr double log2pi = 1.8378770664093454835606594728112;
- size_t n = gc->d().size();
- // log det(Sigma)) = - 2 * gc->logDeterminant()
- double sum = gc->error(x) + n * log2pi - 2 * gc->logDeterminant();
- return -0.5 * sum;
-}
-
-/**
- * Calculate probability density for given values `x`:
- * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
- * where x is the vector of values, and Sigma is the covariance matrix.
- */
-double evaluate(const GaussianConditional::shared_ptr& gc,
- const VectorValues& x) {
- return exp(logDensity(gc, x));
-}
-
-/** Calculate probability for given values `x` */
-double evaluate(const GaussianBayesNet& gbn, const VectorValues& x) {
- double density = 1.0;
- for (const auto& conditional : gbn) {
- if (conditional) density *= evaluate(conditional, x);
- }
- return density;
-}
-
// Check that the evaluate function matches direct calculation with R.
TEST(GaussianBayesNet, Evaluate1) {
// Let's evaluate at the mean
@@ -115,7 +82,7 @@ TEST(GaussianBayesNet, Evaluate1) {
// the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
// The covariance matrix inv(Sigma) = R'*R, so the determinant is
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
- const double actual = evaluate(smallBayesNet, mean);
+ const double actual = smallBayesNet.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
}
@@ -126,7 +93,7 @@ TEST(GaussianBayesNet, Evaluate2) {
const Matrix R = noisyBayesNet.matrix().first;
const Matrix invSigma = R.transpose() * R;
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
- const double actual = evaluate(noisyBayesNet, mean);
+ const double actual = noisyBayesNet.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
}
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index 6ec06a0ceb..a3b64528b4 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -130,6 +130,44 @@ TEST( GaussianConditional, equals )
EXPECT( expected.equals(actual) );
}
+namespace density {
+static const Key key = 77;
+static const auto unitPrior =
+ GaussianConditional(key, Vector1::Constant(5), I_1x1),
+ widerPrior =
+ GaussianConditional(key, Vector1::Constant(5), I_1x1,
+ noiseModel::Isotropic::Sigma(1, 3.0));
+} // namespace density
+
+/* ************************************************************************* */
+// Check that the evaluate function matches direct calculation with R.
+TEST(GaussianConditional, Evaluate1) {
+ // Let's evaluate at the mean
+ const VectorValues mean = density::unitPrior.solve(VectorValues());
+
+ // We get the Hessian matrix, which has noise model applied!
+ const Matrix invSigma = density::unitPrior.information();
+
+ // A Gaussian density ~ exp (-0.5*(Rx-d)'*(Rx-d))
+ // which at the mean is 1.0! So, the only thing we need to calculate is
+ // the normalization constant 1.0/sqrt((2*pi*Sigma).det()).
+ // The covariance matrix inv(Sigma) = R'*R, so the determinant is
+ const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
+ const double actual = density::unitPrior.evaluate(mean);
+ EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+}
+
+// Check the evaluate with non-unit noise.
+TEST(GaussianConditional, Evaluate2) {
+ // See comments in test above.
+ const VectorValues mean = density::widerPrior.solve(VectorValues());
+ const Matrix R = density::widerPrior.R();
+ const Matrix invSigma = density::widerPrior.information();
+ const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
+ const double actual = density::widerPrior.evaluate(mean);
+ EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+}
+
/* ************************************************************************* */
TEST( GaussianConditional, solve )
{
From 8391c783bf24e862ebf15992bd65b9bf63435ff3 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 12:00:37 -0500
Subject: [PATCH 05/10] Correct densities (error already includes 0.5!)
---
gtsam/linear/GaussianBayesNet.h | 18 +++++---
gtsam/linear/GaussianConditional.cpp | 9 ++--
gtsam/linear/GaussianConditional.h | 41 ++++++++++++-------
.../linear/tests/testGaussianConditional.cpp | 37 +++++++++++++++--
4 files changed, 78 insertions(+), 27 deletions(-)
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 381b8ad3a3..426d3bd833 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -89,18 +89,24 @@ namespace gtsam {
/// @{
/**
- * Calculate log-density for given values `x`:
- * -0.5*(error + n*log(2*pi) + log det(Sigma))
+ * Calculate probability density for given values `x`:
+ * exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
+ * Note that error(x)=0.5*e'*e includes the 0.5 factor already.
*/
- double logDensity(const VectorValues& x) const;
+ double evaluate(const VectorValues& x) const;
+
+ /// Evaluate probability density, sugar.
+ double operator()(const VectorValues& x) const {
+ return evaluate(x);
+ }
/**
- * Calculate probability density for given values `x`:
- * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
+ * Calculate log-density for given values `x`:
+ * -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
* where x is the vector of values, and Sigma is the covariance matrix.
*/
- double evaluate(const VectorValues& x) const;
+ double logDensity(const VectorValues& x) const;
/// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
/// back-substitution
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index ec895b8e75..5e8a193cf1 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -169,13 +169,14 @@ double GaussianConditional::logDeterminant() const {
return logDet;
}
- /* ************************************************************************* */
+/* ************************************************************************* */
+// density = exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
+// log = -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
double GaussianConditional::logDensity(const VectorValues& x) const {
constexpr double log2pi = 1.8378770664093454835606594728112;
size_t n = d().size();
- // log det(Sigma)) = - 2 * logDeterminant()
- double sum = error(x) + n * log2pi - 2 * logDeterminant();
- return -0.5 * sum;
+ // log det(Sigma)) = - 2.0 * logDeterminant()
+ return - error(x) - 0.5 * n * log2pi + logDeterminant();
}
/* ************************************************************************* */
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index cf8cada828..a72a739737 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -122,18 +122,24 @@ namespace gtsam {
/// @{
/**
- * Calculate log-density for given values `x`:
- * -0.5*(error + n*log(2*pi) + log det(Sigma))
+ * Calculate probability density for given values `x`:
+ * exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
* where x is the vector of values, and Sigma is the covariance matrix.
+ * Note that error(x)=0.5*e'*e includes the 0.5 factor already.
*/
- double logDensity(const VectorValues& x) const;
+ double evaluate(const VectorValues& x) const;
+
+ /// Evaluate probability density, sugar.
+ double operator()(const VectorValues& x) const {
+ return evaluate(x);
+ }
/**
- * Calculate probability density for given values `x`:
- * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
+ * Calculate log-density for given values `x`:
+ * -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
* where x is the vector of values, and Sigma is the covariance matrix.
*/
- double evaluate(const VectorValues& x) const;
+ double logDensity(const VectorValues& x) const;
/** Return a view of the upper-triangular R block of the conditional */
constABlock R() const { return Ab_.range(0, nrFrontals()); }
@@ -148,23 +154,30 @@ namespace gtsam {
const constBVector d() const { return BaseFactor::getb(); }
/**
- * @brief Compute the log determinant of the R matrix.
- * For numerical stability, the determinant is computed in log
- * form, so it is a summation rather than a multiplication.
+ * @brief Compute the determinant of the R matrix.
+ *
+ * The determinant is computed in log form using logDeterminant for
+ * numerical stability and then exponentiated.
+ *
+ * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
+ * \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
*
* @return double
*/
- double logDeterminant() const;
+ double determinant() const { return exp(this->logDeterminant()); }
/**
- * @brief Compute the determinant of the R matrix.
+ * @brief Compute the log determinant of the R matrix.
+ *
+ * For numerical stability, the determinant is computed in log
+ * form, so it is a summation rather than a multiplication.
*
- * The determinant is computed in log form (hence summation) for numerical
- * stability and then exponentiated.
+ * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
+ * \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
*
* @return double
*/
- double determinant() const { return exp(this->logDeterminant()); }
+ double logDeterminant() const;
/**
* Solves a conditional Gaussian and writes the solution into the entries of
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index a3b64528b4..20d7308560 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -130,13 +130,15 @@ TEST( GaussianConditional, equals )
EXPECT( expected.equals(actual) );
}
+/* ************************************************************************* */
namespace density {
static const Key key = 77;
+static constexpr double sigma = 3.0;
static const auto unitPrior =
GaussianConditional(key, Vector1::Constant(5), I_1x1),
- widerPrior =
- GaussianConditional(key, Vector1::Constant(5), I_1x1,
- noiseModel::Isotropic::Sigma(1, 3.0));
+ widerPrior = GaussianConditional(
+ key, Vector1::Constant(5), I_1x1,
+ noiseModel::Isotropic::Sigma(1, sigma));
} // namespace density
/* ************************************************************************* */
@@ -155,8 +157,23 @@ TEST(GaussianConditional, Evaluate1) {
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
const double actual = density::unitPrior.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+
+ using density::key;
+ using density::sigma;
+
+ // Let's numerically integrate and see that we integrate to 1.0.
+ double integral = 0.0;
+ // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
+ for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
+ VectorValues xValues;
+ xValues.insert(key, mean.at(key) + Vector1(x));
+ const double density = density::unitPrior.evaluate(xValues);
+ integral += 0.1 * sigma * density;
+ }
+ EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9);
}
+/* ************************************************************************* */
// Check the evaluate with non-unit noise.
TEST(GaussianConditional, Evaluate2) {
// See comments in test above.
@@ -166,6 +183,20 @@ TEST(GaussianConditional, Evaluate2) {
const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
const double actual = density::widerPrior.evaluate(mean);
EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+
+ using density::key;
+ using density::sigma;
+
+ // Let's numerically integrate and see that we integrate to 1.0.
+ double integral = 0.0;
+ // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
+ for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
+ VectorValues xValues;
+ xValues.insert(key, mean.at(key) + Vector1(x));
+ const double density = density::widerPrior.evaluate(xValues);
+ integral += 0.1 * sigma * density;
+ }
+ EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-5);
}
/* ************************************************************************* */
From b3b635cd94ee22f15db97eeb957a74b5e3c3f602 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 12:31:13 -0500
Subject: [PATCH 06/10] Test sampling using Monte Carlo integration
---
gtsam/linear/tests/testGaussianBayesNet.cpp | 35 ++++++++++++++++-----
1 file changed, 28 insertions(+), 7 deletions(-)
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index e5e8840c06..15f0d098a0 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -172,14 +172,18 @@ TEST( GaussianBayesNet, optimize3 )
}
/* ************************************************************************* */
-TEST(GaussianBayesNet, sample) {
- GaussianBayesNet gbn;
- Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
- const Vector2 mean(20, 40), b(10, 10);
- const double sigma = 0.01;
+namespace sampling {
+static Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+static const Vector2 mean(20, 40), b(10, 10);
+static const double sigma = 0.01;
+static const GaussianBayesNet gbn =
+ list_of(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma))(
+ GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
+} // namespace sampling
- gbn.add(GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma));
- gbn.add(GaussianDensity::FromMeanAndStddev(X(1), mean, sigma));
+/* ************************************************************************* */
+TEST(GaussianBayesNet, sample) {
+ using namespace sampling;
auto actual = gbn.sample();
EXPECT_LONGS_EQUAL(2, actual.size());
@@ -195,6 +199,23 @@ TEST(GaussianBayesNet, sample) {
// EXPECT(assert_equal(Vector2(110.032083, 230.039811), actual[X(0)], 1e-5));
}
+/* ************************************************************************* */
+// Do Monte Carlo integration of square deviation, should be equal to 9.0.
+TEST(GaussianBayesNet, MonteCarloIntegration) {
+ GaussianBayesNet gbn;
+ gbn.push_back(noisyBayesNet.at(1));
+
+ double sum = 0.0;
+ constexpr size_t N = 500;
+ // loop for N samples:
+ for (size_t i = 0; i < N; i++) {
+ const auto X_i = gbn.sample();
+ sum += pow(X_i[_y_].x() - 5.0, 2.0);
+ }
+ // Expected is variance = 3*3
+ EXPECT_DOUBLES_EQUAL(9.0, sum / N, 0.1);
+}
+
/* ************************************************************************* */
TEST(GaussianBayesNet, ordering)
{
From 911e46b0a0fa9cdb46cef3b2b1c4241888828c91 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 12:55:38 -0500
Subject: [PATCH 07/10] Evaluate for hybrid Bayes nets
---
gtsam/hybrid/HybridBayesNet.cpp | 25 ++++-------
gtsam/hybrid/HybridBayesNet.h | 4 +-
gtsam/hybrid/tests/testHybridBayesNet.cpp | 51 ++++++++++-------------
3 files changed, 33 insertions(+), 47 deletions(-)
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 12b56ece88..6024add073 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -235,30 +235,23 @@ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
/* ************************************************************************* */
double HybridBayesNet::evaluate(const HybridValues &values) const {
- const DiscreteValues& discreteValues = values.discrete();
- const VectorValues& continuosValues = values.continuous();
+ const DiscreteValues &discreteValues = values.discrete();
+ const VectorValues &continuousValues = values.continuous();
double probability = 1.0;
// Iterate over each conditional.
for (auto &&conditional : *this) {
if (conditional->isHybrid()) {
- // If conditional is hybrid, select based on assignment and compute error.
- // GaussianMixture::shared_ptr gm = conditional->asMixture();
- // AlgebraicDecisionTree<Key> conditional_error =
- // gm->error(continuousValues);
-
- // error_tree = error_tree + conditional_error;
-
+ // If conditional is hybrid, select based on assignment and evaluate.
+ const GaussianMixture::shared_ptr gm = conditional->asMixture();
+ const auto conditional = (*gm)(discreteValues);
+ probability *= conditional->evaluate(continuousValues);
} else if (conditional->isContinuous()) {
- // If continuous only, get the (double) error
- // and add it to the error_tree
- // double error = conditional->asGaussian()->error(continuousValues);
- // // Add the computed error to every leaf of the error tree.
- // error_tree = error_tree.apply(
- // [error](double leaf_value) { return leaf_value + error; });
+ // If continuous only, evaluate the probability and multiply.
+ probability *= conditional->asGaussian()->evaluate(continuousValues);
} else if (conditional->isDiscrete()) {
- // Conditional is discrete-only, we skip.
+ // Conditional is discrete-only, so return its probability.
probability *=
conditional->asDiscreteConditional()->operator()(discreteValues);
}
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index ff13ca1b73..4e41cb11df 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -95,10 +95,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
*/
GaussianBayesNet choose(const DiscreteValues &assignment) const;
- //** evaluate for given HybridValues */
+ /// Evaluate hybrid probability density for given HybridValues.
double evaluate(const HybridValues &values) const;
- //** (Preferred) sugar for the above for given DiscreteValues */
+ /// Evaluate hybrid probability density for given HybridValues, sugar.
double operator()(const HybridValues &values) const {
return evaluate(values);
}
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index 8f9632ba35..f90152abe2 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -72,51 +72,44 @@ TEST(HybridBayesNet, evaluatePureDiscrete) {
}
/* ****************************************************************************/
-// Test evaluate for a hybrid Bayes net P(X1|Asia) P(Asia).
+// Test evaluate for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia).
TEST(HybridBayesNet, evaluateHybrid) {
- HybridBayesNet bayesNet;
-
- SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+ const auto continuousConditional = GaussianConditional::FromMeanAndStddev(
+ X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0);
- const Vector2 d0(1, 2);
- Matrix22 R0 = Matrix22::Ones();
- const auto conditional0 =
- boost::make_shared<GaussianConditional>(X(1), d0, R0, model);
+ const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)),
+ model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0));
- const Vector2 d1(2, 1);
- Matrix22 R1 = Matrix22::Ones();
- const auto conditional1 =
- boost::make_shared<GaussianConditional>(X(1), d1, R1, model);
+ const auto conditional0 = boost::make_shared<GaussianConditional>(
+ X(1), Vector1::Constant(5), I_1x1, model0),
+ conditional1 = boost::make_shared<GaussianConditional>(
+ X(1), Vector1::Constant(2), I_1x1, model1);
// TODO(dellaert): creating and adding mixture is clumsy.
- std::vector<GaussianConditional::shared_ptr> conditionals{conditional0,
- conditional1};
- const auto mixture =
- GaussianMixture::FromConditionals({X(1)}, {}, {Asia}, conditionals);
+ const auto mixture = GaussianMixture::FromConditionals(
+ {X(1)}, {}, {Asia}, {conditional0, conditional1});
+
+ // Create hybrid Bayes net.
+ HybridBayesNet bayesNet;
+ bayesNet.push_back(HybridConditional(
+ boost::make_shared<GaussianConditional>(continuousConditional)));
bayesNet.push_back(
HybridConditional(boost::make_shared<GaussianMixture>(mixture)));
-
- // Add component probabilities.
bayesNet.add(Asia, "99/1");
// Create values at which to evaluate.
HybridValues values;
values.insert(asiaKey, 0);
- values.insert(X(1), Vector2(1, 2));
+ values.insert(X(0), Vector1(-6));
+ values.insert(X(1), Vector1(1));
- // TODO(dellaert): we need real probabilities!
const double conditionalProbability =
- conditional0->error(values.continuous());
- EXPECT_DOUBLES_EQUAL(conditionalProbability * 0.99, bayesNet.evaluate(values), 1e-9);
+ continuousConditional.evaluate(values.continuous());
+ const double mixtureProbability = conditional0->evaluate(values.continuous());
+ EXPECT_DOUBLES_EQUAL(conditionalProbability * mixtureProbability * 0.99,
+ bayesNet.evaluate(values), 1e-9);
}
- // const Vector2 d1(2, 1);
- // Matrix22 R1 = Matrix22::Ones();
- // Matrix22 S1 = Matrix22::Identity() * 2;
- // const auto conditional1 =
- // boost::make_shared<GaussianConditional>(X(1), d1, R1, X(2), S1, model);
-
-
/* ****************************************************************************/
// Test choosing an assignment of conditionals
TEST(HybridBayesNet, Choose) {
From c984a5ffa251384b90efb337b6f982b68eacd30e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 13:52:40 -0500
Subject: [PATCH 08/10] Switch to dynamic pointer cast
---
gtsam/hybrid/HybridConditional.h | 23 +++++++++--------------
1 file changed, 9 insertions(+), 14 deletions(-)
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 050f10290e..db03ba59c4 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -131,34 +131,29 @@ class GTSAM_EXPORT HybridConditional
/**
* @brief Return HybridConditional as a GaussianMixture
- *
- * @return GaussianMixture::shared_ptr
+ * @return nullptr if not a mixture
+ * @return GaussianMixture::shared_ptr otherwise
*/
GaussianMixture::shared_ptr asMixture() {
- if (!isHybrid()) throw std::invalid_argument("Not a mixture");
- return boost::static_pointer_cast<GaussianMixture>(inner_);
+ return boost::dynamic_pointer_cast<GaussianMixture>(inner_);
}
/**
* @brief Return HybridConditional as a GaussianConditional
- *
- * @return GaussianConditional::shared_ptr
+ * @return nullptr if not a GaussianConditional
+ * @return GaussianConditional::shared_ptr otherwise
*/
GaussianConditional::shared_ptr asGaussian() {
- if (!isContinuous())
- throw std::invalid_argument("Not a continuous conditional");
- return boost::static_pointer_cast<GaussianConditional>(inner_);
+ return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
}
/**
* @brief Return conditional as a DiscreteConditional
- *
+ * @return nullptr if not a DiscreteConditional
* @return DiscreteConditional::shared_ptr
*/
- DiscreteConditional::shared_ptr asDiscreteConditional() {
- if (!isDiscrete())
- throw std::invalid_argument("Not a discrete conditional");
- return boost::static_pointer_cast<DiscreteConditional>(inner_);
+ DiscreteConditional::shared_ptr asDiscrete() {
+ return boost::dynamic_pointer_cast<DiscreteConditional>(inner_);
}
/// @}
From 1134d1c88e1fb28daa50a3467b7c97408a1ab7b5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 13:52:59 -0500
Subject: [PATCH 09/10] Refactor with uniform dynamic pointer cast API
---
gtsam/hybrid/HybridBayesNet.cpp | 64 ++++++++-----------
gtsam/hybrid/HybridBayesTree.cpp | 4 +-
gtsam/hybrid/tests/testHybridBayesNet.cpp | 6 +-
.../tests/testHybridGaussianFactorGraph.cpp | 2 +-
gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 9 ++-
.../hybrid/tests/testHybridNonlinearISAM.cpp | 9 ++-
6 files changed, 39 insertions(+), 55 deletions(-)
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 6024add073..c598b7d625 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -36,7 +36,7 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
// Convert to a DecisionTreeFactor and add it to the main factor.
- DecisionTreeFactor f(*conditional->asDiscreteConditional());
+ DecisionTreeFactor f(*conditional->asDiscrete());
dtFactor = dtFactor * f;
}
}
@@ -108,7 +108,7 @@ void HybridBayesNet::updateDiscreteConditionals(
HybridConditional::shared_ptr conditional = this->at(i);
if (conditional->isDiscrete()) {
// std::cout << demangle(typeid(conditional).name()) << std::endl;
- auto discrete = conditional->asDiscreteConditional();
+ auto discrete = conditional->asDiscrete();
KeyVector frontals(discrete->frontals().begin(),
discrete->frontals().end());
@@ -151,13 +151,10 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
// Go through all the conditionals in the
// Bayes Net and prune them as per decisionTree.
for (auto &&conditional : *this) {
- if (conditional->isHybrid()) {
- GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture();
-
+ if (auto gm = conditional->asMixture()) {
// Make a copy of the Gaussian mixture and prune it!
- auto prunedGaussianMixture =
- boost::make_shared<GaussianMixture>(*gaussianMixture);
- prunedGaussianMixture->prune(*decisionTree);
+ auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(*gm);
+ prunedGaussianMixture->prune(*decisionTree); // imperative :-(
// Type-erase and add to the pruned Bayes Net fragment.
prunedBayesNetFragment.push_back(
@@ -184,7 +181,7 @@ GaussianConditional::shared_ptr HybridBayesNet::atGaussian(size_t i) const {
/* ************************************************************************* */
DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const {
- return at(i)->asDiscreteConditional();
+ return at(i)->asDiscrete();
}
/* ************************************************************************* */
@@ -192,16 +189,13 @@ GaussianBayesNet HybridBayesNet::choose(
const DiscreteValues &assignment) const {
GaussianBayesNet gbn;
for (auto &&conditional : *this) {
- if (conditional->isHybrid()) {
+ if (auto gm = conditional->asMixture()) {
// If conditional is hybrid, select based on assignment.
- GaussianMixture gm = *conditional->asMixture();
- gbn.push_back(gm(assignment));
-
- } else if (conditional->isContinuous()) {
+ gbn.push_back((*gm)(assignment));
+ } else if (auto gc = conditional->asGaussian()) {
// If continuous only, add Gaussian conditional.
- gbn.push_back((conditional->asGaussian()));
-
- } else if (conditional->isDiscrete()) {
+ gbn.push_back(gc);
+ } else if (auto dc = conditional->asDiscrete()) {
// If conditional is discrete-only, we simply continue.
continue;
}
@@ -216,7 +210,7 @@ HybridValues HybridBayesNet::optimize() const {
DiscreteBayesNet discrete_bn;
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
- discrete_bn.push_back(conditional->asDiscreteConditional());
+ discrete_bn.push_back(conditional->asDiscrete());
}
}
@@ -238,26 +232,23 @@ double HybridBayesNet::evaluate(const HybridValues &values) const {
const DiscreteValues &discreteValues = values.discrete();
const VectorValues &continuousValues = values.continuous();
- double probability = 1.0;
+ double logDensity = 0.0, probability = 1.0;
// Iterate over each conditional.
for (auto &&conditional : *this) {
- if (conditional->isHybrid()) {
- // If conditional is hybrid, select based on assignment and evaluate.
- const GaussianMixture::shared_ptr gm = conditional->asMixture();
- const auto conditional = (*gm)(discreteValues);
- probability *= conditional->evaluate(continuousValues);
- } else if (conditional->isContinuous()) {
+ if (auto gm = conditional->asMixture()) {
+ const auto component = (*gm)(discreteValues);
+ logDensity += component->logDensity(continuousValues);
+ } else if (auto gc = conditional->asGaussian()) {
// If continuous only, evaluate the probability and multiply.
- probability *= conditional->asGaussian()->evaluate(continuousValues);
- } else if (conditional->isDiscrete()) {
+ logDensity += gc->logDensity(continuousValues);
+ } else if (auto dc = conditional->asDiscrete()) {
// Conditional is discrete-only, so return its probability.
- probability *=
- conditional->asDiscreteConditional()->operator()(discreteValues);
+ probability *= dc->operator()(discreteValues);
}
}
- return probability;
+ return probability * exp(logDensity);
}
/* ************************************************************************* */
@@ -267,7 +258,7 @@ HybridValues HybridBayesNet::sample(const HybridValues &given,
for (auto &&conditional : *this) {
if (conditional->isDiscrete()) {
// If conditional is discrete-only, we add to the discrete Bayes net.
- dbn.push_back(conditional->asDiscreteConditional());
+ dbn.push_back(conditional->asDiscrete());
}
}
// Sample a discrete assignment.
@@ -309,23 +300,20 @@ AlgebraicDecisionTree<Key> HybridBayesNet::error(
// Iterate over each conditional.
for (auto &&conditional : *this) {
- if (conditional->isHybrid()) {
+ if (auto gm = conditional->asMixture()) {
// If conditional is hybrid, select based on assignment and compute error.
- GaussianMixture::shared_ptr gm = conditional->asMixture();
AlgebraicDecisionTree<Key> conditional_error =
gm->error(continuousValues);
error_tree = error_tree + conditional_error;
-
- } else if (conditional->isContinuous()) {
+ } else if (auto gc = conditional->asGaussian()) {
// If continuous only, get the (double) error
// and add it to the error_tree
- double error = conditional->asGaussian()->error(continuousValues);
+ double error = gc->error(continuousValues);
// Add the computed error to every leaf of the error tree.
error_tree = error_tree.apply(
[error](double leaf_value) { return leaf_value + error; });
-
- } else if (conditional->isDiscrete()) {
+ } else if (auto dc = conditional->asDiscrete()) {
// Conditional is discrete-only, we skip.
continue;
}
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 8fdedab44f..ed70a0aa9b 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -49,7 +49,7 @@ HybridValues HybridBayesTree::optimize() const {
// The root should be discrete only, we compute the MPE
if (root_conditional->isDiscrete()) {
- dbn.push_back(root_conditional->asDiscreteConditional());
+ dbn.push_back(root_conditional->asDiscrete());
mpe = DiscreteFactorGraph(dbn).optimize();
} else {
throw std::runtime_error(
@@ -147,7 +147,7 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const {
/* ************************************************************************* */
void HybridBayesTree::prune(const size_t maxNrLeaves) {
auto decisionTree =
- this->roots_.at(0)->conditional()->asDiscreteConditional();
+ this->roots_.at(0)->conditional()->asDiscrete();
DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves);
decisionTree->root_ = prunedDecisionTree.root_;
diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
index f90152abe2..d22087f47b 100644
--- a/gtsam/hybrid/tests/testHybridBayesNet.cpp
+++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -317,8 +317,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/,
prunedDecisionTree->nrLeaves());
- auto original_discrete_conditionals =
- *(hybridBayesNet->at(4)->asDiscreteConditional());
+ auto original_discrete_conditionals = *(hybridBayesNet->at(4)->asDiscrete());
// Prune!
hybridBayesNet->prune(maxNrLeaves);
@@ -338,8 +337,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) {
};
// Get the pruned discrete conditionals as an AlgebraicDecisionTree
- auto pruned_discrete_conditionals =
- hybridBayesNet->at(4)->asDiscreteConditional();
+ auto pruned_discrete_conditionals = hybridBayesNet->at(4)->asDiscrete();
auto discrete_conditional_tree =
boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>(
pruned_discrete_conditionals);
diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
index 7877461b67..55e4c28adf 100644
--- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
@@ -133,7 +133,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
auto result =
hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {M(1)}));
- auto dc = result->at(2)->asDiscreteConditional();
+ auto dc = result->at(2)->asDiscrete();
DiscreteValues dv;
dv[M(1)] = 0;
EXPECT_DOUBLES_EQUAL(1, dc->operator()(dv), 1e-3);
diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index 18ce7f10ec..11bd3b4155 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
@@ -111,8 +111,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
// Run update step
isam.update(graph1);
- auto discreteConditional_m0 =
- isam[M(0)]->conditional()->asDiscreteConditional();
+ auto discreteConditional_m0 = isam[M(0)]->conditional()->asDiscrete();
EXPECT(discreteConditional_m0->keys() == KeyVector({M(0)}));
/********************************************************/
@@ -170,10 +169,10 @@ TEST(HybridGaussianElimination, IncrementalInference) {
DiscreteValues m00;
m00[M(0)] = 0, m00[M(1)] = 0;
DiscreteConditional decisionTree =
- *(*discreteBayesTree)[M(1)]->conditional()->asDiscreteConditional();
+ *(*discreteBayesTree)[M(1)]->conditional()->asDiscrete();
double m00_prob = decisionTree(m00);
- auto discreteConditional = isam[M(1)]->conditional()->asDiscreteConditional();
+ auto discreteConditional = isam[M(1)]->conditional()->asDiscrete();
// Test if the probability values are as expected with regression tests.
DiscreteValues assignment;
@@ -535,7 +534,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
// The final discrete graph should not be empty since we have eliminated
// all continuous variables.
- auto discreteTree = inc[M(3)]->conditional()->asDiscreteConditional();
+ auto discreteTree = inc[M(3)]->conditional()->asDiscrete();
EXPECT_LONGS_EQUAL(3, discreteTree->size());
// Test if the optimal discrete mode assignment is (1, 1, 1).
diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index 3bdb5ed1e0..8801a8946e 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -124,8 +124,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
isam.update(graph1, initial);
HybridGaussianISAM bayesTree = isam.bayesTree();
- auto discreteConditional_m0 =
- bayesTree[M(0)]->conditional()->asDiscreteConditional();
+ auto discreteConditional_m0 = bayesTree[M(0)]->conditional()->asDiscrete();
EXPECT(discreteConditional_m0->keys() == KeyVector({M(0)}));
/********************************************************/
@@ -187,11 +186,11 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
DiscreteValues m00;
m00[M(0)] = 0, m00[M(1)] = 0;
DiscreteConditional decisionTree =
- *(*discreteBayesTree)[M(1)]->conditional()->asDiscreteConditional();
+ *(*discreteBayesTree)[M(1)]->conditional()->asDiscrete();
double m00_prob = decisionTree(m00);
auto discreteConditional =
- bayesTree[M(1)]->conditional()->asDiscreteConditional();
+ bayesTree[M(1)]->conditional()->asDiscrete();
// Test if the probability values are as expected with regression tests.
DiscreteValues assignment;
@@ -558,7 +557,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// The final discrete graph should not be empty since we have eliminated
// all continuous variables.
- auto discreteTree = bayesTree[M(3)]->conditional()->asDiscreteConditional();
+ auto discreteTree = bayesTree[M(3)]->conditional()->asDiscrete();
EXPECT_LONGS_EQUAL(3, discreteTree->size());
// Test if the optimal discrete mode assignment is (1, 1, 1).
From d5378679811d005db8f1007723f22a34c273931a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 28 Dec 2022 15:28:50 -0500
Subject: [PATCH 10/10] Relaxed test
---
gtsam/linear/tests/testGaussianBayesNet.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp
index 15f0d098a0..771a24631e 100644
--- a/gtsam/linear/tests/testGaussianBayesNet.cpp
+++ b/gtsam/linear/tests/testGaussianBayesNet.cpp
@@ -206,14 +206,14 @@ TEST(GaussianBayesNet, MonteCarloIntegration) {
gbn.push_back(noisyBayesNet.at(1));
double sum = 0.0;
- constexpr size_t N = 500;
+ constexpr size_t N = 1000;
// loop for N samples:
for (size_t i = 0; i < N; i++) {
const auto X_i = gbn.sample();
sum += pow(X_i[_y_].x() - 5.0, 2.0);
}
// Expected is variance = 3*3
- EXPECT_DOUBLES_EQUAL(9.0, sum / N, 0.1);
+ EXPECT_DOUBLES_EQUAL(9.0, sum / N, 0.5); // Pretty high.
}
/* ************************************************************************* */