From b4c70f2ef94a1a19de68e9f8a1edc13da66a2210 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 3 Sep 2022 22:39:10 -0400
Subject: [PATCH 01/23] add code for simplified hybrid estimation

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp  |  59 ++++-
 gtsam/hybrid/tests/testHybridEstimation.cpp | 226 ++++++++++++++++++++
 2 files changed, 281 insertions(+), 4 deletions(-)
 create mode 100644 gtsam/hybrid/tests/testHybridEstimation.cpp

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index c031b9729e..9dbbb0468b 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -55,6 +55,52 @@ namespace gtsam {
 
 template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
 
+std::string GTDKeyFormatter(const Key &key) {
+  constexpr size_t kMax_uchar_ = std::numeric_limits<uint8_t>::max();
+  constexpr size_t key_bits = sizeof(gtsam::Key) * 8;
+  constexpr size_t ch1_bits = sizeof(uint8_t) * 8;
+  constexpr size_t ch2_bits = sizeof(uint8_t) * 8;
+  constexpr size_t link_bits = sizeof(uint8_t) * 8;
+  constexpr size_t joint_bits = sizeof(uint8_t) * 8;
+  constexpr size_t time_bits =
+      key_bits - ch1_bits - ch2_bits - link_bits - joint_bits;
+
+  constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_)
+                                  << (key_bits - ch1_bits);
+  constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_)
+                                  << (key_bits - ch1_bits - ch2_bits);
+  constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_)
+                                   << (time_bits + joint_bits);
+  constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits;
+  constexpr gtsam::Key time_mask =
+      ~(ch1_mask | ch2_mask | link_mask | joint_mask);
+
+  uint8_t c1_, c2_, link_idx_, joint_idx_;
+  uint64_t t_;
+
+  c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits));
+  c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits));
+  link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits));
+  joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits);
+  t_ = key & time_mask;
+
+  std::string s = "";
+  if (c1_ != 0) {
+    s += c1_;
+  }
+  if (c2_ != 0) {
+    s += c2_;
+  }
+  if (link_idx_ != kMax_uchar_) {
+    s += "[" + std::to_string((int)(link_idx_)) + "]";
+  }
+  if (joint_idx_ != kMax_uchar_) {
+    s += "(" + std::to_string((int)(joint_idx_)) + ")";
+  }
+  s += std::to_string(t_);
+  return s;
+}
+
 /* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
@@ -213,10 +259,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
         result = EliminatePreferCholesky(graph, frontalKeys);
 
     if (keysOfEliminated.empty()) {
-      keysOfEliminated =
-          result.first->keys();  // Initialize the keysOfEliminated to be the
+      // Initialize the keysOfEliminated to be the keys of the
+      // eliminated GaussianConditional
+      keysOfEliminated = result.first->keys();
     }
-    // keysOfEliminated of the GaussianConditional
     if (keysOfSeparator.empty()) {
       keysOfSeparator = result.second->keys();
     }
@@ -237,13 +283,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
+  // frontalKeys.print();
+  // separatorFactors.print("", GTDKeyFormatter, [](const GaussianFactor::shared_ptr &factor) { factor->print(); return "";});
+  // std::cout << "=====================================" << std::endl;
   if (keysOfSeparator.empty()) {
-    VectorValues empty_values;
+    VectorValues empty_values;  //TODO(Varun) Shouldn't this be from the optimized leaf?
     auto factorError = [&](const GaussianFactor::shared_ptr &factor) {
       if (!factor) return 0.0;  // TODO(fan): does this make sense?
       return exp(-factor->error(empty_values));
     };
     DecisionTree<Key, double> fdt(separatorFactors, factorError);
+    std::cout << "\n\nFactor Decision Tree" << std::endl;
+    fdt.print("", GTDKeyFormatter, [](double x) { return std::to_string(x); });
     auto discreteFactor =
         boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
 
diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
new file mode 100644
index 0000000000..1aa3060ce5
--- /dev/null
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -0,0 +1,226 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testHybridEstimation.cpp
+ * @brief   Unit tests for Hybrid Estimation
+ * @author  Varun Agrawal
+ */
+
+#include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
+#include <gtsam/hybrid/MixtureFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+// Include for test suite
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+using noiseModel::Isotropic;
+using symbol_shorthand::M;
+using symbol_shorthand::X;
+
+class Robot {
+  size_t K_;
+  DiscreteKeys modes_;
+  HybridNonlinearFactorGraph nonlinearFactorGraph_;
+  HybridGaussianFactorGraph linearizedFactorGraph_;
+  Values linearizationPoint_;
+
+ public:
+  Robot(size_t K, std::vector<double> measurements) {
+    // Create DiscreteKeys for binary K modes
+    for (size_t k = 0; k < K; k++) {
+      modes_.emplace_back(M(k), 2);
+    }
+
+    ////// Create hybrid factor graph.
+    // Add measurement factors
+    auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0);
+    for (size_t k = 0; k < K; k++) {
+      nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<double>>(
+          X(k), measurements.at(k), measurement_noise);
+    }
+
+    // 2 noise models where moving has a higher covariance.
+    auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2);
+    auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2);
+
+    // Add "motion models".
+    // The idea is that the robot has a higher "freedom" (aka higher covariance)
+    // for movement
+    using MotionModel = BetweenFactor<double>;
+
+    for (size_t k = 1; k < K; k++) {
+      KeyVector keys = {X(k - 1), X(k)};
+      DiscreteKeys dkeys{modes_[k - 1]};
+      auto still = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
+                                                   still_noise_model),
+           moving = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
+                                                    moving_noise_model);
+      std::vector<boost::shared_ptr<MotionModel>> components = {still, moving};
+      nonlinearFactorGraph_.emplace_hybrid<MixtureFactor>(keys, dkeys,
+                                                          components);
+    }
+
+    // Create the linearization point.
+    for (size_t k = 0; k < K; k++) {
+      linearizationPoint_.insert<double>(X(k), static_cast<double>(k + 1));
+    }
+
+    linearizedFactorGraph_ =
+        nonlinearFactorGraph_.linearize(linearizationPoint_);
+  }
+
+  void print() const {
+    nonlinearFactorGraph_.print();
+    linearizationPoint_.print();
+    linearizedFactorGraph_.print();
+  }
+
+  HybridValues optimize() const {
+    Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering();
+    HybridBayesNet::shared_ptr hybridBayesNet =
+        linearizedFactorGraph_.eliminateSequential(hybridOrdering);
+
+    HybridValues delta = hybridBayesNet->optimize();
+    return delta;
+  }
+};
+
+/* ****************************************************************************/
+/**
+ * I am trying to test if setting the hybrid mixture components to just differ
+ * in covariance makes sense. This is done by setting the "moving" covariance to
+ * be 1e2 while the "still" covariance is 1e-2.
+ */
+TEST(Estimation, StillRobot) {
+  size_t K = 2;
+  vector<double> measurements = {0, 0};
+
+  Robot robot(K, measurements);
+  // robot.print();
+
+  HybridValues delta = robot.optimize();
+
+  delta.continuous().print("delta update:");
+
+  if (delta.discrete()[M(0)] == 0) {
+    std::cout << "The robot is stationary!" << std::endl;
+  } else {
+    std::cout << "The robot has moved!" << std::endl;
+  }
+  EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]);
+}
+
+TEST(Estimation, MovingRobot) {
+  size_t K = 2;
+  vector<double> measurements = {0, 2};
+
+  Robot robot(K, measurements);
+  // robot.print();
+
+  HybridValues delta = robot.optimize();
+
+  delta.continuous().print("delta update:");
+
+  if (delta.discrete()[M(0)] == 0) {
+    std::cout << "The robot is stationary!" << std::endl;
+  } else {
+    std::cout << "The robot has moved!" << std::endl;
+  }
+  EXPECT_LONGS_EQUAL(1, delta.discrete()[M(0)]);
+}
+
+/// A robot with a single leg.
+class SingleLeg {
+  size_t K_;
+  DiscreteKeys modes_;
+  HybridNonlinearFactorGraph nonlinearFactorGraph_;
+  HybridGaussianFactorGraph linearizedFactorGraph_;
+  Values linearizationPoint_;
+
+ public:
+  SingleLeg(size_t K, std::vector<double> measurements) {
+    // Create DiscreteKeys for binary K modes
+    for (size_t k = 0; k < K; k++) {
+      modes_.emplace_back(M(k), 2);
+    }
+
+    ////// Create hybrid factor graph.
+    // Add measurement factors
+    auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0);
+    for (size_t k = 0; k < K; k++) {
+      nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<double>>(
+          X(k), measurements.at(k), measurement_noise);
+    }
+
+    // 2 noise models where moving has a higher covariance.
+    auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2);
+    auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2);
+
+    // Add "motion models".
+    // The idea is that the robot has a higher "freedom" (aka higher covariance)
+    // for movement
+    using MotionModel = BetweenFactor<double>;
+
+    for (size_t k = 1; k < K; k++) {
+      KeyVector keys = {X(k - 1), X(k)};
+      DiscreteKeys dkeys{modes_[k - 1]};
+      auto still = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
+                                                   still_noise_model),
+           moving = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
+                                                    moving_noise_model);
+      std::vector<boost::shared_ptr<MotionModel>> components = {still, moving};
+      nonlinearFactorGraph_.emplace_hybrid<MixtureFactor>(keys, dkeys,
+                                                          components);
+    }
+
+    // Create the linearization point.
+    for (size_t k = 0; k < K; k++) {
+      linearizationPoint_.insert<double>(X(k), static_cast<double>(k + 1));
+    }
+
+    linearizedFactorGraph_ =
+        nonlinearFactorGraph_.linearize(linearizationPoint_);
+  }
+
+  void print() const {
+    nonlinearFactorGraph_.print();
+    linearizationPoint_.print();
+    linearizedFactorGraph_.print();
+  }
+
+  HybridValues optimize() const {
+    Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering();
+    HybridBayesNet::shared_ptr hybridBayesNet =
+        linearizedFactorGraph_.eliminateSequential(hybridOrdering);
+
+    HybridValues delta = hybridBayesNet->optimize();
+    return delta;
+  }
+};
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

From b2ca7476d6b2feab9910feec5ca3ec70066c07ce Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 3 Sep 2022 22:54:39 -0400
Subject: [PATCH 02/23] almost done with single legged robot

