add argmax method to TableDistribution

gtsam/discrete/TableDistribution.cpp

@@ -121,6 +121,23 @@ DiscreteConditional::shared_ptr TableDistribution::max(
   return std::make_shared<TableDistribution>(m);
 }
 
+/* ************************************************************************ */
+uint64_t TableDistribution::argmax() const {
+  uint64_t maxIdx = 0;
+  double maxValue = 0.0;
+
+  Eigen::SparseVector<double> 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);

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
   /// @{