add errorConstant method and use it for logNormalizationConstant in Conditional

gtsam/discrete/DiscreteConditional.cpp

@@ -465,6 +465,12 @@ string DiscreteConditional::html(const KeyFormatter& keyFormatter,
 double DiscreteConditional::evaluate(const HybridValues& x) const {
   return this->evaluate(x.discrete());
 }
+
+/* ************************************************************************* */
+double DiscreteConditional::errorConstant() const {
+  return 0.0;
+}
+
 /* ************************************************************************* */
 
 }  // namespace gtsam

gtsam/discrete/DiscreteConditional.h

@@ -264,11 +264,12 @@ class GTSAM_EXPORT DiscreteConditional
   }
 
   /**
-   * logNormalizationConstant K is just zero, such that
+   * errorConstant is just zero, such that
    * logProbability(x) = log(evaluate(x)) = - error(x)
    * and hence error(x) = - log(evaluate(x)) > 0 for all x.
+   * Thus -log(K) for the normalization constant k is 0.
    */
-  double logNormalizationConstant() const override { return 0.0; }
+  double errorConstant() const override;
 
   /// @}
 

gtsam/hybrid/HybridConditional.cpp

@@ -161,18 +161,18 @@ double HybridConditional::logProbability(const HybridValues &values) const {
 }
 
 /* ************************************************************************ */
-double HybridConditional::logNormalizationConstant() const {
+double HybridConditional::errorConstant() const {
   if (auto gc = asGaussian()) {
-    return gc->logNormalizationConstant();
+    return gc->errorConstant();
   }
   if (auto gm = asHybrid()) {
-    return gm->logNormalizationConstant();  // 0.0!
+    return gm->errorConstant();  // 0.0!
   }
   if (auto dc = asDiscrete()) {
-    return dc->logNormalizationConstant();  // 0.0!
+    return dc->errorConstant();  // 0.0!
   }
   throw std::runtime_error(
-      "HybridConditional::logProbability: conditional type not handled");
+      "HybridConditional::errorConstant: conditional type not handled");
 }
 
 /* ************************************************************************ */

gtsam/hybrid/HybridConditional.h

@@ -193,11 +193,12 @@ class GTSAM_EXPORT HybridConditional
   double logProbability(const HybridValues& values) const override;
 
   /**
-   * Return the log normalization constant.
+   * Return the negative log of the normalization constant.
+   * This shows up in the error as -(error(x) + errorConstant)
    * Note this is 0.0 for discrete and hybrid conditionals, but depends
    * on the continuous parameters for Gaussian conditionals.
    */
-  double logNormalizationConstant() const override;
+  double errorConstant() const override;
 
   /// Return the probability (or density) of the underlying conditional.
   double evaluate(const HybridValues& values) const override;

gtsam/hybrid/HybridGaussianConditional.cpp

@@ -36,7 +36,7 @@ HybridGaussianFactor::FactorValuePairs GetFactorValuePairs(
     // Check if conditional is pruned
     if (conditional) {
       // Assign log(\sqrt(|2πΣ|)) = -log(1 / sqrt(|2πΣ|))
-      value = conditional->logNormalizationConstant();
+      value = conditional->errorConstant();
     }
     return {std::dynamic_pointer_cast<GaussianFactor>(conditional), value};
   };
@@ -57,8 +57,8 @@ HybridGaussianConditional::HybridGaussianConditional(
   conditionals_.visit(
       [this](const GaussianConditional::shared_ptr &conditional) {
         if (conditional) {
-          this->logConstant_ = std::min(
+          this->logConstant_ =
-              this->logConstant_, conditional->logNormalizationConstant());
+              std::min(this->logConstant_, conditional->errorConstant());
         }
       });
 }
