fix printing and key bug in MixtureFactor linearize

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 {

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 ";
+  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) << " ";
+    std::cout << " " << formatter(discreteKey.first);
   }
+  std::cout << "]";
 }
 
 }  // namespace gtsam

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

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 << "(";
+    Base::print("", keyFormatter);
-    auto contKeys = keys();
+    std::cout << "\nMixtureFactor\n";
-    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";
     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_,
+    return boost::make_shared<GaussianMixtureFactor>(
-                                                     linearized_factors);
+        continuousKeys_, discreteKeys_, linearized_factors);
   }
 
   /**