From 364417e4aaf90a3727bbf9def41402581ed1c4e9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 29 Dec 2022 13:12:12 -0500
Subject: [PATCH] Fixed equals and print

---
 gtsam/hybrid/GaussianMixtureFactor.cpp | 42 ++++++++++++++++++--------
 1 file changed, 29 insertions(+), 13 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 881a97a1b..32ca1432c 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -35,7 +35,19 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
-  return e != nullptr && Base::equals(*e, tol);
+  if (e == nullptr) return false;
+
+  // This will return false if either factors_ is empty or e->factors_ is empty,
+  // but not if both are empty or both are not empty:
+  if (factors_.empty() ^ e->factors_.empty()) return false;
+
+  // Check the base and the factors:
+  return Base::equals(*e, tol) &&
+         factors_.equals(e->factors_,
+                         [tol](const GaussianFactor::shared_ptr &f1,
+                               const GaussianFactor::shared_ptr &f2) {
+                           return f1->equals(*f2, tol);
+                         });
 }
 
 /* *******************************************************************************/
@@ -43,18 +55,22 @@ void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   std::cout << "{\n";
-  factors_.print(
-      "", [&](Key k) { return formatter(k); },
-      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
-        RedirectCout rd;
-        std::cout << ":\n";
-        if (gf && !gf->empty()) {
-          gf->print("", formatter);
-          return rd.str();
-        } else {
-          return "nullptr";
-        }
-      });
+  if (factors_.empty()) {
+    std::cout << "  empty" << std::endl;
+  } else {
+    factors_.print(
+        "", [&](Key k) { return formatter(k); },
+        [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+          RedirectCout rd;
+          std::cout << ":\n";
+          if (gf && !gf->empty()) {
+            gf->print("", formatter);
+            return rd.str();
+          } else {
+            return "nullptr";
+          }
+        });
+  }
   std::cout << "}" << std::endl;
 }