From aef1669a5072670d6db8dcdc64a94299c88c8421 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 15 Sep 2022 13:24:33 -0400
Subject: [PATCH] Add labelformatter to Assignment for convenience

---
 gtsam/discrete/Assignment.h | 27 +++++++++++++++++++++++----
 1 file changed, 23 insertions(+), 4 deletions(-)

diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h
index 90e2dbdd8..c6e426ca1 100644
--- a/gtsam/discrete/Assignment.h
+++ b/gtsam/discrete/Assignment.h
@@ -11,15 +11,17 @@
 
 /**
  * @file    Assignment.h
- * @brief    An assignment from labels to a discrete value index (size_t)
+ * @brief   An assignment from labels to a discrete value index (size_t)
  * @author  Frank Dellaert
  * @date    Feb 5, 2012
  */
 
 #pragma once
 
+#include <functional>
 #include <iostream>
 #include <map>
+#include <sstream>
 #include <utility>
 #include <vector>
 
@@ -32,13 +34,30 @@ namespace gtsam {
  */
 template <class L>
 class Assignment : public std::map<L, size_t> {
+  /**
+   * @brief Default method used by `labelFormatter` or `valueFormatter` when
+   * printing.
+   *
+   * @param x The value passed to format.
+   * @return std::string
+   */
+  static std::string DefaultFormatter(const L& x) {
+    std::stringstream ss;
+    ss << x;
+    return ss.str();
+  }
+
  public:
   using std::map<L, size_t>::operator=;
 
-  void print(const std::string& s = "Assignment: ") const {
+  void print(const std::string& s = "Assignment: ",
+             const std::function<std::string(L)>& labelFormatter =
+                 &DefaultFormatter) const {
     std::cout << s << ": ";
-    for (const typename Assignment::value_type& keyValue : *this)
-      std::cout << "(" << keyValue.first << ", " << keyValue.second << ")";
+    for (const typename Assignment::value_type& keyValue : *this) {
+      std::cout << "(" << labelFormatter(keyValue.first) << ", "
+                << keyValue.second << ")";
+    }
     std::cout << std::endl;
   }