Resolve GaussianMixture error crisis

gtsam/hybrid/GaussianMixture.cpp

@@ -293,7 +293,7 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
 double GaussianMixture::error(const HybridValues &values) const {
   // Directly index to get the conditional, no need to build the whole tree.
   auto conditional = conditionals_(values.discrete());
-  return conditional->error(values.continuous());
+  return conditional->error(values.continuous()) - conditional->logNormalizationConstant();
 }
 
 /* *******************************************************************************/

gtsam/hybrid/GaussianMixture.h

@@ -176,7 +176,21 @@ class GTSAM_EXPORT GaussianMixture
   /**
    * @brief Compute the error of this Gaussian Mixture.
    *
-   * log(probability(x)) = K - error(x)
+   * This requires some care, as different mixture components may have
+   * different normalization constants. Let's consider p(x|y,m), where m is
+   * discrete. We need the error to satisfy the invariant:
+   *
+   *    error(x;y,m) = K - log(probability(x;y,m))
+   *
+   * For all x,y,m. But note that K, for the GaussianMixture, cannot depend on
+   * any arguments. Hence, we delegate to the underlying Gaussian
+   * conditionals, indexed by m, which do satisfy:
+   * 
+   *    log(probability_m(x;y)) = K_m - error_m(x;y)
+   * 
+   * We resolve by having K == 0.0 and
+   * 
+   *    error(x;y,m) = error_m(x;y) - K_m
    *
    * @param values Continuous values and discrete assignment.
    * @return double