diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 85112d922..6657aec8c 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -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 diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index e949fb865..a34d01388 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -176,15 +176,7 @@ class GTSAM_EXPORT HybridConditional boost::shared_ptr inner() const { return inner_; } /// Return the error of the underlying conditional. - /// Currently only implemented for Gaussian mixture. - 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"); - } - } + double error(const HybridValues& values) const override; /// @}