---
 gtsam/hybrid/tests/testHybridEstimation.cpp | 68 +++++++++++++++------
 1 file changed, 49 insertions(+), 19 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 1aa3060ce5..086c7cb7df 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
@@ -34,6 +35,7 @@ using namespace std;
 using namespace gtsam;
 
 using noiseModel::Isotropic;
+using symbol_shorthand::L;
 using symbol_shorthand::M;
 using symbol_shorthand::X;
 
@@ -158,37 +160,65 @@ class SingleLeg {
   Values linearizationPoint_;
 
  public:
-  SingleLeg(size_t K, std::vector<double> measurements) {
+  /**
+   * @brief Construct a new Single Leg object.
+   *
+   * @param K The number of discrete timesteps
+   * @param pims std::vector of preintegrated IMU measurements.
+   * @param contacts std::vector denoting whether the leg was in contact at each
+   * timestep.
+   */
+  SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<uint64_t> contacts) {
     // Create DiscreteKeys for binary K modes
     for (size_t k = 0; k < K; k++) {
       modes_.emplace_back(M(k), 2);
     }
 
     ////// Create hybrid factor graph.
-    // Add measurement factors
-    auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0);
+
+    // Add measurement factors.
+    // These are the preintegrated IMU measurements of the base.
+    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
     for (size_t k = 0; k < K; k++) {
-      nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<double>>(
-          X(k), measurements.at(k), measurement_noise);
+      nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
+          X(k), X(k + 1), pims.at(k), measurement_noise);
+    }
+
+    // Forward kinematics from base X to foot L
+    auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
+    for (size_t k = 0; k < K; k++) {
+      if (contacts.at(k) == 1) {
+        nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
+            X(k), L(k), Pose2(), fk_noise)
+      } else {
+        nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
+            X(k), L(k), Pose2(), fk_noise)
+      }
     }
 
     // 2 noise models where moving has a higher covariance.
-    auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2);
-    auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2);
+    auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2);
+    auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2);
 
-    // Add "motion models".
-    // The idea is that the robot has a higher "freedom" (aka higher covariance)
-    // for movement
-    using MotionModel = BetweenFactor<double>;
+    // Add "contact models" for the foot.
+    // The idea is that the robot's leg has a tight covariance for stance and
+    // loose covariance for swing.
+    using ContactFactor = BetweenFactor<double>;
 
-    for (size_t k = 1; k < K; k++) {
-      KeyVector keys = {X(k - 1), X(k)};
-      DiscreteKeys dkeys{modes_[k - 1]};
-      auto still = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
-                                                   still_noise_model),
-           moving = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
-                                                    moving_noise_model);
-      std::vector<boost::shared_ptr<MotionModel>> components = {still, moving};
+    for (size_t k = 0; k < K; k++) {
+      KeyVector keys = {L(k), L(k + 1)};
+      DiscreteKeys dkeys{modes_[k], modes_[k + 1]};
+      auto stance = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
+                                                      0.0, stance_model),
+           lift = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0,
+                                                    swing_model),
+           land = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0,
+                                                    swing_model),
+           swing = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
+                                                     0.0, swing_model);
+      // 00 - swing, 01 - land, 10 - toe-off, 11 - stance
+      std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land,
+                                                                  lift, stance};
       nonlinearFactorGraph_.emplace_hybrid<MixtureFactor>(keys, dkeys,
                                                           components);
     }

From 8f94f726a96add1516a57abbe41533ee5f0485c7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 4 Sep 2022 01:59:18 -0400
Subject: [PATCH 03/23] single leg robot test

---
 gtsam/hybrid/tests/testHybridEstimation.cpp | 78 ++++++++++++---------
 1 file changed, 46 insertions(+), 32 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 086c7cb7df..c66c8916d5 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -15,7 +15,6 @@
  * @author  Varun Agrawal
  */
 
-#include <gtsam/base/serializationTestHelpers.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
@@ -40,7 +39,6 @@ using symbol_shorthand::M;
 using symbol_shorthand::X;
 
 class Robot {
-  size_t K_;
   DiscreteKeys modes_;
   HybridNonlinearFactorGraph nonlinearFactorGraph_;
   HybridGaussianFactorGraph linearizedFactorGraph_;
@@ -118,12 +116,10 @@ TEST(Estimation, StillRobot) {
   vector<double> measurements = {0, 0};
 
   Robot robot(K, measurements);
-  // robot.print();
 
   HybridValues delta = robot.optimize();
 
   delta.continuous().print("delta update:");
-
   if (delta.discrete()[M(0)] == 0) {
     std::cout << "The robot is stationary!" << std::endl;
   } else {
@@ -132,17 +128,16 @@ TEST(Estimation, StillRobot) {
   EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]);
 }
 
+/* ****************************************************************************/
 TEST(Estimation, MovingRobot) {
   size_t K = 2;
   vector<double> measurements = {0, 2};
 
   Robot robot(K, measurements);
-  // robot.print();
 
   HybridValues delta = robot.optimize();
 
   delta.continuous().print("delta update:");
-
   if (delta.discrete()[M(0)] == 0) {
     std::cout << "The robot is stationary!" << std::endl;
   } else {
@@ -153,7 +148,6 @@ TEST(Estimation, MovingRobot) {
 
 /// A robot with a single leg.
 class SingleLeg {
-  size_t K_;
   DiscreteKeys modes_;
   HybridNonlinearFactorGraph nonlinearFactorGraph_;
   HybridGaussianFactorGraph linearizedFactorGraph_;
@@ -165,10 +159,9 @@ class SingleLeg {
    *
    * @param K The number of discrete timesteps
    * @param pims std::vector of preintegrated IMU measurements.
-   * @param contacts std::vector denoting whether the leg was in contact at each
-   * timestep.
+   * @param fk std::vector of forward kinematic measurements for the leg.
    */
-  SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<uint64_t> contacts) {
+  SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<Pose2> fk) {
     // Create DiscreteKeys for binary K modes
     for (size_t k = 0; k < K; k++) {
       modes_.emplace_back(M(k), 2);
@@ -176,10 +169,15 @@ class SingleLeg {
 
     ////// Create hybrid factor graph.
 
+    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
+
+    // Add prior on the first pose
+    nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<Pose2>>(
+        X(0), Pose2(0, 2, 0), measurement_noise);
+
     // Add measurement factors.
     // These are the preintegrated IMU measurements of the base.
-    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
-    for (size_t k = 0; k < K; k++) {
+    for (size_t k = 0; k < K - 1; k++) {
       nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
           X(k), X(k + 1), pims.at(k), measurement_noise);
     }
@@ -187,35 +185,30 @@ class SingleLeg {
     // Forward kinematics from base X to foot L
     auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
     for (size_t k = 0; k < K; k++) {
-      if (contacts.at(k) == 1) {
-        nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
-            X(k), L(k), Pose2(), fk_noise)
-      } else {
-        nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
-            X(k), L(k), Pose2(), fk_noise)
-      }
+      nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
+          X(k), L(k), fk.at(k), fk_noise);
     }
 
     // 2 noise models where moving has a higher covariance.
-    auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2);
-    auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2);
+    auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2);
+    auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2);
 
     // Add "contact models" for the foot.
     // The idea is that the robot's leg has a tight covariance for stance and
     // loose covariance for swing.
-    using ContactFactor = BetweenFactor<double>;
+    using ContactFactor = BetweenFactor<Pose2>;
 
-    for (size_t k = 0; k < K; k++) {
+    for (size_t k = 0; k < K - 1; k++) {
       KeyVector keys = {L(k), L(k + 1)};
       DiscreteKeys dkeys{modes_[k], modes_[k + 1]};
-      auto stance = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
-                                                      0.0, stance_model),
-           lift = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0,
-                                                    swing_model),
-           land = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1), 0.0,
-                                                    swing_model),
-           swing = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
-                                                     0.0, swing_model);
+      auto stance = boost::make_shared<ContactFactor>(
+               keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model),
+           lift = boost::make_shared<ContactFactor>(
+               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
+           land = boost::make_shared<ContactFactor>(
+               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
+           swing = boost::make_shared<ContactFactor>(
+               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model);
       // 00 - swing, 01 - land, 10 - toe-off, 11 - stance
       std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land,
                                                                   lift, stance};
@@ -225,7 +218,8 @@ class SingleLeg {
 
     // Create the linearization point.
     for (size_t k = 0; k < K; k++) {
-      linearizationPoint_.insert<double>(X(k), static_cast<double>(k + 1));
+      linearizationPoint_.insert<Pose2>(X(k), Pose2(k, 2, 0));
+      linearizationPoint_.insert<Pose2>(L(k), Pose2(0, 0, 0));
     }
 
     linearizedFactorGraph_ =
@@ -243,11 +237,31 @@ class SingleLeg {
     HybridBayesNet::shared_ptr hybridBayesNet =
         linearizedFactorGraph_.eliminateSequential(hybridOrdering);
 
+    hybridBayesNet->print();
     HybridValues delta = hybridBayesNet->optimize();
     return delta;
   }
+
+  Values linearizationPoint() const { return linearizationPoint_; }
 };
 
+/* ****************************************************************************/
+TEST(Estimation, LeggedRobot) {
+  std::vector<Pose2> pims = {Pose2(1, 0, 0)};
+  // Leg is in stance throughout
+  std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)};
+  SingleLeg robot(2, pims, fk);
+
+  std::cout << "\n\n\n" << std::endl;
+  // robot.print();
+  Values initial = robot.linearizationPoint();
+  // initial.print();
+
+  HybridValues delta = robot.optimize();
+  delta.print();
+
+  initial.retract(delta.continuous()).print("\n\n=========");
+}
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From b5eaaabcb55202139b9b7f47735bc3406699e7d4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 4 Sep 2022 12:06:56 -0400
Subject: [PATCH 04/23] add more tests to show scheme doesn't work

---
 gtsam/hybrid/tests/testHybridEstimation.cpp | 173 +++++++++++++++++++-
 1 file changed, 168 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index c66c8916d5..683617a192 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
 #include <gtsam/hybrid/MixtureFactor.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -204,11 +206,11 @@ class SingleLeg {
       auto stance = boost::make_shared<ContactFactor>(
                keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model),
            lift = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