@@ -84,8 +84,7 @@ GaussianFactorGraphTree HybridGaussianConditional::asGaussianFactorGraphTree()
   auto wrap = [this](const GaussianConditional::shared_ptr &gc) {
     // First check if conditional has not been pruned
     if (gc) {
-      const double Cgm_Kgcm =
+      const double Cgm_Kgcm = gc->errorConstant() - this->logConstant_;
-          gc->logNormalizationConstant() - this->logConstant_;
       // If there is a difference in the covariances, we need to account for
       // that since the error is dependent on the mode.
       if (Cgm_Kgcm > 0.0) {
@@ -215,8 +214,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
       [&](const GaussianConditional::shared_ptr &conditional)
           -> GaussianFactorValuePair {
         const auto likelihood_m = conditional->likelihood(given);
-        const double Cgm_Kgcm =
+        const double Cgm_Kgcm = conditional->errorConstant() - logConstant_;
-            conditional->logNormalizationConstant() - logConstant_;
         if (Cgm_Kgcm == 0.0) {
           return {likelihood_m, 0.0};
         } else {

gtsam/hybrid/HybridGaussianConditional.h

@@ -150,9 +150,15 @@ class GTSAM_EXPORT HybridGaussianConditional
   /// Returns the continuous keys among the parents.
   KeyVector continuousParents() const;
 
-  /// The log normalization constant is max of the the individual
+  /**
-  /// log-normalization constants.
+   * @brief Return log normalization constant in negative log space.
-  double logNormalizationConstant() const override { return logConstant_; }
+   *
+   * The log normalization constant is the max of the individual
+   * log-normalization constants.
+   *
+   * @return double
+   */
+  inline double errorConstant() const override { return logConstant_; }
 
   /**
    * Create a likelihood factor for a hybrid Gaussian conditional,

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -329,9 +329,9 @@ static std::shared_ptr<Factor> createDiscreteFactor(
 
     // Logspace version of:
     // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
-    // logNormalizationConstant gives `-log(k)`
+    // errorConstant gives `-log(k)`
     // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
-    return -factor->error(kEmpty) + conditional->logNormalizationConstant();
+    return -factor->error(kEmpty) + conditional->errorConstant();
   };
 
   AlgebraicDecisionTree<Key> logProbabilities(
@@ -357,7 +357,7 @@ static std::shared_ptr<Factor> createHybridGaussianFactor(
       // Add 2.0 term since the constant term will be premultiplied by 0.5
       // as per the Hessian definition,
       // and negative since we want log(k)
-      hf->constantTerm() += -2.0 * conditional->logNormalizationConstant();
+      hf->constantTerm() += -2.0 * conditional->errorConstant();
     }
     return {factor, 0.0};
   };

gtsam/inference/Conditional-inst.h

@@ -59,10 +59,17 @@ double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
 
 /* ************************************************************************* */
 template <class FACTOR, class DERIVEDCONDITIONAL>
-double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
+double Conditional<FACTOR, DERIVEDCONDITIONAL>::errorConstant()
     const {
   throw std::runtime_error(
-      "Conditional::logNormalizationConstant is not implemented");
+      "Conditional::errorConstant is not implemented");
+}
+
+/* ************************************************************************* */
+template <class FACTOR, class DERIVEDCONDITIONAL>
+double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
+    const {
+  return -errorConstant();
 }
 
 /* ************************************************************************* */
@@ -89,7 +96,7 @@ bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
                    // normalization constant
   const double error = conditional.error(values);
   if (error < 0.0) return false;  // prob_or_density is negative.
-  const double expected = -(conditional.logNormalizationConstant() + error);
+  const double expected = -(conditional.errorConstant() + error);
   if (std::abs(logProb - expected) > 1e-9)
     return false;  // logProb is not consistent with error
   return true;

gtsam/inference/Conditional.h

@@ -164,9 +164,16 @@ namespace gtsam {
     }
 
     /**
-     * All conditional types need to implement a
+     * @brief All conditional types need to implement this as the negative log
-     * (negative) log normalization constant
+     * of the normalization constant.
-     * to make it such that error>=0.
+     *
+     * @return double
+     */
+    virtual double errorConstant() const;
+
+    /**
+     * All conditional types need to implement a log normalization constant to
+     * make it such that error>=0.
      */
     virtual double logNormalizationConstant() const;
 

gtsam/linear/GaussianBayesNet.cpp

@@ -247,14 +247,15 @@ namespace gtsam {
     /*
     normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
     logConstant = -log(normalizationConstant)
-      = 0.5 * n*log(2*pi) + 0.5 * log(det(Sigma))
+      = -0.5 * n*log(2*pi) - 0.5 * log(det(Sigma))
 
     log(det(Sigma)) = -2.0 * logDeterminant()
-    thus, logConstant = 0.5*n*log(2*pi) - logDeterminant()
+    thus, logConstant = -0.5*n*log(2*pi) + logDeterminant()
 
-    BayesNet logConstant = sum(0.5*n_i*log(2*pi) - logDeterminant_i())
+    BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i())
-    = sum(0.5*n_i*log(2*pi)) - sum(logDeterminant_i())
+    = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i())
-    = sum(0.5*n_i*log(2*pi)) - bn->logDeterminant()
+    = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant()
+    = sum(logNormalizationConstant_i)
     */
     double logNormConst = 0.0;
     for (const sharedConditional& cg : *this) {

gtsam/linear/GaussianConditional.cpp

@@ -182,7 +182,7 @@ namespace gtsam {
   /* ************************************************************************* */
   //  normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
   //  neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
-  double GaussianConditional::logNormalizationConstant() const {
+  double GaussianConditional::errorConstant() const {
     constexpr double log2pi = 1.8378770664093454835606594728112;
     size_t n = d().size();
     // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
@@ -195,10 +195,10 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  //  density = 1/k exp(-error(x))
+  //  density = k exp(-error(x))
-  //  log = -log(k) - error(x)
+  //  log = log(k) - error(x)
   double GaussianConditional::logProbability(const VectorValues& x) const {
-    return -logNormalizationConstant() - error(x);
+    return logNormalizationConstant() - error(x);
   }
 
   double GaussianConditional::logProbability(const HybridValues& x) const {

gtsam/linear/GaussianConditional.h

@@ -133,12 +133,14 @@ namespace gtsam {
     /// @{
 
     /**
-     * Return normalization constant in negative log space.
+     * @brief Return the negative log of the normalization constant.
      *
      * normalization constant k = 1.0 / sqrt((2*pi)^n*det(Sigma))
      * -log(k) = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
+     *
+     * @return double 
      */
-    double logNormalizationConstant() const override;
+    double errorConstant() const override;
 
     /**
      * Calculate log-probability log(evaluate(x)) for given values `x`:

gtsam/linear/NoiseModel.cpp

@@ -255,7 +255,7 @@ double Gaussian::logDeterminant() const {
 }
 
 /* *******************************************************************************/
-double Gaussian::logNormalizationConstant() const {
+double Gaussian::errorConstant() const {
   // log(det(Sigma)) = -2.0 * logDetR
   // which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDetR())
   //     = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR())
@@ -266,6 +266,10 @@ double Gaussian::logNormalizationConstant() const {
   return 0.5 * n * log2pi - logDetR();
 }
 
+/* *******************************************************************************/
+double Gaussian::logNormalizationConstant() const {
+  return -errorConstant();
+}
 
 /* ************************************************************************* */
 // Diagonal

gtsam/linear/NoiseModel.h

@@ -271,11 +271,17 @@ namespace gtsam {
       /// Compute the log of |Σ|
       double logDeterminant() const;
 
+      /**
+       * @brief Compute the negative log of the normalization constant
+       * for a Gaussian noise model k = \sqrt(1/|2πΣ|).
+       * 
+       * @return double 
+       */
+      double errorConstant() const;
+
       /**
        * @brief Method to compute the normalization constant
        * for a Gaussian noise model k = \sqrt(1/|2πΣ|).
-       * We compute this in the negative log-space for numerical accuracy,
-       * thus returning -log(k).
        *
        * @return double
        */

gtsam/linear/linear.i

@@ -548,6 +548,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
   bool equals(const gtsam::GaussianConditional& cg, double tol) const;
   
   // Standard Interface
+  double errorConstant() const;
   double logNormalizationConstant() const;
   double logProbability(const gtsam::VectorValues& x) const;
   double evaluate(const gtsam::VectorValues& x) const;