Added error for all versions - should become logDiensity?

gtsam/hybrid/HybridConditional.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridValues.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/inference/Key.h>
 
@@ -107,15 +108,30 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const {
     auto other = e->asMixture();
     return other != nullptr && gm->equals(*other, tol);
   }
-  if (auto gm = asGaussian()) {
+  if (auto gc = asGaussian()) {
     auto other = e->asGaussian();
-    return other != nullptr && gm->equals(*other, tol);
+    return other != nullptr && gc->equals(*other, tol);
   }
-  if (auto gm = asDiscrete()) {
+  if (auto dc = asDiscrete()) {
     auto other = e->asDiscrete();
-    return other != nullptr && gm->equals(*other, tol);
+    return other != nullptr && dc->equals(*other, tol);
   }
   return inner_->equals(*(e->inner_), tol);
 }
 
+/* ************************************************************************ */
+double HybridConditional::error(const HybridValues &values) const {
+  if (auto gm = asMixture()) {
+    return gm->error(values);
+  }
+  if (auto gc = asGaussian()) {
+    return gc->error(values.continuous());
+  }
+  if (auto dc = asDiscrete()) {
+    return -log((*dc)(values.discrete()));
+  }
+  throw std::runtime_error(
+      "HybridConditional::error: conditional type not handled");
+}
+
 }  // namespace gtsam

gtsam/hybrid/HybridConditional.h

@@ -176,15 +176,7 @@ class GTSAM_EXPORT HybridConditional
   boost::shared_ptr<Factor> inner() const { return inner_; }
 
   /// Return the error of the underlying conditional.
-  /// Currently only implemented for Gaussian mixture.
+  double error(const HybridValues& values) const override;
-  double error(const HybridValues& values) const override {
-    if (auto gm = asMixture()) {
-      return gm->error(values);
-    } else {
-      throw std::runtime_error(
-          "HybridConditional::error: only implemented for Gaussian mixture");
-    }
-  }
 
   /// @}