From 693d18233a48f8961b0b669b9983d51a39c5105d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 14 Jan 2023 10:23:00 -0800
Subject: [PATCH] Adapt to continuous densities

---
 gtsam/inference/Conditional-inst.h | 11 ++++++-----
 gtsam/linear/GaussianConditional.h |  2 +-
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h
index 8445b74bd..4aa9c5126 100644
--- a/gtsam/inference/Conditional-inst.h
+++ b/gtsam/inference/Conditional-inst.h
@@ -68,12 +68,13 @@ template <class FACTOR, class DERIVEDCONDITIONAL>
 template <class VALUES>
 bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
     const DERIVEDCONDITIONAL& conditional, const VALUES& values) {
-  const double probability = conditional.evaluate(values);
-  if (probability < 0.0 || probability > 1.0)
-    return false;  // probability is not in [0,1]
+  const double prob_or_density = conditional.evaluate(values);
+  if (prob_or_density < 0.0) return false;  // prob_or_density is negative.
+  if (std::abs(prob_or_density - conditional(values)) > 1e-9)
+    return false;  // operator and evaluate differ
   const double logProb = conditional.logProbability(values);
-  if (std::abs(probability - std::exp(logProb)) > 1e-9)
-    return false;  // logProb is not consistent with probability
+  if (std::abs(prob_or_density - std::exp(logProb)) > 1e-9)
+    return false;  // logProb is not consistent with prob_or_density
   const double expected =
       conditional.logNormalizationConstant() - conditional.error(values);
   if (std::abs(logProb - expected) > 1e-9)
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 15efeae01..4611e30d0 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -34,7 +34,7 @@ namespace gtsam {
   /**
   * A GaussianConditional functions as the node in a Bayes network.
   * It has a set of parents y,z, etc. and implements a probability density on x.
-  * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
+  * The negative log-density is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
   * @ingroup linear
   */
   class GTSAM_EXPORT GaussianConditional :