From 51d2f077093534921d535028dd45fcc8478dc3de Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 8 Aug 2022 18:02:59 -0400
Subject: [PATCH] fix printing and key bug in MixtureFactor linearize

---
 gtsam/hybrid/GaussianMixtureFactor.cpp |  2 +-
 gtsam/hybrid/HybridFactor.cpp          | 10 ++++++----
 gtsam/hybrid/HybridNonlinearFactor.cpp |  2 +-
 gtsam/hybrid/MixtureFactor.h           | 21 ++++-----------------
 4 files changed, 12 insertions(+), 23 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 8f832d8ea..9b5be188a 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -51,7 +51,7 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  std::cout << "]{\n";
+  std::cout << "{\n";
   factors_.print(
       "", [&](Key k) { return formatter(k); },
       [&](const GaussianFactor::shared_ptr &gf) -> std::string {
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 03fe89c18..5871a1303 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -89,17 +89,19 @@ void HybridFactor::print(const std::string &s,
   if (isContinuous_) std::cout << "Continuous ";
   if (isDiscrete_) std::cout << "Discrete ";
   if (isHybrid_) std::cout << "Hybrid ";
-  for (size_t c=0; c<continuousKeys_.size(); c++) {
+  std::cout << "[";
+  for (size_t c = 0; c < continuousKeys_.size(); c++) {
     std::cout << formatter(continuousKeys_.at(c));
     if (c < continuousKeys_.size() - 1) {
       std::cout << " ";
     } else {
-      std::cout << "; ";
+      std::cout << (discreteKeys_.size() > 0 ? ";" : "");
     }
   }
-  for(auto && discreteKey: discreteKeys_) {
-    std::cout << formatter(discreteKey.first) << " ";
+  for (auto &&discreteKey : discreteKeys_) {
+    std::cout << " " << formatter(discreteKey.first);
   }
+  std::cout << "]";
 }
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 0938fd2b1..5a1833d39 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -35,7 +35,7 @@ bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
 void HybridNonlinearFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner_->print("inner: ", formatter);
+  inner_->print("\n", formatter);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h
index ba449976a..8c25457ef 100644
--- a/gtsam/hybrid/MixtureFactor.h
+++ b/gtsam/hybrid/MixtureFactor.h
@@ -131,21 +131,8 @@ class MixtureFactor : public HybridFactor {
       const std::string& s = "MixtureFactor",
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << (s.empty() ? "" : s + " ");
-    std::cout << "(";
-    auto contKeys = keys();
-    auto dKeys = discreteKeys();
-    for (DiscreteKey key : dKeys) {
-      auto it = std::find(contKeys.begin(), contKeys.end(), key.first);
-      contKeys.erase(it);
-    }
-    for (Key key : contKeys) {
-      std::cout << " " << keyFormatter(key);
-    }
-    std::cout << ";";
-    for (DiscreteKey key : dKeys) {
-      std::cout << " " << keyFormatter(key.first);
-    }
-    std::cout << " ) \n";
+    Base::print("", keyFormatter);
+    std::cout << "\nMixtureFactor\n";
     auto valueFormatter = [](const sharedFactor& v) {
       if (v) {
         return (boost::format("Nonlinear factor on %d keys") % v->size()).str();
@@ -200,8 +187,8 @@ class MixtureFactor : public HybridFactor {
     DecisionTree<Key, GaussianFactor::shared_ptr> linearized_factors(
         factors_, linearizeDT);
 
-    return boost::make_shared<GaussianMixtureFactor>(keys_, discreteKeys_,
-                                                     linearized_factors);
+    return boost::make_shared<GaussianMixtureFactor>(
+        continuousKeys_, discreteKeys_, linearized_factors);
   }
 
   /**