From 5566ff64ebe36222ebadd72e66561cbf0cc0ce18 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:04:25 -0500
Subject: [PATCH 01/15] cpp file for utilities

---
 gtsam/base/utilities.cpp | 13 +++++++++++++
 gtsam/base/utilities.h   | 12 ++++++------
 2 files changed, 19 insertions(+), 6 deletions(-)
 create mode 100644 gtsam/base/utilities.cpp

diff --git a/gtsam/base/utilities.cpp b/gtsam/base/utilities.cpp
new file mode 100644
index 0000000000..189156c910
--- /dev/null
+++ b/gtsam/base/utilities.cpp
@@ -0,0 +1,13 @@
+#include <gtsam/base/utilities.h>
+
+namespace gtsam {
+
+std::string RedirectCout::str() const {
+  return ssBuffer_.str();
+}
+
+RedirectCout::~RedirectCout() {
+  std::cout.rdbuf(coutBuffer_);
+}
+
+}
diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h
index 8eb5617a8e..d9b92b8aa3 100644
--- a/gtsam/base/utilities.h
+++ b/gtsam/base/utilities.h
@@ -1,5 +1,9 @@
 #pragma once
 
+#include <string>
+#include <iostream>
+#include <sstream>
+
 namespace gtsam {
 /**
  * For Python __str__().
@@ -12,14 +16,10 @@ struct RedirectCout {
   RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
 
   /// return the string
-  std::string str() const {
-    return ssBuffer_.str();
-  }
+  std::string str() const;
 
   /// destructor -- redirect stdout buffer to its original buffer
-  ~RedirectCout() {
-    std::cout.rdbuf(coutBuffer_);
-  }
+  ~RedirectCout();
 
 private:
   std::stringstream ssBuffer_;

From 26890652c437140d4a2ce928cf2768546080eb4d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:04:40 -0500
Subject: [PATCH 02/15] Default constructor

---
 gtsam/discrete/DiscreteMarginals.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h
index 27352a2110..a2207a10b0 100644
--- a/gtsam/discrete/DiscreteMarginals.h
+++ b/gtsam/discrete/DiscreteMarginals.h
@@ -37,6 +37,8 @@ class GTSAM_EXPORT DiscreteMarginals {
 
   public:
 
+  DiscreteMarginals() {}
+
   /** Construct a marginals class.
    * @param graph The factor graph defining the full joint density on all variables.
    */

From 4cba05a2f7e0110e4f1996bd7ef808c6c76de9a4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:05:48 -0500
Subject: [PATCH 03/15] New constructor

---
 gtsam/discrete/DiscreteKey.cpp | 8 +++-----
 gtsam/discrete/DiscreteKey.h   | 9 +++++++--
 2 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp
index 5ddad22b04..0857f8c1eb 100644
--- a/gtsam/discrete/DiscreteKey.cpp
+++ b/gtsam/discrete/DiscreteKey.cpp
@@ -38,11 +38,9 @@ namespace gtsam {
     return js;
   }
 
-  map<Key,size_t> DiscreteKeys::cardinalities() const {
-    map<Key,size_t> cs;
-    cs.insert(begin(),end());
-//    for(const DiscreteKey& key: *this)
-//      cs.insert(key);
+  map<Key, size_t> DiscreteKeys::cardinalities() const {
+    map<Key, size_t> cs;
+    cs.insert(begin(), end());
     return cs;
   }
 
diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h
index ae4dac38fc..ce0c56dbeb 100644
--- a/gtsam/discrete/DiscreteKey.h
+++ b/gtsam/discrete/DiscreteKey.h
@@ -28,8 +28,8 @@
 namespace gtsam {
 
   /**
-   * Key type for discrete conditionals
-   * Includes name and cardinality
+   * Key type for discrete variables.
+   * Includes Key and cardinality.
    */
   using DiscreteKey = std::pair<Key,size_t>;
 
@@ -45,6 +45,11 @@ namespace gtsam {
     /// Construct from a key
     explicit DiscreteKeys(const DiscreteKey& key) { push_back(key); }
 
+    /// Construct from cardinalities.
+    explicit DiscreteKeys(std::map<Key, size_t> cardinalities) {
+      for (auto&& kv : cardinalities) emplace_back(kv);
+    }
+
     /// Construct from a vector of keys
     DiscreteKeys(const std::vector<DiscreteKey>& keys) :
       std::vector<DiscreteKey>(keys) {

From 785b39d3c04a97944457debc734a71efc254dfa8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:06:06 -0500
Subject: [PATCH 04/15] discreteKeys method

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 18 ++++++++++++++++--
 gtsam/discrete/DiscreteFactorGraph.h   |  3 +++
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index c1248c60b9..cb7c165959 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -43,11 +43,25 @@ namespace gtsam {
   /* ************************************************************************* */
   KeySet DiscreteFactorGraph::keys() const {
     KeySet keys;
-    for(const sharedFactor& factor: *this)
-    if (factor) keys.insert(factor->begin(), factor->end());
+    for (const sharedFactor& factor : *this) {
+      if (factor) keys.insert(factor->begin(), factor->end());
+    }
     return keys;
   }
 
+  /* ************************************************************************* */
+  DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
+    DiscreteKeys result;
+    for (auto&& factor : *this) {
+      if (auto p = boost::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
+        DiscreteKeys factor_keys = p->discreteKeys();
+        result.insert(result.end(), factor_keys.begin(), factor_keys.end());
+      }
+    }
+
+    return result;
+  }
+
   /* ************************************************************************* */
   DecisionTreeFactor DiscreteFactorGraph::product() const {
     DecisionTreeFactor result;
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index 1da840eb8e..baa0213d55 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -114,6 +114,9 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /** Return the set of variables involved in the factors (set union) */
   KeySet keys() const;
 
+  /// Return the DiscreteKeys in this factor graph.
+  DiscreteKeys discreteKeys() const;
+
   /** return product of all factors as a single factor */
   DecisionTreeFactor product() const;
 

From b875914f93aa7e017ceaa8bac7a628299de239dc Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:06:33 -0500
Subject: [PATCH 05/15] expNormalize, from Kevin Doherty

---
 gtsam/discrete/DiscreteFactor.cpp | 47 +++++++++++++++++++++++++++++++
 gtsam/discrete/DiscreteFactor.h   | 20 +++++++++++++
 2 files changed, 67 insertions(+)

diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp
index 0cf7f2a5e8..08309e2e17 100644
--- a/gtsam/discrete/DiscreteFactor.cpp
+++ b/gtsam/discrete/DiscreteFactor.cpp
@@ -17,12 +17,59 @@
  * @author Frank Dellaert
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 
+#include <cmath>
 #include <sstream>
 
 using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+std::vector<double> expNormalize(const std::vector<double>& logProbs) {
+  double maxLogProb = -std::numeric_limits<double>::infinity();
+  for (size_t i = 0; i < logProbs.size(); i++) {
+    double logProb = logProbs[i];
+    if ((logProb != std::numeric_limits<double>::infinity()) &&
+        logProb > maxLogProb) {
+      maxLogProb = logProb;
+    }
+  }
+
+  // After computing the max = "Z" of the log probabilities L_i, we compute
+  // the log of the normalizing constant, log S, where S = sum_j exp(L_j - Z).
+  double total = 0.0;
+  for (size_t i = 0; i < logProbs.size(); i++) {
+    double probPrime = exp(logProbs[i] - maxLogProb);
+    total += probPrime;
+  }
+  double logTotal = log(total);
+
+  // Now we compute the (normalized) probability (for each i):
+  // p_i = exp(L_i - Z - log S)
+  double checkNormalization = 0.0;
+  std::vector<double> probs;
+  for (size_t i = 0; i < logProbs.size(); i++) {
+    double prob = exp(logProbs[i] - maxLogProb - logTotal);
+    probs.push_back(prob);
+    checkNormalization += prob;
+  }
+
+  // Numerical tolerance for floating point comparisons
+  double tol = 1e-9;
+
+  if (!gtsam::fpEqual(checkNormalization, 1.0, tol)) {
+    std::string errMsg =
+        std::string("expNormalize failed to normalize probabilities. ") +
+        std::string("Expected normalization constant = 1.0. Got value: ") +
+        std::to_string(checkNormalization) +
+        std::string(
+            "\n This could have resulted from numerical overflow/underflow.");
+    throw std::logic_error(errMsg);
+  }
+  return probs;
+}
+
 }  // namespace gtsam
diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h
index 8f39fbc23f..212ade8cfb 100644
--- a/gtsam/discrete/DiscreteFactor.h
+++ b/gtsam/discrete/DiscreteFactor.h
@@ -122,4 +122,24 @@ class GTSAM_EXPORT DiscreteFactor: public Factor {
 // traits
 template<> struct traits<DiscreteFactor> : public Testable<DiscreteFactor> {};
 
+
+/**
+ * @brief Normalize a set of log probabilities.
+ *
+ * Normalizing a set of log probabilities in a numerically stable way is
+ * tricky. To avoid overflow/underflow issues, we compute the largest
+ * (finite) log probability and subtract it from each log probability before
+ * normalizing. This comes from the observation that if:
+ *    p_i = exp(L_i) / ( sum_j exp(L_j) ),
+ * Then,
+ *    p_i = exp(Z) exp(L_i - Z) / (exp(Z) sum_j exp(L_j - Z)),
+ *        = exp(L_i - Z) / ( sum_j exp(L_j - Z) )
+ *
+ * Setting Z = max_j L_j, we can avoid numerical issues that arise when all
+ * of the (unnormalized) log probabilities are either very large or very
+ * small.
+ */
+std::vector<double> expNormalize(const std::vector<double> &logProbs);
+
+
 }// namespace gtsam

From eb4309d26431b6edf287e4ec32dbbb8ea219c3be Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:06:52 -0500
Subject: [PATCH 06/15] discreteKeys method

---
 gtsam/discrete/DecisionTreeFactor.cpp | 14 +++++++++++++-
 gtsam/discrete/DecisionTreeFactor.h   |  3 +++
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp
index ad4cbad434..7bd9e9b7fe 100644
--- a/gtsam/discrete/DecisionTreeFactor.cpp
+++ b/gtsam/discrete/DecisionTreeFactor.cpp
@@ -67,7 +67,7 @@ namespace gtsam {
   void DecisionTreeFactor::print(const string& s,
       const KeyFormatter& formatter) const {
     cout << s;
-    ADT::print("Potentials:",formatter);
+    ADT::print("", formatter);
   }
 
   /* ************************************************************************* */
@@ -163,6 +163,18 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  DiscreteKeys DecisionTreeFactor::discreteKeys() const {
+    DiscreteKeys result;
+    for (auto&& key : keys()) {
+      DiscreteKey dkey(key, cardinality(key));
+      if (std::find(result.begin(), result.end(), dkey) == result.end()) {
+        result.push_back(dkey);
+      }
+    }
+    return result;
+  }
+
   /* ************************************************************************* */
   static std::string valueFormatter(const double& v) {
     return (boost::format("%4.2g") % v).str();
diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h
index 8beeb4c4a0..0bfdf6b902 100644
--- a/gtsam/discrete/DecisionTreeFactor.h
+++ b/gtsam/discrete/DecisionTreeFactor.h
@@ -183,6 +183,9 @@ namespace gtsam {
     /// Enumerate all values into a map from values to double.
     std::vector<std::pair<DiscreteValues, double>> enumerate() const;
 
+    /// Return all the discrete keys associated with this factor.
+    DiscreteKeys discreteKeys() const;
+
     /// @}
     /// @name Wrapper support
     /// @{

From deae4499a1094fefb43222dba1762f79c1ba1a0a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:07:09 -0500
Subject: [PATCH 07/15] remove export, typo

---
 gtsam/discrete/DecisionTree-inl.h | 6 +++++-
 gtsam/discrete/DecisionTree.h     | 9 ++++++++-
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index ab14b2a726..84116ccd5f 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -604,7 +604,7 @@ namespace gtsam {
     using MXChoice = typename DecisionTree<M, X>::Choice;
     auto choice = boost::dynamic_pointer_cast<const MXChoice>(f);
     if (!choice) throw std::invalid_argument(
-        "DecisionTree::Convert: Invalid NodePtr");
+        "DecisionTree::convertFrom: Invalid NodePtr");
 
     // get new label
     const M oldLabel = choice->label();
@@ -634,6 +634,8 @@ namespace gtsam {
 
       using Choice = typename DecisionTree<L, Y>::Choice;
       auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      if (!choice)
+        throw std::invalid_argument("DecisionTree::Visit: Invalid NodePtr");
       for (auto&& branch : choice->branches()) (*this)(branch);  // recurse!
     }
   };
@@ -663,6 +665,8 @@ namespace gtsam {
 
       using Choice = typename DecisionTree<L, Y>::Choice;
       auto choice = boost::dynamic_pointer_cast<const Choice>(node);
+      if (!choice)
+        throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
       for (size_t i = 0; i < choice->nrChoices(); i++) {
         choices[choice->label()] = i;    // Set assignment for label to i
         (*this)(choice->branches()[i]);  // recurse!
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 9692094e19..78f3a75b72 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -38,7 +38,7 @@ namespace gtsam {
    * Y = function range (any algebra), e.g., bool, int, double
    */
   template<typename L, typename Y>
-  class GTSAM_EXPORT DecisionTree {
+  class DecisionTree {
 
    protected:
     /// Default method for comparison of two objects of type Y.
@@ -340,4 +340,11 @@ namespace gtsam {
     return f.apply(g, op);
   }
 
+  /// unzip a DecisionTree if its leaves are `std::pair`
+  template<typename L, typename T1, typename T2>
+  std::pair<DecisionTree<L, T1>, DecisionTree<L, T2> > unzip(const DecisionTree<L, std::pair<T1, T2> > &input) {
+    return std::make_pair(DecisionTree<L, T1>(input, [](std::pair<T1, T2> i) { return i.first; }),
+                          DecisionTree<L, T2>(input, [](std::pair<T1, T2> i) { return i.second; }));
+  }
+
 } // namespace gtsam

From 51352bf3f5fe3d35b2e6e7038bf0f00d12b709e5 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:07:26 -0500
Subject: [PATCH 08/15] more precise printing

---
 gtsam/discrete/AlgebraicDecisionTree.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index d2e05927a3..566357a485 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -163,7 +163,7 @@ namespace gtsam {
                const typename Base::LabelFormatter& labelFormatter =
                    &DefaultFormatter) const {
       auto valueFormatter = [](const double& v) {
-        return (boost::format("%4.2g") % v).str();
+        return (boost::format("%4.4g") % v).str();
       };
       Base::print(s, labelFormatter, valueFormatter);
     }

From 59d1a0601602f201f3a94be2e293c0565a0afeee Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:07:33 -0500
Subject: [PATCH 09/15] typo

---
 gtsam/discrete/tests/testDiscreteDistribution.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/discrete/tests/testDiscreteDistribution.cpp b/gtsam/discrete/tests/testDiscreteDistribution.cpp
index 5c0c42e737..19bf7c3770 100644
--- a/gtsam/discrete/tests/testDiscreteDistribution.cpp
+++ b/gtsam/discrete/tests/testDiscreteDistribution.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- * @file    testDiscretePrior.cpp
+ * @file    testDiscreteDistribution.cpp
  * @brief   unit tests for DiscreteDistribution
  * @author  Frank dellaert
  * @date    December 2021

From a281a6522bfccb3f08b06bba31d7de84b91ade6e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:07:44 -0500
Subject: [PATCH 10/15] test new unzip method

---
 gtsam/discrete/tests/testDecisionTree.cpp | 25 +++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp
index 2e6ec59f72..1029417764 100644
--- a/gtsam/discrete/tests/testDecisionTree.cpp
+++ b/gtsam/discrete/tests/testDecisionTree.cpp
@@ -375,6 +375,31 @@ TEST(DecisionTree, labels) {
   EXPECT_LONGS_EQUAL(2, labels.size());
 }
 
+/* ******************************************************************************** */
+// Test retrieving all labels.
+TEST(DecisionTree, unzip) {
+  using DTP = DecisionTree<string, std::pair<int, string>>;
+  using DT1 = DecisionTree<string, int>;
+  using DT2 = DecisionTree<string, string>;
+
+  // Create small two-level tree
+  string A("A"), B("B"), C("C");
+  DTP tree(B,
+           DTP(A, {0, "zero"}, {1, "one"}),
+           DTP(A, {2, "two"}, {1337, "l33t"})
+  );
+
+  DT1 dt1;
+  DT2 dt2;
+  std::tie(dt1, dt2) = unzip(tree);
+  
+  DT1 tree1(B, DT1(A, 0, 1), DT1(A, 2, 1337));
+  DT2 tree2(B, DT2(A, "zero", "one"), DT2(A, "two", "l33t"));
+
+  EXPECT(tree1.equals(dt1));
+  EXPECT(tree2.equals(dt2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From f2518d11f0c32880094f627bd7754fb992341bc8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:08:06 -0500
Subject: [PATCH 11/15] Change template type

---
 gtsam/inference/MetisIndex-inl.h | 4 ++--
 gtsam/inference/MetisIndex.h     | 8 ++++----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h
index eb9670254f..6465233728 100644
--- a/gtsam/inference/MetisIndex-inl.h
+++ b/gtsam/inference/MetisIndex-inl.h
@@ -23,8 +23,8 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-template<class FACTOR>
-void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
+template<class FACTORGRAPH>
+void MetisIndex::augment(const FACTORGRAPH& factors) {
   std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with  adjMap.first
   std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
   std::set<Key> keySet;
diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h
index 7ec435caa1..7431bff4c1 100644
--- a/gtsam/inference/MetisIndex.h
+++ b/gtsam/inference/MetisIndex.h
@@ -62,8 +62,8 @@ class GTSAM_EXPORT MetisIndex {
       nKeys_(0) {
   }
 
-  template<class FG>
-  MetisIndex(const FG& factorGraph) :
+  template<class FACTORGRAPH>
+  MetisIndex(const FACTORGRAPH& factorGraph) :
       nKeys_(0) {
     augment(factorGraph);
   }
@@ -78,8 +78,8 @@ class GTSAM_EXPORT MetisIndex {
    * Augment the variable index with new factors.  This can be used when
    * solving problems incrementally.
    */
-  template<class FACTOR>
-  void augment(const FactorGraph<FACTOR>& factors);
+  template<class FACTORGRAPH>
+  void augment(const FACTORGRAPH& factors);
 
   const std::vector<int32_t>& xadj() const {
     return xadj_;

From 823239090d1fbfb7fedc362217dcb916ce786edd Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:08:17 -0500
Subject: [PATCH 12/15] exact equality

---
 gtsam/inference/FactorGraph.h | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 9c0f10f9a5..afea63da84 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -128,6 +128,11 @@ class FactorGraph {
   /** Collection of factors */
   FastVector<sharedFactor> factors_;
 
+  /// Check exact equality of the factor pointers. Useful for derived ==.
+  bool isEqual(const FactorGraph& other) const {
+    return factors_ == other.factors_;
+  }
+
   /// @name Standard Constructors
   /// @{
 
@@ -290,11 +295,11 @@ class FactorGraph {
   /// @name Testable
   /// @{
 
-  /// print out graph
+  /// Print out graph to std::cout, with optional key formatter.
   virtual void print(const std::string& s = "FactorGraph",
                      const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
-  /** Check equality */
+  /// Check equality up to tolerance.
   bool equals(const This& fg, double tol = 1e-9) const;
   /// @}
 

From f2a65a0531edd0e6af9c63677f2aea56ce7a55a4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:08:34 -0500
Subject: [PATCH 13/15] expose advanced interface

---
 gtsam/inference/Factor.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h
index e6a8dcc604..27b85ef67c 100644
--- a/gtsam/inference/Factor.h
+++ b/gtsam/inference/Factor.h
@@ -158,7 +158,6 @@ typedef FastSet<FactorIndex> FactorIndexSet;
 
     /// @}
 
-  public:
     /// @name Advanced Interface
     /// @{
 

From 600f05ae2c4dc1ec2b32282918ec014204f41e75 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 22 Jan 2022 11:08:46 -0500
Subject: [PATCH 14/15] exact equality

---
 gtsam/linear/GaussianFactorGraph.h | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index f392221222..0d5057aa88 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -99,6 +99,12 @@ namespace gtsam {
 
     /// @}
 
+    /// Check exact equality.
+    friend bool operator==(const GaussianFactorGraph& lhs,
+                            const GaussianFactorGraph& rhs) {
+      return lhs.isEqual(rhs);
+    }
+
     /** Add a factor by value - makes a copy */
     void add(const GaussianFactor& factor) { push_back(factor.clone()); }
 
@@ -414,7 +420,7 @@ namespace gtsam {
    */
   GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
 
-  /****** Linear Algebra Opeations ******/
+  /****** Linear Algebra Operations ******/
 
   ///* matrix-vector operations */
   //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);

From 465976211925f9e239f98ac3d4eda7d7a20119cf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 22 Jan 2022 10:17:12 -0500
Subject: [PATCH 15/15] Update python install in CI to use pip

---
 .github/scripts/python.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh
index 6cc62d2b06..0855dbc21b 100644
--- a/.github/scripts/python.sh
+++ b/.github/scripts/python.sh
@@ -83,6 +83,6 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \
 make -j2 install
 
 cd $GITHUB_WORKSPACE/build/python
-$PYTHON setup.py install --user --prefix=
+$PYTHON -m pip install --user .
 cd $GITHUB_WORKSPACE/python/gtsam/tests
 $PYTHON -m unittest discover -v