+               keys.at(0), keys.at(1), Pose2(0, -1, 0), swing_model),
            land = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model),
+               keys.at(0), keys.at(1), Pose2(0, 1, 0), swing_model),
            swing = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model);
+               keys.at(0), keys.at(1), Pose2(1, 0, 0), swing_model);
       // 00 - swing, 01 - land, 10 - toe-off, 11 - stance
       std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land,
                                                                   lift, stance};
@@ -237,7 +239,6 @@ class SingleLeg {
     HybridBayesNet::shared_ptr hybridBayesNet =
         linearizedFactorGraph_.eliminateSequential(hybridOrdering);
 
-    hybridBayesNet->print();
     HybridValues delta = hybridBayesNet->optimize();
     return delta;
   }
@@ -258,10 +259,172 @@ TEST(Estimation, LeggedRobot) {
   // initial.print();
 
   HybridValues delta = robot.optimize();
-  delta.print();
+  // delta.print();
 
   initial.retract(delta.continuous()).print("\n\n=========");
+  std::cout << "\n\n\n" << std::endl;
 }
+
+/// A robot with a single leg - non-hybrid version.
+class SL {
+  NonlinearFactorGraph nonlinearFactorGraph_;
+  GaussianFactorGraph linearizedFactorGraph_;
+  GaussianBayesNet bayesNet_;
+  Values linearizationPoint_;
+
+ public:
+  /**
+   * @brief Construct a new Single Leg object.
+   *
+   * @param K The number of discrete timesteps
+   * @param pims std::vector of preintegrated IMU measurements.
+   * @param fk std::vector of forward kinematic measurements for the leg.
+   */
+  SL(size_t K, const std::vector<Pose2>& pims, const std::vector<Pose2>& fk,
+     const std::vector<bool>& contacts) {
+    ////// Create hybrid factor graph.
+
+    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
+
+    // Add prior on the first pose
+    nonlinearFactorGraph_.emplace_shared<PriorFactor<Pose2>>(
+        X(0), Pose2(0, 2, 0), measurement_noise);
+
+    // Add measurement factors.
+    // These are the preintegrated IMU measurements of the base.
+    for (size_t k = 0; k < K - 1; k++) {
+      nonlinearFactorGraph_.emplace_shared<BetweenFactor<Pose2>>(
+          X(k), X(k + 1), pims.at(k), measurement_noise);
+    }
+
+    // Forward kinematics from base X to foot L
+    auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
+    for (size_t k = 0; k < K; k++) {
+      nonlinearFactorGraph_.emplace_shared<BetweenFactor<Pose2>>(
+          X(k), L(k), fk.at(k), fk_noise);
+    }
+
+    // 2 noise models where moving has a higher covariance.
+    auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-4);
+    auto swing_model = noiseModel::Isotropic::Sigma(3, 1e8);
+
+    // Add "contact models" for the foot.
+    // The idea is that the robot's leg has a tight covariance for stance and
+    // loose covariance for swing.
+    using ContactFactor = BetweenFactor<Pose2>;
+
+    for (size_t k = 0; k < K - 1; k++) {
+      KeyVector keys = {L(k), L(k + 1)};
+      ContactFactor::shared_ptr factor;
+      if (contacts[k] && contacts[k + 1]) {
+        // stance
+        std::cout << "stance 11" << std::endl;
+        factor = boost::make_shared<ContactFactor>(
+            keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model);
+      } else if (contacts[k] && !contacts[k + 1]) {
+        // toe-off
+        std::cout << "toe-off 10" << std::endl;
+        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
+                                                   Pose2(0, 0, 0), swing_model);
+      } else if (!contacts[k] && contacts[k + 1]) {
+        // land
+        std::cout << "land 01" << std::endl;
+        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
+                                                   Pose2(0, 0, 0), swing_model);
+      } else if (!contacts[k] && !contacts[k + 1]) {
+        // swing
+        std::cout << "swing 00" << std::endl;
+        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
+                                                   Pose2(0, 0, 0), swing_model);
+      }
+
+      nonlinearFactorGraph_.push_back(factor);
+    }
+
+    // Create the linearization point.
+    for (size_t k = 0; k < K; k++) {
+      linearizationPoint_.insert<Pose2>(X(k), Pose2(k, 2, 0));
+      linearizationPoint_.insert<Pose2>(L(k), Pose2(0, 0, 0));
+    }
+
+    linearizedFactorGraph_ =
+        *nonlinearFactorGraph_.linearize(linearizationPoint_);
+  }
+
+  void print() const {
+    nonlinearFactorGraph_.print();
+    linearizationPoint_.print();
+    linearizedFactorGraph_.print();
+  }
+
+  VectorValues optimize() {
+    bayesNet_ = *linearizedFactorGraph_.eliminateSequential();
+
+    // bayesNet->print();
+    VectorValues delta = bayesNet_.optimize();
+    return delta;
+  }
+
+  Values linearizationPoint() const { return linearizationPoint_; }
+  NonlinearFactorGraph nonlinearFactorGraph() const {
+    return nonlinearFactorGraph_;
+  }
+  GaussianFactorGraph linearizedFactorGraph() const {
+    return linearizedFactorGraph_;
+  }
+  GaussianBayesNet bayesNet() const { return bayesNet_; }
+};
+
+/* ****************************************************************************/
+TEST(Estimation, LR) {
+  std::vector<Pose2> pims = {Pose2(1, 0, 0)};
+  // Leg is in stance throughout
+  // std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)};
+  // Leg is in swing
+  // std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)};
+  // Leg is in toe-off
+  // std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)};
+  // Leg is in land
+  std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)};
+
+  vector<bool> contacts;
+  contacts = {1, 1};
+  SL robot11(2, pims, fk, contacts);
+  VectorValues delta = robot11.optimize();
+  // robot11.nonlinearFactorGraph().print();
+  std::cout << "Error with optimized delta: " << robot11.bayesNet().error(delta)
+            << std::endl;
+  robot11.linearizationPoint().retract(delta).print();
+  std::cout << "\n===========================\n\n" << std::endl;
+
+  contacts = {1, 0};
+  SL robot10(2, pims, fk, contacts);
+  delta = robot10.optimize();
+  // robot10.nonlinearFactorGraph().print();
+  std::cout << "Error with optimized delta: " << robot10.bayesNet().error(delta)
+            << std::endl;
+  robot10.linearizationPoint().retract(delta).print();
+  std::cout << "\n===========================\n\n" << std::endl;
+
+  contacts = {0, 1};
+  SL robot01(2, pims, fk, contacts);
+  delta = robot01.optimize();
+  // robot01.nonlinearFactorGraph().print();
+  std::cout << "Error with optimized delta: " << robot01.bayesNet().error(delta)
+            << std::endl;
+  robot01.linearizationPoint().retract(delta).print();
+  std::cout << "\n===========================\n\n" << std::endl;
+
+  contacts = {0, 0};
+  SL robot00(2, pims, fk, contacts);
+  delta = robot00.optimize();
+  // robot00.nonlinearFactorGraph().print();
+  std::cout << "Error with optimized delta: " << robot00.bayesNet().error(delta)
+            << std::endl;
+  robot00.linearizationPoint().retract(delta).print();
+  std::cout << "\n===========================\n\n" << std::endl;
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 858193d5df7ecf50f7188371fecfafe0c0ab5983 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 11 Sep 2022 16:44:18 -0400
Subject: [PATCH 05/23] remove print statements

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 9dbbb0468b..caf7cc0808 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -293,8 +293,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
       return exp(-factor->error(empty_values));
     };
     DecisionTree<Key, double> fdt(separatorFactors, factorError);
-    std::cout << "\n\nFactor Decision Tree" << std::endl;
-    fdt.print("", GTDKeyFormatter, [](double x) { return std::to_string(x); });
     auto discreteFactor =
         boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
 

