negLogConstant methods for HybridBayesNet

gtsam/hybrid/HybridBayesNet.cpp

@@ -197,6 +197,35 @@ AlgebraicDecisionTree<Key> HybridBayesNet::errorTree(
   return result;
 }
 
+/* ************************************************************************* */
+double HybridBayesNet::negLogConstant() const {
+  double negLogNormConst = 0.0;
+  // Iterate over each conditional.
+  for (auto &&conditional : *this) {
+    negLogNormConst += conditional->negLogConstant();
+  }
+  return negLogNormConst;
+}
+
+/* ************************************************************************* */
+double HybridBayesNet::negLogConstant(const DiscreteValues &discrete) const {
+  double negLogNormConst = 0.0;
+  // Iterate over each conditional.
+  for (auto &&conditional : *this) {
+    if (auto gm = conditional->asHybrid()) {
+      negLogNormConst += gm->choose(discrete)->negLogConstant();
+    } else if (auto gc = conditional->asGaussian()) {
+      negLogNormConst += gc->negLogConstant();
+    } else if (auto dc = conditional->asDiscrete()) {
+      negLogNormConst += dc->choose(discrete)->negLogConstant();
+    } else {
+      throw std::runtime_error(
+          "Unknown conditional type when computing negLogConstant");
+    }
+  }
+  return negLogNormConst;
+}
+
 /* ************************************************************************* */
 AlgebraicDecisionTree<Key> HybridBayesNet::discretePosterior(
     const VectorValues &continuousValues) const {

gtsam/hybrid/HybridBayesNet.h

@@ -237,6 +237,23 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
 
   using BayesNet::logProbability;  // expose HybridValues version
 
+  /**
+   * @brief Get the negative log of the normalization constant corresponding
+   * to the joint density represented by this Bayes net.
+   *
+   * @return double
+   */
+  double negLogConstant() const;
+
+  /**
+   * @brief Get the negative log of the normalization constant
+   * corresponding to the joint Gaussian density represented by
+   * this Bayes net indexed by `discrete`.
+   *
+   * @return double
+   */
+  double negLogConstant(const DiscreteValues &discrete) const;
+
   /**
    * @brief Compute normalized posterior P(M|X=x) and return as a tree.
    *