From fbe893eb2cd238f9a83afb39b08db6b459f38531 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 12 Mar 2025 17:54:14 -0400
Subject: [PATCH 1/4] more functional form of involved function

---
 gtsam/hybrid/HybridSmoother.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index 7b8c3faf1..b0fbb35c9 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -171,7 +171,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
   HybridBayesNet updatedHybridBayesNet(hybridBayesNet);
 
   KeySet involvedKeys = newFactors.keys();
-  auto involved = [&involvedKeys](const Key &key) {
+  auto involved = [](const KeySet &involvedKeys, const Key &key) {
     return involvedKeys.find(key) != involvedKeys.end();
   };
 
@@ -195,7 +195,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
       auto conditional = hybridBayesNet.at(i);
 
       for (auto &key : conditional->frontals()) {
-        if (involved(key)) {
+        if (involved(involvedKeys, key)) {
           // Add the conditional parents to involvedKeys
           // so we add those conditionals too.
           for (auto &&parentKey : conditional->parents()) {
@@ -214,7 +214,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors,
       auto conditional = hybridBayesNet.at(i);
 
       for (auto &key : conditional->frontals()) {
-        if (involved(key)) {
+        if (involved(involvedKeys, key)) {
           newConditionals.push_back(conditional);
 
           // Remove the conditional from the updated Bayes net

From 41b42ff6ef33935c24cc1d54a2e210a07eb4f60b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 12 Mar 2025 17:54:39 -0400
Subject: [PATCH 2/4] Print full factors in HybridNonlinearFactor

---
 gtsam/hybrid/HybridNonlinearFactor.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp
index 900102e5d..9b0773073 100644
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@@ -134,10 +134,12 @@ void HybridNonlinearFactor::print(const std::string& s,
   std::cout << (s.empty() ? "" : s + " ");
   Base::print("", keyFormatter);
   std::cout << "\nHybridNonlinearFactor\n";
-  auto valueFormatter = [](const std::pair<sharedFactor, double>& v) {
+  auto valueFormatter = [&keyFormatter](const std::pair<sharedFactor, double>& v) {
     auto [factor, val] = v;
     if (factor) {
-      return "Nonlinear factor on " + std::to_string(factor->size()) + " keys";
+      RedirectCout rd;
+      factor->print("", keyFormatter);
+      return rd.str();
     } else {
       return std::string("nullptr");
     }

From c0da727a14426de49a8f8f4b05685ec45841c820 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Mar 2025 12:15:48 -0400
Subject: [PATCH 3/4] fix test

---
 gtsam/hybrid/tests/testHybridNonlinearFactor.cpp | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
index e7ef9d7d9..9f8fb0638 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
@@ -81,8 +81,14 @@ TEST(HybridNonlinearFactor, Printing) {
       R"(Hybrid [x1 x2; 1]
 HybridNonlinearFactor
  Choice(1) 
- 0 Leaf Nonlinear factor on 2 keys
- 1 Leaf Nonlinear factor on 2 keys
+ 0 Leaf BetweenFactor(x1,x2)
+  measured:  0
+  noise model: diagonal sigmas [1];
+
+ 1 Leaf BetweenFactor(x1,x2)
+  measured:  1
+  noise model: diagonal sigmas [1];
+
 )";
   EXPECT(assert_print_equal(expected, hybridFactor));
 }

From fc0ae78ea53c5e378b4caf0684c26e1129da5128 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Mar 2025 13:33:41 -0400
Subject: [PATCH 4/4] set printing precision

---
 gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
index 3df03021b..09fbf8969 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -535,6 +535,9 @@ TEST(HybridNonlinearFactorGraph, Printing) {
   const auto [hybridBayesNet, remainingFactorGraph] =
       linearizedFactorGraph.eliminatePartialSequential(ordering);
 
+  // Set precision so we are consistent on all platforms
+  std::cout << std::setprecision(6);
+
 #ifdef GTSAM_DT_MERGING
   string expected_hybridFactorGraph = R"(
 size: 7