Expose correct error versions

gtsam/discrete/DiscreteConditional.h

@@ -243,7 +243,8 @@ class GTSAM_EXPORT DiscreteConditional
     return -error(x);
   }
 
-  using DecisionTreeFactor::evaluate;
+  using DecisionTreeFactor::error;     ///< HybridValues version
+  using DecisionTreeFactor::evaluate;  ///< HybridValues version
 
   /// @}
 

gtsam/hybrid/GaussianMixture.cpp

@@ -289,6 +289,13 @@ AlgebraicDecisionTree<Key> GaussianMixture::logProbability(
   return errorTree;
 }
 
+/* *******************************************************************************/
+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());
+}
+
 /* *******************************************************************************/
 double GaussianMixture::logProbability(const HybridValues &values) const {
   // Directly index to get the conditional, no need to build the whole tree.

gtsam/hybrid/GaussianMixture.h

@@ -174,8 +174,17 @@ class GTSAM_EXPORT GaussianMixture
       const VectorValues &continuousValues) const;
 
   /**
-   * @brief Compute the logProbability of this Gaussian Mixture given the
-   * continuous values and a discrete assignment.
+   * @brief Compute the error of this Gaussian Mixture.
+   * 
+   * log(probability(x)) = K - error(x)
+   *
+   * @param values Continuous values and discrete assignment.
+   * @return double
+   */
+  double error(const HybridValues &values) const override;
+
+  /**
+   * @brief Compute the logProbability of this Gaussian Mixture.
    *
    * @param values Continuous values and discrete assignment.
    * @return double

gtsam/hybrid/HybridConditional.cpp

@@ -121,6 +121,21 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const {
                 : !(e->inner_);
 }
 
+/* ************************************************************************ */
+double HybridConditional::error(const HybridValues &values) const {
+  if (auto gc = asGaussian()) {
+    return gc->error(values.continuous());
+  }
+  if (auto gm = asMixture()) {
+    return gm->error(values);
+  }
+  if (auto dc = asDiscrete()) {
+    return dc->error(values.discrete());
+  }
+  throw std::runtime_error(
+      "HybridConditional::error: conditional type not handled");
+}
+
 /* ************************************************************************ */
 double HybridConditional::logProbability(const HybridValues &values) const {
   if (auto gc = asGaussian()) {

gtsam/hybrid/HybridConditional.h

@@ -176,6 +176,9 @@ class GTSAM_EXPORT HybridConditional
   /// Get the type-erased pointer to the inner type
   boost::shared_ptr<Factor> inner() const { return inner_; }
 
+  /// Return the error of the underlying conditional.
+  double error(const HybridValues& values) const override;
+
   /// Return the logProbability of the underlying conditional.
   double logProbability(const HybridValues& values) const override;
 

gtsam/linear/GaussianConditional.h

@@ -264,7 +264,7 @@ namespace gtsam {
 
     using Conditional::evaluate; // Expose evaluate(const HybridValues&) method..
     using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
-    using Base::error; // Expose error(const HybridValues&) method..
+    using JacobianFactor::error; // Expose error(const HybridValues&) method..
 
     /// @}
 

gtsam/linear/HessianFactor.h

@@ -196,6 +196,9 @@ namespace gtsam {
     /** Compare to another factor for testing (implementing Testable) */
     bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
 
+    /// HybridValues simply extracts the \class VectorValues and calls error.
+    using GaussianFactor::error;
+
     /** 
      * Evaluate the factor error f(x). 
      * returns 0.5*[x -1]'*H*[x -1] (also see constructor documentation)

gtsam/linear/JacobianFactor.h

@@ -198,7 +198,12 @@ namespace gtsam {
 
     Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
     Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
-    double error(const VectorValues& c) const override; /**  0.5*(A*x-b)'*D*(A*x-b) */
+
+    /// HybridValues simply extracts the \class VectorValues and calls error.
+    using GaussianFactor::error;
+
+    //// 0.5*(A*x-b)'*D*(A*x-b).
+    double error(const VectorValues& c) const override; 
 
     /** Return the augmented information matrix represented by this GaussianFactor.
      * The augmented information matrix contains the information matrix with an