From 75b2599e829e4db84777c7e5763e74c0506605f7 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 3 Oct 2022 17:31:53 -0400
Subject: [PATCH 06/23] remove unnecessary comments

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index d8881cc2a0..6f48f21767 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -285,11 +285,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
-  // frontalKeys.print();
-  // separatorFactors.print("", GTDKeyFormatter, [](const GaussianFactor::shared_ptr &factor) { factor->print(); return "";});
-  // std::cout << "=====================================" << std::endl;
   if (keysOfSeparator.empty()) {
-    VectorValues empty_values;  //TODO(Varun) Shouldn't this be from the optimized leaf?
+    VectorValues empty_values;
     auto factorError = [&](const GaussianFactor::shared_ptr &factor) {
       if (!factor) return 0.0;  // TODO(fan): does this make sense?
       return exp(-factor->error(empty_values));

From 6bd16d995e15e027d58eecbc9d150a6ee2a62401 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 7 Oct 2022 15:29:20 -0400
Subject: [PATCH 07/23] move GTDKeyFormatter to types.h

---
 gtsam/base/types.cpp                       | 46 ++++++++++++++++++++++
 gtsam/base/types.h                         |  2 +
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 46 ----------------------
 3 files changed, 48 insertions(+), 46 deletions(-)

diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
index edc449b12a..f96e037c1b 100644
--- a/gtsam/base/types.cpp
+++ b/gtsam/base/types.cpp
@@ -64,4 +64,50 @@ std::string demangle(const char* name) {
   return demangled_name;
 }
 
+std::string GTDKeyFormatter(const Key &key) {
+  constexpr size_t kMax_uchar_ = std::numeric_limits<uint8_t>::max();
+  constexpr size_t key_bits = sizeof(gtsam::Key) * 8;
+  constexpr size_t ch1_bits = sizeof(uint8_t) * 8;
+  constexpr size_t ch2_bits = sizeof(uint8_t) * 8;
+  constexpr size_t link_bits = sizeof(uint8_t) * 8;
+  constexpr size_t joint_bits = sizeof(uint8_t) * 8;
+  constexpr size_t time_bits =
+      key_bits - ch1_bits - ch2_bits - link_bits - joint_bits;
+
+  constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_)
+                                  << (key_bits - ch1_bits);
+  constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_)
+                                  << (key_bits - ch1_bits - ch2_bits);
+  constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_)
+                                   << (time_bits + joint_bits);
+  constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits;
+  constexpr gtsam::Key time_mask =
+      ~(ch1_mask | ch2_mask | link_mask | joint_mask);
+
+  uint8_t c1_, c2_, link_idx_, joint_idx_;
+  uint64_t t_;
+
+  c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits));
+  c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits));
+  link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits));
+  joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits);
+  t_ = key & time_mask;
+
+  std::string s = "";
+  if (c1_ != 0) {
+    s += c1_;
+  }
+  if (c2_ != 0) {
+    s += c2_;
+  }
+  if (link_idx_ != kMax_uchar_) {
+    s += "[" + std::to_string((int)(link_idx_)) + "]";
+  }
+  if (joint_idx_ != kMax_uchar_) {
+    s += "(" + std::to_string((int)(joint_idx_)) + ")";
+  }
+  s += std::to_string(t_);
+  return s;
+}
+
 } /* namespace gtsam */
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index a0d24f1a62..dcb389c9bb 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -74,6 +74,8 @@ namespace gtsam {
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 
+  std::string GTDKeyFormatter(const Key &key);
+
   /* ************************************************************************* */
   /**
    * Helper class that uses templates to select between two types based on
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 286129f8bc..041603fbd5 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -55,52 +55,6 @@ namespace gtsam {
 
 template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
 
-std::string GTDKeyFormatter(const Key &key) {
-  constexpr size_t kMax_uchar_ = std::numeric_limits<uint8_t>::max();
-  constexpr size_t key_bits = sizeof(gtsam::Key) * 8;
-  constexpr size_t ch1_bits = sizeof(uint8_t) * 8;
-  constexpr size_t ch2_bits = sizeof(uint8_t) * 8;
-  constexpr size_t link_bits = sizeof(uint8_t) * 8;
-  constexpr size_t joint_bits = sizeof(uint8_t) * 8;
-  constexpr size_t time_bits =
-      key_bits - ch1_bits - ch2_bits - link_bits - joint_bits;
-
-  constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_)
-                                  << (key_bits - ch1_bits);
-  constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_)
-                                  << (key_bits - ch1_bits - ch2_bits);
-  constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_)
-                                   << (time_bits + joint_bits);
-  constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits;
-  constexpr gtsam::Key time_mask =
-      ~(ch1_mask | ch2_mask | link_mask | joint_mask);
-
-  uint8_t c1_, c2_, link_idx_, joint_idx_;
-  uint64_t t_;
-
-  c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits));
-  c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits));
-  link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits));
-  joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits);
-  t_ = key & time_mask;
-
-  std::string s = "";
-  if (c1_ != 0) {
-    s += c1_;
-  }
-  if (c2_ != 0) {
-    s += c2_;
-  }
-  if (link_idx_ != kMax_uchar_) {
-    s += "[" + std::to_string((int)(link_idx_)) + "]";
-  }
-  if (joint_idx_ != kMax_uchar_) {
-    s += "(" + std::to_string((int)(joint_idx_)) + ")";
-  }
-  s += std::to_string(t_);
-  return s;
-}
-
 /* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {

From 9c77c661dcb496aba70a3962ca2cc697b387ff71 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 8 Oct 2022 13:56:46 -0400
Subject: [PATCH 08/23] fix tests

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

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
index 683617a192..5d47a22dda 100644
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ b/gtsam/hybrid/tests/testHybridEstimation.cpp
@@ -88,7 +88,7 @@ class Robot {
     }
 
     linearizedFactorGraph_ =
-        nonlinearFactorGraph_.linearize(linearizationPoint_);
+        *nonlinearFactorGraph_.linearize(linearizationPoint_);
   }
 
   void print() const {
@@ -225,7 +225,7 @@ class SingleLeg {
     }
 
     linearizedFactorGraph_ =
-        nonlinearFactorGraph_.linearize(linearizationPoint_);
+        *nonlinearFactorGraph_.linearize(linearizationPoint_);
   }
 
   void print() const {

From 1a17a8117399286977777ca618d9bd39b15c4eb9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 9 Oct 2022 04:25:45 -0400
Subject: [PATCH 09/23] formatting

---
 gtsam/hybrid/HybridFactor.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index e9d64b2830..1216fd9224 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -50,9 +50,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys),
-      isContinuous_(true),
-      continuousKeys_(keys) {}
+    : Base(keys), isContinuous_(true), continuousKeys_(keys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
@@ -101,7 +99,6 @@ void HybridFactor::print(const std::string &s,
     if (d < discreteKeys_.size() - 1) {
       std::cout << " ";
     }
-    
   }
   std::cout << "]";
 }

From 055df81dd37c28aabe7d4bab8403847eba586fd3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 9 Oct 2022 04:26:34 -0400
Subject: [PATCH 10/23] apply keyformatter to Gaussian iSAM in
 HybridNonlinearISAM

---
 gtsam/hybrid/HybridNonlinearISAM.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp
index d05e081dd4..57e0daf8da 100644
--- a/gtsam/hybrid/HybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/HybridNonlinearISAM.cpp
@@ -99,7 +99,7 @@ void HybridNonlinearISAM::print(const string& s,
                                 const KeyFormatter& keyFormatter) const {
   cout << s << "ReorderInterval: " << reorderInterval_
        << " Current Count: " << reorderCounter_ << endl;
-  isam_.print("HybridGaussianISAM:\n");
+  isam_.print("HybridGaussianISAM:\n", keyFormatter);
   linPoint_.print("Linearization Point:\n", keyFormatter);
   factors_.print("Nonlinear Graph:\n", keyFormatter);
 }

From 3e151846ca30d8249d72530669bca03d0be3f731 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Oct 2022 14:55:02 -0400
Subject: [PATCH 11/23] slight improvement to GaussianMixtureFactor print

---
 gtsam/hybrid/GaussianMixtureFactor.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 9b5be188a6..181b1e6a5a 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -57,11 +57,12 @@ void GaussianMixtureFactor::print(const std::string &s,
       [&](const GaussianFactor::shared_ptr &gf) -> std::string {
         RedirectCout rd;
         std::cout << ":\n";
-        if (gf)
+        if (gf && !gf->empty()) {
           gf->print("", formatter);
-        else
-          return {"nullptr"};
-        return rd.str();
+          return rd.str();
+        } else {
+          return "nullptr";
+        }
       });
   std::cout << "}" << std::endl;
 }

From a00bcbcac92d45a5dcc06d7c74d7f25ef9feb32d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Oct 2022 16:03:36 -0400
Subject: [PATCH 12/23] PrunerFunc helper function

---
 gtsam/hybrid/HybridBayesNet.cpp | 65 +++++++++++++++++++++------------
 1 file changed, 42 insertions(+), 23 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 7889707900..163e77e47b 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -49,6 +49,38 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
   return boost::make_shared<DecisionTreeFactor>(dtFactor);
 }
 
+/**
+ * @brief Helper function to get the pruner functional.
+ *
+ * @param probDecisionTree The probability decision tree of only discrete keys.
+ * @param discreteFactorKeySet Set of DiscreteKeys in probDecisionTree.
+ * Pre-computed for efficiency.
+ * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture.
+ * @return std::function<GaussianConditional::shared_ptr(
+ * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+ */
+std::function<GaussianConditional::shared_ptr(
+    const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree,
+           const std::set<DiscreteKey> &discreteFactorKeySet,
+           const std::set<DiscreteKey> &gaussianMixtureKeySet) {
+  auto pruner = [&](const Assignment<Key> &choices,
+                    const GaussianConditional::shared_ptr &conditional)
+      -> GaussianConditional::shared_ptr {
+    // typecast so we can use this to get probability value
+    DiscreteValues values(choices);
+
+    if ((*probDecisionTree)(values) == 0.0) {
+      // empty aka null pointer
+      boost::shared_ptr<GaussianConditional> null;
+      return null;
+    } else {
+      return conditional;
+    }
+  };
+  return pruner;
+}
+
 /* ************************************************************************* */
 HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // Get the decision tree of only the discrete keys
@@ -57,6 +89,8 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
       boost::make_shared<DecisionTreeFactor>(
           discreteConditionals->prune(maxNrLeaves));
 
+  auto discreteFactorKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys());
+
   /* To Prune, we visitWith every leaf in the GaussianMixture.
    * For each leaf, using the assignment we can check the discrete decision tree
    * for 0.0 probability, then just set the leaf to a nullptr.
@@ -66,23 +100,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
 
   HybridBayesNet prunedBayesNetFragment;
 
-  // Functional which loops over all assignments and create a set of
-  // GaussianConditionals
-  auto pruner = [&](const Assignment<Key> &choices,
-                    const GaussianConditional::shared_ptr &conditional)
-      -> GaussianConditional::shared_ptr {
-    // typecast so we can use this to get probability value
-    DiscreteValues values(choices);
-
-    if ((*discreteFactor)(values) == 0.0) {
-      // empty aka null pointer
-      boost::shared_ptr<GaussianConditional> null;
-      return null;
-    } else {
-      return conditional;
-    }
-  };
-
   // Go through all the conditionals in the
   // Bayes Net and prune them as per discreteFactor.
   for (size_t i = 0; i < this->size(); i++) {
@@ -92,17 +109,19 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
         boost::dynamic_pointer_cast<GaussianMixture>(conditional->inner());
 
     if (gaussianMixture) {
-      // We may have mixtures with less discrete keys than discreteFactor so we
-      // skip those since the label assignment does not exist.
+      // We may have mixtures with less discrete keys than discreteFactor so
+      // we skip those since the label assignment does not exist.
       auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys());
-      auto dfKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys());
-      if (gmKeySet != dfKeySet) {
+      if (gmKeySet != discreteFactorKeySet) {
         // Add the gaussianMixture which doesn't have to be pruned.
         prunedBayesNetFragment.push_back(
             boost::make_shared<HybridConditional>(gaussianMixture));
         continue;
       }
 
+      // Get the pruner function.
+      auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet);
+
       // Run the pruning to get a new, pruned tree
       GaussianMixture::Conditionals prunedTree =
           gaussianMixture->conditionals().apply(pruner);
@@ -173,7 +192,7 @@ GaussianBayesNet HybridBayesNet::choose(
   return gbn;
 }
 
-/* *******************************************************************************/
+/* ************************************************************************* */
 HybridValues HybridBayesNet::optimize() const {
   // Solve for the MPE
   DiscreteBayesNet discrete_bn;
@@ -190,7 +209,7 @@ HybridValues HybridBayesNet::optimize() const {
   return HybridValues(mpe, gbn.optimize());
 }
 
-/* *******************************************************************************/
+/* ************************************************************************* */
 VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const {
   GaussianBayesNet gbn = this->choose(assignment);
   return gbn.optimize();

From 2c8fe2584291a77557e1396306efa1210bc4f3d3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 10 Oct 2022 19:38:10 -0400
Subject: [PATCH 13/23] enumerate missing discrete keys so we can prune all
 gaussian mixtures

---
 gtsam/hybrid/HybridBayesNet.cpp | 41 ++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 11 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 163e77e47b..96a6dfd63f 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -70,12 +70,37 @@ PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree,
     // typecast so we can use this to get probability value
     DiscreteValues values(choices);
 
-    if ((*probDecisionTree)(values) == 0.0) {
-      // empty aka null pointer
-      boost::shared_ptr<GaussianConditional> null;
-      return null;
+    // Case where the gaussian mixture has the same
+    // discrete keys as the decision tree.
+    if (gaussianMixtureKeySet == discreteFactorKeySet) {
+      if ((*probDecisionTree)(values) == 0.0) {
+        // empty aka null pointer
+        boost::shared_ptr<GaussianConditional> null;
+        return null;
+      } else {
+        return conditional;
+      }
     } else {
-      return conditional;
+      std::vector<DiscreteKey> set_diff;
+      std::set_difference(
+          discreteFactorKeySet.begin(), discreteFactorKeySet.end(),
+          gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(),
+          std::back_inserter(set_diff));
+      const std::vector<DiscreteValues> assignments =
+          DiscreteValues::CartesianProduct(set_diff);
+      for (const DiscreteValues &assignment : assignments) {
+        DiscreteValues augmented_values(values);
+        augmented_values.insert(assignment.begin(), assignment.end());
+
+        // If any one of the sub-branches are non-zero,
+        // we need this conditional.
+        if ((*probDecisionTree)(augmented_values) > 0.0) {
+          return conditional;
+        }
+      }
+      // If we are here, it means that all the sub-branches are 0,
+      // so we prune.
+      return nullptr;
     }
   };
   return pruner;
@@ -112,12 +137,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
       // We may have mixtures with less discrete keys than discreteFactor so
       // we skip those since the label assignment does not exist.
       auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys());
-      if (gmKeySet != discreteFactorKeySet) {
-        // Add the gaussianMixture which doesn't have to be pruned.
-        prunedBayesNetFragment.push_back(
-            boost::make_shared<HybridConditional>(gaussianMixture));
-        continue;
-      }
 
       // Get the pruner function.
       auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet);

