diff --git a/gtsam/discrete/TableDistribution.cpp b/gtsam/discrete/TableDistribution.cpp index bedcee42c..2e476b0ee 100644 --- a/gtsam/discrete/TableDistribution.cpp +++ b/gtsam/discrete/TableDistribution.cpp @@ -121,6 +121,23 @@ DiscreteConditional::shared_ptr TableDistribution::max( return std::make_shared(m); } +/* ************************************************************************ */ +uint64_t TableDistribution::argmax() const { + uint64_t maxIdx = 0; + double maxValue = 0.0; + + Eigen::SparseVector sparseTable = table_.sparseTable(); + + for (SparseIt it(sparseTable); it; ++it) { + if (it.value() > maxValue) { + maxIdx = it.index(); + maxValue = it.value(); + } + } + + return maxIdx; +} + /* ****************************************************************************/ void TableDistribution::prune(size_t maxNrAssignments) { table_ = table_.prune(maxNrAssignments); diff --git a/gtsam/discrete/TableDistribution.h b/gtsam/discrete/TableDistribution.h index ce41835d6..9cbca0d26 100644 --- a/gtsam/discrete/TableDistribution.h +++ b/gtsam/discrete/TableDistribution.h @@ -141,6 +141,13 @@ class GTSAM_EXPORT TableDistribution : public DiscreteConditional { virtual DiscreteConditional::shared_ptr max( const Ordering& keys) const override; + /** + * @brief Return assignment that maximizes value. + * + * @return maximizing assignment for the variables. + */ + uint64_t argmax() const; + /// @} /// @name Advanced Interface /// @{