diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index a0a51af55..f27c49180 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -24,11 +24,13 @@ #include #include #include +#include namespace gtsam { class GaussianFactorGraph; +// Needed for wrapper. using GaussianFactorVector = std::vector; /** @@ -125,6 +127,22 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { */ Sum add(const Sum &sum) const; + /** + * @brief Compute error of the GaussianMixtureFactor as a tree. + * + * @param continuousVals The continuous VectorValues. + * @return DecisionTree A decision tree with corresponding keys + * as the factor but leaf values as the error. + */ + DecisionTree error(const VectorValues &c) const { + // functor to convert from sharedFactor to double error value. + auto errorFunc = [c](const GaussianFactor::shared_ptr &factor) { + return factor->error(c); + }; + DecisionTree errorTree(factors_, errorFunc); + return errorTree; + } + /// Add MixtureFactor to a Sum, syntactic sugar. friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) { sum = factor.add(sum); diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 5e7337d0c..5a2383221 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -121,6 +121,22 @@ class MixtureFactor : public HybridFactor { ~MixtureFactor() = default; + /** + * @brief Compute error of the MixtureFactor as a tree. + * + * @param continuousVals The continuous values for which to compute the error. + * @return DecisionTree A decision tree with corresponding keys + * as the factor but leaf values as the error. + */ + DecisionTree error(const Values& continuousVals) const { + // functor to convert from sharedFactor to double error value. + auto errorFunc = [continuousVals](const sharedFactor& factor) { + return factor->error(continuousVals); + }; + DecisionTree errorTree(factors_, errorFunc); + return errorTree; + } + /** * @brief Compute error of factor given both continuous and discrete values. *