From c15cfb606849dcbdb089656da5871118d6f73fdf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 12:10:02 -0400
Subject: [PATCH 14/23] add PrunerFunc to GaussianMixture

---
 gtsam/hybrid/GaussianMixture.cpp | 78 +++++++++++++++++++++++++++-----
 1 file changed, 67 insertions(+), 11 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 3a654ddade..064905d6b8 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -122,31 +122,87 @@ void GaussianMixture::print(const std::string &s,
         if (gf && !gf->empty()) {
           gf->print("", formatter);
           return rd.str();
+          // return "Node()";
         } else {
           return "nullptr";
         }
       });
 }
 
-/* *******************************************************************************/
-void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
-  // Functional which loops over all assignments and create a set of
-  // GaussianConditionals
-  auto pruner = [&decisionTree](
-                    const Assignment<Key> &choices,
+/* ************************************************************************* */
+/// Return the DiscreteKey vector as a set.
+static std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
+  std::set<DiscreteKey> s;
+  s.insert(dkeys.begin(), dkeys.end());
+  return s;
+}
+
+/* ************************************************************************* */
+/**
+ * @brief Helper function to get the pruner functional.
+ *
+ * @param decisionTree The probability decision tree of only discrete keys.
+ * @param decisionTreeKeySet Set of DiscreteKeys in decisionTree.
+ * Pre-computed for efficiency.
+ * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture.
+ * @return std::function<GaussianConditional::shared_ptr(
+ * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+ */
+std::function<GaussianConditional::shared_ptr(
+    const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+PrunerFunc(const DecisionTreeFactor &decisionTree,
+           const std::set<DiscreteKey> &decisionTreeKeySet,
+           const std::set<DiscreteKey> &gaussianMixtureKeySet) {
+  auto pruner = [&](const Assignment<Key> &choices,
                     const GaussianConditional::shared_ptr &conditional)
       -> GaussianConditional::shared_ptr {
     // typecast so we can use this to get probability value
     DiscreteValues values(choices);
 
-    if (decisionTree(values) == 0.0) {
-      // empty aka null pointer
-      boost::shared_ptr<GaussianConditional> null;
-      return null;
+    // Case where the gaussian mixture has the same
+    // discrete keys as the decision tree.
+    if (gaussianMixtureKeySet == decisionTreeKeySet) {
+      if (decisionTree(values) == 0.0) {
+        // empty aka null pointer
+        boost::shared_ptr<GaussianConditional> null;
+        return null;
+      } else {
+        return conditional;
+      }
     } else {
-      return conditional;
+      std::vector<DiscreteKey> set_diff;
+      std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(),
+                          gaussianMixtureKeySet.begin(),
+                          gaussianMixtureKeySet.end(),
+                          std::back_inserter(set_diff));
+
+      const std::vector<DiscreteValues> assignments =
+          DiscreteValues::CartesianProduct(set_diff);
+      for (const DiscreteValues &assignment : assignments) {
+        DiscreteValues augmented_values(values);
+        augmented_values.insert(assignment.begin(), assignment.end());
+
+        // If any one of the sub-branches are non-zero,
+        // we need this conditional.
+        if (decisionTree(augmented_values) > 0.0) {
+          return conditional;
+        }
+      }
+      // If we are here, it means that all the sub-branches are 0,
+      // so we prune.
+      return nullptr;
     }
   };
+  return pruner;
+}
+
+/* *******************************************************************************/
+void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
+  auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
+  auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
+  // Functional which loops over all assignments and create a set of
+  // GaussianConditionals
+  auto pruner = PrunerFunc(decisionTree, decisionTreeKeySet, gmKeySet);
 
   auto pruned_conditionals = conditionals_.apply(pruner);
   conditionals_.root_ = pruned_conditionals.root_;

From 2225ecf44277c82b0f2d408236235c772b5f5b10 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 12:35:58 -0400
Subject: [PATCH 15/23] clean up the prunerFunc

---
 gtsam/hybrid/GaussianMixture.cpp | 19 ++++++++++---------
 gtsam/hybrid/GaussianMixture.h   | 11 +++++++++++
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 064905d6b8..244d527388 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -131,7 +131,7 @@ void GaussianMixture::print(const std::string &s,
 
 /* ************************************************************************* */
 /// Return the DiscreteKey vector as a set.
-static std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
+std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
   std::set<DiscreteKey> s;
   s.insert(dkeys.begin(), dkeys.end());
   return s;
@@ -142,18 +142,19 @@ static std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
  * @brief Helper function to get the pruner functional.
  *
  * @param decisionTree The probability decision tree of only discrete keys.
- * @param decisionTreeKeySet Set of DiscreteKeys in decisionTree.
- * Pre-computed for efficiency.
- * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture.
  * @return std::function<GaussianConditional::shared_ptr(
  * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
  */
 std::function<GaussianConditional::shared_ptr(
     const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-PrunerFunc(const DecisionTreeFactor &decisionTree,
-           const std::set<DiscreteKey> &decisionTreeKeySet,
-           const std::set<DiscreteKey> &gaussianMixtureKeySet) {
-  auto pruner = [&](const Assignment<Key> &choices,
+GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) {
+  // Get the discrete keys as sets for the decision tree
+  // and the gaussian mixture.
+  auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys());
+  auto gaussianMixtureKeySet = DiscreteKeysAsSet(this->discreteKeys());
+
+  auto pruner = [decisionTree, decisionTreeKeySet, gaussianMixtureKeySet](
+                    const Assignment<Key> &choices,
                     const GaussianConditional::shared_ptr &conditional)
       -> GaussianConditional::shared_ptr {
     // typecast so we can use this to get probability value
@@ -202,7 +203,7 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) {
   auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
   // Functional which loops over all assignments and create a set of
   // GaussianConditionals
-  auto pruner = PrunerFunc(decisionTree, decisionTreeKeySet, gmKeySet);
+  auto pruner = prunerFunc(decisionTree);
 
   auto pruned_conditionals = conditionals_.apply(pruner);
   conditionals_.root_ = pruned_conditionals.root_;
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 75deb4d553..9792a85323 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -69,6 +69,17 @@ class GTSAM_EXPORT GaussianMixture
    */
   Sum asGaussianFactorGraphTree() const;
 
+  /**
+   * @brief Helper function to get the pruner functor.
+   *
+   * @param decisionTree The pruned discrete probability decision tree.
+   * @return std::function<GaussianConditional::shared_ptr(
+   * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+   */
+  std::function<GaussianConditional::shared_ptr(
+      const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
+  prunerFunc(const DecisionTreeFactor &decisionTree);
+
  public:
   /// @name Constructors
   /// @{

From 5e99cd7095cbdf1da731dda0de3bc1a6a680d4c9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 12:36:58 -0400
Subject: [PATCH 16/23] HybridBayesNet and HybridBayesTree both use similar
 pruning functions

---
 gtsam/hybrid/HybridBayesNet.cpp               | 101 ++----------------
 gtsam/hybrid/HybridBayesTree.cpp              |  20 ++--
 gtsam/hybrid/tests/testHybridGaussianISAM.cpp |   2 +-
 .../hybrid/tests/testHybridNonlinearISAM.cpp  |   2 +-
 4 files changed, 15 insertions(+), 110 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 96a6dfd63f..cc27600f09 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -22,14 +22,6 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-/// Return the DiscreteKey vector as a set.
-static std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &dkeys) {
-  std::set<DiscreteKey> s;
-  s.insert(dkeys.begin(), dkeys.end());
-  return s;
-}
-
 /* ************************************************************************* */
 DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
   AlgebraicDecisionTree<Key> decisionTree;
@@ -49,63 +41,6 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const {
   return boost::make_shared<DecisionTreeFactor>(dtFactor);
 }
 
-/**
- * @brief Helper function to get the pruner functional.
- *
- * @param probDecisionTree The probability decision tree of only discrete keys.
- * @param discreteFactorKeySet Set of DiscreteKeys in probDecisionTree.
- * Pre-computed for efficiency.
- * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture.
- * @return std::function<GaussianConditional::shared_ptr(
- * const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
- */
-std::function<GaussianConditional::shared_ptr(
-    const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
-PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree,
-           const std::set<DiscreteKey> &discreteFactorKeySet,
-           const std::set<DiscreteKey> &gaussianMixtureKeySet) {
-  auto pruner = [&](const Assignment<Key> &choices,
-                    const GaussianConditional::shared_ptr &conditional)
-      -> GaussianConditional::shared_ptr {
-    // typecast so we can use this to get probability value
-    DiscreteValues values(choices);
-
-    // Case where the gaussian mixture has the same
-    // discrete keys as the decision tree.
-    if (gaussianMixtureKeySet == discreteFactorKeySet) {
-      if ((*probDecisionTree)(values) == 0.0) {
-        // empty aka null pointer
-        boost::shared_ptr<GaussianConditional> null;
-        return null;
-      } else {
-        return conditional;
-      }
-    } else {
-      std::vector<DiscreteKey> set_diff;
-      std::set_difference(
-          discreteFactorKeySet.begin(), discreteFactorKeySet.end(),
-          gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(),
-          std::back_inserter(set_diff));
-      const std::vector<DiscreteValues> assignments =
-          DiscreteValues::CartesianProduct(set_diff);
-      for (const DiscreteValues &assignment : assignments) {
-        DiscreteValues augmented_values(values);
-        augmented_values.insert(assignment.begin(), assignment.end());
-
-        // If any one of the sub-branches are non-zero,
-        // we need this conditional.
-        if ((*probDecisionTree)(augmented_values) > 0.0) {
-          return conditional;
-        }
-      }
-      // If we are here, it means that all the sub-branches are 0,
-      // so we prune.
-      return nullptr;
-    }
-  };
-  return pruner;
-}
-
 /* ************************************************************************* */
 HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   // Get the decision tree of only the discrete keys
@@ -114,8 +49,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
       boost::make_shared<DecisionTreeFactor>(
           discreteConditionals->prune(maxNrLeaves));
 
-  auto discreteFactorKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys());
-
   /* To Prune, we visitWith every leaf in the GaussianMixture.
    * For each leaf, using the assignment we can check the discrete decision tree
    * for 0.0 probability, then just set the leaf to a nullptr.
@@ -130,35 +63,13 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const {
   for (size_t i = 0; i < this->size(); i++) {
     HybridConditional::shared_ptr conditional = this->at(i);
 
-    GaussianMixture::shared_ptr gaussianMixture =
-        boost::dynamic_pointer_cast<GaussianMixture>(conditional->inner());
-
-    if (gaussianMixture) {
-      // We may have mixtures with less discrete keys than discreteFactor so
-      // we skip those since the label assignment does not exist.
-      auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys());
-
-      // Get the pruner function.
-      auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet);
-
-      // Run the pruning to get a new, pruned tree
-      GaussianMixture::Conditionals prunedTree =
-          gaussianMixture->conditionals().apply(pruner);
-
-      DiscreteKeys discreteKeys = gaussianMixture->discreteKeys();
-      // reverse keys to get a natural ordering
-      std::reverse(discreteKeys.begin(), discreteKeys.end());
-
-      // Convert from boost::iterator_range to KeyVector
-      // so we can pass it to constructor.
-      KeyVector frontals(gaussianMixture->frontals().begin(),
-                         gaussianMixture->frontals().end()),
-          parents(gaussianMixture->parents().begin(),
-                  gaussianMixture->parents().end());
+    if (conditional->isHybrid()) {
+      GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture();
 
-      // Create the new gaussian mixture and add it to the bayes net.
-      auto prunedGaussianMixture = boost::make_shared<GaussianMixture>(
-          frontals, parents, discreteKeys, prunedTree);
+      // Make a copy of the gaussian mixture and prune it!
+      auto prunedGaussianMixture =
+          boost::make_shared<GaussianMixture>(*gaussianMixture);
+      prunedGaussianMixture->prune(*discreteFactor);
 
       // Type-erase and add to the pruned Bayes Net fragment.
       prunedBayesNetFragment.push_back(
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 8fb487ae20..266b295dd5 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -149,16 +149,16 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
   auto decisionTree = boost::dynamic_pointer_cast<DecisionTreeFactor>(
       this->roots_.at(0)->conditional()->inner());
 
-  DecisionTreeFactor prunedDiscreteFactor = decisionTree->prune(maxNrLeaves);
-  decisionTree->root_ = prunedDiscreteFactor.root_;
+  DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves);
+  decisionTree->root_ = prunedDecisionTree.root_;
 
   /// Helper struct for pruning the hybrid bayes tree.
   struct HybridPrunerData {
     /// The discrete decision tree after pruning.
-    DecisionTreeFactor prunedDiscreteFactor;
-    HybridPrunerData(const DecisionTreeFactor& prunedDiscreteFactor,
+    DecisionTreeFactor prunedDecisionTree;
+    HybridPrunerData(const DecisionTreeFactor& prunedDecisionTree,
                      const HybridBayesTree::sharedNode& parentClique)
-        : prunedDiscreteFactor(prunedDiscreteFactor) {}
+        : prunedDecisionTree(prunedDecisionTree) {}
 
     /**
      * @brief A function used during tree traversal that operates on each node
@@ -178,19 +178,13 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) {
       if (conditional->isHybrid()) {
         auto gaussianMixture = conditional->asMixture();
 
-        // Check if the number of discrete keys match,
-        // else we get an assignment error.
-        // TODO(Varun) Update prune method to handle assignment subset?
-        if (gaussianMixture->discreteKeys() ==
-            parentData.prunedDiscreteFactor.discreteKeys()) {
-          gaussianMixture->prune(parentData.prunedDiscreteFactor);
-        }
+        gaussianMixture->prune(parentData.prunedDecisionTree);
       }
       return parentData;
     }
   };
 
-  HybridPrunerData rootData(prunedDiscreteFactor, 0);
+  HybridPrunerData rootData(prunedDecisionTree, 0);
   {
     treeTraversal::no_op visitorPost;
     // Limits OpenMP threads since we're mixing TBB and OpenMP
diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
index a0a87933f4..a5e3903d94 100644
--- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp
@@ -337,7 +337,7 @@ TEST(HybridGaussianElimination, Incremental_approximate) {
   EXPECT_LONGS_EQUAL(
       2, incrementalHybrid[X(1)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(
-      4, incrementalHybrid[X(2)]->conditional()->asMixture()->nrComponents());
+      3, incrementalHybrid[X(2)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(
       5, incrementalHybrid[X(3)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(
diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index 4e1710c424..93a8a1e005 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -363,7 +363,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
   EXPECT_LONGS_EQUAL(
       2, bayesTree[X(1)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(
-      4, bayesTree[X(2)]->conditional()->asMixture()->nrComponents());
+      3, bayesTree[X(2)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(
       5, bayesTree[X(3)]->conditional()->asMixture()->nrComponents());
   EXPECT_LONGS_EQUAL(

From c2377f3b34e68212a75e7dfb403f32c87a056e0c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 13:15:50 -0400
Subject: [PATCH 17/23] minor fixes to unit test

---
 .../hybrid/tests/testHybridNonlinearISAM.cpp  | 21 +++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index 93a8a1e005..cc83ccac6c 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -12,7 +12,7 @@
 /**
  * @file    testHybridNonlinearISAM.cpp
  * @brief   Unit tests for nonlinear incremental inference
- * @author  Fan Jiang, Varun Agrawal, Frank Dellaert
+ * @author  Varun Agrawal, Fan Jiang, Frank Dellaert
  * @date    Jan 2021
  */
 
@@ -391,6 +391,23 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
       5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents());
 }
 
+void printConditionals(const HybridNonlinearISAM &inc, const KeyVector &keys) {
+  HybridGaussianISAM bayesTree = inc.bayesTree();
+  for (auto &&key : keys) {
+    std::cout << DefaultKeyFormatter(key) << std::endl;
+    auto conditional = bayesTree[key]->conditional();
+    conditional->printKeys();
+    if (conditional->isHybrid()) {
+      std::cout << conditional->asMixture()->nrComponents() << std::endl;
+    } else if (conditional->isDiscrete()) {
+      std::cout << conditional->asDiscreteConditional()->nrLeaves()
+                << std::endl;
+    } else {
+      // std::cout << conditional->asGaussian()->nrComponents() << std::endl;
+    }
+  }
+}
+
 /* ************************************************************************/
 // A GTSAM-only test for running inference on a single-legged robot.
 // The leg links are represented by the chain X-Y-Z-W, where X is the base and
@@ -432,9 +449,9 @@ TEST(HybridNonlinearISAM, NonTrivial) {
 
   // Don't run update now since we don't have discrete variables involved.
 
-  /*************** Run Round 2 ***************/
   using PlanarMotionModel = BetweenFactor<Pose2>;
 
+  /*************** Run Round 2 ***************/
   // Add odometry factor with discrete modes.
   Pose2 odometry(1.0, 0.0, 0.0);
   KeyVector contKeys = {W(0), W(1)};

From 80908126db77063ab390f6794f7b32ed0a399c12 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 13:16:05 -0400
Subject: [PATCH 18/23] add assertions to remove warning

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

diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
index 6635633a27..ab69e82d77 100644
--- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp
+++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp
@@ -159,6 +159,10 @@ TEST(DiscreteBayesTree, ThinTree) {
       clique->separatorMarginal(EliminateDiscrete);
   DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
 
+  DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9);
+  DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9);
+  DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9);
+
   // check separator marginal P(S9), should be P(14)
   clique = (*self.bayesTree)[9];
   DiscreteFactorGraph separatorMarginal9 =

From eacc888d2065491dbeed5f1dc59b1494534f63b0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 11 Oct 2022 13:27:15 -0400
Subject: [PATCH 19/23] remove print function

---
 gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 17 -----------------
 1 file changed, 17 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
index cc83ccac6c..fbb114ef32 100644
--- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
+++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
@@ -391,23 +391,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) {
       5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents());
 }
 
