diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index fb931c5e5..746c883b2 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -81,12 +81,24 @@ void GaussianMixtureFactor::print(const std::string &s, } /* *******************************************************************************/ -const GaussianMixtureFactor::Mixture GaussianMixtureFactor::factors() const { - return Mixture(factors_, [](const FactorAndConstant &factor_z) { - return factor_z.factor; - }); +GaussianFactor::shared_ptr GaussianMixtureFactor::factor( + const DiscreteValues &assignment) const { + return factors_(assignment).factor; } +/* *******************************************************************************/ +double GaussianMixtureFactor::constant(const DiscreteValues &assignment) const { + return factors_(assignment).constant; +} + +/* *******************************************************************************/ +// NOTE(dellaert): this was not used and is expensive. +// const GaussianMixtureFactor::Mixture GaussianMixtureFactor::factors() const { +// return Mixture(factors_, [](const FactorAndConstant &factor_z) { +// return factor_z.factor; +// }); +// } + /* *******************************************************************************/ GaussianMixtureFactor::Sum GaussianMixtureFactor::add( const GaussianMixtureFactor::Sum &sum) const { diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index f0f8d060d..7e9edbe86 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -151,12 +151,16 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { void print( const std::string &s = "GaussianMixtureFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override; + /// @} /// @name Standard API /// @{ - /// Getter for the underlying Gaussian Factor Decision Tree. - const Mixture factors() const; + /// Get factor at a given discrete assignment. + sharedFactor factor(const DiscreteValues &assignment) const; + + /// Get constant at a given discrete assignment. + double constant(const DiscreteValues &assignment) const; /** * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while