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] 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 2b125265f..b8534a02b 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 )
 {