-void printConditionals(const HybridNonlinearISAM &inc, const KeyVector &keys) {
-  HybridGaussianISAM bayesTree = inc.bayesTree();
-  for (auto &&key : keys) {
-    std::cout << DefaultKeyFormatter(key) << std::endl;
-    auto conditional = bayesTree[key]->conditional();
-    conditional->printKeys();
-    if (conditional->isHybrid()) {
-      std::cout << conditional->asMixture()->nrComponents() << std::endl;
-    } else if (conditional->isDiscrete()) {
-      std::cout << conditional->asDiscreteConditional()->nrLeaves()
-                << std::endl;
-    } else {
-      // std::cout << conditional->asGaussian()->nrComponents() << std::endl;
-    }
-  }
-}
-
 /* ************************************************************************/
 // A GTSAM-only test for running inference on a single-legged robot.
 // The leg links are represented by the chain X-Y-Z-W, where X is the base and

From 8c19f499a3ff8d2f5fb7413aafa4ce03aeb722ea Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Oct 2022 10:13:43 -0400
Subject: [PATCH 20/23] add curlyy brackets to for loop

---
 gtsam/hybrid/HybridGaussianISAM.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
index 57e50104d7..de87dd92f1 100644
--- a/gtsam/hybrid/HybridGaussianISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -59,8 +59,9 @@ void HybridGaussianISAM::updateInternal(
   factors += newFactors;
 
   // Add the orphaned subtrees
-  for (const sharedClique& orphan : *orphans)
-    factors += boost::make_shared<BayesTreeOrphanWrapper<Node> >(orphan);
+  for (const sharedClique& orphan : *orphans) {
+    factors += boost::make_shared<BayesTreeOrphanWrapper<Node>>(orphan);
+  }
 
   // Get all the discrete keys from the factors
   KeySet allDiscrete = factors.discreteKeys();

From 1d70d14c72b04e01e183d070ecfa71665b4123b5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Oct 2022 10:14:46 -0400
Subject: [PATCH 21/23] remove custom keyformatter

---
 gtsam/base/types.cpp | 46 --------------------------------------------
 gtsam/base/types.h   |  2 --
 2 files changed, 48 deletions(-)

diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
index f96e037c1b..edc449b12a 100644
--- a/gtsam/base/types.cpp
+++ b/gtsam/base/types.cpp
@@ -64,50 +64,4 @@ std::string demangle(const char* name) {
   return demangled_name;
 }
 
-std::string GTDKeyFormatter(const Key &key) {
-  constexpr size_t kMax_uchar_ = std::numeric_limits<uint8_t>::max();
-  constexpr size_t key_bits = sizeof(gtsam::Key) * 8;
-  constexpr size_t ch1_bits = sizeof(uint8_t) * 8;
-  constexpr size_t ch2_bits = sizeof(uint8_t) * 8;
-  constexpr size_t link_bits = sizeof(uint8_t) * 8;
-  constexpr size_t joint_bits = sizeof(uint8_t) * 8;
-  constexpr size_t time_bits =
-      key_bits - ch1_bits - ch2_bits - link_bits - joint_bits;
-
-  constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_)
-                                  << (key_bits - ch1_bits);
-  constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_)
-                                  << (key_bits - ch1_bits - ch2_bits);
-  constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_)
-                                   << (time_bits + joint_bits);
-  constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits;
-  constexpr gtsam::Key time_mask =
-      ~(ch1_mask | ch2_mask | link_mask | joint_mask);
-
-  uint8_t c1_, c2_, link_idx_, joint_idx_;
-  uint64_t t_;
-
-  c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits));
-  c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits));
-  link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits));
-  joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits);
-  t_ = key & time_mask;
-
-  std::string s = "";
-  if (c1_ != 0) {
-    s += c1_;
-  }
-  if (c2_ != 0) {
-    s += c2_;
-  }
-  if (link_idx_ != kMax_uchar_) {
-    s += "[" + std::to_string((int)(link_idx_)) + "]";
-  }
-  if (joint_idx_ != kMax_uchar_) {
-    s += "(" + std::to_string((int)(joint_idx_)) + ")";
-  }
-  s += std::to_string(t_);
-  return s;
-}
-
 } /* namespace gtsam */
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index dcb389c9bb..a0d24f1a62 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -74,8 +74,6 @@ namespace gtsam {
   /// The index type for Eigen objects
   typedef ptrdiff_t DenseIndex;
 
-  std::string GTDKeyFormatter(const Key &key);
-
   /* ************************************************************************* */
   /**
    * Helper class that uses templates to select between two types based on

From 8f7473d0265e42227376b9555340f237768b45d4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Oct 2022 10:46:04 -0400
Subject: [PATCH 22/23] remove added test file

---
 gtsam/hybrid/tests/testHybridEstimation.cpp | 433 --------------------
 1 file changed, 433 deletions(-)
 delete mode 100644 gtsam/hybrid/tests/testHybridEstimation.cpp

diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp
deleted file mode 100644
index 5d47a22dda..0000000000
--- a/gtsam/hybrid/tests/testHybridEstimation.cpp
+++ /dev/null
@@ -1,433 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file    testHybridEstimation.cpp
- * @brief   Unit tests for Hybrid Estimation
- * @author  Varun Agrawal
- */
-
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
-#include <gtsam/hybrid/MixtureFactor.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/NoiseModel.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-
-// Include for test suite
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace gtsam;
-
-using noiseModel::Isotropic;
-using symbol_shorthand::L;
-using symbol_shorthand::M;
-using symbol_shorthand::X;
-
-class Robot {
-  DiscreteKeys modes_;
-  HybridNonlinearFactorGraph nonlinearFactorGraph_;
-  HybridGaussianFactorGraph linearizedFactorGraph_;
-  Values linearizationPoint_;
-
- public:
-  Robot(size_t K, std::vector<double> measurements) {
-    // Create DiscreteKeys for binary K modes
-    for (size_t k = 0; k < K; k++) {
-      modes_.emplace_back(M(k), 2);
-    }
-
-    ////// Create hybrid factor graph.
-    // Add measurement factors
-    auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0);
-    for (size_t k = 0; k < K; k++) {
-      nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<double>>(
-          X(k), measurements.at(k), measurement_noise);
-    }
-
-    // 2 noise models where moving has a higher covariance.
-    auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2);
-    auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2);
-
-    // Add "motion models".
-    // The idea is that the robot has a higher "freedom" (aka higher covariance)
-    // for movement
-    using MotionModel = BetweenFactor<double>;
-
-    for (size_t k = 1; k < K; k++) {
-      KeyVector keys = {X(k - 1), X(k)};
-      DiscreteKeys dkeys{modes_[k - 1]};
-      auto still = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
-                                                   still_noise_model),
-           moving = boost::make_shared<MotionModel>(X(k - 1), X(k), 0.0,
-                                                    moving_noise_model);
-      std::vector<boost::shared_ptr<MotionModel>> components = {still, moving};
-      nonlinearFactorGraph_.emplace_hybrid<MixtureFactor>(keys, dkeys,
-                                                          components);
-    }
-
-    // Create the linearization point.
-    for (size_t k = 0; k < K; k++) {
-      linearizationPoint_.insert<double>(X(k), static_cast<double>(k + 1));
-    }
-
-    linearizedFactorGraph_ =
-        *nonlinearFactorGraph_.linearize(linearizationPoint_);
-  }
-
-  void print() const {
-    nonlinearFactorGraph_.print();
-    linearizationPoint_.print();
-    linearizedFactorGraph_.print();
-  }
-
-  HybridValues optimize() const {
-    Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering();
-    HybridBayesNet::shared_ptr hybridBayesNet =
-        linearizedFactorGraph_.eliminateSequential(hybridOrdering);
-
-    HybridValues delta = hybridBayesNet->optimize();
-    return delta;
-  }
-};
-
-/* ****************************************************************************/
-/**
- * I am trying to test if setting the hybrid mixture components to just differ
- * in covariance makes sense. This is done by setting the "moving" covariance to
- * be 1e2 while the "still" covariance is 1e-2.
- */
-TEST(Estimation, StillRobot) {
-  size_t K = 2;
-  vector<double> measurements = {0, 0};
-
-  Robot robot(K, measurements);
-
-  HybridValues delta = robot.optimize();
-
-  delta.continuous().print("delta update:");
-  if (delta.discrete()[M(0)] == 0) {
-    std::cout << "The robot is stationary!" << std::endl;
-  } else {
-    std::cout << "The robot has moved!" << std::endl;
-  }
-  EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]);
-}
-
-/* ****************************************************************************/
-TEST(Estimation, MovingRobot) {
-  size_t K = 2;
-  vector<double> measurements = {0, 2};
-
-  Robot robot(K, measurements);
-
-  HybridValues delta = robot.optimize();
-
-  delta.continuous().print("delta update:");
-  if (delta.discrete()[M(0)] == 0) {
-    std::cout << "The robot is stationary!" << std::endl;
-  } else {
-    std::cout << "The robot has moved!" << std::endl;
-  }
-  EXPECT_LONGS_EQUAL(1, delta.discrete()[M(0)]);
-}
-
-/// A robot with a single leg.
-class SingleLeg {
-  DiscreteKeys modes_;
-  HybridNonlinearFactorGraph nonlinearFactorGraph_;
-  HybridGaussianFactorGraph linearizedFactorGraph_;
-  Values linearizationPoint_;
-
- public:
-  /**
-   * @brief Construct a new Single Leg object.
-   *
-   * @param K The number of discrete timesteps
-   * @param pims std::vector of preintegrated IMU measurements.
-   * @param fk std::vector of forward kinematic measurements for the leg.
-   */
-  SingleLeg(size_t K, std::vector<Pose2> pims, std::vector<Pose2> fk) {
-    // Create DiscreteKeys for binary K modes
-    for (size_t k = 0; k < K; k++) {
-      modes_.emplace_back(M(k), 2);
-    }
-
-    ////// Create hybrid factor graph.
-
-    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
-
-    // Add prior on the first pose
-    nonlinearFactorGraph_.emplace_nonlinear<PriorFactor<Pose2>>(
-        X(0), Pose2(0, 2, 0), measurement_noise);
-
-    // Add measurement factors.
-    // These are the preintegrated IMU measurements of the base.
-    for (size_t k = 0; k < K - 1; k++) {
-      nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
-          X(k), X(k + 1), pims.at(k), measurement_noise);
-    }
-
-    // Forward kinematics from base X to foot L
-    auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
-    for (size_t k = 0; k < K; k++) {
-      nonlinearFactorGraph_.emplace_nonlinear<BetweenFactor<Pose2>>(
-          X(k), L(k), fk.at(k), fk_noise);
-    }
-
-    // 2 noise models where moving has a higher covariance.
-    auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2);
-    auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2);
-
-    // Add "contact models" for the foot.
-    // The idea is that the robot's leg has a tight covariance for stance and
-    // loose covariance for swing.
-    using ContactFactor = BetweenFactor<Pose2>;
-
-    for (size_t k = 0; k < K - 1; k++) {
-      KeyVector keys = {L(k), L(k + 1)};
-      DiscreteKeys dkeys{modes_[k], modes_[k + 1]};
-      auto stance = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model),
-           lift = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, -1, 0), swing_model),
-           land = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(0, 1, 0), swing_model),
-           swing = boost::make_shared<ContactFactor>(
-               keys.at(0), keys.at(1), Pose2(1, 0, 0), swing_model);
-      // 00 - swing, 01 - land, 10 - toe-off, 11 - stance
-      std::vector<boost::shared_ptr<ContactFactor>> components = {swing, land,
-                                                                  lift, stance};
-      nonlinearFactorGraph_.emplace_hybrid<MixtureFactor>(keys, dkeys,
-                                                          components);
-    }
-
-    // Create the linearization point.
-    for (size_t k = 0; k < K; k++) {
-      linearizationPoint_.insert<Pose2>(X(k), Pose2(k, 2, 0));
-      linearizationPoint_.insert<Pose2>(L(k), Pose2(0, 0, 0));
-    }
-
-    linearizedFactorGraph_ =
-        *nonlinearFactorGraph_.linearize(linearizationPoint_);
-  }
-
-  void print() const {
-    nonlinearFactorGraph_.print();
-    linearizationPoint_.print();
-    linearizedFactorGraph_.print();
-  }
-
-  HybridValues optimize() const {
-    Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering();
-    HybridBayesNet::shared_ptr hybridBayesNet =
-        linearizedFactorGraph_.eliminateSequential(hybridOrdering);
-
-    HybridValues delta = hybridBayesNet->optimize();
-    return delta;
-  }
-
-  Values linearizationPoint() const { return linearizationPoint_; }
-};
-
-/* ****************************************************************************/
-TEST(Estimation, LeggedRobot) {
-  std::vector<Pose2> pims = {Pose2(1, 0, 0)};
-  // Leg is in stance throughout
-  std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)};
-  SingleLeg robot(2, pims, fk);
-
-  std::cout << "\n\n\n" << std::endl;
-  // robot.print();
-  Values initial = robot.linearizationPoint();
-  // initial.print();
-
-  HybridValues delta = robot.optimize();
-  // delta.print();
-
-  initial.retract(delta.continuous()).print("\n\n=========");
-  std::cout << "\n\n\n" << std::endl;
-}
-
-/// A robot with a single leg - non-hybrid version.
-class SL {
-  NonlinearFactorGraph nonlinearFactorGraph_;
-  GaussianFactorGraph linearizedFactorGraph_;
-  GaussianBayesNet bayesNet_;
-  Values linearizationPoint_;
-
- public:
-  /**
-   * @brief Construct a new Single Leg object.
-   *
-   * @param K The number of discrete timesteps
-   * @param pims std::vector of preintegrated IMU measurements.
-   * @param fk std::vector of forward kinematic measurements for the leg.
-   */
-  SL(size_t K, const std::vector<Pose2>& pims, const std::vector<Pose2>& fk,
-     const std::vector<bool>& contacts) {
-    ////// Create hybrid factor graph.
-
-    auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0);
-
-    // Add prior on the first pose
-    nonlinearFactorGraph_.emplace_shared<PriorFactor<Pose2>>(
-        X(0), Pose2(0, 2, 0), measurement_noise);
-
-    // Add measurement factors.
-    // These are the preintegrated IMU measurements of the base.
-    for (size_t k = 0; k < K - 1; k++) {
-      nonlinearFactorGraph_.emplace_shared<BetweenFactor<Pose2>>(
-          X(k), X(k + 1), pims.at(k), measurement_noise);
-    }
-
-    // Forward kinematics from base X to foot L
-    auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0);
-    for (size_t k = 0; k < K; k++) {
-      nonlinearFactorGraph_.emplace_shared<BetweenFactor<Pose2>>(
-          X(k), L(k), fk.at(k), fk_noise);
-    }
-
-    // 2 noise models where moving has a higher covariance.
-    auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-4);
-    auto swing_model = noiseModel::Isotropic::Sigma(3, 1e8);
-
-    // Add "contact models" for the foot.
-    // The idea is that the robot's leg has a tight covariance for stance and
-    // loose covariance for swing.
-    using ContactFactor = BetweenFactor<Pose2>;
-
-    for (size_t k = 0; k < K - 1; k++) {
-      KeyVector keys = {L(k), L(k + 1)};
-      ContactFactor::shared_ptr factor;
-      if (contacts[k] && contacts[k + 1]) {
-        // stance
-        std::cout << "stance 11" << std::endl;
-        factor = boost::make_shared<ContactFactor>(
-            keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model);
-      } else if (contacts[k] && !contacts[k + 1]) {
-        // toe-off
-        std::cout << "toe-off 10" << std::endl;
-        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
-                                                   Pose2(0, 0, 0), swing_model);
-      } else if (!contacts[k] && contacts[k + 1]) {
-        // land
-        std::cout << "land 01" << std::endl;
-        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
-                                                   Pose2(0, 0, 0), swing_model);
-      } else if (!contacts[k] && !contacts[k + 1]) {
-        // swing
-        std::cout << "swing 00" << std::endl;
-        factor = boost::make_shared<ContactFactor>(keys.at(0), keys.at(1),
-                                                   Pose2(0, 0, 0), swing_model);
-      }
-
-      nonlinearFactorGraph_.push_back(factor);
-    }
-
-    // Create the linearization point.
-    for (size_t k = 0; k < K; k++) {
-      linearizationPoint_.insert<Pose2>(X(k), Pose2(k, 2, 0));
-      linearizationPoint_.insert<Pose2>(L(k), Pose2(0, 0, 0));
-    }
-
-    linearizedFactorGraph_ =
-        *nonlinearFactorGraph_.linearize(linearizationPoint_);
-  }
-
-  void print() const {
-    nonlinearFactorGraph_.print();
-    linearizationPoint_.print();
-    linearizedFactorGraph_.print();
-  }
-
-  VectorValues optimize() {
-    bayesNet_ = *linearizedFactorGraph_.eliminateSequential();
-
-    // bayesNet->print();
-    VectorValues delta = bayesNet_.optimize();
-    return delta;
-  }
-
-  Values linearizationPoint() const { return linearizationPoint_; }
-  NonlinearFactorGraph nonlinearFactorGraph() const {
-    return nonlinearFactorGraph_;
-  }
-  GaussianFactorGraph linearizedFactorGraph() const {
-    return linearizedFactorGraph_;
-  }
-  GaussianBayesNet bayesNet() const { return bayesNet_; }
-};
-
-/* ****************************************************************************/
-TEST(Estimation, LR) {
-  std::vector<Pose2> pims = {Pose2(1, 0, 0)};
-  // Leg is in stance throughout
-  // std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)};
-  // Leg is in swing
-  // std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)};
-  // Leg is in toe-off
-  // std::vector<Pose2> fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)};
-  // Leg is in land
-  std::vector<Pose2> fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)};
-
-  vector<bool> contacts;
-  contacts = {1, 1};
-  SL robot11(2, pims, fk, contacts);
-  VectorValues delta = robot11.optimize();
-  // robot11.nonlinearFactorGraph().print();
-  std::cout << "Error with optimized delta: " << robot11.bayesNet().error(delta)
-            << std::endl;
-  robot11.linearizationPoint().retract(delta).print();
-  std::cout << "\n===========================\n\n" << std::endl;
-
-  contacts = {1, 0};
-  SL robot10(2, pims, fk, contacts);
-  delta = robot10.optimize();
-  // robot10.nonlinearFactorGraph().print();
-  std::cout << "Error with optimized delta: " << robot10.bayesNet().error(delta)
-            << std::endl;
-  robot10.linearizationPoint().retract(delta).print();
-  std::cout << "\n===========================\n\n" << std::endl;
-
-  contacts = {0, 1};
-  SL robot01(2, pims, fk, contacts);
-  delta = robot01.optimize();
-  // robot01.nonlinearFactorGraph().print();
-  std::cout << "Error with optimized delta: " << robot01.bayesNet().error(delta)
-            << std::endl;
-  robot01.linearizationPoint().retract(delta).print();
-  std::cout << "\n===========================\n\n" << std::endl;
-
-  contacts = {0, 0};
-  SL robot00(2, pims, fk, contacts);
-  delta = robot00.optimize();
-  // robot00.nonlinearFactorGraph().print();
-  std::cout << "Error with optimized delta: " << robot00.bayesNet().error(delta)
-            << std::endl;
-  robot00.linearizationPoint().retract(delta).print();
-  std::cout << "\n===========================\n\n" << std::endl;
-}
-
-/* ************************************************************************* */
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-/* ************************************************************************* */

From 0faf2226d46ff355e2bf5374d01131d595d3b39e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 13 Oct 2022 11:10:42 -0400
Subject: [PATCH 23/23] remove leftover comment

---
 gtsam/hybrid/GaussianMixture.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 244d527388..5172a97983 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -122,7 +122,6 @@ void GaussianMixture::print(const std::string &s,
         if (gf && !gf->empty()) {
           gf->print("", formatter);
           return rd.str();
-          // return "Node()";
         } else {
           return "nullptr";
         }