From 39286f667214a4f7fd88c224943bfb850d5e2a32 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sun, 19 Dec 2021 10:41:07 -0500
Subject: [PATCH 001/175] added clone to play well with gnc

---
 gtsam_unstable/slam/PoseToPointFactor.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h
index cab48e506..b9b2ad5ce 100644
--- a/gtsam_unstable/slam/PoseToPointFactor.h
+++ b/gtsam_unstable/slam/PoseToPointFactor.h
@@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2<POSE, POINT> {
            traits<POINT>::Equals(this->measured_, e->measured_, tol);
   }
 
+  /// @return a deep copy of this factor
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
   /** implement functions needed to derive from Factor */
 
   /** vector of errors

From c59fbc825fe2f77c96d2e8a1bd7c509c3bf7189c Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Wed, 5 Jan 2022 23:01:57 -0500
Subject: [PATCH 002/175] ruled out corner case where weights are outside [0,1]
 in TLS

---
 gtsam/nonlinear/GncOptimizer.h | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index 3025d2468..9601e5f8c 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer {
         for (size_t k : unknownWeights) {
           if (nfg_[k]) {
             double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
-            if (u2_k >= upperbound) {
+            weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
+            if (u2_k >= upperbound || weights[k] < 0) {
               weights[k] = 0;
-            } else if (u2_k <= lowerbound) {
+            } else if (u2_k <= lowerbound || weights[k] > 1) {
               weights[k] = 1;
-            } else {
-              weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k)
-                  - mu;
             }
           }
         }

From 859e4cb37af125b9351db22dc86837077d5c62f9 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Wed, 12 Jan 2022 21:31:22 -0500
Subject: [PATCH 003/175] thresholded mu to avoid case mu = 0 in TLS. improved
 verbosity handling

---
 gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++------
 gtsam/nonlinear/GncParams.h    |  2 ++
 2 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index 3025d2468..6c8519aac 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -206,9 +206,11 @@ class GTSAM_EXPORT GncOptimizer {
         std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
                   << std::endl;
       }
+      if (params_.verbosity >= GncParameters::Verbosity::MU) {
+        std::cout << "mu: " << mu << std::endl;
+      }
       if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
         result.print("result\n");
-        std::cout << "mu: " << mu << std::endl;
       }
       return result;
     }
@@ -217,12 +219,16 @@ class GTSAM_EXPORT GncOptimizer {
     for (iter = 0; iter < params_.maxIterations; iter++) {
 
       // display info
-      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+      if (params_.verbosity >= GncParameters::Verbosity::MU) {
         std::cout << "iter: " << iter << std::endl;
-        result.print("result\n");
         std::cout << "mu: " << mu << std::endl;
+      }
+      if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
         std::cout << "weights: " << weights_ << std::endl;
       }
+      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+        result.print("result\n");
+      }
       // weights update
       weights_ = calculateWeights(result, mu);
 
@@ -253,10 +259,12 @@ class GTSAM_EXPORT GncOptimizer {
     if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
       std::cout << "final iterations: " << iter << std::endl;
       std::cout << "final mu: " << mu << std::endl;
-      std::cout << "final weights: " << weights_ << std::endl;
       std::cout << "previous cost: " << prev_cost << std::endl;
       std::cout << "current cost: " << cost << std::endl;
     }
+    if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
+      std::cout << "final weights: " << weights_ << std::endl;
+    }
     return result;
   }
 
@@ -291,6 +299,9 @@ class GTSAM_EXPORT GncOptimizer {
                 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
           }
         }
+        if (mu_init >= 0 && mu_init < 1e-6)
+          mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
+        // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
         return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
         // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
         // and there is no need to robustify (TLS = least squares)
@@ -338,8 +349,10 @@ class GTSAM_EXPORT GncOptimizer {
   bool checkCostConvergence(const double cost, const double prev_cost) const {
     bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
         < params_.relativeCostTol;
-    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
-      std::cout << "checkCostConvergence = true " << std::endl;
+    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
+      std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
+                << ", curr. cost = " << cost << ")" << std::endl;
+    }
     return costConverged;
   }
 
diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h
index 086f08acc..1f324ae38 100644
--- a/gtsam/nonlinear/GncParams.h
+++ b/gtsam/nonlinear/GncParams.h
@@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams {
   enum Verbosity {
     SILENT = 0,
     SUMMARY,
+    MU,
+    WEIGHTS,
     VALUES
   };
 

From d9a00ded231ea074cd64bdf5cc87690ea5dc0f0a Mon Sep 17 00:00:00 2001
From: Calvin <magicbycalvin@gmail.com>
Date: Tue, 25 Jan 2022 16:39:05 -0600
Subject: [PATCH 004/175] Modified the scaling values for plotting uncertainty
 ellipses to properly display 3 standard deviations for both 2D and 3D cases.

---
 python/gtsam/utils/plot.py | 42 ++++++++++++++++++++++++--------------
 1 file changed, 27 insertions(+), 15 deletions(-)

diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index a632b852a..8060de2fb 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -75,8 +75,9 @@ def plot_covariance_ellipse_3d(axes,
     Plots a Gaussian as an uncertainty ellipse
 
     Based on Maybeck Vol 1, page 366
-    k=2.296 corresponds to 1 std, 68.26% of all probability
-    k=11.82 corresponds to 3 std, 99.74% of all probability
+    For the 3D case:
+    k = 3.527 corresponds to 1 std, 68.26% of all probability
+    k = 14.157 corresponds to 3 std, 99.74% of all probability
 
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
@@ -87,7 +88,7 @@ def plot_covariance_ellipse_3d(axes,
         n: Defines the granularity of the ellipse. Higher values indicate finer ellipses.
         alpha: Transparency value for the plotted surface in the range [0, 1].
     """
-    k = 11.82
+    k = np.sqrt(14.157)
     U, S, _ = np.linalg.svd(P)
 
     radii = k * np.sqrt(S)
@@ -113,7 +114,14 @@ def plot_point2_on_axes(axes,
                         linespec: str,
                         P: Optional[np.ndarray] = None) -> None:
     """
-    Plot a 2D point on given axis `axes` with given `linespec`.
+    Plot a 2D point and its corresponding uncertainty ellipse on given axis
+    `axes` with given `linespec`.
+
+    Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
+    page 366
+    For the 2D case:
+    k = 2.296 corresponds to 1 std, 68.26% of all probability
+    k = 11.820 corresponds to 3 std, 99.74% of all probability
 
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
@@ -125,16 +133,15 @@ def plot_point2_on_axes(axes,
     if P is not None:
         w, v = np.linalg.eig(P)
 
-        # "Sigma" value for drawing the uncertainty ellipse. 5 sigma corresponds
-        # to a 99.9999% confidence, i.e. assuming the estimation has been
-        # computed properly, there is a 99.999% chance that the true position
-        # of the point will lie within the uncertainty ellipse.
-        k = 5.0
+        # Scaling value for the uncertainty ellipse, we multiply by 2 because
+        # matplotlib takes the diameter and not the radius of the major and
+        # minor axes of the ellipse.
+        k = 2*np.sqrt(11.820)
 
         angle = np.arctan2(v[1, 0], v[0, 0])
         e1 = patches.Ellipse(point,
-                             np.sqrt(w[0] * k),
-                             np.sqrt(w[1] * k),
+                             np.sqrt(w[0]) * k,
+                             np.sqrt(w[1]) * k,
                              np.rad2deg(angle),
                              fill=False)
         axes.add_patch(e1)
@@ -178,6 +185,12 @@ def plot_pose2_on_axes(axes,
     """
     Plot a 2D pose on given axis `axes` with given `axis_length`.
 
+    Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
+    page 366
+    For the 2D case:
+    k = 2.296 corresponds to 1 std, 68.26% of all probability
+    k = 11.820 corresponds to 3 std, 99.74% of all probability
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         pose: The pose to be plotted.
@@ -205,13 +218,12 @@ def plot_pose2_on_axes(axes,
 
         w, v = np.linalg.eig(gPp)
 
-        # k = 2.296
-        k = 5.0
+        k = 2*np.sqrt(11.820)
 
         angle = np.arctan2(v[1, 0], v[0, 0])
         e1 = patches.Ellipse(origin,
-                             np.sqrt(w[0] * k),
-                             np.sqrt(w[1] * k),
+                             np.sqrt(w[0]) * k,
+                             np.sqrt(w[1]) * k,
                              np.rad2deg(angle),
                              fill=False)
         axes.add_patch(e1)

From 1b817760dd10d2d08579f4021968eeda262f34c1 Mon Sep 17 00:00:00 2001
From: Calvin <magicbycalvin@gmail.com>
Date: Fri, 28 Jan 2022 13:31:11 -0600
Subject: [PATCH 005/175] Changed all instances of the Sigma value, k, to 5 for
 plotting the covariance ellipse.

---
 python/gtsam/utils/plot.py | 30 ++++++++++++++++++++----------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index 8060de2fb..820cefb7c 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -79,6 +79,8 @@ def plot_covariance_ellipse_3d(axes,
     k = 3.527 corresponds to 1 std, 68.26% of all probability
     k = 14.157 corresponds to 3 std, 99.74% of all probability
 
+    We choose k = 5 which corresponds to 99.99846% of all probability in 3D
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         origin: The origin in the world frame.
@@ -88,7 +90,8 @@ def plot_covariance_ellipse_3d(axes,
         n: Defines the granularity of the ellipse. Higher values indicate finer ellipses.
         alpha: Transparency value for the plotted surface in the range [0, 1].
     """
-    k = np.sqrt(14.157)
+    # Sigma value corresponding to the covariance ellipse
+    k = 5
     U, S, _ = np.linalg.svd(P)
 
     radii = k * np.sqrt(S)
@@ -123,6 +126,8 @@ def plot_point2_on_axes(axes,
     k = 2.296 corresponds to 1 std, 68.26% of all probability
     k = 11.820 corresponds to 3 std, 99.74% of all probability
 
+    We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         point: The point to be plotted.
@@ -133,15 +138,15 @@ def plot_point2_on_axes(axes,
     if P is not None:
         w, v = np.linalg.eig(P)
 
-        # Scaling value for the uncertainty ellipse, we multiply by 2 because
-        # matplotlib takes the diameter and not the radius of the major and
-        # minor axes of the ellipse.
-        k = 2*np.sqrt(11.820)
+        # Sigma value corresponding to the covariance ellipse
+        k = 5
 
         angle = np.arctan2(v[1, 0], v[0, 0])
+        # We multiply k by 2 since k corresponds to the radius but Ellipse uses
+        # the diameter.
         e1 = patches.Ellipse(point,
-                             np.sqrt(w[0]) * k,
-                             np.sqrt(w[1]) * k,
+                             np.sqrt(w[0]) * 2 * k,
+                             np.sqrt(w[1]) * 2 * k,
                              np.rad2deg(angle),
                              fill=False)
         axes.add_patch(e1)
@@ -191,6 +196,8 @@ def plot_pose2_on_axes(axes,
     k = 2.296 corresponds to 1 std, 68.26% of all probability
     k = 11.820 corresponds to 3 std, 99.74% of all probability
 
+    We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         pose: The pose to be plotted.
@@ -218,12 +225,15 @@ def plot_pose2_on_axes(axes,
 
         w, v = np.linalg.eig(gPp)
 
-        k = 2*np.sqrt(11.820)
+        # Sigma value corresponding to the covariance ellipse
+        k = 5
 
         angle = np.arctan2(v[1, 0], v[0, 0])
+        # We multiply k by 2 since k corresponds to the radius but Ellipse uses
+        # the diameter.
         e1 = patches.Ellipse(origin,
-                             np.sqrt(w[0]) * k,
-                             np.sqrt(w[1]) * k,
+                             np.sqrt(w[0]) * 2 * k,
+                             np.sqrt(w[1]) * 2 * k,
                              np.rad2deg(angle),
                              fill=False)
         axes.add_patch(e1)

From e524e2806b6a341053a7ab4a425a5fe0a4afe72e Mon Sep 17 00:00:00 2001
From: Calvin <magicbycalvin@gmail.com>
Date: Fri, 28 Jan 2022 14:16:30 -0600
Subject: [PATCH 006/175] Updated comments to reflect standard deviations
 instead of variances in the discussions regarding the values for k.

---
 python/gtsam/utils/plot.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index 820cefb7c..f409968a8 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -76,8 +76,8 @@ def plot_covariance_ellipse_3d(axes,
 
     Based on Maybeck Vol 1, page 366
     For the 3D case:
-    k = 3.527 corresponds to 1 std, 68.26% of all probability
-    k = 14.157 corresponds to 3 std, 99.74% of all probability
+    k = 1.878 corresponds to 1 std, 68.26% of all probability
+    k = 3.763 corresponds to 3 std, 99.74% of all probability
 
     We choose k = 5 which corresponds to 99.99846% of all probability in 3D
 
@@ -123,8 +123,8 @@ def plot_point2_on_axes(axes,
     Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
     page 366
     For the 2D case:
-    k = 2.296 corresponds to 1 std, 68.26% of all probability
-    k = 11.820 corresponds to 3 std, 99.74% of all probability
+    k = 1.515 corresponds to 1 std, 68.26% of all probability
+    k = 3.438 corresponds to 3 std, 99.74% of all probability
 
     We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
 
@@ -193,8 +193,8 @@ def plot_pose2_on_axes(axes,
     Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
     page 366
     For the 2D case:
-    k = 2.296 corresponds to 1 std, 68.26% of all probability
-    k = 11.820 corresponds to 3 std, 99.74% of all probability
+    k = 1.515 corresponds to 1 std, 68.26% of all probability
+    k = 3.438 corresponds to 3 std, 99.74% of all probability
 
     We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
 

From bf8137b0c404997ba6d1678e0b733e3347b0f007 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 8 Mar 2022 09:48:05 -0500
Subject: [PATCH 007/175] refactor python NonlinearOptimizerTest

---
 python/gtsam/tests/test_NonlinearOptimizer.py | 78 +++++++++----------
 1 file changed, 38 insertions(+), 40 deletions(-)

diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py
index e2561ae52..2158e10bb 100644
--- a/python/gtsam/tests/test_NonlinearOptimizer.py
+++ b/python/gtsam/tests/test_NonlinearOptimizer.py
@@ -15,12 +15,10 @@ from __future__ import print_function
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams,
-                   DummyPreconditionerParameters, GaussNewtonOptimizer,
-                   GaussNewtonParams, GncLMParams, GncLMOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
-                   NonlinearFactorGraph, Ordering,
-                   PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
+                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
+                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
+                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -28,62 +26,62 @@ KEY2 = 2
 
 
 class TestScenario(GtsamTestCase):
-    def test_optimize(self):
-        """Do trivial test with three optimizer variants."""
-        fg = NonlinearFactorGraph()
+    """Do trivial test with three optimizer variants."""
+
+    def setUp(self):
+        """Set up the optimization problem and ordering"""
+        # create graph
+        self.fg = NonlinearFactorGraph()
         model = gtsam.noiseModel.Unit.Create(2)
-        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+        self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
 
         # test error at minimum
         xstar = Point2(0, 0)
-        optimal_values = Values()
-        optimal_values.insert(KEY1, xstar)
-        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+        self.optimal_values = Values()
+        self.optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
 
         # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
         x0 = Point2(3, 3)
-        initial_values = Values()
-        initial_values.insert(KEY1, x0)
-        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+        self.initial_values = Values()
+        self.initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
 
         # optimize parameters
-        ordering = Ordering()
-        ordering.push_back(KEY1)
+        self.ordering = Ordering()
+        self.ordering.push_back(KEY1)
 
-        # Gauss-Newton
+    def test_gauss_newton(self):
         gnParams = GaussNewtonParams()
-        gnParams.setOrdering(ordering)
-        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual1))
+        gnParams.setOrdering(self.ordering)
+        actual = GaussNewtonOptimizer(self.fg, self.initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Levenberg-Marquardt
+    def test_levenberg_marquardt(self):
         lmParams = LevenbergMarquardtParams.CeresDefaults()
-        lmParams.setOrdering(ordering)
-        actual2 = LevenbergMarquardtOptimizer(
-            fg, initial_values, lmParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual2))
+        lmParams.setOrdering(self.ordering)
+        actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Levenberg-Marquardt
+    def test_levenberg_marquardt_pcg(self):
         lmParams = LevenbergMarquardtParams.CeresDefaults()
         lmParams.setLinearSolverType("ITERATIVE")
         cgParams = PCGSolverParameters()
         cgParams.setPreconditionerParams(DummyPreconditionerParameters())
         lmParams.setIterativeParams(cgParams)
-        actual2 = LevenbergMarquardtOptimizer(
-            fg, initial_values, lmParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual2))
+        actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
-        # Dogleg
+    def test_dogleg(self):
         dlParams = DoglegParams()
-        dlParams.setOrdering(ordering)
-        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual3))
-        
-        # Graduated Non-Convexity (GNC)
+        dlParams.setOrdering(self.ordering)
+        actual = DoglegOptimizer(self.fg, self.initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
+
+    def test_graduated_non_convexity(self):
         gncParams = GncLMParams()
-        actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize()
-        self.assertAlmostEqual(0, fg.error(actual4))
-        
+        actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
 
 
 if __name__ == "__main__":

From 5f601f76d5eb22434f8681155f5d492fba840401 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 8 Mar 2022 09:57:11 -0500
Subject: [PATCH 008/175] add iteration hook to NonlinearOptimizerParams

---
 gtsam/nonlinear/nonlinear.i                   |  2 ++
 python/gtsam/tests/test_NonlinearOptimizer.py | 20 +++++++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index da5e8b29c..7d2911374 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -403,6 +403,8 @@ virtual class NonlinearOptimizerParams {
   bool isSequential() const;
   bool isCholmod() const;
   bool isIterative() const;
+
+  gtsam::NonlinearOptimizerParams::IterationHook iterationHook;
 };
 
 bool checkConvergence(double relativeErrorTreshold,
diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py
index 2158e10bb..37afd9e1c 100644
--- a/python/gtsam/tests/test_NonlinearOptimizer.py
+++ b/python/gtsam/tests/test_NonlinearOptimizer.py
@@ -83,6 +83,26 @@ class TestScenario(GtsamTestCase):
         actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
         self.assertAlmostEqual(0, self.fg.error(actual))
 
+    def test_iteration_hook(self):
+        # set up iteration hook to track some testable values
+        iteration_count = 0
+        final_error = 0
+        final_values = None
+        def iteration_hook(iter, error_before, error_after):
+            nonlocal iteration_count, final_error, final_values
+            iteration_count = iter
+            final_error = error_after
+            final_values = optimizer.values()
+        # optimize
+        params = LevenbergMarquardtParams.CeresDefaults()
+        params.setOrdering(self.ordering)
+        params.iterationHook = iteration_hook
+        optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values, params)
+        actual = optimizer.optimize()
+        self.assertAlmostEqual(0, self.fg.error(actual))
+        self.gtsamAssertEquals(final_values, actual)
+        self.assertEqual(self.fg.error(actual), final_error)
+        self.assertEqual(optimizer.iterations(), iteration_count)
 
 if __name__ == "__main__":
     unittest.main()

From ddca736c7bd3f2cf2f0eaae9925fbb4a3d6a7cc7 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 8 Mar 2022 10:13:48 -0500
Subject: [PATCH 009/175] add comment about matlab not having iteration hook

---
 gtsam/nonlinear/nonlinear.i | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 7d2911374..e227de37c 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -404,6 +404,7 @@ virtual class NonlinearOptimizerParams {
   bool isCholmod() const;
   bool isIterative() const;
 
+  // This only applies to python since matlab does not have lambda machinery.
   gtsam::NonlinearOptimizerParams::IterationHook iterationHook;
 };
 

From ea4ebb6843daec9cd42bb7579e431d6686d1d8f7 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 11 Mar 2022 15:51:34 -0500
Subject: [PATCH 010/175] Fix typo

---
 gtsam/inference/EliminateableFactorGraph.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h
index c904d2f7f..900346f7f 100644
--- a/gtsam/inference/EliminateableFactorGraph.h
+++ b/gtsam/inference/EliminateableFactorGraph.h
@@ -204,7 +204,7 @@ namespace gtsam {
       OptionalVariableIndex variableIndex = boost::none) const;
 
     /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
-     *  produce a Bayes net and a remaining factor graph.  This computes the factorization \f$ p(X)
+     *  produce a Bayes tree and a remaining factor graph.  This computes the factorization \f$ p(X)
      *  = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
      *  factor graph, and \f$ B = X\backslash A \f$. */
     std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >

From 0567303ed3334508cf5ab0c19903b22a5d3fd9e1 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 11 Mar 2022 15:52:28 -0500
Subject: [PATCH 011/175] Prototype a type-erased hybrid machinery

---
 gtsam/CMakeLists.txt                         |  1 +
 gtsam/hybrid/CGMixtureFactor.cpp             |  5 +
 gtsam/hybrid/CGMixtureFactor.h               | 23 +++++
 gtsam/hybrid/CMakeLists.txt                  |  8 ++
 gtsam/hybrid/HybridBayesNet.cpp              | 16 ++++
 gtsam/hybrid/HybridBayesNet.h                | 43 +++++++++
 gtsam/hybrid/HybridBayesTree.cpp             | 38 ++++++++
 gtsam/hybrid/HybridBayesTree.h               | 77 +++++++++++++++
 gtsam/hybrid/HybridConditional.cpp           | 23 +++++
 gtsam/hybrid/HybridConditional.h             | 99 ++++++++++++++++++++
 gtsam/hybrid/HybridDiscreteFactor.cpp        | 19 ++++
 gtsam/hybrid/HybridDiscreteFactor.h          | 34 +++++++
 gtsam/hybrid/HybridEliminationTree.cpp       | 43 +++++++++
 gtsam/hybrid/HybridEliminationTree.h         | 62 ++++++++++++
 gtsam/hybrid/HybridFactor.cpp                | 18 ++++
 gtsam/hybrid/HybridFactor.h                  | 83 ++++++++++++++++
 gtsam/hybrid/HybridFactorGraph.cpp           | 58 ++++++++++++
 gtsam/hybrid/HybridFactorGraph.h             | 83 ++++++++++++++++
 gtsam/hybrid/HybridGaussianFactor.cpp        | 23 +++++
 gtsam/hybrid/HybridGaussianFactor.h          | 39 ++++++++
 gtsam/hybrid/HybridJunctionTree.cpp          | 34 +++++++
 gtsam/hybrid/HybridJunctionTree.h            | 67 +++++++++++++
 gtsam/hybrid/tests/CMakeLists.txt            |  1 +
 gtsam/hybrid/tests/testHybridConditional.cpp | 73 +++++++++++++++
 24 files changed, 970 insertions(+)
 create mode 100644 gtsam/hybrid/CGMixtureFactor.cpp
 create mode 100644 gtsam/hybrid/CGMixtureFactor.h
 create mode 100644 gtsam/hybrid/CMakeLists.txt
 create mode 100644 gtsam/hybrid/HybridBayesNet.cpp
 create mode 100644 gtsam/hybrid/HybridBayesNet.h
 create mode 100644 gtsam/hybrid/HybridBayesTree.cpp
 create mode 100644 gtsam/hybrid/HybridBayesTree.h
 create mode 100644 gtsam/hybrid/HybridConditional.cpp
 create mode 100644 gtsam/hybrid/HybridConditional.h
 create mode 100644 gtsam/hybrid/HybridDiscreteFactor.cpp
 create mode 100644 gtsam/hybrid/HybridDiscreteFactor.h
 create mode 100644 gtsam/hybrid/HybridEliminationTree.cpp
 create mode 100644 gtsam/hybrid/HybridEliminationTree.h
 create mode 100644 gtsam/hybrid/HybridFactor.cpp
 create mode 100644 gtsam/hybrid/HybridFactor.h
 create mode 100644 gtsam/hybrid/HybridFactorGraph.cpp
 create mode 100644 gtsam/hybrid/HybridFactorGraph.h
 create mode 100644 gtsam/hybrid/HybridGaussianFactor.cpp
 create mode 100644 gtsam/hybrid/HybridGaussianFactor.h
 create mode 100644 gtsam/hybrid/HybridJunctionTree.cpp
 create mode 100644 gtsam/hybrid/HybridJunctionTree.h
 create mode 100644 gtsam/hybrid/tests/CMakeLists.txt
 create mode 100644 gtsam/hybrid/tests/testHybridConditional.cpp

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index a293c6ec2..845ac7cd0 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -10,6 +10,7 @@ set (gtsam_subdirs
     inference
     symbolic
     discrete
+	hybrid
     linear
     nonlinear
     sam
diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp
new file mode 100644
index 000000000..d789825f7
--- /dev/null
+++ b/gtsam/hybrid/CGMixtureFactor.cpp
@@ -0,0 +1,5 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include "CGMixtureFactor.h"
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
new file mode 100644
index 000000000..f2fa1e54a
--- /dev/null
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -0,0 +1,23 @@
+/* ----------------------------------------------------------------------------
+ * Copyright 2021 The Ambitious Folks of the MRG
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   CGMixtureFactor.h
+ * @brief  Custom hybrid factor graph for discrete + continuous factors
+ * @author Kevin Doherty, kdoherty@mit.edu
+ * @author Varun Agrawal
+ * @author Fan Jiang
+ * @date   December 2021
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class CGMixtureFactor : HybridFactor {
+
+};
+
+}
diff --git a/gtsam/hybrid/CMakeLists.txt b/gtsam/hybrid/CMakeLists.txt
new file mode 100644
index 000000000..f1cfcd5c4
--- /dev/null
+++ b/gtsam/hybrid/CMakeLists.txt
@@ -0,0 +1,8 @@
+# Install headers
+set(subdir hybrid)
+file(GLOB hybrid_headers "*.h")
+# FIXME: exclude headers
+install(FILES ${hybrid_headers} DESTINATION include/gtsam/hybrid)
+
+# Add all tests
+add_subdirectory(tests)
diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
new file mode 100644
index 000000000..1292711d8
--- /dev/null
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -0,0 +1,16 @@
+/* ----------------------------------------------------------------------------
+ * 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   HybridBayesNet.cpp
+ * @brief  A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author Fan Jiang
+ * @date   January 2022
+ */
+
+#include <gtsam/hybrid/HybridBayesNet.h>
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
new file mode 100644
index 000000000..53d10518f
--- /dev/null
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+ * 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    HybridBayesNet.h
+ * @brief   A bayes net of Gaussian Conditionals indexed by discrete keys.
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <iostream>  // TODO!
+
+namespace gtsam {
+
+/**
+ * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures,
+ * or pure Gaussian conditionals.
+ */
+class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
+ public:
+  using Base = BayesNet<HybridConditional>;
+  using This = HybridBayesNet;
+  using ConditionalType = HybridConditional;
+  using shared_ptr = boost::shared_ptr<HybridBayesNet>;
+  using sharedConditional = boost::shared_ptr<ConditionalType>;
+
+  /** Construct empty bayes net */
+  HybridBayesNet() : Base() {}
+};
+
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
new file mode 100644
index 000000000..72f3fd794
--- /dev/null
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HybridBayesTree.cpp
+ * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
+ * @brief   HybridBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/inference/BayesTree-inst.h>
+#include <gtsam/inference/BayesTreeCliqueBase-inst.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+
+namespace gtsam {
+
+// Instantiate base class
+template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>;
+template class BayesTree<HybridBayesTreeClique>;
+
+/* ************************************************************************* */
+bool HybridBayesTree::equals(const This& other, double tol) const {
+  return Base::equals(other, tol);
+}
+
+/* **************************************************************************/
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
new file mode 100644
index 000000000..2ea40cecc
--- /dev/null
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -0,0 +1,77 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    HybridBayesTree.h
+ * @brief   Hybrid Bayes Tree, the result of eliminating a
+ * HybridJunctionTree
+ * @brief   HybridBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/BayesTree.h>
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/inference/BayesTreeCliqueBase.h>
+
+#include <string>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridConditional;
+class VectorValues;
+
+/* ************************************************************************* */
+/** A clique in a HybridBayesTree */
+class GTSAM_EXPORT HybridBayesTreeClique
+    : public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
+ public:
+  typedef HybridBayesTreeClique This;
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>
+      Base;
+  typedef boost::shared_ptr<This> shared_ptr;
+  typedef boost::weak_ptr<This> weak_ptr;
+  HybridBayesTreeClique() {}
+  virtual ~HybridBayesTreeClique() {}
+  HybridBayesTreeClique(
+      const boost::shared_ptr<HybridConditional>& conditional)
+      : Base(conditional) {}
+
+};
+
+/* ************************************************************************* */
+/** A Bayes tree representing a Hybrid density */
+class GTSAM_EXPORT HybridBayesTree
+    : public BayesTree<HybridBayesTreeClique> {
+ private:
+  typedef BayesTree<HybridBayesTreeClique> Base;
+
+ public:
+  typedef HybridBayesTree This;
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// @name Standard interface
+  /// @{
+  /** Default constructor, creates an empty Bayes tree */
+  HybridBayesTree() = default;
+
+  /** Check equality */
+  bool equals(const This& other, double tol = 1e-9) const;
+
+  /// @}
+};
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
new file mode 100644
index 000000000..e6b2eb47f
--- /dev/null
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -0,0 +1,23 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/inference/Conditional-inst.h>
+
+namespace gtsam {
+void HybridConditional::print(const std::string &s,
+                                     const KeyFormatter &formatter) const {
+  Conditional::print(s, formatter);
+}
+
+bool HybridConditional::equals(const HybridFactor &other,
+                                      double tol) const {
+  return false;
+}
+
+HybridConditional HybridConditional::operator*(const HybridConditional &other) const {
+  return {};
+}
+}
+
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
new file mode 100644
index 000000000..d6dd8250f
--- /dev/null
+++ b/gtsam/hybrid/HybridConditional.h
@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridConditional.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/inference/Conditional.h>
+
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <string>
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * Hybrid Conditional Density
+ *
+ * As a type-erased variant of:
+ * - DiscreteConditional
+ * - GaussianMixture
+ * - GaussianConditional
+ */
+class GTSAM_EXPORT HybridConditional
+: public HybridFactor,
+public Conditional<HybridFactor, HybridConditional> {
+public:
+// typedefs needed to play nice with gtsam
+typedef HybridConditional This;            ///< Typedef to this class
+typedef boost::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
+typedef HybridFactor BaseFactor;  ///< Typedef to our factor base class
+typedef Conditional<BaseFactor, This>
+    BaseConditional;  ///< Typedef to our conditional base class
+
+private:
+// Type-erased pointer to the inner type
+std::unique_ptr<Factor> inner;
+
+public:
+/// @name Standard Constructors
+/// @{
+
+/// Default constructor needed for serialization.
+HybridConditional() = default;
+
+/**
+ * @brief Combine two conditionals, yielding a new conditional with the union
+ * of the frontal keys, ordered by gtsam::Key.
+ *
+ * The two conditionals must make a valid Bayes net fragment, i.e.,
+ * the frontal variables cannot overlap, and must be acyclic:
+ * Example of correct use:
+ *   P(A,B) = P(A|B) * P(B)
+ *   P(A,B|C) = P(A|B) * P(B|C)
+ *   P(A,B,C) = P(A,B|C) * P(C)
+ * Example of incorrect use:
+ *   P(A|B) * P(A|C) = ?
+ *   P(A|B) * P(B|A) = ?
+ * We check for overlapping frontals, but do *not* check for cyclic.
+ */
+HybridConditional operator*(const HybridConditional& other) const;
+
+/// @}
+/// @name Testable
+/// @{
+
+/// GTSAM-style print
+void print(
+    const std::string& s = "Hybrid Conditional: ",
+    const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+
+/// GTSAM-style equals
+bool equals(const HybridFactor& other, double tol = 1e-9) const override;
+
+/// @}
+};
+// DiscreteConditional
+
+// traits
+template <>
+struct traits<HybridConditional> : public Testable<DiscreteConditional> {};
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
new file mode 100644
index 000000000..fded0a2df
--- /dev/null
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -0,0 +1,19 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) {
+  inner = other;
+}
+
+HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
+
+};
+
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
new file mode 100644
index 000000000..4b1c00672
--- /dev/null
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridDiscreteFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class HybridDiscreteFactor : public HybridFactor {
+ public:
+  DiscreteFactor::shared_ptr inner;
+
+  // Implicit conversion from a shared ptr of GF
+  HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete JacobianFactor
+  HybridDiscreteFactor(DecisionTreeFactor &&dtf);
+};
+}
diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp
new file mode 100644
index 000000000..ff106095a
--- /dev/null
+++ b/gtsam/hybrid/HybridEliminationTree.cpp
@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridEliminationTree.cpp
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#include <gtsam/inference/EliminationTree-inst.h>
+#include <gtsam/hybrid/HybridEliminationTree.h>
+
+namespace gtsam {
+
+// Instantiate base class
+template class EliminationTree<HybridBayesNet, HybridFactorGraph>;
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridFactorGraph& factorGraph, const VariableIndex& structure,
+    const Ordering& order) :
+    Base(factorGraph, structure, order) {}
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridFactorGraph& factorGraph, const Ordering& order) :
+    Base(factorGraph, order) {}
+
+/* ************************************************************************* */
+bool HybridEliminationTree::equals(const This& other, double tol) const
+{
+  return Base::equals(other, tol);
+}
+
+}
diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
new file mode 100644
index 000000000..e218ce9f6
--- /dev/null
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -0,0 +1,62 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridEliminationTree.h
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/EliminationTree.h>
+
+namespace gtsam {
+
+class GTSAM_EXPORT HybridEliminationTree :
+    public EliminationTree<HybridBayesNet, HybridFactorGraph>
+{
+ public:
+  typedef EliminationTree<HybridBayesNet, HybridFactorGraph> Base; ///< Base class
+  typedef HybridEliminationTree This; ///< This class
+  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+
+  /**
+  * Build the elimination tree of a factor graph using pre-computed column structure.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  * @param structure The set of factors involving each variable.  If this is not
+  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+  * named constructor instead.
+  * @return The elimination tree
+  */
+  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+                          const VariableIndex& structure, const Ordering& order);
+
+  /** Build the elimination tree of a factor graph.  Note that this has to compute the column
+  * structure as a VariableIndex, so if you already have this precomputed, use the other
+  * constructor instead.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  */
+  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+                          const Ordering& order);
+
+  /** Test whether the tree is equal to another */
+  bool equals(const This& other, double tol = 1e-9) const;
+
+ private:
+
+  friend class ::EliminationTreeTester;
+
+};
+
+}
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
new file mode 100644
index 000000000..907350e83
--- /dev/null
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -0,0 +1,18 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridFactor.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
new file mode 100644
index 000000000..5af1a23a9
--- /dev/null
+++ b/gtsam/hybrid/HybridFactor.h
@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Factor.h>
+#include <gtsam/base/Testable.h>
+
+#include <string>
+namespace gtsam {
+
+/**
+ * Base class for hybrid probabilistic factors
+ */
+class GTSAM_EXPORT HybridFactor: public Factor {
+
+public:
+
+// typedefs needed to play nice with gtsam
+typedef HybridFactor This; ///< This class
+typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
+typedef Factor Base; ///< Our base class
+
+using Values = Values; ///< backwards compatibility
+
+public:
+
+/// @name Standard Constructors
+/// @{
+
+/** Default constructor creates empty factor */
+HybridFactor() {}
+
+/** Construct from container of keys.  This constructor is used internally from derived factor
+ *  constructors, either from a container of keys or from a boost::assign::list_of. */
+template<typename CONTAINER>
+HybridFactor(const CONTAINER& keys) : Base(keys) {}
+
+/// Virtual destructor
+virtual ~HybridFactor() {
+}
+
+/// @}
+/// @name Testable
+/// @{
+
+/// equals
+virtual bool equals(const HybridFactor& lf, double tol = 1e-9) const = 0;
+
+/// print
+void print(
+    const std::string& s = "HybridFactor\n",
+    const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+Base::print(s, formatter);
+}
+
+/// @}
+/// @name Standard Interface
+/// @{
+
+/// @}
+};
+// HybridFactor
+
+// traits
+template<> struct traits<HybridFactor> : public Testable<HybridFactor> {};
+
+}// namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
new file mode 100644
index 000000000..686fddc51
--- /dev/null
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -0,0 +1,58 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+
+#include <gtsam/inference/EliminateableFactorGraph-inst.h>
+
+namespace gtsam {
+
+template class EliminateableFactorGraph<HybridFactorGraph>;
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
+EliminateHybrid(const HybridFactorGraph& factors,
+                  const Ordering& frontalKeys) {
+  // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
+  // few cases: continuous variable, we make a GM if there are hybrid factors;
+  // continuous variable, we make a GF if there are no hybrid factors;
+  // discrete variable, no continuous factor is allowed (escapes CG regime), so
+  // we panic, if discrete only we do the discrete elimination.
+
+  // The issue here is that, how can we know which variable is discrete if we
+  // unify Values? Obviously we can tell using the factors, but is that fast?
+
+  // PRODUCT: multiply all factors
+  gttic(product);
+  HybridGaussianFactor product(JacobianFactor(0, I_3x3, Z_3x1));
+//  for (auto&& factor : factors) product = (*factor) * product;
+  gttoc(product);
+
+  // sum out frontals, this is the factor on the separator
+  gttic(sum);
+//  HybridFactor::shared_ptr sum = product.sum(frontalKeys);
+  gttoc(sum);
+
+  // Ordering keys for the conditional so that frontalKeys are really in front
+//  Ordering orderedKeys;
+//  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
+//                     frontalKeys.end());
+//  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(),
+//                     sum->keys().end());
+
+  // now divide product/sum to get conditional
+  gttic(divide);
+//  auto conditional =
+//      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
+  gttoc(divide);
+
+//  return std::make_pair(conditional, sum);
+  return std::make_pair(boost::make_shared<HybridConditional>(), boost::make_shared<HybridGaussianFactor>(product));
+}
+
+}
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
new file mode 100644
index 000000000..fb0de644d
--- /dev/null
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   HybridFactorGraph.h
+ * @brief  Hybrid factor graph that uses type erasure
+ * @author Fan Jiang
+ * @date   Mar 11, 2022
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/EliminateableFactorGraph.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridFactorGraph;
+class HybridConditional;
+class HybridBayesNet;
+class HybridEliminationTree;
+class HybridBayesTree;
+class HybridJunctionTree;
+
+/** Main elimination function for HybridFactorGraph */
+GTSAM_EXPORT std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
+EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
+
+/* ************************************************************************* */
+template<> struct EliminationTraits<HybridFactorGraph>
+{
+  typedef HybridFactor FactorType;                   ///< Type of factors in factor graph
+  typedef HybridFactorGraph FactorGraphType;         ///< Type of the factor graph (e.g. HybridFactorGraph)
+  typedef HybridConditional ConditionalType;         ///< Type of conditionals from elimination
+  typedef HybridBayesNet BayesNetType;               ///< Type of Bayes net from sequential elimination
+  typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree
+  typedef HybridBayesTree BayesTreeType;             ///< Type of Bayes tree
+  typedef HybridJunctionTree JunctionTreeType;       ///< Type of Junction tree
+  /// The default dense elimination function
+  static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
+  DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
+    return EliminateHybrid(factors, keys); }
+};
+
+
+class HybridFactorGraph : public FactorGraph<HybridFactor>, public EliminateableFactorGraph<HybridFactorGraph> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridFactorGraph;          ///< this class
+  using BaseEliminateable =
+  EliminateableFactorGraph<This>;          ///< for elimination
+  using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
+
+  using Values = gtsam::Values;  ///< backwards compatibility
+  using Indices = KeyVector;  ///> map from keys to values
+
+ public:
+  HybridFactorGraph() = default;
+
+  /** Construct from container of factors (shared_ptr or plain objects) */
+  template <class CONTAINER>
+  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+
+  /** Implicit copy/downcast constructor to override explicit template container
+   * constructor */
+  template <class DERIVEDFACTOR>
+  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+
+};
+
+}
+
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
new file mode 100644
index 000000000..1910c3307
--- /dev/null
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -0,0 +1,23 @@
+//
+// Created by Fan Jiang on 3/11/22.
+//
+
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){
+  inner = other;
+}
+
+HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()),  inner(boost::make_shared<JacobianFactor>(std::move(jf))) {
+
+}
+
+bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
+  return false;
+};
+
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
new file mode 100644
index 000000000..e40682dc1
--- /dev/null
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -0,0 +1,39 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridGaussianFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+class HybridGaussianFactor : public HybridFactor {
+ public:
+  using Base = HybridFactor;
+
+  GaussianFactor::shared_ptr inner;
+
+  // Implicit conversion from a shared ptr of GF
+  HybridGaussianFactor(GaussianFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete JacobianFactor
+  HybridGaussianFactor(JacobianFactor &&jf);
+
+ public:
+  virtual bool equals(const HybridFactor& lf, double tol) const override;
+};
+}
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
new file mode 100644
index 000000000..eabfc2e7c
--- /dev/null
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -0,0 +1,34 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridJunctionTree.cpp
+ * @date Mar 29, 2013
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#include <gtsam/inference/JunctionTree-inst.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridEliminationTree.h>
+
+namespace gtsam {
+
+// Instantiate base classes
+template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
+template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
+
+/* ************************************************************************* */
+HybridJunctionTree::HybridJunctionTree(
+    const HybridEliminationTree& eliminationTree) :
+    Base(eliminationTree) {}
+
+}
diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h
new file mode 100644
index 000000000..1901e7007
--- /dev/null
+++ b/gtsam/hybrid/HybridJunctionTree.h
@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridJunctionTree.h
+ * @date Mar 11, 2022
+ * @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/inference/JunctionTree.h>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridEliminationTree;
+
+/**
+ * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,
+ * with the additional property that it represents the clique tree associated with a Bayes net.
+ *
+ * In GTSAM a junction tree is an intermediate data structure in multifrontal
+ * variable elimination.  Each node is a cluster of factors, along with a
+ * clique of variables that are eliminated all at once. In detail, every node k represents
+ * a clique (maximal fully connected subset) of an associated chordal graph, such as a
+ * chordal Bayes net resulting from elimination.
+ *
+ * The difference with the BayesTree is that a JunctionTree stores factors, whereas a
+ * BayesTree stores conditionals, that are the product of eliminating the factors in the
+ * corresponding JunctionTree cliques.
+ *
+ * The tree structure and elimination method are exactly analogous to the EliminationTree,
+ * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
+ *
+ * \addtogroup Multifrontal
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT HybridJunctionTree :
+    public JunctionTree<HybridBayesTree, HybridFactorGraph> {
+ public:
+  typedef JunctionTree<HybridBayesTree, HybridFactorGraph> Base; ///< Base class
+  typedef HybridJunctionTree This; ///< This class
+  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+
+  /**
+  * Build the elimination tree of a factor graph using precomputed column structure.
+  * @param factorGraph The factor graph for which to build the elimination tree
+  * @param structure The set of factors involving each variable.  If this is not
+  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+  * named constructor instead.
+  * @return The elimination tree
+  */
+  HybridJunctionTree(const HybridEliminationTree& eliminationTree);
+};
+
+}
diff --git a/gtsam/hybrid/tests/CMakeLists.txt b/gtsam/hybrid/tests/CMakeLists.txt
new file mode 100644
index 000000000..b52e18586
--- /dev/null
+++ b/gtsam/hybrid/tests/CMakeLists.txt
@@ -0,0 +1 @@
+gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam")
\ No newline at end of file
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
new file mode 100644
index 000000000..295d6011d
--- /dev/null
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -0,0 +1,73 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testHybridConditional.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/inference/BayesNet.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/map.hpp>
+using namespace boost::assign;
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST_UNSAFE(HybridFactorGraph, test) {
+  HybridConditional test;
+  GTSAM_PRINT(test);
+
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+  GTSAM_PRINT(hfg);
+}
+
+TEST_UNSAFE(HybridFactorGraph, eliminate) {
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  auto result = hfg.eliminatePartialSequential({0});
+
+  GTSAM_PRINT(*result.first);
+}
+
+TEST(HybridFactorGraph, eliminateMultifrontal) {
+  HybridFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  auto result = hfg.eliminatePartialMultifrontal({0});
+
+  GTSAM_PRINT(*result.first);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

From efa37ff315fb0ad9f837e2bb8a589afcd2cb9256 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 11 Mar 2022 19:20:39 -0500
Subject: [PATCH 012/175] Add better mocking for actual elimination

---
 gtsam/hybrid/HybridBayesTree.cpp             |  5 ++-
 gtsam/hybrid/HybridBayesTree.h               |  5 ++-
 gtsam/hybrid/HybridConditional.h             |  4 +++
 gtsam/hybrid/HybridDiscreteFactor.cpp        | 11 ++++--
 gtsam/hybrid/HybridDiscreteFactor.h          |  5 +++
 gtsam/hybrid/HybridFactorGraph.cpp           | 37 ++++++++++++++------
 gtsam/hybrid/HybridGaussianFactor.cpp        |  2 +-
 gtsam/hybrid/HybridJunctionTree.cpp          |  5 ++-
 gtsam/hybrid/tests/testHybridConditional.cpp |  5 +++
 9 files changed, 57 insertions(+), 22 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 72f3fd794..85e6c84ae 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -12,9 +12,8 @@
 /**
  * @file    HybridBayesTree.cpp
  * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
- * @brief   HybridBayesTree
- * @author  Frank Dellaert
- * @author  Richard Roberts
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
  */
 
 #include <gtsam/base/treeTraversal-inst.h>
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 2ea40cecc..06df66112 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -13,9 +13,8 @@
  * @file    HybridBayesTree.h
  * @brief   Hybrid Bayes Tree, the result of eliminating a
  * HybridJunctionTree
- * @brief   HybridBayesTree
- * @author  Frank Dellaert
- * @author  Richard Roberts
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
  */
 
 #pragma once
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index d6dd8250f..e9aa5f616 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -59,6 +59,10 @@ public:
 /// Default constructor needed for serialization.
 HybridConditional() = default;
 
+HybridConditional(size_t nFrontals, const KeyVector& keys) : BaseFactor(keys), BaseConditional(nFrontals) {
+
+}
+
 /**
  * @brief Combine two conditionals, yielding a new conditional with the union
  * of the frontal keys, ordered by gtsam::Key.
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index fded0a2df..0bd2c9a64 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -8,12 +8,19 @@
 
 namespace gtsam {
 
-HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other) {
+HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
+    : Base(other->keys()) {
   inner = other;
 }
 
-HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf) : inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
+HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
+    : Base(dtf.keys()),
+      inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
 
+}
+
+bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
+  return false;
 };
 
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 4b1c00672..cfb94bc69 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -23,6 +23,8 @@ namespace gtsam {
 
 class HybridDiscreteFactor : public HybridFactor {
  public:
+  using Base = HybridFactor;
+
   DiscreteFactor::shared_ptr inner;
 
   // Implicit conversion from a shared ptr of GF
@@ -30,5 +32,8 @@ class HybridDiscreteFactor : public HybridFactor {
 
   // Forwarding constructor from concrete JacobianFactor
   HybridDiscreteFactor(DecisionTreeFactor &&dtf);
+
+ public:
+  virtual bool equals(const HybridFactor& lf, double tol) const override;
 };
 }
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 686fddc51..0f9d8ddca 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -7,17 +7,19 @@
 #include <gtsam/hybrid/HybridFactorGraph.h>
 
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
 
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
 namespace gtsam {
 
-template class EliminateableFactorGraph<HybridFactorGraph>;
+template
+class EliminateableFactorGraph<HybridFactorGraph>;
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-EliminateHybrid(const HybridFactorGraph& factors,
-                  const Ordering& frontalKeys) {
+EliminateHybrid(const HybridFactorGraph &factors,
+                const Ordering &frontalKeys) {
   // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
   // few cases: continuous variable, we make a GM if there are hybrid factors;
   // continuous variable, we make a GF if there are no hybrid factors;
@@ -29,7 +31,20 @@ EliminateHybrid(const HybridFactorGraph& factors,
 
   // PRODUCT: multiply all factors
   gttic(product);
-  HybridGaussianFactor product(JacobianFactor(0, I_3x3, Z_3x1));
+  KeySet allKeys;
+  // TODO: we do a mock by just doing the correct key thing
+  std::cout << "Begin Eliminate\n";
+  for (auto &&factor : factors) {
+    std::cout << ">>> Eliminating: ";
+    factor->printKeys();
+    allKeys.insert(factor->begin(), factor->end());
+  }
+  for (auto &k : frontalKeys) {
+    allKeys.erase(k);
+  }
+
+  HybridConditional sum(allKeys.size(), Ordering(allKeys));
+//  HybridDiscreteFactor product(DiscreteConditional());
 //  for (auto&& factor : factors) product = (*factor) * product;
   gttoc(product);
 
@@ -39,11 +54,11 @@ EliminateHybrid(const HybridFactorGraph& factors,
   gttoc(sum);
 
   // Ordering keys for the conditional so that frontalKeys are really in front
-//  Ordering orderedKeys;
-//  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
-//                     frontalKeys.end());
-//  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(),
-//                     sum->keys().end());
+  Ordering orderedKeys;
+  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
+                     frontalKeys.end());
+  orderedKeys.insert(orderedKeys.end(), sum.keys().begin(),
+                     sum.keys().end());
 
   // now divide product/sum to get conditional
   gttic(divide);
@@ -52,7 +67,9 @@ EliminateHybrid(const HybridFactorGraph& factors,
   gttoc(divide);
 
 //  return std::make_pair(conditional, sum);
-  return std::make_pair(boost::make_shared<HybridConditional>(), boost::make_shared<HybridGaussianFactor>(product));
+  return std::make_pair(boost::make_shared<HybridConditional>(frontalKeys.size(),
+                                                              orderedKeys),
+                        boost::make_shared<HybridConditional>(std::move(sum)));
 }
 
 }
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 1910c3307..1e87cbbc3 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -8,7 +8,7 @@
 
 namespace gtsam {
 
-HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()){
+HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) {
   inner = other;
 }
 
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index eabfc2e7c..0e3d2ea00 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -11,9 +11,8 @@
 
 /**
  * @file HybridJunctionTree.cpp
- * @date Mar 29, 2013
- * @author Frank Dellaert
- * @author Richard Roberts
+ * @date Mar 11, 2022
+ * @author Fan Jiang
  */
 
 #include <gtsam/inference/JunctionTree-inst.h>
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 295d6011d..6eef100c1 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 
@@ -57,11 +58,15 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
 TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
+  DiscreteKey X(1, 2);
+
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(X, {2, 8})));
 
   auto result = hfg.eliminatePartialMultifrontal({0});
 
   GTSAM_PRINT(*result.first);
+  GTSAM_PRINT(*result.second);
 }
 
 /* ************************************************************************* */

From 3aac52c3d3b6ec692dbc7f0d7874a0cd335814a1 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 12 Mar 2022 10:49:11 -0500
Subject: [PATCH 013/175] Fix compilation error

---
 gtsam/hybrid/HybridFactor.h        | 49 +++++++++++++++---------------
 gtsam/hybrid/HybridFactorGraph.cpp |  3 ++
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 5af1a23a9..eb28bc168 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -27,47 +27,45 @@ namespace gtsam {
 /**
  * Base class for hybrid probabilistic factors
  */
-class GTSAM_EXPORT HybridFactor: public Factor {
+class GTSAM_EXPORT HybridFactor : public Factor {
 
 public:
 
-// typedefs needed to play nice with gtsam
-typedef HybridFactor This; ///< This class
-typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
-typedef Factor Base; ///< Our base class
-
-using Values = Values; ///< backwards compatibility
+  // typedefs needed to play nice with gtsam
+  typedef HybridFactor This; ///< This class
+  typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
+  typedef Factor Base; ///< Our base class
 
 public:
 
 /// @name Standard Constructors
 /// @{
 
-/** Default constructor creates empty factor */
-HybridFactor() {}
+  /** Default constructor creates empty factor */
+  HybridFactor() {}
 
-/** Construct from container of keys.  This constructor is used internally from derived factor
- *  constructors, either from a container of keys or from a boost::assign::list_of. */
-template<typename CONTAINER>
-HybridFactor(const CONTAINER& keys) : Base(keys) {}
+  /** Construct from container of keys.  This constructor is used internally from derived factor
+   *  constructors, either from a container of keys or from a boost::assign::list_of. */
+  template<typename CONTAINER>
+  HybridFactor(const CONTAINER &keys) : Base(keys) {}
 
-/// Virtual destructor
-virtual ~HybridFactor() {
-}
+  /// Virtual destructor
+  virtual ~HybridFactor() {
+  }
 
 /// @}
 /// @name Testable
 /// @{
 
-/// equals
-virtual bool equals(const HybridFactor& lf, double tol = 1e-9) const = 0;
+  /// equals
+  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0;
 
-/// print
-void print(
-    const std::string& s = "HybridFactor\n",
-    const KeyFormatter& formatter = DefaultKeyFormatter) const override {
-Base::print(s, formatter);
-}
+  /// print
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override {
+    Base::print(s, formatter);
+  }
 
 /// @}
 /// @name Standard Interface
@@ -78,6 +76,7 @@ Base::print(s, formatter);
 // HybridFactor
 
 // traits
-template<> struct traits<HybridFactor> : public Testable<HybridFactor> {};
+template<>
+struct traits<HybridFactor> : public Testable<HybridFactor> {};
 
 }// namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 0f9d8ddca..bd245b16a 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -29,6 +29,9 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // The issue here is that, how can we know which variable is discrete if we
   // unify Values? Obviously we can tell using the factors, but is that fast?
 
+  // In the case of multifrontal, we will need to use a constrained ordering
+  // so that the discrete parts will be guaranteed to be eliminated last!
+
   // PRODUCT: multiply all factors
   gttic(product);
   KeySet allKeys;

From 53551e051dedae2c245f47dbf69bf390844bb3ab Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 12 Mar 2022 11:51:48 -0500
Subject: [PATCH 014/175] Add shorthand for inserting raw JacobianFactor

---
 gtsam/hybrid/CGMixtureFactor.h               | 22 +++++++++++--
 gtsam/hybrid/HybridDiscreteFactor.cpp        |  2 +-
 gtsam/hybrid/HybridDiscreteFactor.h          |  2 ++
 gtsam/hybrid/HybridFactor.h                  | 34 ++++++++++++++++++--
 gtsam/hybrid/HybridFactorGraph.cpp           |  3 ++
 gtsam/hybrid/HybridFactorGraph.h             |  6 ++++
 gtsam/hybrid/HybridGaussianFactor.h          |  2 ++
 gtsam/hybrid/tests/testHybridConditional.cpp | 12 ++++---
 8 files changed, 72 insertions(+), 11 deletions(-)

diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index f2fa1e54a..b46146eb3 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -5,19 +5,35 @@
 
 /**
  * @file   CGMixtureFactor.h
- * @brief  Custom hybrid factor graph for discrete + continuous factors
- * @author Kevin Doherty, kdoherty@mit.edu
+ * @brief  A set of Gaussian factors indexed by a set of discrete keys.
  * @author Varun Agrawal
  * @author Fan Jiang
+ * @author Frank Dellaert
  * @date   December 2021
  */
 
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
 
 namespace gtsam {
 
-class CGMixtureFactor : HybridFactor {
+class CGMixtureFactor : public HybridFactor {
+public:
+  using Base = HybridFactor;
+  using This = CGMixtureFactor;
+  using shared_ptr = boost::shared_ptr<This>;
 
+  using Factors = DecisionTree<Key, GaussianFactor::shared_ptr>;
+
+  Factors factors_;
+
+  CGMixtureFactor() = default;
+
+  CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys) {
+
+  }
 };
 
 }
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 0bd2c9a64..13766933b 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -14,7 +14,7 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
 }
 
 HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
-    : Base(dtf.keys()),
+    : Base(dtf.discreteKeys()),
       inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
 
 }
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index cfb94bc69..0395c9512 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -24,6 +24,8 @@ namespace gtsam {
 class HybridDiscreteFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
+  using This = HybridDiscreteFactor;
+  using shared_ptr = boost::shared_ptr<This>;
 
   DiscreteFactor::shared_ptr inner;
 
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index eb28bc168..64b49f605 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -19,6 +19,7 @@
 
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Factor.h>
+#include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/base/Testable.h>
 
 #include <string>
@@ -36,6 +37,12 @@ public:
   typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
   typedef Factor Base; ///< Our base class
 
+  bool isDiscrete_ = false;
+  bool isContinuous_ = false;
+  bool isHybrid_ = false;
+
+  DiscreteKeys discreteKeys_;
+
 public:
 
 /// @name Standard Constructors
@@ -46,8 +53,25 @@ public:
 
   /** Construct from container of keys.  This constructor is used internally from derived factor
    *  constructors, either from a container of keys or from a boost::assign::list_of. */
-  template<typename CONTAINER>
-  HybridFactor(const CONTAINER &keys) : Base(keys) {}
+//  template<typename CONTAINER>
+//  HybridFactor(const CONTAINER &keys) : Base(keys) {}
+
+  HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
+
+  static KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
+    KeyVector allKeys;
+    std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
+    std::transform(discreteKeys.begin(),
+                   discreteKeys.end(),
+                   std::back_inserter(allKeys),
+                   [](const DiscreteKey &k) { return k.first; });
+    return allKeys;
+  }
+
+  HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base(
+      CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true) {}
+
+  HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true) {}
 
   /// Virtual destructor
   virtual ~HybridFactor() {
@@ -64,7 +88,11 @@ public:
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override {
-    Base::print(s, formatter);
+    std::cout << s;
+    if (isContinuous_) std::cout << "Cont. ";
+    if (isDiscrete_) std::cout << "Disc. ";
+    if (isHybrid_) std::cout << "Hybr. ";
+    this->printKeys("", formatter);
   }
 
 /// @}
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index bd245b16a..080efc0e9 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -75,4 +75,7 @@ EliminateHybrid(const HybridFactorGraph &factors,
                         boost::make_shared<HybridConditional>(std::move(sum)));
 }
 
+void HybridFactorGraph::add(JacobianFactor &&factor) {
+  FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
+}
 }
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index fb0de644d..13bbd005d 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -33,6 +33,8 @@ class HybridEliminationTree;
 class HybridBayesTree;
 class HybridJunctionTree;
 
+class JacobianFactor;
+
 /** Main elimination function for HybridFactorGraph */
 GTSAM_EXPORT std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
 EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
@@ -77,6 +79,10 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
   template <class DERIVEDFACTOR>
   HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+  using FactorGraph::add;
+  
+  /// Add a factor directly using a shared_ptr.
+  void add(JacobianFactor &&factor);
 };
 
 }
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index e40682dc1..d8a97ba30 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -24,6 +24,8 @@ namespace gtsam {
 class HybridGaussianFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
+  using This = HybridGaussianFactor;
+  using shared_ptr = boost::shared_ptr<This>;
 
   GaussianFactor::shared_ptr inner;
 
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 6eef100c1..a56bcadf0 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -26,6 +26,8 @@
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/inference/BayesNet.h>
 
+#include <gtsam/inference/Symbol.h>
+
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/map.hpp>
@@ -34,6 +36,8 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
+using gtsam::symbol_shorthand::X;
+
 /* ************************************************************************* */
 TEST_UNSAFE(HybridFactorGraph, test) {
   HybridConditional test;
@@ -58,12 +62,12 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
 TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
-  DiscreteKey X(1, 2);
+  DiscreteKey x(X(1), 2);
 
-  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(X, {2, 8})));
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
 
-  auto result = hfg.eliminatePartialMultifrontal({0});
+  auto result = hfg.eliminatePartialMultifrontal({X(0)});
 
   GTSAM_PRINT(*result.first);
   GTSAM_PRINT(*result.second);

From 095f6ad7cc96d284fb663a4cfb62f0d5f48f9204 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 12 Mar 2022 12:42:58 -0500
Subject: [PATCH 015/175] Add full elimination

---
 gtsam/hybrid/HybridFactorGraph.h             | 12 +++++++-----
 gtsam/hybrid/tests/testHybridConditional.cpp | 13 +++++++++++++
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 13bbd005d..9a57d36e6 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -70,17 +70,19 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
  public:
   HybridFactorGraph() = default;
 
-  /** Construct from container of factors (shared_ptr or plain objects) */
-  template <class CONTAINER>
-  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+//  /** Construct from container of factors (shared_ptr or plain objects) */
+//  template <class CONTAINER>
+//  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
 
   /** Implicit copy/downcast constructor to override explicit template container
-   * constructor */
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
   template <class DERIVEDFACTOR>
   HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
   using FactorGraph::add;
-  
+
   /// Add a factor directly using a shared_ptr.
   void add(JacobianFactor &&factor);
 };
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index a56bcadf0..d5a21f34d 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -73,6 +73,19 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
   GTSAM_PRINT(*result.second);
 }
 
+TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+  HybridFactorGraph hfg;
+
+  DiscreteKey x(X(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+
+  auto result = hfg.eliminateMultifrontal();
+
+  GTSAM_PRINT(*result);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 2bae2865d74efc139c8c77fcfe8a4d60bcb8eaab Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 12 Mar 2022 16:29:26 -0500
Subject: [PATCH 016/175] More mock-ups added

---
 gtsam/hybrid/CGMixtureFactor.cpp             | 14 ++++++++-
 gtsam/hybrid/CGMixtureFactor.h               | 18 ++++++++----
 gtsam/hybrid/CLGaussianConditional.cpp       | 20 +++++++++++++
 gtsam/hybrid/CLGaussianConditional.h         | 31 ++++++++++++++++++++
 gtsam/hybrid/HybridConditional.h             |  2 +-
 gtsam/hybrid/HybridFactorGraph.cpp           | 14 +++++----
 gtsam/hybrid/HybridGaussianFactor.h          |  6 ++--
 gtsam/hybrid/tests/testHybridConditional.cpp | 25 +++++++++++-----
 8 files changed, 107 insertions(+), 23 deletions(-)
 create mode 100644 gtsam/hybrid/CLGaussianConditional.cpp
 create mode 100644 gtsam/hybrid/CLGaussianConditional.h

diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp
index d789825f7..2ddf80ec2 100644
--- a/gtsam/hybrid/CGMixtureFactor.cpp
+++ b/gtsam/hybrid/CGMixtureFactor.cpp
@@ -2,4 +2,16 @@
 // Created by Fan Jiang on 3/11/22.
 //
 
-#include "CGMixtureFactor.h"
+#include <gtsam/hybrid/CGMixtureFactor.h>
+
+namespace gtsam {
+
+CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys,
+                                 const DiscreteKeys &discreteKeys,
+                                 const Factors &factors) : Base(continuousKeys, discreteKeys),
+                                                           factors_(factors) {}
+bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const {
+  return false;
+}
+
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index b46146eb3..9c9e43ec2 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -1,15 +1,21 @@
 /* ----------------------------------------------------------------------------
- * Copyright 2021 The Ambitious Folks of the MRG
+
+ * 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   CGMixtureFactor.h
  * @brief  A set of Gaussian factors indexed by a set of discrete keys.
- * @author Varun Agrawal
  * @author Fan Jiang
+ * @author Varun Agrawal
  * @author Frank Dellaert
- * @date   December 2021
+ * @date   Mar 12, 2022
  */
 
 #include <gtsam/hybrid/HybridFactor.h>
@@ -31,9 +37,11 @@ public:
 
   CGMixtureFactor() = default;
 
-  CGMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const Factors &factors) : Base(continuousKeys, discreteKeys) {
+  CGMixtureFactor(const KeyVector &continuousKeys,
+                  const DiscreteKeys &discreteKeys,
+                  const Factors &factors);
 
-  }
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 };
 
 }
diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp
new file mode 100644
index 000000000..09babc4e2
--- /dev/null
+++ b/gtsam/hybrid/CLGaussianConditional.cpp
@@ -0,0 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CLGaussianConditional.cpp
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/hybrid/CLGaussianConditional.h>
+
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h
new file mode 100644
index 000000000..03a2d99e7
--- /dev/null
+++ b/gtsam/hybrid/CLGaussianConditional.h
@@ -0,0 +1,31 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CLGaussianConditional.h
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @date   Mar 12, 2022
+ */
+
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+class CLGaussianConditional : public HybridFactor, public Conditional<HybridFactor, CLGaussianConditional> {
+public:
+  using This = CLGaussianConditional;
+  using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
+  using BaseFactor = HybridFactor;
+
+
+};
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index e9aa5f616..26083e13e 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -34,7 +34,7 @@ namespace gtsam {
  *
  * As a type-erased variant of:
  * - DiscreteConditional
- * - GaussianMixture
+ * - CLGaussianConditional
  * - GaussianConditional
  */
 class GTSAM_EXPORT HybridConditional
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 080efc0e9..cd5bc651d 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -32,20 +32,24 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // In the case of multifrontal, we will need to use a constrained ordering
   // so that the discrete parts will be guaranteed to be eliminated last!
 
-  // PRODUCT: multiply all factors
-  gttic(product);
+  // PREPROCESS: Identify the nature of the current elimination
   KeySet allKeys;
   // TODO: we do a mock by just doing the correct key thing
-  std::cout << "Begin Eliminate\n";
+  std::cout << "Begin Eliminate: ";
+  frontalKeys.print();
+
   for (auto &&factor : factors) {
-    std::cout << ">>> Eliminating: ";
-    factor->printKeys();
+    std::cout << ">>> Adding factor: ";
+    factor->print();
     allKeys.insert(factor->begin(), factor->end());
   }
   for (auto &k : frontalKeys) {
     allKeys.erase(k);
   }
 
+  // PRODUCT: multiply all factors
+  gttic(product);
+
   HybridConditional sum(allKeys.size(), Ordering(allKeys));
 //  HybridDiscreteFactor product(DiscreteConditional());
 //  for (auto&& factor : factors) product = (*factor) * product;
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index d8a97ba30..34a7c0004 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -29,11 +29,11 @@ class HybridGaussianFactor : public HybridFactor {
 
   GaussianFactor::shared_ptr inner;
 
-  // Implicit conversion from a shared ptr of GF
-  HybridGaussianFactor(GaussianFactor::shared_ptr other);
+  // Explicit conversion from a shared ptr of GF
+  explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
 
   // Forwarding constructor from concrete JacobianFactor
-  HybridGaussianFactor(JacobianFactor &&jf);
+  explicit HybridGaussianFactor(JacobianFactor &&jf);
 
  public:
   virtual bool equals(const HybridFactor& lf, double tol) const override;
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index d5a21f34d..46ec40475 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/CGMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 
@@ -37,16 +38,15 @@ using namespace std;
 using namespace gtsam;
 
 using gtsam::symbol_shorthand::X;
+using gtsam::symbol_shorthand::C;
 
 /* ************************************************************************* */
-TEST_UNSAFE(HybridFactorGraph, test) {
+TEST_UNSAFE(HybridFactorGraph, creation) {
   HybridConditional test;
-  GTSAM_PRINT(test);
 
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
-  GTSAM_PRINT(hfg);
 }
 
 TEST_UNSAFE(HybridFactorGraph, eliminate) {
@@ -56,7 +56,7 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
 
   auto result = hfg.eliminatePartialSequential({0});
 
-  GTSAM_PRINT(*result.first);
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
 TEST(HybridFactorGraph, eliminateMultifrontal) {
@@ -69,21 +69,30 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
 
   auto result = hfg.eliminatePartialMultifrontal({X(0)});
 
-  GTSAM_PRINT(*result.first);
-  GTSAM_PRINT(*result.second);
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
+  EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
 TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+
+  std::cout << ">>>>>>>>>>>>>>\n";
+
   HybridFactorGraph hfg;
 
-  DiscreteKey x(X(1), 2);
+  DiscreteKey x(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt;
+
+  hfg.add(CGMixtureFactor({X(1)}, { x }, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
 
-  auto result = hfg.eliminateMultifrontal();
+  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   GTSAM_PRINT(*result);
+  GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
 /* ************************************************************************* */

From ee4f9d19f0ff7486076b647327d0c7c3d1274c6a Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 13 Mar 2022 11:42:36 -0400
Subject: [PATCH 017/175] Added mixture factor functionality

---
 gtsam/hybrid/CGMixtureFactor.cpp             | 17 ++++++++++
 gtsam/hybrid/CGMixtureFactor.h               |  3 ++
 gtsam/hybrid/CLGaussianConditional.cpp       | 31 +++++++++++++++++
 gtsam/hybrid/CLGaussianConditional.h         | 17 ++++++++++
 gtsam/hybrid/HybridDiscreteFactor.cpp        |  5 +++
 gtsam/hybrid/HybridDiscreteFactor.h          |  2 ++
 gtsam/hybrid/HybridFactor.cpp                | 35 ++++++++++++++++++++
 gtsam/hybrid/HybridFactor.h                  | 25 +++++---------
 gtsam/hybrid/HybridFactorGraph.cpp           | 15 +++++----
 gtsam/hybrid/HybridGaussianFactor.cpp        |  4 +++
 gtsam/hybrid/HybridGaussianFactor.h          |  2 ++
 gtsam/hybrid/tests/testHybridConditional.cpp | 21 ++++++++++--
 12 files changed, 151 insertions(+), 26 deletions(-)

diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp
index 2ddf80ec2..16ead783e 100644
--- a/gtsam/hybrid/CGMixtureFactor.cpp
+++ b/gtsam/hybrid/CGMixtureFactor.cpp
@@ -4,6 +4,9 @@
 
 #include <gtsam/hybrid/CGMixtureFactor.h>
 
+#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/base/utilities.h>
+
 namespace gtsam {
 
 CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys,
@@ -14,4 +17,18 @@ bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
+void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  factors_.print(
+      "mixture = ",
+      [&](Key k) {
+        return formatter(k);
+      }, [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+        RedirectCout rd;
+        if (!gf->empty()) gf->print("", formatter);
+        else return {"nullptr"};
+        return rd.str();
+      });
+}
+
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index 9c9e43ec2..7ff53b7ed 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -42,6 +42,9 @@ public:
                   const Factors &factors);
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
+
+  void print(const std::string &s = "HybridFactor\n",
+             const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 
 }
diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp
index 09babc4e2..dbc9631c8 100644
--- a/gtsam/hybrid/CLGaussianConditional.cpp
+++ b/gtsam/hybrid/CLGaussianConditional.cpp
@@ -18,3 +18,34 @@
 
 #include <gtsam/hybrid/CLGaussianConditional.h>
 
+#include <gtsam/inference/Conditional-inst.h>
+
+namespace gtsam {
+
+CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals,
+                                             const KeyVector &continuousParents,
+                                             const DiscreteKeys &discreteParents,
+                                             const CLGaussianConditional::Conditionals &factors)
+    : BaseFactor(
+    CollectKeys(continuousFrontals, continuousParents), discreteParents),
+      BaseConditional(continuousFrontals.size()) {
+
+}
+
+bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const {
+  return false;
+}
+
+void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const {
+  std::cout << s << ": ";
+  if (isContinuous_) std::cout << "Cont. ";
+  if (isDiscrete_) std::cout << "Disc. ";
+  if (isHybrid_) std::cout << "Hybr. ";
+  BaseConditional::print("", formatter);
+  std::cout << "Discrete Keys = ";
+  for (auto &dk : discreteKeys_) {
+    std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
+  }
+  std::cout << "\n";
+}
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h
index 03a2d99e7..14989df72 100644
--- a/gtsam/hybrid/CLGaussianConditional.h
+++ b/gtsam/hybrid/CLGaussianConditional.h
@@ -19,13 +19,30 @@
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
 
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/discrete/DecisionTree.h>
+
 namespace gtsam {
 class CLGaussianConditional : public HybridFactor, public Conditional<HybridFactor, CLGaussianConditional> {
 public:
   using This = CLGaussianConditional;
   using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
   using BaseFactor = HybridFactor;
+  using BaseConditional = Conditional<HybridFactor, CLGaussianConditional>;
 
+  using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
 
+public:
+
+  CLGaussianConditional(const KeyVector &continuousFrontals,
+                        const KeyVector &continuousParents,
+                        const DiscreteKeys &discreteParents,
+                        const Conditionals &factors);
+
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
+
+  void print(
+      const std::string &s = "CLGaussianConditional\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 13766933b..1758e9025 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -21,6 +21,11 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
 
 bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
+}
+
+void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  inner->print("inner: ", formatter);
 };
 
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 0395c9512..9d574b736 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -37,5 +37,7 @@ class HybridDiscreteFactor : public HybridFactor {
 
  public:
   virtual bool equals(const HybridFactor& lf, double tol) const override;
+
+  void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 }
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 907350e83..3095136a4 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -16,3 +16,38 @@
  */
 
 #include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
+  KeyVector allKeys;
+  std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
+  std::transform(discreteKeys.begin(),
+                 discreteKeys.end(),
+                 std::back_inserter(allKeys),
+                 [](const DiscreteKey &k) { return k.first; });
+  return allKeys;
+}
+
+KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
+  KeyVector allKeys;
+  std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys));
+  std::copy(keys2.begin(), keys2.end(), std::back_inserter(allKeys));
+  return allKeys;
+}
+
+HybridFactor::HybridFactor() = default;
+
+HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
+
+HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
+    : Base(
+    CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {}
+
+HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)),
+                                                               isDiscrete_(true),
+                                                               discreteKeys_(discreteKeys) {}
+
+HybridFactor::~HybridFactor() = default;
+
+}
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 64b49f605..619d16078 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -25,6 +25,9 @@
 #include <string>
 namespace gtsam {
 
+KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
+KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
+
 /**
  * Base class for hybrid probabilistic factors
  */
@@ -49,33 +52,21 @@ public:
 /// @{
 
   /** Default constructor creates empty factor */
-  HybridFactor() {}
+  HybridFactor();
 
   /** Construct from container of keys.  This constructor is used internally from derived factor
    *  constructors, either from a container of keys or from a boost::assign::list_of. */
 //  template<typename CONTAINER>
 //  HybridFactor(const CONTAINER &keys) : Base(keys) {}
 
-  HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
+  explicit HybridFactor(const KeyVector &keys);
 
-  static KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
-    KeyVector allKeys;
-    std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
-    std::transform(discreteKeys.begin(),
-                   discreteKeys.end(),
-                   std::back_inserter(allKeys),
-                   [](const DiscreteKey &k) { return k.first; });
-    return allKeys;
-  }
+  HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
 
-  HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) : Base(
-      CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true) {}
-
-  HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)), isDiscrete_(true) {}
+  explicit HybridFactor(const DiscreteKeys &discreteKeys);
 
   /// Virtual destructor
-  virtual ~HybridFactor() {
-  }
+  virtual ~HybridFactor();
 
 /// @}
 /// @name Testable
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index cd5bc651d..2dc54d75d 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -34,6 +34,7 @@ EliminateHybrid(const HybridFactorGraph &factors,
 
   // PREPROCESS: Identify the nature of the current elimination
   KeySet allKeys;
+
   // TODO: we do a mock by just doing the correct key thing
   std::cout << "Begin Eliminate: ";
   frontalKeys.print();
@@ -43,6 +44,7 @@ EliminateHybrid(const HybridFactorGraph &factors,
     factor->print();
     allKeys.insert(factor->begin(), factor->end());
   }
+
   for (auto &k : frontalKeys) {
     allKeys.erase(k);
   }
@@ -51,13 +53,14 @@ EliminateHybrid(const HybridFactorGraph &factors,
   gttic(product);
 
   HybridConditional sum(allKeys.size(), Ordering(allKeys));
-//  HybridDiscreteFactor product(DiscreteConditional());
-//  for (auto&& factor : factors) product = (*factor) * product;
+
+  //  HybridDiscreteFactor product(DiscreteConditional());
+  //  for (auto&& factor : factors) product = (*factor) * product;
   gttoc(product);
 
   // sum out frontals, this is the factor on the separator
   gttic(sum);
-//  HybridFactor::shared_ptr sum = product.sum(frontalKeys);
+  //  HybridFactor::shared_ptr sum = product.sum(frontalKeys);
   gttoc(sum);
 
   // Ordering keys for the conditional so that frontalKeys are really in front
@@ -69,11 +72,11 @@ EliminateHybrid(const HybridFactorGraph &factors,
 
   // now divide product/sum to get conditional
   gttic(divide);
-//  auto conditional =
-//      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
+  //  auto conditional =
+  //      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
   gttoc(divide);
 
-//  return std::make_pair(conditional, sum);
+  //  return std::make_pair(conditional, sum);
   return std::make_pair(boost::make_shared<HybridConditional>(frontalKeys.size(),
                                                               orderedKeys),
                         boost::make_shared<HybridConditional>(std::move(sum)));
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 1e87cbbc3..faa4ba998 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -18,6 +18,10 @@ HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()
 
 bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
   return false;
+}
+void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  inner->print("inner: ", formatter);
 };
 
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 34a7c0004..8562075b4 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -37,5 +37,7 @@ class HybridGaussianFactor : public HybridFactor {
 
  public:
   virtual bool equals(const HybridFactor& lf, double tol) const override;
+
+  void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 }
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 46ec40475..4611026b3 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/CLGaussianConditional.h>
 #include <gtsam/hybrid/CGMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
@@ -47,6 +48,13 @@ TEST_UNSAFE(HybridFactorGraph, creation) {
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  CLGaussianConditional clgc(
+      {X(0)}, {X(1)},
+      DiscreteKeys(DiscreteKey{C(0), 2}),
+      CLGaussianConditional::Conditionals()
+  );
+  GTSAM_PRINT(clgc);
 }
 
 TEST_UNSAFE(HybridFactorGraph, eliminate) {
@@ -84,12 +92,19 @@ TEST(HybridFactorGraph, eliminateFullMultifrontal) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  DecisionTree<Key, GaussianFactor::shared_ptr> dt;
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Z_3x1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, { x }, dt));
+  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
 
-  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
+  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
   GTSAM_PRINT(*result);
   GTSAM_PRINT(*result->marginalFactor(C(1)));

From 5ea614a82a0528a78758d5d6782cfbb948a43179 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 13 Mar 2022 20:09:53 -0400
Subject: [PATCH 018/175] Added more mockups and color output of the
 elimination process

---
 gtsam/hybrid/CLGaussianConditional.cpp       | 17 +++-
 gtsam/hybrid/CLGaussianConditional.h         |  4 +-
 gtsam/hybrid/HybridConditional.h             | 71 ++++++++-------
 gtsam/hybrid/HybridFactor.cpp                |  7 ++
 gtsam/hybrid/HybridFactor.h                  |  1 +
 gtsam/hybrid/HybridFactorGraph.cpp           | 93 ++++++++++++++++----
 gtsam/hybrid/tests/testHybridConditional.cpp | 15 +++-
 7 files changed, 151 insertions(+), 57 deletions(-)

diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp
index dbc9631c8..37c82a0d5 100644
--- a/gtsam/hybrid/CLGaussianConditional.cpp
+++ b/gtsam/hybrid/CLGaussianConditional.cpp
@@ -18,17 +18,20 @@
 
 #include <gtsam/hybrid/CLGaussianConditional.h>
 
+#include <gtsam/base/utilities.h>
+
 #include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/discrete/DecisionTree-inl.h>
 
 namespace gtsam {
 
 CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals,
                                              const KeyVector &continuousParents,
                                              const DiscreteKeys &discreteParents,
-                                             const CLGaussianConditional::Conditionals &factors)
+                                             const CLGaussianConditional::Conditionals &conditionals)
     : BaseFactor(
     CollectKeys(continuousFrontals, continuousParents), discreteParents),
-      BaseConditional(continuousFrontals.size()) {
+      BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {
 
 }
 
@@ -47,5 +50,15 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
   std::cout << "\n";
+  conditionals_.print(
+      "",
+      [&](Key k) {
+        return formatter(k);
+      }, [&](const GaussianConditional::shared_ptr &gf) -> std::string {
+        RedirectCout rd;
+        if (!gf->empty()) gf->print("", formatter);
+        else return {"nullptr"};
+        return rd.str();
+      });
 }
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h
index 14989df72..0319c60a7 100644
--- a/gtsam/hybrid/CLGaussianConditional.h
+++ b/gtsam/hybrid/CLGaussianConditional.h
@@ -32,12 +32,14 @@ public:
 
   using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
 
+  Conditionals conditionals_;
+
 public:
 
   CLGaussianConditional(const KeyVector &continuousFrontals,
                         const KeyVector &continuousParents,
                         const DiscreteKeys &discreteParents,
-                        const Conditionals &factors);
+                        const Conditionals &conditionals);
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 26083e13e..92856cae2 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -41,56 +41,59 @@ class GTSAM_EXPORT HybridConditional
 : public HybridFactor,
 public Conditional<HybridFactor, HybridConditional> {
 public:
-// typedefs needed to play nice with gtsam
-typedef HybridConditional This;            ///< Typedef to this class
-typedef boost::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
-typedef HybridFactor BaseFactor;  ///< Typedef to our factor base class
-typedef Conditional<BaseFactor, This>
-    BaseConditional;  ///< Typedef to our conditional base class
+  // typedefs needed to play nice with gtsam
+  typedef HybridConditional This;            ///< Typedef to this class
+  typedef boost::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
+  typedef HybridFactor BaseFactor;  ///< Typedef to our factor base class
+  typedef Conditional<BaseFactor, This>
+      BaseConditional;  ///< Typedef to our conditional base class
 
 private:
-// Type-erased pointer to the inner type
-std::unique_ptr<Factor> inner;
+  // Type-erased pointer to the inner type
+  std::unique_ptr<Factor> inner;
 
 public:
 /// @name Standard Constructors
 /// @{
 
-/// Default constructor needed for serialization.
-HybridConditional() = default;
+  /// Default constructor needed for serialization.
+  HybridConditional() = default;
 
-HybridConditional(size_t nFrontals, const KeyVector& keys) : BaseFactor(keys), BaseConditional(nFrontals) {
+  HybridConditional(const KeyVector &continuousKeys,
+                    const DiscreteKeys &discreteKeys,
+                    size_t nFrontals)
+      : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {
 
-}
+  }
 
-/**
- * @brief Combine two conditionals, yielding a new conditional with the union
- * of the frontal keys, ordered by gtsam::Key.
- *
- * The two conditionals must make a valid Bayes net fragment, i.e.,
- * the frontal variables cannot overlap, and must be acyclic:
- * Example of correct use:
- *   P(A,B) = P(A|B) * P(B)
- *   P(A,B|C) = P(A|B) * P(B|C)
- *   P(A,B,C) = P(A,B|C) * P(C)
- * Example of incorrect use:
- *   P(A|B) * P(A|C) = ?
- *   P(A|B) * P(B|A) = ?
- * We check for overlapping frontals, but do *not* check for cyclic.
- */
-HybridConditional operator*(const HybridConditional& other) const;
+  /**
+   * @brief Combine two conditionals, yielding a new conditional with the union
+   * of the frontal keys, ordered by gtsam::Key.
+   *
+   * The two conditionals must make a valid Bayes net fragment, i.e.,
+   * the frontal variables cannot overlap, and must be acyclic:
+   * Example of correct use:
+   *   P(A,B) = P(A|B) * P(B)
+   *   P(A,B|C) = P(A|B) * P(B|C)
+   *   P(A,B,C) = P(A,B|C) * P(C)
+   * Example of incorrect use:
+   *   P(A|B) * P(A|C) = ?
+   *   P(A|B) * P(B|A) = ?
+   * We check for overlapping frontals, but do *not* check for cyclic.
+   */
+  HybridConditional operator*(const HybridConditional& other) const;
 
 /// @}
 /// @name Testable
 /// @{
 
-/// GTSAM-style print
-void print(
-    const std::string& s = "Hybrid Conditional: ",
-    const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+  /// GTSAM-style print
+  void print(
+      const std::string& s = "Hybrid Conditional: ",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
-/// GTSAM-style equals
-bool equals(const HybridFactor& other, double tol = 1e-9) const override;
+  /// GTSAM-style equals
+  bool equals(const HybridFactor& other, double tol = 1e-9) const override;
 
 /// @}
 };
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 3095136a4..171ea790a 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -36,6 +36,13 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
   return allKeys;
 }
 
+DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) {
+  DiscreteKeys allKeys;
+  std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys));
+  std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys));
+  return allKeys;
+}
+
 HybridFactor::HybridFactor() = default;
 
 HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 619d16078..dd925fee2 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -27,6 +27,7 @@ namespace gtsam {
 
 KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
+DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2);
 
 /**
  * Base class for hybrid probabilistic factors
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 2dc54d75d..177513f60 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -5,17 +5,26 @@
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridFactor.h>
 
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
+#include <unordered_map>
+
 namespace gtsam {
 
 template
 class EliminateableFactorGraph<HybridFactorGraph>;
 
+static std::string BLACK_BOLD = "\033[1;30m";
+static std::string RED_BOLD = "\033[1;31m";
+static std::string GREEN = "\033[0;32m";
+static std::string GREEN_BOLD = "\033[1;32m";
+static std::string RESET = "\033[0m";
+
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 EliminateHybrid(const HybridFactorGraph &factors,
@@ -33,26 +42,66 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // so that the discrete parts will be guaranteed to be eliminated last!
 
   // PREPROCESS: Identify the nature of the current elimination
-  KeySet allKeys;
+  std::unordered_map<Key, DiscreteKey> discreteCardinalities;
+  std::set<DiscreteKey> discreteSeparator;
+  std::set<DiscreteKey> discreteFrontals;
+
+  KeySet separatorKeys;
+  KeySet allContinuousKeys;
+  KeySet continuousFrontals;
+  KeySet continuousSeparator;
 
   // TODO: we do a mock by just doing the correct key thing
-  std::cout << "Begin Eliminate: ";
+
+  // This initializes separatorKeys and discreteCardinalities
+  for (auto &&factor : factors) {
+    std::cout << ">>> Adding factor: " << GREEN;
+    factor->print();
+    std::cout << RESET;
+    separatorKeys.insert(factor->begin(), factor->end());
+    if (!factor->isContinuous_) {
+      for (auto &k : factor->discreteKeys_) {
+        discreteCardinalities[k.first] = k;
+      }
+    }
+  }
+
+  // remove frontals from separator
+  for (auto &k : frontalKeys) {
+    separatorKeys.erase(k);
+  }
+
+  // Fill in discrete frontals and continuous frontals for the end result
+  for (auto &k : frontalKeys) {
+    if (discreteCardinalities.find(k) != discreteCardinalities.end()) {
+      discreteFrontals.insert(discreteCardinalities.at(k));
+    } else {
+      continuousFrontals.insert(k);
+    }
+  }
+
+  // Fill in discrete frontals and continuous frontals for the end result
+  for (auto &k : separatorKeys) {
+    if (discreteCardinalities.find(k) != discreteCardinalities.end()) {
+      discreteSeparator.insert(discreteCardinalities.at(k));
+    } else {
+      continuousSeparator.insert(k);
+    }
+  }
+
+  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
   frontalKeys.print();
 
-  for (auto &&factor : factors) {
-    std::cout << ">>> Adding factor: ";
-    factor->print();
-    allKeys.insert(factor->begin(), factor->end());
-  }
-
-  for (auto &k : frontalKeys) {
-    allKeys.erase(k);
-  }
-
+  std::cout << RED_BOLD << "Discrete Keys: " << RESET;
+  for (auto &&key : discreteCardinalities)
+    std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.second.first) % key.second.second;
+  std::cout << "\n" << RESET;
   // PRODUCT: multiply all factors
   gttic(product);
 
-  HybridConditional sum(allKeys.size(), Ordering(allKeys));
+  HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
+                        DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
+                        separatorKeys.size());
 
   //  HybridDiscreteFactor product(DiscreteConditional());
   //  for (auto&& factor : factors) product = (*factor) * product;
@@ -76,10 +125,22 @@ EliminateHybrid(const HybridFactorGraph &factors,
   //      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
   gttoc(divide);
 
+  auto conditional = boost::make_shared<HybridConditional>(
+      CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
+                  {continuousSeparator.begin(), continuousSeparator.end()}),
+      CollectDiscreteKeys({discreteFrontals.begin(), discreteFrontals.end()},
+                          {discreteSeparator.begin(), discreteSeparator.end()}),
+      continuousFrontals.size() + discreteFrontals.size());
+  std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
+  conditional->print();
+  std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
+  sum.print();
+  std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
+
   //  return std::make_pair(conditional, sum);
-  return std::make_pair(boost::make_shared<HybridConditional>(frontalKeys.size(),
-                                                              orderedKeys),
-                        boost::make_shared<HybridConditional>(std::move(sum)));
+  return std::make_pair(
+      conditional,
+      boost::make_shared<HybridConditional>(std::move(sum)));
 }
 
 void HybridFactorGraph::add(JacobianFactor &&factor) {
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 4611026b3..7292cb3f5 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -42,7 +42,7 @@ using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::C;
 
 /* ************************************************************************* */
-TEST_UNSAFE(HybridFactorGraph, creation) {
+TEST(HybridFactorGraph, creation) {
   HybridConditional test;
 
   HybridFactorGraph hfg;
@@ -52,12 +52,19 @@ TEST_UNSAFE(HybridFactorGraph, creation) {
   CLGaussianConditional clgc(
       {X(0)}, {X(1)},
       DiscreteKeys(DiscreteKey{C(0), 2}),
-      CLGaussianConditional::Conditionals()
+      CLGaussianConditional::Conditionals(
+          C(0),
+          boost::make_shared<GaussianConditional>(
+              X(0), Z_3x1, I_3x3, X(1), I_3x3),
+          boost::make_shared<GaussianConditional>(
+              X(0), Vector3::Ones(),
+              I_3x3, X(1),
+              I_3x3))
   );
   GTSAM_PRINT(clgc);
 }
 
-TEST_UNSAFE(HybridFactorGraph, eliminate) {
+TEST_DISABLED(HybridFactorGraph, eliminate) {
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@@ -67,7 +74,7 @@ TEST_UNSAFE(HybridFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
-TEST(HybridFactorGraph, eliminateMultifrontal) {
+TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
   DiscreteKey x(X(1), 2);

From 0aeb59695e542bc1513ba63f900afd601dd86525 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 13 Mar 2022 23:15:20 -0400
Subject: [PATCH 019/175] add more comments

---
 gtsam/hybrid/HybridConditional.cpp           | 21 +++++++++++--
 gtsam/hybrid/HybridFactorGraph.cpp           |  7 ++++-
 gtsam/hybrid/tests/testHybridConditional.cpp | 31 +++++++++++++++++++-
 3 files changed, 55 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index e6b2eb47f..b3009d5b9 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -7,8 +7,25 @@
 
 namespace gtsam {
 void HybridConditional::print(const std::string &s,
-                                     const KeyFormatter &formatter) const {
-  Conditional::print(s, formatter);
+                              const KeyFormatter &formatter) const {
+  std::cout << s << "P(";
+  int index = 0;
+  const size_t N = keys().size();
+  const size_t contN = N - discreteKeys_.size();
+  while (index < N) {
+    if (index > 0) {
+      if (index == nrFrontals_) std::cout << " | "; else std::cout << ", ";
+    }
+    if (index < contN) {
+      std::cout << formatter(keys()[index]);
+    } else {
+      auto &dk = discreteKeys_[index - contN];
+      std::cout << "(" << formatter(dk.first) << ", " << dk.second << ")";
+    }
+    index++;
+  }
+  std::cout << ")\n";
+  if (inner) inner->print("", formatter);
 }
 
 bool HybridConditional::equals(const HybridFactor &other,
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 177513f60..af4c993df 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -30,11 +30,16 @@ std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 EliminateHybrid(const HybridFactorGraph &factors,
                 const Ordering &frontalKeys) {
   // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
-  // few cases: continuous variable, we make a GM if there are hybrid factors;
+  // a few cases:
+  // continuous variable, we make a GM if there are hybrid factors;
   // continuous variable, we make a GF if there are no hybrid factors;
   // discrete variable, no continuous factor is allowed (escapes CG regime), so
   // we panic, if discrete only we do the discrete elimination.
 
+  // However it is not that simple. During elimination it is possible that the multifrontal needs
+  // to eliminate an ordering that contains both Gaussian and hybrid variables, for example x1, c1.
+  // In this scenario, we will have a density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
+
   // The issue here is that, how can we know which variable is discrete if we
   // unify Values? Obviously we can tell using the factors, but is that fast?
 
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 7292cb3f5..f97989506 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -88,7 +88,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
-TEST(HybridFactorGraph, eliminateFullMultifrontal) {
+TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
 
   std::cout << ">>>>>>>>>>>>>>\n";
 
@@ -117,6 +117,35 @@ TEST(HybridFactorGraph, eliminateFullMultifrontal) {
   GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
+TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
+
+  std::cout << ">>>>>>>>>>>>>>\n";
+
+  HybridFactorGraph hfg;
+
+  DiscreteKey x(C(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Z_3x1),
+                                                   boost::make_shared<JacobianFactor>(X(1),
+                                                                                      I_3x3,
+                                                                                      Vector3::Ones()));
+
+  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+//  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+
+  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
+
+  GTSAM_PRINT(*result);
+  GTSAM_PRINT(*result->marginalFactor(C(1)));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From fe5dde7e27e080f5a9abcf4692bdf16dc3164fce Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Mon, 14 Mar 2022 19:22:20 -0400
Subject: [PATCH 020/175] even better printing and comments

---
 gtsam/hybrid/HybridFactorGraph.cpp           | 30 ++++++++++++++++----
 gtsam/hybrid/tests/testHybridConditional.cpp | 26 ++++++++++++-----
 2 files changed, 44 insertions(+), 12 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index af4c993df..03610166e 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -2,6 +2,7 @@
 // Created by Fan Jiang on 3/11/22.
 //
 
+#include "gtsam/inference/Key.h"
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
@@ -12,6 +13,7 @@
 
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
+#include <iostream>
 #include <unordered_map>
 
 namespace gtsam {
@@ -57,6 +59,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
   KeySet continuousSeparator;
 
   // TODO: we do a mock by just doing the correct key thing
+  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
+  frontalKeys.print();
 
   // This initializes separatorKeys and discreteCardinalities
   for (auto &&factor : factors) {
@@ -94,12 +98,28 @@ EliminateHybrid(const HybridFactorGraph &factors,
     }
   }
 
-  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
-  frontalKeys.print();
+  std::cout << RED_BOLD << "Keys: " << RESET;
+  for (auto &f : frontalKeys) {
+    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+      auto &key = discreteCardinalities.at(f);
+      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+    } else {
+      std::cout << " " << DefaultKeyFormatter(f) << ",";
+    }
+  }
 
-  std::cout << RED_BOLD << "Discrete Keys: " << RESET;
-  for (auto &&key : discreteCardinalities)
-    std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.second.first) % key.second.second;
+  if (separatorKeys.size() > 0) {
+    std::cout << " | ";
+  }
+
+  for (auto &f : separatorKeys) {
+    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+      auto &key = discreteCardinalities.at(f);
+      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+    } else {
+      std::cout << DefaultKeyFormatter(f) << ",";
+    }
+  }
   std::cout << "\n" << RESET;
   // PRODUCT: multiply all factors
   gttic(product);
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index f97989506..c682f5216 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -15,6 +15,7 @@
  *  @author Fan Jiang
  */
 
+#include "Test.h"
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
@@ -94,7 +95,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
 
   HybridFactorGraph hfg;
 
-  DiscreteKey x(C(1), 2);
+  DiscreteKey c1(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
@@ -107,9 +108,12 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
                                                                                       I_3x3,
                                                                                       Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
+  hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, {C(1), 2}}, "1 2 2 1")));
 
   auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
@@ -123,7 +127,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
 
   HybridFactorGraph hfg;
 
-  DiscreteKey x(C(1), 2);
+  DiscreteKey c(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
@@ -136,14 +140,22 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
                                                                                       I_3x3,
                                                                                       Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {x}, dt));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(CGMixtureFactor({X(1)}, {c}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
 //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
 
   auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   GTSAM_PRINT(*result);
-  GTSAM_PRINT(*result->marginalFactor(C(1)));
+
+  // We immediately need to escape the CLG domain if we do this!!!
+  GTSAM_PRINT(*result->marginalFactor(X(1)));
+  /*
+  Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which
+  is not possible because it involves eliminating discrete before continuous. The solution to this,
+  however, is in Murphy02. TLDR is that this is 1. expensive and 2. inexact. neverless it is doable.
+  And I believe that we should do this.
+  */
 }
 
 /* ************************************************************************* */

From f237438bf30da2dab660761eb2523570c90121b5 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 15 Mar 2022 12:50:31 -0400
Subject: [PATCH 021/175] Address comments

---
 gtsam/hybrid/CGMixtureFactor.cpp             | 22 ++++-
 gtsam/hybrid/CGMixtureFactor.h               | 16 ++--
 gtsam/hybrid/CLGaussianConditional.cpp       | 40 +++++----
 gtsam/hybrid/CLGaussianConditional.h         | 18 ++--
 gtsam/hybrid/HybridBayesTree.h               | 15 ++--
 gtsam/hybrid/HybridConditional.cpp           | 33 ++++++--
 gtsam/hybrid/HybridConditional.h             | 36 ++++----
 gtsam/hybrid/HybridDiscreteFactor.cpp        | 29 +++++--
 gtsam/hybrid/HybridDiscreteFactor.h          | 10 ++-
 gtsam/hybrid/HybridEliminationTree.h         | 45 +++++-----
 gtsam/hybrid/HybridFactor.cpp                | 32 ++++---
 gtsam/hybrid/HybridFactor.h                  | 61 +++++++-------
 gtsam/hybrid/HybridFactorGraph.cpp           | 72 +++++++++-------
 gtsam/hybrid/HybridFactorGraph.h             | 54 ++++++------
 gtsam/hybrid/HybridGaussianFactor.cpp        | 35 +++++---
 gtsam/hybrid/HybridGaussianFactor.h          | 10 ++-
 gtsam/hybrid/HybridJunctionTree.cpp          | 10 +--
 gtsam/hybrid/tests/testHybridConditional.cpp | 87 +++++++++-----------
 18 files changed, 346 insertions(+), 279 deletions(-)

diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp
index 16ead783e..705160273 100644
--- a/gtsam/hybrid/CGMixtureFactor.cpp
+++ b/gtsam/hybrid/CGMixtureFactor.cpp
@@ -1,6 +1,22 @@
-//
-// Created by Fan Jiang on 3/11/22.
-//
+/* ----------------------------------------------------------------------------
+
+ * 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   CGMixtureFactor.cpp
+ * @brief  A set of Gaussian factors indexed by a set of discrete keys.
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
+ * @date   Mar 12, 2022
+ */
 
 #include <gtsam/hybrid/CGMixtureFactor.h>
 
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index 7ff53b7ed..0528d5401 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -18,15 +18,15 @@
  * @date   Mar 12, 2022
  */
 
-#include <gtsam/hybrid/HybridFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
 
 namespace gtsam {
 
 class CGMixtureFactor : public HybridFactor {
-public:
+ public:
   using Base = HybridFactor;
   using This = CGMixtureFactor;
   using shared_ptr = boost::shared_ptr<This>;
@@ -38,13 +38,13 @@ public:
   CGMixtureFactor() = default;
 
   CGMixtureFactor(const KeyVector &continuousKeys,
-                  const DiscreteKeys &discreteKeys,
-                  const Factors &factors);
+                  const DiscreteKeys &discreteKeys, const Factors &factors);
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
-  void print(const std::string &s = "HybridFactor\n",
-             const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/CLGaussianConditional.cpp
index 37c82a0d5..30531c023 100644
--- a/gtsam/hybrid/CLGaussianConditional.cpp
+++ b/gtsam/hybrid/CLGaussianConditional.cpp
@@ -16,30 +16,28 @@
  * @date   Mar 12, 2022
  */
 
-#include <gtsam/hybrid/CLGaussianConditional.h>
-
 #include <gtsam/base/utilities.h>
-
-#include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/hybrid/CLGaussianConditional.h>
+#include <gtsam/inference/Conditional-inst.h>
 
 namespace gtsam {
 
-CLGaussianConditional::CLGaussianConditional(const KeyVector &continuousFrontals,
-                                             const KeyVector &continuousParents,
-                                             const DiscreteKeys &discreteParents,
-                                             const CLGaussianConditional::Conditionals &conditionals)
-    : BaseFactor(
-    CollectKeys(continuousFrontals, continuousParents), discreteParents),
-      BaseConditional(continuousFrontals.size()), conditionals_(conditionals) {
-
-}
+CLGaussianConditional::CLGaussianConditional(
+    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+    const DiscreteKeys &discreteParents,
+    const CLGaussianConditional::Conditionals &conditionals)
+    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
+                 discreteParents),
+      BaseConditional(continuousFrontals.size()),
+      conditionals_(conditionals) {}
 
 bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void CLGaussianConditional::print(const std::string &s, const KeyFormatter &formatter) const {
+void CLGaussianConditional::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
   std::cout << s << ": ";
   if (isContinuous_) std::cout << "Cont. ";
   if (isDiscrete_) std::cout << "Disc. ";
@@ -51,14 +49,14 @@ void CLGaussianConditional::print(const std::string &s, const KeyFormatter &form
   }
   std::cout << "\n";
   conditionals_.print(
-      "",
-      [&](Key k) {
-        return formatter(k);
-      }, [&](const GaussianConditional::shared_ptr &gf) -> std::string {
+      "", [&](Key k) { return formatter(k); },
+      [&](const GaussianConditional::shared_ptr &gf) -> std::string {
         RedirectCout rd;
-        if (!gf->empty()) gf->print("", formatter);
-        else return {"nullptr"};
+        if (!gf->empty())
+          gf->print("", formatter);
+        else
+          return {"nullptr"};
         return rd.str();
       });
 }
-}
\ No newline at end of file
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h
index 0319c60a7..0429f4835 100644
--- a/gtsam/hybrid/CLGaussianConditional.h
+++ b/gtsam/hybrid/CLGaussianConditional.h
@@ -16,15 +16,16 @@
  * @date   Mar 12, 2022
  */
 
-#include <gtsam/inference/Conditional.h>
-#include <gtsam/hybrid/HybridFactor.h>
-
-#include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/inference/Conditional.h>
+#include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
-class CLGaussianConditional : public HybridFactor, public Conditional<HybridFactor, CLGaussianConditional> {
-public:
+class CLGaussianConditional
+    : public HybridFactor,
+      public Conditional<HybridFactor, CLGaussianConditional> {
+ public:
   using This = CLGaussianConditional;
   using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
   using BaseFactor = HybridFactor;
@@ -34,8 +35,7 @@ public:
 
   Conditionals conditionals_;
 
-public:
-
+ public:
   CLGaussianConditional(const KeyVector &continuousFrontals,
                         const KeyVector &continuousParents,
                         const DiscreteKeys &discreteParents,
@@ -47,4 +47,4 @@ public:
       const std::string &s = "CLGaussianConditional\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
-}
\ No newline at end of file
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 06df66112..74b967fc8 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -11,8 +11,7 @@
 
 /**
  * @file    HybridBayesTree.h
- * @brief   Hybrid Bayes Tree, the result of eliminating a
- * HybridJunctionTree
+ * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
  * @date Mar 11, 2022
  * @author  Fan Jiang
  */
@@ -22,8 +21,8 @@
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/BayesTree.h>
-#include <gtsam/inference/Conditional.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
+#include <gtsam/inference/Conditional.h>
 
 #include <string>
 
@@ -39,22 +38,18 @@ class GTSAM_EXPORT HybridBayesTreeClique
     : public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
  public:
   typedef HybridBayesTreeClique This;
-  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>
-      Base;
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> Base;
   typedef boost::shared_ptr<This> shared_ptr;
   typedef boost::weak_ptr<This> weak_ptr;
   HybridBayesTreeClique() {}
   virtual ~HybridBayesTreeClique() {}
-  HybridBayesTreeClique(
-      const boost::shared_ptr<HybridConditional>& conditional)
+  HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
       : Base(conditional) {}
-
 };
 
 /* ************************************************************************* */
 /** A Bayes tree representing a Hybrid density */
-class GTSAM_EXPORT HybridBayesTree
-    : public BayesTree<HybridBayesTreeClique> {
+class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
  private:
   typedef BayesTree<HybridBayesTreeClique> Base;
 
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index b3009d5b9..5cb98d921 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -1,6 +1,19 @@
-//
-// Created by Fan Jiang on 3/11/22.
-//
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridConditional.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
 
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/Conditional-inst.h>
@@ -14,7 +27,10 @@ void HybridConditional::print(const std::string &s,
   const size_t contN = N - discreteKeys_.size();
   while (index < N) {
     if (index > 0) {
-      if (index == nrFrontals_) std::cout << " | "; else std::cout << ", ";
+      if (index == nrFrontals_)
+        std::cout << " | ";
+      else
+        std::cout << ", ";
     }
     if (index < contN) {
       std::cout << formatter(keys()[index]);
@@ -28,13 +44,12 @@ void HybridConditional::print(const std::string &s,
   if (inner) inner->print("", formatter);
 }
 
-bool HybridConditional::equals(const HybridFactor &other,
-                                      double tol) const {
+bool HybridConditional::equals(const HybridFactor &other, double tol) const {
   return false;
 }
 
-HybridConditional HybridConditional::operator*(const HybridConditional &other) const {
+HybridConditional HybridConditional::operator*(
+    const HybridConditional &other) const {
   return {};
 }
-}
-
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 92856cae2..89071092f 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -17,11 +17,10 @@
 
 #pragma once
 
-#include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/inference/Conditional.h>
 
-
 #include <boost/make_shared.hpp>
 #include <boost/shared_ptr.hpp>
 #include <string>
@@ -38,33 +37,30 @@ namespace gtsam {
  * - GaussianConditional
  */
 class GTSAM_EXPORT HybridConditional
-: public HybridFactor,
-public Conditional<HybridFactor, HybridConditional> {
-public:
+    : public HybridFactor,
+      public Conditional<HybridFactor, HybridConditional> {
+ public:
   // typedefs needed to play nice with gtsam
-  typedef HybridConditional This;            ///< Typedef to this class
+  typedef HybridConditional This;              ///< Typedef to this class
   typedef boost::shared_ptr<This> shared_ptr;  ///< shared_ptr to this class
   typedef HybridFactor BaseFactor;  ///< Typedef to our factor base class
   typedef Conditional<BaseFactor, This>
       BaseConditional;  ///< Typedef to our conditional base class
 
-private:
+ private:
   // Type-erased pointer to the inner type
   std::unique_ptr<Factor> inner;
 
-public:
-/// @name Standard Constructors
-/// @{
+ public:
+  /// @name Standard Constructors
+  /// @{
 
   /// Default constructor needed for serialization.
   HybridConditional() = default;
 
-  HybridConditional(const KeyVector &continuousKeys,
-                    const DiscreteKeys &discreteKeys,
-                    size_t nFrontals)
-      : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {
-
-  }
+  HybridConditional(const KeyVector& continuousKeys,
+                    const DiscreteKeys& discreteKeys, size_t nFrontals)
+      : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
 
   /**
    * @brief Combine two conditionals, yielding a new conditional with the union
@@ -83,9 +79,9 @@ public:
    */
   HybridConditional operator*(const HybridConditional& other) const;
 
-/// @}
-/// @name Testable
-/// @{
+  /// @}
+  /// @name Testable
+  /// @{
 
   /// GTSAM-style print
   void print(
@@ -95,7 +91,7 @@ public:
   /// GTSAM-style equals
   bool equals(const HybridFactor& other, double tol = 1e-9) const override;
 
-/// @}
+  /// @}
 };
 // DiscreteConditional
 
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 1758e9025..96d3842b8 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -1,6 +1,20 @@
-//
-// Created by Fan Jiang on 3/11/22.
-//
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridDiscreteFactor.cpp
+ *  @brief Wrapper for a discrete factor
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
 
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 
@@ -15,17 +29,16 @@ HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
 
 HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
     : Base(dtf.discreteKeys()),
-      inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {
-
-}
+      inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
 
 bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void HybridDiscreteFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+void HybridDiscreteFactor::print(const std::string &s,
+                                 const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   inner->print("inner: ", formatter);
 };
 
-}
\ No newline at end of file
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 9d574b736..09da9cf70 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -15,8 +15,8 @@
  *  @author Fan Jiang
  */
 
-#include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/hybrid/HybridFactor.h>
 
 namespace gtsam {
@@ -36,8 +36,10 @@ class HybridDiscreteFactor : public HybridFactor {
   HybridDiscreteFactor(DecisionTreeFactor &&dtf);
 
  public:
-  virtual bool equals(const HybridFactor& lf, double tol) const override;
+  virtual bool equals(const HybridFactor &lf, double tol) const override;
 
-  void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
index e218ce9f6..b72bbcad9 100644
--- a/gtsam/hybrid/HybridEliminationTree.h
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -23,40 +23,39 @@
 
 namespace gtsam {
 
-class GTSAM_EXPORT HybridEliminationTree :
-    public EliminationTree<HybridBayesNet, HybridFactorGraph>
-{
+class GTSAM_EXPORT HybridEliminationTree
+    : public EliminationTree<HybridBayesNet, HybridFactorGraph> {
  public:
-  typedef EliminationTree<HybridBayesNet, HybridFactorGraph> Base; ///< Base class
-  typedef HybridEliminationTree This; ///< This class
-  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+  typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
+      Base;                                    ///< Base class
+  typedef HybridEliminationTree This;          ///< This class
+  typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
   /**
-  * Build the elimination tree of a factor graph using pre-computed column structure.
-  * @param factorGraph The factor graph for which to build the elimination tree
-  * @param structure The set of factors involving each variable.  If this is not
-  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
-  * named constructor instead.
-  * @return The elimination tree
-  */
+   * Build the elimination tree of a factor graph using pre-computed column
+   * structure.
+   * @param factorGraph The factor graph for which to build the elimination tree
+   * @param structure The set of factors involving each variable.  If this is
+   * not precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+   * named constructor instead.
+   * @return The elimination tree
+   */
   HybridEliminationTree(const HybridFactorGraph& factorGraph,
-                          const VariableIndex& structure, const Ordering& order);
+                        const VariableIndex& structure, const Ordering& order);
 
-  /** Build the elimination tree of a factor graph.  Note that this has to compute the column
-  * structure as a VariableIndex, so if you already have this precomputed, use the other
-  * constructor instead.
-  * @param factorGraph The factor graph for which to build the elimination tree
-  */
+  /** Build the elimination tree of a factor graph.  Note that this has to
+   * compute the column structure as a VariableIndex, so if you already have
+   * this precomputed, use the other constructor instead.
+   * @param factorGraph The factor graph for which to build the elimination tree
+   */
   HybridEliminationTree(const HybridFactorGraph& factorGraph,
-                          const Ordering& order);
+                        const Ordering& order);
 
   /** Test whether the tree is equal to another */
   bool equals(const This& other, double tol = 1e-9) const;
 
  private:
-
   friend class ::EliminationTreeTester;
-
 };
 
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 171ea790a..f16092eff 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -19,11 +19,12 @@
 
 namespace gtsam {
 
-KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) {
+KeyVector CollectKeys(const KeyVector &continuousKeys,
+                      const DiscreteKeys &discreteKeys) {
   KeyVector allKeys;
-  std::copy(continuousKeys.begin(), continuousKeys.end(), std::back_inserter(allKeys));
-  std::transform(discreteKeys.begin(),
-                 discreteKeys.end(),
+  std::copy(continuousKeys.begin(), continuousKeys.end(),
+            std::back_inserter(allKeys));
+  std::transform(discreteKeys.begin(), discreteKeys.end(),
                  std::back_inserter(allKeys),
                  [](const DiscreteKey &k) { return k.first; });
   return allKeys;
@@ -36,7 +37,8 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
   return allKeys;
 }
 
-DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2) {
+DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
+                                 const DiscreteKeys &key2) {
   DiscreteKeys allKeys;
   std::copy(key1.begin(), key1.end(), std::back_inserter(allKeys));
   std::copy(key2.begin(), key2.end(), std::back_inserter(allKeys));
@@ -45,16 +47,20 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &k
 
 HybridFactor::HybridFactor() = default;
 
-HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true) {}
+HybridFactor::HybridFactor(const KeyVector &keys)
+    : Base(keys), isContinuous_(true) {}
 
-HybridFactor::HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
-    : Base(
-    CollectKeys(continuousKeys, discreteKeys)), isHybrid_(true), discreteKeys_(discreteKeys) {}
+HybridFactor::HybridFactor(const KeyVector &continuousKeys,
+                           const DiscreteKeys &discreteKeys)
+    : Base(CollectKeys(continuousKeys, discreteKeys)),
+      isHybrid_(true),
+      discreteKeys_(discreteKeys) {}
 
-HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys) : Base(CollectKeys({}, discreteKeys)),
-                                                               isDiscrete_(true),
-                                                               discreteKeys_(discreteKeys) {}
+HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
+    : Base(CollectKeys({}, discreteKeys)),
+      isDiscrete_(true),
+      discreteKeys_(discreteKeys) {}
 
 HybridFactor::~HybridFactor() = default;
 
-}
\ No newline at end of file
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index dd925fee2..cd562869e 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -17,29 +17,30 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Factor.h>
-#include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/inference/Factor.h>
+#include <gtsam/nonlinear/Values.h>
 
 #include <string>
 namespace gtsam {
 
-KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
+KeyVector CollectKeys(const KeyVector &continuousKeys,
+                      const DiscreteKeys &discreteKeys);
 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
-DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, const DiscreteKeys &key2);
+DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
+                                 const DiscreteKeys &key2);
 
 /**
  * Base class for hybrid probabilistic factors
  */
 class GTSAM_EXPORT HybridFactor : public Factor {
-
-public:
-
+ public:
   // typedefs needed to play nice with gtsam
-  typedef HybridFactor This; ///< This class
-  typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
-  typedef Factor Base; ///< Our base class
+  typedef HybridFactor This;  ///< This class
+  typedef boost::shared_ptr<HybridFactor>
+      shared_ptr;       ///< shared_ptr to this class
+  typedef Factor Base;  ///< Our base class
 
   bool isDiscrete_ = false;
   bool isContinuous_ = false;
@@ -47,31 +48,33 @@ public:
 
   DiscreteKeys discreteKeys_;
 
-public:
-
-/// @name Standard Constructors
-/// @{
+ public:
+  /// @name Standard Constructors
+  /// @{
 
   /** Default constructor creates empty factor */
   HybridFactor();
 
-  /** Construct from container of keys.  This constructor is used internally from derived factor
-   *  constructors, either from a container of keys or from a boost::assign::list_of. */
-//  template<typename CONTAINER>
-//  HybridFactor(const CONTAINER &keys) : Base(keys) {}
+  /** Construct from container of keys.  This constructor is used internally
+   * from derived factor
+   *  constructors, either from a container of keys or from a
+   * boost::assign::list_of. */
+  //  template<typename CONTAINER>
+  //  HybridFactor(const CONTAINER &keys) : Base(keys) {}
 
   explicit HybridFactor(const KeyVector &keys);
 
-  HybridFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys);
+  HybridFactor(const KeyVector &continuousKeys,
+               const DiscreteKeys &discreteKeys);
 
   explicit HybridFactor(const DiscreteKeys &discreteKeys);
 
   /// Virtual destructor
   virtual ~HybridFactor();
 
-/// @}
-/// @name Testable
-/// @{
+  /// @}
+  /// @name Testable
+  /// @{
 
   /// equals
   virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0;
@@ -87,16 +90,16 @@ public:
     this->printKeys("", formatter);
   }
 
-/// @}
-/// @name Standard Interface
-/// @{
+  /// @}
+  /// @name Standard Interface
+  /// @{
 
-/// @}
+  /// @}
 };
 // HybridFactor
 
 // traits
-template<>
+template <>
 struct traits<HybridFactor> : public Testable<HybridFactor> {};
 
-}// namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 03610166e..7d4daaceb 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -1,25 +1,37 @@
-//
-// Created by Fan Jiang on 3/11/22.
-//
+/* ----------------------------------------------------------------------------
 
-#include "gtsam/inference/Key.h"
-#include <gtsam/hybrid/HybridEliminationTree.h>
-#include <gtsam/hybrid/HybridJunctionTree.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
-#include <gtsam/hybrid/HybridFactor.h>
+ * 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   HybridFactorGraph.cpp
+ * @brief  Hybrid factor graph that uses type erasure
+ * @author Fan Jiang
+ * @date   Mar 11, 2022
+ */
 
-#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
-
+#include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 
 #include <iostream>
 #include <unordered_map>
 
+#include "gtsam/inference/Key.h"
+
 namespace gtsam {
 
-template
-class EliminateableFactorGraph<HybridFactorGraph>;
+template class EliminateableFactorGraph<HybridFactorGraph>;
 
 static std::string BLACK_BOLD = "\033[1;30m";
 static std::string RED_BOLD = "\033[1;31m";
@@ -29,8 +41,7 @@ static std::string RESET = "\033[0m";
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-EliminateHybrid(const HybridFactorGraph &factors,
-                const Ordering &frontalKeys) {
+EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
   // a few cases:
   // continuous variable, we make a GM if there are hybrid factors;
@@ -38,9 +49,10 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // discrete variable, no continuous factor is allowed (escapes CG regime), so
   // we panic, if discrete only we do the discrete elimination.
 
-  // However it is not that simple. During elimination it is possible that the multifrontal needs
-  // to eliminate an ordering that contains both Gaussian and hybrid variables, for example x1, c1.
-  // In this scenario, we will have a density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
+  // However it is not that simple. During elimination it is possible that the
+  // multifrontal needs to eliminate an ordering that contains both Gaussian and
+  // hybrid variables, for example x1, c1. In this scenario, we will have a
+  // density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
 
   // The issue here is that, how can we know which variable is discrete if we
   // unify Values? Obviously we can tell using the factors, but is that fast?
@@ -102,7 +114,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
   for (auto &f : frontalKeys) {
     if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
       auto &key = discreteCardinalities.at(f);
-      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+      std::cout << boost::format(" (%1%,%2%),") %
+                       DefaultKeyFormatter(key.first) % key.second;
     } else {
       std::cout << " " << DefaultKeyFormatter(f) << ",";
     }
@@ -115,7 +128,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
   for (auto &f : separatorKeys) {
     if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
       auto &key = discreteCardinalities.at(f);
-      std::cout << boost::format(" (%1%,%2%),") % DefaultKeyFormatter(key.first) % key.second;
+      std::cout << boost::format(" (%1%,%2%),") %
+                       DefaultKeyFormatter(key.first) % key.second;
     } else {
       std::cout << DefaultKeyFormatter(f) << ",";
     }
@@ -124,9 +138,10 @@ EliminateHybrid(const HybridFactorGraph &factors,
   // PRODUCT: multiply all factors
   gttic(product);
 
-  HybridConditional sum(KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
-                        DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
-                        separatorKeys.size());
+  HybridConditional sum(
+      KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
+      DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
+      separatorKeys.size());
 
   //  HybridDiscreteFactor product(DiscreteConditional());
   //  for (auto&& factor : factors) product = (*factor) * product;
@@ -139,10 +154,8 @@ EliminateHybrid(const HybridFactorGraph &factors,
 
   // Ordering keys for the conditional so that frontalKeys are really in front
   Ordering orderedKeys;
-  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
-                     frontalKeys.end());
-  orderedKeys.insert(orderedKeys.end(), sum.keys().begin(),
-                     sum.keys().end());
+  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
+  orderedKeys.insert(orderedKeys.end(), sum.keys().begin(), sum.keys().end());
 
   // now divide product/sum to get conditional
   gttic(divide);
@@ -163,12 +176,11 @@ EliminateHybrid(const HybridFactorGraph &factors,
   std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
 
   //  return std::make_pair(conditional, sum);
-  return std::make_pair(
-      conditional,
-      boost::make_shared<HybridConditional>(std::move(sum)));
+  return std::make_pair(conditional,
+                        boost::make_shared<HybridConditional>(std::move(sum)));
 }
 
 void HybridFactorGraph::add(JacobianFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
 }
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 9a57d36e6..92f98c8f2 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -36,43 +36,50 @@ class HybridJunctionTree;
 class JacobianFactor;
 
 /** Main elimination function for HybridFactorGraph */
-GTSAM_EXPORT std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
-EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
+GTSAM_EXPORT
+    std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
+    EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
 
 /* ************************************************************************* */
-template<> struct EliminationTraits<HybridFactorGraph>
-{
-  typedef HybridFactor FactorType;                   ///< Type of factors in factor graph
-  typedef HybridFactorGraph FactorGraphType;         ///< Type of the factor graph (e.g. HybridFactorGraph)
-  typedef HybridConditional ConditionalType;         ///< Type of conditionals from elimination
-  typedef HybridBayesNet BayesNetType;               ///< Type of Bayes net from sequential elimination
-  typedef HybridEliminationTree EliminationTreeType; ///< Type of elimination tree
-  typedef HybridBayesTree BayesTreeType;             ///< Type of Bayes tree
-  typedef HybridJunctionTree JunctionTreeType;       ///< Type of Junction tree
+template <>
+struct EliminationTraits<HybridFactorGraph> {
+  typedef HybridFactor FactorType;  ///< Type of factors in factor graph
+  typedef HybridFactorGraph
+      FactorGraphType;  ///< Type of the factor graph (e.g. HybridFactorGraph)
+  typedef HybridConditional
+      ConditionalType;  ///< Type of conditionals from elimination
+  typedef HybridBayesNet
+      BayesNetType;  ///< Type of Bayes net from sequential elimination
+  typedef HybridEliminationTree
+      EliminationTreeType;                      ///< Type of elimination tree
+  typedef HybridBayesTree BayesTreeType;        ///< Type of Bayes tree
+  typedef HybridJunctionTree JunctionTreeType;  ///< Type of Junction tree
   /// The default dense elimination function
-  static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
+  static std::pair<boost::shared_ptr<ConditionalType>,
+                   boost::shared_ptr<FactorType> >
   DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
-    return EliminateHybrid(factors, keys); }
+    return EliminateHybrid(factors, keys);
+  }
 };
 
-
-class HybridFactorGraph : public FactorGraph<HybridFactor>, public EliminateableFactorGraph<HybridFactorGraph> {
+class HybridFactorGraph : public FactorGraph<HybridFactor>,
+                          public EliminateableFactorGraph<HybridFactorGraph> {
  public:
   using Base = FactorGraph<HybridFactor>;
-  using This = HybridFactorGraph;          ///< this class
+  using This = HybridFactorGraph;  ///< this class
   using BaseEliminateable =
-  EliminateableFactorGraph<This>;          ///< for elimination
+      EliminateableFactorGraph<This>;          ///< for elimination
   using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
 
   using Values = gtsam::Values;  ///< backwards compatibility
-  using Indices = KeyVector;  ///> map from keys to values
+  using Indices = KeyVector;     ///> map from keys to values
 
  public:
   HybridFactorGraph() = default;
 
-//  /** Construct from container of factors (shared_ptr or plain objects) */
-//  template <class CONTAINER>
-//  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
+  //  /** Construct from container of factors (shared_ptr or plain objects) */
+  //  template <class CONTAINER>
+  //  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
 
   /** Implicit copy/downcast constructor to override explicit template container
    * constructor. In BayesTree this is used for:
@@ -84,8 +91,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>, public Eliminateable
   using FactorGraph::add;
 
   /// Add a factor directly using a shared_ptr.
-  void add(JacobianFactor &&factor);
+  void add(JacobianFactor&& factor);
 };
 
-}
-
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index faa4ba998..d83f90024 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -1,6 +1,19 @@
-//
-// Created by Fan Jiang on 3/11/22.
-//
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridGaussianFactor.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
 
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 
@@ -8,20 +21,22 @@
 
 namespace gtsam {
 
-HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other) : Base(other->keys()) {
+HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
+    : Base(other->keys()) {
   inner = other;
 }
 
-HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf) : Base(jf.keys()),  inner(boost::make_shared<JacobianFactor>(std::move(jf))) {
+HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
+    : Base(jf.keys()),
+      inner(boost::make_shared<JacobianFactor>(std::move(jf))) {}
 
-}
-
-bool HybridGaussianFactor::equals(const HybridFactor& lf, double tol) const {
+bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
-void HybridGaussianFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+void HybridGaussianFactor::print(const std::string &s,
+                                 const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   inner->print("inner: ", formatter);
 };
 
-}
\ No newline at end of file
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 8562075b4..23a3c0110 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -15,9 +15,9 @@
  *  @author Fan Jiang
  */
 
+#include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/hybrid/HybridFactor.h>
 
 namespace gtsam {
 
@@ -36,8 +36,10 @@ class HybridGaussianFactor : public HybridFactor {
   explicit HybridGaussianFactor(JacobianFactor &&jf);
 
  public:
-  virtual bool equals(const HybridFactor& lf, double tol) const override;
+  virtual bool equals(const HybridFactor &lf, double tol) const override;
 
-  void print(const std::string &s = "HybridFactor\n", const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 0e3d2ea00..9da1bfed3 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -15,9 +15,9 @@
  * @author Fan Jiang
  */
 
-#include <gtsam/inference/JunctionTree-inst.h>
-#include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/inference/JunctionTree-inst.h>
 
 namespace gtsam {
 
@@ -27,7 +27,7 @@ template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
 
 /* ************************************************************************* */
 HybridJunctionTree::HybridJunctionTree(
-    const HybridEliminationTree& eliminationTree) :
-    Base(eliminationTree) {}
+    const HybridEliminationTree& eliminationTree)
+    : Base(eliminationTree) {}
 
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index c682f5216..f945d0702 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -15,32 +15,30 @@
  *  @author Fan Jiang
  */
 
-#include "Test.h"
-#include <gtsam/hybrid/HybridFactor.h>
-#include <gtsam/hybrid/HybridConditional.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
-#include <gtsam/hybrid/HybridGaussianFactor.h>
-#include <gtsam/hybrid/HybridDiscreteFactor.h>
-#include <gtsam/hybrid/CLGaussianConditional.h>
+#include <CppUnitLite/Test.h>
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/hybrid/CGMixtureFactor.h>
+#include <gtsam/hybrid/CLGaussianConditional.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
-
-#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/inference/BayesNet.h>
-
 #include <gtsam/inference/Symbol.h>
-
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/linear/JacobianFactor.h>
 
 #include <boost/assign/std/map.hpp>
+
 using namespace boost::assign;
 
 using namespace std;
 using namespace gtsam;
 
-using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::C;
+using gtsam::symbol_shorthand::X;
 
 /* ************************************************************************* */
 TEST(HybridFactorGraph, creation) {
@@ -51,17 +49,13 @@ TEST(HybridFactorGraph, creation) {
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
   CLGaussianConditional clgc(
-      {X(0)}, {X(1)},
-      DiscreteKeys(DiscreteKey{C(0), 2}),
+      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
       CLGaussianConditional::Conditionals(
           C(0),
-          boost::make_shared<GaussianConditional>(
-              X(0), Z_3x1, I_3x3, X(1), I_3x3),
-          boost::make_shared<GaussianConditional>(
-              X(0), Vector3::Ones(),
-              I_3x3, X(1),
-              I_3x3))
-  );
+          boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
+                                                  I_3x3),
+          boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
+                                                  X(1), I_3x3)));
   GTSAM_PRINT(clgc);
 }
 
@@ -90,7 +84,6 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
 }
 
 TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
-
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -100,29 +93,27 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
-                                                   boost::make_shared<JacobianFactor>(X(1),
-                                                                                      I_3x3,
-                                                                                      Z_3x1),
-                                                   boost::make_shared<JacobianFactor>(X(1),
-                                                                                      I_3x3,
-                                                                                      Vector3::Ones()));
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
   hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
   hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
-  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1 2 3 4")));
-  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2}, {C(1), 2}}, "1 2 2 1")));
+  hfg.add(HybridDiscreteFactor(
+      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
+  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
+  // {C(1), 2}}, "1 2 2 1")));
 
-  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
+  auto result = hfg.eliminateMultifrontal(
+      Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
   GTSAM_PRINT(*result);
   GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
 TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
-
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -132,29 +123,28 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  DecisionTree<Key, GaussianFactor::shared_ptr> dt(C(1),
-                                                   boost::make_shared<JacobianFactor>(X(1),
-                                                                                      I_3x3,
-                                                                                      Z_3x1),
-                                                   boost::make_shared<JacobianFactor>(X(1),
-                                                                                      I_3x3,
-                                                                                      Vector3::Ones()));
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
   hfg.add(CGMixtureFactor({X(1)}, {c}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
-//  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
+  //  2 3 4")));
 
-  auto result = hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
+  auto result =
+      hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
 
   GTSAM_PRINT(*result);
 
   // We immediately need to escape the CLG domain if we do this!!!
   GTSAM_PRINT(*result->marginalFactor(X(1)));
   /*
-  Explanation: the Junction tree will need to reeliminate to get to the marginal on X(1), which
-  is not possible because it involves eliminating discrete before continuous. The solution to this,
-  however, is in Murphy02. TLDR is that this is 1. expensive and 2. inexact. neverless it is doable.
-  And I believe that we should do this.
+  Explanation: the Junction tree will need to reeliminate to get to the marginal
+  on X(1), which is not possible because it involves eliminating discrete before
+  continuous. The solution to this, however, is in Murphy02. TLDR is that this
+  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
+  should do this.
   */
 }
 
@@ -164,4 +154,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */
-

From 36ee4ce7cbc8dc5ffdf5ca71b107fd7a2dc3beb0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 15 Mar 2022 12:53:58 -0400
Subject: [PATCH 022/175] Missing pragma onces

---
 gtsam/CMakeLists.txt                 | 2 +-
 gtsam/hybrid/CGMixtureFactor.h       | 2 ++
 gtsam/hybrid/CLGaussianConditional.h | 2 ++
 gtsam/hybrid/HybridDiscreteFactor.h  | 2 ++
 gtsam/hybrid/HybridGaussianFactor.h  | 2 ++
 5 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 845ac7cd0..09f1ea806 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -10,7 +10,7 @@ set (gtsam_subdirs
     inference
     symbolic
     discrete
-	hybrid
+    hybrid
     linear
     nonlinear
     sam
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index 0528d5401..556c53cc5 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -18,6 +18,8 @@
  * @date   Mar 12, 2022
  */
 
+#pragma once
+
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/CLGaussianConditional.h
index 0429f4835..d80cb804f 100644
--- a/gtsam/hybrid/CLGaussianConditional.h
+++ b/gtsam/hybrid/CLGaussianConditional.h
@@ -16,6 +16,8 @@
  * @date   Mar 12, 2022
  */
 
+#pragma once
+
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/inference/Conditional.h>
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 09da9cf70..f182f90a9 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -15,6 +15,8 @@
  *  @author Fan Jiang
  */
 
+#pragma once
+
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteFactor.h>
 #include <gtsam/hybrid/HybridFactor.h>
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 23a3c0110..03c49564e 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -15,6 +15,8 @@
  *  @author Fan Jiang
  */
 
+#pragma once
+
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>

From 7ad2031b2fa81710fac83d96b021d4830d296ba7 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 22 Mar 2022 15:37:28 -0400
Subject: [PATCH 023/175] Now we have real multifrontal!

---
 ...ianConditional.cpp => GaussianMixture.cpp} |  12 +-
 ...aussianConditional.h => GaussianMixture.h} |  16 +-
 gtsam/hybrid/HybridConditional.cpp            |  24 ++-
 gtsam/hybrid/HybridConditional.h              |  11 +-
 gtsam/hybrid/HybridFactor.cpp                 |  14 +-
 gtsam/hybrid/HybridFactor.h                   |   8 +-
 gtsam/hybrid/HybridFactorGraph.cpp            |  92 ++++++++---
 gtsam/hybrid/HybridJunctionTree.cpp           | 152 +++++++++++++++++-
 gtsam/hybrid/tests/testHybridConditional.cpp  |  83 ++++++++--
 gtsam/inference/JunctionTree.h                |   2 +-
 10 files changed, 360 insertions(+), 54 deletions(-)
 rename gtsam/hybrid/{CLGaussianConditional.cpp => GaussianMixture.cpp} (83%)
 rename gtsam/hybrid/{CLGaussianConditional.h => GaussianMixture.h} (74%)

diff --git a/gtsam/hybrid/CLGaussianConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp
similarity index 83%
rename from gtsam/hybrid/CLGaussianConditional.cpp
rename to gtsam/hybrid/GaussianMixture.cpp
index 30531c023..6af5f7192 100644
--- a/gtsam/hybrid/CLGaussianConditional.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   CLGaussianConditional.cpp
+ * @file   GaussianMixture.cpp
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @date   Mar 12, 2022
@@ -18,25 +18,25 @@
 
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
-#include <gtsam/hybrid/CLGaussianConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/Conditional-inst.h>
 
 namespace gtsam {
 
-CLGaussianConditional::CLGaussianConditional(
+GaussianMixture::GaussianMixture(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
-    const CLGaussianConditional::Conditionals &conditionals)
+    const GaussianMixture::Conditionals &conditionals)
     : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
                  discreteParents),
       BaseConditional(continuousFrontals.size()),
       conditionals_(conditionals) {}
 
-bool CLGaussianConditional::equals(const HybridFactor &lf, double tol) const {
+bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void CLGaussianConditional::print(const std::string &s,
+void GaussianMixture::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   std::cout << s << ": ";
   if (isContinuous_) std::cout << "Cont. ";
diff --git a/gtsam/hybrid/CLGaussianConditional.h b/gtsam/hybrid/GaussianMixture.h
similarity index 74%
rename from gtsam/hybrid/CLGaussianConditional.h
rename to gtsam/hybrid/GaussianMixture.h
index d80cb804f..541a2b8a6 100644
--- a/gtsam/hybrid/CLGaussianConditional.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   CLGaussianConditional.h
+ * @file   GaussianMixture.h
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @date   Mar 12, 2022
@@ -24,21 +24,21 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
-class CLGaussianConditional
+class GaussianMixture
     : public HybridFactor,
-      public Conditional<HybridFactor, CLGaussianConditional> {
+      public Conditional<HybridFactor, GaussianMixture> {
  public:
-  using This = CLGaussianConditional;
-  using shared_ptr = boost::shared_ptr<CLGaussianConditional>;
+  using This = GaussianMixture;
+  using shared_ptr = boost::shared_ptr<GaussianMixture>;
   using BaseFactor = HybridFactor;
-  using BaseConditional = Conditional<HybridFactor, CLGaussianConditional>;
+  using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
 
   using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
 
   Conditionals conditionals_;
 
  public:
-  CLGaussianConditional(const KeyVector &continuousFrontals,
+  GaussianMixture(const KeyVector &continuousFrontals,
                         const KeyVector &continuousParents,
                         const DiscreteKeys &discreteParents,
                         const Conditionals &conditionals);
@@ -46,7 +46,7 @@ class CLGaussianConditional
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
-      const std::string &s = "CLGaussianConditional\n",
+      const std::string &s = "GaussianMixture\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 }  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 5cb98d921..e212f4534 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -19,9 +19,31 @@
 #include <gtsam/inference/Conditional-inst.h>
 
 namespace gtsam {
+
+HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
+                                     const DiscreteKeys &discreteFrontals,
+                                     const KeyVector &continuousParents,
+                                     const DiscreteKeys &discreteParents)
+    : HybridConditional(
+          CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
+                      {continuousParents.begin(), continuousParents.end()}),
+          CollectDiscreteKeys(
+              {discreteFrontals.begin(), discreteFrontals.end()},
+              {discreteParents.begin(), discreteParents.end()}),
+          continuousFrontals.size() + discreteFrontals.size()) {}
+
+HybridConditional::HybridConditional(
+    boost::shared_ptr<GaussianConditional> continuousConditional)
+    : BaseFactor(continuousConditional->keys()),
+      BaseConditional(continuousConditional->nrFrontals()) {}
+
 void HybridConditional::print(const std::string &s,
                               const KeyFormatter &formatter) const {
-  std::cout << s << "P(";
+  std::cout << s;
+  if (isContinuous_) std::cout << "Cont. ";
+  if (isDiscrete_) std::cout << "Disc. ";
+  if (isHybrid_) std::cout << "Hybr. ";
+  std::cout << "P(";
   int index = 0;
   const size_t N = keys().size();
   const size_t contN = N - discreteKeys_.size();
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 89071092f..d851cd68e 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -25,6 +25,8 @@
 #include <boost/shared_ptr.hpp>
 #include <string>
 #include <vector>
+#include "gtsam/inference/Key.h"
+#include "gtsam/linear/GaussianConditional.h"
 
 namespace gtsam {
 
@@ -33,8 +35,8 @@ namespace gtsam {
  *
  * As a type-erased variant of:
  * - DiscreteConditional
- * - CLGaussianConditional
  * - GaussianConditional
+ * - GaussianMixture
  */
 class GTSAM_EXPORT HybridConditional
     : public HybridFactor,
@@ -62,6 +64,13 @@ class GTSAM_EXPORT HybridConditional
                     const DiscreteKeys& discreteKeys, size_t nFrontals)
       : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
 
+  HybridConditional(const KeyVector& continuousFrontals,
+                    const DiscreteKeys& discreteFrontals,
+                    const KeyVector& continuousParents,
+                    const DiscreteKeys& discreteParents);
+
+  HybridConditional(boost::shared_ptr<GaussianConditional> continuousConditional);
+  
   /**
    * @brief Combine two conditionals, yielding a new conditional with the union
    * of the frontal keys, ordered by gtsam::Key.
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index f16092eff..48a9dc468 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -53,7 +53,9 @@ HybridFactor::HybridFactor(const KeyVector &keys)
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
                            const DiscreteKeys &discreteKeys)
     : Base(CollectKeys(continuousKeys, discreteKeys)),
-      isHybrid_(true),
+      isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
+      isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
+      isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
       discreteKeys_(discreteKeys) {}
 
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
@@ -61,6 +63,16 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
       isDiscrete_(true),
       discreteKeys_(discreteKeys) {}
 
+void HybridFactor::print(
+      const std::string &s,
+      const KeyFormatter &formatter) const {
+  std::cout << s;
+  if (isContinuous_) std::cout << "Cont. ";
+  if (isDiscrete_) std::cout << "Disc. ";
+  if (isHybrid_) std::cout << "Hybr. ";
+  this->printKeys("", formatter);
+}
+
 HybridFactor::~HybridFactor() = default;
 
 }  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index cd562869e..84d9b71bc 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -82,13 +82,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// print
   void print(
       const std::string &s = "HybridFactor\n",
-      const KeyFormatter &formatter = DefaultKeyFormatter) const override {
-    std::cout << s;
-    if (isContinuous_) std::cout << "Cont. ";
-    if (isDiscrete_) std::cout << "Disc. ";
-    if (isHybrid_) std::cout << "Hybr. ";
-    this->printKeys("", formatter);
-  }
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
   /// @}
   /// @name Standard Interface
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 7d4daaceb..367f9f9ab 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -26,8 +26,11 @@
 
 #include <iostream>
 #include <unordered_map>
+#include <utility>
 
 #include "gtsam/inference/Key.h"
+#include "gtsam/linear/GaussianFactorGraph.h"
+#include "gtsam/linear/HessianFactor.h"
 
 namespace gtsam {
 
@@ -60,6 +63,28 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // In the case of multifrontal, we will need to use a constrained ordering
   // so that the discrete parts will be guaranteed to be eliminated last!
 
+  // Because of all these reasons, we need to think very carefully about how to
+  // implement the hybrid factors so that we do not get poor performance.
+  // 
+  // The first thing is how to represent the GaussianMixture. A very possible
+  // scenario is that the incoming factors will have different levels of discrete
+  // keys. For example, imagine we are going to eliminate the fragment:
+  // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will need
+  // to know how to retrieve the corresponding continuous densities for the assi-
+  // -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). And we 
+  // also need to consider when there is pruning. Two mixture factors could have
+  // different pruning patterns-one could have (c1=0,c2=1) pruned, and another
+  // could have (c2=0,c3=1) pruned, and this creates a big problem in how to 
+  // identify the intersection of non-pruned branches.
+
+  // One possible approach is first building the collection of all discrete keys.
+  // After that we enumerate the space of all key combinations *lazily* so that
+  // the exploration branch terminates whenever an assignment yields NULL in any
+  // of the hybrid factors.
+
+  // When the number of assignments is large we may encounter stack overflows.
+  // However this is also the case with iSAM2, so no pressure :)
+
   // PREPROCESS: Identify the nature of the current elimination
   std::unordered_map<Key, DiscreteKey> discreteCardinalities;
   std::set<DiscreteKey> discreteSeparator;
@@ -110,31 +135,62 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     }
   }
 
-  std::cout << RED_BOLD << "Keys: " << RESET;
-  for (auto &f : frontalKeys) {
-    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
-      auto &key = discreteCardinalities.at(f);
-      std::cout << boost::format(" (%1%,%2%),") %
-                       DefaultKeyFormatter(key.first) % key.second;
-    } else {
-      std::cout << " " << DefaultKeyFormatter(f) << ",";
+  {
+    std::cout << RED_BOLD << "Keys: " << RESET;
+    for (auto &f : frontalKeys) {
+      if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+        auto &key = discreteCardinalities.at(f);
+        std::cout << boost::format(" (%1%,%2%),") %
+                         DefaultKeyFormatter(key.first) % key.second;
+      } else {
+        std::cout << " " << DefaultKeyFormatter(f) << ",";
+      }
     }
+
+    if (separatorKeys.size() > 0) {
+      std::cout << " | ";
+    }
+
+    for (auto &f : separatorKeys) {
+      if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
+        auto &key = discreteCardinalities.at(f);
+        std::cout << boost::format(" (%1%,%2%),") %
+                         DefaultKeyFormatter(key.first) % key.second;
+      } else {
+        std::cout << DefaultKeyFormatter(f) << ",";
+      }
+    }
+    std::cout << "\n" << RESET;
   }
 
-  if (separatorKeys.size() > 0) {
-    std::cout << " | ";
+  // NOTE: We should really defer the product here because of pruning
+  
+  // Case 1: we are only dealing with continuous
+  if (discreteCardinalities.empty()) {
+    GaussianFactorGraph gfg;
+    for (auto &fp : factors) {
+      gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
+    }
+
+    auto result = EliminatePreferCholesky(gfg, frontalKeys);
+    return std::make_pair(
+        boost::make_shared<HybridConditional>(result.first),
+        boost::make_shared<HybridGaussianFactor>(result.second));
   }
 
-  for (auto &f : separatorKeys) {
-    if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
-      auto &key = discreteCardinalities.at(f);
-      std::cout << boost::format(" (%1%,%2%),") %
-                       DefaultKeyFormatter(key.first) % key.second;
-    } else {
-      std::cout << DefaultKeyFormatter(f) << ",";
+  // Case 2: we are only dealing with discrete
+  if (discreteCardinalities.empty()) {
+    GaussianFactorGraph gfg;
+    for (auto &fp : factors) {
+      gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
     }
+
+    auto result = EliminatePreferCholesky(gfg, frontalKeys);
+    return std::make_pair(
+        boost::make_shared<HybridConditional>(result.first),
+        boost::make_shared<HybridGaussianFactor>(result.second));
   }
-  std::cout << "\n" << RESET;
+
   // PRODUCT: multiply all factors
   gttic(product);
 
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 9da1bfed3..9445e26b8 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -19,15 +19,163 @@
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/JunctionTree-inst.h>
 
+#include <unordered_map>
+
+#include "gtsam/hybrid/HybridFactorGraph.h"
+#include "gtsam/inference/Key.h"
+
 namespace gtsam {
 
 // Instantiate base classes
 template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
 template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
 
+struct HybridConstructorTraversalData {
+  typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::Node Node;
+  typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::sharedNode
+      sharedNode;
+
+  HybridConstructorTraversalData* const parentData;
+  sharedNode myJTNode;
+  FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
+  FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
+  KeySet discreteKeys;
+
+  // Small inner class to store symbolic factors
+  class SymbolicFactors : public FactorGraph<Factor> {};
+
+  HybridConstructorTraversalData(HybridConstructorTraversalData* _parentData)
+      : parentData(_parentData) {}
+
+  // Pre-order visitor function
+  static HybridConstructorTraversalData ConstructorTraversalVisitorPre(
+      const boost::shared_ptr<HybridEliminationTree::Node>& node,
+      HybridConstructorTraversalData& parentData) {
+    // On the pre-order pass, before children have been visited, we just set up
+    // a traversal data structure with its own JT node, and create a child
+    // pointer in its parent.
+    HybridConstructorTraversalData myData =
+        HybridConstructorTraversalData(&parentData);
+    myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
+    parentData.myJTNode->addChild(myData.myJTNode);
+
+    std::cout << "Getting discrete info: ";
+    for (HybridFactor::shared_ptr& f : node->factors) {
+      for (auto& k : f->discreteKeys_) {
+        std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
+        myData.discreteKeys.insert(k.first);
+      }
+    }
+
+    return myData;
+  }
+
+  // Post-order visitor function
+  static void ConstructorTraversalVisitorPostAlg2(
+      const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
+      const HybridConstructorTraversalData& myData) {
+    // In this post-order visitor, we combine the symbolic elimination results
+    // from the elimination tree children and symbolically eliminate the current
+    // elimination tree node.  We then check whether each of our elimination
+    // tree child nodes should be merged with us.  The check for this is that
+    // our number of symbolic elimination parents is exactly 1 less than
+    // our child's symbolic elimination parents - this condition indicates that
+    // eliminating the current node did not introduce any parents beyond those
+    // already in the child->
+
+    // Do symbolic elimination for this node
+    SymbolicFactors symbolicFactors;
+    symbolicFactors.reserve(ETreeNode->factors.size() +
+                            myData.childSymbolicFactors.size());
+    // Add ETree node factors
+    symbolicFactors += ETreeNode->factors;
+    // Add symbolic factors passed up from children
+    symbolicFactors += myData.childSymbolicFactors;
+
+    Ordering keyAsOrdering;
+    keyAsOrdering.push_back(ETreeNode->key);
+    SymbolicConditional::shared_ptr myConditional;
+    SymbolicFactor::shared_ptr mySeparatorFactor;
+    boost::tie(myConditional, mySeparatorFactor) =
+        internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
+
+    std::cout << "Symbolic: ";
+    myConditional->print();
+
+    // Store symbolic elimination results in the parent
+    myData.parentData->childSymbolicConditionals.push_back(myConditional);
+    myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
+    myData.parentData->discreteKeys.merge(myData.discreteKeys);
+
+    sharedNode node = myData.myJTNode;
+    const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
+        myData.childSymbolicConditionals;
+    node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size());
+
+    // Merge our children if they are in our clique - if our conditional has
+    // exactly one fewer parent than our child's conditional.
+    const size_t myNrParents = myConditional->nrParents();
+    const size_t nrChildren = node->nrChildren();
+    assert(childConditionals.size() == nrChildren);
+
+    // decide which children to merge, as index into children
+    std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
+    std::vector<bool> merge(nrChildren, false);
+    size_t myNrFrontals = 1;
+    for (size_t i = 0; i < nrChildren; i++) {
+      // Check if we should merge the i^th child
+      if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
+        const bool myType =
+            myData.discreteKeys.exists(myConditional->frontals()[0]);
+        const bool theirType =
+            myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
+        std::cout << "Type: "
+                  << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs "
+                  << DefaultKeyFormatter(childConditionals[i]->frontals()[0])
+                  << "\n";
+        if (myType == theirType) {
+          // Increment number of frontal variables
+          myNrFrontals += nrFrontals[i];
+          std::cout << "Merging ";
+          childConditionals[i]->print();
+          merge[i] = true;
+        }
+      }
+    }
+
+    // now really merge
+    node->mergeChildren(merge);
+  }
+};
+
 /* ************************************************************************* */
 HybridJunctionTree::HybridJunctionTree(
-    const HybridEliminationTree& eliminationTree)
-    : Base(eliminationTree) {}
+    const HybridEliminationTree& eliminationTree) {
+  gttic(JunctionTree_FromEliminationTree);
+  // Here we rely on the BayesNet having been produced by this elimination tree,
+  // such that the conditionals are arranged in DFS post-order.  We traverse the
+  // elimination tree, and inspect the symbolic conditional corresponding to
+  // each node.  The elimination tree node is added to the same clique with its
+  // parent if it has exactly one more Bayes net conditional parent than
+  // does its elimination tree parent.
+
+  // Traverse the elimination tree, doing symbolic elimination and merging nodes
+  // as we go.  Gather the created junction tree roots in a dummy Node.
+  typedef typename HybridEliminationTree::Node ETreeNode;
+  typedef HybridConstructorTraversalData Data;
+  Data rootData(0);
+  rootData.myJTNode =
+      boost::make_shared<typename Base::Node>();  // Make a dummy node to gather
+                                                  // the junction tree roots
+  treeTraversal::DepthFirstForest(eliminationTree, rootData,
+                                  Data::ConstructorTraversalVisitorPre,
+                                  Data::ConstructorTraversalVisitorPostAlg2);
+
+  // Assign roots from the dummy node
+  this->addChildrenAsRoots(rootData.myJTNode);
+
+  // Transfer remaining factors from elimination tree
+  Base::remainingFactors_ = eliminationTree.remainingFactors();
+}
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index f945d0702..d16715300 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -18,7 +18,7 @@
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/hybrid/CGMixtureFactor.h>
-#include <gtsam/hybrid/CLGaussianConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -31,6 +31,7 @@
 #include <gtsam/linear/JacobianFactor.h>
 
 #include <boost/assign/std/map.hpp>
+#include "gtsam/base/Testable.h"
 
 using namespace boost::assign;
 
@@ -48,9 +49,9 @@ TEST(HybridFactorGraph, creation) {
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  CLGaussianConditional clgc(
+  GaussianMixture clgc(
       {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-      CLGaussianConditional::Conditionals(
+      GaussianMixture::Conditionals(
           C(0),
           boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
                                                   I_3x3),
@@ -69,7 +70,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
+TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
   DiscreteKey x(X(1), 2);
@@ -132,13 +133,77 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
   //  2 3 4")));
 
-  auto result =
-      hfg.eliminateMultifrontal(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
+  auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
 
-  GTSAM_PRINT(*result);
+  HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
 
-  // We immediately need to escape the CLG domain if we do this!!!
-  GTSAM_PRINT(*result->marginalFactor(X(1)));
+  GTSAM_PRINT(*hbt);
+  /*
+  Explanation: the Junction tree will need to reeliminate to get to the marginal
+  on X(1), which is not possible because it involves eliminating discrete before
+  continuous. The solution to this, however, is in Murphy02. TLDR is that this
+  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
+  should do this.
+  */
+}
+
+/**
+ * This test is about how to assemble the Bayes Tree roots after we do partial elimination
+*/
+TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
+  std::cout << ">>>>>>>>>>>>>>\n";
+
+  HybridFactorGraph hfg;
+
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
+
+  {
+    DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+        C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
+        boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
+
+    hfg.add(CGMixtureFactor({X(0)}, {{C(0), 2}}, dt));
+
+    DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
+        C(1), boost::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
+        boost::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));
+
+    hfg.add(CGMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
+  }
+
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+
+  hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
+
+  {
+    DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+        C(3), boost::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
+        boost::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));
+
+    hfg.add(CGMixtureFactor({X(3)}, {{C(3), 2}}, dt));
+
+    DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
+        C(2), boost::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
+        boost::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));
+
+    hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
+  }
+
+  auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
+
+  GTSAM_PRINT(ordering_full);
+
+  HybridBayesTree::shared_ptr hbt;
+  HybridFactorGraph::shared_ptr remaining;
+  std::tie(hbt, remaining) =
+      hfg.eliminatePartialMultifrontal(Ordering(ordering_full.begin(), ordering_full.end()));
+
+  GTSAM_PRINT(*hbt);
+
+  GTSAM_PRINT(*remaining);
   /*
   Explanation: the Junction tree will need to reeliminate to get to the marginal
   on X(1), which is not possible because it involves eliminating discrete before
diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h
index e01f3721a..e914c325e 100644
--- a/gtsam/inference/JunctionTree.h
+++ b/gtsam/inference/JunctionTree.h
@@ -70,7 +70,7 @@ namespace gtsam {
 
     /// @}
 
-  private:
+  protected:
 
     // Private default constructor (used in static construction methods)
     JunctionTree() {}

From 7b0356cbea59c3660c8e85e81f07917b007d0e9c Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Tue, 22 Mar 2022 20:40:08 -0400
Subject: [PATCH 024/175] edited

---
 gtsam/nonlinear/GncOptimizer.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index c5c5d1883..96b03e803 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -447,11 +447,11 @@ class GTSAM_EXPORT GncOptimizer {
         return weights;
       }
       case GncLossType::TLS: {  // use eq (14) in GNC paper
-        double upperbound = (mu + 1) / mu * barcSq_.maxCoeff();
-        double lowerbound = mu / (mu + 1) * barcSq_.minCoeff();
         for (size_t k : unknownWeights) {
           if (nfg_[k]) {
             double u2_k = nfg_[k]->error(currentEstimate);  // squared (and whitened) residual
+            double upperbound = (mu + 1) / mu * barcSq_[k];
+            double lowerbound = mu / (mu + 1) * barcSq_[k];
             weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
             if (u2_k >= upperbound || weights[k] < 0) {
               weights[k] = 0;

From d42fc214176fb8ced3436edc4c9662dc2f1ef65a Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:16:05 -0400
Subject: [PATCH 025/175] Fully working Multifrontal

---
 gtsam/hybrid/CGMixtureFactor.cpp             |  27 ++
 gtsam/hybrid/CGMixtureFactor.h               |  12 +
 gtsam/hybrid/GaussianMixture.cpp             |  27 ++
 gtsam/hybrid/GaussianMixture.h               |  10 +
 gtsam/hybrid/HybridConditional.cpp           |  29 +-
 gtsam/hybrid/HybridConditional.h             |  43 +--
 gtsam/hybrid/HybridDiscreteFactor.cpp        |   5 +-
 gtsam/hybrid/HybridFactor.cpp                |   3 +-
 gtsam/hybrid/HybridFactor.h                  |   3 +
 gtsam/hybrid/HybridFactorGraph.cpp           | 345 +++++++++++++++----
 gtsam/hybrid/tests/testHybridConditional.cpp |  56 +--
 11 files changed, 454 insertions(+), 106 deletions(-)

diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/CGMixtureFactor.cpp
index 705160273..08ae8b6e9 100644
--- a/gtsam/hybrid/CGMixtureFactor.cpp
+++ b/gtsam/hybrid/CGMixtureFactor.cpp
@@ -20,7 +20,9 @@
 
 #include <gtsam/hybrid/CGMixtureFactor.h>
 
+#include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/base/utilities.h>
 
 namespace gtsam {
@@ -47,4 +49,29 @@ void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter)
       });
 }
 
+const CGMixtureFactor::Factors& CGMixtureFactor::factors() {
+  return factors_;
+}
+
+/* *******************************************************************************/
+CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) const {
+  using Y = GaussianFactorGraph;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    result.push_back(graph2);
+    return result;
+  };
+  const Sum wrapped = wrappedFactors();
+  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+}
+
+/* *******************************************************************************/
+CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const {
+  auto wrap = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {factors_, wrap};
+}
 }
\ No newline at end of file
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/CGMixtureFactor.h
index 556c53cc5..fd81cdd91 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/CGMixtureFactor.h
@@ -27,6 +27,8 @@
 
 namespace gtsam {
 
+class GaussianFactorGraph;
+
 class CGMixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
@@ -42,6 +44,16 @@ class CGMixtureFactor : public HybridFactor {
   CGMixtureFactor(const KeyVector &continuousKeys,
                   const DiscreteKeys &discreteKeys, const Factors &factors);
 
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  const Factors& factors();
+
+  /* *******************************************************************************/
+  Sum addTo(const Sum &sum) const;
+
+  /* *******************************************************************************/
+  Sum wrappedFactors() const;
+
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 6af5f7192..8135b2d2d 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
@@ -32,6 +33,32 @@ GaussianMixture::GaussianMixture(
       BaseConditional(continuousFrontals.size()),
       conditionals_(conditionals) {}
 
+const GaussianMixture::Conditionals& GaussianMixture::conditionals() {
+  return conditionals_;
+}
+
+/* *******************************************************************************/
+GaussianMixture::Sum GaussianMixture::addTo(const GaussianMixture::Sum &sum) const {
+  using Y = GaussianFactorGraph;
+  auto add = [](const Y &graph1, const Y &graph2) {
+    auto result = graph1;
+    result.push_back(graph2);
+    return result;
+  };
+  const Sum wrapped = wrappedConditionals();
+  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+}
+
+/* *******************************************************************************/
+GaussianMixture::Sum GaussianMixture::wrappedConditionals() const {
+  auto wrap = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {conditionals_, wrap};
+}
+
 bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 541a2b8a6..13171ae5d 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -43,6 +43,16 @@ class GaussianMixture
                         const DiscreteKeys &discreteParents,
                         const Conditionals &conditionals);
 
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  const Conditionals& conditionals();
+
+  /* *******************************************************************************/
+  Sum addTo(const Sum &sum) const;
+
+  /* *******************************************************************************/
+  Sum wrappedConditionals() const;
+
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index e212f4534..7ada061b9 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -17,6 +17,8 @@
 
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/Conditional-inst.h>
+#include "gtsam/hybrid/HybridFactor.h"
+#include "gtsam/inference/Key.h"
 
 namespace gtsam {
 
@@ -34,8 +36,27 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
 
 HybridConditional::HybridConditional(
     boost::shared_ptr<GaussianConditional> continuousConditional)
-    : BaseFactor(continuousConditional->keys()),
-      BaseConditional(continuousConditional->nrFrontals()) {}
+    : HybridConditional(continuousConditional->keys(), {},
+                        continuousConditional->nrFrontals()) {
+  inner = continuousConditional;
+}
+
+HybridConditional::HybridConditional(
+    boost::shared_ptr<DiscreteConditional> discreteConditional)
+    : HybridConditional({}, discreteConditional->discreteKeys(),
+                        discreteConditional->nrFrontals()) {
+  inner = discreteConditional;
+}
+
+HybridConditional::HybridConditional(
+    boost::shared_ptr<GaussianMixture> gaussianMixture)
+    : BaseFactor(KeyVector(gaussianMixture->keys().begin(),
+                           gaussianMixture->keys().begin() +
+                               gaussianMixture->nrContinuous),
+                 gaussianMixture->discreteKeys_),
+      BaseConditional(gaussianMixture->nrFrontals()) {
+  inner = gaussianMixture;
+}
 
 void HybridConditional::print(const std::string &s,
                               const KeyFormatter &formatter) const {
@@ -70,8 +91,4 @@ bool HybridConditional::equals(const HybridFactor &other, double tol) const {
   return false;
 }
 
-HybridConditional HybridConditional::operator*(
-    const HybridConditional &other) const {
-  return {};
-}
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index d851cd68e..5fdf5064a 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -23,13 +23,20 @@
 
 #include <boost/make_shared.hpp>
 #include <boost/shared_ptr.hpp>
+#include <stdexcept>
 #include <string>
+#include <typeinfo>
 #include <vector>
-#include "gtsam/inference/Key.h"
-#include "gtsam/linear/GaussianConditional.h"
+#include "gtsam/hybrid/GaussianMixture.h"
+
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
 
+class HybridFactorGraph;
+
 /**
  * Hybrid Conditional Density
  *
@@ -49,9 +56,9 @@ class GTSAM_EXPORT HybridConditional
   typedef Conditional<BaseFactor, This>
       BaseConditional;  ///< Typedef to our conditional base class
 
- private:
+ protected:
   // Type-erased pointer to the inner type
-  std::unique_ptr<Factor> inner;
+  boost::shared_ptr<Factor> inner;
 
  public:
   /// @name Standard Constructors
@@ -70,23 +77,15 @@ class GTSAM_EXPORT HybridConditional
                     const DiscreteKeys& discreteParents);
 
   HybridConditional(boost::shared_ptr<GaussianConditional> continuousConditional);
+
+  HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
   
-  /**
-   * @brief Combine two conditionals, yielding a new conditional with the union
-   * of the frontal keys, ordered by gtsam::Key.
-   *
-   * The two conditionals must make a valid Bayes net fragment, i.e.,
-   * the frontal variables cannot overlap, and must be acyclic:
-   * Example of correct use:
-   *   P(A,B) = P(A|B) * P(B)
-   *   P(A,B|C) = P(A|B) * P(B|C)
-   *   P(A,B,C) = P(A,B|C) * P(C)
-   * Example of incorrect use:
-   *   P(A|B) * P(A|C) = ?
-   *   P(A|B) * P(B|A) = ?
-   * We check for overlapping frontals, but do *not* check for cyclic.
-   */
-  HybridConditional operator*(const HybridConditional& other) const;
+  HybridConditional(boost::shared_ptr<GaussianMixture> gaussianMixture);
+
+  GaussianMixture::shared_ptr asMixture() {
+      if (!isHybrid_) throw std::invalid_argument("Not a mixture");
+      return boost::static_pointer_cast<GaussianMixture>(inner);
+  }
 
   /// @}
   /// @name Testable
@@ -101,6 +100,10 @@ class GTSAM_EXPORT HybridConditional
   bool equals(const HybridFactor& other, double tol = 1e-9) const override;
 
   /// @}
+
+  friend std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
+  EliminateHybrid(const HybridFactorGraph& factors,
+                  const Ordering& frontalKeys);
 };
 // DiscreteConditional
 
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 96d3842b8..be5659f04 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -19,12 +19,15 @@
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 
 #include <boost/make_shared.hpp>
+#include "gtsam/discrete/DecisionTreeFactor.h"
 
 namespace gtsam {
 
+// TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right!
 HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
-    : Base(other->keys()) {
+    : Base(boost::dynamic_pointer_cast<DecisionTreeFactor>(other)->discreteKeys()) {
   inner = other;
+
 }
 
 HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 48a9dc468..a5ce8bd4e 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -48,7 +48,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 HybridFactor::HybridFactor() = default;
 
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true) {}
+    : Base(keys), isContinuous_(true), nrContinuous(keys.size()) {}
 
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
                            const DiscreteKeys &discreteKeys)
@@ -56,6 +56,7 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys,
       isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
       isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
       isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
+      nrContinuous(continuousKeys.size()),
       discreteKeys_(discreteKeys) {}
 
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 84d9b71bc..653d4270a 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -22,6 +22,7 @@
 #include <gtsam/inference/Factor.h>
 #include <gtsam/nonlinear/Values.h>
 
+#include <cstddef>
 #include <string>
 namespace gtsam {
 
@@ -46,6 +47,8 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   bool isContinuous_ = false;
   bool isHybrid_ = false;
 
+  size_t nrContinuous = 0;
+
   DiscreteKeys discreteKeys_;
 
  public:
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 367f9f9ab..735b8cfa9 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -13,9 +13,17 @@
  * @file   HybridFactorGraph.cpp
  * @brief  Hybrid factor graph that uses type erasure
  * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
  * @date   Mar 11, 2022
  */
 
+#include <gtsam/base/utilities.h>
+#include <gtsam/discrete/Assignment.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/hybrid/CGMixtureFactor.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
@@ -23,14 +31,20 @@
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/HessianFactor.h>
 
+#include <algorithm>
+#include <cstddef>
 #include <iostream>
+#include <iterator>
+#include <memory>
+#include <stdexcept>
 #include <unordered_map>
 #include <utility>
-
-#include "gtsam/inference/Key.h"
-#include "gtsam/linear/GaussianFactorGraph.h"
-#include "gtsam/linear/HessianFactor.h"
+#include <vector>
 
 namespace gtsam {
 
@@ -42,6 +56,26 @@ static std::string GREEN = "\033[0;32m";
 static std::string GREEN_BOLD = "\033[1;32m";
 static std::string RESET = "\033[0m";
 
+static CGMixtureFactor::Sum &addGaussian(
+    CGMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
+  using Y = GaussianFactorGraph;
+  // If the decision tree is not intiialized, then intialize it.
+  if (sum.empty()) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    sum = CGMixtureFactor::Sum(result);
+
+  } else {
+    auto add = [&factor](const Y &graph) {
+      auto result = graph;
+      result.push_back(factor);
+      return result;
+    };
+    sum = sum.apply(add);
+  }
+  return sum;
+}
+
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
@@ -65,29 +99,29 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Because of all these reasons, we need to think very carefully about how to
   // implement the hybrid factors so that we do not get poor performance.
-  // 
+  //
   // The first thing is how to represent the GaussianMixture. A very possible
-  // scenario is that the incoming factors will have different levels of discrete
-  // keys. For example, imagine we are going to eliminate the fragment:
-  // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will need
-  // to know how to retrieve the corresponding continuous densities for the assi-
-  // -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!). And we 
-  // also need to consider when there is pruning. Two mixture factors could have
-  // different pruning patterns-one could have (c1=0,c2=1) pruned, and another
-  // could have (c2=0,c3=1) pruned, and this creates a big problem in how to 
-  // identify the intersection of non-pruned branches.
+  // scenario is that the incoming factors will have different levels of
+  // discrete keys. For example, imagine we are going to eliminate the fragment:
+  // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will
+  // need to know how to retrieve the corresponding continuous densities for the
+  // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!).
+  // And we also need to consider when there is pruning. Two mixture factors
+  // could have different pruning patterns-one could have (c1=0,c2=1) pruned,
+  // and another could have (c2=0,c3=1) pruned, and this creates a big problem
+  // in how to identify the intersection of non-pruned branches.
 
-  // One possible approach is first building the collection of all discrete keys.
-  // After that we enumerate the space of all key combinations *lazily* so that
-  // the exploration branch terminates whenever an assignment yields NULL in any
-  // of the hybrid factors.
+  // One possible approach is first building the collection of all discrete
+  // keys. After that we enumerate the space of all key combinations *lazily* so
+  // that the exploration branch terminates whenever an assignment yields NULL
+  // in any of the hybrid factors.
 
   // When the number of assignments is large we may encounter stack overflows.
   // However this is also the case with iSAM2, so no pressure :)
 
   // PREPROCESS: Identify the nature of the current elimination
   std::unordered_map<Key, DiscreteKey> discreteCardinalities;
-  std::set<DiscreteKey> discreteSeparator;
+  std::set<DiscreteKey> discreteSeparatorSet;
   std::set<DiscreteKey> discreteFrontals;
 
   KeySet separatorKeys;
@@ -123,15 +157,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
       discreteFrontals.insert(discreteCardinalities.at(k));
     } else {
       continuousFrontals.insert(k);
+      allContinuousKeys.insert(k);
     }
   }
 
   // Fill in discrete frontals and continuous frontals for the end result
   for (auto &k : separatorKeys) {
     if (discreteCardinalities.find(k) != discreteCardinalities.end()) {
-      discreteSeparator.insert(discreteCardinalities.at(k));
+      discreteSeparatorSet.insert(discreteCardinalities.at(k));
     } else {
       continuousSeparator.insert(k);
+      allContinuousKeys.insert(k);
     }
   }
 
@@ -164,12 +200,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   }
 
   // NOTE: We should really defer the product here because of pruning
-  
+
   // Case 1: we are only dealing with continuous
-  if (discreteCardinalities.empty()) {
+  if (discreteCardinalities.empty() && !allContinuousKeys.empty()) {
+    std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
     GaussianFactorGraph gfg;
     for (auto &fp : factors) {
-      gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
+      auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
+      if (ptr)
+        gfg.push_back(ptr->inner);
+      else
+        gfg.push_back(boost::static_pointer_cast<GaussianConditional>(
+            boost::static_pointer_cast<HybridConditional>(fp)->inner));
     }
 
     auto result = EliminatePreferCholesky(gfg, frontalKeys);
@@ -179,61 +221,248 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   }
 
   // Case 2: we are only dealing with discrete
-  if (discreteCardinalities.empty()) {
-    GaussianFactorGraph gfg;
+  if (allContinuousKeys.empty()) {
+    std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n";
+    DiscreteFactorGraph dfg;
     for (auto &fp : factors) {
-      gfg.push_back(boost::static_pointer_cast<HybridGaussianFactor>(fp)->inner);
+      auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
+      if (ptr)
+        dfg.push_back(ptr->inner);
+      else
+        dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(
+            boost::static_pointer_cast<HybridConditional>(fp)->inner));
     }
 
-    auto result = EliminatePreferCholesky(gfg, frontalKeys);
+    auto result = EliminateDiscrete(dfg, frontalKeys);
+
     return std::make_pair(
         boost::make_shared<HybridConditional>(result.first),
-        boost::make_shared<HybridGaussianFactor>(result.second));
+        boost::make_shared<HybridDiscreteFactor>(result.second));
   }
 
+  // Case 3: We are now in the hybrid land!
+  // NOTE: since we use the special JunctionTree, only possiblity is cont.
+  // conditioned on disc.
+  DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(),
+                                 discreteSeparatorSet.end());
+
+  // We will need to know a mapping on when will a factor be fully determined by
+  // discrete keys std::vector<std::vector<HybridFactor::shared_ptr>>
+  // availableFactors;
+
+  // {
+  //   std::vector<std::pair<KeySet, HybridFactor::shared_ptr>> keysForFactor;
+  //   keysForFactor.reserve(factors.size());
+  //   std::transform(
+  //       factors.begin(), factors.end(), std::back_inserter(keysForFactor),
+  //       [](HybridFactor::shared_ptr factor)
+  //           -> std::pair<KeySet, HybridFactor::shared_ptr> {
+  //         return {KeySet(factor->keys().begin() + factor->nrContinuous,
+  //                        factor->keys().end()),
+  //                 factor};
+  //       });
+
+  //   KeySet currentAllKeys;
+  //   const auto N = discreteSeparator.size();
+  //   for (size_t k = 0; k < N; k++) {
+  //     currentAllKeys.insert(discreteSeparator.at(k).first);
+  //     std::vector<bool> shouldRemove(N, false);
+  //     for (size_t i = 0; i < keysForFactor.size(); i++) {
+  //       availableFactors.emplace_back();
+
+  //       if (std::includes(currentAllKeys.begin(), currentAllKeys.end(),
+  //                         keysForFactor[i].first.begin(),
+  //                         keysForFactor[i].first.end())) {
+  //         // mark for delete
+  //         shouldRemove[i] = true;
+  //         availableFactors.back().push_back(keysForFactor[i].second);
+  //       }
+  //       keysForFactor.erase(
+  //           std::remove_if(keysForFactor.begin(), keysForFactor.end(),
+  //                          [&shouldRemove, &keysForFactor](std::pair<KeySet,
+  //                          HybridFactor::shared_ptr> const &i) {
+  //                            return shouldRemove.at(&i -
+  //                            keysForFactor.data());
+  //                          }),
+  //           keysForFactor.end());
+  //     }
+  //   }
+  // }
+
+  // std::function<boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>>(
+  //     (Assignment<Key>, GaussianFactorGraph, int))>
+  //     visitor = [&](Assignment<Key> history, GaussianFactorGraph gf, int pos)
+  //     -> boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>> {
+  //   const auto currentKey = discreteSeparator[pos].first;
+  //   const auto currentCard = discreteSeparator[pos].second;
+
+  //   std::vector<boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>>>
+  //       children(currentCard, nullptr);
+  //   for (size_t choice = 0; choice < currentCard; choice++) {
+  //     Assignment<Key> new_assignment = history;
+  //     GaussianFactorGraph new_gf(gf);
+  //     // we try to get all currently available factors
+  //     for (auto &factor : availableFactors[pos]) {
+  //       if (!factor) {
+  //         continue;
+  //       }
+
+  //       auto ptr_mf = boost::dynamic_pointer_cast<CGMixtureFactor>(factor);
+  //       if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment));
+
+  //       auto ptr_gm = boost::dynamic_pointer_cast<GaussianMixture>(factor);
+  //       if (ptr_gm) gf.push_back(ptr_gm->conditionals_(new_assignment));
+
+  //       children[choice] = visitor(new_assignment, new_gf, pos + 1);
+  //     }
+  //   }
+  // };
+
   // PRODUCT: multiply all factors
-  gttic(product);
-
-  HybridConditional sum(
-      KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
-      DiscreteKeys(discreteSeparator.begin(), discreteSeparator.end()),
-      separatorKeys.size());
-
-  //  HybridDiscreteFactor product(DiscreteConditional());
-  //  for (auto&& factor : factors) product = (*factor) * product;
-  gttoc(product);
+  // HybridConditional sum_factor(
+  //     KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
+  //     DiscreteKeys(discreteSeparatorSet.begin(), discreteSeparatorSet.end()),
+  //     separatorKeys.size());
 
   // sum out frontals, this is the factor on the separator
   gttic(sum);
-  //  HybridFactor::shared_ptr sum = product.sum(frontalKeys);
+
+  std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
+
+  CGMixtureFactor::Sum sum;
+
+  std::vector<GaussianFactor::shared_ptr> deferredFactors;
+
+  for (auto &f : factors) {
+    if (f->isHybrid_) {
+      auto cgmf = boost::dynamic_pointer_cast<CGMixtureFactor>(f);
+      if (cgmf) {
+        sum = cgmf->addTo(sum);
+      }
+
+      auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
+      if (gm) {
+        sum = gm->asMixture()->addTo(sum);
+      }
+
+    } else if (f->isContinuous_) {
+      deferredFactors.push_back(
+          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner);
+    } else {
+      throw std::invalid_argument(
+          "factor is discrete in continuous elimination");
+    }
+  }
+
+  for (auto &f : deferredFactors) {
+    std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n";
+    sum = addGaussian(sum, f);
+  }
+
+  std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET;
+  sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) {
+    RedirectCout rd;
+    gfg.print("");
+    return rd.str();
+  });
+
   gttoc(sum);
 
-  // Ordering keys for the conditional so that frontalKeys are really in front
-  Ordering orderedKeys;
-  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
-  orderedKeys.insert(orderedKeys.end(), sum.keys().begin(), sum.keys().end());
+  using EliminationPair = GaussianFactorGraph::EliminationResult;
 
-  // now divide product/sum to get conditional
-  gttic(divide);
-  //  auto conditional =
-  //      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
-  gttoc(divide);
+  KeyVector keysOfEliminated;  // Not the ordering
+  KeyVector keysOfSeparator;   // TODO(frank): Is this just (keys - ordering)?
+
+  auto eliminate = [&](const GaussianFactorGraph &graph)
+      -> GaussianFactorGraph::EliminationResult {
+    if (graph.empty()) {
+      return {nullptr, nullptr};
+    }
+    auto result = EliminatePreferCholesky(graph, frontalKeys);
+    if (keysOfEliminated.empty()) {
+      keysOfEliminated =
+          result.first->keys();  // Initialize the keysOfEliminated to be the
+    }
+    // keysOfEliminated of the GaussianConditional
+    if (keysOfSeparator.empty()) {
+      keysOfSeparator = result.second->keys();
+    }
+    return result;
+  };
+
+  DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate);
+
+  auto pair = unzip(eliminationResults);
+
+  const GaussianMixture::Conditionals &conditionals = pair.first;
+  const CGMixtureFactor::Factors &separatorFactors = pair.second;
+
+  // Create the GaussianMixture from the conditionals
+  auto conditional = boost::make_shared<GaussianMixture>(
+      frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
 
-  auto conditional = boost::make_shared<HybridConditional>(
-      CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
-                  {continuousSeparator.begin(), continuousSeparator.end()}),
-      CollectDiscreteKeys({discreteFrontals.begin(), discreteFrontals.end()},
-                          {discreteSeparator.begin(), discreteSeparator.end()}),
-      continuousFrontals.size() + discreteFrontals.size());
   std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
   conditional->print();
   std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
-  sum.print();
+  separatorFactors.print("", DefaultKeyFormatter,
+                         [](GaussianFactor::shared_ptr gc) {
+                           RedirectCout rd;
+                           gc->print("");
+                           return rd.str();
+                         });
   std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
 
-  //  return std::make_pair(conditional, sum);
-  return std::make_pair(conditional,
-                        boost::make_shared<HybridConditional>(std::move(sum)));
+  // If there are no more continuous parents, then we should create here a
+  // DiscreteFactor, with the error for each discrete choice.
+  if (keysOfSeparator.empty()) {
+    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));
+    };
+    DecisionTree<Key, double> fdt(separatorFactors, factorError);
+    auto discreteFactor =
+        boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
+
+    return {boost::make_shared<HybridConditional>(conditional),
+            boost::make_shared<HybridDiscreteFactor>(discreteFactor)};
+
+  } else {
+    // Create a resulting DCGaussianMixture on the separator.
+    auto factor = boost::make_shared<CGMixtureFactor>(
+        frontalKeys, discreteSeparator, separatorFactors);
+    return {boost::make_shared<HybridConditional>(conditional), factor};
+  }
+
+  // Ordering keys for the conditional so that frontalKeys are really in front
+  // Ordering orderedKeys;
+  // orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
+  // frontalKeys.end()); orderedKeys.insert(orderedKeys.end(),
+  // sum_factor.keys().begin(),
+  //                    sum_factor.keys().end());
+
+  // // now divide product/sum to get conditional
+  // gttic(divide);
+  // //  auto conditional =
+  // //      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
+  // gttoc(divide);
+
+  // auto conditional = boost::make_shared<HybridConditional>(
+  //     CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
+  //                 {continuousSeparator.begin(), continuousSeparator.end()}),
+  //     CollectDiscreteKeys(
+  //         {discreteFrontals.begin(), discreteFrontals.end()},
+  //         {discreteSeparatorSet.begin(), discreteSeparatorSet.end()}),
+  //     continuousFrontals.size() + discreteFrontals.size());
+  // std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
+  // conditional->print();
+  // std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
+  // sum_factor.print();
+  // std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
+
+  // //  return std::make_pair(conditional, sum);
+  // return std::make_pair(conditional, boost::make_shared<HybridConditional>(
+  //                                        std::move(sum_factor)));
 }
 
 void HybridFactorGraph::add(JacobianFactor &&factor) {
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index d16715300..6672a1870 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -31,7 +31,6 @@
 #include <gtsam/linear/JacobianFactor.h>
 
 #include <boost/assign/std/map.hpp>
-#include "gtsam/base/Testable.h"
 
 using namespace boost::assign;
 
@@ -41,22 +40,33 @@ using namespace gtsam;
 using gtsam::symbol_shorthand::C;
 using gtsam::symbol_shorthand::X;
 
+#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED
+
+#include <signal.h>  // ::signal, ::raise
+
+#include <boost/stacktrace.hpp>
+
+void my_signal_handler(int signum) {
+  ::signal(signum, SIG_DFL);
+  std::cout << boost::stacktrace::stacktrace();
+  ::raise(SIGABRT);
+}
+
 /* ************************************************************************* */
-TEST(HybridFactorGraph, creation) {
+TEST_DISABLED(HybridFactorGraph, creation) {
   HybridConditional test;
 
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  GaussianMixture clgc(
-      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-      GaussianMixture::Conditionals(
-          C(0),
-          boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
-                                                  I_3x3),
-          boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
-                                                  X(1), I_3x3)));
+  GaussianMixture clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
+                       GaussianMixture::Conditionals(
+                           C(0),
+                           boost::make_shared<GaussianConditional>(
+                               X(0), Z_3x1, I_3x3, X(1), I_3x3),
+                           boost::make_shared<GaussianConditional>(
+                               X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
   GTSAM_PRINT(clgc);
 }
 
@@ -70,7 +80,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
-TEST(HybridFactorGraph, eliminateMultifrontal) {
+TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
   DiscreteKey x(X(1), 2);
@@ -84,7 +94,7 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
+TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -99,7 +109,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
   hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
-  hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
   hfg.add(HybridDiscreteFactor(
       DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
@@ -114,7 +124,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   GTSAM_PRINT(*result->marginalFactor(C(1)));
 }
 
-TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
+TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -148,8 +158,9 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
 }
 
 /**
- * This test is about how to assemble the Bayes Tree roots after we do partial elimination
-*/
+ * This test is about how to assemble the Bayes Tree roots after we do partial
+ * elimination
+ */
 TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
@@ -173,7 +184,8 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   }
 
   // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  hfg.add(HybridDiscreteFactor(
+      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
 
   hfg.add(JacobianFactor(X(3), I_3x3, X(4), -I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(4), I_3x3, X(5), -I_3x3, Z_3x1));
@@ -192,14 +204,15 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
     hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
   }
 
-  auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
+  auto ordering_full =
+      Ordering::ColamdConstrainedLast(hfg, {C(0), C(1), C(2), C(3)});
 
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
   HybridFactorGraph::shared_ptr remaining;
-  std::tie(hbt, remaining) =
-      hfg.eliminatePartialMultifrontal(Ordering(ordering_full.begin(), ordering_full.end()));
+  std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(
+      Ordering(ordering_full.begin(), ordering_full.end()));
 
   GTSAM_PRINT(*hbt);
 
@@ -215,6 +228,9 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
 
 /* ************************************************************************* */
 int main() {
+  ::signal(SIGSEGV, &my_signal_handler);
+  ::signal(SIGBUS, &my_signal_handler);
+
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

From 1e8aae3f06b5cb704de3a1a7578b15216760159a Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:18:02 -0400
Subject: [PATCH 026/175] Add a test for EliminateSequential

---
 gtsam/hybrid/tests/testHybridConditional.cpp | 31 +++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 6672a1870..32cb05325 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -94,6 +94,35 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
+TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
+  std::cout << ">>>>>>>>>>>>>>\n";
+
+  HybridFactorGraph hfg;
+
+  DiscreteKey c1(C(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+
+  hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
+  // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
+  hfg.add(HybridDiscreteFactor(
+      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
+  // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
+  // {C(1), 2}}, "1 2 2 1")));
+
+  auto result = hfg.eliminateSequential(
+      Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
+
+  GTSAM_PRINT(*result);
+}
+
 TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
@@ -121,7 +150,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
       Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
 
   GTSAM_PRINT(*result);
-  GTSAM_PRINT(*result->marginalFactor(C(1)));
+  GTSAM_PRINT(*result->marginalFactor(C(2)));
 }
 
 TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) {

From b4f8eea23138266f795b994048f559861939b180 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:36:18 -0400
Subject: [PATCH 027/175] Address comments

---
 ...reFactor.cpp => GaussianMixtureFactor.cpp} | 18 ++++++++---------
 ...ixtureFactor.h => GaussianMixtureFactor.h} | 10 +++++-----
 gtsam/hybrid/HybridBayesNet.h                 |  6 +++---
 gtsam/hybrid/HybridBayesTree.h                |  4 +++-
 gtsam/hybrid/HybridDiscreteFactor.h           |  4 ++++
 gtsam/hybrid/HybridFactor.h                   |  5 +++++
 gtsam/hybrid/HybridFactorGraph.cpp            | 18 ++++++++---------
 gtsam/hybrid/HybridGaussianFactor.h           |  4 ++++
 gtsam/hybrid/tests/CMakeLists.txt             |  2 +-
 gtsam/hybrid/tests/testHybridConditional.cpp  | 20 +++++++++----------
 10 files changed, 53 insertions(+), 38 deletions(-)
 rename gtsam/hybrid/{CGMixtureFactor.cpp => GaussianMixtureFactor.cpp} (77%)
 rename gtsam/hybrid/{CGMixtureFactor.h => GaussianMixtureFactor.h} (87%)

diff --git a/gtsam/hybrid/CGMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
similarity index 77%
rename from gtsam/hybrid/CGMixtureFactor.cpp
rename to gtsam/hybrid/GaussianMixtureFactor.cpp
index 08ae8b6e9..61cfb1d70 100644
--- a/gtsam/hybrid/CGMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   CGMixtureFactor.cpp
+ * @file   GaussianMixtureFactor.cpp
  * @brief  A set of Gaussian factors indexed by a set of discrete keys.
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -18,7 +18,7 @@
  * @date   Mar 12, 2022
  */
 
-#include <gtsam/hybrid/CGMixtureFactor.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 
 #include <gtsam/discrete/DecisionTree.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
@@ -27,15 +27,15 @@
 
 namespace gtsam {
 
-CGMixtureFactor::CGMixtureFactor(const KeyVector &continuousKeys,
+GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
                                  const DiscreteKeys &discreteKeys,
                                  const Factors &factors) : Base(continuousKeys, discreteKeys),
                                                            factors_(factors) {}
-bool CGMixtureFactor::equals(const HybridFactor &lf, double tol) const {
+bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   factors_.print(
       "mixture = ",
@@ -49,12 +49,12 @@ void CGMixtureFactor::print(const std::string &s, const KeyFormatter &formatter)
       });
 }
 
-const CGMixtureFactor::Factors& CGMixtureFactor::factors() {
+const GaussianMixtureFactor::Factors& GaussianMixtureFactor::factors() {
   return factors_;
 }
 
 /* *******************************************************************************/
-CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) const {
+GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(const GaussianMixtureFactor::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
     auto result = graph1;
@@ -66,7 +66,7 @@ CGMixtureFactor::Sum CGMixtureFactor::addTo(const CGMixtureFactor::Sum &sum) con
 }
 
 /* *******************************************************************************/
-CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const {
+GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const {
   auto wrap = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -74,4 +74,4 @@ CGMixtureFactor::Sum CGMixtureFactor::wrappedFactors() const {
   };
   return {factors_, wrap};
 }
-}
\ No newline at end of file
+}
diff --git a/gtsam/hybrid/CGMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
similarity index 87%
rename from gtsam/hybrid/CGMixtureFactor.h
rename to gtsam/hybrid/GaussianMixtureFactor.h
index fd81cdd91..4cd5e71de 100644
--- a/gtsam/hybrid/CGMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   CGMixtureFactor.h
+ * @file   GaussianMixtureFactor.h
  * @brief  A set of Gaussian factors indexed by a set of discrete keys.
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -29,19 +29,19 @@ namespace gtsam {
 
 class GaussianFactorGraph;
 
-class CGMixtureFactor : public HybridFactor {
+class GaussianMixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
-  using This = CGMixtureFactor;
+  using This = GaussianMixtureFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
   using Factors = DecisionTree<Key, GaussianFactor::shared_ptr>;
 
   Factors factors_;
 
-  CGMixtureFactor() = default;
+  GaussianMixtureFactor() = default;
 
-  CGMixtureFactor(const KeyVector &continuousKeys,
+  GaussianMixtureFactor(const KeyVector &continuousKeys,
                   const DiscreteKeys &discreteKeys, const Factors &factors);
 
   using Sum = DecisionTree<Key, GaussianFactorGraph>;
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 53d10518f..d7e2f33af 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -25,8 +25,8 @@
 namespace gtsam {
 
 /**
- * A hybrid Bayes net can have discrete conditionals, Gaussian mixtures,
- * or pure Gaussian conditionals.
+ * A hybrid Bayes net is a collection of HybridConditionals, which can have
+ * discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals.
  */
 class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
  public:
@@ -40,4 +40,4 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   HybridBayesNet() : Base() {}
 };
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 74b967fc8..ee51bdd6c 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -33,7 +33,9 @@ class HybridConditional;
 class VectorValues;
 
 /* ************************************************************************* */
-/** A clique in a HybridBayesTree */
+/** A clique in a HybridBayesTree
+ * which is a HybridConditional internally.
+ */
 class GTSAM_EXPORT HybridBayesTreeClique
     : public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
  public:
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index f182f90a9..c55c5ecf0 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -23,6 +23,10 @@
 
 namespace gtsam {
 
+/**
+ * A HybridDiscreteFactor is a wrapper for DiscreteFactor, so we hide the
+ * implementation of DiscreteFactor, and thus avoiding diamond inheritance.
+ */
 class HybridDiscreteFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 653d4270a..3d5bd7b21 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -34,6 +34,11 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /**
  * Base class for hybrid probabilistic factors
+ * Examples:
+ *  - HybridGaussianFactor
+ *  - HybridDiscreteFactor
+ *  - GaussianMixtureFactor
+ *  - GaussianMixture
  */
 class GTSAM_EXPORT HybridFactor : public Factor {
  public:
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 735b8cfa9..574c3b781 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -21,7 +21,7 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/Assignment.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/hybrid/CGMixtureFactor.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
@@ -56,14 +56,14 @@ static std::string GREEN = "\033[0;32m";
 static std::string GREEN_BOLD = "\033[1;32m";
 static std::string RESET = "\033[0m";
 
-static CGMixtureFactor::Sum &addGaussian(
-    CGMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
+static GaussianMixtureFactor::Sum &addGaussian(
+    GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
   using Y = GaussianFactorGraph;
   // If the decision tree is not intiialized, then intialize it.
   if (sum.empty()) {
     GaussianFactorGraph result;
     result.push_back(factor);
-    sum = CGMixtureFactor::Sum(result);
+    sum = GaussianMixtureFactor::Sum(result);
 
   } else {
     auto add = [&factor](const Y &graph) {
@@ -307,7 +307,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   //         continue;
   //       }
 
-  //       auto ptr_mf = boost::dynamic_pointer_cast<CGMixtureFactor>(factor);
+  //       auto ptr_mf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(factor);
   //       if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment));
 
   //       auto ptr_gm = boost::dynamic_pointer_cast<GaussianMixture>(factor);
@@ -329,13 +329,13 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
 
-  CGMixtureFactor::Sum sum;
+  GaussianMixtureFactor::Sum sum;
 
   std::vector<GaussianFactor::shared_ptr> deferredFactors;
 
   for (auto &f : factors) {
     if (f->isHybrid_) {
-      auto cgmf = boost::dynamic_pointer_cast<CGMixtureFactor>(f);
+      auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
       if (cgmf) {
         sum = cgmf->addTo(sum);
       }
@@ -395,7 +395,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   auto pair = unzip(eliminationResults);
 
   const GaussianMixture::Conditionals &conditionals = pair.first;
-  const CGMixtureFactor::Factors &separatorFactors = pair.second;
+  const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
 
   // Create the GaussianMixture from the conditionals
   auto conditional = boost::make_shared<GaussianMixture>(
@@ -429,7 +429,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   } else {
     // Create a resulting DCGaussianMixture on the separator.
-    auto factor = boost::make_shared<CGMixtureFactor>(
+    auto factor = boost::make_shared<GaussianMixtureFactor>(
         frontalKeys, discreteSeparator, separatorFactors);
     return {boost::make_shared<HybridConditional>(conditional), factor};
   }
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 03c49564e..86c87a0ec 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -23,6 +23,10 @@
 
 namespace gtsam {
 
+/**
+ * A HybridGaussianFactor is a wrapper for GaussianFactor so that we do not have
+ * a diamond inheritance.
+ */
 class HybridGaussianFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
diff --git a/gtsam/hybrid/tests/CMakeLists.txt b/gtsam/hybrid/tests/CMakeLists.txt
index b52e18586..06ad2c505 100644
--- a/gtsam/hybrid/tests/CMakeLists.txt
+++ b/gtsam/hybrid/tests/CMakeLists.txt
@@ -1 +1 @@
-gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam")
\ No newline at end of file
+gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam")
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 32cb05325..876105510 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -17,7 +17,7 @@
 
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/hybrid/CGMixtureFactor.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
@@ -108,8 +108,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
-  // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
+  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
   hfg.add(HybridDiscreteFactor(
       DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
@@ -137,8 +137,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {c1}, dt));
-  // hfg.add(CGMixtureFactor({X(0)}, {c1}, dt));
+  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
+  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
   hfg.add(HybridDiscreteFactor(
       DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
@@ -167,7 +167,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) {
       C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
-  hfg.add(CGMixtureFactor({X(1)}, {c}, dt));
+  hfg.add(GaussianMixtureFactor({X(1)}, {c}, dt));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
   //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
   //  2 3 4")));
@@ -203,13 +203,13 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
         C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
         boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
 
-    hfg.add(CGMixtureFactor({X(0)}, {{C(0), 2}}, dt));
+    hfg.add(GaussianMixtureFactor({X(0)}, {{C(0), 2}}, dt));
 
     DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
         C(1), boost::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
         boost::make_shared<JacobianFactor>(X(2), I_3x3, Vector3::Ones()));
 
-    hfg.add(CGMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
+    hfg.add(GaussianMixtureFactor({X(2)}, {{C(1), 2}}, dt1));
   }
 
   // hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
@@ -224,13 +224,13 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
         C(3), boost::make_shared<JacobianFactor>(X(3), I_3x3, Z_3x1),
         boost::make_shared<JacobianFactor>(X(3), I_3x3, Vector3::Ones()));
 
-    hfg.add(CGMixtureFactor({X(3)}, {{C(3), 2}}, dt));
+    hfg.add(GaussianMixtureFactor({X(3)}, {{C(3), 2}}, dt));
 
     DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
         C(2), boost::make_shared<JacobianFactor>(X(5), I_3x3, Z_3x1),
         boost::make_shared<JacobianFactor>(X(5), I_3x3, Vector3::Ones()));
 
-    hfg.add(CGMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
+    hfg.add(GaussianMixtureFactor({X(5)}, {{C(2), 2}}, dt1));
   }
 
   auto ordering_full =

From 4f8dfeb5b254ab75f34fca053c56bede74e3f5d6 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:36:56 -0400
Subject: [PATCH 028/175] Remove warning

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

diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 9445e26b8..7c48934fc 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -161,7 +161,6 @@ HybridJunctionTree::HybridJunctionTree(
 
   // Traverse the elimination tree, doing symbolic elimination and merging nodes
   // as we go.  Gather the created junction tree roots in a dummy Node.
-  typedef typename HybridEliminationTree::Node ETreeNode;
   typedef HybridConstructorTraversalData Data;
   Data rootData(0);
   rootData.myJTNode =

From 293ef614acc01156bdbb767761a7f5dba42b998b Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:42:26 -0400
Subject: [PATCH 029/175] Address comments

---
 gtsam/hybrid/HybridEliminationTree.h  |  3 +++
 gtsam/hybrid/HybridFactorGraph.h      | 10 ++++++++--
 gtsam/hybrid/HybridGaussianFactor.cpp |  2 +-
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
index b72bbcad9..902beb279 100644
--- a/gtsam/hybrid/HybridEliminationTree.h
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -23,6 +23,9 @@
 
 namespace gtsam {
 
+/**
+ * Elimination Tree type for Hybrid
+ */
 class GTSAM_EXPORT HybridEliminationTree
     : public EliminationTree<HybridBayesNet, HybridFactorGraph> {
  public:
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 92f98c8f2..21332d80a 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -37,8 +37,8 @@ class JacobianFactor;
 
 /** Main elimination function for HybridFactorGraph */
 GTSAM_EXPORT
-    std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
-    EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
+std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
+EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
 
 /* ************************************************************************* */
 template <>
@@ -62,6 +62,12 @@ struct EliminationTraits<HybridFactorGraph> {
   }
 };
 
+/**
+ * Hybrid Factor Graph
+ * -----------------------
+ * This is the linear version of a hybrid factor graph. Everything inside needs
+ * to be hybrid factor or hybrid conditional.
+ */
 class HybridFactorGraph : public FactorGraph<HybridFactor>,
                           public EliminateableFactorGraph<HybridFactorGraph> {
  public:
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index d83f90024..5fa4b555a 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -39,4 +39,4 @@ void HybridGaussianFactor::print(const std::string &s,
   inner->print("inner: ", formatter);
 };
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam

From a36c86a4f168a03f316d3c13f8bb78b13dfc3a71 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:50:36 -0400
Subject: [PATCH 030/175] Display debug messages only when DEBUG = true

---
 gtsam/hybrid/HybridFactorGraph.cpp | 186 ++++++++---------------------
 1 file changed, 49 insertions(+), 137 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 574c3b781..9e7eebc57 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -21,8 +21,8 @@
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/Assignment.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
@@ -56,6 +56,8 @@ static std::string GREEN = "\033[0;32m";
 static std::string GREEN_BOLD = "\033[1;32m";
 static std::string RESET = "\033[0m";
 
+static bool DEBUG = false;
+
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
   using Y = GaussianFactorGraph;
@@ -129,15 +131,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   KeySet continuousFrontals;
   KeySet continuousSeparator;
 
-  // TODO: we do a mock by just doing the correct key thing
-  std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
-  frontalKeys.print();
+  if (DEBUG) {
+    std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
+    frontalKeys.print();
+  }
 
   // This initializes separatorKeys and discreteCardinalities
   for (auto &&factor : factors) {
-    std::cout << ">>> Adding factor: " << GREEN;
-    factor->print();
-    std::cout << RESET;
+    if (DEBUG) {
+      std::cout << ">>> Adding factor: " << GREEN;
+      factor->print();
+      std::cout << RESET;
+    }
     separatorKeys.insert(factor->begin(), factor->end());
     if (!factor->isContinuous_) {
       for (auto &k : factor->discreteKeys_) {
@@ -171,7 +176,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     }
   }
 
-  {
+  // Only for printing
+  if (DEBUG) {
     std::cout << RED_BOLD << "Keys: " << RESET;
     for (auto &f : frontalKeys) {
       if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
@@ -203,7 +209,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Case 1: we are only dealing with continuous
   if (discreteCardinalities.empty() && !allContinuousKeys.empty()) {
-    std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
+    if (DEBUG) {
+      std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
+    }
+
     GaussianFactorGraph gfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
@@ -222,7 +231,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Case 2: we are only dealing with discrete
   if (allContinuousKeys.empty()) {
-    std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n";
+    if (DEBUG) {
+      std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n";
+    }
+
     DiscreteFactorGraph dfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
@@ -246,88 +258,12 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(),
                                  discreteSeparatorSet.end());
 
-  // We will need to know a mapping on when will a factor be fully determined by
-  // discrete keys std::vector<std::vector<HybridFactor::shared_ptr>>
-  // availableFactors;
-
-  // {
-  //   std::vector<std::pair<KeySet, HybridFactor::shared_ptr>> keysForFactor;
-  //   keysForFactor.reserve(factors.size());
-  //   std::transform(
-  //       factors.begin(), factors.end(), std::back_inserter(keysForFactor),
-  //       [](HybridFactor::shared_ptr factor)
-  //           -> std::pair<KeySet, HybridFactor::shared_ptr> {
-  //         return {KeySet(factor->keys().begin() + factor->nrContinuous,
-  //                        factor->keys().end()),
-  //                 factor};
-  //       });
-
-  //   KeySet currentAllKeys;
-  //   const auto N = discreteSeparator.size();
-  //   for (size_t k = 0; k < N; k++) {
-  //     currentAllKeys.insert(discreteSeparator.at(k).first);
-  //     std::vector<bool> shouldRemove(N, false);
-  //     for (size_t i = 0; i < keysForFactor.size(); i++) {
-  //       availableFactors.emplace_back();
-
-  //       if (std::includes(currentAllKeys.begin(), currentAllKeys.end(),
-  //                         keysForFactor[i].first.begin(),
-  //                         keysForFactor[i].first.end())) {
-  //         // mark for delete
-  //         shouldRemove[i] = true;
-  //         availableFactors.back().push_back(keysForFactor[i].second);
-  //       }
-  //       keysForFactor.erase(
-  //           std::remove_if(keysForFactor.begin(), keysForFactor.end(),
-  //                          [&shouldRemove, &keysForFactor](std::pair<KeySet,
-  //                          HybridFactor::shared_ptr> const &i) {
-  //                            return shouldRemove.at(&i -
-  //                            keysForFactor.data());
-  //                          }),
-  //           keysForFactor.end());
-  //     }
-  //   }
-  // }
-
-  // std::function<boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>>(
-  //     (Assignment<Key>, GaussianFactorGraph, int))>
-  //     visitor = [&](Assignment<Key> history, GaussianFactorGraph gf, int pos)
-  //     -> boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>> {
-  //   const auto currentKey = discreteSeparator[pos].first;
-  //   const auto currentCard = discreteSeparator[pos].second;
-
-  //   std::vector<boost::shared_ptr<DecisionTree<Key, GaussianFactorGraph>>>
-  //       children(currentCard, nullptr);
-  //   for (size_t choice = 0; choice < currentCard; choice++) {
-  //     Assignment<Key> new_assignment = history;
-  //     GaussianFactorGraph new_gf(gf);
-  //     // we try to get all currently available factors
-  //     for (auto &factor : availableFactors[pos]) {
-  //       if (!factor) {
-  //         continue;
-  //       }
-
-  //       auto ptr_mf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(factor);
-  //       if (ptr_mf) gf.push_back(ptr_mf->factors_(new_assignment));
-
-  //       auto ptr_gm = boost::dynamic_pointer_cast<GaussianMixture>(factor);
-  //       if (ptr_gm) gf.push_back(ptr_gm->conditionals_(new_assignment));
-
-  //       children[choice] = visitor(new_assignment, new_gf, pos + 1);
-  //     }
-  //   }
-  // };
-
-  // PRODUCT: multiply all factors
-  // HybridConditional sum_factor(
-  //     KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
-  //     DiscreteKeys(discreteSeparatorSet.begin(), discreteSeparatorSet.end()),
-  //     separatorKeys.size());
-
   // sum out frontals, this is the factor on the separator
   gttic(sum);
 
-  std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
+  if (DEBUG) {
+    std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
+  }
 
   GaussianMixtureFactor::Sum sum;
 
@@ -355,16 +291,20 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   }
 
   for (auto &f : deferredFactors) {
-    std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n";
+    if (DEBUG) {
+      std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n";
+    }
     sum = addGaussian(sum, f);
   }
 
-  std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET;
-  sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) {
-    RedirectCout rd;
-    gfg.print("");
-    return rd.str();
-  });
+  if (DEBUG) {
+    std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET;
+    sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) {
+      RedirectCout rd;
+      gfg.print("");
+      return rd.str();
+    });
+  }
 
   gttoc(sum);
 
@@ -401,23 +341,25 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   auto conditional = boost::make_shared<GaussianMixture>(
       frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
 
-  std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
-  conditional->print();
-  std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
-  separatorFactors.print("", DefaultKeyFormatter,
-                         [](GaussianFactor::shared_ptr gc) {
-                           RedirectCout rd;
-                           gc->print("");
-                           return rd.str();
-                         });
-  std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
+  if (DEBUG) {
+    std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
+    conditional->print();
+    std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
+    separatorFactors.print("", DefaultKeyFormatter,
+                           [](GaussianFactor::shared_ptr gc) {
+                             RedirectCout rd;
+                             gc->print("");
+                             return rd.str();
+                           });
+    std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
+  }
 
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
   if (keysOfSeparator.empty()) {
     VectorValues empty_values;
     auto factorError = [&](const GaussianFactor::shared_ptr &factor) {
-      if (!factor) return 0.0; // TODO(fan): does this make sense?
+      if (!factor) return 0.0;  // TODO(fan): does this make sense?
       return exp(-factor->error(empty_values));
     };
     DecisionTree<Key, double> fdt(separatorFactors, factorError);
@@ -433,36 +375,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
         frontalKeys, discreteSeparator, separatorFactors);
     return {boost::make_shared<HybridConditional>(conditional), factor};
   }
-
-  // Ordering keys for the conditional so that frontalKeys are really in front
-  // Ordering orderedKeys;
-  // orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
-  // frontalKeys.end()); orderedKeys.insert(orderedKeys.end(),
-  // sum_factor.keys().begin(),
-  //                    sum_factor.keys().end());
-
-  // // now divide product/sum to get conditional
-  // gttic(divide);
-  // //  auto conditional =
-  // //      boost::make_shared<HybridConditional>(product, *sum, orderedKeys);
-  // gttoc(divide);
-
-  // auto conditional = boost::make_shared<HybridConditional>(
-  //     CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
-  //                 {continuousSeparator.begin(), continuousSeparator.end()}),
-  //     CollectDiscreteKeys(
-  //         {discreteFrontals.begin(), discreteFrontals.end()},
-  //         {discreteSeparatorSet.begin(), discreteSeparatorSet.end()}),
-  //     continuousFrontals.size() + discreteFrontals.size());
-  // std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
-  // conditional->print();
-  // std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
-  // sum_factor.print();
-  // std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
-
-  // //  return std::make_pair(conditional, sum);
-  // return std::make_pair(conditional, boost::make_shared<HybridConditional>(
-  //                                        std::move(sum_factor)));
 }
 
 void HybridFactorGraph::add(JacobianFactor &&factor) {

From 8b4283b281a691c7c797ba943abfe1355ca8d162 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:54:53 -0400
Subject: [PATCH 031/175] Add doxygen for GMM

---
 gtsam/hybrid/GaussianMixture.h | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 13171ae5d..9879e80bd 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -24,9 +24,8 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
-class GaussianMixture
-    : public HybridFactor,
-      public Conditional<HybridFactor, GaussianMixture> {
+class GaussianMixture : public HybridFactor,
+                        public Conditional<HybridFactor, GaussianMixture> {
  public:
   using This = GaussianMixture;
   using shared_ptr = boost::shared_ptr<GaussianMixture>;
@@ -38,14 +37,22 @@ class GaussianMixture
   Conditionals conditionals_;
 
  public:
+  /**
+   * @brief Construct a new Gaussian Mixture object
+   *
+   * @param continuousFrontals the continuous frontals.
+   * @param continuousParents the continuous parents.
+   * @param discreteParents the discrete parents. Will be placed last.
+   * @param conditionals a decision tree of GaussianConditionals.
+   */
   GaussianMixture(const KeyVector &continuousFrontals,
-                        const KeyVector &continuousParents,
-                        const DiscreteKeys &discreteParents,
-                        const Conditionals &conditionals);
+                  const KeyVector &continuousParents,
+                  const DiscreteKeys &discreteParents,
+                  const Conditionals &conditionals);
 
   using Sum = DecisionTree<Key, GaussianFactorGraph>;
 
-  const Conditionals& conditionals();
+  const Conditionals &conditionals();
 
   /* *******************************************************************************/
   Sum addTo(const Sum &sum) const;

From 5f03e0f68b27aa62d6fe519ea65beecdb838ef8d Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 20:59:31 -0400
Subject: [PATCH 032/175] Address compilation error on GCC

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

diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 7ada061b9..ed4b01608 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -28,7 +28,7 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
                                      const DiscreteKeys &discreteParents)
     : HybridConditional(
           CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
-                      {continuousParents.begin(), continuousParents.end()}),
+                      KeyVector {continuousParents.begin(), continuousParents.end()}),
           CollectDiscreteKeys(
               {discreteFrontals.begin(), discreteFrontals.end()},
               {discreteParents.begin(), discreteParents.end()}),

From 8d888606a36fe48fee634c4ef0a8bcbffb365e44 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Wed, 23 Mar 2022 21:31:22 -0400
Subject: [PATCH 033/175] Fix GCC error

---
 gtsam/hybrid/GaussianMixture.cpp             | 2 ++
 gtsam/hybrid/tests/testHybridConditional.cpp | 4 ++--
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 8135b2d2d..26668da59 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -13,6 +13,8 @@
  * @file   GaussianMixture.cpp
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
  * @date   Mar 12, 2022
  */
 
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 876105510..95703b0af 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -17,8 +17,8 @@
 
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -75,7 +75,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  auto result = hfg.eliminatePartialSequential({0});
+  auto result = hfg.eliminatePartialSequential(KeyVector{0});
 
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }

From d2dc620b1e3031fc35c5ebe9d5030235b54ebf68 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 25 Mar 2022 17:14:00 -0600
Subject: [PATCH 034/175] Add Python bindings

---
 gtsam/hybrid/GaussianMixture.cpp             |  19 ++-
 gtsam/hybrid/GaussianMixture.h               |   5 +
 gtsam/hybrid/GaussianMixtureFactor.cpp       |  45 ++++---
 gtsam/hybrid/GaussianMixtureFactor.h         |  11 +-
 gtsam/hybrid/HybridConditional.cpp           |   9 +-
 gtsam/hybrid/HybridDiscreteFactor.h          |   4 +-
 gtsam/hybrid/HybridFactorGraph.cpp           |  14 ++
 gtsam/hybrid/HybridFactorGraph.h             |   7 +
 gtsam/hybrid/hybrid.i                        | 134 +++++++++++++++++++
 gtsam/hybrid/tests/testHybridConditional.cpp |  29 ++--
 gtsam/inference/inference.i                  |  13 +-
 python/CMakeLists.txt                        |   1 +
 python/gtsam/preamble/hybrid.h               |  14 ++
 python/gtsam/specializations/hybrid.h        |   4 +
 python/gtsam/tests/test_HybridFactorGraph.py |  49 +++++++
 15 files changed, 311 insertions(+), 47 deletions(-)
 create mode 100644 gtsam/hybrid/hybrid.i
 create mode 100644 python/gtsam/preamble/hybrid.h
 create mode 100644 python/gtsam/specializations/hybrid.h
 create mode 100644 python/gtsam/tests/test_HybridFactorGraph.py

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 26668da59..bc674966c 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -35,12 +35,23 @@ GaussianMixture::GaussianMixture(
       BaseConditional(continuousFrontals.size()),
       conditionals_(conditionals) {}
 
-const GaussianMixture::Conditionals& GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
   return conditionals_;
 }
 
+GaussianMixture GaussianMixture::FromConditionalList(
+    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+    const DiscreteKeys &discreteParents,
+    const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
+  Conditionals dt(discreteParents, conditionalsList);
+
+  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+                         dt);
+}
+
 /* *******************************************************************************/
-GaussianMixture::Sum GaussianMixture::addTo(const GaussianMixture::Sum &sum) const {
+GaussianMixture::Sum GaussianMixture::addTo(
+    const GaussianMixture::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
     auto result = graph1;
@@ -66,7 +77,7 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
 }
 
 void GaussianMixture::print(const std::string &s,
-                                  const KeyFormatter &formatter) const {
+                            const KeyFormatter &formatter) const {
   std::cout << s << ": ";
   if (isContinuous_) std::cout << "Cont. ";
   if (isDiscrete_) std::cout << "Disc. ";
@@ -88,4 +99,4 @@ void GaussianMixture::print(const std::string &s,
         return rd.str();
       });
 }
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 9879e80bd..4412e741c 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -60,6 +60,11 @@ class GaussianMixture : public HybridFactor,
   /* *******************************************************************************/
   Sum wrappedConditionals() const;
 
+  static This FromConditionalList(
+      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+      const DiscreteKeys &discreteParents,
+      const std::vector<GaussianConditional::shared_ptr> &conditionals);
+
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 61cfb1d70..3963e675e 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -18,43 +18,52 @@
  * @date   Mar 12, 2022
  */
 
-#include <gtsam/hybrid/GaussianMixtureFactor.h>
-
-#include <gtsam/discrete/DecisionTree.h>
-#include <gtsam/discrete/DecisionTree-inl.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/base/utilities.h>
+#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
 GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
-                                 const DiscreteKeys &discreteKeys,
-                                 const Factors &factors) : Base(continuousKeys, discreteKeys),
-                                                           factors_(factors) {}
+                                             const DiscreteKeys &discreteKeys,
+                                             const Factors &factors)
+    : Base(continuousKeys, discreteKeys), factors_(factors) {}
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const {
+GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
+    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+    const std::vector<GaussianFactor::shared_ptr> &factorsList) {
+  Factors dt(discreteKeys, factorsList);
+
+  return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
+}
+
+void GaussianMixtureFactor::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
   factors_.print(
-      "mixture = ",
-      [&](Key k) {
-        return formatter(k);
-      }, [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+      "mixture = ", [&](Key k) { return formatter(k); },
+      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
         RedirectCout rd;
-        if (!gf->empty()) gf->print("", formatter);
-        else return {"nullptr"};
+        if (!gf->empty())
+          gf->print("", formatter);
+        else
+          return {"nullptr"};
         return rd.str();
       });
 }
 
-const GaussianMixtureFactor::Factors& GaussianMixtureFactor::factors() {
+const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
   return factors_;
 }
 
 /* *******************************************************************************/
-GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(const GaussianMixtureFactor::Sum &sum) const {
+GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(
+    const GaussianMixtureFactor::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
     auto result = graph1;
@@ -74,4 +83,4 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const {
   };
   return {factors_, wrap};
 }
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 4cd5e71de..57a0cca03 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -29,6 +29,8 @@ namespace gtsam {
 
 class GaussianFactorGraph;
 
+typedef std::vector<gtsam::GaussianFactor::shared_ptr> GaussianFactorVector;
+
 class GaussianMixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
@@ -42,11 +44,16 @@ class GaussianMixtureFactor : public HybridFactor {
   GaussianMixtureFactor() = default;
 
   GaussianMixtureFactor(const KeyVector &continuousKeys,
-                  const DiscreteKeys &discreteKeys, const Factors &factors);
+                        const DiscreteKeys &discreteKeys,
+                        const Factors &factors);
 
   using Sum = DecisionTree<Key, GaussianFactorGraph>;
 
-  const Factors& factors();
+  const Factors &factors();
+
+  static This FromFactorList(
+      const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+      const std::vector<GaussianFactor::shared_ptr> &factors);
 
   /* *******************************************************************************/
   Sum addTo(const Sum &sum) const;
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index ed4b01608..48bee192c 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -16,9 +16,9 @@
  */
 
 #include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/inference/Conditional-inst.h>
-#include "gtsam/hybrid/HybridFactor.h"
-#include "gtsam/inference/Key.h"
+#include <gtsam/inference/Key.h>
 
 namespace gtsam {
 
@@ -27,8 +27,9 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
                                      const KeyVector &continuousParents,
                                      const DiscreteKeys &discreteParents)
     : HybridConditional(
-          CollectKeys({continuousFrontals.begin(), continuousFrontals.end()},
-                      KeyVector {continuousParents.begin(), continuousParents.end()}),
+          CollectKeys(
+              {continuousFrontals.begin(), continuousFrontals.end()},
+              KeyVector{continuousParents.begin(), continuousParents.end()}),
           CollectDiscreteKeys(
               {discreteFrontals.begin(), discreteFrontals.end()},
               {discreteParents.begin(), discreteParents.end()}),
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index c55c5ecf0..809510eac 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -35,10 +35,10 @@ class HybridDiscreteFactor : public HybridFactor {
 
   DiscreteFactor::shared_ptr inner;
 
-  // Implicit conversion from a shared ptr of GF
+  // Implicit conversion from a shared ptr of DF
   HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
 
-  // Forwarding constructor from concrete JacobianFactor
+  // Forwarding constructor from concrete DecisionTreeFactor
   HybridDiscreteFactor(DecisionTreeFactor &&dtf);
 
  public:
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 9e7eebc57..6c7599a12 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -35,6 +35,7 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
 
 #include <algorithm>
 #include <cstddef>
@@ -380,4 +381,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 void HybridFactorGraph::add(JacobianFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
 }
+
+void HybridFactorGraph::add(DecisionTreeFactor &&factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
+}
+
+void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
+}
+
+void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
+  FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
+}
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index 21332d80a..bfd6a8690 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -32,6 +32,7 @@ class HybridBayesNet;
 class HybridEliminationTree;
 class HybridBayesTree;
 class HybridJunctionTree;
+class DecisionTreeFactor;
 
 class JacobianFactor;
 
@@ -98,6 +99,12 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
 
   /// Add a factor directly using a shared_ptr.
   void add(JacobianFactor&& factor);
+
+  void add(DecisionTreeFactor&& factor);
+
+  void add(boost::shared_ptr<DecisionTreeFactor> factor);
+
+  void add(boost::shared_ptr<JacobianFactor> factor);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
new file mode 100644
index 000000000..c84cd82c5
--- /dev/null
+++ b/gtsam/hybrid/hybrid.i
@@ -0,0 +1,134 @@
+//*************************************************************************
+// hybrid
+//*************************************************************************
+
+namespace gtsam {
+
+#include <gtsam/hybrid/HybridFactor.h>
+virtual class HybridFactor {
+  void print(string s = "HybridFactor\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::HybridFactor& other, double tol = 1e-9) const;
+  bool empty() const;
+  size_t size() const;
+  gtsam::KeyVector keys() const;
+};
+
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+class GaussianMixtureFactor : gtsam::HybridFactor {
+  static GaussianMixtureFactor FromFactorList(
+      const gtsam::KeyVector& continuousKeys,
+      const gtsam::DiscreteKeys& discreteKeys,
+      const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
+
+  void print(string s = "GaussianMixtureFactor\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+};
+
+#include <gtsam/hybrid/GaussianMixture.h>
+class GaussianMixture : gtsam::HybridFactor {
+  static GaussianMixture FromConditionalList(
+      const gtsam::KeyVector& continuousFrontals,
+      const gtsam::KeyVector& continuousParents,
+      const gtsam::DiscreteKeys& discreteParents,
+      const std::vector<gtsam::GaussianConditional::shared_ptr>&
+          conditionalsList);
+
+  void print(string s = "GaussianMixture\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+};
+
+#include <gtsam/hybrid/HybridBayesTree.h>
+class HybridBayesTreeClique {
+  HybridBayesTreeClique();
+  HybridBayesTreeClique(const gtsam::HybridConditional* conditional);
+  const gtsam::HybridConditional* conditional() const;
+  bool isRoot() const;
+  // double evaluate(const gtsam::HybridValues& values) const;
+};
+
+#include <gtsam/hybrid/HybridBayesTree.h>
+class HybridBayesTree {
+  HybridBayesTree();
+  void print(string s = "HybridBayesTree\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::HybridBayesTree& other, double tol = 1e-9) const;
+
+  size_t size() const;
+  bool empty() const;
+  const HybridBayesTreeClique* operator[](size_t j) const;
+
+  string dot(const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+};
+
+class HybridBayesNet {
+  HybridBayesNet();
+  void add(const gtsam::HybridConditional& s);
+  bool empty() const;
+  size_t size() const;
+  gtsam::KeySet keys() const;
+  const gtsam::HybridConditional* at(size_t i) const;
+  void print(string s = "HybridBayesNet\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::HybridBayesNet& other, double tol = 1e-9) const;
+
+  string dot(
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
+      const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
+  void saveGraph(
+      string s,
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
+      const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
+};
+
+#include <gtsam/hybrid/HybridFactorGraph.h>
+class HybridFactorGraph {
+  HybridFactorGraph();
+  HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
+
+  // Building the graph
+  void push_back(const gtsam::HybridFactor* factor);
+  void push_back(const gtsam::HybridConditional* conditional);
+  void push_back(const gtsam::HybridFactorGraph& graph);
+  void push_back(const gtsam::HybridBayesNet& bayesNet);
+  void push_back(const gtsam::HybridBayesTree& bayesTree);
+  void push_back(const gtsam::GaussianMixtureFactor* gmm);
+
+  void add(gtsam::DecisionTreeFactor* factor);
+  void add(gtsam::JacobianFactor* factor);
+
+  bool empty() const;
+  size_t size() const;
+  gtsam::KeySet keys() const;
+  const gtsam::HybridFactor* at(size_t i) const;
+
+  void print(string s = "") const;
+  bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const;
+
+  gtsam::HybridBayesNet* eliminateSequential();
+  gtsam::HybridBayesNet* eliminateSequential(
+      gtsam::Ordering::OrderingType type);
+  gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
+  pair<gtsam::HybridBayesNet*, gtsam::HybridFactorGraph*>
+  eliminatePartialSequential(const gtsam::Ordering& ordering);
+
+  gtsam::HybridBayesTree* eliminateMultifrontal();
+  gtsam::HybridBayesTree* eliminateMultifrontal(
+      gtsam::Ordering::OrderingType type);
+  gtsam::HybridBayesTree* eliminateMultifrontal(
+      const gtsam::Ordering& ordering);
+  pair<gtsam::HybridBayesTree*, gtsam::HybridFactorGraph*>
+  eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+
+  string dot(
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
+      const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
+};
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 95703b0af..97685b87d 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -133,15 +133,19 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
 
-  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
-      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
-      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+  // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+  //     C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+  //     boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+
+  // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
+  hfg.add(GaussianMixtureFactor::FromFactorList(
+      {X(1)}, {{C(1), 2}},
+      {boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+       boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
 
-  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
   // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c1, {2, 8})));
-  hfg.add(HybridDiscreteFactor(
-      DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4")));
+  hfg.add(DecisionTreeFactor(c1, {2, 8}));
+  hfg.add(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1 2 3 4"));
   // hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(2), 2}, {C(3), 2}}, "1
   // 2 3 4"))); hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(3), 2},
   // {C(1), 2}}, "1 2 2 1")));
@@ -199,11 +203,14 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
 
   {
-    DecisionTree<Key, GaussianFactor::shared_ptr> dt(
-        C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
-        boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
+    // DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+    //     C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
+    //     boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
 
-    hfg.add(GaussianMixtureFactor({X(0)}, {{C(0), 2}}, dt));
+    hfg.add(GaussianMixtureFactor::FromFactorList(
+        {X(0)}, {{C(0), 2}},
+        {boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
+         boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));
 
     DecisionTree<Key, GaussianFactor::shared_ptr> dt1(
         C(1), boost::make_shared<JacobianFactor>(X(2), I_3x3, Z_3x1),
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index e7b074ec4..bbf1b2daa 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -9,6 +9,7 @@ namespace gtsam {
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
 
 #include <gtsam/inference/Key.h>
 
@@ -106,36 +107,36 @@ class Ordering {
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering ColamdConstrainedLast(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering ColamdConstrainedFirst(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
   static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
                                 const FACTOR_GRAPH& graph);
 
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 85ddc7b6d..5059ac95d 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -65,6 +65,7 @@ set(interface_headers
     ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i
     ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i
     ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i
+    ${PROJECT_SOURCE_DIR}/gtsam/hybrid/hybrid.i
 )
 
 set(GTSAM_PYTHON_TARGET gtsam_py)
diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h
new file mode 100644
index 000000000..56a07cfdd
--- /dev/null
+++ b/python/gtsam/preamble/hybrid.h
@@ -0,0 +1,14 @@
+/* Please refer to:
+ * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11
+ * automatic STL binding, such that the raw objects can be accessed in Python.
+ * Without this they will be automatically converted to a Python object, and all
+ * mutations on Python side will not be reflected on C++.
+ */
+
+#include <pybind11/stl.h>
diff --git a/python/gtsam/specializations/hybrid.h b/python/gtsam/specializations/hybrid.h
new file mode 100644
index 000000000..bede6d86c
--- /dev/null
+++ b/python/gtsam/specializations/hybrid.h
@@ -0,0 +1,4 @@
+
+py::bind_vector<std::vector<gtsam::GaussianFactor::shared_ptr> >(m_, "GaussianFactorVector");
+
+py::implicitly_convertible<py::list, std::vector<gtsam::GaussianFactor::shared_ptr> >();
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
new file mode 100644
index 000000000..d8b0f1f33
--- /dev/null
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -0,0 +1,49 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for Hybrid Factor Graphs.
+Author: Fan Jiang
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.symbol_shorthand import X, C
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestHybridFactorGraph(GtsamTestCase):
+    def test_create(self):
+        noiseModel = gtsam.noiseModel.Unit.Create(3)
+        dk = gtsam.DiscreteKeys()
+        dk.push_back((C(0), 2))
+
+        # print(dk.at(0))
+        jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
+                                   noiseModel)
+        jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
+                                   noiseModel)
+
+        gmf = gtsam.GaussianMixtureFactor.FromFactorList([X(0)], dk,
+                                                         [jf1, jf2])
+
+        hfg = gtsam.HybridFactorGraph()
+        hfg.add(jf1)
+        hfg.add(jf2)
+        hfg.push_back(gmf)
+
+        hfg.eliminateSequential(
+            gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(
+                hfg, [C(0)])).print()
+
+
+if __name__ == "__main__":
+    unittest.main()

From 7f2fa61fb53422aa195cd5815d51f3b5c4e47814 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 25 Mar 2022 23:28:40 -0600
Subject: [PATCH 035/175] Added more Python examples

---
 gtsam/hybrid/HybridConditional.h             | 34 +++++++++++++++-----
 gtsam/hybrid/HybridFactorGraph.cpp           | 26 +++++++--------
 gtsam/hybrid/HybridJunctionTree.cpp          | 11 +++++++
 gtsam/hybrid/hybrid.i                        | 11 +++++++
 python/gtsam/tests/test_HybridFactorGraph.py | 12 +++++--
 5 files changed, 71 insertions(+), 23 deletions(-)

diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 5fdf5064a..dea8fa9f4 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -19,7 +19,10 @@
 
 #include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/linear/GaussianConditional.h>
 
 #include <boost/make_shared.hpp>
 #include <boost/shared_ptr.hpp>
@@ -27,11 +30,8 @@
 #include <string>
 #include <typeinfo>
 #include <vector>
-#include "gtsam/hybrid/GaussianMixture.h"
 
-#include <gtsam/hybrid/HybridFactorGraph.h>
-#include <gtsam/inference/Key.h>
-#include <gtsam/linear/GaussianConditional.h>
+#include "gtsam/hybrid/GaussianMixture.h"
 
 namespace gtsam {
 
@@ -44,6 +44,19 @@ class HybridFactorGraph;
  * - DiscreteConditional
  * - GaussianConditional
  * - GaussianMixture
+ *
+ * The reason why this is important is that `Conditional<T>` is a CRTP class.
+ * CRTP is static polymorphism such that all CRTP classes, while bearing the
+ * same name, are different classes not sharing a vtable. This prevents them
+ * from being contained in any container, and thus it is impossible to
+ * dynamically cast between them. A better option, as illustrated here, is
+ * treating them as an implementation detail - such that the hybrid mechanism
+ * does not know what is inside the HybridConditional. This prevents us from
+ * having diamond inheritances, and neutralized the need to change other
+ * components of GTSAM to make hybrid elimination work.
+ *
+ * A great reference to the type-erasure pattern is Edurado Madrid's CppCon
+ * talk.
  */
 class GTSAM_EXPORT HybridConditional
     : public HybridFactor,
@@ -76,15 +89,20 @@ class GTSAM_EXPORT HybridConditional
                     const KeyVector& continuousParents,
                     const DiscreteKeys& discreteParents);
 
-  HybridConditional(boost::shared_ptr<GaussianConditional> continuousConditional);
+  HybridConditional(
+      boost::shared_ptr<GaussianConditional> continuousConditional);
 
   HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
-  
+
   HybridConditional(boost::shared_ptr<GaussianMixture> gaussianMixture);
 
   GaussianMixture::shared_ptr asMixture() {
-      if (!isHybrid_) throw std::invalid_argument("Not a mixture");
-      return boost::static_pointer_cast<GaussianMixture>(inner);
+    if (!isHybrid_) throw std::invalid_argument("Not a mixture");
+    return boost::static_pointer_cast<GaussianMixture>(inner);
+  }
+
+  boost::shared_ptr<Factor> getInner() {
+      return inner;
   }
 
   /// @}
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 6c7599a12..7a26dfadb 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -57,7 +57,7 @@ static std::string GREEN = "\033[0;32m";
 static std::string GREEN_BOLD = "\033[1;32m";
 static std::string RESET = "\033[0m";
 
-static bool DEBUG = false;
+constexpr bool DEBUG = false;
 
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
@@ -123,7 +123,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // However this is also the case with iSAM2, so no pressure :)
 
   // PREPROCESS: Identify the nature of the current elimination
-  std::unordered_map<Key, DiscreteKey> discreteCardinalities;
+  std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
   std::set<DiscreteKey> discreteSeparatorSet;
   std::set<DiscreteKey> discreteFrontals;
 
@@ -137,7 +137,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     frontalKeys.print();
   }
 
-  // This initializes separatorKeys and discreteCardinalities
+  // This initializes separatorKeys and mapFromKeyToDiscreteKey
   for (auto &&factor : factors) {
     if (DEBUG) {
       std::cout << ">>> Adding factor: " << GREEN;
@@ -147,7 +147,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     separatorKeys.insert(factor->begin(), factor->end());
     if (!factor->isContinuous_) {
       for (auto &k : factor->discreteKeys_) {
-        discreteCardinalities[k.first] = k;
+        mapFromKeyToDiscreteKey[k.first] = k;
       }
     }
   }
@@ -159,8 +159,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Fill in discrete frontals and continuous frontals for the end result
   for (auto &k : frontalKeys) {
-    if (discreteCardinalities.find(k) != discreteCardinalities.end()) {
-      discreteFrontals.insert(discreteCardinalities.at(k));
+    if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
+      discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
     } else {
       continuousFrontals.insert(k);
       allContinuousKeys.insert(k);
@@ -169,8 +169,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Fill in discrete frontals and continuous frontals for the end result
   for (auto &k : separatorKeys) {
-    if (discreteCardinalities.find(k) != discreteCardinalities.end()) {
-      discreteSeparatorSet.insert(discreteCardinalities.at(k));
+    if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
+      discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
     } else {
       continuousSeparator.insert(k);
       allContinuousKeys.insert(k);
@@ -181,8 +181,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   if (DEBUG) {
     std::cout << RED_BOLD << "Keys: " << RESET;
     for (auto &f : frontalKeys) {
-      if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
-        auto &key = discreteCardinalities.at(f);
+      if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
+        auto &key = mapFromKeyToDiscreteKey.at(f);
         std::cout << boost::format(" (%1%,%2%),") %
                          DefaultKeyFormatter(key.first) % key.second;
       } else {
@@ -195,8 +195,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     }
 
     for (auto &f : separatorKeys) {
-      if (discreteCardinalities.find(f) != discreteCardinalities.end()) {
-        auto &key = discreteCardinalities.at(f);
+      if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
+        auto &key = mapFromKeyToDiscreteKey.at(f);
         std::cout << boost::format(" (%1%,%2%),") %
                          DefaultKeyFormatter(key.first) % key.second;
       } else {
@@ -209,7 +209,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // NOTE: We should really defer the product here because of pruning
 
   // Case 1: we are only dealing with continuous
-  if (discreteCardinalities.empty() && !allContinuousKeys.empty()) {
+  if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
     if (DEBUG) {
       std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
     }
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 7c48934fc..22b40ad05 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -59,10 +59,15 @@ struct HybridConstructorTraversalData {
     myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
     parentData.myJTNode->addChild(myData.myJTNode);
 
+#ifndef NDEBUG
     std::cout << "Getting discrete info: ";
+#endif
     for (HybridFactor::shared_ptr& f : node->factors) {
       for (auto& k : f->discreteKeys_) {
+#ifndef NDEBUG
         std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
+#endif
+
         myData.discreteKeys.insert(k.first);
       }
     }
@@ -99,8 +104,10 @@ struct HybridConstructorTraversalData {
     boost::tie(myConditional, mySeparatorFactor) =
         internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
 
+#ifndef NDEBUG
     std::cout << "Symbolic: ";
     myConditional->print();
+#endif
 
     // Store symbolic elimination results in the parent
     myData.parentData->childSymbolicConditionals.push_back(myConditional);
@@ -129,15 +136,19 @@ struct HybridConstructorTraversalData {
             myData.discreteKeys.exists(myConditional->frontals()[0]);
         const bool theirType =
             myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
+#ifndef NDEBUG
         std::cout << "Type: "
                   << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs "
                   << DefaultKeyFormatter(childConditionals[i]->frontals()[0])
                   << "\n";
+#endif
         if (myType == theirType) {
           // Increment number of frontal variables
           myNrFrontals += nrFrontals[i];
+#ifndef NDEBUG
           std::cout << "Merging ";
           childConditionals[i]->print();
+#endif
           merge[i] = true;
         }
       }
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index c84cd82c5..052575011 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -15,6 +15,17 @@ virtual class HybridFactor {
   gtsam::KeyVector keys() const;
 };
 
+#include <gtsam/hybrid/HybridConditional.h>
+virtual class HybridConditional {
+  void print(string s = "Hybrid Conditional\n",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
+  bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const;
+  size_t nrFrontals() const;
+  size_t nrParents() const;
+  Factor* getInner();
+};
+
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 class GaussianMixtureFactor : gtsam::HybridFactor {
   static GaussianMixtureFactor FromFactorList(
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index d8b0f1f33..48187b7a2 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -40,9 +40,17 @@ class TestHybridFactorGraph(GtsamTestCase):
         hfg.add(jf2)
         hfg.push_back(gmf)
 
-        hfg.eliminateSequential(
+        hbn = hfg.eliminateSequential(
             gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(
-                hfg, [C(0)])).print()
+                hfg, [C(0)]))
+
+        print("hbn = ", hbn)
+
+        mixture = hbn.at(0).getInner()
+        print(mixture)
+
+        discrete_conditional = hbn.at(hbn.size()-1).getInner()
+        print(discrete_conditional)
 
 
 if __name__ == "__main__":

From 0f69b4c93f09cda7fe802c7fdec27f73134cb257 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 26 Mar 2022 23:13:57 -0600
Subject: [PATCH 036/175] Added plotting for nested dissection

---
 gtsam/hybrid/HybridConditional.h             |  2 +-
 gtsam/hybrid/tests/testHybridConditional.cpp | 87 ++++++++++++++++++--
 2 files changed, 82 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index dea8fa9f4..b91d2b91a 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -31,7 +31,7 @@
 #include <typeinfo>
 #include <vector>
 
-#include "gtsam/hybrid/GaussianMixture.h"
+#include <gtsam/hybrid/GaussianMixture.h>
 
 namespace gtsam {
 
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 97685b87d..703ec136e 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -31,6 +31,11 @@
 #include <gtsam/linear/JacobianFactor.h>
 
 #include <boost/assign/std/map.hpp>
+#include <cstddef>
+
+#include "gtsam/inference/DotWriter.h"
+#include "gtsam/inference/Key.h"
+#include "gtsam/inference/Ordering.h"
 
 using namespace boost::assign;
 
@@ -83,10 +88,10 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
 TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
-  DiscreteKey x(X(1), 2);
+  DiscreteKey c(C(1), 2);
 
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
-  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(x, {2, 8})));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
 
   auto result = hfg.eliminatePartialMultifrontal({X(0)});
 
@@ -94,7 +99,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
-TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
+TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -123,7 +128,7 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
   GTSAM_PRINT(*result);
 }
 
-TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
+TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -247,12 +252,82 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
 
   HybridBayesTree::shared_ptr hbt;
   HybridFactorGraph::shared_ptr remaining;
-  std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(
-      Ordering(ordering_full.begin(), ordering_full.end()));
+  std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
 
   GTSAM_PRINT(*hbt);
 
   GTSAM_PRINT(*remaining);
+
+  hbt->dot(std::cout);
+  /*
+  Explanation: the Junction tree will need to reeliminate to get to the marginal
+  on X(1), which is not possible because it involves eliminating discrete before
+  continuous. The solution to this, however, is in Murphy02. TLDR is that this
+  is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
+  should do this.
+  */
+}
+
+HybridFactorGraph::shared_ptr makeSwitchingChain(size_t n) {
+  HybridFactorGraph hfg;
+
+  hfg.add(JacobianFactor(X(1), I_3x3, Z_3x1));
+
+  // X(1) to X(n+1)
+  for (size_t t = 1; t < n; t++) {
+    hfg.add(GaussianMixtureFactor::FromFactorList(
+        {X(t), X(t + 1)}, {{C(t), 2}},
+        {boost::make_shared<JacobianFactor>(X(t), I_3x3, X(t + 1), I_3x3,
+                                            Z_3x1),
+         boost::make_shared<JacobianFactor>(X(t), I_3x3, X(t + 1), I_3x3,
+                                            Vector3::Ones())}));
+  }
+
+  return boost::make_shared<HybridFactorGraph>(std::move(hfg));
+}
+
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridFactorGraph, Switching) {
+  auto hfg = makeSwitchingChain(9);
+
+  // X(5) will be the center, X(1-4), X(6-9)
+  // X(3), X(7)
+  // X(2), X(8)
+  // X(1), X(4), X(6), X(9)
+  // C(5) will be the center, C(1-4), C(6-8)
+  // C(3), C(7)
+  // C(1), C(4), C(2), C(6), C(8)
+  auto ordering_full =
+      Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), X(5),
+                         C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
+
+  GTSAM_PRINT(*hfg);
+  GTSAM_PRINT(ordering_full);
+
+  HybridBayesTree::shared_ptr hbt;
+  HybridFactorGraph::shared_ptr remaining;
+  std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
+
+  GTSAM_PRINT(*hbt);
+
+  GTSAM_PRINT(*remaining);
+
+  {
+    DotWriter dw;
+    dw.positionHints['c'] = 2;
+    dw.positionHints['x'] = 1;
+    std::cout << hfg->dot(DefaultKeyFormatter, dw);
+    std::cout << "\n";
+    hbt->dot(std::cout);
+  }
+
+  {
+    DotWriter dw;
+    dw.positionHints['x'] = 1;
+    std::cout << "\n";
+    std::cout << hfg->eliminateSequential(ordering_full)
+                     ->dot(DefaultKeyFormatter, dw);
+  }
   /*
   Explanation: the Junction tree will need to reeliminate to get to the marginal
   on X(1), which is not possible because it involves eliminating discrete before

From a9c1d43dc530092f6fb4cbf35047a70069eb21de Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Fri, 1 Apr 2022 00:20:22 -0400
Subject: [PATCH 037/175] Working iSAM for Hybrid

---
 gtsam/hybrid/HybridBayesTree.h               |  29 ++
 gtsam/hybrid/HybridConditional.h             |  10 +-
 gtsam/hybrid/HybridFactorGraph.cpp           |  45 ++-
 gtsam/hybrid/HybridISAM.cpp                  |  99 ++++++
 gtsam/hybrid/HybridISAM.h                    |  59 ++++
 gtsam/hybrid/HybridJunctionTree.cpp          |   6 +-
 gtsam/hybrid/tests/Switching.h               |  68 ++++
 gtsam/hybrid/tests/testHybridConditional.cpp | 313 +++++++++++++++++--
 gtsam/inference/BayesTree.h                  |  24 +-
 9 files changed, 595 insertions(+), 58 deletions(-)
 create mode 100644 gtsam/hybrid/HybridISAM.cpp
 create mode 100644 gtsam/hybrid/HybridISAM.h
 create mode 100644 gtsam/hybrid/tests/Switching.h

diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index ee51bdd6c..dd8b7f022 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -70,4 +70,33 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   /// @}
 };
 
+/* This does special stuff for the hybrid case */
+template <class CLIQUE>
+class BayesTreeOrphanWrapper<
+    CLIQUE,
+    boost::enable_if_t<boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
+    : public CLIQUE::ConditionalType {
+ public:
+  typedef CLIQUE CliqueType;
+  typedef typename CLIQUE::ConditionalType Base;
+
+  boost::shared_ptr<CliqueType> clique;
+
+  BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
+      : clique(clique) {
+    // Store parent keys in our base type factor so that eliminating those
+    // parent keys will pull this subtree into the elimination.
+    this->keys_.assign(clique->conditional()->beginParents(),
+                       clique->conditional()->endParents());
+    this->discreteKeys_.assign(clique->conditional()->discreteKeys_.begin(),
+                               clique->conditional()->discreteKeys_.end());
+  }
+
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+    clique->print(s + "stored clique", formatter);
+  }
+};
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index b91d2b91a..5d7ee2351 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
@@ -31,8 +32,6 @@
 #include <typeinfo>
 #include <vector>
 
-#include <gtsam/hybrid/GaussianMixture.h>
-
 namespace gtsam {
 
 class HybridFactorGraph;
@@ -101,10 +100,13 @@ class GTSAM_EXPORT HybridConditional
     return boost::static_pointer_cast<GaussianMixture>(inner);
   }
 
-  boost::shared_ptr<Factor> getInner() {
-      return inner;
+  DiscreteConditional::shared_ptr asDiscreteConditional() {
+    if (!isDiscrete_) throw std::invalid_argument("Not a discrete conditional");
+    return boost::static_pointer_cast<DiscreteConditional>(inner);
   }
 
+  boost::shared_ptr<Factor> getInner() { return inner; }
+
   /// @}
   /// @name Testable
   /// @{
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 7a26dfadb..ee6a5dd82 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -217,11 +217,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     GaussianFactorGraph gfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
-      if (ptr)
+      if (ptr) {
         gfg.push_back(ptr->inner);
-      else
-        gfg.push_back(boost::static_pointer_cast<GaussianConditional>(
-            boost::static_pointer_cast<HybridConditional>(fp)->inner));
+      } else {
+        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner;
+        if (p) {
+          gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
+        } else {
+          // It is an orphan wrapper
+          if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
+        }
+      }
     }
 
     auto result = EliminatePreferCholesky(gfg, frontalKeys);
@@ -239,11 +245,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     DiscreteFactorGraph dfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
-      if (ptr)
+      if (ptr) {
         dfg.push_back(ptr->inner);
-      else
-        dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(
-            boost::static_pointer_cast<HybridConditional>(fp)->inner));
+      } else {
+        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner;
+        if (p) {
+          dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
+        } else {
+          // It is an orphan wrapper
+          if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
+        }
+      }
     }
 
     auto result = EliminateDiscrete(dfg, frontalKeys);
@@ -286,8 +298,18 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
       deferredFactors.push_back(
           boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner);
     } else {
-      throw std::invalid_argument(
-          "factor is discrete in continuous elimination");
+      // We need to handle the case where the object is actually an
+      // BayesTreeOrphanWrapper!
+      auto orphan = boost::dynamic_pointer_cast<
+          BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
+      if (orphan) {
+        if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
+      } else {
+        auto &fr = *f;
+        throw std::invalid_argument(
+            std::string("factor is discrete in continuous elimination") +
+            typeid(fr).name());
+      }
     }
   }
 
@@ -373,7 +395,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   } else {
     // Create a resulting DCGaussianMixture on the separator.
     auto factor = boost::make_shared<GaussianMixtureFactor>(
-        frontalKeys, discreteSeparator, separatorFactors);
+        KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
+        discreteSeparator, separatorFactors);
     return {boost::make_shared<HybridConditional>(conditional), factor};
   }
 }
diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp
new file mode 100644
index 000000000..0db30f1f3
--- /dev/null
+++ b/gtsam/hybrid/HybridISAM.cpp
@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridISAM.h
+ * @date March 31, 2022
+ * @author Fan Jiang
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/HybridISAM.h>
+#include <gtsam/inference/ISAM-inst.h>
+#include <gtsam/inference/Key.h>
+
+#include <iterator>
+
+namespace gtsam {
+
+// Instantiate base class
+// template class ISAM<HybridBayesTree>;
+
+/* ************************************************************************* */
+HybridISAM::HybridISAM() {}
+
+/* ************************************************************************* */
+HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
+
+void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
+                                HybridBayesTree::Cliques* orphans,
+                                const HybridBayesTree::Eliminate& function) {
+  // Remove the contaminated part of the Bayes tree
+  BayesNetType bn;
+  const KeySet newFactorKeys = newFactors.keys();
+  if (!this->empty()) {
+    KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
+    this->removeTop(keyVector, &bn, orphans);
+  }
+
+  // Add the removed top and the new factors
+  FactorGraphType factors;
+  factors += bn;
+  factors += newFactors;
+
+  // Add the orphaned subtrees
+  for (const sharedClique& orphan : *orphans)
+    factors += boost::make_shared<BayesTreeOrphanWrapper<Node> >(orphan);
+
+  KeySet allDiscrete;
+  for (auto& factor : factors) {
+    for (auto& k : factor->discreteKeys_) {
+      allDiscrete.insert(k.first);
+    }
+  }
+  KeyVector newKeysDiscreteLast;
+  for (auto& k : newFactorKeys) {
+    if (!allDiscrete.exists(k)) {
+      newKeysDiscreteLast.push_back(k);
+    }
+  }
+  std::copy(allDiscrete.begin(), allDiscrete.end(),
+            std::back_inserter(newKeysDiscreteLast));
+
+  // KeyVector new
+
+  // Get an ordering where the new keys are eliminated last
+  const VariableIndex index(factors);
+  const Ordering ordering = Ordering::ColamdConstrainedLast(
+      index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
+      true);
+
+  ordering.print("ORD");
+
+  // eliminate all factors (top, added, orphans) into a new Bayes tree
+  auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
+
+  // Re-add into Bayes tree data structures
+  this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
+                      bayesTree->roots().end());
+  this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
+}
+
+void HybridISAM::update(const HybridFactorGraph& newFactors,
+                        const HybridBayesTree::Eliminate& function) {
+  Cliques orphans;
+  this->updateInternal(newFactors, &orphans, function);
+}
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h
new file mode 100644
index 000000000..ac9e17e83
--- /dev/null
+++ b/gtsam/hybrid/HybridISAM.h
@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridISAM.h
+ * @date March 31, 2022
+ * @author Fan Jiang
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/ISAM.h>
+
+namespace gtsam {
+
+class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
+ public:
+  typedef ISAM<HybridBayesTree> Base;
+  typedef HybridISAM This;
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** Create an empty Bayes Tree */
+  HybridISAM();
+
+  /** Copy constructor */
+  HybridISAM(const HybridBayesTree& bayesTree);
+
+  void updateInternal(
+      const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
+      const HybridBayesTree::Eliminate& function =
+          HybridBayesTree::EliminationTraitsType::DefaultEliminate);
+
+  void update(const HybridFactorGraph& newFactors,
+              const HybridBayesTree::Eliminate& function =
+                  HybridBayesTree::EliminationTraitsType::DefaultEliminate);
+  /// @}
+};
+
+/// traits
+template <>
+struct traits<HybridISAM> : public Testable<HybridISAM> {};
+
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 22b40ad05..77e623a41 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -16,14 +16,14 @@
  */
 
 #include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/JunctionTree-inst.h>
+#include <gtsam/inference/Key.h>
 
 #include <unordered_map>
 
-#include "gtsam/hybrid/HybridFactorGraph.h"
-#include "gtsam/inference/Key.h"
-
+// #undef NDEBUG
 namespace gtsam {
 
 // Instantiate base classes
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
new file mode 100644
index 000000000..0816db8df
--- /dev/null
+++ b/gtsam/hybrid/tests/Switching.h
@@ -0,0 +1,68 @@
+#include <gtsam/base/Matrix.h>
+#include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+#pragma once
+
+using gtsam::symbol_shorthand::C;
+using gtsam::symbol_shorthand::X;
+
+namespace gtsam {
+inline HybridFactorGraph::shared_ptr makeSwitchingChain(
+    size_t n, std::function<Key(int)> keyFunc = X,
+    std::function<Key(int)> dKeyFunc = C) {
+  HybridFactorGraph hfg;
+
+  hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
+
+  // keyFunc(1) to keyFunc(n+1)
+  for (size_t t = 1; t < n; t++) {
+    hfg.add(GaussianMixtureFactor::FromFactorList(
+        {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
+        {boost::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
+                                            I_3x3, Z_3x1),
+         boost::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
+                                            I_3x3, Vector3::Ones())}));
+
+    if (t > 1) {
+      hfg.add(DecisionTreeFactor({{dKeyFunc(t - 1), 2}, {dKeyFunc(t), 2}},
+                                 "0 1 1 3"));
+    }
+  }
+
+  return boost::make_shared<HybridFactorGraph>(std::move(hfg));
+}
+
+inline std::pair<std::vector<Key>, std::vector<int>> makeBinaryOrdering(
+    std::vector<Key> &input) {
+  std::vector<Key> new_order;
+  std::vector<int> levels(input.size());
+  std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
+                     int)>
+      bsg = [&bsg, &new_order, &levels, &input](
+                std::vector<Key>::iterator begin,
+                std::vector<Key>::iterator end, int lvl) {
+        if (std::distance(begin, end) > 1) {
+          std::vector<Key>::iterator pivot =
+              begin + std::distance(begin, end) / 2;
+
+          new_order.push_back(*pivot);
+          levels[std::distance(input.begin(), pivot)] = lvl;
+          bsg(begin, pivot, lvl + 1);
+          bsg(pivot + 1, end, lvl + 1);
+        } else if (std::distance(begin, end) == 1) {
+          new_order.push_back(*begin);
+          levels[std::distance(input.begin(), begin)] = lvl;
+        }
+      };
+
+  bsg(input.begin(), input.end(), 0);
+  std::reverse(new_order.begin(), new_order.end());
+  // std::reverse(levels.begin(), levels.end());
+  return {new_order, levels};
+}
+
+}  // namespace gtsam
\ No newline at end of file
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index 703ec136e..f3664e619 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -17,6 +17,9 @@
 
 #include <CppUnitLite/Test.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
@@ -26,16 +29,23 @@
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridISAM.h>
 #include <gtsam/inference/BayesNet.h>
+#include <gtsam/inference/DotWriter.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 
+#include <algorithm>
 #include <boost/assign/std/map.hpp>
 #include <cstddef>
+#include <functional>
+#include <iostream>
+#include <iterator>
+#include <vector>
 
-#include "gtsam/inference/DotWriter.h"
-#include "gtsam/inference/Key.h"
-#include "gtsam/inference/Ordering.h"
+#include "Switching.h"
 
 using namespace boost::assign;
 
@@ -43,7 +53,9 @@ using namespace std;
 using namespace gtsam;
 
 using gtsam::symbol_shorthand::C;
+using gtsam::symbol_shorthand::D;
 using gtsam::symbol_shorthand::X;
+using gtsam::symbol_shorthand::Y;
 
 #define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED
 
@@ -99,6 +111,29 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
+TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
+  HybridFactorGraph hfg;
+
+  DiscreteKey c1(C(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
+
+  DecisionTree<Key, GaussianFactor::shared_ptr> dt(
+      C(1), boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
+      boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
+
+  hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
+
+  auto result =
+      hfg.eliminateSequential(Ordering::ColamdConstrainedLast(hfg, {C(1)}));
+
+  auto dc = result->at(2)->asDiscreteConditional();
+  DiscreteValues dv;
+  dv[C(1)] = 0;
+  EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
+}
+
 TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
@@ -268,27 +303,10 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   */
 }
 
-HybridFactorGraph::shared_ptr makeSwitchingChain(size_t n) {
-  HybridFactorGraph hfg;
-
-  hfg.add(JacobianFactor(X(1), I_3x3, Z_3x1));
-
-  // X(1) to X(n+1)
-  for (size_t t = 1; t < n; t++) {
-    hfg.add(GaussianMixtureFactor::FromFactorList(
-        {X(t), X(t + 1)}, {{C(t), 2}},
-        {boost::make_shared<JacobianFactor>(X(t), I_3x3, X(t + 1), I_3x3,
-                                            Z_3x1),
-         boost::make_shared<JacobianFactor>(X(t), I_3x3, X(t + 1), I_3x3,
-                                            Vector3::Ones())}));
-  }
-
-  return boost::make_shared<HybridFactorGraph>(std::move(hfg));
-}
-
 // TODO(fan): make a graph like Varun's paper one
 TEST(HybridFactorGraph, Switching) {
-  auto hfg = makeSwitchingChain(9);
+  auto N = 12;
+  auto hfg = makeSwitchingChain(N);
 
   // X(5) will be the center, X(1-4), X(6-9)
   // X(3), X(7)
@@ -297,20 +315,73 @@ TEST(HybridFactorGraph, Switching) {
   // C(5) will be the center, C(1-4), C(6-8)
   // C(3), C(7)
   // C(1), C(4), C(2), C(6), C(8)
-  auto ordering_full =
-      Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7), X(5),
-                         C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
+  // auto ordering_full =
+  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
+  //     X(5),
+  //                        C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
+  KeyVector ordering;
 
-  GTSAM_PRINT(*hfg);
+  {
+    std::vector<int> naturalX(N);
+    std::iota(naturalX.begin(), naturalX.end(), 1);
+    std::vector<Key> ordX;
+    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
+                   [](int x) { return X(x); });
+
+    KeyVector ndX;
+    std::vector<int> lvls;
+    std::tie(ndX, lvls) = makeBinaryOrdering(ordX);
+    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
+    for (auto &l : lvls) {
+      l = -l;
+    }
+    std::copy(lvls.begin(), lvls.end(),
+              std::ostream_iterator<int>(std::cout, ","));
+    std::cout << "\n";
+  }
+  {
+    std::vector<int> naturalC(N - 1);
+    std::iota(naturalC.begin(), naturalC.end(), 1);
+    std::vector<Key> ordC;
+    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
+                   [](int x) { return C(x); });
+    KeyVector ndC;
+    std::vector<int> lvls;
+
+    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
+    std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
+    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
+    std::copy(lvls.begin(), lvls.end(),
+              std::ostream_iterator<int>(std::cout, ","));
+  }
+  auto ordering_full = Ordering(ordering);
+
+  // auto ordering_full =
+  //     Ordering();
+
+  // for (int i = 1; i <= 9; i++) {
+  //   ordering_full.push_back(X(i));
+  // }
+
+  // for (int i = 1; i < 9; i++) {
+  //   ordering_full.push_back(C(i));
+  // }
+
+  // auto ordering_full =
+  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
+  //     X(5),
+  //                        C(1), C(2), C(3), C(4), C(5), C(6), C(7), C(8)});
+
+  // GTSAM_PRINT(*hfg);
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
   HybridFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
-  GTSAM_PRINT(*hbt);
+  // GTSAM_PRINT(*hbt);
 
-  GTSAM_PRINT(*remaining);
+  // GTSAM_PRINT(*remaining);
 
   {
     DotWriter dw;
@@ -323,7 +394,8 @@ TEST(HybridFactorGraph, Switching) {
 
   {
     DotWriter dw;
-    dw.positionHints['x'] = 1;
+    // dw.positionHints['c'] = 2;
+    // dw.positionHints['x'] = 1;
     std::cout << "\n";
     std::cout << hfg->eliminateSequential(ordering_full)
                      ->dot(DefaultKeyFormatter, dw);
@@ -335,6 +407,189 @@ TEST(HybridFactorGraph, Switching) {
   is 1. expensive and 2. inexact. neverless it is doable. And I believe that we
   should do this.
   */
+  hbt->marginalFactor(C(11))->print("HBT: ");
+}
+
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridFactorGraph, SwitchingISAM) {
+  auto N = 11;
+  auto hfg = makeSwitchingChain(N);
+
+  // X(5) will be the center, X(1-4), X(6-9)
+  // X(3), X(7)
+  // X(2), X(8)
+  // X(1), X(4), X(6), X(9)
+  // C(5) will be the center, C(1-4), C(6-8)
+  // C(3), C(7)
+  // C(1), C(4), C(2), C(6), C(8)
+  // auto ordering_full =
+  //     Ordering(KeyVector{X(1), X(4), X(2), X(6), X(9), X(8), X(3), X(7),
+  //     X(5),
+  //                        C(1), C(4), C(2), C(6), C(8), C(3), C(7), C(5)});
+  KeyVector ordering;
+
+  {
+    std::vector<int> naturalX(N);
+    std::iota(naturalX.begin(), naturalX.end(), 1);
+    std::vector<Key> ordX;
+    std::transform(naturalX.begin(), naturalX.end(), std::back_inserter(ordX),
+                   [](int x) { return X(x); });
+
+    KeyVector ndX;
+    std::vector<int> lvls;
+    std::tie(ndX, lvls) = makeBinaryOrdering(ordX);
+    std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
+    for (auto &l : lvls) {
+      l = -l;
+    }
+    std::copy(lvls.begin(), lvls.end(),
+              std::ostream_iterator<int>(std::cout, ","));
+    std::cout << "\n";
+  }
+  {
+    std::vector<int> naturalC(N - 1);
+    std::iota(naturalC.begin(), naturalC.end(), 1);
+    std::vector<Key> ordC;
+    std::transform(naturalC.begin(), naturalC.end(), std::back_inserter(ordC),
+                   [](int x) { return C(x); });
+    KeyVector ndC;
+    std::vector<int> lvls;
+
+    // std::copy(ordC.begin(), ordC.end(), std::back_inserter(ordering));
+    std::tie(ndC, lvls) = makeBinaryOrdering(ordC);
+    std::copy(ndC.begin(), ndC.end(), std::back_inserter(ordering));
+    std::copy(lvls.begin(), lvls.end(),
+              std::ostream_iterator<int>(std::cout, ","));
+  }
+  auto ordering_full = Ordering(ordering);
+
+  // GTSAM_PRINT(*hfg);
+  GTSAM_PRINT(ordering_full);
+
+  HybridBayesTree::shared_ptr hbt;
+  HybridFactorGraph::shared_ptr remaining;
+  std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
+
+  // GTSAM_PRINT(*hbt);
+
+  // GTSAM_PRINT(*remaining);
+
+  {
+    DotWriter dw;
+    dw.positionHints['c'] = 2;
+    dw.positionHints['x'] = 1;
+    std::cout << hfg->dot(DefaultKeyFormatter, dw);
+    std::cout << "\n";
+    hbt->dot(std::cout);
+  }
+
+  {
+    DotWriter dw;
+    // dw.positionHints['c'] = 2;
+    // dw.positionHints['x'] = 1;
+    std::cout << "\n";
+    std::cout << hfg->eliminateSequential(ordering_full)
+                     ->dot(DefaultKeyFormatter, dw);
+  }
+
+  auto new_fg = makeSwitchingChain(12);
+  auto isam = HybridISAM(*hbt);
+
+  {
+    HybridFactorGraph factorGraph;
+    factorGraph.push_back(new_fg->at(new_fg->size() - 2));
+    factorGraph.push_back(new_fg->at(new_fg->size() - 1));
+    isam.update(factorGraph);
+    std::cout << isam.dot();
+    isam.marginalFactor(C(11))->print();
+  }
+}
+
+TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) {
+  const int N = 7;
+  auto hfg = makeSwitchingChain(N, X);
+  hfg->push_back(*makeSwitchingChain(N, Y, D));
+
+  for (int t = 1; t <= N; t++) {
+    hfg->add(JacobianFactor(X(t), I_3x3, Y(t), -I_3x3, Vector3(1.0, 0.0, 0.0)));
+  }
+
+  KeyVector ordering;
+
+  KeyVector naturalX(N);
+  std::iota(naturalX.begin(), naturalX.end(), 1);
+  KeyVector ordX;
+  for (size_t i = 1; i <= N; i++) {
+    ordX.emplace_back(X(i));
+    ordX.emplace_back(Y(i));
+  }
+
+  // {
+  //   KeyVector ndX;
+  //   std::vector<int> lvls;
+  //   std::tie(ndX, lvls) = makeBinaryOrdering(naturalX);
+  //   std::copy(ndX.begin(), ndX.end(), std::back_inserter(ordering));
+  //   std::copy(lvls.begin(), lvls.end(),
+  //             std::ostream_iterator<int>(std::cout, ","));
+  //   std::cout << "\n";
+
+  //   for (size_t i = 0; i < N; i++) {
+  //     ordX.emplace_back(X(ndX[i]));
+  //     ordX.emplace_back(Y(ndX[i]));
+  //   }
+  // }
+
+  for (size_t i = 1; i <= N - 1; i++) {
+    ordX.emplace_back(C(i));
+  }
+  for (size_t i = 1; i <= N - 1; i++) {
+    ordX.emplace_back(D(i));
+  }
+
+  {
+    DotWriter dw;
+    dw.positionHints['x'] = 1;
+    dw.positionHints['c'] = 0;
+    dw.positionHints['d'] = 3;
+    dw.positionHints['y'] = 2;
+    std::cout << hfg->dot(DefaultKeyFormatter, dw);
+    std::cout << "\n";
+  }
+
+  {
+    DotWriter dw;
+    dw.positionHints['y'] = 9;
+    // dw.positionHints['c'] = 0;
+    // dw.positionHints['d'] = 3;
+    dw.positionHints['x'] = 1;
+    std::cout << "\n";
+    // std::cout << hfg->eliminateSequential(Ordering(ordX))
+    //                  ->dot(DefaultKeyFormatter, dw);
+    hfg->eliminateMultifrontal(Ordering(ordX))->dot(std::cout);
+  }
+
+  Ordering ordering_partial;
+  for (size_t i = 1; i <= N; i++) {
+    ordering_partial.emplace_back(X(i));
+    ordering_partial.emplace_back(Y(i));
+  }
+  {
+    HybridBayesNet::shared_ptr hbn;
+    HybridFactorGraph::shared_ptr remaining;
+    std::tie(hbn, remaining) =
+        hfg->eliminatePartialSequential(ordering_partial);
+
+    // remaining->print();
+    {
+      DotWriter dw;
+      dw.positionHints['x'] = 1;
+      dw.positionHints['c'] = 0;
+      dw.positionHints['d'] = 3;
+      dw.positionHints['y'] = 2;
+      std::cout << remaining->dot(DefaultKeyFormatter, dw);
+      std::cout << "\n";
+    }
+  }
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index a32b3ce22..1abc0a682 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -33,6 +33,7 @@ namespace gtsam {
   // Forward declarations
   template<class FACTOR> class FactorGraph;
   template<class BAYESTREE, class GRAPH> class EliminatableClusterTree;
+  class HybridBayesTreeClique;
 
   /* ************************************************************************* */
   /** clique statistics */
@@ -272,24 +273,25 @@ namespace gtsam {
   }; // BayesTree
 
   /* ************************************************************************* */
-  template<class CLIQUE>
-  class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
-  {
-  public:
+  template <class CLIQUE, typename = void>
+  class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType {
+   public:
     typedef CLIQUE CliqueType;
     typedef typename CLIQUE::ConditionalType Base;
 
     boost::shared_ptr<CliqueType> clique;
 
-    BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) :
-      clique(clique)
-    {
-      // Store parent keys in our base type factor so that eliminating those parent keys will pull
-      // this subtree into the elimination.
-      this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
+    BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
+        : clique(clique) {
+      // Store parent keys in our base type factor so that eliminating those
+      // parent keys will pull this subtree into the elimination.
+      this->keys_.assign(clique->conditional()->beginParents(),
+                         clique->conditional()->endParents());
     }
 
-    void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+    void print(
+        const std::string& s = "",
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override {
       clique->print(s + "stored clique", formatter);
     }
   };

From 215682e64f4063911639320dcab209aab2f062fb Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Thu, 7 Apr 2022 21:33:54 -0400
Subject: [PATCH 038/175] FIX: Robust loss error calculation

---
 gtsam/linear/NoiseModel.h |  6 +++++
 tests/testRobust.cpp      | 53 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+)
 create mode 100644 tests/testRobust.cpp

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 5c379beb8..5a29e5d7d 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -697,6 +697,12 @@ namespace gtsam {
         return robust_->loss(std::sqrt(squared_distance));
       }
 
+      // NOTE: This is special because in whiten the base version will do the reweighting
+      // which is incorrect!
+      double squaredMahalanobisDistance(const Vector& v) const override {
+        return noise_->squaredMahalanobisDistance(v);
+      }
+
       // These are really robust iterated re-weighting support functions
       virtual void WhitenSystem(Vector& b) const;
       void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
diff --git a/tests/testRobust.cpp b/tests/testRobust.cpp
new file mode 100644
index 000000000..ad361f6d9
--- /dev/null
+++ b/tests/testRobust.cpp
@@ -0,0 +1,53 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testRobust.cpp
+ *  @brief  Unit tests for Robust loss functions
+ *  @author Fan Jiang
+ *  @author Yetong Zhang
+ *  @date   Apr 7, 2022
+ **/
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+using namespace gtsam;
+
+TEST(RobustNoise, loss) {
+  // Keys.
+  gtsam::Key x1_key = 1;
+  gtsam::Key x2_key = 2;
+
+  auto gm = noiseModel::mEstimator::GemanMcClure::Create(1.0);
+  auto noise = noiseModel::Robust::Create(gm, noiseModel::Unit::Create(1));
+
+  auto factor = PriorFactor<double>(x1_key, 0.0, noise);
+  auto between_factor = BetweenFactor<double>(x1_key, x2_key, 0.0, noise);
+
+  Values values;
+  values.insert(x1_key, 10.0);
+  values.insert(x2_key, 0.0);
+
+  EXPECT_DOUBLES_EQUAL(0.49505, factor.error(values), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.49505, between_factor.error(values), 1e-5);
+  EXPECT_DOUBLES_EQUAL(0.49505, gm->loss(10.0), 1e-5);
+}
+
+int main() {
+  TestResult tr;
+
+  return TestRegistry::runAllTests(tr);
+}

From 7a47815e0b0c69828a1c5ebf8f43ec9a38502b21 Mon Sep 17 00:00:00 2001
From: Fan Jiang <ProfFan@users.noreply.github.com>
Date: Fri, 8 Apr 2022 16:30:40 -0400
Subject: [PATCH 039/175] Fix the test

---
 gtsam/linear/tests/testNoiseModel.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index b974b6cd5..92774a133 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -668,7 +668,7 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
   for (int i = 0; i < 5; i++) {
     Vector3 error = Vector3(i, 0, 0);
     DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
-                  robust->squaredMahalanobisDistance(error), 1e-8);
+                  robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8);
   }
 }
 

From d7fbaaab639416fc08d881c58751dd8022f02c62 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 27 Mar 2022 11:24:45 -0400
Subject: [PATCH 040/175] Bump version

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b86598663..cfb251663 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a5")
+set (GTSAM_PRERELEASE_VERSION "a6")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

From 938d4093956335351a5f841bb14d70557876dd25 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 13 Apr 2022 21:34:54 -0400
Subject: [PATCH 041/175] Notebook

---
 .gitignore                            |   1 +
 python/gtsam/notebooks/ellipses.ipynb | 136 ++++++++++++++++++++++++++
 2 files changed, 137 insertions(+)
 create mode 100644 python/gtsam/notebooks/ellipses.ipynb

diff --git a/.gitignore b/.gitignore
index 0e34eed34..e3f7613fe 100644
--- a/.gitignore
+++ b/.gitignore
@@ -18,3 +18,4 @@
 CMakeLists.txt.user*
 xcode/
 /Dockerfile
+/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb
new file mode 100644
index 000000000..d1ce9a015
--- /dev/null
+++ b/python/gtsam/notebooks/ellipses.ipynb
@@ -0,0 +1,136 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Ellipse Scaling\n",
+    "\n",
+    "The code to calculate the percentages included in ellipses with various values of \"k\" in `plot.py`.\n",
+    "\n",
+    "Thanks to @senselessDev, January 26, for providing the code in [PR #1067](https://github.com/borglab/gtsam/pull/1067)."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import scipy\n",
+    "import scipy.stats"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def pct_to_sigma(pct, dof):\n",
+    "    return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof))\n",
+    "\n",
+    "def sigma_to_pct(sigma, dof):\n",
+    "    return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 23,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "0D\t    1    \t    2    \t    3    \t    4    \t    5    \n",
+      "1D\t68.26895%\t95.44997%\t99.73002%\t99.99367%\t99.99994%\n",
+      "2D\t39.34693%\t86.46647%\t98.88910%\t99.96645%\t99.99963%\n",
+      "3D\t19.87480%\t73.85359%\t97.07091%\t99.88660%\t99.99846%\n"
+     ]
+    }
+   ],
+   "source": [
+    "for dim in range(0, 4):\n",
+    "    print(\"{}D\".format(dim), end=\"\")\n",
+    "    for sigma in range(1, 6):\n",
+    "        if dim == 0: print(\"\\t    {}    \".format(sigma), end=\"\")\n",
+    "        else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n",
+    "    print()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 29,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "1D\n",
+      "\n",
+      "sigma=1.0 -> 68.26895%\n",
+      "sigma=2.0 -> 95.44997%\n",
+      "sigma=2.5 -> 98.75807%\n",
+      "sigma=5.0 -> 99.99994%\n",
+      "\n",
+      "2D\n",
+      "\n",
+      "sigma=1.0 -> 39.34693%\n",
+      "sigma=2.0 -> 86.46647%\n",
+      "sigma=2.5 -> 95.60631%\n",
+      "sigma=5.0 -> 99.99963%\n",
+      "\n",
+      "3D\n",
+      "\n",
+      "sigma=1.0 -> 19.87480%\n",
+      "sigma=2.0 -> 73.85359%\n",
+      "sigma=2.5 -> 89.99392%\n",
+      "sigma=5.0 -> 99.99846%\n",
+      "\n"
+     ]
+    }
+   ],
+   "source": [
+    "for dim in range(1, 4):\n",
+    "    print(\"{}D\\n\".format(dim))\n",
+    "    for sigma in [1, 2, 2.5, 5]:\n",
+    "        print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n",
+    "        print()\n",
+    "    print()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "interpreter": {
+   "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101"
+  },
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.10.4"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

From 71aa20ff33ed08c9fbd708d771005f810b370257 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Apr 2022 16:03:38 -0400
Subject: [PATCH 042/175] Update `logging_optimizer.gtsam_optimize` to use
 NonlinearOptimizerParams::iterationHook

---
 python/gtsam/tests/test_logging_optimizer.py |  6 +++++-
 python/gtsam/utils/logging_optimizer.py      | 15 ++++++---------
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py
index 47eb32e7b..4ec782635 100644
--- a/python/gtsam/tests/test_logging_optimizer.py
+++ b/python/gtsam/tests/test_logging_optimizer.py
@@ -63,12 +63,14 @@ class TestOptimizeComet(GtsamTestCase):
         def hook(_, error):
             print(error)
 
-        # Only thing we require from optimizer is an iterate method
+        # Wrapper function sets the hook and calls optimizer.optimize() for us.
         gtsam_optimize(self.optimizer, self.params, hook)
 
         # Check that optimizing yields the identity.
         actual = self.optimizer.values()
         self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+        self.assertEqual(self.capturedOutput.getvalue(),
+                         "0.020000000000000004\n0.010000000000000005\n0.010000000000000004\n")
 
     def test_lm_simple_printing(self):
         """Make sure we are properly terminating LM"""
@@ -79,6 +81,8 @@ class TestOptimizeComet(GtsamTestCase):
 
         actual = self.lmoptimizer.values()
         self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+        self.assertEqual(self.capturedOutput.getvalue(),
+                         "0.020000000000000004\n0.010000000000249996\n0.009999999999999998\n")
 
     @unittest.skip("Not a test we want run every time, as needs comet.ml account")
     def test_comet(self):
diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index 3d9175951..1e55ce990 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -21,7 +21,8 @@ def optimize(optimizer, check_convergence, hook):
     current_error = optimizer.error()
     hook(optimizer, current_error)
 
-    # Iterative loop
+    # Iterative loop.  Cannot use `params.iterationHook` because we don't have access to params
+    # (backwards compatibility issue).
     while True:
         # Do next iteration
         optimizer.iterate()
@@ -35,7 +36,7 @@ def optimize(optimizer, check_convergence, hook):
 def gtsam_optimize(optimizer,
                    params,
                    hook):
-    """ Given an optimizer and params, iterate until convergence.
+    """ Given an optimizer and its params, iterate until convergence.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
         Arguments:
@@ -43,10 +44,6 @@ def gtsam_optimize(optimizer,
                 params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
                 hook -- hook function to record the error
     """
-    def check_convergence(optimizer, current_error, new_error):
-        return (optimizer.iterations() >= params.getMaxIterations()) or (
-            gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
-                                   current_error, new_error)) or (
-            isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
-    optimize(optimizer, check_convergence, hook)
-    return optimizer.values()
+    hook(optimizer, optimizer.error()) # call once at start (backwards compatibility)
+    params.iterationHook = lambda iteration, error_before, error_after: hook(optimizer, error_after)
+    return optimizer.optimize()

From 5cfc87010d8496f39c194647632e4c27e28e4f89 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Tue, 19 Apr 2022 16:10:36 -0400
Subject: [PATCH 043/175] Add comment to docstring of `gtsam_optimize`

---
 python/gtsam/utils/logging_optimizer.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index 1e55ce990..d2cd9b9f6 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -39,6 +39,7 @@ def gtsam_optimize(optimizer,
     """ Given an optimizer and its params, iterate until convergence.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
+        Optimizer must have been created with the same params as the one passed into this function!
         Arguments:
                 optimizer {NonlinearOptimizer} -- Nonlinear optimizer
                 params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters

From 2de3169976e876578c736e844605495dac6c7f30 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 19 Apr 2022 21:50:15 -0400
Subject: [PATCH 044/175] Fix compile

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

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 0816db8df..f29b8d9d5 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -36,9 +36,9 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain(
   return boost::make_shared<HybridFactorGraph>(std::move(hfg));
 }
 
-inline std::pair<std::vector<Key>, std::vector<int>> makeBinaryOrdering(
+inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
     std::vector<Key> &input) {
-  std::vector<Key> new_order;
+  KeyVector new_order;
   std::vector<int> levels(input.size());
   std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
                      int)>

From 2a17bb1715b0d74fe6cbc6f87b807cf4af6cb881 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Apr 2022 15:40:09 -0400
Subject: [PATCH 045/175] Revert "Add comment to docstring of `gtsam_optimize`"

This reverts commit 5cfc87010d8496f39c194647632e4c27e28e4f89.
---
 python/gtsam/utils/logging_optimizer.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index d2cd9b9f6..1e55ce990 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -39,7 +39,6 @@ def gtsam_optimize(optimizer,
     """ Given an optimizer and its params, iterate until convergence.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
-        Optimizer must have been created with the same params as the one passed into this function!
         Arguments:
                 optimizer {NonlinearOptimizer} -- Nonlinear optimizer
                 params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters

From 61eef0639abcb89b632412bda1a1e1906fecf474 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Apr 2022 15:44:17 -0400
Subject: [PATCH 046/175] Partially Revert "Update
 `logging_optimizer.gtsam_optimize` to use
 NonlinearOptimizerParams::iterationHook"

This reverts commit 71aa20ff33ed08c9fbd708d771005f810b370257.
---
 python/gtsam/utils/logging_optimizer.py | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index 1e55ce990..bf727f997 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -36,7 +36,7 @@ def optimize(optimizer, check_convergence, hook):
 def gtsam_optimize(optimizer,
                    params,
                    hook):
-    """ Given an optimizer and its params, iterate until convergence.
+    """ Given an optimizer and params, iterate until convergence.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
         Arguments:
@@ -44,6 +44,10 @@ def gtsam_optimize(optimizer,
                 params {NonlinearOptimizarParams} -- Nonlinear optimizer parameters
                 hook -- hook function to record the error
     """
-    hook(optimizer, optimizer.error()) # call once at start (backwards compatibility)
-    params.iterationHook = lambda iteration, error_before, error_after: hook(optimizer, error_after)
-    return optimizer.optimize()
+    def check_convergence(optimizer, current_error, new_error):
+        return (optimizer.iterations() >= params.getMaxIterations()) or (
+            gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
+                                   current_error, new_error)) or (
+            isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
+    optimize(optimizer, check_convergence, hook)
+    return optimizer.values()

From 5796fe348820c5d6a2e35110b2223e88eb77488c Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Apr 2022 16:21:59 -0400
Subject: [PATCH 047/175] Create convenience wrapper function in
 logging_optimizer

---
 python/gtsam/tests/test_logging_optimizer.py | 54 ++++++++++----------
 python/gtsam/utils/logging_optimizer.py      | 45 ++++++++++++++++
 2 files changed, 73 insertions(+), 26 deletions(-)

diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py
index 4ec782635..b4f32b14f 100644
--- a/python/gtsam/tests/test_logging_optimizer.py
+++ b/python/gtsam/tests/test_logging_optimizer.py
@@ -18,7 +18,7 @@ import numpy as np
 from gtsam import Rot3
 from gtsam.utils.test_case import GtsamTestCase
 
-from gtsam.utils.logging_optimizer import gtsam_optimize
+from gtsam.utils.logging_optimizer import gtsam_optimize, optimize_using
 
 KEY = 0
 MODEL = gtsam.noiseModel.Unit.Create(3)
@@ -34,19 +34,18 @@ class TestOptimizeComet(GtsamTestCase):
         rotations = {R, R.inverse()}  # mean is the identity
         self.expected = Rot3()
 
-        graph = gtsam.NonlinearFactorGraph()
-        for R in rotations:
-            graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
-        initial = gtsam.Values()
-        initial.insert(KEY, R)
-        self.params = gtsam.GaussNewtonParams()
-        self.optimizer = gtsam.GaussNewtonOptimizer(
-            graph, initial, self.params)
+        def check(actual):
+            # Check that optimizing yields the identity
+            self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
+            # Check that logging output prints out 3 lines (exact intermediate values differ by OS)
+            self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3)
+        self.check = check
 
-        self.lmparams = gtsam.LevenbergMarquardtParams()
-        self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
-            graph, initial, self.lmparams
-        )
+        self.graph = gtsam.NonlinearFactorGraph()
+        for R in rotations:
+            self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
+        self.initial = gtsam.Values()
+        self.initial.insert(KEY, R)
 
         # setup output capture
         self.capturedOutput = StringIO()
@@ -64,25 +63,28 @@ class TestOptimizeComet(GtsamTestCase):
             print(error)
 
         # Wrapper function sets the hook and calls optimizer.optimize() for us.
-        gtsam_optimize(self.optimizer, self.params, hook)
-
-        # Check that optimizing yields the identity.
-        actual = self.optimizer.values()
-        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
-        self.assertEqual(self.capturedOutput.getvalue(),
-                         "0.020000000000000004\n0.010000000000000005\n0.010000000000000004\n")
+        params = gtsam.GaussNewtonParams()
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial)
+        self.check(actual)
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial, params)
+        self.check(actual)
+        actual = gtsam_optimize(gtsam.GaussNewtonOptimizer(self.graph, self.initial, params),
+                                params, hook)
+        self.check(actual)
 
     def test_lm_simple_printing(self):
         """Make sure we are properly terminating LM"""
         def hook(_, error):
             print(error)
 
-        gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
-
-        actual = self.lmoptimizer.values()
-        self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
-        self.assertEqual(self.capturedOutput.getvalue(),
-                         "0.020000000000000004\n0.010000000000249996\n0.009999999999999998\n")
+        params = gtsam.LevenbergMarquardtParams()
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial)
+        self.check(actual)
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial,
+                                                                         params)
+        self.check(actual)
+        actual = gtsam_optimize(gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params),
+                                params, hook)
 
     @unittest.skip("Not a test we want run every time, as needs comet.ml account")
     def test_comet(self):
diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index bf727f997..f89208bc5 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -6,6 +6,50 @@ Author: Jing Wu and Frank Dellaert
 
 from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
 import gtsam
+from typing import Any, Callable
+
+OPTIMIZER_PARAMS_MAP = {
+    gtsam.GaussNewtonOptimizer: gtsam.GaussNewtonParams,
+    gtsam.LevenbergMarquardtOptimizer: gtsam.LevenbergMarquardtParams,
+    gtsam.DoglegOptimizer: gtsam.DoglegParams,
+    gtsam.GncGaussNewtonOptimizer: gtsam.GaussNewtonParams,
+    gtsam.GncLMOptimizer: gtsam.LevenbergMarquardtParams
+}
+
+
+def optimize_using(OptimizerClass, hook) -> Callable[[Any], gtsam.Values]:
+    """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration
+        hook.
+        Example usage:
+            solution = optimize_using(gtsam.GaussNewtonOptimizer, hook)(graph, init, params)
+
+    Args:
+        OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer,
+            LevenbergMarquadrtOptimizer)
+        hook ([T, double] -> None): Function to callback after each iteration.  Args are (optimizer,
+            error) and return should be None.
+    Returns:
+        (Callable[*, gtsam.Values]): Call the returned function with the usual NonlinearOptimizer
+            arguments (will be forwarded to constructor) and it will return a Values object
+            representing the solution.  See example usage above.
+    """
+
+    def wrapped_optimize(*args):
+        for arg in args:
+            if isinstance(arg, gtsam.NonlinearOptimizerParams):
+                arg.iterationHook = lambda iteration, error_before, error_after: hook(
+                    optimizer, error_after)
+                break
+        else:
+            params = OPTIMIZER_PARAMS_MAP[OptimizerClass]()
+            params.iterationHook = lambda iteration, error_before, error_after: hook(
+                optimizer, error_after)
+            args = (*args, params)
+        optimizer = OptimizerClass(*args)
+        hook(optimizer, optimizer.error())
+        return optimizer.optimize()
+
+    return wrapped_optimize
 
 
 def optimize(optimizer, check_convergence, hook):
@@ -37,6 +81,7 @@ def gtsam_optimize(optimizer,
                    params,
                    hook):
     """ Given an optimizer and params, iterate until convergence.
+        Recommend using optimize_using instead.
         After each iteration, hook(optimizer) is called.
         After the function, use values and errors to get the result.
         Arguments:

From 1e03c8b1955b25fcf95ba37f401bdf3c2e0b50be Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Apr 2022 16:55:51 -0400
Subject: [PATCH 048/175] reset captured stdout buffer for each test

---
 python/gtsam/tests/test_logging_optimizer.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py
index b4f32b14f..c58e4f121 100644
--- a/python/gtsam/tests/test_logging_optimizer.py
+++ b/python/gtsam/tests/test_logging_optimizer.py
@@ -39,6 +39,8 @@ class TestOptimizeComet(GtsamTestCase):
             self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
             # Check that logging output prints out 3 lines (exact intermediate values differ by OS)
             self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3)
+            # reset stdout catcher
+            self.capturedOutput.truncate(0)
         self.check = check
 
         self.graph = gtsam.NonlinearFactorGraph()

From 7fe577b553f92232bce23e049bc089fdb765e702 Mon Sep 17 00:00:00 2001
From: senselessDev <senselessDev@users.noreply.github.com>
Date: Fri, 22 Apr 2022 18:39:51 +0200
Subject: [PATCH 049/175] update explanation of uncertainty ellipses, try to
 fix #1067

---
 python/gtsam/utils/plot.py | 155 +++++++++++++++++++++++--------------
 1 file changed, 96 insertions(+), 59 deletions(-)

diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index abadf62aa..db78135b7 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -12,13 +12,42 @@ from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=unused-import
 import gtsam
 from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
 
-# For future reference: following
-# https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/
-# we have, in 2D:
-# def kk(p): return math.sqrt(-2*math.log(1-p)) # k to get p probability mass
-# def pp(k): return 1-math.exp(-float(k**2)/2.0) # p as a function of k
-# Some values:
-# k = 5 => p = 99.9996 %
+
+# For translation between a scaling of the uncertainty ellipse and the percentage of
+# inliers see discussion in https://github.com/borglab/gtsam/pull/1067
+#
+# Both directions can be calculated by the following functions:
+# def pct_to_sigma(pct, dof):
+#     return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof))
+#
+# def sigma_to_pct(sigma, dof):
+#     return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100.
+#
+# In the following, the default scaling is chosen for 95% inliers, which
+# translates to the following sigma values:
+# 2D: pct_to_sigma(95, 2) -> 2.447746830680816
+# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074
+#
+# Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
+# page 366 and  https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/
+#
+# For reference, here are the inlier percentages for some sigma values:
+#   	    1    	    2    	    3    	    4    	    5
+# 1D	68.26895	95.44997	99.73002	99.99367	99.99994
+# 2D	39.34693	86.46647	98.88910	99.96645	99.99963
+# 3D	19.87480	73.85359	97.07091	99.88660	99.99846
+#
+# This can be generated by:
+# for dim in range(0, 4):
+#     print("{}D".format(dim), end="")
+#     for n_sigma in range(1, 6):
+#         if dim == 0: print("\t    {}    ".format(n_sigma), end="")
+#         else: print("\t{:.5f}".format(sigma_to_pct(n_sigma, dim)), end="")
+#     print()
+#
+# The translation routines are not included in GTSAM, because it would introduce
+# scipy as a new dependency.
+
 
 def set_axes_equal(fignum: int) -> None:
     """
@@ -81,12 +110,8 @@ def plot_covariance_ellipse_3d(axes,
     """
     Plots a Gaussian as an uncertainty ellipse
 
-    Based on Maybeck Vol 1, page 366
-    For the 3D case:
-    k = 1.878 corresponds to 1 std, 68.26% of all probability
-    k = 3.763 corresponds to 3 std, 99.74% of all probability
-
-    We choose k = 5 which corresponds to 99.99846% of all probability in 3D
+    The ellipse is scaled in such a way that 95% of drawn samples are inliers.
+    Derivation of the scaling factor is explained at the beginning of this file.
 
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
@@ -97,8 +122,8 @@ def plot_covariance_ellipse_3d(axes,
         n: Defines the granularity of the ellipse. Higher values indicate finer ellipses.
         alpha: Transparency value for the plotted surface in the range [0, 1].
     """
-    # Sigma value corresponding to the covariance ellipse
-    k = 5
+    # this corresponds to 95%, see note above
+    k = 2.7954834829151074
     U, S, _ = np.linalg.svd(P)
 
     radii = k * np.sqrt(S)
@@ -119,6 +144,38 @@ def plot_covariance_ellipse_3d(axes,
     axes.plot_surface(x, y, z, alpha=alpha, cmap='hot')
 
 
+def plot_covariance_ellipse_2d(axes,
+                               origin: Point2,
+                               covariance: np.ndarray) -> None:
+    """
+    Plots a Gaussian as an uncertainty ellipse
+
+    The ellipse is scaled in such a way that 95% of drawn samples are inliers.
+    Derivation of the scaling factor is explained at the beginning of this file.
+
+    Args:
+        axes (matplotlib.axes.Axes): Matplotlib axes.
+        origin: The origin in the world frame.
+        covariance: The marginal covariance matrix of the 2D point
+                    which will be represented as an ellipse.
+    """
+
+    w, v = np.linalg.eig(covariance)
+
+    # this corresponds to 95%, see note above
+    k = 2.447746830680816
+
+    angle = np.arctan2(v[1, 0], v[0, 0])
+    # We multiply k by 2 since k corresponds to the radius but Ellipse uses
+    # the diameter.
+    e1 = patches.Ellipse(origin,
+                         np.sqrt(w[0]) * 2 * k,
+                         np.sqrt(w[1]) * 2 * k,
+                         np.rad2deg(angle),
+                         fill=False)
+    axes.add_patch(e1)
+
+
 def plot_point2_on_axes(axes,
                         point: Point2,
                         linespec: str,
@@ -127,13 +184,8 @@ def plot_point2_on_axes(axes,
     Plot a 2D point and its corresponding uncertainty ellipse on given axis
     `axes` with given `linespec`.
 
-    Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
-    page 366
-    For the 2D case:
-    k = 1.515 corresponds to 1 std, 68.26% of all probability
-    k = 3.438 corresponds to 3 std, 99.74% of all probability
-
-    We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
 
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
@@ -143,21 +195,7 @@ def plot_point2_on_axes(axes,
     """
     axes.plot([point[0]], [point[1]], linespec, marker='.', markersize=10)
     if P is not None:
-        w, v = np.linalg.eig(P)
-
-        # 5 sigma corresponds to 99.9996%, see note above
-        k = 5.0
-
-        angle = np.arctan2(v[1, 0], v[0, 0])
-        # We multiply k by 2 since k corresponds to the radius but Ellipse uses
-        # the diameter.
-        e1 = patches.Ellipse(point,
-                             np.sqrt(w[0]) * 2 * k,
-                             np.sqrt(w[1]) * 2 * k,
-                             np.rad2deg(angle),
-                             fill=False)
-        axes.add_patch(e1)
-
+        plot_covariance_ellipse_2d(axes, point, P)
 
 def plot_point2(
     fignum: int,
@@ -169,6 +207,9 @@ def plot_point2(
     """
     Plot a 2D point on given figure with given `linespec`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
+
     Args:
         fignum: Integer representing the figure number to use for plotting.
         point: The point to be plotted.
@@ -197,13 +238,8 @@ def plot_pose2_on_axes(axes,
     """
     Plot a 2D pose on given axis `axes` with given `axis_length`.
 
-    Based on Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
-    page 366
-    For the 2D case:
-    k = 1.515 corresponds to 1 std, 68.26% of all probability
-    k = 3.438 corresponds to 3 std, 99.74% of all probability
-
-    We choose k = 5 which corresponds to 99.99963% of all probability for 2D.
+    The ellipse is scaled in such a way that 95% of drawn samples are inliers,
+    see `plot_covariance_ellipse_2d`.
 
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
@@ -229,21 +265,7 @@ def plot_pose2_on_axes(axes,
     if covariance is not None:
         pPp = covariance[0:2, 0:2]
         gPp = np.matmul(np.matmul(gRp, pPp), gRp.T)
-
-        w, v = np.linalg.eig(gPp)
-
-        # 5 sigma corresponds to 99.9996%, see note above
-        k = 5.0
-
-        angle = np.arctan2(v[1, 0], v[0, 0])
-        # We multiply k by 2 since k corresponds to the radius but Ellipse uses
-        # the diameter.
-        e1 = patches.Ellipse(origin,
-                             np.sqrt(w[0]) * 2 * k,
-                             np.sqrt(w[1]) * 2 * k,
-                             np.rad2deg(angle),
-                             fill=False)
-        axes.add_patch(e1)
+        plot_covariance_ellipse_2d(axes, origin, gPp)
 
 
 def plot_pose2(
@@ -256,6 +278,9 @@ def plot_pose2(
     """
     Plot a 2D pose on given figure with given `axis_length`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_2d`.
+
     Args:
         fignum: Integer representing the figure number to use for plotting.
         pose: The pose to be plotted.
@@ -285,6 +310,9 @@ def plot_point3_on_axes(axes,
     """
     Plot a 3D point on given axis `axes` with given `linespec`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         point: The point to be plotted.
@@ -306,6 +334,9 @@ def plot_point3(
     """
     Plot a 3D point on given figure with given `linespec`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
+
     Args:
         fignum: Integer representing the figure number to use for plotting.
         point: The point to be plotted.
@@ -380,6 +411,9 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
     """
     Plot a 3D pose on given axis `axes` with given `axis_length`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
+
     Args:
         axes (matplotlib.axes.Axes): Matplotlib axes.
         point (gtsam.Point3): The point to be plotted.
@@ -422,6 +456,9 @@ def plot_pose3(
     """
     Plot a 3D pose on given figure with given `axis_length`.
 
+    The uncertainty ellipse (if covariance is given) is scaled in such a way
+    that 95% of drawn samples are inliers, see `plot_covariance_ellipse_3d`.
+
     Args:
         fignum: Integer representing the figure number to use for plotting.
         pose (gtsam.Pose3): 3D pose to be plotted.

From 3ea19f4bd0df147558c46a2c82ee8c2928a580f0 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 23 Apr 2022 19:08:16 +0530
Subject: [PATCH 050/175] add additional methods to TA + wrapper

---
 gtsam/sfm/TranslationRecovery.cpp | 48 ++++++++++++++++++++++++++-----
 gtsam/sfm/TranslationRecovery.h   | 17 ++++++++++-
 gtsam/sfm/sfm.i                   | 16 +++++++++--
 tests/testTranslationRecovery.cpp | 39 ++++++++++++++++++++++++-
 4 files changed, 109 insertions(+), 11 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 2e81c2d56..7938d6b93 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -21,6 +21,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@@ -28,6 +29,7 @@
 #include <gtsam/sfm/TranslationFactor.h>
 #include <gtsam/sfm/TranslationRecovery.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
 
 #include <set>
 #include <utility>
@@ -81,7 +83,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
 }
 
 void TranslationRecovery::addPrior(
-    const double scale, NonlinearFactorGraph *graph,
+    const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
   if (edge == relativeTranslations_.end()) return;
@@ -91,6 +93,32 @@ void TranslationRecovery::addPrior(
       edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
 }
 
+void TranslationRecovery::addPrior(
+    Key i, const Point3 &prior,
+    const boost::shared_ptr<NonlinearFactorGraph> graph,
+    const SharedNoiseModel &priorNoiseModel) const {
+  graph->addPrior(getUniqueKey(i), prior, priorNoiseModel);
+}
+
+Key TranslationRecovery::getUniqueKey(const Key i) const {
+  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
+    Key optimizedKey = optimizedAndDuplicateKeys.first;
+    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
+    if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey;
+  }
+  // Unlikely case, when i is not in the graph.
+  return i;
+}
+
+void TranslationRecovery::addRelativeHardConstraint(
+    Key i, Key j, const Point3 &w_itj,
+    const boost::shared_ptr<NonlinearFactorGraph> graph) const {
+  Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j));
+  Expression<Point3> w_itj_ = wtj_ - wti_;
+  graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj,
+                             w_itj_);
+}
+
 Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
@@ -125,12 +153,17 @@ Values TranslationRecovery::initializeRandomly() const {
 }
 
 Values TranslationRecovery::run(const double scale) const {
-  auto graph = buildGraph();
-  addPrior(scale, &graph);
+  boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
+      boost::make_shared<NonlinearFactorGraph>(buildGraph());
+  addPrior(scale, graph_ptr);
   const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(graph, initial, params_);
+  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_);
   Values result = lm.optimize();
+  return addDuplicateNodes(result);
+}
 
+Values TranslationRecovery::addDuplicateNodes(const Values &result) const {
+  Values final_result = result;
   // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
   // from a key that was optimized to keys that were not optimized. Iterate over
   // map and add results for keys not optimized.
@@ -139,11 +172,12 @@ Values TranslationRecovery::run(const double scale) const {
     std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
     // Add the result for the duplicate key if it does not already exist.
     for (const Key duplicateKey : duplicateKeys) {
-      if (result.exists(duplicateKey)) continue;
-      result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
+      if (final_result.exists(duplicateKey)) continue;
+      final_result.insert<Point3>(duplicateKey,
+                                  final_result.at<Point3>(optimizedKey));
     }
   }
-  return result;
+  return final_result;
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 30c9a14e3..a0bcb5dd4 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -93,10 +93,20 @@ class TranslationRecovery {
    * @param graph factor graph to which prior is added.
    * @param priorNoiseModel the noise model to use with the prior.
    */
-  void addPrior(const double scale, NonlinearFactorGraph *graph,
+  void addPrior(const double scale,
+                const boost::shared_ptr<NonlinearFactorGraph> graph,
                 const SharedNoiseModel &priorNoiseModel =
                     noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
+  void addPrior(Key i, const Point3 &prior,
+                const boost::shared_ptr<NonlinearFactorGraph> graph,
+                const SharedNoiseModel &priorNoiseModel =
+                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+
+  void addRelativeHardConstraint(
+      Key i, Key j, const Point3 &w_itj,
+      const boost::shared_ptr<NonlinearFactorGraph> graph) const;
+
   /**
    * @brief Create random initial translations.
    *
@@ -120,6 +130,8 @@ class TranslationRecovery {
    */
   Values run(const double scale = 1.0) const;
 
+  Values addDuplicateNodes(const Values &result) const;
+
   /**
    * @brief Simulate translation direction measurements
    *
@@ -131,5 +143,8 @@ class TranslationRecovery {
    */
   static TranslationEdges SimulateMeasurements(
       const Values &poses, const std::vector<KeyPair> &edges);
+
+ private:
+  Key getUniqueKey(const Key i) const;
 };
 }  // namespace gtsam
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index bf9a73ac5..36a73b792 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -4,6 +4,8 @@
 
 namespace gtsam {
 
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
 class SfmTrack {
   SfmTrack();
@@ -142,8 +144,8 @@ class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
                    const gtsam::ShonanAveragingParameters2& parameters);
-  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
-                   const gtsam::ShonanAveragingParameters2 &parameters);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors,
+                   const gtsam::ShonanAveragingParameters2& parameters);
 
   // Query properties
   size_t nrUnknowns() const;
@@ -259,6 +261,16 @@ class TranslationRecovery {
   TranslationRecovery(
       const gtsam::BinaryMeasurementsUnit3&
           relativeTranslations);  // default LevenbergMarquardtParams
+  gtsam::NonlinearFactorGraph buildGraph() const;
+  gtsam::Values initializeRandomly() const;
+  void addPrior(gtsam::Key i, const gtsam::Point3& prior,
+                gtsam::NonlinearFactorGraph* graph,
+                const gtsam::SharedNoiseModel& model =
+                    gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const;
+  void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j,
+                                 const gtsam::Point3& w_itj,
+                                 gtsam::NonlinearFactorGraph* graph) const;
+  gtsam::Values addDuplicateNodes(const gtsam::Values& result) const;
   gtsam::Values run(const double scale) const;
   gtsam::Values run() const;  // default scale = 1.0
 };
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 833f11355..006750252 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -17,8 +17,8 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/sfm/TranslationRecovery.h>
 #include <gtsam/sfm/SfmData.h>
+#include <gtsam/sfm/TranslationRecovery.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std;
@@ -265,6 +265,43 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
 }
 
+TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
+  // Create a dataset with 3 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //    \ __ /
+  //     \\//
+  //       3
+  //
+  // 0 and 1 face in the same direction but have a translation offset. 3 is in
+  // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
+
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+
+  auto relativeTranslations =
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}});
+
+  TranslationRecovery algorithm(relativeTranslations);
+  boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
+      boost::make_shared<NonlinearFactorGraph>(algorithm.buildGraph());
+  algorithm.addPrior(0, Point3(), graph_ptr);
+  algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr);
+  const Values initial = algorithm.initializeRandomly();
+  LevenbergMarquardtParams params;
+  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params);
+  auto result = algorithm.addDuplicateNodes(lm.optimize());
+  EXPECT_LONGS_EQUAL(4, graph_ptr->size());
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 099ba8a8ca8f8dfc05fd871402d1f081bb47c3c0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:52:09 -0400
Subject: [PATCH 051/175] Add block Jacobi preconditioner wrap

---
 gtsam/linear/linear.i | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index f1bc92f69..943b661d8 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
   DummyPreconditionerParameters();
 };
 
+virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
+  BlockJacobiPreconditionerParameters();
+};
+
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();

From 3571420010a332743293a8903daeba0afc927d90 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:54:25 -0400
Subject: [PATCH 052/175] Added PinholePose in wrap and FromPose3 in
 EssentialMatrix

---
 gtsam/geometry/geometry.i   | 71 ++++++++++++++++++++++++++++++++++++-
 gtsam/nonlinear/nonlinear.i | 16 +++++++++
 2 files changed, 86 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 415aa0dc4..63fd7056a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -547,6 +547,12 @@ class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
+  // Constructors from Pose3
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                            Eigen::Ref<Eigen::MatrixXd> H);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@@ -904,6 +910,12 @@ class PinholeCamera {
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
                             Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+
+  gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpose,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                                  Eigen::Ref<Eigen::MatrixXd> Dcal);
+
   double range(const gtsam::Point3& point);
   double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
                Eigen::Ref<Eigen::MatrixXd> Dpoint);
@@ -914,7 +926,58 @@ class PinholeCamera {
   // enabling serialization functionality
   void serialize() const;
 };
-  
+
+#include <gtsam/geometry/PinholePose.h>
+template <CALIBRATION>
+class PinholePose {
+  // Standard Constructors and Named Constructors
+  PinholePose();
+  PinholePose(const gtsam::PinholePose<CALIBRATION> other);
+  PinholePose(const gtsam::Pose3& pose);
+  PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION* K);
+
+  // Testable
+  void print(string s = "PinholePose") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -971,6 +1034,12 @@ typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
 
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
 template <T>
 class CameraSet {
   CameraSet();
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 3fff71978..30181e08d 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -226,6 +226,10 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -269,6 +273,10 @@ class Values {
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -310,6 +318,10 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, Vector vector);
@@ -351,6 +363,10 @@ class Values {
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                 gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,

From d4eadbaf20099791e9fbae43075652535be31370 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:54:45 -0400
Subject: [PATCH 053/175] Added wrapping for Shonan constructor

---
 gtsam/sfm/sfm.i | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index bf9a73ac5..901270d9c 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -9,6 +9,8 @@ class SfmTrack {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
+  
+  Point3 p;
 
   double r;
   double g;
@@ -34,6 +36,9 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
+  std::vector<gtsam::SfmTrack> tracks;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>> cameras;
+
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
@@ -184,6 +189,10 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
+  ShonanAveraging3(
+      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
+      const gtsam::ShonanAveragingParameters3& parameters =
+          gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);

From acaf6a273ac8d91d411004f514bf758f7619023d Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:55:08 -0400
Subject: [PATCH 054/175] Wrap GeneralSFMFactor for PinholePose

---
 gtsam/slam/slam.i | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
index 4e943253e..8e1e06d5b 100644
--- a/gtsam/slam/slam.i
+++ b/gtsam/slam/slam.i
@@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorCal3Unified;
 
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Unified;
+
 template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

From fcec6839d3562c4b34b62243f5ff13b126bad3fc Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:56:41 -0400
Subject: [PATCH 055/175] added SfmTracks and SfmCameras so we can easily
 access them in Python

---
 python/gtsam/preamble/sfm.h        | 11 ++++++++++-
 python/gtsam/specializations/sfm.h | 13 +++++++++++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index a34e73058..778375b6b 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -9,4 +9,13 @@
  * automatic STL binding, such that the raw objects can be accessed in Python.
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
- */
\ No newline at end of file
+ */
+
+#include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmTrack>);
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmCamera>);
\ No newline at end of file
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
index 6de15217f..a7a1e822f 100644
--- a/python/gtsam/specializations/sfm.h
+++ b/python/gtsam/specializations/sfm.h
@@ -14,3 +14,16 @@
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+
+py::bind_vector<
+    std::vector<gtsam::SfmTrack> >(
+    m_, "SfmTracks");
+
+py::bind_vector<
+    std::vector<gtsam::SfmCamera> >(
+    m_, "SfmCameras");
+
+py::bind_vector<
+    std::vector<std::pair<size_t, gtsam::Point2>>>(
+        m_, "SfmMeasurementVector"
+    );

From 86b9761b5b4dcc4f6a361df376645c7ddf6f5cfc Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 13:57:48 -0400
Subject: [PATCH 056/175] Also copy the preambles and specializations

---
 python/CMakeLists.txt | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index d3cbff32d..0d143354d 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -107,6 +107,14 @@ file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py"
 foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
         configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
+foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
+endforeach()
+file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
+foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
+endforeach()
 
 # Common directory for data/datasets stored with the package.
 # This will store the data in the Python site package directly.

From 21b8365d7bda979b21bbcf949f7ea78682e05030 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 16:02:00 -0400
Subject: [PATCH 057/175] Address comments

---
 gtsam/geometry/geometry.i | 32 ++++++++++++++++----------------
 gtsam/sfm/SfmData.h       |  8 ++++++--
 gtsam/sfm/sfm.i           |  8 ++++----
 3 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 63fd7056a..0dc23c160 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -927,6 +927,16 @@ class PinholeCamera {
   void serialize() const;
 };
 
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
+
 #include <gtsam/geometry/PinholePose.h>
 template <CALIBRATION>
 class PinholePose {
@@ -978,6 +988,12 @@ class PinholePose {
   void serialize() const;
 };
 
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
 #include <gtsam/geometry/Similarity2.h>
 class Similarity2 {
   // Standard Constructors
@@ -1024,22 +1040,6 @@ class Similarity3 {
   double scale() const;
 };
 
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
-
-typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
-typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
-typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
-typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
-typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
-
 template <T>
 class CameraSet {
   CameraSet();
diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h
index afce12205..430e107ad 100644
--- a/gtsam/sfm/SfmData.h
+++ b/gtsam/sfm/SfmData.h
@@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
   size_t numberCameras() const { return cameras.size(); }
 
   /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const { return tracks[idx]; }
+  const SfmTrack& track(size_t idx) const { return tracks[idx]; }
 
   /// The camera pose at frame index `idx`
-  SfmCamera camera(size_t idx) const { return cameras[idx]; }
+  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
+
+  /// Getters
+  const std::vector<SfmCamera>& cameraList() const { return cameras; }
+  const std::vector<SfmTrack>& trackList() const { return tracks; }
 
   /**
    * @brief Create projection factors using keys i and P(j)
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 901270d9c..0a61874c1 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -36,15 +36,15 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
-  std::vector<gtsam::SfmTrack> tracks;
-  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>> cameras;
+  std::vector<gtsam::SfmTrack>& trackList() const;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
 
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
   size_t numberCameras() const;
-  gtsam::SfmTrack track(size_t idx) const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::SfmTrack& track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
 
   gtsam::NonlinearFactorGraph generalSfmFactors(
       const gtsam::SharedNoiseModel& model =

From 28b216b72fc0e1cc95a13e2998f2972c377783d0 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 3 May 2022 16:24:09 -0400
Subject: [PATCH 058/175] Correct the binding for BinaryMeasurement

---
 gtsam/slam/dataset.h               | 1 +
 python/CMakeLists.txt              | 2 ++
 python/gtsam/preamble/sfm.h        | 6 +++++-
 python/gtsam/specializations/sfm.h | 2 ++
 4 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index dc450a9f7..7c4d7fea7 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
                size_t maxIndex = 0);
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
+using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 0d143354d..0c8dc314b 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -48,6 +48,7 @@ set(ignore
     gtsam::Rot3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
+    gtsam::BinaryMeasurementsRot3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
 
@@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::KeyVector
             gtsam::FixedLagSmootherKeyTimestampMapValue
             gtsam::BinaryMeasurementsUnit3
+            gtsam::BinaryMeasurementsRot3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified
diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index 778375b6b..c74f37109 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -18,4 +18,8 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::SfmTrack>);
 
 PYBIND11_MAKE_OPAQUE(
-    std::vector<gtsam::SfmCamera>);
\ No newline at end of file
+    std::vector<gtsam::SfmCamera>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
index a7a1e822f..90a2b8417 100644
--- a/python/gtsam/specializations/sfm.h
+++ b/python/gtsam/specializations/sfm.h
@@ -13,6 +13,8 @@
 
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
+    m_, "BinaryMeasurementsRot3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
 
 py::bind_vector<

From 4d275a45e64dc3c39b975e582bcdf05b8c248aa3 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 11:23:51 -0400
Subject: [PATCH 059/175] remove key formatting from keys when defining
 vertices and edges to avoid syntax errors

---
 gtsam/inference/DotWriter.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp
index ad5330575..eac0c90f9 100644
--- a/gtsam/inference/DotWriter.cpp
+++ b/gtsam/inference/DotWriter.cpp
@@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
                              const boost::optional<Vector2>& position,
                              ostream* os) const {
   // Label the node with the label from the KeyFormatter
-  *os << "  var" << keyFormatter(key) << "[label=\"" << keyFormatter(key)
+  *os << "  var" << key << "[label=\"" << keyFormatter(key)
       << "\"";
   if (position) {
     *os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
 
 static void ConnectVariables(Key key1, Key key2,
                              const KeyFormatter& keyFormatter, ostream* os) {
-  *os << "  var" << keyFormatter(key1) << "--"
-      << "var" << keyFormatter(key2) << ";\n";
+  *os << "  var" << key1 << "--"
+      << "var" << key2 << ";\n";
 }
 
 static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
                                   size_t i, ostream* os) {
-  *os << "  var" << keyFormatter(key) << "--"
+  *os << "  var" << key << "--"
       << "factor" << i << ";\n";
 }
 

From 51d1c27f2d8c829dfb4711739f2a0fd4d1cd3311 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 11:24:26 -0400
Subject: [PATCH 060/175] add prior factor tests and remove TODO

---
 gtsam/nonlinear/Marginals.h          |  2 +-
 gtsam/nonlinear/PriorFactor.h        |  1 -
 gtsam/slam/tests/testPriorFactor.cpp | 40 +++++++++++++++++++++++++---
 3 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h
index 028545d01..3c5aa9cab 100644
--- a/gtsam/nonlinear/Marginals.h
+++ b/gtsam/nonlinear/Marginals.h
@@ -121,7 +121,7 @@ public:
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
-            
+
 protected:
   
   /** Compute the Bayes Tree as a helper function to the constructor */
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index c745f7bd9..a490162ac 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -94,7 +94,6 @@ namespace gtsam {
     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
-      // TODO(ASL) Add Jacobians.
       return -traits<T>::Local(x, prior_);
     }
 
diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp
index 2dc083cb2..d1a60e346 100644
--- a/gtsam/slam/tests/testPriorFactor.cpp
+++ b/gtsam/slam/tests/testPriorFactor.cpp
@@ -5,12 +5,16 @@
  * @date   Nov 4, 2014
  */
 
-#include <gtsam/base/Vector.h>
-#include <gtsam/nonlinear/PriorFactor.h>
 #include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/nonlinear/factorTesting.h>
 
 using namespace std;
+using namespace std::placeholders;
 using namespace gtsam;
+using namespace imuBias;
 
 /* ************************************************************************* */
 
@@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
 // Constructor vector3
 TEST(PriorFactor, ConstructorVector3) {
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
-  PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
+  PriorFactor<Vector3> factor(1, Vector3(1, 2, 3), model);
 }
 
 // Constructor dynamic sized vector
 TEST(PriorFactor, ConstructorDynamicSizeVector) {
-  Vector v(5); v << 1, 2, 3, 4, 5;
+  Vector v(5);
+  v << 1, 2, 3, 4, 5;
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
   PriorFactor<Vector> factor(1, v, model);
 }
 
+Vector callEvaluateError(const PriorFactor<ConstantBias>& factor,
+                         const ConstantBias& bias) {
+  return factor.evaluateError(bias);
+}
+
+// Test for imuBias::ConstantBias
+TEST(PriorFactor, ConstantBias) {
+  Vector3 biasAcc(1, 2, 3);
+  Vector3 biasGyro(0.1, 0.2, 0.3);
+  ConstantBias bias(biasAcc, biasGyro);
+
+  PriorFactor<ConstantBias> factor(1, bias,
+                                   noiseModel::Isotropic::Sigma(6, 0.1));
+  Values values;
+  values.insert(1, bias);
+
+  EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+
+  ConstantBias incorrectBias(
+      (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished());
+  values.clear();
+  values.insert(1, incorrectBias);
+  EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 0cfeb8621c5605b0d32fb469f4aa8d1600c4c8fd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 14:35:10 -0400
Subject: [PATCH 061/175] fix tests and remove additional keyformatter calls

---
 .../tests/testDiscreteFactorGraph.cpp         | 14 +++----
 gtsam/inference/BayesNet-inst.h               |  5 ++-
 gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 18 ++++-----
 tests/testNonlinearFactorGraph.cpp            | 40 +++++++++----------
 4 files changed, 39 insertions(+), 38 deletions(-)

diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
index 0a7d869ec..3d9621aff 100644
--- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
+++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp
@@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varC[label=\"C\"];\n"
-      "  varA[label=\"A\"];\n"
-      "  varB[label=\"B\"];\n"
+      "  var0[label=\"C\"];\n"
+      "  var1[label=\"A\"];\n"
+      "  var2[label=\"B\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varC--factor0;\n"
-      "  varA--factor0;\n"
+      "  var0--factor0;\n"
+      "  var1--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varC--factor1;\n"
-      "  varB--factor1;\n"
+      "  var0--factor1;\n"
+      "  var2--factor1;\n"
       "}\n";
   EXPECT(actual == expected);
 }
diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h
index afde5498d..e792b5c03 100644
--- a/gtsam/inference/BayesNet-inst.h
+++ b/gtsam/inference/BayesNet-inst.h
@@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
     auto frontals = conditional->frontals();
     const Key me = frontals.front();
     auto parents = conditional->parents();
-    for (const Key& p : parents)
-      os << "  var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
+    for (const Key& p : parents) {
+      os << "  var" << p << "->var" << me << "\n";
+    }
   }
 
   os << "}";
diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
index 2e13be10e..7795d5b89 100644
--- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
+++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp
@@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
          "digraph {\n"
          "  size=\"5,5\";\n"
          "\n"
-         "  vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n"
-         "  vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n"
-         "  varx1[label=\"x1\", pos=\"1,1!\"];\n"
-         "  varx2[label=\"x2\", pos=\"2,1!\"];\n"
-         "  varx3[label=\"x3\", pos=\"3,1!\"];\n"
+         "  var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
+         "  var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
+         "  var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
+         "  var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
+         "  var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
          "\n"
-         "  varx1->varx2\n"
-         "  vara1->varx2\n"
-         "  varx2->varx3\n"
-         "  vara2->varx3\n"
+         "  var8646911284551352321->var8646911284551352322\n"
+         "  var6989586621679009793->var8646911284551352322\n"
+         "  var8646911284551352322->var8646911284551352323\n"
+         "  var6989586621679009794->var8646911284551352323\n"
          "}");
 }
 
diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp
index 05a6e7f45..e1a88d616 100644
--- a/tests/testNonlinearFactorGraph.cpp
+++ b/tests/testNonlinearFactorGraph.cpp
@@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\"];\n"
-      "  varx1[label=\"x1\"];\n"
-      "  varx2[label=\"x2\"];\n"
+      "  var7782220156096217089[label=\"l1\"];\n"
+      "  var8646911284551352321[label=\"x1\"];\n"
+      "  var8646911284551352322[label=\"x2\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varl1[label=\"l1\", pos=\"0,0!\"];\n"
-      "  varx1[label=\"x1\", pos=\"1,0!\"];\n"
-      "  varx2[label=\"x2\", pos=\"1,1.5!\"];\n"
+      "  var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
+      "  var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
+      "  var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varx1--factor0;\n"
+      "  var8646911284551352321--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varx1--factor1;\n"
-      "  varx2--factor1;\n"
+      "  var8646911284551352321--factor1;\n"
+      "  var8646911284551352322--factor1;\n"
       "  factor2[label=\"\", shape=point];\n"
-      "  varx1--factor2;\n"
-      "  varl1--factor2;\n"
+      "  var8646911284551352321--factor2;\n"
+      "  var7782220156096217089--factor2;\n"
       "  factor3[label=\"\", shape=point];\n"
-      "  varx2--factor3;\n"
-      "  varl1--factor3;\n"
+      "  var8646911284551352322--factor3;\n"
+      "  var7782220156096217089--factor3;\n"
       "}\n";
 
   const NonlinearFactorGraph fg = createNonlinearFactorGraph();

From 1d77ba55e400560e59dce39961d56ea59ccd240b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 5 May 2022 16:49:03 -0400
Subject: [PATCH 062/175] fix python test

---
 python/gtsam/tests/test_DiscreteBayesNet.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py
index 74191dcc7..10c5db612 100644
--- a/python/gtsam/tests/test_DiscreteBayesNet.py
+++ b/python/gtsam/tests/test_DiscreteBayesNet.py
@@ -11,12 +11,12 @@ Author: Frank Dellaert
 
 # pylint: disable=no-name-in-module, invalid-name
 
-import unittest
 import textwrap
+import unittest
 
 import gtsam
-from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph,
-                   DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering)
+from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
+                   DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
 from gtsam.utils.test_case import GtsamTestCase
 
 # Some keys:
@@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
               var4[label="4"];
               var5[label="5"];
               var6[label="6"];
-              vara0[label="a0", pos="0,2!"];
+              var6989586621679009792[label="a0", pos="0,2!"];
 
               var4->var6
-              vara0->var3
+              var6989586621679009792->var3
               var3->var5
               var6->var5
             }"""

From 1d6fd5409af322749b84b07c90986d99e6a88ee8 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Thu, 5 May 2022 18:40:58 -0700
Subject: [PATCH 063/175] change to input params, add docstring

---
 gtsam/sfm/TranslationRecovery.cpp | 57 ++++++++++++------------
 gtsam/sfm/TranslationRecovery.h   | 73 ++++++++++++++++++++++++-------
 tests/testTranslationRecovery.cpp | 55 ++++++++++++++++++-----
 3 files changed, 129 insertions(+), 56 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 7938d6b93..da258bbc0 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -28,6 +28,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/TranslationFactor.h>
 #include <gtsam/sfm/TranslationRecovery.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/expressions.h>
 
@@ -42,8 +43,8 @@ static std::mt19937 kRandomNumberGenerator(42);
 
 TranslationRecovery::TranslationRecovery(
     const TranslationRecovery::TranslationEdges &relativeTranslations,
-    const LevenbergMarquardtParams &lmParams)
-    : params_(lmParams) {
+    const TranslationRecoveryParams &params)
+    : params_(params) {
   // Some relative translations may be zero. We treat nodes that have a zero
   // relativeTranslation as a single node.
 
@@ -73,12 +74,21 @@ TranslationRecovery::TranslationRecovery(
 NonlinearFactorGraph TranslationRecovery::buildGraph() const {
   NonlinearFactorGraph graph;
 
-  // Add all relative translation edges
+  // Add translation factors for input translation directions.
   for (auto edge : relativeTranslations_) {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
 
+  // Add between factors for optional relative translations.
+  for (auto edge : params_.getBetweenTranslations()) {
+    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2());
+    if (k1 != k2) {
+      graph.emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
+                                                  edge.noiseModel());
+    }
+  }
+
   return graph;
 }
 
@@ -87,17 +97,13 @@ void TranslationRecovery::addPrior(
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
   if (edge == relativeTranslations_.end()) return;
-  graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
-                                              priorNoiseModel);
-  graph->emplace_shared<PriorFactor<Point3> >(
-      edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
-}
-
-void TranslationRecovery::addPrior(
-    Key i, const Point3 &prior,
-    const boost::shared_ptr<NonlinearFactorGraph> graph,
-    const SharedNoiseModel &priorNoiseModel) const {
-  graph->addPrior(getUniqueKey(i), prior, priorNoiseModel);
+  graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
+                                             priorNoiseModel);
+  // Add a scale prior only if no other between factors were added.
+  if (params_.getBetweenTranslations().empty()) {
+    graph->emplace_shared<PriorFactor<Point3>>(
+        edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+  }
 }
 
 Key TranslationRecovery::getUniqueKey(const Key i) const {
@@ -110,25 +116,21 @@ Key TranslationRecovery::getUniqueKey(const Key i) const {
   return i;
 }
 
-void TranslationRecovery::addRelativeHardConstraint(
-    Key i, Key j, const Point3 &w_itj,
-    const boost::shared_ptr<NonlinearFactorGraph> graph) const {
-  Point3_ wti_(getUniqueKey(i)), wtj_(getUniqueKey(j));
-  Expression<Point3> w_itj_ = wtj_ - wti_;
-  graph->addExpressionFactor(noiseModel::Constrained::All(3, 1e9), w_itj,
-                             w_itj_);
-}
-
 Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
+  const Values inputInitial = params_.getInitialValues();
   auto insert = [&](Key j) {
-    if (!initial.exists(j)) {
+    if (initial.exists(j)) return;
+    if (inputInitial.exists(j)) {
+      initial.insert<Point3>(j, inputInitial.at<Point3>(j));
+    } else {
       initial.insert<Point3>(
           j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
     }
+    // Assumes all nodes connected by zero-edges have the same initialization.
   };
 
   // Loop over measurements and add a random translation
@@ -157,12 +159,13 @@ Values TranslationRecovery::run(const double scale) const {
       boost::make_shared<NonlinearFactorGraph>(buildGraph());
   addPrior(scale, graph_ptr);
   const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_);
+  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams());
   Values result = lm.optimize();
-  return addDuplicateNodes(result);
+  return addSameTranslationNodes(result);
 }
 
-Values TranslationRecovery::addDuplicateNodes(const Values &result) const {
+Values TranslationRecovery::addSameTranslationNodes(
+    const Values &result) const {
   Values final_result = result;
   // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
   // from a key that was optimized to keys that were not optimized. Iterate over
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index a0bcb5dd4..e555941e0 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -28,6 +28,40 @@
 
 namespace gtsam {
 
+// Parameters for the Translation Recovery problem.
+class TranslationRecoveryParams {
+ public:
+  std::vector<BinaryMeasurement<Point3>> getBetweenTranslations() const {
+    return betweenTranslations_;
+  }
+
+  void setBetweenTranslations(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations) {
+    betweenTranslations_ = betweenTranslations;
+  }
+
+  LevenbergMarquardtParams getLMParams() const { return lmParams_; }
+
+  Values getInitialValues() const { return initial_; }
+
+  void setInitialValues(const Values &values) { initial_ = values; }
+
+  void setLMParams(const LevenbergMarquardtParams &lmParams) {
+    lmParams_ = lmParams;
+  }
+
+ private:
+  // Relative translations with a known scale used as between factors in the
+  // problem if provided.
+  std::vector<BinaryMeasurement<Point3>> betweenTranslations_;
+
+  // LevenbergMarquardtParams for optimization.
+  LevenbergMarquardtParams lmParams_;
+
+  // Initial values, random intialization will be used if not provided.
+  Values initial_;
+};
+
 // Set up an optimization problem for the unknown translations Ti in the world
 // coordinate frame, given the known camera attitudes wRi with respect to the
 // world frame, and a set of (noisy) translation directions of type Unit3,
@@ -57,8 +91,8 @@ class TranslationRecovery {
   // Translation directions between camera pairs.
   TranslationEdges relativeTranslations_;
 
-  // Parameters used by the LM Optimizer.
-  LevenbergMarquardtParams params_;
+  // Parameters.
+  TranslationRecoveryParams params_;
 
   // Map from a key in the graph to a set of keys that share the same
   // translation.
@@ -71,13 +105,11 @@ class TranslationRecovery {
    * @param relativeTranslations the relative translations, in world coordinate
    * frames, vector of BinaryMeasurements of Unit3, where each key of a
    * measurement is a point in 3D.
-   * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
-   * used to modify the parameters for the LM optimizer. By default, uses the
-   * default LM parameters.
+   * @param params (optional) parameters for the recovery problem.
    */
   TranslationRecovery(
       const TranslationEdges &relativeTranslations,
-      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
+      const TranslationRecoveryParams &params = TranslationRecoveryParams());
 
   /**
    * @brief Build the factor graph to do the optimization.
@@ -98,15 +130,6 @@ class TranslationRecovery {
                 const SharedNoiseModel &priorNoiseModel =
                     noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
-  void addPrior(Key i, const Point3 &prior,
-                const boost::shared_ptr<NonlinearFactorGraph> graph,
-                const SharedNoiseModel &priorNoiseModel =
-                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
-
-  void addRelativeHardConstraint(
-      Key i, Key j, const Point3 &w_itj,
-      const boost::shared_ptr<NonlinearFactorGraph> graph) const;
-
   /**
    * @brief Create random initial translations.
    *
@@ -126,12 +149,11 @@ class TranslationRecovery {
    * @brief Build and optimize factor graph.
    *
    * @param scale scale for first relative translation which fixes gauge.
+   * The scale is only used if relativeTranslations in the params is empty.
    * @return Values
    */
   Values run(const double scale = 1.0) const;
 
-  Values addDuplicateNodes(const Values &result) const;
-
   /**
    * @brief Simulate translation direction measurements
    *
@@ -145,6 +167,23 @@ class TranslationRecovery {
       const Values &poses, const std::vector<KeyPair> &edges);
 
  private:
+  /**
+   * @brief Gets the key of the variable being optimized among multiple input
+   * variables that have the same translation.
+   *
+   * @param i key of input variable.
+   * @return Key of optimized variable - same as input if it does not have any
+   * zero-translation edges.
+   */
   Key getUniqueKey(const Key i) const;
+
+  /**
+   * @brief Adds nodes that were not optimized for because they were connected
+   * to another node with a zero-translation edge in the input.
+   *
+   * @param result optimization problem result
+   * @return translation estimates for all variables in the input.
+   */
+  Values addSameTranslationNodes(const Values &result) const;
 };
 }  // namespace gtsam
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 006750252..b419d8c58 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -265,6 +265,41 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
 }
 
+TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
+  // Create a dataset with 3 poses.
+  // __      __
+  // \/      \/
+  //  0 _____ 1
+  //    \ __ /
+  //     \\//
+  //       3
+  //
+  // 0 and 1 face in the same direction but have a translation offset. 3 is in
+  // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
+
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+
+  auto relativeTranslations =
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}});
+
+  TranslationRecoveryParams params;
+  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
+  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2));
+  params.setBetweenTranslations(betweenTranslations);
+
+  TranslationRecovery algorithm(relativeTranslations, params);
+  auto result = algorithm.run();
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
+}
+
+
 TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   // Create a dataset with 3 poses.
   // __      __
@@ -283,25 +318,21 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
 
   auto relativeTranslations =
-      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {3, 0}});
+      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}});
 
-  TranslationRecovery algorithm(relativeTranslations);
-  boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
-      boost::make_shared<NonlinearFactorGraph>(algorithm.buildGraph());
-  algorithm.addPrior(0, Point3(), graph_ptr);
-  algorithm.addRelativeHardConstraint(0, 1, Point3(2, 0, 0), graph_ptr);
-  const Values initial = algorithm.initializeRandomly();
-  LevenbergMarquardtParams params;
-  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params);
-  auto result = algorithm.addDuplicateNodes(lm.optimize());
-  EXPECT_LONGS_EQUAL(4, graph_ptr->size());
+  TranslationRecoveryParams params;
+  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
+  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2));
+  params.setBetweenTranslations(betweenTranslations);
+
+  TranslationRecovery algorithm(relativeTranslations, params);
+  auto result = algorithm.run();
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
   EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
 }
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 279b9bedf9aed461e19422d2f0dac8381af4143c Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Thu, 5 May 2022 18:52:01 -0700
Subject: [PATCH 064/175] wrapper changes

---
 gtsam/sfm/sfm.i                    | 36 ++++++++++++++++++------------
 gtsam/slam/dataset.h               |  1 +
 python/CMakeLists.txt              |  2 ++
 python/gtsam/specializations/sfm.h |  2 ++
 4 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 36a73b792..2b6f37a45 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -85,6 +85,7 @@ class BinaryMeasurement {
 
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
+typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
 
 class BinaryMeasurementsUnit3 {
   BinaryMeasurementsUnit3();
@@ -93,6 +94,13 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsPoint3 {
+  BinaryMeasurementsPoint3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in
@@ -254,23 +262,23 @@ class MFAS {
 };
 
 #include <gtsam/sfm/TranslationRecovery.h>
+class TranslationRecoveryParams {
+  gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const;
+  gtsam::Values getInitialValues() const;
+  gtsam::LevenbergMarquardtParams getLMParams() const;
+
+  void setBetweenTranslations(
+      const gtsam::BinaryMeasurementsPoint3& betweenTranslations);
+  void setInitialValues(const gtsam::Values& values);
+  void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams);
+};
+
 class TranslationRecovery {
   TranslationRecovery(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
-      const gtsam::LevenbergMarquardtParams& lmParams);
-  TranslationRecovery(
-      const gtsam::BinaryMeasurementsUnit3&
-          relativeTranslations);  // default LevenbergMarquardtParams
-  gtsam::NonlinearFactorGraph buildGraph() const;
-  gtsam::Values initializeRandomly() const;
-  void addPrior(gtsam::Key i, const gtsam::Point3& prior,
-                gtsam::NonlinearFactorGraph* graph,
-                const gtsam::SharedNoiseModel& model =
-                    gtsam::noiseModel::Isotropic::Sigma(3, 0.01)) const;
-  void addRelativeHardConstraint(gtsam::Key i, gtsam::Key j,
-                                 const gtsam::Point3& w_itj,
-                                 gtsam::NonlinearFactorGraph* graph) const;
-  gtsam::Values addDuplicateNodes(const gtsam::Values& result) const;
+      const gtsam::TranslationRecoveryParams& lmParams);
+  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3&
+                          relativeTranslations);  // default params
   gtsam::Values run(const double scale) const;
   gtsam::Values run() const;  // default scale = 1.0
 };
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index dc450a9f7..247be5bae 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
                size_t maxIndex = 0);
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
+using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index f5869b145..6c0c6e83a 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -47,6 +47,7 @@ set(ignore
     gtsam::Pose3Vector
     gtsam::Rot3Vector
     gtsam::KeyVector
+    gtsam::BinaryMeasurementsPoint3
     gtsam::BinaryMeasurementsUnit3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
@@ -124,6 +125,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::Pose3Vector
             gtsam::KeyVector
             gtsam::FixedLagSmootherKeyTimestampMapValue
+            gtsam::BinaryMeasurementsPoint3
             gtsam::BinaryMeasurementsUnit3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h
index 6de15217f..7a5084d4a 100644
--- a/python/gtsam/specializations/sfm.h
+++ b/python/gtsam/specializations/sfm.h
@@ -11,6 +11,8 @@
  * and saves one copy operation.
  */
 
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
+    m_, "BinaryMeasurementsPoint3");
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");

From e517c344640882fb3e68d97392228abe3e5a925f Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 09:12:56 -0700
Subject: [PATCH 065/175] move betweenTranslations outside params

---
 gtsam/sfm/TranslationRecovery.cpp | 29 ++++++++++++-----------
 gtsam/sfm/TranslationRecovery.h   | 26 +++++++--------------
 tests/testTranslationRecovery.cpp | 39 ++++++++++++++-----------------
 3 files changed, 42 insertions(+), 52 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index da258bbc0..ce332ed0b 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -79,28 +79,29 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
-
-  // Add between factors for optional relative translations.
-  for (auto edge : params_.getBetweenTranslations()) {
-    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2());
-    if (k1 != k2) {
-      graph.emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
-                                                  edge.noiseModel());
-    }
-  }
-
   return graph;
 }
 
 void TranslationRecovery::addPrior(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
   if (edge == relativeTranslations_.end()) return;
   graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
                                              priorNoiseModel);
+
+  // Add between factors for optional relative translations.
+  for (auto edge : betweenTranslations) {
+    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2());
+    if (k1 != k2) {
+      graph->emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
+                                                   edge.noiseModel());
+    }
+  }
+
   // Add a scale prior only if no other between factors were added.
-  if (params_.getBetweenTranslations().empty()) {
+  if (betweenTranslations.empty()) {
     graph->emplace_shared<PriorFactor<Point3>>(
         edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
   }
@@ -154,10 +155,12 @@ Values TranslationRecovery::initializeRandomly() const {
   return initializeRandomly(&kRandomNumberGenerator);
 }
 
-Values TranslationRecovery::run(const double scale) const {
+Values TranslationRecovery::run(
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const double scale) const {
   boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
       boost::make_shared<NonlinearFactorGraph>(buildGraph());
-  addPrior(scale, graph_ptr);
+  addPrior(betweenTranslations, scale, graph_ptr);
   const Values initial = initializeRandomly();
   LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams());
   Values result = lm.optimize();
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index e555941e0..33de91bf1 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -31,15 +31,6 @@ namespace gtsam {
 // Parameters for the Translation Recovery problem.
 class TranslationRecoveryParams {
  public:
-  std::vector<BinaryMeasurement<Point3>> getBetweenTranslations() const {
-    return betweenTranslations_;
-  }
-
-  void setBetweenTranslations(
-      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations) {
-    betweenTranslations_ = betweenTranslations;
-  }
-
   LevenbergMarquardtParams getLMParams() const { return lmParams_; }
 
   Values getInitialValues() const { return initial_; }
@@ -51,10 +42,6 @@ class TranslationRecoveryParams {
   }
 
  private:
-  // Relative translations with a known scale used as between factors in the
-  // problem if provided.
-  std::vector<BinaryMeasurement<Point3>> betweenTranslations_;
-
   // LevenbergMarquardtParams for optimization.
   LevenbergMarquardtParams lmParams_;
 
@@ -125,10 +112,11 @@ class TranslationRecovery {
    * @param graph factor graph to which prior is added.
    * @param priorNoiseModel the noise model to use with the prior.
    */
-  void addPrior(const double scale,
-                const boost::shared_ptr<NonlinearFactorGraph> graph,
-                const SharedNoiseModel &priorNoiseModel =
-                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+  void addPrior(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
    * @brief Create random initial translations.
@@ -152,7 +140,9 @@ class TranslationRecovery {
    * The scale is only used if relativeTranslations in the params is empty.
    * @return Values
    */
-  Values run(const double scale = 1.0) const;
+  Values run(
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
+      const double scale = 1.0) const;
 
   /**
    * @brief Simulate translation direction measurements
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index b419d8c58..6c7233a37 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -68,7 +68,7 @@ TEST(TranslationRecovery, BAL) {
 
   // Run translation recovery
   const double scale = 2.0;
-  const auto result = algorithm.run(scale);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, scale);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
@@ -112,7 +112,7 @@ TEST(TranslationRecovery, TwoPoseTest) {
   EXPECT_LONGS_EQUAL(1, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -149,7 +149,7 @@ TEST(TranslationRecovery, ThreePoseTest) {
   const auto graph = algorithm.buildGraph();
   EXPECT_LONGS_EQUAL(3, graph.size());
 
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -186,7 +186,7 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
   EXPECT_LONGS_EQUAL(1, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/3.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -227,7 +227,7 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
   EXPECT_LONGS_EQUAL(3, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/4.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -257,7 +257,7 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
   EXPECT_LONGS_EQUAL(0, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*scale=*/4.0);
+  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -282,16 +282,15 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
   poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
   poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
 
-  auto relativeTranslations =
-      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}});
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
 
-  TranslationRecoveryParams params;
   std::vector<BinaryMeasurement<Point3>> betweenTranslations;
-  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0), noiseModel::Isotropic::Sigma(3, 1e-2));
-  params.setBetweenTranslations(betweenTranslations);
+  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0),
+                                   noiseModel::Isotropic::Sigma(3, 1e-2));
 
-  TranslationRecovery algorithm(relativeTranslations, params);
-  auto result = algorithm.run();
+  TranslationRecovery algorithm(relativeTranslations);
+  auto result = algorithm.run(betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
@@ -299,7 +298,6 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
 }
 
-
 TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   // Create a dataset with 3 poses.
   // __      __
@@ -317,16 +315,15 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
   poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
 
-  auto relativeTranslations =
-      TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {0, 3}, {1, 3}});
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
 
-  TranslationRecoveryParams params;
   std::vector<BinaryMeasurement<Point3>> betweenTranslations;
-  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0), noiseModel::Constrained::All(3, 1e2));
-  params.setBetweenTranslations(betweenTranslations);
+  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
+                                   noiseModel::Constrained::All(3, 1e2));
 
-  TranslationRecovery algorithm(relativeTranslations, params);
-  auto result = algorithm.run();
+  TranslationRecovery algorithm(relativeTranslations);
+  auto result = algorithm.run(betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));

From 0e8c5eb5a7bd93265199add2083ce81091ec43c6 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 09:23:00 -0700
Subject: [PATCH 066/175] ../gtsfm/sfm/sfm.i

---
 gtsam/sfm/sfm.i | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 2b6f37a45..064bc1e63 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -263,12 +263,9 @@ class MFAS {
 
 #include <gtsam/sfm/TranslationRecovery.h>
 class TranslationRecoveryParams {
-  gtsam::BinaryMeasurementsPoint3 getBetweenTranslations() const;
   gtsam::Values getInitialValues() const;
   gtsam::LevenbergMarquardtParams getLMParams() const;
 
-  void setBetweenTranslations(
-      const gtsam::BinaryMeasurementsPoint3& betweenTranslations);
   void setInitialValues(const gtsam::Values& values);
   void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams);
 };
@@ -279,6 +276,9 @@ class TranslationRecovery {
       const gtsam::TranslationRecoveryParams& lmParams);
   TranslationRecovery(const gtsam::BinaryMeasurementsUnit3&
                           relativeTranslations);  // default params
+  gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                    const double scale) const;
+  // default empty betweenTranslations
   gtsam::Values run(const double scale) const;
   gtsam::Values run() const;  // default scale = 1.0
 };

From b42d3a3d2ff9bf50f98e07ba9d6df6aff5782ed2 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 11:24:31 -0700
Subject: [PATCH 067/175] change params to struct

---
 gtsam/sfm/TranslationRecovery.cpp | 14 ++++++--------
 gtsam/sfm/TranslationRecovery.h   | 20 ++++----------------
 gtsam/sfm/sfm.i                   |  9 +++------
 3 files changed, 13 insertions(+), 30 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index ce332ed0b..ce8e9c079 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -84,7 +84,7 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
 
 void TranslationRecovery::addPrior(
     const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
-    const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
+    const double scale, NonlinearFactorGraph *graph,
     const SharedNoiseModel &priorNoiseModel) const {
   auto edge = relativeTranslations_.begin();
   if (edge == relativeTranslations_.end()) return;
@@ -122,11 +122,10 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
-  const Values inputInitial = params_.getInitialValues();
   auto insert = [&](Key j) {
     if (initial.exists(j)) return;
-    if (inputInitial.exists(j)) {
-      initial.insert<Point3>(j, inputInitial.at<Point3>(j));
+    if (params_.initial.exists(j)) {
+      initial.insert<Point3>(j, params_.initial.at<Point3>(j));
     } else {
       initial.insert<Point3>(
           j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
@@ -158,11 +157,10 @@ Values TranslationRecovery::initializeRandomly() const {
 Values TranslationRecovery::run(
     const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const double scale) const {
-  boost::shared_ptr<NonlinearFactorGraph> graph_ptr =
-      boost::make_shared<NonlinearFactorGraph>(buildGraph());
-  addPrior(betweenTranslations, scale, graph_ptr);
+  NonlinearFactorGraph graph = buildGraph();
+  addPrior(betweenTranslations, scale, &graph);
   const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(*graph_ptr, initial, params_.getLMParams());
+  LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
   Values result = lm.optimize();
   return addSameTranslationNodes(result);
 }
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 33de91bf1..0b25ad7b4 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -29,24 +29,12 @@
 namespace gtsam {
 
 // Parameters for the Translation Recovery problem.
-class TranslationRecoveryParams {
- public:
-  LevenbergMarquardtParams getLMParams() const { return lmParams_; }
-
-  Values getInitialValues() const { return initial_; }
-
-  void setInitialValues(const Values &values) { initial_ = values; }
-
-  void setLMParams(const LevenbergMarquardtParams &lmParams) {
-    lmParams_ = lmParams;
-  }
-
- private:
+struct TranslationRecoveryParams {
   // LevenbergMarquardtParams for optimization.
-  LevenbergMarquardtParams lmParams_;
+  LevenbergMarquardtParams lmParams;
 
   // Initial values, random intialization will be used if not provided.
-  Values initial_;
+  Values initial;
 };
 
 // Set up an optimization problem for the unknown translations Ti in the world
@@ -114,7 +102,7 @@ class TranslationRecovery {
    */
   void addPrior(
       const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
-      const double scale, const boost::shared_ptr<NonlinearFactorGraph> graph,
+      const double scale, NonlinearFactorGraph *graph,
       const SharedNoiseModel &priorNoiseModel =
           noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 064bc1e63..66280037c 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -263,11 +263,8 @@ class MFAS {
 
 #include <gtsam/sfm/TranslationRecovery.h>
 class TranslationRecoveryParams {
-  gtsam::Values getInitialValues() const;
-  gtsam::LevenbergMarquardtParams getLMParams() const;
-
-  void setInitialValues(const gtsam::Values& values);
-  void setLMParams(const gtsam::LevenbergMarquardtParams& lmParams);
+  gtsam::LevenbergMarquardtParams lmParams;
+  gtsam::Values initial;
 };
 
 class TranslationRecovery {
@@ -279,7 +276,7 @@ class TranslationRecovery {
   gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
                     const double scale) const;
   // default empty betweenTranslations
-  gtsam::Values run(const double scale) const;
+  // gtsam::Values run(const double scale) const;
   gtsam::Values run() const;  // default scale = 1.0
 };
 

From c1a7cf21d58e2bb2e42db1838c492aa854a69b2e Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 11:26:21 -0700
Subject: [PATCH 068/175] rename getUniqueKey

---
 gtsam/sfm/TranslationRecovery.cpp | 5 +++--
 gtsam/sfm/TranslationRecovery.h   | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index ce8e9c079..aa75b8fc6 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -93,7 +93,8 @@ void TranslationRecovery::addPrior(
 
   // Add between factors for optional relative translations.
   for (auto edge : betweenTranslations) {
-    Key k1 = getUniqueKey(edge.key1()), k2 = getUniqueKey(edge.key2());
+    Key k1 = getSameTranslationRootNode(edge.key1()),
+        k2 = getSameTranslationRootNode(edge.key2());
     if (k1 != k2) {
       graph->emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
                                                    edge.noiseModel());
@@ -107,7 +108,7 @@ void TranslationRecovery::addPrior(
   }
 }
 
-Key TranslationRecovery::getUniqueKey(const Key i) const {
+Key TranslationRecovery::getSameTranslationRootNode(const Key i) const {
   for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
     Key optimizedKey = optimizedAndDuplicateKeys.first;
     std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 0b25ad7b4..931d072c8 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -153,7 +153,7 @@ class TranslationRecovery {
    * @return Key of optimized variable - same as input if it does not have any
    * zero-translation edges.
    */
-  Key getUniqueKey(const Key i) const;
+  Key getSameTranslationRootNode(const Key i) const;
 
   /**
    * @brief Adds nodes that were not optimized for because they were connected

From e2a26346528f997e036bc52d00ffe91dbf9d3d9e Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 7 May 2022 15:53:36 -0700
Subject: [PATCH 069/175] Finally debugged the root cause!

---
 python/gtsam/preamble/sfm.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h
index c74f37109..8ff0ea82e 100644
--- a/python/gtsam/preamble/sfm.h
+++ b/python/gtsam/preamble/sfm.h
@@ -11,7 +11,8 @@
  * mutations on Python side will not be reflected on C++.
  */
 
-#include <pybind11/stl.h>
+// Including <stl.h> can cause some serious hard-to-debug bugs!!!
+// #include <pybind11/stl.h>
 #include <pybind11/stl_bind.h>
 
 PYBIND11_MAKE_OPAQUE(

From 02dce469d185f105f46eb42c147712ece2ff2f3d Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 7 May 2022 15:55:06 -0700
Subject: [PATCH 070/175] Add fallback entry for MATLAB

---
 gtsam/sfm/sfm.i | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 0a61874c1..19e21b10c 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -96,6 +96,13 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in

From 230bb8eb115254739faa6bd6609b8d6434d2b016 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 18:15:44 -0700
Subject: [PATCH 071/175] move relativeTranslations to run()

---
 gtsam/sfm/TranslationRecovery.cpp | 171 ++++++++++++++++--------------
 gtsam/sfm/TranslationRecovery.h   |  65 +++++-------
 tests/testTranslationRecovery.cpp |  49 ++++-----
 3 files changed, 140 insertions(+), 145 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index aa75b8fc6..122b17ce6 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -41,16 +41,13 @@ using namespace std;
 // In Wrappers we have no access to this so have a default ready.
 static std::mt19937 kRandomNumberGenerator(42);
 
-TranslationRecovery::TranslationRecovery(
-    const TranslationRecovery::TranslationEdges &relativeTranslations,
-    const TranslationRecoveryParams &params)
-    : params_(params) {
-  // Some relative translations may be zero. We treat nodes that have a zero
-  // relativeTranslation as a single node.
-
-  // A DSFMap is used to find sets of nodes that have a zero relative
-  // translation. Add the nodes in each edge to the DSFMap, and merge nodes that
-  // are connected by a zero relative translation.
+// Some relative translations may be zero. We treat nodes that have a zero
+// relativeTranslation as a single node.
+// A DSFMap is used to find sets of nodes that have a zero relative
+// translation. Add the nodes in each edge to the DSFMap, and merge nodes that
+// are connected by a zero relative translation.
+DSFMap<Key> getSameTranslationDSFMap(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) {
   DSFMap<Key> sameTranslationDSF;
   for (const auto &edge : relativeTranslations) {
     Key key1 = sameTranslationDSF.find(edge.key1());
@@ -59,23 +56,52 @@ TranslationRecovery::TranslationRecovery(
       sameTranslationDSF.merge(key1, key2);
     }
   }
-  // Use only those edges for which two keys have a distinct root in the DSFMap.
-  for (const auto &edge : relativeTranslations) {
-    Key key1 = sameTranslationDSF.find(edge.key1());
-    Key key2 = sameTranslationDSF.find(edge.key2());
-    if (key1 == key2) continue;
-    relativeTranslations_.emplace_back(key1, key2, edge.measured(),
-                                       edge.noiseModel());
-  }
-  // Store the DSF map for post-processing results.
-  sameTranslationNodes_ = sameTranslationDSF.sets();
+  return sameTranslationDSF;
 }
 
-NonlinearFactorGraph TranslationRecovery::buildGraph() const {
+// Removes zero-translation edges from measurements, and combines the nodes in
+// these edges into a single node.
+template <typename T>
+std::vector<BinaryMeasurement<T>> removeSameTranslationNodes(
+    const std::vector<BinaryMeasurement<T>> &edges,
+    const DSFMap<Key> &sameTranslationDSFMap) {
+  std::vector<BinaryMeasurement<T>> newEdges;
+  for (const auto &edge : edges) {
+    Key key1 = sameTranslationDSFMap.find(edge.key1());
+    Key key2 = sameTranslationDSFMap.find(edge.key2());
+    if (key1 == key2) continue;
+    newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel());
+  }
+  return newEdges;
+}
+
+// Adds nodes that were not optimized for because they were connected
+// to another node with a zero-translation edge in the input.
+Values addSameTranslationNodes(const Values &result,
+                               const DSFMap<Key> &sameTranslationDSFMap) {
+  Values final_result = result;
+  // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
+  // from a key that was optimized to keys that were not optimized. Iterate over
+  // map and add results for keys not optimized.
+  for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
+    Key optimizedKey = optimizedAndDuplicateKeys.first;
+    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
+    // Add the result for the duplicate key if it does not already exist.
+    for (const Key duplicateKey : duplicateKeys) {
+      if (final_result.exists(duplicateKey)) continue;
+      final_result.insert<Point3>(duplicateKey,
+                                  final_result.at<Point3>(optimizedKey));
+    }
+  }
+  return final_result;
+}
+
+NonlinearFactorGraph TranslationRecovery::buildGraph(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
   NonlinearFactorGraph graph;
 
   // Add translation factors for input translation directions.
-  for (auto edge : relativeTranslations_) {
+  for (auto edge : relativeTranslations) {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
@@ -83,22 +109,20 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
 }
 
 void TranslationRecovery::addPrior(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const double scale,
     const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
-    const double scale, NonlinearFactorGraph *graph,
+    NonlinearFactorGraph *graph,
     const SharedNoiseModel &priorNoiseModel) const {
-  auto edge = relativeTranslations_.begin();
-  if (edge == relativeTranslations_.end()) return;
+  auto edge = relativeTranslations.begin();
+  if (edge == relativeTranslations.end()) return;
   graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
                                              priorNoiseModel);
 
   // Add between factors for optional relative translations.
   for (auto edge : betweenTranslations) {
-    Key k1 = getSameTranslationRootNode(edge.key1()),
-        k2 = getSameTranslationRootNode(edge.key2());
-    if (k1 != k2) {
-      graph->emplace_shared<BetweenFactor<Point3>>(k1, k2, edge.measured(),
-                                                   edge.noiseModel());
-    }
+    graph->emplace_shared<BetweenFactor<Point3>>(
+        edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
   }
 
   // Add a scale prior only if no other between factors were added.
@@ -108,17 +132,9 @@ void TranslationRecovery::addPrior(
   }
 }
 
-Key TranslationRecovery::getSameTranslationRootNode(const Key i) const {
-  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-    Key optimizedKey = optimizedAndDuplicateKeys.first;
-    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
-    if (i == optimizedKey || duplicateKeys.count(i)) return optimizedKey;
-  }
-  // Unlikely case, when i is not in the graph.
-  return i;
-}
-
-Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
+Values TranslationRecovery::initializeRandomly(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    std::mt19937 *rng) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
@@ -135,54 +151,53 @@ Values TranslationRecovery::initializeRandomly(std::mt19937 *rng) const {
   };
 
   // Loop over measurements and add a random translation
-  for (auto edge : relativeTranslations_) {
+  for (auto edge : relativeTranslations) {
     insert(edge.key1());
     insert(edge.key2());
   }
-
-  // If there are no valid edges, but zero-distance edges exist, initialize one
-  // of the nodes in a connected component of zero-distance edges.
-  if (initial.empty() && !sameTranslationNodes_.empty()) {
-    for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-      Key optimizedKey = optimizedAndDuplicateKeys.first;
-      initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
-    }
-  }
   return initial;
 }
 
-Values TranslationRecovery::initializeRandomly() const {
-  return initializeRandomly(&kRandomNumberGenerator);
+Values TranslationRecovery::initializeRandomly(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
+  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator);
 }
 
 Values TranslationRecovery::run(
-    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
-    const double scale) const {
-  NonlinearFactorGraph graph = buildGraph();
-  addPrior(betweenTranslations, scale, &graph);
-  const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
-  Values result = lm.optimize();
-  return addSameTranslationNodes(result);
-}
+    const TranslationEdges &relativeTranslations, const double scale,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations) const {
+  // Find edges that have a zero-translation, and recompute relativeTranslations
+  // and betweenTranslations by retaining only one node for every zero-edge.
+  DSFMap<Key> sameTranslationDSFMap =
+      getSameTranslationDSFMap(relativeTranslations);
+  const TranslationEdges nonzeroRelativeTranslations =
+      removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap);
+  const std::vector<BinaryMeasurement<Point3>> nonzeroBetweenTranslations =
+      removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap);
 
-Values TranslationRecovery::addSameTranslationNodes(
-    const Values &result) const {
-  Values final_result = result;
-  // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
-  // from a key that was optimized to keys that were not optimized. Iterate over
-  // map and add results for keys not optimized.
-  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-    Key optimizedKey = optimizedAndDuplicateKeys.first;
-    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
-    // Add the result for the duplicate key if it does not already exist.
-    for (const Key duplicateKey : duplicateKeys) {
-      if (final_result.exists(duplicateKey)) continue;
-      final_result.insert<Point3>(duplicateKey,
-                                  final_result.at<Point3>(optimizedKey));
+  // Create graph of translation factors.
+  NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations);
+
+  // Add global frame prior and scale (either from betweenTranslations or
+  // scale).
+  addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations,
+           &graph);
+
+  // Uses initial values from params if provided.
+  Values initial = initializeRandomly(nonzeroRelativeTranslations);
+
+  // If there are no valid edges, but zero-distance edges exist, initialize one
+  // of the nodes in a connected component of zero-distance edges.
+  if (initial.empty() && !sameTranslationDSFMap.sets().empty()) {
+    for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
+      Key optimizedKey = optimizedAndDuplicateKeys.first;
+      initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
     }
   }
-  return final_result;
+
+  LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
+  Values result = lm.optimize();
+  return addSameTranslationNodes(result, sameTranslationDSFMap);
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 931d072c8..a9f823d1a 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -69,29 +69,25 @@ class TranslationRecovery {
   // Parameters.
   TranslationRecoveryParams params_;
 
-  // Map from a key in the graph to a set of keys that share the same
-  // translation.
-  std::map<Key, std::set<Key>> sameTranslationNodes_;
-
  public:
   /**
    * @brief Construct a new Translation Recovery object
    *
-   * @param relativeTranslations the relative translations, in world coordinate
-   * frames, vector of BinaryMeasurements of Unit3, where each key of a
-   * measurement is a point in 3D.
-   * @param params (optional) parameters for the recovery problem.
+   * @param params parameters for the recovery problem.
    */
-  TranslationRecovery(
-      const TranslationEdges &relativeTranslations,
-      const TranslationRecoveryParams &params = TranslationRecoveryParams());
+  TranslationRecovery(const TranslationRecoveryParams &params)
+      : params_(params) {}
+
+  // Same as above, with default parameters.
+  TranslationRecovery() = default;
 
   /**
    * @brief Build the factor graph to do the optimization.
    *
    * @return NonlinearFactorGraph
    */
-  NonlinearFactorGraph buildGraph() const;
+  NonlinearFactorGraph buildGraph(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
 
   /**
    * @brief Add priors on ednpoints of first measurement edge.
@@ -101,8 +97,10 @@ class TranslationRecovery {
    * @param priorNoiseModel the noise model to use with the prior.
    */
   void addPrior(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const double scale,
       const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
-      const double scale, NonlinearFactorGraph *graph,
+      NonlinearFactorGraph *graph,
       const SharedNoiseModel &priorNoiseModel =
           noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
@@ -112,25 +110,34 @@ class TranslationRecovery {
    * @param rng random number generator
    * @return Values
    */
-  Values initializeRandomly(std::mt19937 *rng) const;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      std::mt19937 *rng) const;
 
   /**
    * @brief Version of initializeRandomly with a fixed seed.
    *
    * @return Values
    */
-  Values initializeRandomly() const;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
 
   /**
    * @brief Build and optimize factor graph.
    *
+   * @param relativeTranslations the relative translations, in world coordinate
+   * frames, vector of BinaryMeasurements of Unit3, where each key of a
+   * measurement is a point in 3D.
    * @param scale scale for first relative translation which fixes gauge.
-   * The scale is only used if relativeTranslations in the params is empty.
+   * The scale is only used if betweenTranslations is empty.
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @return Values
    */
-  Values run(
-      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
-      const double scale = 1.0) const;
+  Values run(const TranslationEdges &relativeTranslations,
+             const double scale = 1.0,
+             const std::vector<BinaryMeasurement<Point3>> &betweenTranslations =
+                 {}) const;
 
   /**
    * @brief Simulate translation direction measurements
@@ -143,25 +150,5 @@ class TranslationRecovery {
    */
   static TranslationEdges SimulateMeasurements(
       const Values &poses, const std::vector<KeyPair> &edges);
-
- private:
-  /**
-   * @brief Gets the key of the variable being optimized among multiple input
-   * variables that have the same translation.
-   *
-   * @param i key of input variable.
-   * @return Key of optimized variable - same as input if it does not have any
-   * zero-translation edges.
-   */
-  Key getSameTranslationRootNode(const Key i) const;
-
-  /**
-   * @brief Adds nodes that were not optimized for because they were connected
-   * to another node with a zero-translation edge in the input.
-   *
-   * @param result optimization problem result
-   * @return translation estimates for all variables in the input.
-   */
-  Values addSameTranslationNodes(const Values &result) const;
 };
 }  // namespace gtsam
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 6c7233a37..5dd319d30 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -62,13 +62,13 @@ TEST(TranslationRecovery, BAL) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
+  TranslationRecovery algorithm;
+  const auto graph = algorithm.buildGraph(relativeTranslations);
   EXPECT_LONGS_EQUAL(3, graph.size());
 
   // Run translation recovery
   const double scale = 2.0;
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, scale);
+  const auto result = algorithm.run(relativeTranslations, scale);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
@@ -107,12 +107,12 @@ TEST(TranslationRecovery, TwoPoseTest) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
+  TranslationRecovery algorithm;
+  const auto graph = algorithm.buildGraph(relativeTranslations);
   EXPECT_LONGS_EQUAL(1, graph.size());
 
   // Run translation recovery
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
+  const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
 
   // Check result for first two translations, determined by prior
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -145,11 +145,11 @@ TEST(TranslationRecovery, ThreePoseTest) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
+  TranslationRecovery algorithm;
+  const auto graph = algorithm.buildGraph(relativeTranslations);
   EXPECT_LONGS_EQUAL(3, graph.size());
 
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
+  const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -180,13 +180,9 @@ TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
-  // There is only 1 non-zero translation edge.
-  EXPECT_LONGS_EQUAL(1, graph.size());
-
+  TranslationRecovery algorithm;
   // Run translation recovery
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/3.0);
+  const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -222,12 +218,10 @@ TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
-  EXPECT_LONGS_EQUAL(3, graph.size());
+  TranslationRecovery algorithm;
 
   // Run translation recovery
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
+  const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -251,13 +245,10 @@ TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
                         unitTranslation.measured()));
   }
 
-  TranslationRecovery algorithm(relativeTranslations);
-  const auto graph = algorithm.buildGraph();
-  // Graph size will be zero as there no 'non-zero distance' edges.
-  EXPECT_LONGS_EQUAL(0, graph.size());
+  TranslationRecovery algorithm;
 
   // Run translation recovery
-  const auto result = algorithm.run(/*betweenTranslations=*/{}, /*scale=*/4.0);
+  const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
@@ -289,8 +280,9 @@ TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
   betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0),
                                    noiseModel::Isotropic::Sigma(3, 1e-2));
 
-  TranslationRecovery algorithm(relativeTranslations);
-  auto result = algorithm.run(betweenTranslations);
+  TranslationRecovery algorithm;
+  auto result =
+      algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
@@ -322,8 +314,9 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
                                    noiseModel::Constrained::All(3, 1e2));
 
-  TranslationRecovery algorithm(relativeTranslations);
-  auto result = algorithm.run(betweenTranslations);
+  TranslationRecovery algorithm;
+  auto result =
+      algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
 
   // Check result
   EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));

From c0e5ce9ef9da661440d13e47953400d96be9ab54 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 18:28:54 -0700
Subject: [PATCH 072/175] wrapper updates

---
 gtsam/sfm/sfm.i                               | 30 ++++++++++++++-----
 .../examples/TranslationAveragingExample.py   |  2 +-
 .../gtsam/tests/test_TranslationRecovery.py   | 10 +++++--
 3 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 66280037c..122a9b5a6 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -265,19 +265,33 @@ class MFAS {
 class TranslationRecoveryParams {
   gtsam::LevenbergMarquardtParams lmParams;
   gtsam::Values initial;
+  TranslationRecoveryParams();
 };
 
 class TranslationRecovery {
-  TranslationRecovery(
+  TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams);
+  TranslationRecovery();  // default params.
+  void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                const double scale,
+                const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                gtsam::NonlinearFactorGraph @graph,
+                const gtsam::SharedNoiseModel& priorNoiseModel) const;
+  void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                const double scale,
+                const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                gtsam::NonlinearFactorGraph @graph) const;
+  gtsam::NonlinearFactorGraph buildGraph(
+      const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
+  gtsam::Values run(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
-      const gtsam::TranslationRecoveryParams& lmParams);
-  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3&
-                          relativeTranslations);  // default params
-  gtsam::Values run(const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
-                    const double scale) const;
+      const double scale,
+      const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
   // default empty betweenTranslations
-  // gtsam::Values run(const double scale) const;
-  gtsam::Values run() const;  // default scale = 1.0
+  gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                    const double scale) const;
+  // default scale = 1.0, empty betweenTranslations
+  gtsam::Values run(
+      const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
 };
 
 }  // namespace gtsam
diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py
index 054b61126..92a7d04e3 100644
--- a/python/gtsam/examples/TranslationAveragingExample.py
+++ b/python/gtsam/examples/TranslationAveragingExample.py
@@ -123,7 +123,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
     w_iZj_inliers = filter_outliers(w_iZj_list)
 
     # Run the optimizer to obtain translations for normalized directions.
-    wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run()
+    wtc_values = gtsam.TranslationRecovery().run(w_iZj_inliers)
 
     wTc_values = gtsam.Values()
     for key in wRc_values.keys():
diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py
index 0fb0518b6..2875e897e 100644
--- a/python/gtsam/tests/test_TranslationRecovery.py
+++ b/python/gtsam/tests/test_TranslationRecovery.py
@@ -34,8 +34,10 @@ class TestTranslationRecovery(unittest.TestCase):
 
     def test_constructor(self):
         """Construct from binary measurements."""
-        algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3())
+        algorithm = gtsam.TranslationRecovery()
         self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
+        algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams())
+        self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
 
     def test_run(self):
         gt_poses = ExampleValues()
@@ -44,10 +46,12 @@ class TestTranslationRecovery(unittest.TestCase):
         # Set verbosity to Silent for tests
         lmParams = gtsam.LevenbergMarquardtParams()
         lmParams.setVerbosityLM("silent")
+        params = gtsam.TranslationRecoveryParams()
+        params.lmParams = lmParams
 
-        algorithm = gtsam.TranslationRecovery(measurements, lmParams)
+        algorithm = gtsam.TranslationRecovery(params)
         scale = 2.0
-        result = algorithm.run(scale)
+        result = algorithm.run(measurements, scale)
 
         w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
         w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()

From 3b77df652c8a8840ffa5785d061344553534b441 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 7 May 2022 18:36:00 -0700
Subject: [PATCH 073/175] docstring updates

---
 gtsam/sfm/TranslationRecovery.h | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index a9f823d1a..3462cce02 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -11,7 +11,7 @@
 
 /**
  * @file TranslationRecovery.h
- * @author Frank Dellaert
+ * @author Frank Dellaert, Akshay Krishnan
  * @date March 2020
  * @brief Recovering translations in an epipolar graph when rotations are given.
  */
@@ -78,12 +78,16 @@ class TranslationRecovery {
   TranslationRecovery(const TranslationRecoveryParams &params)
       : params_(params) {}
 
-  // Same as above, with default parameters.
+  /**
+   * @brief Default constructor.
+   */
   TranslationRecovery() = default;
 
   /**
    * @brief Build the factor graph to do the optimization.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @return NonlinearFactorGraph
    */
   NonlinearFactorGraph buildGraph(
@@ -92,8 +96,12 @@ class TranslationRecovery {
   /**
    * @brief Add priors on ednpoints of first measurement edge.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @param scale scale for first relative translation which fixes gauge.
    * @param graph factor graph to which prior is added.
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param priorNoiseModel the noise model to use with the prior.
    */
   void addPrior(
@@ -105,8 +113,11 @@ class TranslationRecovery {
           noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
-   * @brief Create random initial translations.
+   * @brief Create random initial translations. Uses inial values from params if
+   * provided.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @param rng random number generator
    * @return Values
    */
@@ -115,8 +126,11 @@ class TranslationRecovery {
       std::mt19937 *rng) const;
 
   /**
-   * @brief Version of initializeRandomly with a fixed seed.
+   * @brief Version of initializeRandomly with a fixed seed. Uses initial values
+   * from params if provided.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @return Values
    */
   Values initializeRandomly(

From e17eddf99f8eac95077887b871f1859f7d5eb802 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Mon, 9 May 2022 22:46:16 -0700
Subject: [PATCH 074/175] move values from params to run, remove params struct

---
 gtsam/sfm/TranslationRecovery.cpp             | 20 ++++----
 gtsam/sfm/TranslationRecovery.h               | 46 +++++++++----------
 gtsam/sfm/sfm.i                               | 12 ++---
 .../gtsam/tests/test_TranslationRecovery.py   |  4 +-
 4 files changed, 41 insertions(+), 41 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 122b17ce6..810fe7de9 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -134,15 +134,15 @@ void TranslationRecovery::addPrior(
 
 Values TranslationRecovery::initializeRandomly(
     const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
-    std::mt19937 *rng) const {
+    std::mt19937 *rng, const Values &initialValues) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
   // initializes if not.
   Values initial;
   auto insert = [&](Key j) {
     if (initial.exists(j)) return;
-    if (params_.initial.exists(j)) {
-      initial.insert<Point3>(j, params_.initial.at<Point3>(j));
+    if (initialValues.exists(j)) {
+      initial.insert<Point3>(j, initialValues.at<Point3>(j));
     } else {
       initial.insert<Point3>(
           j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
@@ -159,13 +159,16 @@ Values TranslationRecovery::initializeRandomly(
 }
 
 Values TranslationRecovery::initializeRandomly(
-    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
-  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator);
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const Values &initialValues) const {
+  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
+                            initialValues);
 }
 
 Values TranslationRecovery::run(
     const TranslationEdges &relativeTranslations, const double scale,
-    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations) const {
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const Values &initialValues) const {
   // Find edges that have a zero-translation, and recompute relativeTranslations
   // and betweenTranslations by retaining only one node for every zero-edge.
   DSFMap<Key> sameTranslationDSFMap =
@@ -184,7 +187,8 @@ Values TranslationRecovery::run(
            &graph);
 
   // Uses initial values from params if provided.
-  Values initial = initializeRandomly(nonzeroRelativeTranslations);
+  Values initial =
+      initializeRandomly(nonzeroRelativeTranslations, initialValues);
 
   // If there are no valid edges, but zero-distance edges exist, initialize one
   // of the nodes in a connected component of zero-distance edges.
@@ -195,7 +199,7 @@ Values TranslationRecovery::run(
     }
   }
 
-  LevenbergMarquardtOptimizer lm(graph, initial, params_.lmParams);
+  LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
   Values result = lm.optimize();
   return addSameTranslationNodes(result, sameTranslationDSFMap);
 }
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 3462cce02..7863f5133 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -28,15 +28,6 @@
 
 namespace gtsam {
 
-// Parameters for the Translation Recovery problem.
-struct TranslationRecoveryParams {
-  // LevenbergMarquardtParams for optimization.
-  LevenbergMarquardtParams lmParams;
-
-  // Initial values, random intialization will be used if not provided.
-  Values initial;
-};
-
 // Set up an optimization problem for the unknown translations Ti in the world
 // coordinate frame, given the known camera attitudes wRi with respect to the
 // world frame, and a set of (noisy) translation directions of type Unit3,
@@ -67,16 +58,16 @@ class TranslationRecovery {
   TranslationEdges relativeTranslations_;
 
   // Parameters.
-  TranslationRecoveryParams params_;
+  LevenbergMarquardtParams lmParams_;
 
  public:
   /**
    * @brief Construct a new Translation Recovery object
    *
-   * @param params parameters for the recovery problem.
+   * @param lmParams parameters for optimization.
    */
-  TranslationRecovery(const TranslationRecoveryParams &params)
-      : params_(params) {}
+  TranslationRecovery(const LevenbergMarquardtParams &lmParams)
+      : lmParams_(lmParams) {}
 
   /**
    * @brief Default constructor.
@@ -94,7 +85,11 @@ class TranslationRecovery {
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
 
   /**
-   * @brief Add priors on ednpoints of first measurement edge.
+   * @brief Add 3 factors to the graph:
+   *    - A prior on the first point to lie at (0, 0, 0)
+   *    - If betweenTranslations is non-empty, between factors provided by it.
+   *    - If betweenTranslations is empty, a prior on scale of the first
+   * relativeTranslations edge.
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
@@ -113,28 +108,29 @@ class TranslationRecovery {
           noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
-   * @brief Create random initial translations. Uses inial values from params if
-   * provided.
+   * @brief Create random initial translations.
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
    * @param rng random number generator
+   * @param intialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
-      std::mt19937 *rng) const;
+      std::mt19937 *rng, const Values &initialValues = Values()) const;
 
   /**
-   * @brief Version of initializeRandomly with a fixed seed. Uses initial values
-   * from params if provided.
+   * @brief Version of initializeRandomly with a fixed seed.
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
+   * @param initialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
-      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Build and optimize factor graph.
@@ -146,12 +142,14 @@ class TranslationRecovery {
    * The scale is only used if betweenTranslations is empty.
    * @param betweenTranslations relative translations (with scale) between 2
    * points in world coordinate frame known a priori.
+   * @param initialValues intial values for optimization. Initializes randomly
+   * if not provided.
    * @return Values
    */
-  Values run(const TranslationEdges &relativeTranslations,
-             const double scale = 1.0,
-             const std::vector<BinaryMeasurement<Point3>> &betweenTranslations =
-                 {}) const;
+  Values run(
+      const TranslationEdges &relativeTranslations, const double scale = 1.0,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Simulate translation direction measurements
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 122a9b5a6..33f23614a 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -262,14 +262,9 @@ class MFAS {
 };
 
 #include <gtsam/sfm/TranslationRecovery.h>
-class TranslationRecoveryParams {
-  gtsam::LevenbergMarquardtParams lmParams;
-  gtsam::Values initial;
-  TranslationRecoveryParams();
-};
 
 class TranslationRecovery {
-  TranslationRecovery(const gtsam::TranslationRecoveryParams& lmParams);
+  TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams);
   TranslationRecovery();  // default params.
   void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
                 const double scale,
@@ -282,6 +277,11 @@ class TranslationRecovery {
                 gtsam::NonlinearFactorGraph @graph) const;
   gtsam::NonlinearFactorGraph buildGraph(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
+  gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                    const double scale,
+                    const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                    const gtsam::Values& initialValues) const;
+  // default random initial values
   gtsam::Values run(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
       const double scale,
diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py
index 2875e897e..0908f0a69 100644
--- a/python/gtsam/tests/test_TranslationRecovery.py
+++ b/python/gtsam/tests/test_TranslationRecovery.py
@@ -46,10 +46,8 @@ class TestTranslationRecovery(unittest.TestCase):
         # Set verbosity to Silent for tests
         lmParams = gtsam.LevenbergMarquardtParams()
         lmParams.setVerbosityLM("silent")
-        params = gtsam.TranslationRecoveryParams()
-        params.lmParams = lmParams
 
-        algorithm = gtsam.TranslationRecovery(params)
+        algorithm = gtsam.TranslationRecovery(lmParams)
         scale = 2.0
         result = algorithm.run(measurements, scale)
 

From 50b77ceb551b60a35b2fd8bc272b753b9edb506d Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 10 May 2022 08:36:16 -0700
Subject: [PATCH 075/175] fixing merge conflict in wrapper

---
 gtsam/sfm/sfm.i | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index df0cb279b..83bd07b13 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -104,6 +104,7 @@ class BinaryMeasurementsPoint3 {
   size_t size() const;
   gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
   void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+};
 
 class BinaryMeasurementsRot3 {
   BinaryMeasurementsRot3();

From 4e481ea1e111cd08ff0222f86c2ad2447da0fbd7 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 10 May 2022 17:45:50 -0700
Subject: [PATCH 076/175] Fix the enable_if_t absence

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

diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index dd8b7f022..626f2b10c 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
 template <class CLIQUE>
 class BayesTreeOrphanWrapper<
     CLIQUE,
-    boost::enable_if_t<boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
+    typename std::enable_if<boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
     : public CLIQUE::ConditionalType {
  public:
   typedef CLIQUE CliqueType;

From 29787373a6ec40cbb526eed1f3616fb892bdd621 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 10 May 2022 17:46:52 -0700
Subject: [PATCH 077/175] update python test constructor

---
 python/gtsam/tests/test_TranslationRecovery.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py
index 0908f0a69..99fbce89e 100644
--- a/python/gtsam/tests/test_TranslationRecovery.py
+++ b/python/gtsam/tests/test_TranslationRecovery.py
@@ -36,7 +36,7 @@ class TestTranslationRecovery(unittest.TestCase):
         """Construct from binary measurements."""
         algorithm = gtsam.TranslationRecovery()
         self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
-        algorithm_params = gtsam.TranslationRecovery(gtsam.TranslationRecoveryParams())
+        algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams())
         self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
 
     def test_run(self):

From d012bcd4dc1fa53ad805d6c1dc50ff6e3ea2e6d8 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 10 May 2022 19:22:50 -0700
Subject: [PATCH 078/175] Remove most debug messages

---
 gtsam/hybrid/HybridJunctionTree.cpp          | 10 +++++-----
 gtsam/hybrid/tests/testHybridConditional.cpp |  5 ++++-
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 77e623a41..bf088c5aa 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -59,12 +59,12 @@ struct HybridConstructorTraversalData {
     myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
     parentData.myJTNode->addChild(myData.myJTNode);
 
-#ifndef NDEBUG
+#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
     std::cout << "Getting discrete info: ";
 #endif
     for (HybridFactor::shared_ptr& f : node->factors) {
       for (auto& k : f->discreteKeys_) {
-#ifndef NDEBUG
+#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
         std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
 #endif
 
@@ -104,7 +104,7 @@ struct HybridConstructorTraversalData {
     boost::tie(myConditional, mySeparatorFactor) =
         internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
 
-#ifndef NDEBUG
+#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
     std::cout << "Symbolic: ";
     myConditional->print();
 #endif
@@ -136,7 +136,7 @@ struct HybridConstructorTraversalData {
             myData.discreteKeys.exists(myConditional->frontals()[0]);
         const bool theirType =
             myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
-#ifndef NDEBUG
+#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
         std::cout << "Type: "
                   << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs "
                   << DefaultKeyFormatter(childConditionals[i]->frontals()[0])
@@ -145,7 +145,7 @@ struct HybridConstructorTraversalData {
         if (myType == theirType) {
           // Increment number of frontal variables
           myNrFrontals += nrFrontals[i];
-#ifndef NDEBUG
+#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
           std::cout << "Merging ";
           childConditionals[i]->print();
 #endif
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridConditional.cpp
index f3664e619..3d96ebd6a 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridConditional.cpp
@@ -57,6 +57,7 @@ using gtsam::symbol_shorthand::D;
 using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::Y;
 
+#ifdef HYBRID_DEBUG
 #define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED
 
 #include <signal.h>  // ::signal, ::raise
@@ -68,6 +69,7 @@ void my_signal_handler(int signum) {
   std::cout << boost::stacktrace::stacktrace();
   ::raise(SIGABRT);
 }
+#endif
 
 /* ************************************************************************* */
 TEST_DISABLED(HybridFactorGraph, creation) {
@@ -594,9 +596,10 @@ TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) {
 
 /* ************************************************************************* */
 int main() {
+#ifdef HYBRID_DEBUG
   ::signal(SIGSEGV, &my_signal_handler);
   ::signal(SIGBUS, &my_signal_handler);
-
+#endif
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

From de1827ca40b100cd08bfdb69341e9e7d44ed10a4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 May 2022 22:50:10 -0400
Subject: [PATCH 079/175] Added notebook with SenselessDev code.

---
 python/gtsam/notebooks/ellipses.ipynb | 55 +++++++++++++--------------
 python/gtsam/utils/plot.py            | 38 ++++++------------
 2 files changed, 37 insertions(+), 56 deletions(-)

diff --git a/python/gtsam/notebooks/ellipses.ipynb b/python/gtsam/notebooks/ellipses.ipynb
index d1ce9a015..06938f696 100644
--- a/python/gtsam/notebooks/ellipses.ipynb
+++ b/python/gtsam/notebooks/ellipses.ipynb
@@ -13,17 +13,18 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
     "import scipy\n",
-    "import scipy.stats"
+    "import scipy.stats\n",
+    "import numpy as np"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -36,7 +37,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 23,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [
     {
@@ -51,17 +52,17 @@
     }
    ],
    "source": [
-    "for dim in range(0, 4):\n",
-    "    print(\"{}D\".format(dim), end=\"\")\n",
+    "for dof in range(0, 4):\n",
+    "    print(\"{}D\".format(dof), end=\"\")\n",
     "    for sigma in range(1, 6):\n",
-    "        if dim == 0: print(\"\\t    {}    \".format(sigma), end=\"\")\n",
-    "        else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dim)), end=\"\")\n",
+    "        if dof == 0: print(\"\\t    {}    \".format(sigma), end=\"\")\n",
+    "        else: print(\"\\t{:.5f}%\".format(sigma_to_pct(sigma, dof)), end=\"\")\n",
     "    print()"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 29,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
@@ -70,34 +71,30 @@
      "text": [
       "1D\n",
       "\n",
-      "sigma=1.0 -> 68.26895%\n",
-      "sigma=2.0 -> 95.44997%\n",
-      "sigma=2.5 -> 98.75807%\n",
-      "sigma=5.0 -> 99.99994%\n",
+      "pct=50.0 -> sigma=0.674489750196\n",
+      "pct=95.0 -> sigma=1.959963984540\n",
+      "pct=99.0 -> sigma=2.575829303549\n",
       "\n",
       "2D\n",
       "\n",
-      "sigma=1.0 -> 39.34693%\n",
-      "sigma=2.0 -> 86.46647%\n",
-      "sigma=2.5 -> 95.60631%\n",
-      "sigma=5.0 -> 99.99963%\n",
+      "pct=50.0 -> sigma=1.177410022515\n",
+      "pct=95.0 -> sigma=2.447746830681\n",
+      "pct=99.0 -> sigma=3.034854258770\n",
       "\n",
       "3D\n",
       "\n",
-      "sigma=1.0 -> 19.87480%\n",
-      "sigma=2.0 -> 73.85359%\n",
-      "sigma=2.5 -> 89.99392%\n",
-      "sigma=5.0 -> 99.99846%\n",
+      "pct=50.0 -> sigma=1.538172254455\n",
+      "pct=95.0 -> sigma=2.795483482915\n",
+      "pct=99.0 -> sigma=3.368214175219\n",
       "\n"
      ]
     }
    ],
    "source": [
-    "for dim in range(1, 4):\n",
-    "    print(\"{}D\\n\".format(dim))\n",
-    "    for sigma in [1, 2, 2.5, 5]:\n",
-    "        print(\"sigma={:.1f} -> {:.5f}%\".format(sigma, sigma_to_pct(sigma, dim)), end=\"\")\n",
-    "        print()\n",
+    "for dof in range(1, 4):\n",
+    "    print(\"{}D\\n\".format(dof))\n",
+    "    for pct in [50, 95, 99]:\n",
+    "        print(\"pct={:.1f} -> sigma={:.12f}\".format(pct, pct_to_sigma(pct, dof)))\n",
     "    print()"
    ]
   },
@@ -111,10 +108,10 @@
  ],
  "metadata": {
   "interpreter": {
-   "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101"
+   "hash": "4d608302ba82e7596903db5446e6fa05f049271852e8cc6e1cafaafe5fbd9fed"
   },
   "kernelspec": {
-   "display_name": "Python 3 (ipykernel)",
+   "display_name": "Python 3.8.13 ('gtsfm-v1')",
    "language": "python",
    "name": "python3"
   },
@@ -128,7 +125,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.10.4"
+   "version": "3.8.13"
   }
  },
  "nbformat": 4,
diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index db78135b7..a4d19f72b 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -13,41 +13,25 @@ import gtsam
 from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
 
 
-# For translation between a scaling of the uncertainty ellipse and the percentage of
-# inliers see discussion in https://github.com/borglab/gtsam/pull/1067
-#
-# Both directions can be calculated by the following functions:
-# def pct_to_sigma(pct, dof):
-#     return np.sqrt(scipy.stats.chi2.ppf(pct / 100., df=dof))
-#
-# def sigma_to_pct(sigma, dof):
-#     return scipy.stats.chi2.cdf(sigma**2, df=dof) * 100.
+# For translation between a scaling of the uncertainty ellipse and the 
+# percentage of inliers see discussion in 
+#   [PR 1067](https://github.com/borglab/gtsam/pull/1067)
+# and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy).
 #
 # In the following, the default scaling is chosen for 95% inliers, which
 # translates to the following sigma values:
-# 2D: pct_to_sigma(95, 2) -> 2.447746830680816
-# 3D: pct_to_sigma(95, 3) -> 2.7954834829151074
+# 1D: 1.959963984540
+# 2D: 2.447746830681
+# 3D: 2.795483482915
 #
 # Further references are Stochastic Models, Estimation, and Control Vol 1 by Maybeck,
-# page 366 and  https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/
+# page 366 and https://www.xarg.org/2018/04/how-to-plot-a-covariance-error-ellipse/
 #
 # For reference, here are the inlier percentages for some sigma values:
 #   	    1    	    2    	    3    	    4    	    5
 # 1D	68.26895	95.44997	99.73002	99.99367	99.99994
 # 2D	39.34693	86.46647	98.88910	99.96645	99.99963
 # 3D	19.87480	73.85359	97.07091	99.88660	99.99846
-#
-# This can be generated by:
-# for dim in range(0, 4):
-#     print("{}D".format(dim), end="")
-#     for n_sigma in range(1, 6):
-#         if dim == 0: print("\t    {}    ".format(n_sigma), end="")
-#         else: print("\t{:.5f}".format(sigma_to_pct(n_sigma, dim)), end="")
-#     print()
-#
-# The translation routines are not included in GTSAM, because it would introduce
-# scipy as a new dependency.
-
 
 def set_axes_equal(fignum: int) -> None:
     """
@@ -123,7 +107,7 @@ def plot_covariance_ellipse_3d(axes,
         alpha: Transparency value for the plotted surface in the range [0, 1].
     """
     # this corresponds to 95%, see note above
-    k = 2.7954834829151074
+    k = 2.795483482915
     U, S, _ = np.linalg.svd(P)
 
     radii = k * np.sqrt(S)
@@ -160,10 +144,10 @@ def plot_covariance_ellipse_2d(axes,
                     which will be represented as an ellipse.
     """
 
-    w, v = np.linalg.eig(covariance)
+    w, v = np.linalg.eigh(covariance)
 
     # this corresponds to 95%, see note above
-    k = 2.447746830680816
+    k = 2.447746830681
 
     angle = np.arctan2(v[1, 0], v[0, 0])
     # We multiply k by 2 since k corresponds to the radius but Ellipse uses

From b5e9dc04fd478c12bca67ba02f0b6e8216958db8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 May 2022 22:56:55 -0400
Subject: [PATCH 080/175] Make status a public field

---
 gtsam/geometry/triangulation.h | 66 ++++++++++++++--------------------
 1 file changed, 26 insertions(+), 40 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 49b5aef04..401fd2d0b 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -510,18 +511,18 @@ private:
 };
 
 /**
- * TriangulationResult is an optional point, along with the reasons why it is invalid.
+ * TriangulationResult is an optional point, along with the reasons why it is
+ * invalid.
  */
-class TriangulationResult: public boost::optional<Point3> {
-  enum Status {
-    VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
-  };
-  Status status_;
-  TriangulationResult(Status s) :
-      status_(s) {
-  }
-public:
+class TriangulationResult : public boost::optional<Point3> {
+ public:
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
+  Status status;
 
+ private:
+  TriangulationResult(Status s) : status(s) {}
+
+ public:
   /**
    * Default constructor, only for serialization
    */
@@ -530,54 +531,38 @@ public:
   /**
    * Constructor
    */
-  TriangulationResult(const Point3& p) :
-      status_(VALID) {
-    reset(p);
-  }
+  TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
   static TriangulationResult Degenerate() {
     return TriangulationResult(DEGENERATE);
   }
-  static TriangulationResult Outlier() {
-    return TriangulationResult(OUTLIER);
-  }
+  static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
   static TriangulationResult FarPoint() {
     return TriangulationResult(FAR_POINT);
   }
   static TriangulationResult BehindCamera() {
     return TriangulationResult(BEHIND_CAMERA);
   }
-  bool valid() const {
-    return status_ == VALID;
-  }
-  bool degenerate() const {
-    return status_ == DEGENERATE;
-  }
-  bool outlier() const {
-    return status_ == OUTLIER;
-  }
-  bool farPoint() const {
-    return status_ == FAR_POINT;
-  }
-  bool behindCamera() const {
-    return status_ == BEHIND_CAMERA;
-  }
+  bool valid() const { return status == VALID; }
+  bool degenerate() const { return status == DEGENERATE; }
+  bool outlier() const { return status == OUTLIER; }
+  bool farPoint() const { return status == FAR_POINT; }
+  bool behindCamera() const { return status == BEHIND_CAMERA; }
   // stream to output
-  friend std::ostream &operator<<(std::ostream &os,
-      const TriangulationResult& result) {
+  friend std::ostream& operator<<(std::ostream& os,
+                                  const TriangulationResult& result) {
     if (result)
       os << "point = " << *result << std::endl;
     else
-      os << "no point, status = " << result.status_ << std::endl;
+      os << "no point, status = " << result.status << std::endl;
     return os;
   }
 
-private:
-
+ private:
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(status_);
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int version) {
+    ar& BOOST_SERIALIZATION_NVP(status);
   }
 };
 
@@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;

From 58cbdcddd52b8fb81f9f5fe2f4b9a3c310529d58 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 May 2022 22:57:38 -0400
Subject: [PATCH 081/175] Wrap TriangulationResult

---
 gtsam/geometry/geometry.i | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 0dc23c160..8d70fb679 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1089,6 +1089,11 @@ class StereoCamera {
 
 #include <gtsam/geometry/triangulation.h>
 
+virtual class TriangulationResult : boost::optional<gtsam::Point3> {
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
+  Status status;
+};
+
 // Templates appear not yet supported for free functions - issue raised at
 // borglab/wrap#14 to add support
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,

From e60f4ac0c6c426617ed2be875d8d64eee195e869 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 May 2022 22:58:07 -0400
Subject: [PATCH 082/175] Be consistent in wrapping templated triangulation
 versions

---
 gtsam/geometry/geometry.i | 89 +++++++++++++++++++++++++++++----------
 1 file changed, 66 insertions(+), 23 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 8d70fb679..c297ef82e 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1096,56 +1096,99 @@ virtual class TriangulationResult : boost::optional<gtsam::Point3> {
 
 // Templates appear not yet supported for free functions - issue raised at
 // borglab/wrap#14 to add support
+
+// Cal3_S2 versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3_S2* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3DS2* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3Bundler* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3_S2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+
+// Cal3DS2 versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3DS2* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3DS2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+
+// Cal3Bundler versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Bundler* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Bundler* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+
+// Cal3Fisheye versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Fisheye* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Fisheye* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+
+// Cal3Unified versions                                
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Unified* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3_S2* sharedCal,
+                                   gtsam::Cal3Unified* sharedCal,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3DS2* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3Bundler* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
 
+
+
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>
 class BearingRange {

From 02828e2ec7aa62a117341c641de64e9bcc76f57f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 May 2022 23:11:40 -0400
Subject: [PATCH 083/175] Fix unrelated warning

---
 gtsam/linear/SubgraphBuilder.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp
index 18e19cd20..de7ae7060 100644
--- a/gtsam/linear/SubgraphBuilder.cpp
+++ b/gtsam/linear/SubgraphBuilder.cpp
@@ -337,7 +337,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
   DSFVector dsf(n);
 
   size_t count = 0;
-  double sum = 0.0;
   for (const size_t index : sortedIndices) {
     const GaussianFactor &gf = *gfg[index];
     const auto keys = gf.keys();
@@ -347,7 +346,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
     if (dsf.find(u) != dsf.find(v)) {
       dsf.merge(u, v);
       treeIndices.push_back(index);
-      sum += weights[index];
       if (++count == n - 1) break;
     }
   }

From 8333c8f15c9a081a1a993dcce9b25d2b30afa921 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 00:04:42 -0400
Subject: [PATCH 084/175] Wrapped safe triangulation versions

---
 gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index c297ef82e..96249b6e0 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1094,6 +1094,19 @@ virtual class TriangulationResult : boost::optional<gtsam::Point3> {
   Status status;
 };
 
+class TriangulationParameters {
+  double rankTolerance;
+  bool enableEPI;
+  double landmarkDistanceThreshold;
+  double dynamicOutlierRejectionThreshold;
+  SharedNoiseModel noiseModel;
+  TriangulationParameters(const double rankTolerance = 1.0,
+                          const bool enableEPI = false,
+                          double landmarkDistanceThreshold = -1,
+                          double dynamicOutlierRejectionThreshold = -1,
+                          const gtsam::SharedNoiseModel& noiseModel = nullptr);
+};
+
 // Templates appear not yet supported for free functions - issue raised at
 // borglab/wrap#14 to add support
 
@@ -1114,6 +1127,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3_S2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
 
 // Cal3DS2 versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
@@ -1132,6 +1149,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3DS2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
 
 // Cal3Bundler versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
@@ -1150,6 +1171,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Bundler& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
 
 // Cal3Fisheye versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
@@ -1168,6 +1193,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Fisheye& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
 
 // Cal3Unified versions                                
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
@@ -1186,6 +1215,10 @@ gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Unified& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
 
 
 

From 287ea289209b6cee9c9dbb6d141fb46aa2d5ee98 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 00:27:50 -0400
Subject: [PATCH 085/175] Camera sets

---
 gtsam/geometry/geometry.i               | 10 +++++++---
 python/gtsam/specializations/geometry.h |  2 ++
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 96249b6e0..1e9444b02 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1040,7 +1040,11 @@ class Similarity3 {
   double scale() const;
 };
 
-template <T>
+template <T = {gtsam::PinholeCameraCal3_S2,
+               gtsam::PinholeCameraCal3DS2,
+               gtsam::PinholeCameraCal3Bundler,
+               gtsam::PinholeCameraCal3Fisheye,
+               gtsam::PinholeCameraCal3Unified}>
 class CameraSet {
   CameraSet();
 
@@ -1088,10 +1092,10 @@ class StereoCamera {
 };
 
 #include <gtsam/geometry/triangulation.h>
-
-virtual class TriangulationResult : boost::optional<gtsam::Point3> {
+class TriangulationResult {
   enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
   Status status;
+  const gtsam::Point3& get() const;
 };
 
 class TriangulationParameters {
diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h
index 5a0ea35ef..99f40253f 100644
--- a/python/gtsam/specializations/geometry.h
+++ b/python/gtsam/specializations/geometry.h
@@ -21,6 +21,8 @@ py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
     m_, "CameraSetCal3_S2");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3DS2>>>(
+    m_, "CameraSetCal3DS2");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(
     m_, "CameraSetCal3Bundler");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>>(

From db371c2fdb0ff463049ebee3f7574da4e98e9136 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 00:28:01 -0400
Subject: [PATCH 086/175] Added safe triangulation tests

---
 python/gtsam/tests/test_Triangulation.py | 83 +++++++++++++++++++-----
 1 file changed, 68 insertions(+), 15 deletions(-)

diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py
index 0a258a0af..c079e76b8 100644
--- a/python/gtsam/tests/test_Triangulation.py
+++ b/python/gtsam/tests/test_Triangulation.py
@@ -8,26 +8,17 @@ See LICENSE for the license information
 Test Triangulation
 Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
 """
+# pylint: disable=no-name-in-module, invalid-name, no-member
 import unittest
 from typing import Iterable, List, Optional, Tuple, Union
-
 import numpy as np
 
 import gtsam
-from gtsam import (
-    Cal3_S2,
-    Cal3Bundler,
-    CameraSetCal3_S2,
-    CameraSetCal3Bundler,
-    PinholeCameraCal3_S2,
-    PinholeCameraCal3Bundler,
-    Point2,
-    Point2Vector,
-    Point3,
-    Pose3,
-    Pose3Vector,
-    Rot3,
-)
+from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
+                   CameraSetCal3Bundler, PinholeCameraCal3_S2,
+                   PinholeCameraCal3Bundler, Point2, Point2Vector, Point3,
+                   Pose3, Pose3Vector, Rot3, TriangulationParameters,
+                   TriangulationResult)
 from gtsam.utils.test_case import GtsamTestCase
 
 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
@@ -218,6 +209,68 @@ class TestTriangulationExample(GtsamTestCase):
         # using the Huber loss we now have a quite small error!! nice!
         self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
 
+    def test_outliers_and_far_landmarks(self) -> None:
+        """Check safe triangulation function."""
+        pose1, pose2 = self.poses
+
+        K1 = Cal3_S2(1500, 1200, 0, 640, 480)
+        # create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+        camera1 = PinholeCameraCal3_S2(pose1, K1)
+
+        # create second camera 1 meter to the right of first camera
+        K2 = Cal3_S2(1600, 1300, 0, 650, 440)
+        camera2 = PinholeCameraCal3_S2(pose2, K2)
+
+        # 1. Project two landmarks into two cameras and triangulate
+        z1 = camera1.project(self.landmark)
+        z2 = camera2.project(self.landmark)
+
+        cameras = CameraSetCal3_S2()
+        measurements = Point2Vector()
+
+        cameras.push_back(camera1)
+        cameras.push_back(camera2)
+        measurements.push_back(z1)
+        measurements.push_back(z2)
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        # all default except landmarkDistanceThreshold: 
+        params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
+        actual: TriangulationResult = gtsam.triangulateSafe(
+            cameras, measurements, params)
+        self.gtsamAssertEquals(actual, self.landmark, 1e-2)
+        self.assertTrue(actual.valid())
+
+        landmarkDistanceThreshold = 4  # landmark is farther than that
+        params2 = TriangulationParameters(
+            1.0, False, landmarkDistanceThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params2)
+        self.assertTrue(actual.farPoint())
+
+        # 3. Add a slightly rotated third camera above with a wrong measurement
+        # (OUTLIER)
+        pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
+        K3 = Cal3_S2(700, 500, 0, 640, 480)
+        camera3 = PinholeCameraCal3_S2(pose3, K3)
+        z3 = camera3.project(self.landmark)
+
+        cameras.push_back(camera3)
+        measurements.push_back(z3 + Point2(10, -10))
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        outlierThreshold = 100   # loose, the outlier is going to pass
+        params3 = TriangulationParameters(1.0, False, landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params3)
+        self.assertTrue(actual.valid())
+
+        # now set stricter threshold for outlier rejection
+        outlierThreshold = 5  # tighter, the outlier is not going to pass
+        params4 = TriangulationParameters(1.0, False, landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params4)
+        self.assertTrue(actual.outlier())
+
 
 if __name__ == "__main__":
     unittest.main()

From 7bd0ae799de8d6ce52ff3edf346dfa5a9a3bd11a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 10:11:25 -0400
Subject: [PATCH 087/175] Fix pybind11 issue

---
 gtsam/geometry/geometry.i | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 1e9444b02..d2771edbd 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1040,11 +1040,7 @@ class Similarity3 {
   double scale() const;
 };
 
-template <T = {gtsam::PinholeCameraCal3_S2,
-               gtsam::PinholeCameraCal3DS2,
-               gtsam::PinholeCameraCal3Bundler,
-               gtsam::PinholeCameraCal3Fisheye,
-               gtsam::PinholeCameraCal3Unified}>
+template <T>
 class CameraSet {
   CameraSet();
 

From b78a4e64b19d10c989de67cc8b151d35d19d8a87 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 11 May 2022 10:11:57 -0400
Subject: [PATCH 088/175] Include all constructors, methods

---
 gtsam/geometry/geometry.i                | 10 ++++++++++
 python/gtsam/tests/test_Triangulation.py | 14 +++++++-------
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index d2771edbd..8e3c93224 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1091,7 +1091,17 @@ class StereoCamera {
 class TriangulationResult {
   enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
   Status status;
+  TriangulationResult(const gtsam::Point3& p);
   const gtsam::Point3& get() const;
+  static TriangulationResult Degenerate();
+  static TriangulationResult Outlier();
+  static TriangulationResult FarPoint();
+  static TriangulationResult BehindCamera();
+  bool valid() const;
+  bool degenerate() const;
+  bool outlier() const;
+  bool farPoint() const;
+  bool behindCamera() const;
 };
 
 class TriangulationParameters {
diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py
index c079e76b8..8630e1da7 100644
--- a/python/gtsam/tests/test_Triangulation.py
+++ b/python/gtsam/tests/test_Triangulation.py
@@ -228,17 +228,17 @@ class TestTriangulationExample(GtsamTestCase):
         cameras = CameraSetCal3_S2()
         measurements = Point2Vector()
 
-        cameras.push_back(camera1)
-        cameras.push_back(camera2)
-        measurements.push_back(z1)
-        measurements.push_back(z2)
+        cameras.append(camera1)
+        cameras.append(camera2)
+        measurements.append(z1)
+        measurements.append(z2)
 
         landmarkDistanceThreshold = 10  # landmark is closer than that
         # all default except landmarkDistanceThreshold: 
         params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
         actual: TriangulationResult = gtsam.triangulateSafe(
             cameras, measurements, params)
-        self.gtsamAssertEquals(actual, self.landmark, 1e-2)
+        self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
         self.assertTrue(actual.valid())
 
         landmarkDistanceThreshold = 4  # landmark is farther than that
@@ -254,8 +254,8 @@ class TestTriangulationExample(GtsamTestCase):
         camera3 = PinholeCameraCal3_S2(pose3, K3)
         z3 = camera3.project(self.landmark)
 
-        cameras.push_back(camera3)
-        measurements.push_back(z3 + Point2(10, -10))
+        cameras.append(camera3)
+        measurements.append(z3 + Point2(10, -10))
 
         landmarkDistanceThreshold = 10  # landmark is closer than that
         outlierThreshold = 100   # loose, the outlier is going to pass

From 1d67019f54004b87493cf0a7437300fd5e464ad6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 May 2022 12:06:11 -0400
Subject: [PATCH 089/175] remove user flag for pip install

---
 python/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 05606c584..c14f02dda 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -188,7 +188,7 @@ endif()
 # Add custom target so we can install with `make python-install`
 set(GTSAM_PYTHON_INSTALL_TARGET python-install)
 add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET}
-        COMMAND ${PYTHON_EXECUTABLE} -m pip install --user .
+        COMMAND ${PYTHON_EXECUTABLE} -m pip install .
         DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}
         WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY})
 

From dab30890032ae829fa384dff2ad86a0f819342ba Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 11 May 2022 12:06:33 -0400
Subject: [PATCH 090/175] include patch version for Python interpreter to help
 disambiguate

---
 cmake/HandlePython.cmake | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake
index 0c24824bc..16215986b 100644
--- a/cmake/HandlePython.cmake
+++ b/cmake/HandlePython.cmake
@@ -16,6 +16,7 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
 
       set(Python_VERSION_MAJOR ${PYTHON_VERSION_MAJOR})
       set(Python_VERSION_MINOR ${PYTHON_VERSION_MINOR})
+      set(Python_VERSION_PATCH ${PYTHON_VERSION_PATCH})
       set(Python_EXECUTABLE ${PYTHON_EXECUTABLE})
 
     else()
@@ -31,11 +32,12 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
 
       set(Python_VERSION_MAJOR ${Python3_VERSION_MAJOR})
       set(Python_VERSION_MINOR ${Python3_VERSION_MINOR})
+      set(Python_VERSION_PATCH ${Python3_VERSION_PATCH})
 
     endif()
 
     set(GTSAM_PYTHON_VERSION
-        "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}"
+        "${Python_VERSION_MAJOR}.${Python_VERSION_MINOR}.${Python_VERSION_PATCH}"
         CACHE STRING "The version of Python to build the wrappers against."
               FORCE)
 

From 78c7a6b72cb011475e0aadeaabfe2acf247d4481 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Fri, 13 May 2022 10:24:49 -0400
Subject: [PATCH 091/175] Change `optimize_using` to simpler function call

---
 python/gtsam/tests/test_logging_optimizer.py | 10 ++---
 python/gtsam/utils/logging_optimizer.py      | 47 +++++++++++---------
 2 files changed, 30 insertions(+), 27 deletions(-)

diff --git a/python/gtsam/tests/test_logging_optimizer.py b/python/gtsam/tests/test_logging_optimizer.py
index c58e4f121..602aeffc9 100644
--- a/python/gtsam/tests/test_logging_optimizer.py
+++ b/python/gtsam/tests/test_logging_optimizer.py
@@ -66,9 +66,9 @@ class TestOptimizeComet(GtsamTestCase):
 
         # Wrapper function sets the hook and calls optimizer.optimize() for us.
         params = gtsam.GaussNewtonParams()
-        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial)
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial)
         self.check(actual)
-        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook)(self.graph, self.initial, params)
+        actual = optimize_using(gtsam.GaussNewtonOptimizer, hook, self.graph, self.initial, params)
         self.check(actual)
         actual = gtsam_optimize(gtsam.GaussNewtonOptimizer(self.graph, self.initial, params),
                                 params, hook)
@@ -80,10 +80,10 @@ class TestOptimizeComet(GtsamTestCase):
             print(error)
 
         params = gtsam.LevenbergMarquardtParams()
-        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial)
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial)
         self.check(actual)
-        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook)(self.graph, self.initial,
-                                                                         params)
+        actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook, self.graph, self.initial,
+                                params)
         self.check(actual)
         actual = gtsam_optimize(gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial, params),
                                 params, hook)
diff --git a/python/gtsam/utils/logging_optimizer.py b/python/gtsam/utils/logging_optimizer.py
index f89208bc5..fe2f717d8 100644
--- a/python/gtsam/utils/logging_optimizer.py
+++ b/python/gtsam/utils/logging_optimizer.py
@@ -17,39 +17,42 @@ OPTIMIZER_PARAMS_MAP = {
 }
 
 
-def optimize_using(OptimizerClass, hook) -> Callable[[Any], gtsam.Values]:
+def optimize_using(OptimizerClass, hook, *args) -> gtsam.Values:
     """ Wraps the constructor and "optimize()" call for an Optimizer together and adds an iteration
         hook.
         Example usage:
-            solution = optimize_using(gtsam.GaussNewtonOptimizer, hook)(graph, init, params)
+            ```python
+            def hook(optimizer, error):
+                print("iteration {:}, error = {:}".format(optimizer.iterations(), error))
+            solution = optimize_using(gtsam.GaussNewtonOptimizer, hook, graph, init, params)
+            ```
+        Iteration hook's args are (optimizer, error) and return type should be None
 
     Args:
         OptimizerClass (T): A NonlinearOptimizer class (e.g. GaussNewtonOptimizer,
-            LevenbergMarquadrtOptimizer)
+            LevenbergMarquardtOptimizer)
         hook ([T, double] -> None): Function to callback after each iteration.  Args are (optimizer,
             error) and return should be None.
+        *args: Arguments that would be passed into the OptimizerClass constructor, usually:
+            graph, init, [params]
     Returns:
-        (Callable[*, gtsam.Values]): Call the returned function with the usual NonlinearOptimizer
-            arguments (will be forwarded to constructor) and it will return a Values object
-            representing the solution.  See example usage above.
+        (gtsam.Values): A Values object representing the optimization solution.
     """
-
-    def wrapped_optimize(*args):
-        for arg in args:
-            if isinstance(arg, gtsam.NonlinearOptimizerParams):
-                arg.iterationHook = lambda iteration, error_before, error_after: hook(
-                    optimizer, error_after)
-                break
-        else:
-            params = OPTIMIZER_PARAMS_MAP[OptimizerClass]()
-            params.iterationHook = lambda iteration, error_before, error_after: hook(
+    # Add the iteration hook to the NonlinearOptimizerParams
+    for arg in args:
+        if isinstance(arg, gtsam.NonlinearOptimizerParams):
+            arg.iterationHook = lambda iteration, error_before, error_after: hook(
                 optimizer, error_after)
-            args = (*args, params)
-        optimizer = OptimizerClass(*args)
-        hook(optimizer, optimizer.error())
-        return optimizer.optimize()
-
-    return wrapped_optimize
+            break
+    else:
+        params = OPTIMIZER_PARAMS_MAP[OptimizerClass]()
+        params.iterationHook = lambda iteration, error_before, error_after: hook(
+            optimizer, error_after)
+        args = (*args, params)
+    # Construct Optimizer and optimize
+    optimizer = OptimizerClass(*args)
+    hook(optimizer, optimizer.error())  # Call hook once with init values to match behavior below
+    return optimizer.optimize()
 
 
 def optimize(optimizer, check_convergence, hook):

From 30412b8110f4b5479f3a4665f8f1fdd6ec91b910 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 13 May 2022 14:33:04 -0400
Subject: [PATCH 092/175] Bump version

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index cfb251663..74433f333 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a6")
+set (GTSAM_PRERELEASE_VERSION "a7")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

From fa743ae0ac861897bd8d63f7e7815314ec48d3c4 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 14 May 2022 00:18:04 -0700
Subject: [PATCH 093/175] intialize values that may have between factors

---
 gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++++----
 gtsam/sfm/TranslationRecovery.h   |  6 ++++++
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 810fe7de9..8f1108806 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -134,6 +134,7 @@ void TranslationRecovery::addPrior(
 
 Values TranslationRecovery::initializeRandomly(
     const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     std::mt19937 *rng, const Values &initialValues) const {
   uniform_real_distribution<double> randomVal(-1, 1);
   // Create a lambda expression that checks whether value exists and randomly
@@ -155,14 +156,19 @@ Values TranslationRecovery::initializeRandomly(
     insert(edge.key1());
     insert(edge.key2());
   }
+  for (auto edge : betweenTranslations) {
+    insert(edge.key1());
+    insert(edge.key2());
+  }
   return initial;
 }
 
 Values TranslationRecovery::initializeRandomly(
     const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
     const Values &initialValues) const {
-  return initializeRandomly(relativeTranslations, &kRandomNumberGenerator,
-                            initialValues);
+  return initializeRandomly(relativeTranslations, betweenTranslations,
+                            &kRandomNumberGenerator, initialValues);
 }
 
 Values TranslationRecovery::run(
@@ -187,8 +193,8 @@ Values TranslationRecovery::run(
            &graph);
 
   // Uses initial values from params if provided.
-  Values initial =
-      initializeRandomly(nonzeroRelativeTranslations, initialValues);
+  Values initial = initializeRandomly(
+      nonzeroRelativeTranslations, nonzeroBetweenTranslations, initialValues);
 
   // If there are no valid edges, but zero-distance edges exist, initialize one
   // of the nodes in a connected component of zero-distance edges.
diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 7863f5133..6ccc39ddb 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -112,12 +112,15 @@ class TranslationRecovery {
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param rng random number generator
    * @param intialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
       std::mt19937 *rng, const Values &initialValues = Values()) const;
 
   /**
@@ -125,11 +128,14 @@ class TranslationRecovery {
    *
    * @param relativeTranslations unit translation directions between
    * translations to be estimated
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param initialValues (optional) initial values from a prior
    * @return Values
    */
   Values initializeRandomly(
       const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
       const Values &initialValues = Values()) const;
 
   /**

From 2c4990b613833a8fd38346a008965a60aac28dda Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 21 May 2022 14:23:05 -0700
Subject: [PATCH 094/175] Address Varun's comments

---
 gtsam/hybrid/GaussianMixture.cpp              | 11 +++++-----
 gtsam/hybrid/GaussianMixture.h                |  7 ++++---
 gtsam/hybrid/GaussianMixtureFactor.cpp        |  2 +-
 gtsam/hybrid/GaussianMixtureFactor.h          |  2 +-
 gtsam/hybrid/HybridFactor.cpp                 |  2 +-
 gtsam/hybrid/HybridFactorGraph.cpp            |  4 ++--
 gtsam/hybrid/HybridGaussianFactor.h           |  2 +-
 gtsam/hybrid/tests/Switching.h                | 20 ++++++++++++++++++-
 ...ditional.cpp => testHybridFactorGraph.cpp} |  2 +-
 9 files changed, 36 insertions(+), 16 deletions(-)
 rename gtsam/hybrid/tests/{testHybridConditional.cpp => testHybridFactorGraph.cpp} (99%)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index bc674966c..66971b69f 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -50,7 +50,7 @@ GaussianMixture GaussianMixture::FromConditionalList(
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum GaussianMixture::addTo(
+GaussianMixture::Sum GaussianMixture::add(
     const GaussianMixture::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
@@ -58,20 +58,21 @@ GaussianMixture::Sum GaussianMixture::addTo(
     result.push_back(graph2);
     return result;
   };
-  const Sum wrapped = wrappedConditionals();
+  const Sum wrapped = asGraph();
   return sum.empty() ? wrapped : sum.apply(wrapped, add);
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum GaussianMixture::wrappedConditionals() const {
-  auto wrap = [](const GaussianFactor::shared_ptr &factor) {
+GaussianMixture::Sum GaussianMixture::asGraph() const {
+  auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
     return result;
   };
-  return {conditionals_, wrap};
+  return {conditionals_, lambda};
 }
 
+/* TODO(fan): this (for Testable) is not implemented! */
 bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 4412e741c..4379ea1ca 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -13,6 +13,7 @@
  * @file   GaussianMixture.h
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
+ * @author Varun Agrawal
  * @date   Mar 12, 2022
  */
 
@@ -55,10 +56,10 @@ class GaussianMixture : public HybridFactor,
   const Conditionals &conditionals();
 
   /* *******************************************************************************/
-  Sum addTo(const Sum &sum) const;
+  Sum add(const Sum &sum) const;
 
   /* *******************************************************************************/
-  Sum wrappedConditionals() const;
+  Sum asGraph() const;
 
   static This FromConditionalList(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
@@ -71,4 +72,4 @@ class GaussianMixture : public HybridFactor,
       const std::string &s = "GaussianMixture\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 3963e675e..c85383322 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -62,7 +62,7 @@ const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
 }
 
 /* *******************************************************************************/
-GaussianMixtureFactor::Sum GaussianMixtureFactor::addTo(
+GaussianMixtureFactor::Sum GaussianMixtureFactor::add(
     const GaussianMixtureFactor::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 57a0cca03..1a3c582ae 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -56,7 +56,7 @@ class GaussianMixtureFactor : public HybridFactor {
       const std::vector<GaussianFactor::shared_ptr> &factors);
 
   /* *******************************************************************************/
-  Sum addTo(const Sum &sum) const;
+  Sum add(const Sum &sum) const;
 
   /* *******************************************************************************/
   Sum wrappedFactors() const;
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index a5ce8bd4e..a0c213a17 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -76,4 +76,4 @@ void HybridFactor::print(
 
 HybridFactor::~HybridFactor() = default;
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index ee6a5dd82..5ec562dc0 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -286,12 +286,12 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     if (f->isHybrid_) {
       auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
       if (cgmf) {
-        sum = cgmf->addTo(sum);
+        sum = cgmf->add(sum);
       }
 
       auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
       if (gm) {
-        sum = gm->asMixture()->addTo(sum);
+        sum = gm->asMixture()->add(sum);
       }
 
     } else if (f->isContinuous_) {
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 86c87a0ec..4a7939cd4 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -24,7 +24,7 @@
 namespace gtsam {
 
 /**
- * A HybridGaussianFactor is a wrapper for GaussianFactor so that we do not have
+ * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
  * a diamond inheritance.
  */
 class HybridGaussianFactor : public HybridFactor {
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index f29b8d9d5..74d682372 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -1,3 +1,21 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Switching.h
+ *  @date Mar 11, 2022
+ *  @author Varun Agrawal
+ *  @author Fan Jiang
+ */
+
 #include <gtsam/base/Matrix.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
@@ -65,4 +83,4 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
   return {new_order, levels};
 }
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/tests/testHybridConditional.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
similarity index 99%
rename from gtsam/hybrid/tests/testHybridConditional.cpp
rename to gtsam/hybrid/tests/testHybridFactorGraph.cpp
index 3d96ebd6a..d8ff617b0 100644
--- a/gtsam/hybrid/tests/testHybridConditional.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- *  @file testHybridConditional.cpp
+ *  @file testHybridFactorGraph.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
  */

From 04593ccb005d5ec0a0505fb511e33da929c8a29c Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sat, 21 May 2022 15:04:43 -0700
Subject: [PATCH 095/175] Fix compile error

---
 gtsam/hybrid/tests/testHybridFactorGraph.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index d8ff617b0..79c16d21a 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -72,7 +72,7 @@ void my_signal_handler(int signum) {
 #endif
 
 /* ************************************************************************* */
-TEST_DISABLED(HybridFactorGraph, creation) {
+TEST(HybridFactorGraph, creation) {
   HybridConditional test;
 
   HybridFactorGraph hfg;
@@ -89,7 +89,7 @@ TEST_DISABLED(HybridFactorGraph, creation) {
   GTSAM_PRINT(clgc);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminate) {
+TEST(HybridFactorGraph, eliminate) {
   HybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
@@ -99,7 +99,7 @@ TEST_DISABLED(HybridFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
+TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
@@ -107,7 +107,9 @@ TEST_DISABLED(HybridFactorGraph, eliminateMultifrontal) {
   hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
   hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
 
-  auto result = hfg.eliminatePartialMultifrontal({X(0)});
+  Ordering ordering;
+  ordering.push_back(X(0));
+  auto result = hfg.eliminatePartialMultifrontal(ordering);
 
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
@@ -136,7 +138,7 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
   EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) {
+TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -165,7 +167,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullSequentialSimple) {
   GTSAM_PRINT(*result);
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
+TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -199,7 +201,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   GTSAM_PRINT(*result->marginalFactor(C(2)));
 }
 
-TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) {
+TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -236,7 +238,7 @@ TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalCLG) {
  * This test is about how to assemble the Bayes Tree roots after we do partial
  * elimination
  */
-TEST_DISABLED(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
+TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   std::cout << ">>>>>>>>>>>>>>\n";
 
   HybridFactorGraph hfg;
@@ -507,7 +509,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
   }
 }
 
-TEST_DISABLED(HybridFactorGraph, SwitchingTwoVar) {
+TEST(HybridFactorGraph, SwitchingTwoVar) {
   const int N = 7;
   auto hfg = makeSwitchingChain(N, X);
   hfg->push_back(*makeSwitchingChain(N, Y, D));

From 2ae2cb6dc375a579d3c0754f45bf2c1060b22468 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 22 May 2022 14:16:13 -0700
Subject: [PATCH 096/175] Don't crash anymore

---
 python/gtsam/preamble/base.h             | 4 +++-
 python/gtsam/preamble/discrete.h         | 1 -
 python/gtsam/preamble/hybrid.h           | 2 +-
 python/gtsam/preamble/inference.h        | 2 --
 python/gtsam/specializations/inference.h | 1 +
 5 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h
index 626b47ae4..5cf633e65 100644
--- a/python/gtsam/preamble/base.h
+++ b/python/gtsam/preamble/base.h
@@ -11,6 +11,8 @@
  * mutations on Python side will not be reflected on C++.
  */
 
-PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
+PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector);
+
+PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSet);
 
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>);  // JacobianVector
diff --git a/python/gtsam/preamble/discrete.h b/python/gtsam/preamble/discrete.h
index 608508c32..320e0ac71 100644
--- a/python/gtsam/preamble/discrete.h
+++ b/python/gtsam/preamble/discrete.h
@@ -13,4 +13,3 @@
 
 #include <pybind11/stl.h>
 
-PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys);
diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h
index 56a07cfdd..5e5a71e48 100644
--- a/python/gtsam/preamble/hybrid.h
+++ b/python/gtsam/preamble/hybrid.h
@@ -11,4 +11,4 @@
  * mutations on Python side will not be reflected on C++.
  */
 
-#include <pybind11/stl.h>
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::GaussianFactor::shared_ptr>);
diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h
index 4106c794a..d07a75f6f 100644
--- a/python/gtsam/preamble/inference.h
+++ b/python/gtsam/preamble/inference.h
@@ -10,5 +10,3 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
-
-#include <pybind11/stl.h>
\ No newline at end of file
diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h
index 22fe3beff..9e23444ea 100644
--- a/python/gtsam/specializations/inference.h
+++ b/python/gtsam/specializations/inference.h
@@ -11,3 +11,4 @@
  * and saves one copy operation.
  */
 
+py::bind_map<std::map<char, double>>(m_, "__MapCharDouble");

From b8299d7ed62b4dc6c4be9a98c5e02d3bdefdcbfb Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 22 May 2022 17:11:27 -0700
Subject: [PATCH 097/175] Don't use Python dict method since it is not

---
 python/gtsam/tests/test_DiscreteBayesNet.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py
index 10c5db612..ff2ba99d1 100644
--- a/python/gtsam/tests/test_DiscreteBayesNet.py
+++ b/python/gtsam/tests/test_DiscreteBayesNet.py
@@ -139,7 +139,7 @@ class TestDiscreteBayesNet(GtsamTestCase):
         # Make sure we can *update* position hints
         writer = gtsam.DotWriter()
         ph: dict = writer.positionHints
-        ph.update({'a': 2})  # hint at symbol position
+        ph['a'] = 2 # hint at symbol position
         writer.positionHints = ph
 
         # Check the output of dot

From 74af969f68ce661fd296a24106878864fcedca57 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 22 May 2022 21:11:40 -0700
Subject: [PATCH 098/175] Trying to make MSVC happy

---
 gtsam/hybrid/HybridFactorGraph.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 5ec562dc0..f44ad898b 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -20,6 +20,7 @@
 
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/Assignment.h>
+#include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>

From b215d3a3779e31ba04cd80b085e580445d3585ae Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Sun, 22 May 2022 21:29:12 -0700
Subject: [PATCH 099/175] Address PR comments

---
 ...ure.cpp => GaussianMixtureConditional.cpp} | 27 ++++++++-------
 ...Mixture.h => GaussianMixtureConditional.h} | 34 +++++++++++++------
 gtsam/hybrid/GaussianMixtureFactor.cpp        |  4 +++
 gtsam/hybrid/GaussianMixtureFactor.h          |  2 --
 gtsam/hybrid/HybridBayesNet.h                 |  2 --
 gtsam/hybrid/HybridConditional.cpp            |  2 +-
 gtsam/hybrid/HybridConditional.h              | 10 +++---
 gtsam/hybrid/HybridFactorGraph.cpp            | 10 +++---
 gtsam/hybrid/hybrid.i                         |  8 ++---
 gtsam/hybrid/tests/testHybridFactorGraph.cpp  |  6 ++--
 10 files changed, 61 insertions(+), 44 deletions(-)
 rename gtsam/hybrid/{GaussianMixture.cpp => GaussianMixtureConditional.cpp} (72%)
 rename gtsam/hybrid/{GaussianMixture.h => GaussianMixtureConditional.h} (70%)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp
similarity index 72%
rename from gtsam/hybrid/GaussianMixture.cpp
rename to gtsam/hybrid/GaussianMixtureConditional.cpp
index 66971b69f..5fc3b4f83 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixtureConditional.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianMixture.cpp
+ * @file   GaussianMixtureConditional.cpp
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -20,38 +20,40 @@
 
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
-#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
-GaussianMixture::GaussianMixture(
+GaussianMixtureConditional::GaussianMixtureConditional(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
-    const GaussianMixture::Conditionals &conditionals)
+    const GaussianMixtureConditional::Conditionals &conditionals)
     : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
                  discreteParents),
       BaseConditional(continuousFrontals.size()),
       conditionals_(conditionals) {}
 
-const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
+/* *******************************************************************************/
+const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() {
   return conditionals_;
 }
 
-GaussianMixture GaussianMixture::FromConditionalList(
+/* *******************************************************************************/
+GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+  return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents,
                          dt);
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum GaussianMixture::add(
-    const GaussianMixture::Sum &sum) const {
+GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
+    const GaussianMixtureConditional::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
     auto result = graph1;
@@ -63,7 +65,7 @@ GaussianMixture::Sum GaussianMixture::add(
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum GaussianMixture::asGraph() const {
+GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -73,11 +75,12 @@ GaussianMixture::Sum GaussianMixture::asGraph() const {
 }
 
 /* TODO(fan): this (for Testable) is not implemented! */
-bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
+bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
-void GaussianMixture::print(const std::string &s,
+/* *******************************************************************************/
+void GaussianMixtureConditional::print(const std::string &s,
                             const KeyFormatter &formatter) const {
   std::cout << s << ": ";
   if (isContinuous_) std::cout << "Cont. ";
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixtureConditional.h
similarity index 70%
rename from gtsam/hybrid/GaussianMixture.h
rename to gtsam/hybrid/GaussianMixtureConditional.h
index 4379ea1ca..e0cf7c050 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixtureConditional.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianMixture.h
+ * @file   GaussianMixtureConditional.h
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -25,13 +25,13 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
-class GaussianMixture : public HybridFactor,
-                        public Conditional<HybridFactor, GaussianMixture> {
+class GaussianMixtureConditional : public HybridFactor,
+                        public Conditional<HybridFactor, GaussianMixtureConditional> {
  public:
-  using This = GaussianMixture;
-  using shared_ptr = boost::shared_ptr<GaussianMixture>;
+  using This = GaussianMixtureConditional;
+  using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
   using BaseFactor = HybridFactor;
-  using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
+  using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
 
   using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
 
@@ -46,7 +46,7 @@ class GaussianMixture : public HybridFactor,
    * @param discreteParents the discrete parents. Will be placed last.
    * @param conditionals a decision tree of GaussianConditionals.
    */
-  GaussianMixture(const KeyVector &continuousFrontals,
+  GaussianMixtureConditional(const KeyVector &continuousFrontals,
                   const KeyVector &continuousParents,
                   const DiscreteKeys &discreteParents,
                   const Conditionals &conditionals);
@@ -55,21 +55,35 @@ class GaussianMixture : public HybridFactor,
 
   const Conditionals &conditionals();
 
-  /* *******************************************************************************/
+  /**
+   * @brief Combine Decision Trees
+   */
   Sum add(const Sum &sum) const;
 
-  /* *******************************************************************************/
+	/**
+	 * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+	 */
   Sum asGraph() const;
 
+	/**
+	 * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
+	 * 
+	 * @param continuousFrontals The continuous frontal variables
+	 * @param continuousParents The continuous parent variables
+	 * @param discreteParents Discrete parents variables
+	 * @param conditionals List of conditionals
+	 */
   static This FromConditionalList(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
+	/* TODO: this is only a stub */
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
+	/* print utility */
   void print(
-      const std::string &s = "GaussianMixture\n",
+      const std::string &s = "GaussianMixtureConditional\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 };
 }  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index c85383322..65c5c7001 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -34,6 +34,7 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
 
+/* *******************************************************************************/
 GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
     const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
     const std::vector<GaussianFactor::shared_ptr> &factorsList) {
@@ -42,6 +43,8 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
   return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
 }
 
+
+/* *******************************************************************************/
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
@@ -57,6 +60,7 @@ void GaussianMixtureFactor::print(const std::string &s,
       });
 }
 
+/* *******************************************************************************/
 const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
   return factors_;
 }
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 1a3c582ae..f0f55911a 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -55,10 +55,8 @@ class GaussianMixtureFactor : public HybridFactor {
       const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
       const std::vector<GaussianFactor::shared_ptr> &factors);
 
-  /* *******************************************************************************/
   Sum add(const Sum &sum) const;
 
-  /* *******************************************************************************/
   Sum wrappedFactors() const;
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index d7e2f33af..4e411b781 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -20,8 +20,6 @@
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/inference/BayesNet.h>
 
-#include <iostream>  // TODO!
-
 namespace gtsam {
 
 /**
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 48bee192c..73e7747c6 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -50,7 +50,7 @@ HybridConditional::HybridConditional(
 }
 
 HybridConditional::HybridConditional(
-    boost::shared_ptr<GaussianMixture> gaussianMixture)
+    boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
     : BaseFactor(KeyVector(gaussianMixture->keys().begin(),
                            gaussianMixture->keys().begin() +
                                gaussianMixture->nrContinuous),
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 5d7ee2351..76d5b4833 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -18,7 +18,7 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
@@ -42,7 +42,7 @@ class HybridFactorGraph;
  * As a type-erased variant of:
  * - DiscreteConditional
  * - GaussianConditional
- * - GaussianMixture
+ * - GaussianMixtureConditional
  *
  * The reason why this is important is that `Conditional<T>` is a CRTP class.
  * CRTP is static polymorphism such that all CRTP classes, while bearing the
@@ -93,11 +93,11 @@ class GTSAM_EXPORT HybridConditional
 
   HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
 
-  HybridConditional(boost::shared_ptr<GaussianMixture> gaussianMixture);
+  HybridConditional(boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
 
-  GaussianMixture::shared_ptr asMixture() {
+  GaussianMixtureConditional::shared_ptr asMixture() {
     if (!isHybrid_) throw std::invalid_argument("Not a mixture");
-    return boost::static_pointer_cast<GaussianMixture>(inner);
+    return boost::static_pointer_cast<GaussianMixtureConditional>(inner);
   }
 
   DiscreteConditional::shared_ptr asDiscreteConditional() {
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index f44ad898b..699e6d2c6 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -22,7 +22,7 @@
 #include <gtsam/discrete/Assignment.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
@@ -104,7 +104,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // Because of all these reasons, we need to think very carefully about how to
   // implement the hybrid factors so that we do not get poor performance.
   //
-  // The first thing is how to represent the GaussianMixture. A very possible
+  // The first thing is how to represent the GaussianMixtureConditional. A very possible
   // scenario is that the incoming factors will have different levels of
   // discrete keys. For example, imagine we are going to eliminate the fragment:
   // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will
@@ -358,11 +358,11 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   auto pair = unzip(eliminationResults);
 
-  const GaussianMixture::Conditionals &conditionals = pair.first;
+  const GaussianMixtureConditional::Conditionals &conditionals = pair.first;
   const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
 
-  // Create the GaussianMixture from the conditionals
-  auto conditional = boost::make_shared<GaussianMixture>(
+  // Create the GaussianMixtureConditional from the conditionals
+  auto conditional = boost::make_shared<GaussianMixtureConditional>(
       frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
 
   if (DEBUG) {
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 052575011..5a76aaf48 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
                  gtsam::DefaultKeyFormatter) const;
 };
 
-#include <gtsam/hybrid/GaussianMixture.h>
-class GaussianMixture : gtsam::HybridFactor {
-  static GaussianMixture FromConditionalList(
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
+class GaussianMixtureConditional : gtsam::HybridFactor {
+  static GaussianMixtureConditional FromConditionalList(
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
       const std::vector<gtsam::GaussianConditional::shared_ptr>&
           conditionalsList);
 
-  void print(string s = "GaussianMixture\n",
+  void print(string s = "GaussianMixtureConditional\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
 };
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index 79c16d21a..4986cc2a7 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
-#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
@@ -79,8 +79,8 @@ TEST(HybridFactorGraph, creation) {
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  GaussianMixture clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-                       GaussianMixture::Conditionals(
+  GaussianMixtureConditional clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
+                       GaussianMixtureConditional::Conditionals(
                            C(0),
                            boost::make_shared<GaussianConditional>(
                                X(0), Z_3x1, I_3x3, X(1), I_3x3),

From e36583e6d530080fc6062331a6a13629f21bce79 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 23 May 2022 20:37:57 -0400
Subject: [PATCH 100/175] include missing headers for msvc and fix warning

---
 gtsam/hybrid/HybridConditional.cpp | 2 +-
 gtsam/hybrid/HybridFactorGraph.cpp | 9 ++++++---
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 73e7747c6..5b3c9d7b4 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -66,7 +66,7 @@ void HybridConditional::print(const std::string &s,
   if (isDiscrete_) std::cout << "Disc. ";
   if (isHybrid_) std::cout << "Hybr. ";
   std::cout << "P(";
-  int index = 0;
+  size_t index = 0;
   const size_t N = keys().size();
   const size_t contN = N - discreteKeys_.size();
   while (index < N) {
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 699e6d2c6..b72f50e8d 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -22,6 +22,7 @@
 #include <gtsam/discrete/Assignment.h>
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteJunctionTree.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -34,6 +35,7 @@
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
@@ -104,9 +106,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // Because of all these reasons, we need to think very carefully about how to
   // implement the hybrid factors so that we do not get poor performance.
   //
-  // The first thing is how to represent the GaussianMixtureConditional. A very possible
-  // scenario is that the incoming factors will have different levels of
-  // discrete keys. For example, imagine we are going to eliminate the fragment:
+  // The first thing is how to represent the GaussianMixtureConditional. A very
+  // possible scenario is that the incoming factors will have different levels
+  // of discrete keys. For example, imagine we are going to eliminate the
+  // fragment:
   // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will
   // need to know how to retrieve the corresponding continuous densities for the
   // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!).

From e325cd1c4bce9f58322d13d8952f639396b62a3d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 24 May 2022 00:14:20 -0400
Subject: [PATCH 101/175] include GaussianJunctionTree

---
 gtsam/hybrid/HybridFactorGraph.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index b72f50e8d..cb961b807 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -37,6 +37,7 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/JacobianFactor.h>
 

From eb074e7424c8747727e7af04ff535e7c50e1a46e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 24 May 2022 11:36:12 -0400
Subject: [PATCH 102/175] run formatting and rename wrappedFactors to
 asGaussianFactorGraphTree

---
 gtsam/hybrid/GaussianMixtureConditional.cpp | 23 +++++----
 gtsam/hybrid/GaussianMixtureConditional.h   | 43 +++++++++--------
 gtsam/hybrid/GaussianMixtureFactor.cpp      | 11 +++--
 gtsam/hybrid/GaussianMixtureFactor.h        | 11 ++++-
 gtsam/hybrid/HybridBayesTree.cpp            |  4 +-
 gtsam/hybrid/HybridBayesTree.h              |  4 +-
 gtsam/hybrid/HybridConditional.h            |  3 +-
 gtsam/hybrid/HybridDiscreteFactor.cpp       |  5 +-
 gtsam/hybrid/HybridDiscreteFactor.h         |  5 +-
 gtsam/hybrid/HybridEliminationTree.cpp      | 15 +++---
 gtsam/hybrid/HybridFactor.cpp               |  5 +-
 gtsam/hybrid/HybridFactorGraph.cpp          |  4 +-
 gtsam/hybrid/HybridJunctionTree.h           | 52 +++++++++++----------
 13 files changed, 103 insertions(+), 82 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp
index 5fc3b4f83..f0f3e8359 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.cpp
+++ b/gtsam/hybrid/GaussianMixtureConditional.cpp
@@ -36,7 +36,8 @@ GaussianMixtureConditional::GaussianMixtureConditional(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixtureConditional::Conditionals &GaussianMixtureConditional::conditionals() {
+const GaussianMixtureConditional::Conditionals &
+GaussianMixtureConditional::conditionals() {
   return conditionals_;
 }
 
@@ -47,8 +48,8 @@ GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList(
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixtureConditional(continuousFrontals, continuousParents, discreteParents,
-                         dt);
+  return GaussianMixtureConditional(continuousFrontals, continuousParents,
+                                    discreteParents, dt);
 }
 
 /* *******************************************************************************/
@@ -60,12 +61,13 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
     result.push_back(graph2);
     return result;
   };
-  const Sum wrapped = asGraph();
-  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+  const Sum tree = asGaussianFactorGraphTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
 }
 
 /* *******************************************************************************/
-GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const {
+GaussianMixtureConditional::Sum
+GaussianMixtureConditional::asGaussianFactorGraphTree() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -74,14 +76,15 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::asGraph() const {
   return {conditionals_, lambda};
 }
 
-/* TODO(fan): this (for Testable) is not implemented! */
-bool GaussianMixtureConditional::equals(const HybridFactor &lf, double tol) const {
-  return false;
+/* *******************************************************************************/
+bool GaussianMixtureConditional::equals(const HybridFactor &lf,
+                                        double tol) const {
+  return BaseFactor::equals(lf, tol);
 }
 
 /* *******************************************************************************/
 void GaussianMixtureConditional::print(const std::string &s,
-                            const KeyFormatter &formatter) const {
+                                       const KeyFormatter &formatter) const {
   std::cout << s << ": ";
   if (isContinuous_) std::cout << "Cont. ";
   if (isDiscrete_) std::cout << "Disc. ";
diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h
index e0cf7c050..3c74115f8 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.h
+++ b/gtsam/hybrid/GaussianMixtureConditional.h
@@ -25,8 +25,9 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
-class GaussianMixtureConditional : public HybridFactor,
-                        public Conditional<HybridFactor, GaussianMixtureConditional> {
+class GaussianMixtureConditional
+    : public HybridFactor,
+      public Conditional<HybridFactor, GaussianMixtureConditional> {
  public:
   using This = GaussianMixtureConditional;
   using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
@@ -47,9 +48,9 @@ class GaussianMixtureConditional : public HybridFactor,
    * @param conditionals a decision tree of GaussianConditionals.
    */
   GaussianMixtureConditional(const KeyVector &continuousFrontals,
-                  const KeyVector &continuousParents,
-                  const DiscreteKeys &discreteParents,
-                  const Conditionals &conditionals);
+                             const KeyVector &continuousParents,
+                             const DiscreteKeys &discreteParents,
+                             const Conditionals &conditionals);
 
   using Sum = DecisionTree<Key, GaussianFactorGraph>;
 
@@ -60,30 +61,32 @@ class GaussianMixtureConditional : public HybridFactor,
    */
   Sum add(const Sum &sum) const;
 
-	/**
-	 * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
-	 */
-  Sum asGraph() const;
-
-	/**
-	 * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
-	 * 
-	 * @param continuousFrontals The continuous frontal variables
-	 * @param continuousParents The continuous parent variables
-	 * @param discreteParents Discrete parents variables
-	 * @param conditionals List of conditionals
-	 */
+  /**
+   * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
+   *
+   * @param continuousFrontals The continuous frontal variables
+   * @param continuousParents The continuous parent variables
+   * @param discreteParents Discrete parents variables
+   * @param conditionals List of conditionals
+   */
   static This FromConditionalList(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
-	/* TODO: this is only a stub */
+  /// Test equality with base HybridFactor
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
-	/* print utility */
+  /* print utility */
   void print(
       const std::string &s = "GaussianMixtureConditional\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+ protected:
+  /**
+   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   */
+  Sum asGaussianFactorGraphTree() const;
 };
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 65c5c7001..3df274db3 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -26,10 +26,13 @@
 
 namespace gtsam {
 
+/* *******************************************************************************/
 GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
                                              const DiscreteKeys &discreteKeys,
                                              const Factors &factors)
     : Base(continuousKeys, discreteKeys), factors_(factors) {}
+
+/* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
 }
@@ -43,7 +46,6 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
   return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
 }
 
-
 /* *******************************************************************************/
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
@@ -74,12 +76,13 @@ GaussianMixtureFactor::Sum GaussianMixtureFactor::add(
     result.push_back(graph2);
     return result;
   };
-  const Sum wrapped = wrappedFactors();
-  return sum.empty() ? wrapped : sum.apply(wrapped, add);
+  const Sum tree = asGaussianFactorGraphTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
 }
 
 /* *******************************************************************************/
-GaussianMixtureFactor::Sum GaussianMixtureFactor::wrappedFactors() const {
+GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
+    const {
   auto wrap = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index f0f55911a..c6389c540 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -57,13 +57,20 @@ class GaussianMixtureFactor : public HybridFactor {
 
   Sum add(const Sum &sum) const;
 
-  Sum wrappedFactors() const;
-
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+ protected:
+  /**
+   * @brief Helper function to return factors and functional to create a
+   * DecisionTree of Gaussian Factor Graphs.
+   *
+   * @return Sum (DecisionTree<Key, GaussianFactorGraph)
+   */
+  Sum asGaussianFactorGraphTree() const;
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 85e6c84ae..426202861 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -17,10 +17,10 @@
  */
 
 #include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridBayesTree.h>
 #include <gtsam/inference/BayesTree-inst.h>
 #include <gtsam/inference/BayesTreeCliqueBase-inst.h>
-#include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridBayesNet.h>
 
 namespace gtsam {
 
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 626f2b10c..74bd234d8 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -73,8 +73,8 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
 /* This does special stuff for the hybrid case */
 template <class CLIQUE>
 class BayesTreeOrphanWrapper<
-    CLIQUE,
-    typename std::enable_if<boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
+    CLIQUE, typename std::enable_if<
+                boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
     : public CLIQUE::ConditionalType {
  public:
   typedef CLIQUE CliqueType;
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 76d5b4833..3bc25414e 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -93,7 +93,8 @@ class GTSAM_EXPORT HybridConditional
 
   HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
 
-  HybridConditional(boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
+  HybridConditional(
+      boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
 
   GaussianMixtureConditional::shared_ptr asMixture() {
     if (!isHybrid_) throw std::invalid_argument("Not a mixture");
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index be5659f04..54b193196 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -19,15 +19,16 @@
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 
 #include <boost/make_shared.hpp>
+
 #include "gtsam/discrete/DecisionTreeFactor.h"
 
 namespace gtsam {
 
 // TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right!
 HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
-    : Base(boost::dynamic_pointer_cast<DecisionTreeFactor>(other)->discreteKeys()) {
+    : Base(boost::dynamic_pointer_cast<DecisionTreeFactor>(other)
+               ->discreteKeys()) {
   inner = other;
-
 }
 
 HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 809510eac..0f731f8b5 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -24,8 +24,9 @@
 namespace gtsam {
 
 /**
- * A HybridDiscreteFactor is a wrapper for DiscreteFactor, so we hide the
- * implementation of DiscreteFactor, and thus avoiding diamond inheritance.
+ * A HybridDiscreteFactor is a thin container for DiscreteFactor, which allows
+ * us to hide the implementation of DiscreteFactor and thus avoid diamond
+ * inheritance.
  */
 class HybridDiscreteFactor : public HybridFactor {
  public:
diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp
index ff106095a..ecac96724 100644
--- a/gtsam/hybrid/HybridEliminationTree.cpp
+++ b/gtsam/hybrid/HybridEliminationTree.cpp
@@ -15,8 +15,8 @@
  * @author Fan Jiang
  */
 
-#include <gtsam/inference/EliminationTree-inst.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/inference/EliminationTree-inst.h>
 
 namespace gtsam {
 
@@ -26,18 +26,17 @@ template class EliminationTree<HybridBayesNet, HybridFactorGraph>;
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
     const HybridFactorGraph& factorGraph, const VariableIndex& structure,
-    const Ordering& order) :
-    Base(factorGraph, structure, order) {}
+    const Ordering& order)
+    : Base(factorGraph, structure, order) {}
 
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
-    const HybridFactorGraph& factorGraph, const Ordering& order) :
-    Base(factorGraph, order) {}
+    const HybridFactorGraph& factorGraph, const Ordering& order)
+    : Base(factorGraph, order) {}
 
 /* ************************************************************************* */
-bool HybridEliminationTree::equals(const This& other, double tol) const
-{
+bool HybridEliminationTree::equals(const This& other, double tol) const {
   return Base::equals(other, tol);
 }
 
-}
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index a0c213a17..1e9955c58 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -64,9 +64,8 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
       isDiscrete_(true),
       discreteKeys_(discreteKeys) {}
 
-void HybridFactor::print(
-      const std::string &s,
-      const KeyFormatter &formatter) const {
+void HybridFactor::print(const std::string &s,
+                         const KeyFormatter &formatter) const {
   std::cout << s;
   if (isContinuous_) std::cout << "Cont. ";
   if (isDiscrete_) std::cout << "Disc. ";
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index cb961b807..954ee57a6 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -229,8 +229,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
         if (p) {
           gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
         } else {
-          // It is an orphan wrapper
-          if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
+          // It is an orphan wrapped conditional
+          if (DEBUG) std::cout << "Got an orphan conditional\n";
         }
       }
     }
diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h
index 1901e7007..824fa4f85 100644
--- a/gtsam/hybrid/HybridJunctionTree.h
+++ b/gtsam/hybrid/HybridJunctionTree.h
@@ -17,8 +17,8 @@
 
 #pragma once
 
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/JunctionTree.h>
 
 namespace gtsam {
@@ -27,41 +27,45 @@ namespace gtsam {
 class HybridEliminationTree;
 
 /**
- * An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,
- * with the additional property that it represents the clique tree associated with a Bayes net.
+ * An EliminatableClusterTree, i.e., a set of variable clusters with factors,
+ * arranged in a tree, with the additional property that it represents the
+ * clique tree associated with a Bayes net.
  *
  * In GTSAM a junction tree is an intermediate data structure in multifrontal
  * variable elimination.  Each node is a cluster of factors, along with a
- * clique of variables that are eliminated all at once. In detail, every node k represents
- * a clique (maximal fully connected subset) of an associated chordal graph, such as a
- * chordal Bayes net resulting from elimination.
+ * clique of variables that are eliminated all at once. In detail, every node k
+ * represents a clique (maximal fully connected subset) of an associated chordal
+ * graph, such as a chordal Bayes net resulting from elimination.
  *
- * The difference with the BayesTree is that a JunctionTree stores factors, whereas a
- * BayesTree stores conditionals, that are the product of eliminating the factors in the
- * corresponding JunctionTree cliques.
+ * The difference with the BayesTree is that a JunctionTree stores factors,
+ * whereas a BayesTree stores conditionals, that are the product of eliminating
+ * the factors in the corresponding JunctionTree cliques.
  *
- * The tree structure and elimination method are exactly analogous to the EliminationTree,
- * except that in the JunctionTree, at each node multiple variables are eliminated at a time.
+ * The tree structure and elimination method are exactly analogous to the
+ * EliminationTree, except that in the JunctionTree, at each node multiple
+ * variables are eliminated at a time.
  *
  * \addtogroup Multifrontal
  * \nosubgrouping
  */
-class GTSAM_EXPORT HybridJunctionTree :
-    public JunctionTree<HybridBayesTree, HybridFactorGraph> {
+class GTSAM_EXPORT HybridJunctionTree
+    : public JunctionTree<HybridBayesTree, HybridFactorGraph> {
  public:
-  typedef JunctionTree<HybridBayesTree, HybridFactorGraph> Base; ///< Base class
-  typedef HybridJunctionTree This; ///< This class
-  typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
+  typedef JunctionTree<HybridBayesTree, HybridFactorGraph>
+      Base;                                    ///< Base class
+  typedef HybridJunctionTree This;             ///< This class
+  typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
   /**
-  * Build the elimination tree of a factor graph using precomputed column structure.
-  * @param factorGraph The factor graph for which to build the elimination tree
-  * @param structure The set of factors involving each variable.  If this is not
-  * precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
-  * named constructor instead.
-  * @return The elimination tree
-  */
+   * Build the elimination tree of a factor graph using precomputed column
+   * structure.
+   * @param factorGraph The factor graph for which to build the elimination tree
+   * @param structure The set of factors involving each variable.  If this is
+   * not precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
+   * named constructor instead.
+   * @return The elimination tree
+   */
   HybridJunctionTree(const HybridEliminationTree& eliminationTree);
 };
 
-}
+}  // namespace gtsam

From 3f239c28bef65e392a293bb9826878f4cf223186 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 24 May 2022 11:48:59 -0400
Subject: [PATCH 103/175] fix equality checks

---
 gtsam/hybrid/GaussianMixtureFactor.cpp | 2 +-
 gtsam/hybrid/HybridConditional.cpp     | 2 +-
 gtsam/hybrid/HybridFactor.cpp          | 4 ++++
 gtsam/hybrid/HybridFactor.h            | 2 +-
 4 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 3df274db3..edf94d040 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -34,7 +34,7 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
 
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
-  return false;
+  return Base::equals(lf, tol);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 5b3c9d7b4..ea83c5f86 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -89,7 +89,7 @@ void HybridConditional::print(const std::string &s,
 }
 
 bool HybridConditional::equals(const HybridFactor &other, double tol) const {
-  return false;
+  return BaseFactor::equals(other, tol);
 }
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 1e9955c58..7a233bb1b 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -64,6 +64,10 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
       isDiscrete_(true),
       discreteKeys_(discreteKeys) {}
 
+bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
+  return Base::equals(lf, tol);
+}
+
 void HybridFactor::print(const std::string &s,
                          const KeyFormatter &formatter) const {
   std::cout << s;
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 3d5bd7b21..c6e4a5ffa 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -85,7 +85,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// @{
 
   /// equals
-  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const = 0;
+  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;
 
   /// print
   void print(

From 10001f4cea1927d3f67f84770c41faa6d5e1b581 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 25 May 2022 17:52:49 -0400
Subject: [PATCH 104/175] use This for templated type in geometry.i

---
 gtsam/geometry/geometry.i | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 8e3c93224..8079e2c2a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -873,7 +873,7 @@ template <CALIBRATION>
 class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
-  PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
+  PinholeCamera(const This other);
   PinholeCamera(const gtsam::Pose3& pose);
   PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
   static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@@ -942,7 +942,7 @@ template <CALIBRATION>
 class PinholePose {
   // Standard Constructors and Named Constructors
   PinholePose();
-  PinholePose(const gtsam::PinholePose<CALIBRATION> other);
+  PinholePose(const This other);
   PinholePose(const gtsam::Pose3& pose);
   PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
   static This Level(const gtsam::Pose2& pose, double height);

From f95b449904d55b56f82f4befe962f8cd01eff485 Mon Sep 17 00:00:00 2001
From: "lilong.huang" <llhuang@weride.ai>
Date: Thu, 26 May 2022 09:21:29 +0800
Subject: [PATCH 105/175] fix typo issue in comment

---
 gtsam/geometry/Rot3.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 8f8088a74..08213b065 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -292,8 +292,8 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
     (*H)(1, 8) = yHb22 * cx;
 
     // Next, calculate the derivate of z. We have
-    // c20 = a10 * cy + a11 * sx * sy + a12 * cx * sy
-    // c22 = a11 * cx - a12 * sx
+    // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
+    // c11 = a11 * cx - a12 * sx
     const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
     const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
     Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);

From c3a92a4705642f8708b610d4a5f9e81aba1ddcb9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:12:19 -0400
Subject: [PATCH 106/175] Hybrid and Gaussian Mixture conditional docs and some
 refactor

---
 gtsam/hybrid/GaussianMixtureConditional.cpp | 10 +--
 gtsam/hybrid/GaussianMixtureConditional.h   | 56 ++++++++++++-----
 gtsam/hybrid/HybridConditional.cpp          | 24 ++++---
 gtsam/hybrid/HybridConditional.h            | 69 ++++++++++++++++-----
 4 files changed, 116 insertions(+), 43 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp
index f0f3e8359..68c3f505e 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.cpp
+++ b/gtsam/hybrid/GaussianMixtureConditional.cpp
@@ -42,7 +42,7 @@ GaussianMixtureConditional::conditionals() {
 }
 
 /* *******************************************************************************/
-GaussianMixtureConditional GaussianMixtureConditional::FromConditionalList(
+GaussianMixtureConditional GaussianMixtureConditional::FromConditionals(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
@@ -86,12 +86,12 @@ bool GaussianMixtureConditional::equals(const HybridFactor &lf,
 void GaussianMixtureConditional::print(const std::string &s,
                                        const KeyFormatter &formatter) const {
   std::cout << s << ": ";
-  if (isContinuous_) std::cout << "Cont. ";
-  if (isDiscrete_) std::cout << "Disc. ";
-  if (isHybrid_) std::cout << "Hybr. ";
+  if (isContinuous()) std::cout << "Cont. ";
+  if (isDiscrete()) std::cout << "Disc. ";
+  if (isHybrid()) std::cout << "Hybr. ";
   BaseConditional::print("", formatter);
   std::cout << "Discrete Keys = ";
-  for (auto &dk : discreteKeys_) {
+  for (auto &dk : discreteKeys()) {
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
   std::cout << "\n";
diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h
index 3c74115f8..d12fa09d7 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.h
+++ b/gtsam/hybrid/GaussianMixtureConditional.h
@@ -25,6 +25,14 @@
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
+
+/**
+ * @brief A conditional of gaussian mixtures indexed by discrete variables.
+ *
+ * Represents the conditional density P(X | M, Z) where X is a continuous random
+ * variable, M is the discrete variable and Z is the set of measurements.
+ *
+ */
 class GaussianMixtureConditional
     : public HybridFactor,
       public Conditional<HybridFactor, GaussianMixtureConditional> {
@@ -34,13 +42,28 @@ class GaussianMixtureConditional
   using BaseFactor = HybridFactor;
   using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
 
+  /// Alias for DecisionTree of GaussianFactorGraphs
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  /// typedef for Decision Tree of Gaussian Conditionals
   using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
 
+ private:
   Conditionals conditionals_;
 
- public:
   /**
-   * @brief Construct a new Gaussian Mixture object
+   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   */
+  Sum asGaussianFactorGraphTree() const;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Defaut constructor, mainly for serialization.
+  GaussianMixtureConditional() = default;
+  /**
+   * @brief Construct a new GaussianMixtureConditional object
    *
    * @param continuousFrontals the continuous frontals.
    * @param continuousParents the continuous parents.
@@ -52,15 +75,6 @@ class GaussianMixtureConditional
                              const DiscreteKeys &discreteParents,
                              const Conditionals &conditionals);
 
-  using Sum = DecisionTree<Key, GaussianFactorGraph>;
-
-  const Conditionals &conditionals();
-
-  /**
-   * @brief Combine Decision Trees
-   */
-  Sum add(const Sum &sum) const;
-
   /**
    * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
    *
@@ -69,11 +83,15 @@ class GaussianMixtureConditional
    * @param discreteParents Discrete parents variables
    * @param conditionals List of conditionals
    */
-  static This FromConditionalList(
+  static This FromConditionals(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
+  /// @}
+  /// @name Testable
+  /// @{
+
   /// Test equality with base HybridFactor
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
@@ -82,11 +100,19 @@ class GaussianMixtureConditional
       const std::string &s = "GaussianMixtureConditional\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
- protected:
+  /// @}
+
+  /// Getter for the underlying Conditionals DecisionTree
+  const Conditionals &conditionals();
+
   /**
-   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while
+   * maintaining the decision tree structure.
+   *
+   * @param sum Decision Tree of Gaussian Factor Graphs
+   * @return Sum
    */
-  Sum asGaussianFactorGraphTree() const;
+  Sum add(const Sum &sum) const;
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index ea83c5f86..e70d100c3 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -22,6 +22,7 @@
 
 namespace gtsam {
 
+/* ************************************************************************ */
 HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
                                      const DiscreteKeys &discreteFrontals,
                                      const KeyVector &continuousParents,
@@ -35,36 +36,40 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
               {discreteParents.begin(), discreteParents.end()}),
           continuousFrontals.size() + discreteFrontals.size()) {}
 
+/* ************************************************************************ */
 HybridConditional::HybridConditional(
     boost::shared_ptr<GaussianConditional> continuousConditional)
     : HybridConditional(continuousConditional->keys(), {},
                         continuousConditional->nrFrontals()) {
-  inner = continuousConditional;
+  inner_ = continuousConditional;
 }
 
+/* ************************************************************************ */
 HybridConditional::HybridConditional(
     boost::shared_ptr<DiscreteConditional> discreteConditional)
     : HybridConditional({}, discreteConditional->discreteKeys(),
                         discreteConditional->nrFrontals()) {
-  inner = discreteConditional;
+  inner_ = discreteConditional;
 }
 
+/* ************************************************************************ */
 HybridConditional::HybridConditional(
     boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
     : BaseFactor(KeyVector(gaussianMixture->keys().begin(),
                            gaussianMixture->keys().begin() +
-                               gaussianMixture->nrContinuous),
-                 gaussianMixture->discreteKeys_),
+                               gaussianMixture->nrContinuous()),
+                 gaussianMixture->discreteKeys()),
       BaseConditional(gaussianMixture->nrFrontals()) {
-  inner = gaussianMixture;
+  inner_ = gaussianMixture;
 }
 
+/* ************************************************************************ */
 void HybridConditional::print(const std::string &s,
                               const KeyFormatter &formatter) const {
   std::cout << s;
-  if (isContinuous_) std::cout << "Cont. ";
-  if (isDiscrete_) std::cout << "Disc. ";
-  if (isHybrid_) std::cout << "Hybr. ";
+  if (isContinuous()) std::cout << "Cont. ";
+  if (isDiscrete()) std::cout << "Disc. ";
+  if (isHybrid()) std::cout << "Hybr. ";
   std::cout << "P(";
   size_t index = 0;
   const size_t N = keys().size();
@@ -85,9 +90,10 @@ void HybridConditional::print(const std::string &s,
     index++;
   }
   std::cout << ")\n";
-  if (inner) inner->print("", formatter);
+  if (inner_) inner_->print("", formatter);
 }
 
+/* ************************************************************************ */
 bool HybridConditional::equals(const HybridFactor &other, double tol) const {
   return BaseFactor::equals(other, tol);
 }
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 3bc25414e..b942773cb 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -54,8 +54,8 @@ class HybridFactorGraph;
  * having diamond inheritances, and neutralized the need to change other
  * components of GTSAM to make hybrid elimination work.
  *
- * A great reference to the type-erasure pattern is Edurado Madrid's CppCon
- * talk.
+ * A great reference to the type-erasure pattern is Eduaado Madrid's CppCon
+ * talk (https://www.youtube.com/watch?v=s082Qmd_nHs).
  */
 class GTSAM_EXPORT HybridConditional
     : public HybridFactor,
@@ -70,7 +70,7 @@ class GTSAM_EXPORT HybridConditional
 
  protected:
   // Type-erased pointer to the inner type
-  boost::shared_ptr<Factor> inner;
+  boost::shared_ptr<Factor> inner_;
 
  public:
   /// @name Standard Constructors
@@ -79,35 +79,77 @@ class GTSAM_EXPORT HybridConditional
   /// Default constructor needed for serialization.
   HybridConditional() = default;
 
+  /**
+   * @brief Construct a new Hybrid Conditional object
+   *
+   * @param continuousKeys Vector of keys for continuous variables.
+   * @param discreteKeys Keys and cardinalities for discrete variables.
+   * @param nFrontals The number of frontal variables in the conditional.
+   */
   HybridConditional(const KeyVector& continuousKeys,
                     const DiscreteKeys& discreteKeys, size_t nFrontals)
       : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
 
+  /**
+   * @brief Construct a new Hybrid Conditional object
+   *
+   * @param continuousFrontals Vector of keys for continuous variables.
+   * @param discreteFrontals Keys and cardinalities for discrete variables.
+   * @param continuousParents Vector of keys for parent continuous variables.
+   * @param discreteParents Keys and cardinalities for parent discrete
+   * variables.
+   */
   HybridConditional(const KeyVector& continuousFrontals,
                     const DiscreteKeys& discreteFrontals,
                     const KeyVector& continuousParents,
                     const DiscreteKeys& discreteParents);
 
+  /**
+   * @brief Construct a new Hybrid Conditional object
+   *
+   * @param continuousConditional Conditional used to create the
+   * HybridConditional.
+   */
   HybridConditional(
       boost::shared_ptr<GaussianConditional> continuousConditional);
 
+  /**
+   * @brief Construct a new Hybrid Conditional object
+   *
+   * @param discreteConditional Conditional used to create the
+   * HybridConditional.
+   */
   HybridConditional(boost::shared_ptr<DiscreteConditional> discreteConditional);
 
+  /**
+   * @brief Construct a new Hybrid Conditional object
+   *
+   * @param gaussianMixture Gaussian Mixture Conditional used to create the
+   * HybridConditional.
+   */
   HybridConditional(
       boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
 
+  /**
+   * @brief Return HybridConditional as a GaussianMixtureConditional
+   *
+   * @return GaussianMixtureConditional::shared_ptr
+   */
   GaussianMixtureConditional::shared_ptr asMixture() {
-    if (!isHybrid_) throw std::invalid_argument("Not a mixture");
-    return boost::static_pointer_cast<GaussianMixtureConditional>(inner);
+    if (!isHybrid()) throw std::invalid_argument("Not a mixture");
+    return boost::static_pointer_cast<GaussianMixtureConditional>(inner_);
   }
 
+  /**
+   * @brief Return conditional as a DiscreteConditional
+   *
+   * @return DiscreteConditional::shared_ptr
+   */
   DiscreteConditional::shared_ptr asDiscreteConditional() {
-    if (!isDiscrete_) throw std::invalid_argument("Not a discrete conditional");
-    return boost::static_pointer_cast<DiscreteConditional>(inner);
+    if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional");
+    return boost::static_pointer_cast<DiscreteConditional>(inner_);
   }
 
-  boost::shared_ptr<Factor> getInner() { return inner; }
-
   /// @}
   /// @name Testable
   /// @{
@@ -122,11 +164,10 @@ class GTSAM_EXPORT HybridConditional
 
   /// @}
 
-  friend std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-  EliminateHybrid(const HybridFactorGraph& factors,
-                  const Ordering& frontalKeys);
-};
-// DiscreteConditional
+  /// Get the type-erased pointer to the inner type
+  boost::shared_ptr<Factor> inner() { return inner_; }
+
+};  // DiscreteConditional
 
 // traits
 template <>

From b3cab1bd4ec44e416bfb2e58a900117fb6150ba8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:13:39 -0400
Subject: [PATCH 107/175] GaussianMixtureFactor docs

---
 gtsam/hybrid/GaussianMixtureFactor.cpp |  6 +--
 gtsam/hybrid/GaussianMixtureFactor.h   | 64 ++++++++++++++++++++------
 2 files changed, 54 insertions(+), 16 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index edf94d040..589e5c660 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -38,10 +38,10 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
 }
 
 /* *******************************************************************************/
-GaussianMixtureFactor GaussianMixtureFactor::FromFactorList(
+GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
     const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
-    const std::vector<GaussianFactor::shared_ptr> &factorsList) {
-  Factors dt(discreteKeys, factorsList);
+    const std::vector<GaussianFactor::shared_ptr> &factors) {
+  Factors dt(discreteKeys, factors);
 
   return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
 }
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index c6389c540..b2fbe4aef 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -11,7 +11,7 @@
 
 /**
  * @file   GaussianMixtureFactor.h
- * @brief  A set of Gaussian factors indexed by a set of discrete keys.
+ * @brief  A factor that is a function of discrete and continuous variables.
  * @author Fan Jiang
  * @author Varun Agrawal
  * @author Frank Dellaert
@@ -29,48 +29,86 @@ namespace gtsam {
 
 class GaussianFactorGraph;
 
-typedef std::vector<gtsam::GaussianFactor::shared_ptr> GaussianFactorVector;
+using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
 
+/**
+ * @brief A linear factor that is a function of both discrete and continuous
+ * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is
+ * the set of discrete variables and Z is the measurement set.
+ *
+ * Represents the underlying Gaussian Mixture as a Decision Tree, where the set
+ * of discrete variables indexes to the continuous gaussian distribution.
+ *
+ */
 class GaussianMixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
   using This = GaussianMixtureFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
+  using Sum = DecisionTree<Key, GaussianFactorGraph>;
+
+  /// typedef for Decision Tree of Gaussian Factors
   using Factors = DecisionTree<Key, GaussianFactor::shared_ptr>;
 
+ private:
   Factors factors_;
 
+  /**
+   * @brief Helper function to return factors and functional to create a
+   * DecisionTree of Gaussian Factor Graphs.
+   *
+   * @return Sum (DecisionTree<Key, GaussianFactorGraph>)
+   */
+  Sum asGaussianFactorGraphTree() const;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor, mainly for serialization.
   GaussianMixtureFactor() = default;
 
+  /**
+   * @brief Construct a new Gaussian Mixture Factor object.
+   *
+   * @param continuousKeys A vector of keys representing continuous variables.
+   * @param discreteKeys A vector of keys representing discrete variables and
+   * their cardinalities.
+   * @param factors The decision tree of Gaussian Factors stored as the mixture
+   * density.
+   */
   GaussianMixtureFactor(const KeyVector &continuousKeys,
                         const DiscreteKeys &discreteKeys,
                         const Factors &factors);
 
-  using Sum = DecisionTree<Key, GaussianFactorGraph>;
-
-  const Factors &factors();
-
-  static This FromFactorList(
+  static This FromFactors(
       const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
       const std::vector<GaussianFactor::shared_ptr> &factors);
 
-  Sum add(const Sum &sum) const;
+  /// @}
+  /// @name Testable
+  /// @{
 
   bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
 
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+  /// @}
+
+  /// Getter for the underlying Gaussian Factor Decision Tree.
+  const Factors &factors();
 
- protected:
   /**
-   * @brief Helper function to return factors and functional to create a
-   * DecisionTree of Gaussian Factor Graphs.
+   * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while
+   * maintaining the original tree structure.
    *
-   * @return Sum (DecisionTree<Key, GaussianFactorGraph)
+   * @param sum Decision Tree of Gaussian Factor Graphs indexed by the
+   * variables.
+   * @return Sum
    */
-  Sum asGaussianFactorGraphTree() const;
+  Sum add(const Sum &sum) const;
 };
 
 }  // namespace gtsam

From 865d10da9cca0d87ae0c47e12af2dd6e5b20e2d2 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:15:34 -0400
Subject: [PATCH 108/175] Hybrid factor docs and minor refactor

---
 gtsam/hybrid/HybridFactor.cpp         | 16 ++++++----
 gtsam/hybrid/HybridFactor.h           | 45 ++++++++++++++++-----------
 gtsam/hybrid/HybridGaussianFactor.cpp |  6 ++--
 gtsam/hybrid/HybridGaussianFactor.h   |  7 +++--
 4 files changed, 45 insertions(+), 29 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 7a233bb1b..9358c473d 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -19,6 +19,7 @@
 
 namespace gtsam {
 
+/* ************************************************************************ */
 KeyVector CollectKeys(const KeyVector &continuousKeys,
                       const DiscreteKeys &discreteKeys) {
   KeyVector allKeys;
@@ -30,6 +31,7 @@ KeyVector CollectKeys(const KeyVector &continuousKeys,
   return allKeys;
 }
 
+/* ************************************************************************ */
 KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
   KeyVector allKeys;
   std::copy(keys1.begin(), keys1.end(), std::back_inserter(allKeys));
@@ -37,6 +39,7 @@ KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2) {
   return allKeys;
 }
 
+/* ************************************************************************ */
 DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
                                  const DiscreteKeys &key2) {
   DiscreteKeys allKeys;
@@ -45,29 +48,32 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
   return allKeys;
 }
 
-HybridFactor::HybridFactor() = default;
-
+/* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true), nrContinuous(keys.size()) {}
+    : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {}
 
+/* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
                            const DiscreteKeys &discreteKeys)
     : Base(CollectKeys(continuousKeys, discreteKeys)),
       isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)),
       isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
       isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
-      nrContinuous(continuousKeys.size()),
+      nrContinuous_(continuousKeys.size()),
       discreteKeys_(discreteKeys) {}
 
+/* ************************************************************************ */
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
     : Base(CollectKeys({}, discreteKeys)),
       isDiscrete_(true),
       discreteKeys_(discreteKeys) {}
 
+/* ************************************************************************ */
 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
   return Base::equals(lf, tol);
 }
 
+/* ************************************************************************ */
 void HybridFactor::print(const std::string &s,
                          const KeyFormatter &formatter) const {
   std::cout << s;
@@ -77,6 +83,4 @@ void HybridFactor::print(const std::string &s,
   this->printKeys("", formatter);
 }
 
-HybridFactor::~HybridFactor() = default;
-
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index c6e4a5ffa..da103da43 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -41,6 +41,16 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
  *  - GaussianMixture
  */
 class GTSAM_EXPORT HybridFactor : public Factor {
+ private:
+  bool isDiscrete_ = false;
+  bool isContinuous_ = false;
+  bool isHybrid_ = false;
+
+  size_t nrContinuous_ = 0;
+
+ protected:
+  DiscreteKeys discreteKeys_;
+
  public:
   // typedefs needed to play nice with gtsam
   typedef HybridFactor This;  ///< This class
@@ -48,27 +58,11 @@ class GTSAM_EXPORT HybridFactor : public Factor {
       shared_ptr;       ///< shared_ptr to this class
   typedef Factor Base;  ///< Our base class
 
-  bool isDiscrete_ = false;
-  bool isContinuous_ = false;
-  bool isHybrid_ = false;
-
-  size_t nrContinuous = 0;
-
-  DiscreteKeys discreteKeys_;
-
- public:
   /// @name Standard Constructors
   /// @{
 
   /** Default constructor creates empty factor */
-  HybridFactor();
-
-  /** Construct from container of keys.  This constructor is used internally
-   * from derived factor
-   *  constructors, either from a container of keys or from a
-   * boost::assign::list_of. */
-  //  template<typename CONTAINER>
-  //  HybridFactor(const CONTAINER &keys) : Base(keys) {}
+  HybridFactor() = default;
 
   explicit HybridFactor(const KeyVector &keys);
 
@@ -78,7 +72,7 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   explicit HybridFactor(const DiscreteKeys &discreteKeys);
 
   /// Virtual destructor
-  virtual ~HybridFactor();
+  virtual ~HybridFactor() = default;
 
   /// @}
   /// @name Testable
@@ -96,6 +90,21 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /// @name Standard Interface
   /// @{
 
+  /// True if this is a factor of discrete variables only.
+  bool isDiscrete() const { return isDiscrete_; }
+
+  /// True if this is a factor of continuous variables only.
+  bool isContinuous() const { return isContinuous_; }
+
+  /// True is this is a Discrete-Continuous factor.
+  bool isHybrid() const { return isHybrid_; }
+
+  /// Return the number of continuous variables in this factor.
+  size_t nrContinuous() const { return nrContinuous_; }
+
+  /// Return vector of discrete keys.
+  DiscreteKeys discreteKeys() const { return discreteKeys_; }
+
   /// @}
 };
 // HybridFactor
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 5fa4b555a..721cb4cc7 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -23,12 +23,12 @@ namespace gtsam {
 
 HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
     : Base(other->keys()) {
-  inner = other;
+  inner_ = other;
 }
 
 HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
     : Base(jf.keys()),
-      inner(boost::make_shared<JacobianFactor>(std::move(jf))) {}
+      inner_(boost::make_shared<JacobianFactor>(std::move(jf))) {}
 
 bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
   return false;
@@ -36,7 +36,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
 void HybridGaussianFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner->print("inner: ", formatter);
+  inner_->print("inner: ", formatter);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 4a7939cd4..1749c8e41 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -28,13 +28,14 @@ namespace gtsam {
  * a diamond inheritance.
  */
 class HybridGaussianFactor : public HybridFactor {
+ private:
+  GaussianFactor::shared_ptr inner_;
+
  public:
   using Base = HybridFactor;
   using This = HybridGaussianFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
-  GaussianFactor::shared_ptr inner;
-
   // Explicit conversion from a shared ptr of GF
   explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
 
@@ -47,5 +48,7 @@ class HybridGaussianFactor : public HybridFactor {
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  GaussianFactor::shared_ptr inner() const { return inner_; }
 };
 }  // namespace gtsam

From 573448f126cfe8ebe7fb2b3f149d1a7abe2b08e9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:16:28 -0400
Subject: [PATCH 109/175] make functional

---
 gtsam/hybrid/HybridBayesNet.h        |  2 +-
 gtsam/hybrid/HybridBayesTree.cpp     |  1 -
 gtsam/hybrid/HybridBayesTree.h       | 18 +++++++++++++++---
 gtsam/hybrid/HybridEliminationTree.h | 11 ++++++++---
 gtsam/hybrid/HybridFactorGraph.cpp   | 18 +++++++++---------
 gtsam/hybrid/HybridISAM.cpp          |  2 +-
 gtsam/hybrid/HybridJunctionTree.cpp  |  2 +-
 7 files changed, 35 insertions(+), 19 deletions(-)

diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h
index 4e411b781..43eead280 100644
--- a/gtsam/hybrid/HybridBayesNet.h
+++ b/gtsam/hybrid/HybridBayesNet.h
@@ -35,7 +35,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
   using sharedConditional = boost::shared_ptr<ConditionalType>;
 
   /** Construct empty bayes net */
-  HybridBayesNet() : Base() {}
+  HybridBayesNet() = default;
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 426202861..c8d70e67e 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -33,5 +33,4 @@ bool HybridBayesTree::equals(const This& other, double tol) const {
   return Base::equals(other, tol);
 }
 
-/* **************************************************************************/
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 74bd234d8..f8a90d6b4 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -70,7 +70,13 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
   /// @}
 };
 
-/* This does special stuff for the hybrid case */
+/**
+ * @brief Class for Hybrid Bayes tree orphan subtrees.
+ *
+ * This does special stuff for the hybrid case
+ *
+ * @tparam CLIQUE
+ */
 template <class CLIQUE>
 class BayesTreeOrphanWrapper<
     CLIQUE, typename std::enable_if<
@@ -82,16 +88,22 @@ class BayesTreeOrphanWrapper<
 
   boost::shared_ptr<CliqueType> clique;
 
+  /**
+   * @brief Construct a new Bayes Tree Orphan Wrapper object.
+   *
+   * @param clique Bayes tree clique.
+   */
   BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
       : clique(clique) {
     // Store parent keys in our base type factor so that eliminating those
     // parent keys will pull this subtree into the elimination.
     this->keys_.assign(clique->conditional()->beginParents(),
                        clique->conditional()->endParents());
-    this->discreteKeys_.assign(clique->conditional()->discreteKeys_.begin(),
-                               clique->conditional()->discreteKeys_.end());
+    this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(),
+                               clique->conditional()->discreteKeys().end());
   }
 
+  /// print utility
   void print(
       const std::string& s = "",
       const KeyFormatter& formatter = DefaultKeyFormatter) const override {
diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
index 902beb279..27766724a 100644
--- a/gtsam/hybrid/HybridEliminationTree.h
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -28,12 +28,18 @@ namespace gtsam {
  */
 class GTSAM_EXPORT HybridEliminationTree
     : public EliminationTree<HybridBayesNet, HybridFactorGraph> {
+ private:
+  friend class ::EliminationTreeTester;
+
  public:
   typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
       Base;                                    ///< Base class
   typedef HybridEliminationTree This;          ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
+  /// @name Constructors
+  /// @{
+
   /**
    * Build the elimination tree of a factor graph using pre-computed column
    * structure.
@@ -54,11 +60,10 @@ class GTSAM_EXPORT HybridEliminationTree
   HybridEliminationTree(const HybridFactorGraph& factorGraph,
                         const Ordering& order);
 
+  /// @}
+
   /** Test whether the tree is equal to another */
   bool equals(const This& other, double tol = 1e-9) const;
-
- private:
-  friend class ::EliminationTreeTester;
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 954ee57a6..450636ab3 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -150,8 +150,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
       std::cout << RESET;
     }
     separatorKeys.insert(factor->begin(), factor->end());
-    if (!factor->isContinuous_) {
-      for (auto &k : factor->discreteKeys_) {
+    if (!factor->isContinuous()) {
+      for (auto &k : factor->discreteKeys()) {
         mapFromKeyToDiscreteKey[k.first] = k;
       }
     }
@@ -223,9 +223,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
       if (ptr) {
-        gfg.push_back(ptr->inner);
+        gfg.push_back(ptr->inner());
       } else {
-        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner;
+        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
         if (p) {
           gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
         } else {
@@ -251,9 +251,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
       if (ptr) {
-        dfg.push_back(ptr->inner);
+        dfg.push_back(ptr->inner());
       } else {
-        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner;
+        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
         if (p) {
           dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
         } else {
@@ -288,7 +288,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   std::vector<GaussianFactor::shared_ptr> deferredFactors;
 
   for (auto &f : factors) {
-    if (f->isHybrid_) {
+    if (f->isHybrid()) {
       auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
       if (cgmf) {
         sum = cgmf->add(sum);
@@ -299,9 +299,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
         sum = gm->asMixture()->add(sum);
       }
 
-    } else if (f->isContinuous_) {
+    } else if (f->isContinuous()) {
       deferredFactors.push_back(
-          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner);
+          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
     } else {
       // We need to handle the case where the object is actually an
       // BayesTreeOrphanWrapper!
diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp
index 0db30f1f3..f743b92f1 100644
--- a/gtsam/hybrid/HybridISAM.cpp
+++ b/gtsam/hybrid/HybridISAM.cpp
@@ -58,7 +58,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
 
   KeySet allDiscrete;
   for (auto& factor : factors) {
-    for (auto& k : factor->discreteKeys_) {
+    for (auto& k : factor->discreteKeys()) {
       allDiscrete.insert(k.first);
     }
   }
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index bf088c5aa..d1e19f852 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -63,7 +63,7 @@ struct HybridConstructorTraversalData {
     std::cout << "Getting discrete info: ";
 #endif
     for (HybridFactor::shared_ptr& f : node->factors) {
-      for (auto& k : f->discreteKeys_) {
+      for (auto& k : f->discreteKeys()) {
 #ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
         std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
 #endif

From 6d26818e79c12a022cd9d18d56e88fe1ceeb7bad Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:17:22 -0400
Subject: [PATCH 110/175] HybridDiscreteFactor docs and minor refactor

---
 gtsam/hybrid/HybridDiscreteFactor.cpp | 17 ++++++++++-------
 gtsam/hybrid/HybridDiscreteFactor.h   | 16 ++++++++++++++--
 2 files changed, 24 insertions(+), 9 deletions(-)

diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 54b193196..989127a28 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -24,25 +24,28 @@
 
 namespace gtsam {
 
+/* ************************************************************************ */
 // TODO(fan): THIS IS VERY VERY DIRTY! We need to get DiscreteFactor right!
 HybridDiscreteFactor::HybridDiscreteFactor(DiscreteFactor::shared_ptr other)
     : Base(boost::dynamic_pointer_cast<DecisionTreeFactor>(other)
-               ->discreteKeys()) {
-  inner = other;
-}
+               ->discreteKeys()),
+      inner_(other) {}
 
+/* ************************************************************************ */
 HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
     : Base(dtf.discreteKeys()),
-      inner(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
+      inner_(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
 
+/* ************************************************************************ */
 bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
-  return false;
+  return Base::equals(lf, tol);
 }
 
+/* ************************************************************************ */
 void HybridDiscreteFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner->print("inner: ", formatter);
+  inner_->print("inner: ", formatter);
 };
 
-}  // namespace gtsam
\ No newline at end of file
+}  // namespace gtsam
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 0f731f8b5..572ddfbcd 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -13,6 +13,7 @@
  *  @file HybridDiscreteFactor.h
  *  @date Mar 11, 2022
  *  @author Fan Jiang
+ *  @author Varun Agrawal
  */
 
 #pragma once
@@ -29,12 +30,16 @@ namespace gtsam {
  * inheritance.
  */
 class HybridDiscreteFactor : public HybridFactor {
+ private:
+  DiscreteFactor::shared_ptr inner_;
+
  public:
   using Base = HybridFactor;
   using This = HybridDiscreteFactor;
   using shared_ptr = boost::shared_ptr<This>;
 
-  DiscreteFactor::shared_ptr inner;
+  /// @name Constructors
+  /// @{
 
   // Implicit conversion from a shared ptr of DF
   HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
@@ -42,11 +47,18 @@ class HybridDiscreteFactor : public HybridFactor {
   // Forwarding constructor from concrete DecisionTreeFactor
   HybridDiscreteFactor(DecisionTreeFactor &&dtf);
 
- public:
+  /// @}
+  /// @name Testable
+  /// @{
   virtual bool equals(const HybridFactor &lf, double tol) const override;
 
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  /// @}
+
+  /// Return pointer to the internal discrete factor
+  DiscreteFactor::shared_ptr inner() const { return inner_; }
 };
 }  // namespace gtsam

From 7bfa0118862b4ee1b745597e169e4d99dcaa6321 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:17:30 -0400
Subject: [PATCH 111/175] update tests

---
 gtsam/hybrid/tests/Switching.h               |  2 +-
 gtsam/hybrid/tests/testHybridFactorGraph.cpp | 21 ++++++++++----------
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 74d682372..1a4db7898 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -38,7 +38,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain(
 
   // keyFunc(1) to keyFunc(n+1)
   for (size_t t = 1; t < n; t++) {
-    hfg.add(GaussianMixtureFactor::FromFactorList(
+    hfg.add(GaussianMixtureFactor::FromFactors(
         {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}},
         {boost::make_shared<JacobianFactor>(keyFunc(t), I_3x3, keyFunc(t + 1),
                                             I_3x3, Z_3x1),
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index 4986cc2a7..55b2a9b3b 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -79,13 +79,14 @@ TEST(HybridFactorGraph, creation) {
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  GaussianMixtureConditional clgc({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-                       GaussianMixtureConditional::Conditionals(
-                           C(0),
-                           boost::make_shared<GaussianConditional>(
-                               X(0), Z_3x1, I_3x3, X(1), I_3x3),
-                           boost::make_shared<GaussianConditional>(
-                               X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
+  GaussianMixtureConditional clgc(
+      {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
+      GaussianMixtureConditional::Conditionals(
+          C(0),
+          boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
+                                                  I_3x3),
+          boost::make_shared<GaussianConditional>(X(0), Vector3::Ones(), I_3x3,
+                                                  X(1), I_3x3)));
   GTSAM_PRINT(clgc);
 }
 
@@ -182,7 +183,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   //     boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones()));
 
   // hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt));
-  hfg.add(GaussianMixtureFactor::FromFactorList(
+  hfg.add(GaussianMixtureFactor::FromFactors(
       {X(1)}, {{C(1), 2}},
       {boost::make_shared<JacobianFactor>(X(1), I_3x3, Z_3x1),
        boost::make_shared<JacobianFactor>(X(1), I_3x3, Vector3::Ones())}));
@@ -234,7 +235,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   */
 }
 
-/**
+/*
  * This test is about how to assemble the Bayes Tree roots after we do partial
  * elimination
  */
@@ -251,7 +252,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
     //     C(0), boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
     //     boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones()));
 
-    hfg.add(GaussianMixtureFactor::FromFactorList(
+    hfg.add(GaussianMixtureFactor::FromFactors(
         {X(0)}, {{C(0), 2}},
         {boost::make_shared<JacobianFactor>(X(0), I_3x3, Z_3x1),
          boost::make_shared<JacobianFactor>(X(0), I_3x3, Vector3::Ones())}));

From d60c4aca5a30312e935319af335efd1f675c3c80 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 15:28:11 -0400
Subject: [PATCH 112/175] add demarkers

---
 gtsam/hybrid/hybrid.i                        |  6 +++---
 gtsam/hybrid/tests/testHybridFactorGraph.cpp | 22 +++++++++-----------
 2 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 5a76aaf48..d3ff98719 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -23,12 +23,12 @@ virtual class HybridConditional {
   bool equals(const gtsam::HybridConditional& other, double tol = 1e-9) const;
   size_t nrFrontals() const;
   size_t nrParents() const;
-  Factor* getInner();
+  Factor* inner();
 };
 
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 class GaussianMixtureFactor : gtsam::HybridFactor {
-  static GaussianMixtureFactor FromFactorList(
+  static GaussianMixtureFactor FromFactors(
       const gtsam::KeyVector& continuousKeys,
       const gtsam::DiscreteKeys& discreteKeys,
       const std::vector<gtsam::GaussianFactor::shared_ptr>& factorsList);
@@ -40,7 +40,7 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
 
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 class GaussianMixtureConditional : gtsam::HybridFactor {
-  static GaussianMixtureConditional FromConditionalList(
+  static GaussianMixtureConditional FromConditionals(
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
index 55b2a9b3b..860fdfdb8 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testHybridFactorGraph.cpp
@@ -90,6 +90,7 @@ TEST(HybridFactorGraph, creation) {
   GTSAM_PRINT(clgc);
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminate) {
   HybridFactorGraph hfg;
 
@@ -100,6 +101,7 @@ TEST(HybridFactorGraph, eliminate) {
   EXPECT_LONGS_EQUAL(result.first->size(), 1);
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminateMultifrontal) {
   HybridFactorGraph hfg;
 
@@ -116,6 +118,7 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
   EXPECT_LONGS_EQUAL(result.second->size(), 1);
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
   HybridFactorGraph hfg;
 
@@ -139,9 +142,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
   EXPECT_DOUBLES_EQUAL(0.6225, dc->operator()(dv), 1e-3);
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
-  std::cout << ">>>>>>>>>>>>>>\n";
-
   HybridFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
@@ -168,9 +170,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
   GTSAM_PRINT(*result);
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
-  std::cout << ">>>>>>>>>>>>>>\n";
-
   HybridFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
@@ -202,9 +203,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
   GTSAM_PRINT(*result->marginalFactor(C(2)));
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
-  std::cout << ">>>>>>>>>>>>>>\n";
-
   HybridFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
@@ -235,13 +235,12 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
   */
 }
 
+/* ************************************************************************* */
 /*
  * This test is about how to assemble the Bayes Tree roots after we do partial
  * elimination
  */
 TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
-  std::cout << ">>>>>>>>>>>>>>\n";
-
   HybridFactorGraph hfg;
 
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
@@ -308,6 +307,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   */
 }
 
+/* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
 TEST(HybridFactorGraph, Switching) {
   auto N = 12;
@@ -415,6 +415,7 @@ TEST(HybridFactorGraph, Switching) {
   hbt->marginalFactor(C(11))->print("HBT: ");
 }
 
+/* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
 TEST(HybridFactorGraph, SwitchingISAM) {
   auto N = 11;
@@ -510,6 +511,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
   }
 }
 
+/* ************************************************************************* */
 TEST(HybridFactorGraph, SwitchingTwoVar) {
   const int N = 7;
   auto hfg = makeSwitchingChain(N, X);
@@ -599,10 +601,6 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
 
 /* ************************************************************************* */
 int main() {
-#ifdef HYBRID_DEBUG
-  ::signal(SIGSEGV, &my_signal_handler);
-  ::signal(SIGBUS, &my_signal_handler);
-#endif
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

From adcbb65e5c33f3a9f803a6d0a89379bd5830450d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:07:52 -0400
Subject: [PATCH 113/175] HybridFactorGraph docs and minor updates

---
 gtsam/hybrid/HybridFactorGraph.cpp | 21 +++++++++++++--------
 gtsam/hybrid/HybridFactorGraph.h   | 22 +++++++++++++---------
 2 files changed, 26 insertions(+), 17 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 450636ab3..344b4220b 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -63,6 +63,7 @@ static std::string RESET = "\033[0m";
 
 constexpr bool DEBUG = false;
 
+/* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
   using Y = GaussianFactorGraph;
@@ -406,20 +407,24 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   }
 }
 
+/* ************************************************************************ */
 void HybridFactorGraph::add(JacobianFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
 }
 
-void HybridFactorGraph::add(DecisionTreeFactor &&factor) {
-  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
-}
-
-void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
-  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
-}
-
+/* ************************************************************************ */
 void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
 }
 
+/* ************************************************************************ */
+void HybridFactorGraph::add(DecisionTreeFactor &&factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
+}
+
+/* ************************************************************************ */
+void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
+}
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index bfd6a8690..10670b6b7 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -81,30 +81,34 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
   using Values = gtsam::Values;  ///< backwards compatibility
   using Indices = KeyVector;     ///> map from keys to values
 
- public:
+  /// @name Constructors
+  /// @{
+
   HybridFactorGraph() = default;
 
-  //  /** Construct from container of factors (shared_ptr or plain objects) */
-  //  template <class CONTAINER>
-  //  explicit HybridFactorGraph(const CONTAINER& factors) : Base(factors) {}
-
-  /** Implicit copy/downcast constructor to override explicit template container
+  /**
+   * Implicit copy/downcast constructor to override explicit template container
    * constructor. In BayesTree this is used for:
    * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
    * */
   template <class DERIVEDFACTOR>
   HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+  /// @}
+
   using FactorGraph::add;
 
-  /// Add a factor directly using a shared_ptr.
+  /// Add a Jacobian factor to the factor graph.
   void add(JacobianFactor&& factor);
 
+  /// Add a Jacobian factor as a shared ptr.
+  void add(boost::shared_ptr<JacobianFactor> factor);
+
+  /// Add a DecisionTreeFactor to the factor graph.
   void add(DecisionTreeFactor&& factor);
 
+  /// Add a DecisionTreeFactor as a shared ptr.
   void add(boost::shared_ptr<DecisionTreeFactor> factor);
-
-  void add(boost::shared_ptr<JacobianFactor> factor);
 };
 
 }  // namespace gtsam

From 5d3ffb7fe825a526810a8052cebeae9945528646 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:14:39 -0400
Subject: [PATCH 114/175] docs update

---
 gtsam/inference/BayesTree.h | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index 0c7d0a8fb..924a505a2 100644
--- a/gtsam/inference/BayesTree.h
+++ b/gtsam/inference/BayesTree.h
@@ -281,10 +281,18 @@ namespace gtsam {
 
     boost::shared_ptr<CliqueType> clique;
 
+    /**
+     * @brief Construct a new Bayes Tree Orphan Wrapper object
+     *
+     * This object stores parent keys in our base type factor so that
+     * eliminating those parent keys will pull this subtree into the
+     * elimination.
+     *
+     * @param clique Orphan clique to add for further consideration in
+     * elimination.
+     */
     BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
         : clique(clique) {
-      // Store parent keys in our base type factor so that eliminating those
-      // parent keys will pull this subtree into the elimination.
       this->keys_.assign(clique->conditional()->beginParents(),
                          clique->conditional()->endParents());
     }

From e2c775302aa8d6f364d1a9f72b012c133cb197bd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:15:53 -0400
Subject: [PATCH 115/175] remove debug statements

---
 gtsam/hybrid/HybridJunctionTree.cpp | 24 +-----------------------
 1 file changed, 1 insertion(+), 23 deletions(-)

diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index d1e19f852..981cd6f32 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -23,7 +23,6 @@
 
 #include <unordered_map>
 
-// #undef NDEBUG
 namespace gtsam {
 
 // Instantiate base classes
@@ -59,15 +58,8 @@ struct HybridConstructorTraversalData {
     myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
     parentData.myJTNode->addChild(myData.myJTNode);
 
-#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
-    std::cout << "Getting discrete info: ";
-#endif
     for (HybridFactor::shared_ptr& f : node->factors) {
       for (auto& k : f->discreteKeys()) {
-#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
-        std::cout << "DK: " << DefaultKeyFormatter(k.first) << "\n";
-#endif
-
         myData.discreteKeys.insert(k.first);
       }
     }
@@ -104,11 +96,6 @@ struct HybridConstructorTraversalData {
     boost::tie(myConditional, mySeparatorFactor) =
         internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
 
-#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
-    std::cout << "Symbolic: ";
-    myConditional->print();
-#endif
-
     // Store symbolic elimination results in the parent
     myData.parentData->childSymbolicConditionals.push_back(myConditional);
     myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
@@ -136,19 +123,10 @@ struct HybridConstructorTraversalData {
             myData.discreteKeys.exists(myConditional->frontals()[0]);
         const bool theirType =
             myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
-#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
-        std::cout << "Type: "
-                  << DefaultKeyFormatter(myConditional->frontals()[0]) << " vs "
-                  << DefaultKeyFormatter(childConditionals[i]->frontals()[0])
-                  << "\n";
-#endif
+
         if (myType == theirType) {
           // Increment number of frontal variables
           myNrFrontals += nrFrontals[i];
-#ifdef GTSAM_HYBRID_JUNCTIONTREE_DEBUG
-          std::cout << "Merging ";
-          childConditionals[i]->print();
-#endif
           merge[i] = true;
         }
       }

From 30c8e1dfa723d987fd2ad5c35474646e448bdbc1 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:16:30 -0400
Subject: [PATCH 116/175] fix doxygen section

---
 gtsam/hybrid/HybridISAM.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h
index ac9e17e83..7f9e77b8b 100644
--- a/gtsam/hybrid/HybridISAM.h
+++ b/gtsam/hybrid/HybridISAM.h
@@ -41,6 +41,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
   /** Copy constructor */
   HybridISAM(const HybridBayesTree& bayesTree);
 
+  /// @}
+
   void updateInternal(
       const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
       const HybridBayesTree::Eliminate& function =
@@ -49,7 +51,6 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
   void update(const HybridFactorGraph& newFactors,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
-  /// @}
 };
 
 /// traits

From 4ee4b37f2f85eeeda35c728cd6e3fc2c860b2727 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:23:35 -0400
Subject: [PATCH 117/175] HybridISAM docs

---
 gtsam/hybrid/HybridISAM.cpp | 4 ++--
 gtsam/hybrid/HybridISAM.h   | 9 +++++++++
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp
index f743b92f1..057f784ad 100644
--- a/gtsam/hybrid/HybridISAM.cpp
+++ b/gtsam/hybrid/HybridISAM.cpp
@@ -36,6 +36,7 @@ HybridISAM::HybridISAM() {}
 /* ************************************************************************* */
 HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
 
+/* ************************************************************************* */
 void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
                                 HybridBayesTree::Cliques* orphans,
                                 const HybridBayesTree::Eliminate& function) {
@@ -79,8 +80,6 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
       index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()),
       true);
 
-  ordering.print("ORD");
-
   // eliminate all factors (top, added, orphans) into a new Bayes tree
   auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
 
@@ -90,6 +89,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
   this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
 }
 
+/* ************************************************************************* */
 void HybridISAM::update(const HybridFactorGraph& newFactors,
                         const HybridBayesTree::Eliminate& function) {
   Cliques orphans;
diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h
index 7f9e77b8b..0738f46c8 100644
--- a/gtsam/hybrid/HybridISAM.h
+++ b/gtsam/hybrid/HybridISAM.h
@@ -43,11 +43,20 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
 
   /// @}
 
+ private:
+  /// Internal method that performs the ISAM update.
   void updateInternal(
       const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
       const HybridBayesTree::Eliminate& function =
           HybridBayesTree::EliminationTraitsType::DefaultEliminate);
 
+ public:
+  /**
+   * @brief Perform update step with new factors.
+   *
+   * @param newFactors Factor graph of new factors to add and eliminate.
+   * @param function Elimination function.
+   */
   void update(const HybridFactorGraph& newFactors,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);

From 9d26a3dc9d2433e937ec41256073578cc28c134c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 16:43:19 -0400
Subject: [PATCH 118/175] remove debug statements and add docs

---
 gtsam/hybrid/HybridFactorGraph.cpp  | 82 +----------------------------
 gtsam/hybrid/HybridGaussianFactor.h |  7 +++
 2 files changed, 8 insertions(+), 81 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 344b4220b..bffb0327c 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -138,18 +138,8 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   KeySet continuousFrontals;
   KeySet continuousSeparator;
 
-  if (DEBUG) {
-    std::cout << RED_BOLD << "Begin Eliminate: " << RESET;
-    frontalKeys.print();
-  }
-
   // This initializes separatorKeys and mapFromKeyToDiscreteKey
   for (auto &&factor : factors) {
-    if (DEBUG) {
-      std::cout << ">>> Adding factor: " << GREEN;
-      factor->print();
-      std::cout << RESET;
-    }
     separatorKeys.insert(factor->begin(), factor->end());
     if (!factor->isContinuous()) {
       for (auto &k : factor->discreteKeys()) {
@@ -183,43 +173,10 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
     }
   }
 
-  // Only for printing
-  if (DEBUG) {
-    std::cout << RED_BOLD << "Keys: " << RESET;
-    for (auto &f : frontalKeys) {
-      if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
-        auto &key = mapFromKeyToDiscreteKey.at(f);
-        std::cout << boost::format(" (%1%,%2%),") %
-                         DefaultKeyFormatter(key.first) % key.second;
-      } else {
-        std::cout << " " << DefaultKeyFormatter(f) << ",";
-      }
-    }
-
-    if (separatorKeys.size() > 0) {
-      std::cout << " | ";
-    }
-
-    for (auto &f : separatorKeys) {
-      if (mapFromKeyToDiscreteKey.find(f) != mapFromKeyToDiscreteKey.end()) {
-        auto &key = mapFromKeyToDiscreteKey.at(f);
-        std::cout << boost::format(" (%1%,%2%),") %
-                         DefaultKeyFormatter(key.first) % key.second;
-      } else {
-        std::cout << DefaultKeyFormatter(f) << ",";
-      }
-    }
-    std::cout << "\n" << RESET;
-  }
-
   // NOTE: We should really defer the product here because of pruning
 
   // Case 1: we are only dealing with continuous
   if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
-    if (DEBUG) {
-      std::cout << RED_BOLD << "CONT. ONLY" << RESET << "\n";
-    }
-
     GaussianFactorGraph gfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
@@ -231,7 +188,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
           gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
         } else {
           // It is an orphan wrapped conditional
-          if (DEBUG) std::cout << "Got an orphan conditional\n";
         }
       }
     }
@@ -244,10 +200,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Case 2: we are only dealing with discrete
   if (allContinuousKeys.empty()) {
-    if (DEBUG) {
-      std::cout << RED_BOLD << "DISCRETE ONLY" << RESET << "\n";
-    }
-
     DiscreteFactorGraph dfg;
     for (auto &fp : factors) {
       auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
@@ -259,7 +211,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
           dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
         } else {
           // It is an orphan wrapper
-          if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
         }
       }
     }
@@ -280,10 +231,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   // sum out frontals, this is the factor on the separator
   gttic(sum);
 
-  if (DEBUG) {
-    std::cout << RED_BOLD << "HYBRID ELIM." << RESET << "\n";
-  }
-
   GaussianMixtureFactor::Sum sum;
 
   std::vector<GaussianFactor::shared_ptr> deferredFactors;
@@ -308,9 +255,7 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
       // BayesTreeOrphanWrapper!
       auto orphan = boost::dynamic_pointer_cast<
           BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
-      if (orphan) {
-        if (DEBUG) std::cout << "Got an orphan wrapper conditional\n";
-      } else {
+      if (!orphan) {
         auto &fr = *f;
         throw std::invalid_argument(
             std::string("factor is discrete in continuous elimination") +
@@ -320,21 +265,9 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   }
 
   for (auto &f : deferredFactors) {
-    if (DEBUG) {
-      std::cout << GREEN_BOLD << "Adding Gaussian" << RESET << "\n";
-    }
     sum = addGaussian(sum, f);
   }
 
-  if (DEBUG) {
-    std::cout << GREEN_BOLD << "[GFG Tree]\n" << RESET;
-    sum.print("", DefaultKeyFormatter, [](GaussianFactorGraph gfg) {
-      RedirectCout rd;
-      gfg.print("");
-      return rd.str();
-    });
-  }
-
   gttoc(sum);
 
   using EliminationPair = GaussianFactorGraph::EliminationResult;
@@ -370,19 +303,6 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
   auto conditional = boost::make_shared<GaussianMixtureConditional>(
       frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
 
-  if (DEBUG) {
-    std::cout << GREEN_BOLD << "[Conditional]\n" << RESET;
-    conditional->print();
-    std::cout << GREEN_BOLD << "[Separator]\n" << RESET;
-    separatorFactors.print("", DefaultKeyFormatter,
-                           [](GaussianFactor::shared_ptr gc) {
-                             RedirectCout rd;
-                             gc->print("");
-                             return rd.str();
-                           });
-    std::cout << RED_BOLD << "[End Eliminate]\n" << RESET;
-  }
-
   // If there are no more continuous parents, then we should create here a
   // DiscreteFactor, with the error for each discrete choice.
   if (keysOfSeparator.empty()) {
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 1749c8e41..8d457e778 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -43,12 +43,19 @@ class HybridGaussianFactor : public HybridFactor {
   explicit HybridGaussianFactor(JacobianFactor &&jf);
 
  public:
+  /// @name Testable
+  /// @{
+
+  /// Check equality.
   virtual bool equals(const HybridFactor &lf, double tol) const override;
 
+  /// GTSAM print utility.
   void print(
       const std::string &s = "HybridFactor\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
+  /// @}
+
   GaussianFactor::shared_ptr inner() const { return inner_; }
 };
 }  // namespace gtsam

From 3bde044248385442101532b91490359c8eb175ba Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 18:13:06 -0400
Subject: [PATCH 119/175] add doc strings to python unit test and add
 assertions

---
 gtsam/hybrid/GaussianMixtureConditional.cpp  | 10 ++--
 gtsam/hybrid/HybridConditional.cpp           | 50 +++++++++++---------
 gtsam/hybrid/HybridFactor.cpp                |  6 +--
 python/gtsam/tests/test_HybridFactorGraph.py | 24 +++++-----
 4 files changed, 49 insertions(+), 41 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp
index 68c3f505e..726af6d5f 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.cpp
+++ b/gtsam/hybrid/GaussianMixtureConditional.cpp
@@ -85,12 +85,12 @@ bool GaussianMixtureConditional::equals(const HybridFactor &lf,
 /* *******************************************************************************/
 void GaussianMixtureConditional::print(const std::string &s,
                                        const KeyFormatter &formatter) const {
-  std::cout << s << ": ";
-  if (isContinuous()) std::cout << "Cont. ";
-  if (isDiscrete()) std::cout << "Disc. ";
-  if (isHybrid()) std::cout << "Hybr. ";
+  std::cout << s;
+  if (isContinuous()) std::cout << "Continuous ";
+  if (isDiscrete()) std::cout << "Discrete ";
+  if (isHybrid()) std::cout << "Hybrid ";
   BaseConditional::print("", formatter);
-  std::cout << "Discrete Keys = ";
+  std::cout << "\nDiscrete Keys = ";
   for (auto &dk : discreteKeys()) {
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index e70d100c3..7d1b72067 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -67,30 +67,36 @@ HybridConditional::HybridConditional(
 void HybridConditional::print(const std::string &s,
                               const KeyFormatter &formatter) const {
   std::cout << s;
-  if (isContinuous()) std::cout << "Cont. ";
-  if (isDiscrete()) std::cout << "Disc. ";
-  if (isHybrid()) std::cout << "Hybr. ";
-  std::cout << "P(";
-  size_t index = 0;
-  const size_t N = keys().size();
-  const size_t contN = N - discreteKeys_.size();
-  while (index < N) {
-    if (index > 0) {
-      if (index == nrFrontals_)
-        std::cout << " | ";
-      else
-        std::cout << ", ";
+
+  if (inner_) {
+    inner_->print("", formatter);
+
+  } else {
+    if (isContinuous()) std::cout << "Continuous ";
+    if (isDiscrete()) std::cout << "Discrete ";
+    if (isHybrid()) std::cout << "Hybrid ";
+    BaseConditional::print("", formatter);
+
+    std::cout << "P(";
+    size_t index = 0;
+    const size_t N = keys().size();
+    const size_t contN = N - discreteKeys_.size();
+    while (index < N) {
+      if (index > 0) {
+        if (index == nrFrontals_)
+          std::cout << " | ";
+        else
+          std::cout << ", ";
+      }
+      if (index < contN) {
+        std::cout << formatter(keys()[index]);
+      } else {
+        auto &dk = discreteKeys_[index - contN];
+        std::cout << "(" << formatter(dk.first) << ", " << dk.second << ")";
+      }
+      index++;
     }
-    if (index < contN) {
-      std::cout << formatter(keys()[index]);
-    } else {
-      auto &dk = discreteKeys_[index - contN];
-      std::cout << "(" << formatter(dk.first) << ", " << dk.second << ")";
-    }
-    index++;
   }
-  std::cout << ")\n";
-  if (inner_) inner_->print("", formatter);
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 9358c473d..815ea0415 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -77,9 +77,9 @@ bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
 void HybridFactor::print(const std::string &s,
                          const KeyFormatter &formatter) const {
   std::cout << s;
-  if (isContinuous_) std::cout << "Cont. ";
-  if (isDiscrete_) std::cout << "Disc. ";
-  if (isHybrid_) std::cout << "Hybr. ";
+  if (isContinuous_) std::cout << "Continuous ";
+  if (isDiscrete_) std::cout << "Discrete ";
+  if (isHybrid_) std::cout << "Hybrid ";
   this->printKeys("", formatter);
 }
 
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 48187b7a2..144183816 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -16,24 +16,25 @@ import unittest
 
 import gtsam
 import numpy as np
-from gtsam.symbol_shorthand import X, C
+from gtsam.symbol_shorthand import C, X
 from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestHybridFactorGraph(GtsamTestCase):
+    """Unit tests for HybridFactorGraph."""
+
     def test_create(self):
+        """Test contruction of hybrid factor graph."""
         noiseModel = gtsam.noiseModel.Unit.Create(3)
         dk = gtsam.DiscreteKeys()
         dk.push_back((C(0), 2))
 
-        # print(dk.at(0))
         jf1 = gtsam.JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)),
                                    noiseModel)
         jf2 = gtsam.JacobianFactor(X(0), np.eye(3), np.ones((3, 1)),
                                    noiseModel)
 
-        gmf = gtsam.GaussianMixtureFactor.FromFactorList([X(0)], dk,
-                                                         [jf1, jf2])
+        gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
 
         hfg = gtsam.HybridFactorGraph()
         hfg.add(jf1)
@@ -41,16 +42,17 @@ class TestHybridFactorGraph(GtsamTestCase):
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(
-                hfg, [C(0)]))
+            gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)]))
 
-        print("hbn = ", hbn)
+        # print("hbn = ", hbn)
+        self.assertEqual(hbn.size(), 2)
 
-        mixture = hbn.at(0).getInner()
-        print(mixture)
+        mixture = hbn.at(0).inner()
+        self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional)
+        self.assertEqual(len(mixture.keys()), 2)
 
-        discrete_conditional = hbn.at(hbn.size()-1).getInner()
-        print(discrete_conditional)
+        discrete_conditional = hbn.at(hbn.size() - 1).inner()
+        self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional)
 
 
 if __name__ == "__main__":

From f443cf30e0c2517da1da26a99ec13460297c7ccf Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 18:32:19 -0400
Subject: [PATCH 120/175] add docs for HybridFactor

---
 gtsam/hybrid/HybridFactor.h | 21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index da103da43..244fba4cc 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -34,6 +34,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /**
  * Base class for hybrid probabilistic factors
+ *
  * Examples:
  *  - HybridGaussianFactor
  *  - HybridDiscreteFactor
@@ -64,13 +65,29 @@ class GTSAM_EXPORT HybridFactor : public Factor {
   /** Default constructor creates empty factor */
   HybridFactor() = default;
 
+  /**
+   * @brief Construct hybrid factor from continuous keys.
+   *
+   * @param keys Vector of continuous keys.
+   */
   explicit HybridFactor(const KeyVector &keys);
 
+  /**
+   * @brief Construct hybrid factor from discrete keys.
+   *
+   * @param keys Vector of discrete keys.
+   */
+  explicit HybridFactor(const DiscreteKeys &discreteKeys);
+
+  /**
+   * @brief Construct a new Hybrid Factor object.
+   *
+   * @param continuousKeys Vector of keys for continuous variables.
+   * @param discreteKeys Vector of keys for discrete variables.
+   */
   HybridFactor(const KeyVector &continuousKeys,
                const DiscreteKeys &discreteKeys);
 
-  explicit HybridFactor(const DiscreteKeys &discreteKeys);
-
   /// Virtual destructor
   virtual ~HybridFactor() = default;
 

From 6cd20fba4d866092c3da220bc1323ebae5a8b901 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 18:34:13 -0400
Subject: [PATCH 121/175] break up EliminateHybrid into smaller functions

---
 gtsam/hybrid/HybridFactorGraph.cpp | 310 +++++++++++++++--------------
 1 file changed, 161 insertions(+), 149 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index bffb0327c..426c86fa3 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -55,14 +55,6 @@ namespace gtsam {
 
 template class EliminateableFactorGraph<HybridFactorGraph>;
 
-static std::string BLACK_BOLD = "\033[1;30m";
-static std::string RED_BOLD = "\033[1;31m";
-static std::string GREEN = "\033[0;32m";
-static std::string GREEN_BOLD = "\033[1;32m";
-static std::string RESET = "\033[0m";
-
-constexpr bool DEBUG = false;
-
 /* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
     GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {
@@ -84,6 +76,163 @@ static GaussianMixtureFactor::Sum &addGaussian(
   return sum;
 }
 
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
+continuousElimination(const HybridFactorGraph &factors,
+                      const Ordering &frontalKeys) {
+  GaussianFactorGraph gfg;
+  for (auto &fp : factors) {
+    auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
+    if (ptr) {
+      gfg.push_back(ptr->inner());
+    } else {
+      auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
+      if (p) {
+        gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
+      } else {
+        // It is an orphan wrapped conditional
+      }
+    }
+  }
+
+  auto result = EliminatePreferCholesky(gfg, frontalKeys);
+  return std::make_pair(
+      boost::make_shared<HybridConditional>(result.first),
+      boost::make_shared<HybridGaussianFactor>(result.second));
+}
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
+discreteElimination(const HybridFactorGraph &factors,
+                    const Ordering &frontalKeys) {
+  DiscreteFactorGraph dfg;
+  for (auto &fp : factors) {
+    auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
+    if (ptr) {
+      dfg.push_back(ptr->inner());
+    } else {
+      auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
+      if (p) {
+        dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
+      } else {
+        // It is an orphan wrapper
+      }
+    }
+  }
+
+  auto result = EliminateDiscrete(dfg, frontalKeys);
+
+  return std::make_pair(
+      boost::make_shared<HybridConditional>(result.first),
+      boost::make_shared<HybridDiscreteFactor>(result.second));
+}
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
+hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
+                  const KeySet &continuousSeparator,
+                  const std::set<DiscreteKey> &discreteSeparatorSet) {
+  // NOTE: since we use the special JunctionTree,
+  // only possiblity is continuous conditioned on discrete.
+  DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(),
+                                 discreteSeparatorSet.end());
+
+  // sum out frontals, this is the factor on the separator
+  gttic(sum);
+
+  GaussianMixtureFactor::Sum sum;
+  std::vector<GaussianFactor::shared_ptr> deferredFactors;
+
+  for (auto &f : factors) {
+    if (f->isHybrid()) {
+      auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
+      if (cgmf) {
+        sum = cgmf->add(sum);
+      }
+
+      auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
+      if (gm) {
+        sum = gm->asMixture()->add(sum);
+      }
+
+    } else if (f->isContinuous()) {
+      deferredFactors.push_back(
+          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
+    } else {
+      // We need to handle the case where the object is actually an
+      // BayesTreeOrphanWrapper!
+      auto orphan = boost::dynamic_pointer_cast<
+          BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
+      if (!orphan) {
+        auto &fr = *f;
+        throw std::invalid_argument(
+            std::string("factor is discrete in continuous elimination") +
+            typeid(fr).name());
+      }
+    }
+  }
+
+  for (auto &f : deferredFactors) {
+    sum = addGaussian(sum, f);
+  }
+
+  gttoc(sum);
+
+  using EliminationPair = GaussianFactorGraph::EliminationResult;
+
+  KeyVector keysOfEliminated;  // Not the ordering
+  KeyVector keysOfSeparator;   // TODO(frank): Is this just (keys - ordering)?
+
+  auto eliminate = [&](const GaussianFactorGraph &graph)
+      -> GaussianFactorGraph::EliminationResult {
+    if (graph.empty()) {
+      return {nullptr, nullptr};
+    }
+    auto result = EliminatePreferCholesky(graph, frontalKeys);
+    if (keysOfEliminated.empty()) {
+      keysOfEliminated =
+          result.first->keys();  // Initialize the keysOfEliminated to be the
+    }
+    // keysOfEliminated of the GaussianConditional
+    if (keysOfSeparator.empty()) {
+      keysOfSeparator = result.second->keys();
+    }
+    return result;
+  };
+
+  DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate);
+
+  auto pair = unzip(eliminationResults);
+
+  const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
+
+  // Create the GaussianMixtureConditional from the conditionals
+  auto conditional = boost::make_shared<GaussianMixtureConditional>(
+      frontalKeys, keysOfSeparator, discreteSeparator, pair.first);
+
+  // If there are no more continuous parents, then we should create here a
+  // DiscreteFactor, with the error for each discrete choice.
+  if (keysOfSeparator.empty()) {
+    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));
+    };
+    DecisionTree<Key, double> fdt(separatorFactors, factorError);
+    auto discreteFactor =
+        boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
+
+    return {boost::make_shared<HybridConditional>(conditional),
+            boost::make_shared<HybridDiscreteFactor>(discreteFactor)};
+
+  } else {
+    // Create a resulting DCGaussianMixture on the separator.
+    auto factor = boost::make_shared<GaussianMixtureFactor>(
+        KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
+        discreteSeparator, separatorFactors);
+    return {boost::make_shared<HybridConditional>(conditional), factor};
+  }
+}
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
@@ -177,154 +326,17 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 
   // Case 1: we are only dealing with continuous
   if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
-    GaussianFactorGraph gfg;
-    for (auto &fp : factors) {
-      auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
-      if (ptr) {
-        gfg.push_back(ptr->inner());
-      } else {
-        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
-        if (p) {
-          gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
-        } else {
-          // It is an orphan wrapped conditional
-        }
-      }
-    }
-
-    auto result = EliminatePreferCholesky(gfg, frontalKeys);
-    return std::make_pair(
-        boost::make_shared<HybridConditional>(result.first),
-        boost::make_shared<HybridGaussianFactor>(result.second));
+    return continuousElimination(factors, frontalKeys);
   }
 
   // Case 2: we are only dealing with discrete
   if (allContinuousKeys.empty()) {
-    DiscreteFactorGraph dfg;
-    for (auto &fp : factors) {
-      auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
-      if (ptr) {
-        dfg.push_back(ptr->inner());
-      } else {
-        auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
-        if (p) {
-          dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
-        } else {
-          // It is an orphan wrapper
-        }
-      }
-    }
-
-    auto result = EliminateDiscrete(dfg, frontalKeys);
-
-    return std::make_pair(
-        boost::make_shared<HybridConditional>(result.first),
-        boost::make_shared<HybridDiscreteFactor>(result.second));
+    return discreteElimination(factors, frontalKeys);
   }
 
   // Case 3: We are now in the hybrid land!
-  // NOTE: since we use the special JunctionTree, only possiblity is cont.
-  // conditioned on disc.
-  DiscreteKeys discreteSeparator(discreteSeparatorSet.begin(),
-                                 discreteSeparatorSet.end());
-
-  // sum out frontals, this is the factor on the separator
-  gttic(sum);
-
-  GaussianMixtureFactor::Sum sum;
-
-  std::vector<GaussianFactor::shared_ptr> deferredFactors;
-
-  for (auto &f : factors) {
-    if (f->isHybrid()) {
-      auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
-      if (cgmf) {
-        sum = cgmf->add(sum);
-      }
-
-      auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
-      if (gm) {
-        sum = gm->asMixture()->add(sum);
-      }
-
-    } else if (f->isContinuous()) {
-      deferredFactors.push_back(
-          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
-    } else {
-      // We need to handle the case where the object is actually an
-      // BayesTreeOrphanWrapper!
-      auto orphan = boost::dynamic_pointer_cast<
-          BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
-      if (!orphan) {
-        auto &fr = *f;
-        throw std::invalid_argument(
-            std::string("factor is discrete in continuous elimination") +
-            typeid(fr).name());
-      }
-    }
-  }
-
-  for (auto &f : deferredFactors) {
-    sum = addGaussian(sum, f);
-  }
-
-  gttoc(sum);
-
-  using EliminationPair = GaussianFactorGraph::EliminationResult;
-
-  KeyVector keysOfEliminated;  // Not the ordering
-  KeyVector keysOfSeparator;   // TODO(frank): Is this just (keys - ordering)?
-
-  auto eliminate = [&](const GaussianFactorGraph &graph)
-      -> GaussianFactorGraph::EliminationResult {
-    if (graph.empty()) {
-      return {nullptr, nullptr};
-    }
-    auto result = EliminatePreferCholesky(graph, frontalKeys);
-    if (keysOfEliminated.empty()) {
-      keysOfEliminated =
-          result.first->keys();  // Initialize the keysOfEliminated to be the
-    }
-    // keysOfEliminated of the GaussianConditional
-    if (keysOfSeparator.empty()) {
-      keysOfSeparator = result.second->keys();
-    }
-    return result;
-  };
-
-  DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate);
-
-  auto pair = unzip(eliminationResults);
-
-  const GaussianMixtureConditional::Conditionals &conditionals = pair.first;
-  const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
-
-  // Create the GaussianMixtureConditional from the conditionals
-  auto conditional = boost::make_shared<GaussianMixtureConditional>(
-      frontalKeys, keysOfSeparator, discreteSeparator, conditionals);
-
-  // If there are no more continuous parents, then we should create here a
-  // DiscreteFactor, with the error for each discrete choice.
-  if (keysOfSeparator.empty()) {
-    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));
-    };
-    DecisionTree<Key, double> fdt(separatorFactors, factorError);
-    auto discreteFactor =
-        boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
-
-    return {boost::make_shared<HybridConditional>(conditional),
-            boost::make_shared<HybridDiscreteFactor>(discreteFactor)};
-
-  } else {
-    // Create a resulting DCGaussianMixture on the separator.
-    auto factor = boost::make_shared<GaussianMixtureFactor>(
-        KeyVector(continuousSeparator.begin(), continuousSeparator.end()),
-        discreteSeparator, separatorFactors);
-    return {boost::make_shared<HybridConditional>(conditional), factor};
-  }
+  return hybridElimination(factors, frontalKeys, continuousSeparator,
+                           discreteSeparatorSet);
 }
 
 /* ************************************************************************ */

From c3a59cfd620ec3de15f381c7d2cd9585c7641731 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 27 May 2022 18:46:52 -0400
Subject: [PATCH 122/175] update the EliminateHybrid note to be more specific

---
 gtsam/hybrid/HybridFactorGraph.cpp | 53 ++++++++++++++++--------------
 1 file changed, 28 insertions(+), 25 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 426c86fa3..dbb4a62b3 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -236,43 +236,46 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
 EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
-  // NOTE(fan): Because we are in the Conditional Gaussian regime there are only
+  // NOTE: Because we are in the Conditional Gaussian regime there are only
   // a few cases:
-  // continuous variable, we make a GM if there are hybrid factors;
-  // continuous variable, we make a GF if there are no hybrid factors;
-  // discrete variable, no continuous factor is allowed (escapes CG regime), so
-  // we panic, if discrete only we do the discrete elimination.
+  // 1. continuous variable, make a Gaussian Mixture if there are hybrid
+  // factors;
+  // 2. continuous variable, we make a Gaussian Factor if there are no hybrid
+  // factors;
+  // 3. discrete variable, no continuous factor is allowed
+  // (escapes Conditional Gaussian regime), if discrete only we do the discrete
+  // elimination.
 
   // However it is not that simple. During elimination it is possible that the
   // multifrontal needs to eliminate an ordering that contains both Gaussian and
-  // hybrid variables, for example x1, c1. In this scenario, we will have a
-  // density P(x1, c1) that is a CLG P(x1|c1)P(c1) (see Murphy02)
+  // hybrid variables, for example x1, c1.
+  // In this scenario, we will have a density P(x1, c1) that is a Conditional
+  // Linear Gaussian P(x1|c1)P(c1) (see Murphy02).
 
   // The issue here is that, how can we know which variable is discrete if we
   // unify Values? Obviously we can tell using the factors, but is that fast?
 
   // In the case of multifrontal, we will need to use a constrained ordering
   // so that the discrete parts will be guaranteed to be eliminated last!
-
-  // Because of all these reasons, we need to think very carefully about how to
+  // Because of all these reasons, we carefully consider how to
   // implement the hybrid factors so that we do not get poor performance.
-  //
-  // The first thing is how to represent the GaussianMixtureConditional. A very
-  // possible scenario is that the incoming factors will have different levels
-  // of discrete keys. For example, imagine we are going to eliminate the
-  // fragment:
-  // $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid. Now we will
-  // need to know how to retrieve the corresponding continuous densities for the
-  // assi- -gnment (c1,c2,c3) (OR (c2,c3,c1)! note there is NO defined order!).
-  // And we also need to consider when there is pruning. Two mixture factors
-  // could have different pruning patterns-one could have (c1=0,c2=1) pruned,
-  // and another could have (c2=0,c3=1) pruned, and this creates a big problem
-  // in how to identify the intersection of non-pruned branches.
 
-  // One possible approach is first building the collection of all discrete
-  // keys. After that we enumerate the space of all key combinations *lazily* so
-  // that the exploration branch terminates whenever an assignment yields NULL
-  // in any of the hybrid factors.
+  // The first thing is how to represent the GaussianMixtureConditional.
+  // A very possible scenario is that the incoming factors will have different
+  // levels of discrete keys. For example, imagine we are going to eliminate the
+  // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid.
+  // Now we will need to know how to retrieve the corresponding continuous
+  // densities for the assignment (c1,c2,c3) (OR (c2,c3,c1), note there is NO
+  // defined order!). We also need to consider when there is pruning. Two
+  // mixture factors could have different pruning patterns - one could have
+  // (c1=0,c2=1) pruned, and another could have (c2=0,c3=1) pruned, and this
+  // creates a big problem in how to identify the intersection of non-pruned
+  // branches.
+
+  // Our approach is first building the collection of all discrete keys. After
+  // that we enumerate the space of all key combinations *lazily* so that the
+  // exploration branch terminates whenever an assignment yields NULL in any of
+  // the hybrid factors.
 
   // When the number of assignments is large we may encounter stack overflows.
   // However this is also the case with iSAM2, so no pressure :)

From 852a9b9ef65ff4f621f843d635484ca8b8375ccb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 28 May 2022 14:24:03 -0400
Subject: [PATCH 123/175] rename HybridFactorGraph to GaussianHybridFactorGraph

---
 ...raph.cpp => GaussianHybridFactorGraph.cpp} | 24 +++++----
 ...torGraph.h => GaussianHybridFactorGraph.h} | 35 +++++++------
 gtsam/hybrid/HybridBayesTree.cpp              |  3 +-
 gtsam/hybrid/HybridBayesTree.h                |  8 +--
 gtsam/hybrid/HybridConditional.h              |  7 +--
 gtsam/hybrid/HybridEliminationTree.cpp        |  8 +--
 gtsam/hybrid/HybridEliminationTree.h          | 10 ++--
 gtsam/hybrid/HybridISAM.cpp                   |  6 +--
 gtsam/hybrid/HybridISAM.h                     |  7 +--
 gtsam/hybrid/HybridJunctionTree.cpp           | 16 +++---
 gtsam/hybrid/HybridJunctionTree.h             |  6 +--
 gtsam/hybrid/hybrid.i                         | 16 +++---
 gtsam/hybrid/tests/Switching.h                |  8 +--
 ....cpp => testGaussianHybridFactorGraph.cpp} | 52 +++++++++----------
 gtsam/inference/inference.i                   | 14 ++---
 python/gtsam/tests/test_HybridFactorGraph.py  |  9 ++--
 16 files changed, 122 insertions(+), 107 deletions(-)
 rename gtsam/hybrid/{HybridFactorGraph.cpp => GaussianHybridFactorGraph.cpp} (94%)
 rename gtsam/hybrid/{HybridFactorGraph.h => GaussianHybridFactorGraph.h} (74%)
 rename gtsam/hybrid/tests/{testHybridFactorGraph.cpp => testGaussianHybridFactorGraph.cpp} (93%)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/GaussianHybridFactorGraph.cpp
similarity index 94%
rename from gtsam/hybrid/HybridFactorGraph.cpp
rename to gtsam/hybrid/GaussianHybridFactorGraph.cpp
index dbb4a62b3..3354d5b4d 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/GaussianHybridFactorGraph.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   HybridFactorGraph.cpp
+ * @file   GaussianHybridFactorGraph.cpp
  * @brief  Hybrid factor graph that uses type erasure
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -23,13 +23,13 @@
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
@@ -53,7 +53,7 @@
 
 namespace gtsam {
 
-template class EliminateableFactorGraph<HybridFactorGraph>;
+template class EliminateableFactorGraph<GaussianHybridFactorGraph>;
 
 /* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
@@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian(
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-continuousElimination(const HybridFactorGraph &factors,
+continuousElimination(const GaussianHybridFactorGraph &factors,
                       const Ordering &frontalKeys) {
   GaussianFactorGraph gfg;
   for (auto &fp : factors) {
@@ -103,7 +103,7 @@ continuousElimination(const HybridFactorGraph &factors,
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-discreteElimination(const HybridFactorGraph &factors,
+discreteElimination(const GaussianHybridFactorGraph &factors,
                     const Ordering &frontalKeys) {
   DiscreteFactorGraph dfg;
   for (auto &fp : factors) {
@@ -129,7 +129,8 @@ discreteElimination(const HybridFactorGraph &factors,
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
+hybridElimination(const GaussianHybridFactorGraph &factors,
+                  const Ordering &frontalKeys,
                   const KeySet &continuousSeparator,
                   const std::set<DiscreteKey> &discreteSeparatorSet) {
   // NOTE: since we use the special JunctionTree,
@@ -235,7 +236,8 @@ hybridElimination(const HybridFactorGraph &factors, const Ordering &frontalKeys,
 }
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
+EliminateHybrid(const GaussianHybridFactorGraph &factors,
+                const Ordering &frontalKeys) {
   // NOTE: Because we are in the Conditional Gaussian regime there are only
   // a few cases:
   // 1. continuous variable, make a Gaussian Mixture if there are hybrid
@@ -343,22 +345,22 @@ EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {
 }
 
 /* ************************************************************************ */
-void HybridFactorGraph::add(JacobianFactor &&factor) {
+void GaussianHybridFactorGraph::add(JacobianFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
 }
 
 /* ************************************************************************ */
-void HybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
+void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
 }
 
 /* ************************************************************************ */
-void HybridFactorGraph::add(DecisionTreeFactor &&factor) {
+void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
 }
 
 /* ************************************************************************ */
-void HybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
+void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
 }
 
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/GaussianHybridFactorGraph.h
similarity index 74%
rename from gtsam/hybrid/HybridFactorGraph.h
rename to gtsam/hybrid/GaussianHybridFactorGraph.h
index 10670b6b7..c8e0718fc 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/GaussianHybridFactorGraph.h
@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   HybridFactorGraph.h
- * @brief  Hybrid factor graph that uses type erasure
+ * @file   GaussianHybridFactorGraph.h
+ * @brief  Linearized Hybrid factor graph that uses type erasure
  * @author Fan Jiang
  * @date   Mar 11, 2022
  */
@@ -26,7 +26,7 @@
 namespace gtsam {
 
 // Forward declarations
-class HybridFactorGraph;
+class GaussianHybridFactorGraph;
 class HybridConditional;
 class HybridBayesNet;
 class HybridEliminationTree;
@@ -36,17 +36,18 @@ class DecisionTreeFactor;
 
 class JacobianFactor;
 
-/** Main elimination function for HybridFactorGraph */
+/** Main elimination function for GaussianHybridFactorGraph */
 GTSAM_EXPORT
 std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
-EliminateHybrid(const HybridFactorGraph& factors, const Ordering& keys);
+EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
 
 /* ************************************************************************* */
 template <>
-struct EliminationTraits<HybridFactorGraph> {
+struct EliminationTraits<GaussianHybridFactorGraph> {
   typedef HybridFactor FactorType;  ///< Type of factors in factor graph
-  typedef HybridFactorGraph
-      FactorGraphType;  ///< Type of the factor graph (e.g. HybridFactorGraph)
+  typedef GaussianHybridFactorGraph
+      FactorGraphType;  ///< Type of the factor graph (e.g.
+                        ///< GaussianHybridFactorGraph)
   typedef HybridConditional
       ConditionalType;  ///< Type of conditionals from elimination
   typedef HybridBayesNet
@@ -64,16 +65,17 @@ struct EliminationTraits<HybridFactorGraph> {
 };
 
 /**
- * Hybrid Factor Graph
+ * Gaussian Hybrid Factor Graph
  * -----------------------
- * This is the linear version of a hybrid factor graph. Everything inside needs
- * to be hybrid factor or hybrid conditional.
+ * This is the linearized version of a hybrid factor graph.
+ * Everything inside needs to be hybrid factor or hybrid conditional.
  */
-class HybridFactorGraph : public FactorGraph<HybridFactor>,
-                          public EliminateableFactorGraph<HybridFactorGraph> {
+class GaussianHybridFactorGraph
+    : public FactorGraph<HybridFactor>,
+      public EliminateableFactorGraph<GaussianHybridFactorGraph> {
  public:
   using Base = FactorGraph<HybridFactor>;
-  using This = HybridFactorGraph;  ///< this class
+  using This = GaussianHybridFactorGraph;  ///< this class
   using BaseEliminateable =
       EliminateableFactorGraph<This>;          ///< for elimination
   using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
@@ -84,7 +86,7 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
   /// @name Constructors
   /// @{
 
-  HybridFactorGraph() = default;
+  GaussianHybridFactorGraph() = default;
 
   /**
    * Implicit copy/downcast constructor to override explicit template container
@@ -92,7 +94,8 @@ class HybridFactorGraph : public FactorGraph<HybridFactor>,
    * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
    * */
   template <class DERIVEDFACTOR>
-  HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
+  GaussianHybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
+      : Base(graph) {}
 
   /// @}
 
diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index c8d70e67e..400cac5e7 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -25,7 +25,8 @@
 namespace gtsam {
 
 // Instantiate base class
-template class BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph>;
+template class BayesTreeCliqueBase<HybridBayesTreeClique,
+                                   GaussianHybridFactorGraph>;
 template class BayesTree<HybridBayesTreeClique>;
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index f8a90d6b4..610181458 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -18,8 +18,8 @@
 
 #pragma once
 
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/inference/Conditional.h>
@@ -37,10 +37,12 @@ class VectorValues;
  * which is a HybridConditional internally.
  */
 class GTSAM_EXPORT HybridBayesTreeClique
-    : public BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> {
+    : public BayesTreeCliqueBase<HybridBayesTreeClique,
+                                 GaussianHybridFactorGraph> {
  public:
   typedef HybridBayesTreeClique This;
-  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridFactorGraph> Base;
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, GaussianHybridFactorGraph>
+      Base;
   typedef boost::shared_ptr<This> shared_ptr;
   typedef boost::weak_ptr<This> weak_ptr;
   HybridBayesTreeClique() {}
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index b942773cb..9592b0c69 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -18,9 +18,9 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -34,7 +34,7 @@
 
 namespace gtsam {
 
-class HybridFactorGraph;
+class GaussianHybridFactorGraph;
 
 /**
  * Hybrid Conditional Density
@@ -146,7 +146,8 @@ class GTSAM_EXPORT HybridConditional
    * @return DiscreteConditional::shared_ptr
    */
   DiscreteConditional::shared_ptr asDiscreteConditional() {
-    if (!isDiscrete()) throw std::invalid_argument("Not a discrete conditional");
+    if (!isDiscrete())
+      throw std::invalid_argument("Not a discrete conditional");
     return boost::static_pointer_cast<DiscreteConditional>(inner_);
   }
 
diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp
index ecac96724..148685327 100644
--- a/gtsam/hybrid/HybridEliminationTree.cpp
+++ b/gtsam/hybrid/HybridEliminationTree.cpp
@@ -21,17 +21,17 @@
 namespace gtsam {
 
 // Instantiate base class
-template class EliminationTree<HybridBayesNet, HybridFactorGraph>;
+template class EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>;
 
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
-    const HybridFactorGraph& factorGraph, const VariableIndex& structure,
-    const Ordering& order)
+    const GaussianHybridFactorGraph& factorGraph,
+    const VariableIndex& structure, const Ordering& order)
     : Base(factorGraph, structure, order) {}
 
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
-    const HybridFactorGraph& factorGraph, const Ordering& order)
+    const GaussianHybridFactorGraph& factorGraph, const Ordering& order)
     : Base(factorGraph, order) {}
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
index 27766724a..04bd9cd35 100644
--- a/gtsam/hybrid/HybridEliminationTree.h
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -17,8 +17,8 @@
 
 #pragma once
 
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/EliminationTree.h>
 
 namespace gtsam {
@@ -27,12 +27,12 @@ namespace gtsam {
  * Elimination Tree type for Hybrid
  */
 class GTSAM_EXPORT HybridEliminationTree
-    : public EliminationTree<HybridBayesNet, HybridFactorGraph> {
+    : public EliminationTree<HybridBayesNet, GaussianHybridFactorGraph> {
  private:
   friend class ::EliminationTreeTester;
 
  public:
-  typedef EliminationTree<HybridBayesNet, HybridFactorGraph>
+  typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
       Base;                                    ///< Base class
   typedef HybridEliminationTree This;          ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
@@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree
    * named constructor instead.
    * @return The elimination tree
    */
-  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+  HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
                         const VariableIndex& structure, const Ordering& order);
 
   /** Build the elimination tree of a factor graph.  Note that this has to
@@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree
    * this precomputed, use the other constructor instead.
    * @param factorGraph The factor graph for which to build the elimination tree
    */
-  HybridEliminationTree(const HybridFactorGraph& factorGraph,
+  HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
                         const Ordering& order);
 
   /// @}
diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp
index 057f784ad..476149126 100644
--- a/gtsam/hybrid/HybridISAM.cpp
+++ b/gtsam/hybrid/HybridISAM.cpp
@@ -17,8 +17,8 @@
  * @author Richard Roberts
  */
 
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridISAM.h>
 #include <gtsam/inference/ISAM-inst.h>
 #include <gtsam/inference/Key.h>
@@ -37,7 +37,7 @@ HybridISAM::HybridISAM() {}
 HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
 
 /* ************************************************************************* */
-void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
+void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors,
                                 HybridBayesTree::Cliques* orphans,
                                 const HybridBayesTree::Eliminate& function) {
   // Remove the contaminated part of the Bayes tree
@@ -90,7 +90,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors,
 }
 
 /* ************************************************************************* */
-void HybridISAM::update(const HybridFactorGraph& newFactors,
+void HybridISAM::update(const GaussianHybridFactorGraph& newFactors,
                         const HybridBayesTree::Eliminate& function) {
   Cliques orphans;
   this->updateInternal(newFactors, &orphans, function);
diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h
index 0738f46c8..fae7efafd 100644
--- a/gtsam/hybrid/HybridISAM.h
+++ b/gtsam/hybrid/HybridISAM.h
@@ -20,8 +20,8 @@
 #pragma once
 
 #include <gtsam/base/Testable.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/ISAM.h>
 
 namespace gtsam {
@@ -46,7 +46,8 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
  private:
   /// Internal method that performs the ISAM update.
   void updateInternal(
-      const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans,
+      const GaussianHybridFactorGraph& newFactors,
+      HybridBayesTree::Cliques* orphans,
       const HybridBayesTree::Eliminate& function =
           HybridBayesTree::EliminationTraitsType::DefaultEliminate);
 
@@ -57,7 +58,7 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
    * @param newFactors Factor graph of new factors to add and eliminate.
    * @param function Elimination function.
    */
-  void update(const HybridFactorGraph& newFactors,
+  void update(const GaussianHybridFactorGraph& newFactors,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
 };
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
index 981cd6f32..8fa3aa033 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -15,8 +15,8 @@
  * @author Fan Jiang
  */
 
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/JunctionTree-inst.h>
 #include <gtsam/inference/Key.h>
@@ -26,13 +26,17 @@
 namespace gtsam {
 
 // Instantiate base classes
-template class EliminatableClusterTree<HybridBayesTree, HybridFactorGraph>;
-template class JunctionTree<HybridBayesTree, HybridFactorGraph>;
+template class EliminatableClusterTree<HybridBayesTree,
+                                       GaussianHybridFactorGraph>;
+template class JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>;
 
 struct HybridConstructorTraversalData {
-  typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::Node Node;
-  typedef typename JunctionTree<HybridBayesTree, HybridFactorGraph>::sharedNode
-      sharedNode;
+  typedef
+      typename JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>::Node
+          Node;
+  typedef
+      typename JunctionTree<HybridBayesTree,
+                            GaussianHybridFactorGraph>::sharedNode sharedNode;
 
   HybridConstructorTraversalData* const parentData;
   sharedNode myJTNode;
diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h
index 824fa4f85..ce9b818e6 100644
--- a/gtsam/hybrid/HybridJunctionTree.h
+++ b/gtsam/hybrid/HybridJunctionTree.h
@@ -17,8 +17,8 @@
 
 #pragma once
 
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/JunctionTree.h>
 
 namespace gtsam {
@@ -49,9 +49,9 @@ class HybridEliminationTree;
  * \nosubgrouping
  */
 class GTSAM_EXPORT HybridJunctionTree
-    : public JunctionTree<HybridBayesTree, HybridFactorGraph> {
+    : public JunctionTree<HybridBayesTree, GaussianHybridFactorGraph> {
  public:
-  typedef JunctionTree<HybridBayesTree, HybridFactorGraph>
+  typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
       Base;                                    ///< Base class
   typedef HybridJunctionTree This;             ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index d3ff98719..612f3abc5 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -98,15 +98,15 @@ class HybridBayesNet {
       const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
 };
 
-#include <gtsam/hybrid/HybridFactorGraph.h>
-class HybridFactorGraph {
-  HybridFactorGraph();
-  HybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
+class GaussianHybridFactorGraph {
+  GaussianHybridFactorGraph();
+  GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
 
   // Building the graph
   void push_back(const gtsam::HybridFactor* factor);
   void push_back(const gtsam::HybridConditional* conditional);
-  void push_back(const gtsam::HybridFactorGraph& graph);
+  void push_back(const gtsam::GaussianHybridFactorGraph& graph);
   void push_back(const gtsam::HybridBayesNet& bayesNet);
   void push_back(const gtsam::HybridBayesTree& bayesTree);
   void push_back(const gtsam::GaussianMixtureFactor* gmm);
@@ -120,13 +120,13 @@ class HybridFactorGraph {
   const gtsam::HybridFactor* at(size_t i) const;
 
   void print(string s = "") const;
-  bool equals(const gtsam::HybridFactorGraph& fg, double tol = 1e-9) const;
+  bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const;
 
   gtsam::HybridBayesNet* eliminateSequential();
   gtsam::HybridBayesNet* eliminateSequential(
       gtsam::Ordering::OrderingType type);
   gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
-  pair<gtsam::HybridBayesNet*, gtsam::HybridFactorGraph*>
+  pair<gtsam::HybridBayesNet*, gtsam::GaussianHybridFactorGraph*>
   eliminatePartialSequential(const gtsam::Ordering& ordering);
 
   gtsam::HybridBayesTree* eliminateMultifrontal();
@@ -134,7 +134,7 @@ class HybridFactorGraph {
       gtsam::Ordering::OrderingType type);
   gtsam::HybridBayesTree* eliminateMultifrontal(
       const gtsam::Ordering& ordering);
-  pair<gtsam::HybridBayesTree*, gtsam::HybridFactorGraph*>
+  pair<gtsam::HybridBayesTree*, gtsam::GaussianHybridFactorGraph*>
   eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 
   string dot(
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 1a4db7898..77d8182c8 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -18,8 +18,8 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 
@@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C;
 using gtsam::symbol_shorthand::X;
 
 namespace gtsam {
-inline HybridFactorGraph::shared_ptr makeSwitchingChain(
+inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
     size_t n, std::function<Key(int)> keyFunc = X,
     std::function<Key(int)> dKeyFunc = C) {
-  HybridFactorGraph hfg;
+  GaussianHybridFactorGraph hfg;
 
   hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
 
@@ -51,7 +51,7 @@ inline HybridFactorGraph::shared_ptr makeSwitchingChain(
     }
   }
 
-  return boost::make_shared<HybridFactorGraph>(std::move(hfg));
+  return boost::make_shared<GaussianHybridFactorGraph>(std::move(hfg));
 }
 
 inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
diff --git a/gtsam/hybrid/tests/testHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
similarity index 93%
rename from gtsam/hybrid/tests/testHybridFactorGraph.cpp
rename to gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
index 860fdfdb8..853353278 100644
--- a/gtsam/hybrid/tests/testHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- *  @file testHybridFactorGraph.cpp
+ *  @file testGaussianHybridFactorGraph.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
  */
@@ -20,6 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
@@ -27,7 +28,6 @@
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridFactor.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridISAM.h>
 #include <gtsam/inference/BayesNet.h>
@@ -72,10 +72,10 @@ void my_signal_handler(int signum) {
 #endif
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, creation) {
+TEST(GaussianHybridFactorGraph, creation) {
   HybridConditional test;
 
-  HybridFactorGraph hfg;
+  GaussianHybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
@@ -91,8 +91,8 @@ TEST(HybridFactorGraph, creation) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminate) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminate) {
+  GaussianHybridFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
@@ -102,8 +102,8 @@ TEST(HybridFactorGraph, eliminate) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminateMultifrontal) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
+  GaussianHybridFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
 
@@ -119,8 +119,8 @@ TEST(HybridFactorGraph, eliminateMultifrontal) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
+  GaussianHybridFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -143,8 +143,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialEqualChance) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
+  GaussianHybridFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -171,8 +171,8 @@ TEST(HybridFactorGraph, eliminateFullSequentialSimple) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
+  GaussianHybridFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -204,8 +204,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalSimple) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
+  GaussianHybridFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
 
@@ -240,8 +240,8 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalCLG) {
  * This test is about how to assemble the Bayes Tree roots after we do partial
  * elimination
  */
-TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
-  HybridFactorGraph hfg;
+TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
+  GaussianHybridFactorGraph hfg;
 
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
@@ -290,7 +290,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  HybridFactorGraph::shared_ptr remaining;
+  GaussianHybridFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
 
   GTSAM_PRINT(*hbt);
@@ -309,7 +309,7 @@ TEST(HybridFactorGraph, eliminateFullMultifrontalTwoClique) {
 
 /* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
-TEST(HybridFactorGraph, Switching) {
+TEST(GaussianHybridFactorGraph, Switching) {
   auto N = 12;
   auto hfg = makeSwitchingChain(N);
 
@@ -381,7 +381,7 @@ TEST(HybridFactorGraph, Switching) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  HybridFactorGraph::shared_ptr remaining;
+  GaussianHybridFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
   // GTSAM_PRINT(*hbt);
@@ -417,7 +417,7 @@ TEST(HybridFactorGraph, Switching) {
 
 /* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
-TEST(HybridFactorGraph, SwitchingISAM) {
+TEST(GaussianHybridFactorGraph, SwitchingISAM) {
   auto N = 11;
   auto hfg = makeSwitchingChain(N);
 
@@ -473,7 +473,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  HybridFactorGraph::shared_ptr remaining;
+  GaussianHybridFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
   // GTSAM_PRINT(*hbt);
@@ -502,7 +502,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
   auto isam = HybridISAM(*hbt);
 
   {
-    HybridFactorGraph factorGraph;
+    GaussianHybridFactorGraph factorGraph;
     factorGraph.push_back(new_fg->at(new_fg->size() - 2));
     factorGraph.push_back(new_fg->at(new_fg->size() - 1));
     isam.update(factorGraph);
@@ -512,7 +512,7 @@ TEST(HybridFactorGraph, SwitchingISAM) {
 }
 
 /* ************************************************************************* */
-TEST(HybridFactorGraph, SwitchingTwoVar) {
+TEST(GaussianHybridFactorGraph, SwitchingTwoVar) {
   const int N = 7;
   auto hfg = makeSwitchingChain(N, X);
   hfg->push_back(*makeSwitchingChain(N, Y, D));
@@ -582,7 +582,7 @@ TEST(HybridFactorGraph, SwitchingTwoVar) {
   }
   {
     HybridBayesNet::shared_ptr hbn;
-    HybridFactorGraph::shared_ptr remaining;
+    GaussianHybridFactorGraph::shared_ptr remaining;
     std::tie(hbn, remaining) =
         hfg->eliminatePartialSequential(ordering_partial);
 
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index bbf1b2daa..e8d918a1d 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -9,7 +9,7 @@ namespace gtsam {
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 
 #include <gtsam/inference/Key.h>
 
@@ -107,36 +107,36 @@ class Ordering {
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering ColamdConstrainedLast(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering ColamdConstrainedFirst(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
   static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
                                 const FACTOR_GRAPH& graph);
 
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 144183816..895c9e14e 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X
 from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestHybridFactorGraph(GtsamTestCase):
-    """Unit tests for HybridFactorGraph."""
+class TestGaussianHybridFactorGraph(GtsamTestCase):
+    """Unit tests for GaussianHybridFactorGraph."""
 
     def test_create(self):
         """Test contruction of hybrid factor graph."""
@@ -36,13 +36,14 @@ class TestHybridFactorGraph(GtsamTestCase):
 
         gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.HybridFactorGraph()
+        hfg = gtsam.GaussianHybridFactorGraph()
         hfg.add(jf1)
         hfg.add(jf2)
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastHybridFactorGraph(hfg, [C(0)]))
+            gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph(
+                hfg, [C(0)]))
 
         # print("hbn = ", hbn)
         self.assertEqual(hbn.size(), 2)

From 841e6b01df76685ca8357305906731d32e2963b4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 28 May 2022 18:03:52 -0400
Subject: [PATCH 124/175] improved equality checks

---
 gtsam/hybrid/GaussianMixtureConditional.cpp |  3 ++-
 gtsam/hybrid/GaussianMixtureFactor.cpp      |  3 ++-
 gtsam/hybrid/GaussianMixtureFactor.h        | 10 ++++++----
 gtsam/hybrid/HybridConditional.cpp          |  3 ++-
 gtsam/hybrid/HybridDiscreteFactor.cpp       |  4 +++-
 gtsam/hybrid/HybridFactor.cpp               |  5 ++++-
 gtsam/hybrid/HybridGaussianFactor.cpp       | 15 ++++++++++-----
 gtsam/hybrid/HybridGaussianFactor.h         |  3 ++-
 8 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixtureConditional.cpp
index 726af6d5f..81d1244c2 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.cpp
+++ b/gtsam/hybrid/GaussianMixtureConditional.cpp
@@ -79,7 +79,8 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const {
 /* *******************************************************************************/
 bool GaussianMixtureConditional::equals(const HybridFactor &lf,
                                         double tol) const {
-  return BaseFactor::equals(lf, tol);
+  const This *e = dynamic_cast<const This *>(&lf);
+  return e != nullptr && BaseFactor::equals(*e, tol);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index 589e5c660..a81cf341d 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -34,7 +34,8 @@ GaussianMixtureFactor::GaussianMixtureFactor(const KeyVector &continuousKeys,
 
 /* *******************************************************************************/
 bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
-  return Base::equals(lf, tol);
+  const This *e = dynamic_cast<const This *>(&lf);
+  return e != nullptr && Base::equals(*e, tol);
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index b2fbe4aef..b3569183b 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -11,7 +11,7 @@
 
 /**
  * @file   GaussianMixtureFactor.h
- * @brief  A factor that is a function of discrete and continuous variables.
+ * @brief  A set of GaussianFactors, indexed by a set of discrete keys.
  * @author Fan Jiang
  * @author Varun Agrawal
  * @author Frank Dellaert
@@ -32,9 +32,10 @@ class GaussianFactorGraph;
 using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
 
 /**
- * @brief A linear factor that is a function of both discrete and continuous
- * variables, i.e. P(X, M | Z) where X is the set of continuous variables, M is
- * the set of discrete variables and Z is the measurement set.
+ * @brief Implementation of a discrete conditional mixture factor.
+ * Implements a joint discrete-continuous factor where the discrete variable
+ * serves to "select" a mixture component corresponding to a GaussianFactor type
+ * of measurement.
  *
  * Represents the underlying Gaussian Mixture as a Decision Tree, where the set
  * of discrete variables indexes to the continuous gaussian distribution.
@@ -52,6 +53,7 @@ class GaussianMixtureFactor : public HybridFactor {
   using Factors = DecisionTree<Key, GaussianFactor::shared_ptr>;
 
  private:
+  /// Decision tree of Gaussian factors indexed by discrete keys.
   Factors factors_;
 
   /**
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 7d1b72067..e3932308b 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -101,7 +101,8 @@ void HybridConditional::print(const std::string &s,
 
 /* ************************************************************************ */
 bool HybridConditional::equals(const HybridFactor &other, double tol) const {
-  return BaseFactor::equals(other, tol);
+  const This *e = dynamic_cast<const This *>(&other);
+  return e != nullptr && BaseFactor::equals(*e, tol);
 }
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 989127a28..2bdcdee8c 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -38,7 +38,9 @@ HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
 
 /* ************************************************************************ */
 bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
-  return Base::equals(lf, tol);
+  const This *e = dynamic_cast<const This *>(&lf);
+  // TODO(Varun) How to compare inner_ when they are abstract types?
+  return e != nullptr && Base::equals(*e, tol);
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 815ea0415..127c9761c 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -70,7 +70,10 @@ HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
 
 /* ************************************************************************ */
 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
-  return Base::equals(lf, tol);
+  const This *e = dynamic_cast<const This *>(&lf);
+  return e != nullptr && Base::equals(*e, tol) &&
+         isDiscrete_ == e->isDiscrete_ && isContinuous_ == e->isContinuous_ &&
+         isHybrid_ == e->isHybrid_ && nrContinuous_ == e->nrContinuous_;
 }
 
 /* ************************************************************************ */
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 721cb4cc7..59d20fb79 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -21,18 +21,23 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
 HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
-    : Base(other->keys()) {
-  inner_ = other;
-}
+    : Base(other->keys()), inner_(other) {}
 
+/* ************************************************************************* */
 HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
     : Base(jf.keys()),
       inner_(boost::make_shared<JacobianFactor>(std::move(jf))) {}
 
-bool HybridGaussianFactor::equals(const HybridFactor &lf, double tol) const {
-  return false;
+/* ************************************************************************* */
+bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
+  const This *e = dynamic_cast<const This *>(&other);
+  // TODO(Varun) How to compare inner_ when they are abstract types?
+  return e != nullptr && Base::equals(*e, tol);
 }
+
+/* ************************************************************************* */
 void HybridGaussianFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 8d457e778..6fa83b726 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -25,7 +25,8 @@ namespace gtsam {
 
 /**
  * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
- * a diamond inheritance.
+ * a diamond inheritance i.e. an extra factor type that inherits from both
+ * HybridFactor and GaussianFactor.
  */
 class HybridGaussianFactor : public HybridFactor {
  private:

From 7c7b5dd03096c1e1516a964563b0b840a2b77a45 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 1 Jun 2022 23:52:21 -0400
Subject: [PATCH 125/175] Rename files so that everything is HybridX

---
 gtsam/hybrid/HybridBayesTree.cpp              |  5 +-
 gtsam/hybrid/HybridBayesTree.h                |  9 +--
 gtsam/hybrid/HybridConditional.h              |  4 +-
 gtsam/hybrid/HybridEliminationTree.cpp        |  6 +-
 gtsam/hybrid/HybridEliminationTree.h          | 10 +--
 ...raph.cpp => HybridGaussianFactorGraph.cpp} | 24 +++----
 ...torGraph.h => HybridGaussianFactorGraph.h} | 33 ++++-----
 ...{HybridISAM.cpp => HybridGaussianISAM.cpp} | 22 +++---
 .../{HybridISAM.h => HybridGaussianISAM.h}    | 18 ++---
 ...ree.cpp => HybridGaussianJunctionTree.cpp} | 70 +++++++++----------
 ...ionTree.h => HybridGaussianJunctionTree.h} | 14 ++--
 gtsam/hybrid/hybrid.i                         | 16 ++---
 gtsam/hybrid/tests/Switching.h                |  8 +--
 .../tests/testGaussianHybridFactorGraph.cpp   | 70 ++++++++-----------
 gtsam/inference/inference.i                   | 14 ++--
 python/gtsam/tests/test_HybridFactorGraph.py  |  8 +--
 16 files changed, 161 insertions(+), 170 deletions(-)
 rename gtsam/hybrid/{GaussianHybridFactorGraph.cpp => HybridGaussianFactorGraph.cpp} (94%)
 rename gtsam/hybrid/{GaussianHybridFactorGraph.h => HybridGaussianFactorGraph.h} (77%)
 rename gtsam/hybrid/{HybridISAM.cpp => HybridGaussianISAM.cpp} (84%)
 rename gtsam/hybrid/{HybridISAM.h => HybridGaussianISAM.h} (76%)
 rename gtsam/hybrid/{HybridJunctionTree.cpp => HybridGaussianJunctionTree.cpp} (73%)
 rename gtsam/hybrid/{HybridJunctionTree.h => HybridGaussianJunctionTree.h} (84%)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index 400cac5e7..ff07f1817 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -11,7 +11,8 @@
 
 /**
  * @file    HybridBayesTree.cpp
- * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
+ * @brief   Hybrid Bayes Tree, the result of eliminating a
+ * HybridGaussianJunctionTree
  * @date Mar 11, 2022
  * @author  Fan Jiang
  */
@@ -26,7 +27,7 @@ namespace gtsam {
 
 // Instantiate base class
 template class BayesTreeCliqueBase<HybridBayesTreeClique,
-                                   GaussianHybridFactorGraph>;
+                                   HybridGaussianFactorGraph>;
 template class BayesTree<HybridBayesTreeClique>;
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 610181458..28d9ef809 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -11,15 +11,16 @@
 
 /**
  * @file    HybridBayesTree.h
- * @brief   Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree
+ * @brief   Hybrid Bayes Tree, the result of eliminating a
+ * HybridGaussianJunctionTree
  * @date Mar 11, 2022
  * @author  Fan Jiang
  */
 
 #pragma once
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/inference/Conditional.h>
@@ -38,10 +39,10 @@ class VectorValues;
  */
 class GTSAM_EXPORT HybridBayesTreeClique
     : public BayesTreeCliqueBase<HybridBayesTreeClique,
-                                 GaussianHybridFactorGraph> {
+                                 HybridGaussianFactorGraph> {
  public:
   typedef HybridBayesTreeClique This;
-  typedef BayesTreeCliqueBase<HybridBayesTreeClique, GaussianHybridFactorGraph>
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
       Base;
   typedef boost::shared_ptr<This> shared_ptr;
   typedef boost::weak_ptr<This> weak_ptr;
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 9592b0c69..887b49f12 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -18,9 +18,9 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -34,7 +34,7 @@
 
 namespace gtsam {
 
-class GaussianHybridFactorGraph;
+class HybridGaussianFactorGraph;
 
 /**
  * Hybrid Conditional Density
diff --git a/gtsam/hybrid/HybridEliminationTree.cpp b/gtsam/hybrid/HybridEliminationTree.cpp
index 148685327..c2df2dd60 100644
--- a/gtsam/hybrid/HybridEliminationTree.cpp
+++ b/gtsam/hybrid/HybridEliminationTree.cpp
@@ -21,17 +21,17 @@
 namespace gtsam {
 
 // Instantiate base class
-template class EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>;
+template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
 
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
-    const GaussianHybridFactorGraph& factorGraph,
+    const HybridGaussianFactorGraph& factorGraph,
     const VariableIndex& structure, const Ordering& order)
     : Base(factorGraph, structure, order) {}
 
 /* ************************************************************************* */
 HybridEliminationTree::HybridEliminationTree(
-    const GaussianHybridFactorGraph& factorGraph, const Ordering& order)
+    const HybridGaussianFactorGraph& factorGraph, const Ordering& order)
     : Base(factorGraph, order) {}
 
 /* ************************************************************************* */
diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h
index 04bd9cd35..77a84fea8 100644
--- a/gtsam/hybrid/HybridEliminationTree.h
+++ b/gtsam/hybrid/HybridEliminationTree.h
@@ -17,8 +17,8 @@
 
 #pragma once
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/EliminationTree.h>
 
 namespace gtsam {
@@ -27,12 +27,12 @@ namespace gtsam {
  * Elimination Tree type for Hybrid
  */
 class GTSAM_EXPORT HybridEliminationTree
-    : public EliminationTree<HybridBayesNet, GaussianHybridFactorGraph> {
+    : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
  private:
   friend class ::EliminationTreeTester;
 
  public:
-  typedef EliminationTree<HybridBayesNet, GaussianHybridFactorGraph>
+  typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
       Base;                                    ///< Base class
   typedef HybridEliminationTree This;          ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
@@ -49,7 +49,7 @@ class GTSAM_EXPORT HybridEliminationTree
    * named constructor instead.
    * @return The elimination tree
    */
-  HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
+  HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
                         const VariableIndex& structure, const Ordering& order);
 
   /** Build the elimination tree of a factor graph.  Note that this has to
@@ -57,7 +57,7 @@ class GTSAM_EXPORT HybridEliminationTree
    * this precomputed, use the other constructor instead.
    * @param factorGraph The factor graph for which to build the elimination tree
    */
-  HybridEliminationTree(const GaussianHybridFactorGraph& factorGraph,
+  HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
                         const Ordering& order);
 
   /// @}
diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
similarity index 94%
rename from gtsam/hybrid/GaussianHybridFactorGraph.cpp
rename to gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 3354d5b4d..0ac2c2656 100644
--- a/gtsam/hybrid/GaussianHybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianHybridFactorGraph.cpp
+ * @file   HybridGaussianFactorGraph.cpp
  * @brief  Hybrid factor graph that uses type erasure
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -23,7 +23,6 @@
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
@@ -31,7 +30,8 @@
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
-#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -53,7 +53,7 @@
 
 namespace gtsam {
 
-template class EliminateableFactorGraph<GaussianHybridFactorGraph>;
+template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
 
 /* ************************************************************************ */
 static GaussianMixtureFactor::Sum &addGaussian(
@@ -78,7 +78,7 @@ static GaussianMixtureFactor::Sum &addGaussian(
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-continuousElimination(const GaussianHybridFactorGraph &factors,
+continuousElimination(const HybridGaussianFactorGraph &factors,
                       const Ordering &frontalKeys) {
   GaussianFactorGraph gfg;
   for (auto &fp : factors) {
@@ -103,7 +103,7 @@ continuousElimination(const GaussianHybridFactorGraph &factors,
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-discreteElimination(const GaussianHybridFactorGraph &factors,
+discreteElimination(const HybridGaussianFactorGraph &factors,
                     const Ordering &frontalKeys) {
   DiscreteFactorGraph dfg;
   for (auto &fp : factors) {
@@ -129,7 +129,7 @@ discreteElimination(const GaussianHybridFactorGraph &factors,
 
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
-hybridElimination(const GaussianHybridFactorGraph &factors,
+hybridElimination(const HybridGaussianFactorGraph &factors,
                   const Ordering &frontalKeys,
                   const KeySet &continuousSeparator,
                   const std::set<DiscreteKey> &discreteSeparatorSet) {
@@ -236,7 +236,7 @@ hybridElimination(const GaussianHybridFactorGraph &factors,
 }
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
-EliminateHybrid(const GaussianHybridFactorGraph &factors,
+EliminateHybrid(const HybridGaussianFactorGraph &factors,
                 const Ordering &frontalKeys) {
   // NOTE: Because we are in the Conditional Gaussian regime there are only
   // a few cases:
@@ -345,22 +345,22 @@ EliminateHybrid(const GaussianHybridFactorGraph &factors,
 }
 
 /* ************************************************************************ */
-void GaussianHybridFactorGraph::add(JacobianFactor &&factor) {
+void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
 }
 
 /* ************************************************************************ */
-void GaussianHybridFactorGraph::add(JacobianFactor::shared_ptr factor) {
+void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
 }
 
 /* ************************************************************************ */
-void GaussianHybridFactorGraph::add(DecisionTreeFactor &&factor) {
+void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
 }
 
 /* ************************************************************************ */
-void GaussianHybridFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
+void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
   FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
 }
 
diff --git a/gtsam/hybrid/GaussianHybridFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
similarity index 77%
rename from gtsam/hybrid/GaussianHybridFactorGraph.h
rename to gtsam/hybrid/HybridGaussianFactorGraph.h
index c8e0718fc..6944af510 100644
--- a/gtsam/hybrid/GaussianHybridFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianHybridFactorGraph.h
+ * @file   HybridGaussianFactorGraph.h
  * @brief  Linearized Hybrid factor graph that uses type erasure
  * @author Fan Jiang
  * @date   Mar 11, 2022
@@ -26,36 +26,37 @@
 namespace gtsam {
 
 // Forward declarations
-class GaussianHybridFactorGraph;
+class HybridGaussianFactorGraph;
 class HybridConditional;
 class HybridBayesNet;
 class HybridEliminationTree;
 class HybridBayesTree;
-class HybridJunctionTree;
+class HybridGaussianJunctionTree;
 class DecisionTreeFactor;
 
 class JacobianFactor;
 
-/** Main elimination function for GaussianHybridFactorGraph */
+/** Main elimination function for HybridGaussianFactorGraph */
 GTSAM_EXPORT
 std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
-EliminateHybrid(const GaussianHybridFactorGraph& factors, const Ordering& keys);
+EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
 
 /* ************************************************************************* */
 template <>
-struct EliminationTraits<GaussianHybridFactorGraph> {
+struct EliminationTraits<HybridGaussianFactorGraph> {
   typedef HybridFactor FactorType;  ///< Type of factors in factor graph
-  typedef GaussianHybridFactorGraph
+  typedef HybridGaussianFactorGraph
       FactorGraphType;  ///< Type of the factor graph (e.g.
-                        ///< GaussianHybridFactorGraph)
+                        ///< HybridGaussianFactorGraph)
   typedef HybridConditional
       ConditionalType;  ///< Type of conditionals from elimination
   typedef HybridBayesNet
       BayesNetType;  ///< Type of Bayes net from sequential elimination
   typedef HybridEliminationTree
-      EliminationTreeType;                      ///< Type of elimination tree
-  typedef HybridBayesTree BayesTreeType;        ///< Type of Bayes tree
-  typedef HybridJunctionTree JunctionTreeType;  ///< Type of Junction tree
+      EliminationTreeType;                ///< Type of elimination tree
+  typedef HybridBayesTree BayesTreeType;  ///< Type of Bayes tree
+  typedef HybridGaussianJunctionTree
+      JunctionTreeType;  ///< Type of Junction tree
   /// The default dense elimination function
   static std::pair<boost::shared_ptr<ConditionalType>,
                    boost::shared_ptr<FactorType> >
@@ -70,12 +71,12 @@ struct EliminationTraits<GaussianHybridFactorGraph> {
  * This is the linearized version of a hybrid factor graph.
  * Everything inside needs to be hybrid factor or hybrid conditional.
  */
-class GaussianHybridFactorGraph
+class HybridGaussianFactorGraph
     : public FactorGraph<HybridFactor>,
-      public EliminateableFactorGraph<GaussianHybridFactorGraph> {
+      public EliminateableFactorGraph<HybridGaussianFactorGraph> {
  public:
   using Base = FactorGraph<HybridFactor>;
-  using This = GaussianHybridFactorGraph;  ///< this class
+  using This = HybridGaussianFactorGraph;  ///< this class
   using BaseEliminateable =
       EliminateableFactorGraph<This>;          ///< for elimination
   using shared_ptr = boost::shared_ptr<This>;  ///< shared_ptr to This
@@ -86,7 +87,7 @@ class GaussianHybridFactorGraph
   /// @name Constructors
   /// @{
 
-  GaussianHybridFactorGraph() = default;
+  HybridGaussianFactorGraph() = default;
 
   /**
    * Implicit copy/downcast constructor to override explicit template container
@@ -94,7 +95,7 @@ class GaussianHybridFactorGraph
    * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
    * */
   template <class DERIVEDFACTOR>
-  GaussianHybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
+  HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
       : Base(graph) {}
 
   /// @}
diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
similarity index 84%
rename from gtsam/hybrid/HybridISAM.cpp
rename to gtsam/hybrid/HybridGaussianISAM.cpp
index 476149126..7783a88dd 100644
--- a/gtsam/hybrid/HybridISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -10,16 +10,16 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridISAM.h
+ * @file HybridGaussianISAM.h
  * @date March 31, 2022
  * @author Fan Jiang
  * @author Frank Dellaert
  * @author Richard Roberts
  */
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
-#include <gtsam/hybrid/HybridISAM.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianISAM.h>
 #include <gtsam/inference/ISAM-inst.h>
 #include <gtsam/inference/Key.h>
 
@@ -31,15 +31,17 @@ namespace gtsam {
 // template class ISAM<HybridBayesTree>;
 
 /* ************************************************************************* */
-HybridISAM::HybridISAM() {}
+HybridGaussianISAM::HybridGaussianISAM() {}
 
 /* ************************************************************************* */
-HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {}
+HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
+    : Base(bayesTree) {}
 
 /* ************************************************************************* */
-void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors,
-                                HybridBayesTree::Cliques* orphans,
-                                const HybridBayesTree::Eliminate& function) {
+void HybridGaussianISAM::updateInternal(
+    const HybridGaussianFactorGraph& newFactors,
+    HybridBayesTree::Cliques* orphans,
+    const HybridBayesTree::Eliminate& function) {
   // Remove the contaminated part of the Bayes tree
   BayesNetType bn;
   const KeySet newFactorKeys = newFactors.keys();
@@ -90,8 +92,8 @@ void HybridISAM::updateInternal(const GaussianHybridFactorGraph& newFactors,
 }
 
 /* ************************************************************************* */
-void HybridISAM::update(const GaussianHybridFactorGraph& newFactors,
-                        const HybridBayesTree::Eliminate& function) {
+void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
+                                const HybridBayesTree::Eliminate& function) {
   Cliques orphans;
   this->updateInternal(newFactors, &orphans, function);
 }
diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridGaussianISAM.h
similarity index 76%
rename from gtsam/hybrid/HybridISAM.h
rename to gtsam/hybrid/HybridGaussianISAM.h
index fae7efafd..d5b6271da 100644
--- a/gtsam/hybrid/HybridISAM.h
+++ b/gtsam/hybrid/HybridGaussianISAM.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridISAM.h
+ * @file HybridGaussianISAM.h
  * @date March 31, 2022
  * @author Fan Jiang
  * @author Frank Dellaert
@@ -20,33 +20,33 @@
 #pragma once
 
 #include <gtsam/base/Testable.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/ISAM.h>
 
 namespace gtsam {
 
-class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
+class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
  public:
   typedef ISAM<HybridBayesTree> Base;
-  typedef HybridISAM This;
+  typedef HybridGaussianISAM This;
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// @name Standard Constructors
   /// @{
 
   /** Create an empty Bayes Tree */
-  HybridISAM();
+  HybridGaussianISAM();
 
   /** Copy constructor */
-  HybridISAM(const HybridBayesTree& bayesTree);
+  HybridGaussianISAM(const HybridBayesTree& bayesTree);
 
   /// @}
 
  private:
   /// Internal method that performs the ISAM update.
   void updateInternal(
-      const GaussianHybridFactorGraph& newFactors,
+      const HybridGaussianFactorGraph& newFactors,
       HybridBayesTree::Cliques* orphans,
       const HybridBayesTree::Eliminate& function =
           HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@@ -58,13 +58,13 @@ class GTSAM_EXPORT HybridISAM : public ISAM<HybridBayesTree> {
    * @param newFactors Factor graph of new factors to add and eliminate.
    * @param function Elimination function.
    */
-  void update(const GaussianHybridFactorGraph& newFactors,
+  void update(const HybridGaussianFactorGraph& newFactors,
               const HybridBayesTree::Eliminate& function =
                   HybridBayesTree::EliminationTraitsType::DefaultEliminate);
 };
 
 /// traits
 template <>
-struct traits<HybridISAM> : public Testable<HybridISAM> {};
+struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridGaussianJunctionTree.cpp
similarity index 73%
rename from gtsam/hybrid/HybridJunctionTree.cpp
rename to gtsam/hybrid/HybridGaussianJunctionTree.cpp
index 8fa3aa033..8ceb7c87b 100644
--- a/gtsam/hybrid/HybridJunctionTree.cpp
+++ b/gtsam/hybrid/HybridGaussianJunctionTree.cpp
@@ -10,14 +10,14 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridJunctionTree.cpp
+ * @file HybridGaussianJunctionTree.cpp
  * @date Mar 11, 2022
  * @author Fan Jiang
  */
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridEliminationTree.h>
-#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
 #include <gtsam/inference/JunctionTree-inst.h>
 #include <gtsam/inference/Key.h>
 
@@ -27,19 +27,19 @@ namespace gtsam {
 
 // Instantiate base classes
 template class EliminatableClusterTree<HybridBayesTree,
-                                       GaussianHybridFactorGraph>;
-template class JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>;
+                                       HybridGaussianFactorGraph>;
+template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
 
 struct HybridConstructorTraversalData {
   typedef
-      typename JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>::Node
+      typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
           Node;
   typedef
       typename JunctionTree<HybridBayesTree,
-                            GaussianHybridFactorGraph>::sharedNode sharedNode;
+                            HybridGaussianFactorGraph>::sharedNode sharedNode;
 
   HybridConstructorTraversalData* const parentData;
-  sharedNode myJTNode;
+  sharedNode junctionTreeNode;
   FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
   FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
   KeySet discreteKeys;
@@ -57,24 +57,24 @@ struct HybridConstructorTraversalData {
     // On the pre-order pass, before children have been visited, we just set up
     // a traversal data structure with its own JT node, and create a child
     // pointer in its parent.
-    HybridConstructorTraversalData myData =
+    HybridConstructorTraversalData data =
         HybridConstructorTraversalData(&parentData);
-    myData.myJTNode = boost::make_shared<Node>(node->key, node->factors);
-    parentData.myJTNode->addChild(myData.myJTNode);
+    data.junctionTreeNode = boost::make_shared<Node>(node->key, node->factors);
+    parentData.junctionTreeNode->addChild(data.junctionTreeNode);
 
     for (HybridFactor::shared_ptr& f : node->factors) {
       for (auto& k : f->discreteKeys()) {
-        myData.discreteKeys.insert(k.first);
+        data.discreteKeys.insert(k.first);
       }
     }
 
-    return myData;
+    return data;
   }
 
   // Post-order visitor function
   static void ConstructorTraversalVisitorPostAlg2(
       const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
-      const HybridConstructorTraversalData& myData) {
+      const HybridConstructorTraversalData& data) {
     // In this post-order visitor, we combine the symbolic elimination results
     // from the elimination tree children and symbolically eliminate the current
     // elimination tree node.  We then check whether each of our elimination
@@ -87,50 +87,50 @@ struct HybridConstructorTraversalData {
     // Do symbolic elimination for this node
     SymbolicFactors symbolicFactors;
     symbolicFactors.reserve(ETreeNode->factors.size() +
-                            myData.childSymbolicFactors.size());
+                            data.childSymbolicFactors.size());
     // Add ETree node factors
     symbolicFactors += ETreeNode->factors;
     // Add symbolic factors passed up from children
-    symbolicFactors += myData.childSymbolicFactors;
+    symbolicFactors += data.childSymbolicFactors;
 
     Ordering keyAsOrdering;
     keyAsOrdering.push_back(ETreeNode->key);
-    SymbolicConditional::shared_ptr myConditional;
-    SymbolicFactor::shared_ptr mySeparatorFactor;
-    boost::tie(myConditional, mySeparatorFactor) =
+    SymbolicConditional::shared_ptr conditional;
+    SymbolicFactor::shared_ptr separatorFactor;
+    boost::tie(conditional, separatorFactor) =
         internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
 
     // Store symbolic elimination results in the parent
-    myData.parentData->childSymbolicConditionals.push_back(myConditional);
-    myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
-    myData.parentData->discreteKeys.merge(myData.discreteKeys);
+    data.parentData->childSymbolicConditionals.push_back(conditional);
+    data.parentData->childSymbolicFactors.push_back(separatorFactor);
+    data.parentData->discreteKeys.merge(data.discreteKeys);
 
-    sharedNode node = myData.myJTNode;
+    sharedNode node = data.junctionTreeNode;
     const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
-        myData.childSymbolicConditionals;
-    node->problemSize_ = (int)(myConditional->size() * symbolicFactors.size());
+        data.childSymbolicConditionals;
+    node->problemSize_ = (int)(conditional->size() * symbolicFactors.size());
 
     // Merge our children if they are in our clique - if our conditional has
     // exactly one fewer parent than our child's conditional.
-    const size_t myNrParents = myConditional->nrParents();
+    const size_t nrParents = conditional->nrParents();
     const size_t nrChildren = node->nrChildren();
     assert(childConditionals.size() == nrChildren);
 
     // decide which children to merge, as index into children
-    std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
+    std::vector<size_t> nrChildrenFrontals = node->nrFrontalsOfChildren();
     std::vector<bool> merge(nrChildren, false);
-    size_t myNrFrontals = 1;
+    size_t nrFrontals = 1;
     for (size_t i = 0; i < nrChildren; i++) {
       // Check if we should merge the i^th child
-      if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
+      if (nrParents + nrFrontals == childConditionals[i]->nrParents()) {
         const bool myType =
-            myData.discreteKeys.exists(myConditional->frontals()[0]);
+            data.discreteKeys.exists(conditional->frontals()[0]);
         const bool theirType =
-            myData.discreteKeys.exists(childConditionals[i]->frontals()[0]);
+            data.discreteKeys.exists(childConditionals[i]->frontals()[0]);
 
         if (myType == theirType) {
           // Increment number of frontal variables
-          myNrFrontals += nrFrontals[i];
+          nrFrontals += nrChildrenFrontals[i];
           merge[i] = true;
         }
       }
@@ -142,7 +142,7 @@ struct HybridConstructorTraversalData {
 };
 
 /* ************************************************************************* */
-HybridJunctionTree::HybridJunctionTree(
+HybridGaussianJunctionTree::HybridGaussianJunctionTree(
     const HybridEliminationTree& eliminationTree) {
   gttic(JunctionTree_FromEliminationTree);
   // Here we rely on the BayesNet having been produced by this elimination tree,
@@ -156,7 +156,7 @@ HybridJunctionTree::HybridJunctionTree(
   // as we go.  Gather the created junction tree roots in a dummy Node.
   typedef HybridConstructorTraversalData Data;
   Data rootData(0);
-  rootData.myJTNode =
+  rootData.junctionTreeNode =
       boost::make_shared<typename Base::Node>();  // Make a dummy node to gather
                                                   // the junction tree roots
   treeTraversal::DepthFirstForest(eliminationTree, rootData,
@@ -164,7 +164,7 @@ HybridJunctionTree::HybridJunctionTree(
                                   Data::ConstructorTraversalVisitorPostAlg2);
 
   // Assign roots from the dummy node
-  this->addChildrenAsRoots(rootData.myJTNode);
+  this->addChildrenAsRoots(rootData.junctionTreeNode);
 
   // Transfer remaining factors from elimination tree
   Base::remainingFactors_ = eliminationTree.remainingFactors();
diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridGaussianJunctionTree.h
similarity index 84%
rename from gtsam/hybrid/HybridJunctionTree.h
rename to gtsam/hybrid/HybridGaussianJunctionTree.h
index ce9b818e6..314e7daa6 100644
--- a/gtsam/hybrid/HybridJunctionTree.h
+++ b/gtsam/hybrid/HybridGaussianJunctionTree.h
@@ -10,15 +10,15 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridJunctionTree.h
+ * @file HybridGaussianJunctionTree.h
  * @date Mar 11, 2022
  * @author Fan Jiang
  */
 
 #pragma once
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/JunctionTree.h>
 
 namespace gtsam {
@@ -48,12 +48,12 @@ class HybridEliminationTree;
  * \addtogroup Multifrontal
  * \nosubgrouping
  */
-class GTSAM_EXPORT HybridJunctionTree
-    : public JunctionTree<HybridBayesTree, GaussianHybridFactorGraph> {
+class GTSAM_EXPORT HybridGaussianJunctionTree
+    : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
  public:
-  typedef JunctionTree<HybridBayesTree, GaussianHybridFactorGraph>
+  typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
       Base;                                    ///< Base class
-  typedef HybridJunctionTree This;             ///< This class
+  typedef HybridGaussianJunctionTree This;     ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
   /**
@@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridJunctionTree
    * named constructor instead.
    * @return The elimination tree
    */
-  HybridJunctionTree(const HybridEliminationTree& eliminationTree);
+  HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 612f3abc5..94eaa88b2 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -98,15 +98,15 @@ class HybridBayesNet {
       const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
 };
 
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
-class GaussianHybridFactorGraph {
-  GaussianHybridFactorGraph();
-  GaussianHybridFactorGraph(const gtsam::HybridBayesNet& bayesNet);
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+class HybridGaussianFactorGraph {
+  HybridGaussianFactorGraph();
+  HybridGaussianFactorGraph(const gtsam::HybridBayesNet& bayesNet);
 
   // Building the graph
   void push_back(const gtsam::HybridFactor* factor);
   void push_back(const gtsam::HybridConditional* conditional);
-  void push_back(const gtsam::GaussianHybridFactorGraph& graph);
+  void push_back(const gtsam::HybridGaussianFactorGraph& graph);
   void push_back(const gtsam::HybridBayesNet& bayesNet);
   void push_back(const gtsam::HybridBayesTree& bayesTree);
   void push_back(const gtsam::GaussianMixtureFactor* gmm);
@@ -120,13 +120,13 @@ class GaussianHybridFactorGraph {
   const gtsam::HybridFactor* at(size_t i) const;
 
   void print(string s = "") const;
-  bool equals(const gtsam::GaussianHybridFactorGraph& fg, double tol = 1e-9) const;
+  bool equals(const gtsam::HybridGaussianFactorGraph& fg, double tol = 1e-9) const;
 
   gtsam::HybridBayesNet* eliminateSequential();
   gtsam::HybridBayesNet* eliminateSequential(
       gtsam::Ordering::OrderingType type);
   gtsam::HybridBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
-  pair<gtsam::HybridBayesNet*, gtsam::GaussianHybridFactorGraph*>
+  pair<gtsam::HybridBayesNet*, gtsam::HybridGaussianFactorGraph*>
   eliminatePartialSequential(const gtsam::Ordering& ordering);
 
   gtsam::HybridBayesTree* eliminateMultifrontal();
@@ -134,7 +134,7 @@ class GaussianHybridFactorGraph {
       gtsam::Ordering::OrderingType type);
   gtsam::HybridBayesTree* eliminateMultifrontal(
       const gtsam::Ordering& ordering);
-  pair<gtsam::HybridBayesTree*, gtsam::GaussianHybridFactorGraph*>
+  pair<gtsam::HybridBayesTree*, gtsam::HybridGaussianFactorGraph*>
   eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
 
   string dot(
diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h
index 77d8182c8..c081b8e87 100644
--- a/gtsam/hybrid/tests/Switching.h
+++ b/gtsam/hybrid/tests/Switching.h
@@ -18,8 +18,8 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/discrete/DecisionTreeFactor.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 
@@ -29,10 +29,10 @@ using gtsam::symbol_shorthand::C;
 using gtsam::symbol_shorthand::X;
 
 namespace gtsam {
-inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
+inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
     size_t n, std::function<Key(int)> keyFunc = X,
     std::function<Key(int)> dKeyFunc = C) {
-  GaussianHybridFactorGraph hfg;
+  HybridGaussianFactorGraph hfg;
 
   hfg.add(JacobianFactor(keyFunc(1), I_3x3, Z_3x1));
 
@@ -51,7 +51,7 @@ inline GaussianHybridFactorGraph::shared_ptr makeSwitchingChain(
     }
   }
 
-  return boost::make_shared<GaussianHybridFactorGraph>(std::move(hfg));
+  return boost::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
 }
 
 inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
index 853353278..8ff959d74 100644
--- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- *  @file testGaussianHybridFactorGraph.cpp
+ *  @file testHybridGaussianFactorGraph.cpp
  *  @date Mar 11, 2022
  *  @author Fan Jiang
  */
@@ -20,7 +20,6 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
 #include <gtsam/hybrid/GaussianMixtureConditional.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
@@ -29,7 +28,8 @@
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
-#include <gtsam/hybrid/HybridISAM.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianISAM.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/DotWriter.h>
 #include <gtsam/inference/Key.h>
@@ -57,25 +57,11 @@ using gtsam::symbol_shorthand::D;
 using gtsam::symbol_shorthand::X;
 using gtsam::symbol_shorthand::Y;
 
-#ifdef HYBRID_DEBUG
-#define BOOST_STACKTRACE_GNU_SOURCE_NOT_REQUIRED
-
-#include <signal.h>  // ::signal, ::raise
-
-#include <boost/stacktrace.hpp>
-
-void my_signal_handler(int signum) {
-  ::signal(signum, SIG_DFL);
-  std::cout << boost::stacktrace::stacktrace();
-  ::raise(SIGABRT);
-}
-#endif
-
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, creation) {
+TEST(HybridGaussianFactorGraph, creation) {
   HybridConditional test;
 
-  GaussianHybridFactorGraph hfg;
+  HybridGaussianFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
@@ -91,8 +77,8 @@ TEST(GaussianHybridFactorGraph, creation) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminate) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminate) {
+  HybridGaussianFactorGraph hfg;
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
@@ -102,8 +88,8 @@ TEST(GaussianHybridFactorGraph, eliminate) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
+  HybridGaussianFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
 
@@ -119,8 +105,8 @@ TEST(GaussianHybridFactorGraph, eliminateMultifrontal) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
+  HybridGaussianFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -143,8 +129,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialEqualChance) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
+  HybridGaussianFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -171,8 +157,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullSequentialSimple) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
+  HybridGaussianFactorGraph hfg;
 
   DiscreteKey c1(C(1), 2);
 
@@ -204,8 +190,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalSimple) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
+  HybridGaussianFactorGraph hfg;
 
   DiscreteKey c(C(1), 2);
 
@@ -240,8 +226,8 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalCLG) {
  * This test is about how to assemble the Bayes Tree roots after we do partial
  * elimination
  */
-TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
-  GaussianHybridFactorGraph hfg;
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) {
+  HybridGaussianFactorGraph hfg;
 
   hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));
   hfg.add(JacobianFactor(X(1), I_3x3, X(2), -I_3x3, Z_3x1));
@@ -290,7 +276,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  GaussianHybridFactorGraph::shared_ptr remaining;
+  HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg.eliminatePartialMultifrontal(ordering_full);
 
   GTSAM_PRINT(*hbt);
@@ -309,7 +295,7 @@ TEST(GaussianHybridFactorGraph, eliminateFullMultifrontalTwoClique) {
 
 /* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
-TEST(GaussianHybridFactorGraph, Switching) {
+TEST(HybridGaussianFactorGraph, Switching) {
   auto N = 12;
   auto hfg = makeSwitchingChain(N);
 
@@ -381,7 +367,7 @@ TEST(GaussianHybridFactorGraph, Switching) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  GaussianHybridFactorGraph::shared_ptr remaining;
+  HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
   // GTSAM_PRINT(*hbt);
@@ -417,7 +403,7 @@ TEST(GaussianHybridFactorGraph, Switching) {
 
 /* ************************************************************************* */
 // TODO(fan): make a graph like Varun's paper one
-TEST(GaussianHybridFactorGraph, SwitchingISAM) {
+TEST(HybridGaussianFactorGraph, SwitchingISAM) {
   auto N = 11;
   auto hfg = makeSwitchingChain(N);
 
@@ -473,7 +459,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) {
   GTSAM_PRINT(ordering_full);
 
   HybridBayesTree::shared_ptr hbt;
-  GaussianHybridFactorGraph::shared_ptr remaining;
+  HybridGaussianFactorGraph::shared_ptr remaining;
   std::tie(hbt, remaining) = hfg->eliminatePartialMultifrontal(ordering_full);
 
   // GTSAM_PRINT(*hbt);
@@ -499,10 +485,10 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) {
   }
 
   auto new_fg = makeSwitchingChain(12);
-  auto isam = HybridISAM(*hbt);
+  auto isam = HybridGaussianISAM(*hbt);
 
   {
-    GaussianHybridFactorGraph factorGraph;
+    HybridGaussianFactorGraph factorGraph;
     factorGraph.push_back(new_fg->at(new_fg->size() - 2));
     factorGraph.push_back(new_fg->at(new_fg->size() - 1));
     isam.update(factorGraph);
@@ -512,7 +498,7 @@ TEST(GaussianHybridFactorGraph, SwitchingISAM) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianHybridFactorGraph, SwitchingTwoVar) {
+TEST(HybridGaussianFactorGraph, SwitchingTwoVar) {
   const int N = 7;
   auto hfg = makeSwitchingChain(N, X);
   hfg->push_back(*makeSwitchingChain(N, Y, D));
@@ -582,7 +568,7 @@ TEST(GaussianHybridFactorGraph, SwitchingTwoVar) {
   }
   {
     HybridBayesNet::shared_ptr hbn;
-    GaussianHybridFactorGraph::shared_ptr remaining;
+    HybridGaussianFactorGraph::shared_ptr remaining;
     std::tie(hbn, remaining) =
         hfg->eliminatePartialSequential(ordering_partial);
 
diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i
index e8d918a1d..fbdd70fdf 100644
--- a/gtsam/inference/inference.i
+++ b/gtsam/inference/inference.i
@@ -9,7 +9,7 @@ namespace gtsam {
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-#include <gtsam/hybrid/GaussianHybridFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 
 #include <gtsam/inference/Key.h>
 
@@ -107,36 +107,36 @@ class Ordering {
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering ColamdConstrainedLast(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering ColamdConstrainedFirst(
       const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
       bool forceOrder = false);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::GaussianHybridFactorGraph}>
+                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
                                 const FACTOR_GRAPH& graph);
 
diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 895c9e14e..07a8178e6 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -20,8 +20,8 @@ from gtsam.symbol_shorthand import C, X
 from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestGaussianHybridFactorGraph(GtsamTestCase):
-    """Unit tests for GaussianHybridFactorGraph."""
+class TestHybridGaussianFactorGraph(GtsamTestCase):
+    """Unit tests for HybridGaussianFactorGraph."""
 
     def test_create(self):
         """Test contruction of hybrid factor graph."""
@@ -36,13 +36,13 @@ class TestGaussianHybridFactorGraph(GtsamTestCase):
 
         gmf = gtsam.GaussianMixtureFactor.FromFactors([X(0)], dk, [jf1, jf2])
 
-        hfg = gtsam.GaussianHybridFactorGraph()
+        hfg = gtsam.HybridGaussianFactorGraph()
         hfg.add(jf1)
         hfg.add(jf2)
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            gtsam.Ordering.ColamdConstrainedLastGaussianHybridFactorGraph(
+            gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
                 hfg, [C(0)]))
 
         # print("hbn = ", hbn)

From 31ab1a32f3b93c9c7d2e97d95e9bd1c47013ef73 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 00:03:31 -0400
Subject: [PATCH 126/175] update description of GaussianMixtureConditional

---
 gtsam/hybrid/GaussianMixtureConditional.h | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h
index d12fa09d7..a743c00f3 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.h
+++ b/gtsam/hybrid/GaussianMixtureConditional.h
@@ -27,13 +27,19 @@
 namespace gtsam {
 
 /**
- * @brief A conditional of gaussian mixtures indexed by discrete variables.
+ * @brief A conditional of gaussian mixtures indexed by discrete variables, as
+ * part of a Bayes Network.
  *
  * Represents the conditional density P(X | M, Z) where X is a continuous random
- * variable, M is the discrete variable and Z is the set of measurements.
+ * variable, M is the selection of discrete variables corresponding to a subset
+ * of the Gaussian variables and Z is parent of this node
+ *
+ * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2}
+ * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing
+ * coefficient.
  *
  */
-class GaussianMixtureConditional
+class GTSAM_EXPORT GaussianMixtureConditional
     : public HybridFactor,
       public Conditional<HybridFactor, GaussianMixtureConditional> {
  public:

From d2029f3d0339175fbf8b1e4fe0734e9b8e5a37bb Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 00:09:38 -0400
Subject: [PATCH 127/175] Add more information on conditionals requirement for
 GaussianMixture

---
 gtsam/hybrid/GaussianMixtureConditional.h | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixtureConditional.h
index a743c00f3..ab0e2ce43 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.h
+++ b/gtsam/hybrid/GaussianMixtureConditional.h
@@ -68,13 +68,17 @@ class GTSAM_EXPORT GaussianMixtureConditional
 
   /// Defaut constructor, mainly for serialization.
   GaussianMixtureConditional() = default;
+
   /**
-   * @brief Construct a new GaussianMixtureConditional object
+   * @brief Construct a new GaussianMixtureConditional object.
    *
    * @param continuousFrontals the continuous frontals.
    * @param continuousParents the continuous parents.
    * @param discreteParents the discrete parents. Will be placed last.
-   * @param conditionals a decision tree of GaussianConditionals.
+   * @param conditionals a decision tree of GaussianConditionals. The number of
+   * conditionals should be C^(number of discrete parents), where C is the
+   * cardinality of the DiscreteKeys in discreteParents, since the
+   * discreteParents will be used as the labels in the decision tree.
    */
   GaussianMixtureConditional(const KeyVector &continuousFrontals,
                              const KeyVector &continuousParents,

From c8bf9d350ca9ace6de6218607dabc9357b7340a8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 00:13:02 -0400
Subject: [PATCH 128/175] rename GaussianMixtureConditional to GaussianMixture

---
 ...ureConditional.cpp => GaussianMixture.cpp} | 28 +++++++++----------
 ...MixtureConditional.h => GaussianMixture.h} | 20 ++++++-------
 gtsam/hybrid/HybridConditional.cpp            |  2 +-
 gtsam/hybrid/HybridConditional.h              | 14 +++++-----
 gtsam/hybrid/HybridGaussianFactorGraph.cpp    |  8 +++---
 gtsam/hybrid/hybrid.i                         |  8 +++---
 .../tests/testGaussianHybridFactorGraph.cpp   |  6 ++--
 7 files changed, 43 insertions(+), 43 deletions(-)
 rename gtsam/hybrid/{GaussianMixtureConditional.cpp => GaussianMixture.cpp} (80%)
 rename gtsam/hybrid/{GaussianMixtureConditional.h => GaussianMixture.h} (87%)

diff --git a/gtsam/hybrid/GaussianMixtureConditional.cpp b/gtsam/hybrid/GaussianMixture.cpp
similarity index 80%
rename from gtsam/hybrid/GaussianMixtureConditional.cpp
rename to gtsam/hybrid/GaussianMixture.cpp
index 726af6d5f..1663236f0 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianMixtureConditional.cpp
+ * @file   GaussianMixture.cpp
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -20,41 +20,41 @@
 
 #include <gtsam/base/utilities.h>
 #include <gtsam/discrete/DecisionTree-inl.h>
-#include <gtsam/hybrid/GaussianMixtureConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 
-GaussianMixtureConditional::GaussianMixtureConditional(
+GaussianMixture::GaussianMixture(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
-    const GaussianMixtureConditional::Conditionals &conditionals)
+    const GaussianMixture::Conditionals &conditionals)
     : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
                  discreteParents),
       BaseConditional(continuousFrontals.size()),
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixtureConditional::Conditionals &
-GaussianMixtureConditional::conditionals() {
+const GaussianMixture::Conditionals &
+GaussianMixture::conditionals() {
   return conditionals_;
 }
 
 /* *******************************************************************************/
-GaussianMixtureConditional GaussianMixtureConditional::FromConditionals(
+GaussianMixture GaussianMixture::FromConditionals(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
     const DiscreteKeys &discreteParents,
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixtureConditional(continuousFrontals, continuousParents,
+  return GaussianMixture(continuousFrontals, continuousParents,
                                     discreteParents, dt);
 }
 
 /* *******************************************************************************/
-GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
-    const GaussianMixtureConditional::Sum &sum) const {
+GaussianMixture::Sum GaussianMixture::add(
+    const GaussianMixture::Sum &sum) const {
   using Y = GaussianFactorGraph;
   auto add = [](const Y &graph1, const Y &graph2) {
     auto result = graph1;
@@ -66,8 +66,8 @@ GaussianMixtureConditional::Sum GaussianMixtureConditional::add(
 }
 
 /* *******************************************************************************/
-GaussianMixtureConditional::Sum
-GaussianMixtureConditional::asGaussianFactorGraphTree() const {
+GaussianMixture::Sum
+GaussianMixture::asGaussianFactorGraphTree() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -77,13 +77,13 @@ GaussianMixtureConditional::asGaussianFactorGraphTree() const {
 }
 
 /* *******************************************************************************/
-bool GaussianMixtureConditional::equals(const HybridFactor &lf,
+bool GaussianMixture::equals(const HybridFactor &lf,
                                         double tol) const {
   return BaseFactor::equals(lf, tol);
 }
 
 /* *******************************************************************************/
-void GaussianMixtureConditional::print(const std::string &s,
+void GaussianMixture::print(const std::string &s,
                                        const KeyFormatter &formatter) const {
   std::cout << s;
   if (isContinuous()) std::cout << "Continuous ";
diff --git a/gtsam/hybrid/GaussianMixtureConditional.h b/gtsam/hybrid/GaussianMixture.h
similarity index 87%
rename from gtsam/hybrid/GaussianMixtureConditional.h
rename to gtsam/hybrid/GaussianMixture.h
index ab0e2ce43..756e7b77b 100644
--- a/gtsam/hybrid/GaussianMixtureConditional.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file   GaussianMixtureConditional.h
+ * @file   GaussianMixture.h
  * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
  * @author Fan Jiang
  * @author Varun Agrawal
@@ -39,14 +39,14 @@ namespace gtsam {
  * coefficient.
  *
  */
-class GTSAM_EXPORT GaussianMixtureConditional
+class GTSAM_EXPORT GaussianMixture
     : public HybridFactor,
-      public Conditional<HybridFactor, GaussianMixtureConditional> {
+      public Conditional<HybridFactor, GaussianMixture> {
  public:
-  using This = GaussianMixtureConditional;
-  using shared_ptr = boost::shared_ptr<GaussianMixtureConditional>;
+  using This = GaussianMixture;
+  using shared_ptr = boost::shared_ptr<GaussianMixture>;
   using BaseFactor = HybridFactor;
-  using BaseConditional = Conditional<HybridFactor, GaussianMixtureConditional>;
+  using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
 
   /// Alias for DecisionTree of GaussianFactorGraphs
   using Sum = DecisionTree<Key, GaussianFactorGraph>;
@@ -67,10 +67,10 @@ class GTSAM_EXPORT GaussianMixtureConditional
   /// @{
 
   /// Defaut constructor, mainly for serialization.
-  GaussianMixtureConditional() = default;
+  GaussianMixture() = default;
 
   /**
-   * @brief Construct a new GaussianMixtureConditional object.
+   * @brief Construct a new GaussianMixture object.
    *
    * @param continuousFrontals the continuous frontals.
    * @param continuousParents the continuous parents.
@@ -80,7 +80,7 @@ class GTSAM_EXPORT GaussianMixtureConditional
    * cardinality of the DiscreteKeys in discreteParents, since the
    * discreteParents will be used as the labels in the decision tree.
    */
-  GaussianMixtureConditional(const KeyVector &continuousFrontals,
+  GaussianMixture(const KeyVector &continuousFrontals,
                              const KeyVector &continuousParents,
                              const DiscreteKeys &discreteParents,
                              const Conditionals &conditionals);
@@ -107,7 +107,7 @@ class GTSAM_EXPORT GaussianMixtureConditional
 
   /* print utility */
   void print(
-      const std::string &s = "GaussianMixtureConditional\n",
+      const std::string &s = "GaussianMixture\n",
       const KeyFormatter &formatter = DefaultKeyFormatter) const override;
 
   /// @}
diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp
index 7d1b72067..8f95caf34 100644
--- a/gtsam/hybrid/HybridConditional.cpp
+++ b/gtsam/hybrid/HybridConditional.cpp
@@ -54,7 +54,7 @@ HybridConditional::HybridConditional(
 
 /* ************************************************************************ */
 HybridConditional::HybridConditional(
-    boost::shared_ptr<GaussianMixtureConditional> gaussianMixture)
+    boost::shared_ptr<GaussianMixture> gaussianMixture)
     : BaseFactor(KeyVector(gaussianMixture->keys().begin(),
                            gaussianMixture->keys().begin() +
                                gaussianMixture->nrContinuous()),
diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h
index 887b49f12..3ba5da393 100644
--- a/gtsam/hybrid/HybridConditional.h
+++ b/gtsam/hybrid/HybridConditional.h
@@ -18,7 +18,7 @@
 #pragma once
 
 #include <gtsam/discrete/DiscreteConditional.h>
-#include <gtsam/hybrid/GaussianMixtureConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
 #include <gtsam/inference/Conditional.h>
@@ -42,7 +42,7 @@ class HybridGaussianFactorGraph;
  * As a type-erased variant of:
  * - DiscreteConditional
  * - GaussianConditional
- * - GaussianMixtureConditional
+ * - GaussianMixture
  *
  * The reason why this is important is that `Conditional<T>` is a CRTP class.
  * CRTP is static polymorphism such that all CRTP classes, while bearing the
@@ -128,16 +128,16 @@ class GTSAM_EXPORT HybridConditional
    * HybridConditional.
    */
   HybridConditional(
-      boost::shared_ptr<GaussianMixtureConditional> gaussianMixture);
+      boost::shared_ptr<GaussianMixture> gaussianMixture);
 
   /**
-   * @brief Return HybridConditional as a GaussianMixtureConditional
+   * @brief Return HybridConditional as a GaussianMixture
    *
-   * @return GaussianMixtureConditional::shared_ptr
+   * @return GaussianMixture::shared_ptr
    */
-  GaussianMixtureConditional::shared_ptr asMixture() {
+  GaussianMixture::shared_ptr asMixture() {
     if (!isHybrid()) throw std::invalid_argument("Not a mixture");
-    return boost::static_pointer_cast<GaussianMixtureConditional>(inner_);
+    return boost::static_pointer_cast<GaussianMixture>(inner_);
   }
 
   /**
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 0ac2c2656..f4f09701f 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -23,7 +23,7 @@
 #include <gtsam/discrete/DiscreteEliminationTree.h>
 #include <gtsam/discrete/DiscreteFactorGraph.h>
 #include <gtsam/discrete/DiscreteJunctionTree.h>
-#include <gtsam/hybrid/GaussianMixtureConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridConditional.h>
 #include <gtsam/hybrid/HybridDiscreteFactor.h>
@@ -207,8 +207,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
   const GaussianMixtureFactor::Factors &separatorFactors = pair.second;
 
-  // Create the GaussianMixtureConditional from the conditionals
-  auto conditional = boost::make_shared<GaussianMixtureConditional>(
+  // Create the GaussianMixture from the conditionals
+  auto conditional = boost::make_shared<GaussianMixture>(
       frontalKeys, keysOfSeparator, discreteSeparator, pair.first);
 
   // If there are no more continuous parents, then we should create here a
@@ -262,7 +262,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
   // Because of all these reasons, we carefully consider how to
   // implement the hybrid factors so that we do not get poor performance.
 
-  // The first thing is how to represent the GaussianMixtureConditional.
+  // The first thing is how to represent the GaussianMixture.
   // A very possible scenario is that the incoming factors will have different
   // levels of discrete keys. For example, imagine we are going to eliminate the
   // fragment: $\phi(x1,c1,c2)$, $\phi(x1,c2,c3)$, which is perfectly valid.
diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
index 94eaa88b2..bbe1e2400 100644
--- a/gtsam/hybrid/hybrid.i
+++ b/gtsam/hybrid/hybrid.i
@@ -38,16 +38,16 @@ class GaussianMixtureFactor : gtsam::HybridFactor {
                  gtsam::DefaultKeyFormatter) const;
 };
 
-#include <gtsam/hybrid/GaussianMixtureConditional.h>
-class GaussianMixtureConditional : gtsam::HybridFactor {
-  static GaussianMixtureConditional FromConditionals(
+#include <gtsam/hybrid/GaussianMixture.h>
+class GaussianMixture : gtsam::HybridFactor {
+  static GaussianMixture FromConditionals(
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
       const gtsam::DiscreteKeys& discreteParents,
       const std::vector<gtsam::GaussianConditional::shared_ptr>&
           conditionalsList);
 
-  void print(string s = "GaussianMixtureConditional\n",
+  void print(string s = "GaussianMixture\n",
              const gtsam::KeyFormatter& keyFormatter =
                  gtsam::DefaultKeyFormatter) const;
 };
diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
index 8ff959d74..552bb18f5 100644
--- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
+++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp
@@ -20,7 +20,7 @@
 #include <gtsam/discrete/DecisionTreeFactor.h>
 #include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/discrete/DiscreteValues.h>
-#include <gtsam/hybrid/GaussianMixtureConditional.h>
+#include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/hybrid/GaussianMixtureFactor.h>
 #include <gtsam/hybrid/HybridBayesNet.h>
 #include <gtsam/hybrid/HybridBayesTree.h>
@@ -65,9 +65,9 @@ TEST(HybridGaussianFactorGraph, creation) {
 
   hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
 
-  GaussianMixtureConditional clgc(
+  GaussianMixture clgc(
       {X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{C(0), 2}),
-      GaussianMixtureConditional::Conditionals(
+      GaussianMixture::Conditionals(
           C(0),
           boost::make_shared<GaussianConditional>(X(0), Z_3x1, I_3x3, X(1),
                                                   I_3x3),

From 932e65c7a2d418c711a67019fb8763da298dd97f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 00:39:52 -0400
Subject: [PATCH 129/175] Add GTSAM_EXPORT and Testable traits

---
 gtsam/hybrid/GaussianMixture.h           | 10 +++++++---
 gtsam/hybrid/GaussianMixtureFactor.h     |  7 ++++++-
 gtsam/hybrid/HybridDiscreteFactor.h      |  7 ++++++-
 gtsam/hybrid/HybridGaussianFactor.h      |  7 ++++++-
 gtsam/hybrid/HybridGaussianFactorGraph.h |  2 +-
 5 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index 756e7b77b..f805c76c6 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -81,9 +81,9 @@ class GTSAM_EXPORT GaussianMixture
    * discreteParents will be used as the labels in the decision tree.
    */
   GaussianMixture(const KeyVector &continuousFrontals,
-                             const KeyVector &continuousParents,
-                             const DiscreteKeys &discreteParents,
-                             const Conditionals &conditionals);
+                  const KeyVector &continuousParents,
+                  const DiscreteKeys &discreteParents,
+                  const Conditionals &conditionals);
 
   /**
    * @brief Make a Gaussian Mixture from a list of Gaussian conditionals
@@ -125,4 +125,8 @@ class GTSAM_EXPORT GaussianMixture
   Sum add(const Sum &sum) const;
 };
 
+// traits
+template <>
+struct traits<GaussianMixture> : public Testable<GaussianMixture> {};
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index b3569183b..21770f836 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -41,7 +41,7 @@ using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
  * of discrete variables indexes to the continuous gaussian distribution.
  *
  */
-class GaussianMixtureFactor : public HybridFactor {
+class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
  public:
   using Base = HybridFactor;
   using This = GaussianMixtureFactor;
@@ -113,4 +113,9 @@ class GaussianMixtureFactor : public HybridFactor {
   Sum add(const Sum &sum) const;
 };
 
+// traits
+template <>
+struct traits<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
+};
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridDiscreteFactor.h b/gtsam/hybrid/HybridDiscreteFactor.h
index 572ddfbcd..9cbea8170 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.h
+++ b/gtsam/hybrid/HybridDiscreteFactor.h
@@ -29,7 +29,7 @@ namespace gtsam {
  * us to hide the implementation of DiscreteFactor and thus avoid diamond
  * inheritance.
  */
-class HybridDiscreteFactor : public HybridFactor {
+class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor {
  private:
   DiscreteFactor::shared_ptr inner_;
 
@@ -61,4 +61,9 @@ class HybridDiscreteFactor : public HybridFactor {
   /// Return pointer to the internal discrete factor
   DiscreteFactor::shared_ptr inner() const { return inner_; }
 };
+
+// traits
+template <>
+struct traits<HybridDiscreteFactor> : public Testable<HybridDiscreteFactor> {};
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h
index 6fa83b726..2a92c717c 100644
--- a/gtsam/hybrid/HybridGaussianFactor.h
+++ b/gtsam/hybrid/HybridGaussianFactor.h
@@ -28,7 +28,7 @@ namespace gtsam {
  * a diamond inheritance i.e. an extra factor type that inherits from both
  * HybridFactor and GaussianFactor.
  */
-class HybridGaussianFactor : public HybridFactor {
+class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
  private:
   GaussianFactor::shared_ptr inner_;
 
@@ -59,4 +59,9 @@ class HybridGaussianFactor : public HybridFactor {
 
   GaussianFactor::shared_ptr inner() const { return inner_; }
 };
+
+// traits
+template <>
+struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
+
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 6944af510..e7dc3dfca 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -71,7 +71,7 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
  * This is the linearized version of a hybrid factor graph.
  * Everything inside needs to be hybrid factor or hybrid conditional.
  */
-class HybridGaussianFactorGraph
+class GTSAM_EXPORT HybridGaussianFactorGraph
     : public FactorGraph<HybridFactor>,
       public EliminateableFactorGraph<HybridGaussianFactorGraph> {
  public:

From 2afa6781f80044d2841a4de436e42a5df8129c3b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 09:20:43 -0400
Subject: [PATCH 130/175] fix python test

---
 python/gtsam/tests/test_HybridFactorGraph.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
index 07a8178e6..781cfd924 100644
--- a/python/gtsam/tests/test_HybridFactorGraph.py
+++ b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -49,7 +49,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         self.assertEqual(hbn.size(), 2)
 
         mixture = hbn.at(0).inner()
-        self.assertIsInstance(mixture, gtsam.GaussianMixtureConditional)
+        self.assertIsInstance(mixture, gtsam.GaussianMixture)
         self.assertEqual(len(mixture.keys()), 2)
 
         discrete_conditional = hbn.at(hbn.size() - 1).inner()

From 3e10514846c8d32000ab6d51de296ce5722ad8f5 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 22:45:12 -0400
Subject: [PATCH 131/175] if checks for dynamic_cast

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 42 +++++++++-------------
 1 file changed, 16 insertions(+), 26 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index f4f09701f..99202ba6a 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -82,23 +82,19 @@ continuousElimination(const HybridGaussianFactorGraph &factors,
                       const Ordering &frontalKeys) {
   GaussianFactorGraph gfg;
   for (auto &fp : factors) {
-    auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp);
-    if (ptr) {
+    if (auto ptr = boost::dynamic_pointer_cast<HybridGaussianFactor>(fp)) {
       gfg.push_back(ptr->inner());
+    } else if (auto p =
+                   boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
+      gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
     } else {
-      auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
-      if (p) {
-        gfg.push_back(boost::static_pointer_cast<GaussianConditional>(p));
-      } else {
-        // It is an orphan wrapped conditional
-      }
+      // It is an orphan wrapped conditional
     }
   }
 
   auto result = EliminatePreferCholesky(gfg, frontalKeys);
-  return std::make_pair(
-      boost::make_shared<HybridConditional>(result.first),
-      boost::make_shared<HybridGaussianFactor>(result.second));
+  return {boost::make_shared<HybridConditional>(result.first),
+          boost::make_shared<HybridGaussianFactor>(result.second)};
 }
 
 /* ************************************************************************ */
@@ -107,24 +103,20 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
                     const Ordering &frontalKeys) {
   DiscreteFactorGraph dfg;
   for (auto &fp : factors) {
-    auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp);
-    if (ptr) {
+    if (auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp)) {
       dfg.push_back(ptr->inner());
+    } else if (auto p =
+                   boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
+      dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
     } else {
-      auto p = boost::static_pointer_cast<HybridConditional>(fp)->inner();
-      if (p) {
-        dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
-      } else {
-        // It is an orphan wrapper
-      }
+      // It is an orphan wrapper
     }
   }
 
   auto result = EliminateDiscrete(dfg, frontalKeys);
 
-  return std::make_pair(
-      boost::make_shared<HybridConditional>(result.first),
-      boost::make_shared<HybridDiscreteFactor>(result.second));
+  return {boost::make_shared<HybridConditional>(result.first),
+          boost::make_shared<HybridDiscreteFactor>(result.second)};
 }
 
 /* ************************************************************************ */
@@ -146,13 +138,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
 
   for (auto &f : factors) {
     if (f->isHybrid()) {
-      auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f);
-      if (cgmf) {
+      if (auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
         sum = cgmf->add(sum);
       }
 
-      auto gm = boost::dynamic_pointer_cast<HybridConditional>(f);
-      if (gm) {
+      if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) {
         sum = gm->asMixture()->add(sum);
       }
 

From dd877479fa7a3fc124435408afcf186ccc46293e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 22:47:45 -0400
Subject: [PATCH 132/175] separate out summing of frontals into separate
 function

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp | 83 ++++++++++++----------
 1 file changed, 46 insertions(+), 37 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 99202ba6a..3fa6e3e6b 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -76,6 +76,51 @@ static GaussianMixtureFactor::Sum &addGaussian(
   return sum;
 }
 
+/* ************************************************************************ */
+GaussianMixtureFactor::Sum sumFrontals(
+    const HybridGaussianFactorGraph &factors) {
+  // sum out frontals, this is the factor on the separator
+  gttic(sum);
+
+  GaussianMixtureFactor::Sum sum;
+  std::vector<GaussianFactor::shared_ptr> deferredFactors;
+
+  for (auto &f : factors) {
+    if (f->isHybrid()) {
+      if (auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
+        sum = cgmf->add(sum);
+      }
+
+      if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) {
+        sum = gm->asMixture()->add(sum);
+      }
+
+    } else if (f->isContinuous()) {
+      deferredFactors.push_back(
+          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
+    } else {
+      // We need to handle the case where the object is actually an
+      // BayesTreeOrphanWrapper!
+      auto orphan = boost::dynamic_pointer_cast<
+          BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
+      if (!orphan) {
+        auto &fr = *f;
+        throw std::invalid_argument(
+            std::string("factor is discrete in continuous elimination") +
+            typeid(fr).name());
+      }
+    }
+  }
+
+  for (auto &f : deferredFactors) {
+    sum = addGaussian(sum, f);
+  }
+
+  gttoc(sum);
+
+  return sum;
+}
+
 /* ************************************************************************ */
 std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
 continuousElimination(const HybridGaussianFactorGraph &factors,
@@ -131,43 +176,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
                                  discreteSeparatorSet.end());
 
   // sum out frontals, this is the factor on the separator
-  gttic(sum);
-
-  GaussianMixtureFactor::Sum sum;
-  std::vector<GaussianFactor::shared_ptr> deferredFactors;
-
-  for (auto &f : factors) {
-    if (f->isHybrid()) {
-      if (auto cgmf = boost::dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
-        sum = cgmf->add(sum);
-      }
-
-      if (auto gm = boost::dynamic_pointer_cast<HybridConditional>(f)) {
-        sum = gm->asMixture()->add(sum);
-      }
-
-    } else if (f->isContinuous()) {
-      deferredFactors.push_back(
-          boost::dynamic_pointer_cast<HybridGaussianFactor>(f)->inner());
-    } else {
-      // We need to handle the case where the object is actually an
-      // BayesTreeOrphanWrapper!
-      auto orphan = boost::dynamic_pointer_cast<
-          BayesTreeOrphanWrapper<HybridBayesTree::Clique>>(f);
-      if (!orphan) {
-        auto &fr = *f;
-        throw std::invalid_argument(
-            std::string("factor is discrete in continuous elimination") +
-            typeid(fr).name());
-      }
-    }
-  }
-
-  for (auto &f : deferredFactors) {
-    sum = addGaussian(sum, f);
-  }
-
-  gttoc(sum);
+  GaussianMixtureFactor::Sum sum = sumFrontals(factors);
 
   using EliminationPair = GaussianFactorGraph::EliminationResult;
 

From 92176db645bed9374cf3a6fda677fbc8b59d31de Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 22:50:44 -0400
Subject: [PATCH 133/175] add comments

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

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index 3fa6e3e6b..c1251309c 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -183,6 +183,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
   KeyVector keysOfEliminated;  // Not the ordering
   KeyVector keysOfSeparator;   // TODO(frank): Is this just (keys - ordering)?
 
+  // This is the elimination method on the leaf nodes
   auto eliminate = [&](const GaussianFactorGraph &graph)
       -> GaussianFactorGraph::EliminationResult {
     if (graph.empty()) {
@@ -200,8 +201,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
     return result;
   };
 
+  // Perform elimination!
   DecisionTree<Key, EliminationPair> eliminationResults(sum, eliminate);
 
+  // Separate out decision tree into conditionals and remaining factors.
   auto pair = unzip(eliminationResults);
 
   const GaussianMixtureFactor::Factors &separatorFactors = pair.second;

From b47cd9d97bec48b0c4598a61dcda16b407d11c0f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 2 Jun 2022 22:55:46 -0400
Subject: [PATCH 134/175] update GaussianMixture docstring

---
 gtsam/hybrid/GaussianMixture.h | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index f805c76c6..e85506715 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -34,9 +34,10 @@ namespace gtsam {
  * variable, M is the selection of discrete variables corresponding to a subset
  * of the Gaussian variables and Z is parent of this node
  *
- * The negative log-probability is given by \f$ \sum_{m=1}^M \pi_m \frac{1}{2}
- * |Rx - (d - Sy - Tz - ...)|^2 \f$, where \f$ \pi_m \f$ is the mixing
- * coefficient.
+ * The probability P(x|y,z,...) is proportional to
+ * \f$ \sum_i k_i \exp - \frac{1}{2} |R_i x - (d_i - S_i y - T_i z - ...)|^2 \f$
+ * where i indexes the components and k_i is a component-wise normalization
+ * constant.
  *
  */
 class GTSAM_EXPORT GaussianMixture

From eeecb27f14aca16f38c46821f5a5f2f16a512ef9 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 13:29:05 -0400
Subject: [PATCH 135/175] rename back to HybridJunctionTree

---
 gtsam/hybrid/HybridBayesTree.cpp                          | 2 +-
 gtsam/hybrid/HybridBayesTree.h                            | 2 +-
 gtsam/hybrid/HybridGaussianFactorGraph.cpp                | 2 +-
 gtsam/hybrid/HybridGaussianFactorGraph.h                  | 4 ++--
 ...ridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} | 6 +++---
 ...{HybridGaussianJunctionTree.h => HybridJunctionTree.h} | 8 ++++----
 6 files changed, 12 insertions(+), 12 deletions(-)
 rename gtsam/hybrid/{HybridGaussianJunctionTree.cpp => HybridJunctionTree.cpp} (97%)
 rename gtsam/hybrid/{HybridGaussianJunctionTree.h => HybridJunctionTree.h} (91%)

diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp
index ff07f1817..d65270f91 100644
--- a/gtsam/hybrid/HybridBayesTree.cpp
+++ b/gtsam/hybrid/HybridBayesTree.cpp
@@ -12,7 +12,7 @@
 /**
  * @file    HybridBayesTree.cpp
  * @brief   Hybrid Bayes Tree, the result of eliminating a
- * HybridGaussianJunctionTree
+ * HybridJunctionTree
  * @date Mar 11, 2022
  * @author  Fan Jiang
  */
diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h
index 28d9ef809..0b89ca8c4 100644
--- a/gtsam/hybrid/HybridBayesTree.h
+++ b/gtsam/hybrid/HybridBayesTree.h
@@ -12,7 +12,7 @@
 /**
  * @file    HybridBayesTree.h
  * @brief   Hybrid Bayes Tree, the result of eliminating a
- * HybridGaussianJunctionTree
+ * HybridJunctionTree
  * @date Mar 11, 2022
  * @author  Fan Jiang
  */
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index c1251309c..88730cae9 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -31,7 +31,7 @@
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactor.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
-#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/EliminateableFactorGraph-inst.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/linear/GaussianConditional.h>
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index e7dc3dfca..0188aa652 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -31,7 +31,7 @@ class HybridConditional;
 class HybridBayesNet;
 class HybridEliminationTree;
 class HybridBayesTree;
-class HybridGaussianJunctionTree;
+class HybridJunctionTree;
 class DecisionTreeFactor;
 
 class JacobianFactor;
@@ -55,7 +55,7 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
   typedef HybridEliminationTree
       EliminationTreeType;                ///< Type of elimination tree
   typedef HybridBayesTree BayesTreeType;  ///< Type of Bayes tree
-  typedef HybridGaussianJunctionTree
+  typedef HybridJunctionTree
       JunctionTreeType;  ///< Type of Junction tree
   /// The default dense elimination function
   static std::pair<boost::shared_ptr<ConditionalType>,
diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp
similarity index 97%
rename from gtsam/hybrid/HybridGaussianJunctionTree.cpp
rename to gtsam/hybrid/HybridJunctionTree.cpp
index 8ceb7c87b..7725742cf 100644
--- a/gtsam/hybrid/HybridGaussianJunctionTree.cpp
+++ b/gtsam/hybrid/HybridJunctionTree.cpp
@@ -10,14 +10,14 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridGaussianJunctionTree.cpp
+ * @file HybridJunctionTree.cpp
  * @date Mar 11, 2022
  * @author Fan Jiang
  */
 
 #include <gtsam/hybrid/HybridEliminationTree.h>
 #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
-#include <gtsam/hybrid/HybridGaussianJunctionTree.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
 #include <gtsam/inference/JunctionTree-inst.h>
 #include <gtsam/inference/Key.h>
 
@@ -142,7 +142,7 @@ struct HybridConstructorTraversalData {
 };
 
 /* ************************************************************************* */
-HybridGaussianJunctionTree::HybridGaussianJunctionTree(
+HybridJunctionTree::HybridJunctionTree(
     const HybridEliminationTree& eliminationTree) {
   gttic(JunctionTree_FromEliminationTree);
   // Here we rely on the BayesNet having been produced by this elimination tree,
diff --git a/gtsam/hybrid/HybridGaussianJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h
similarity index 91%
rename from gtsam/hybrid/HybridGaussianJunctionTree.h
rename to gtsam/hybrid/HybridJunctionTree.h
index 314e7daa6..cad1e15a1 100644
--- a/gtsam/hybrid/HybridGaussianJunctionTree.h
+++ b/gtsam/hybrid/HybridJunctionTree.h
@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file HybridGaussianJunctionTree.h
+ * @file HybridJunctionTree.h
  * @date Mar 11, 2022
  * @author Fan Jiang
  */
@@ -48,12 +48,12 @@ class HybridEliminationTree;
  * \addtogroup Multifrontal
  * \nosubgrouping
  */
-class GTSAM_EXPORT HybridGaussianJunctionTree
+class GTSAM_EXPORT HybridJunctionTree
     : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
  public:
   typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
       Base;                                    ///< Base class
-  typedef HybridGaussianJunctionTree This;     ///< This class
+  typedef HybridJunctionTree This;     ///< This class
   typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer to this class
 
   /**
@@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridGaussianJunctionTree
    * named constructor instead.
    * @return The elimination tree
    */
-  HybridGaussianJunctionTree(const HybridEliminationTree& eliminationTree);
+  HybridJunctionTree(const HybridEliminationTree& eliminationTree);
 };
 
 }  // namespace gtsam

From 8482cee10b66f8bcc7c3674235c6fbbcb07373e0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 3 Jun 2022 13:53:25 -0400
Subject: [PATCH 136/175] improvements made during the old hybrid scheme
 implementation

---
 gtsam/discrete/DecisionTree-inl.h      |  9 +++------
 gtsam/discrete/DecisionTree.h          |  2 +-
 gtsam/discrete/DiscreteFactorGraph.h   |  4 ++++
 gtsam/inference/FactorGraph.h          |  2 +-
 gtsam/inference/Ordering.cpp           | 11 +++++++++++
 gtsam/inference/Ordering.h             | 18 +++++++++++++++++-
 gtsam/inference/tests/testOrdering.cpp | 26 ++++++++++++++++++++++++++
 gtsam/linear/GaussianBayesNet.h        |  2 +-
 gtsam/linear/GaussianConditional.cpp   |  4 ++--
 gtsam/linear/GaussianConditional.h     |  1 +
 10 files changed, 67 insertions(+), 12 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 99f29b8e5..f72743206 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -39,10 +39,10 @@
 #include <string>
 #include <vector>
 
-using boost::assign::operator+=;
-
 namespace gtsam {
 
+  using boost::assign::operator+=;
+
   /****************************************************************************/
   // Node
   /****************************************************************************/
@@ -291,10 +291,7 @@ namespace gtsam {
         }
 
         os << "\"" << this->id() << "\" -> \"" << branch->id() << "\"";
-        if (B == 2) {
-          if (i == 0) os << " [style=dashed]";
-          if (i > 1) os << " [style=bold]";
-        }
+        if (B == 2 && i == 0) os << " [style=dashed]";
         os << std::endl;
         branch->dot(os, labelFormatter, valueFormatter, showZero);
       }
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 1f45d320b..7449ae995 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -220,7 +220,7 @@ namespace gtsam {
     /// @{
 
     /// Make virtual
-    virtual ~DecisionTree() {}
+    virtual ~DecisionTree() = default;
 
     /// Check if tree is empty.
     bool empty() const { return !root_; }
diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index f962b1802..bfa57e7db 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -209,6 +209,10 @@ class GTSAM_EXPORT DiscreteFactorGraph
   /// @}
 };  // \ DiscreteFactorGraph
 
+std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
+EliminateForMPE(const DiscreteFactorGraph& factors,
+                const Ordering& frontalKeys);
+
 /// traits
 template <>
 struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index afea63da8..101134c83 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -361,7 +361,7 @@ class FactorGraph {
    * less than the original, factors at the end will be removed.  If the new
    * size is larger than the original, null factors will be appended.
    */
-  void resize(size_t size) { factors_.resize(size); }
+  virtual void resize(size_t size) { factors_.resize(size); }
 
   /** delete factor without re-arranging indexes by inserting a nullptr pointer
    */
diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp
index 2ac2c0dde..7b7ff1403 100644
--- a/gtsam/inference/Ordering.cpp
+++ b/gtsam/inference/Ordering.cpp
@@ -283,6 +283,17 @@ void Ordering::print(const std::string& str,
   cout.flush();
 }
 
+/* ************************************************************************* */
+Ordering::This& Ordering::operator+=(KeyVector& keys) {
+  this->insert(this->end(), keys.begin(), keys.end());
+  return *this;
+}
+
+/* ************************************************************************* */
+bool Ordering::contains(const Key& key) const {
+  return std::find(this->begin(), this->end(), key) != this->end();
+}
+
 /* ************************************************************************* */
 bool Ordering::equals(const Ordering& other, double tol) const {
   return (*this) == other;
diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h
index a2d165792..b366b3b3a 100644
--- a/gtsam/inference/Ordering.h
+++ b/gtsam/inference/Ordering.h
@@ -70,7 +70,23 @@ public:
         boost::assign_detail::call_push_back<This>(*this))(key);
   }
 
-  /// Invert (not reverse) the ordering - returns a map from key to order position
+  /**
+   * @brief Append new keys to the ordering as `ordering += keys`.
+   *
+   * @param key
+   * @return The ordering variable with appended keys.
+   */
+  This& operator+=(KeyVector& keys);
+
+  /// Check if key exists in ordering.
+  bool contains(const Key& key) const;
+
+  /**
+   * @brief Invert (not reverse) the ordering - returns a map from key to order
+   * position.
+   *
+   * @return FastMap<Key, size_t>
+   */
   FastMap<Key, size_t> invert() const;
 
   /// @name Fill-reducing Orderings @{
diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp
index 6fdca0d89..1afa1cfde 100644
--- a/gtsam/inference/tests/testOrdering.cpp
+++ b/gtsam/inference/tests/testOrdering.cpp
@@ -199,6 +199,32 @@ TEST(Ordering, csr_format_3) {
   EXPECT(adjExpected == adjAcutal);
 }
 
+/* ************************************************************************* */
+TEST(Ordering, AppendVector) {
+  using symbol_shorthand::X;
+  Ordering actual;
+  KeyVector keys = {X(0), X(1), X(2)};
+  actual += keys;
+
+  Ordering expected;
+  expected += X(0);
+  expected += X(1);
+  expected += X(2);
+  EXPECT(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Ordering, Contains) {
+  using symbol_shorthand::X;
+  Ordering ordering;
+  ordering += X(0);
+  ordering += X(1);
+  ordering += X(2);
+
+  EXPECT(ordering.contains(X(1)));
+  EXPECT(!ordering.contains(X(4)));
+}
+
 /* ************************************************************************* */
 #ifdef GTSAM_SUPPORT_NESTED_DISSECTION
 TEST(Ordering, csr_format_4) {
diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h
index 940ffd882..e4f2950ed 100644
--- a/gtsam/linear/GaussianBayesNet.h
+++ b/gtsam/linear/GaussianBayesNet.h
@@ -45,7 +45,7 @@ namespace gtsam {
     /// @name Standard Constructors
     /// @{
 
-    /** Construct empty factor graph */
+    /** Construct empty bayes net */
     GaussianBayesNet() {}
 
     /** Construct from iterator over conditionals */
diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 6199f91a7..951577641 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -15,10 +15,10 @@
  * @author Christian Potthast, Frank Dellaert
  */
 
-#include <gtsam/linear/linearExceptions.h>
 #include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/Sampler.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/linearExceptions.h>
 
 #include <boost/format.hpp>
 #ifdef __GNUC__
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index b2b616dab..11fe1f318 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -24,6 +24,7 @@
 #include <gtsam/global_includes.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/inference/Conditional.h>
+#include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <random> // for std::mt19937_64 

From 2396bca22f4c3345e6a76f74a71ee0073a68cf4b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 10:07:53 -0400
Subject: [PATCH 137/175] Add GaussianMixture tests

---
 gtsam/hybrid/GaussianMixture.cpp           | 27 ++++++-
 gtsam/hybrid/GaussianMixture.h             | 12 +++
 gtsam/hybrid/tests/testGaussianMixture.cpp | 92 ++++++++++++++++++++++
 3 files changed, 128 insertions(+), 3 deletions(-)
 create mode 100644 gtsam/hybrid/tests/testGaussianMixture.cpp

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 000057518..3d4eb79fa 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -19,7 +19,7 @@
  */
 
 #include <gtsam/base/utilities.h>
-#include <gtsam/discrete/DecisionTree-inl.h>
+#include <gtsam/discrete/DiscreteValues.h>
 #include <gtsam/hybrid/GaussianMixture.h>
 #include <gtsam/inference/Conditional-inst.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
@@ -77,8 +77,29 @@ GaussianMixture::asGaussianFactorGraphTree() const {
 }
 
 /* *******************************************************************************/
-bool GaussianMixture::equals(const HybridFactor &lf,
-                                        double tol) const {
+size_t GaussianMixture::nrComponents() const {
+  size_t total = 0;
+  conditionals_.visit([&total](const GaussianFactor::shared_ptr &node) {
+    if (node) total += 1;
+  });
+  return total;
+}
+
+/* *******************************************************************************/
+GaussianConditional::shared_ptr GaussianMixture::operator()(
+    const DiscreteValues &discreteVals) const {
+  auto &ptr = conditionals_(discreteVals);
+  if (!ptr) return nullptr;
+  auto conditional = boost::dynamic_pointer_cast<GaussianConditional>(ptr);
+  if (conditional)
+    return conditional;
+  else
+    throw std::logic_error(
+        "A GaussianMixture unexpectedly contained a non-conditional");
+}
+
+/* *******************************************************************************/
+bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
   const This *e = dynamic_cast<const This *>(&lf);
   return e != nullptr && BaseFactor::equals(*e, tol);
 }
diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h
index e85506715..fc1eb0f06 100644
--- a/gtsam/hybrid/GaussianMixture.h
+++ b/gtsam/hybrid/GaussianMixture.h
@@ -19,7 +19,9 @@
 
 #pragma once
 
+#include <gtsam/discrete/DecisionTree-inl.h>
 #include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/inference/Conditional.h>
 #include <gtsam/linear/GaussianConditional.h>
@@ -99,6 +101,16 @@ class GTSAM_EXPORT GaussianMixture
       const DiscreteKeys &discreteParents,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
+  /// @}
+  /// @name Standard API
+  /// @{
+
+  GaussianConditional::shared_ptr operator()(
+      const DiscreteValues &discreteVals) const;
+
+  /// Returns the total number of continuous components
+  size_t nrComponents() const;
+
   /// @}
   /// @name Testable
   /// @{
diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
new file mode 100644
index 000000000..8a91a86cc
--- /dev/null
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testGaussianMixture.cpp
+ * @brief   Unit tests for GaussianMixture class
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianConditional.h>
+
+#include <vector>
+
+// 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;
+
+/* ************************************************************************* */
+TEST(GaussianConditional, Equals) {
+  // create a conditional gaussian node
+  Matrix S1(2, 2);
+  S1(0, 0) = 1;
+  S1(1, 0) = 2;
+  S1(0, 1) = 3;
+  S1(1, 1) = 4;
+
+  Matrix S2(2, 2);
+  S2(0, 0) = 6;
+  S2(1, 0) = 0.2;
+  S2(0, 1) = 8;
+  S2(1, 1) = 0.4;
+
+  Matrix R1(2, 2);
+  R1(0, 0) = 0.1;
+  R1(1, 0) = 0.3;
+  R1(0, 1) = 0.0;
+  R1(1, 1) = 0.34;
+
+  Matrix R2(2, 2);
+  R2(0, 0) = 0.1;
+  R2(1, 0) = 0.3;
+  R2(0, 1) = 0.0;
+  R2(1, 1) = 0.34;
+
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34));
+
+  Vector2 d1(0.2, 0.5), d2(0.5, 0.2);
+
+  auto conditional0 = boost::make_shared<GaussianConditional>(X(1), d1, R1,
+                                                              X(2), S1, model),
+       conditional1 = boost::make_shared<GaussianConditional>(X(1), d2, R2,
+                                                              X(2), S2, model);
+
+  // Create decision tree
+  DiscreteKey m1(1, 2);
+  GaussianMixture::Conditionals conditionals(
+      {m1},
+      vector<GaussianConditional::shared_ptr>{conditional0, conditional1});
+  GaussianMixture mixtureFactor({X(1)}, {X(2)}, {m1}, conditionals);
+
+  // Let's check that this worked:
+  DiscreteValues mode;
+  mode[m1.first] = 1;
+  auto actual = mixtureFactor(mode);
+  EXPECT(actual == conditional1);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
\ No newline at end of file

From fc939b08e045d443a31858db107f676b693a3948 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 10:48:08 -0400
Subject: [PATCH 138/175] GaussianMixtureFactor tests

---
 gtsam/hybrid/GaussianMixtureFactor.cpp        |   7 +-
 gtsam/hybrid/GaussianMixtureFactor.h          |  19 +++
 gtsam/hybrid/HybridFactor.cpp                 |  23 ++-
 gtsam/hybrid/HybridFactor.h                   |   3 +
 .../tests/testGaussianMixtureFactor.cpp       | 159 ++++++++++++++++++
 5 files changed, 205 insertions(+), 6 deletions(-)
 create mode 100644 gtsam/hybrid/tests/testGaussianMixtureFactor.cpp

diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp
index a81cf341d..8f832d8ea 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.cpp
+++ b/gtsam/hybrid/GaussianMixtureFactor.cpp
@@ -51,16 +51,19 @@ GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
 void GaussianMixtureFactor::print(const std::string &s,
                                   const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
+  std::cout << "]{\n";
   factors_.print(
-      "mixture = ", [&](Key k) { return formatter(k); },
+      "", [&](Key k) { return formatter(k); },
       [&](const GaussianFactor::shared_ptr &gf) -> std::string {
         RedirectCout rd;
-        if (!gf->empty())
+        std::cout << ":\n";
+        if (gf)
           gf->print("", formatter);
         else
           return {"nullptr"};
         return rd.str();
       });
+  std::cout << "}" << std::endl;
 }
 
 /* *******************************************************************************/
diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h
index 21770f836..6c90ee6a7 100644
--- a/gtsam/hybrid/GaussianMixtureFactor.h
+++ b/gtsam/hybrid/GaussianMixtureFactor.h
@@ -84,6 +84,19 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
                         const DiscreteKeys &discreteKeys,
                         const Factors &factors);
 
+  /**
+   * @brief Construct a new GaussianMixtureFactor object using a vector of
+   * GaussianFactor shared pointers.
+   *
+   * @param keys Vector of keys for continuous factors.
+   * @param discreteKeys Vector of discrete keys.
+   * @param factors Vector of gaussian factor shared pointers.
+   */
+  GaussianMixtureFactor(const KeyVector &keys, const DiscreteKeys &discreteKeys,
+                        const std::vector<GaussianFactor::shared_ptr> &factors)
+      : GaussianMixtureFactor(keys, discreteKeys,
+                              Factors(discreteKeys, factors)) {}
+
   static This FromFactors(
       const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
       const std::vector<GaussianFactor::shared_ptr> &factors);
@@ -111,6 +124,12 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
    * @return Sum
    */
   Sum add(const Sum &sum) const;
+
+  /// Add MixtureFactor to a Sum, syntactic sugar.
+  friend Sum &operator+=(Sum &sum, const GaussianMixtureFactor &factor) {
+    sum = factor.add(sum);
+    return sum;
+  }
 };
 
 // traits
diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp
index 127c9761c..8df2d524f 100644
--- a/gtsam/hybrid/HybridFactor.cpp
+++ b/gtsam/hybrid/HybridFactor.cpp
@@ -50,7 +50,10 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &keys)
-    : Base(keys), isContinuous_(true), nrContinuous_(keys.size()) {}
+    : Base(keys),
+      isContinuous_(true),
+      nrContinuous_(keys.size()),
+      continuousKeys_(keys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const KeyVector &continuousKeys,
@@ -60,13 +63,15 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys,
       isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)),
       isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)),
       nrContinuous_(continuousKeys.size()),
-      discreteKeys_(discreteKeys) {}
+      discreteKeys_(discreteKeys),
+      continuousKeys_(continuousKeys) {}
 
 /* ************************************************************************ */
 HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
     : Base(CollectKeys({}, discreteKeys)),
       isDiscrete_(true),
-      discreteKeys_(discreteKeys) {}
+      discreteKeys_(discreteKeys),
+      continuousKeys_({}) {}
 
 /* ************************************************************************ */
 bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
@@ -83,7 +88,17 @@ void HybridFactor::print(const std::string &s,
   if (isContinuous_) std::cout << "Continuous ";
   if (isDiscrete_) std::cout << "Discrete ";
   if (isHybrid_) std::cout << "Hybrid ";
-  this->printKeys("", formatter);
+  for (size_t c=0; c<continuousKeys_.size(); c++) {
+    std::cout << formatter(continuousKeys_.at(c));
+    if (c < continuousKeys_.size() - 1) {
+      std::cout << " ";
+    } else {
+      std::cout << "; ";
+    }
+  }
+  for(auto && discreteKey: discreteKeys_) {
+    std::cout << formatter(discreteKey.first) << " ";
+  }
 }
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h
index 244fba4cc..3e91e2487 100644
--- a/gtsam/hybrid/HybridFactor.h
+++ b/gtsam/hybrid/HybridFactor.h
@@ -52,6 +52,9 @@ class GTSAM_EXPORT HybridFactor : public Factor {
  protected:
   DiscreteKeys discreteKeys_;
 
+  /// Record continuous keys for book-keeping
+  KeyVector continuousKeys_;
+
  public:
   // typedefs needed to play nice with gtsam
   typedef HybridFactor This;  ///< This class
diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
new file mode 100644
index 000000000..36477218b
--- /dev/null
+++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
@@ -0,0 +1,159 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    GaussianMixtureFactor.cpp
+ * @brief   Unit tests for GaussianMixtureFactor
+ * @author  Varun Agrawal
+ * @author  Fan Jiang
+ * @author  Frank Dellaert
+ * @date    December 2021
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.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;
+
+/* ************************************************************************* */
+// Check iterators of empty mixture.
+TEST(GaussianMixtureFactor, Constructor) {
+  GaussianMixtureFactor factor;
+  GaussianMixtureFactor::const_iterator const_it = factor.begin();
+  CHECK(const_it == factor.end());
+  GaussianMixtureFactor::iterator it = factor.begin();
+  CHECK(it == factor.end());
+}
+
+/* ************************************************************************* */
+// "Add" two mixture factors together.
+TEST(GaussianMixtureFactor, Sum) {
+  DiscreteKey m1(1, 2), m2(2, 3);
+
+  auto A1 = Matrix::Zero(2, 1);
+  auto A2 = Matrix::Zero(2, 2);
+  auto A3 = Matrix::Zero(2, 3);
+  auto b = Matrix::Zero(2, 1);
+  Vector2 sigmas;
+  sigmas << 1, 2;
+  auto model = noiseModel::Diagonal::Sigmas(sigmas, true);
+
+  auto f10 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f20 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  auto f21 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  auto f22 = boost::make_shared<JacobianFactor>(X(1), A1, X(3), A3, b);
+  std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
+  std::vector<GaussianFactor::shared_ptr> factorsB{f20, f21, f22};
+
+  // TODO(Frank): why specify keys at all? And: keys in factor should be *all*
+  // keys, deviating from Kevin's scheme. Should we index DT on DiscreteKey?
+  // Design review!
+  GaussianMixtureFactor mixtureFactorA({X(1), X(2)}, {m1}, factorsA);
+  GaussianMixtureFactor mixtureFactorB({X(1), X(3)}, {m2}, factorsB);
+
+  // Check that number of keys is 3
+  EXPECT_LONGS_EQUAL(3, mixtureFactorA.keys().size());
+
+  // Check that number of discrete keys is 1 // TODO(Frank): should not exist?
+  EXPECT_LONGS_EQUAL(1, mixtureFactorA.discreteKeys().size());
+
+  // Create sum of two mixture factors: it will be a decision tree now on both
+  // discrete variables m1 and m2:
+  GaussianMixtureFactor::Sum sum;
+  sum += mixtureFactorA;
+  sum += mixtureFactorB;
+
+  // Let's check that this worked:
+  Assignment<Key> mode;
+  mode[m1.first] = 1;
+  mode[m2.first] = 2;
+  auto actual = sum(mode);
+  EXPECT(actual.at(0) == f11);
+  EXPECT(actual.at(1) == f22);
+}
+
+TEST(GaussianMixtureFactor, Printing) {
+  DiscreteKey m1(1, 2);
+  auto A1 = Matrix::Zero(2, 1);
+  auto A2 = Matrix::Zero(2, 2);
+  auto b = Matrix::Zero(2, 1);
+  auto f10 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  auto f11 = boost::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b);
+  std::vector<GaussianFactor::shared_ptr> factors{f10, f11};
+
+  GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
+
+  std::string expected =
+      R"(Hybrid x1 x2; 1 ]{
+ Choice(1) 
+ 0 Leaf :
+  A[x1] = [
+	0;
+	0
+]
+  A[x2] = [
+	0, 0;
+	0, 0
+]
+  b = [ 0 0 ]
+  No noise model
+
+ 1 Leaf :
+  A[x1] = [
+	0;
+	0
+]
+  A[x2] = [
+	0, 0;
+	0, 0
+]
+  b = [ 0 0 ]
+  No noise model
+
+}
+)";
+  EXPECT(assert_print_equal(expected, mixtureFactor));
+}
+
+TEST_UNSAFE(GaussianMixtureFactor, GaussianMixture) {
+  KeyVector keys;
+  keys.push_back(X(0));
+  keys.push_back(X(1));
+
+  DiscreteKeys dKeys;
+  dKeys.emplace_back(M(0), 2);
+  dKeys.emplace_back(M(1), 2);
+
+  auto gaussians = boost::make_shared<GaussianConditional>();
+  GaussianMixture::Conditionals conditionals(gaussians);
+  GaussianMixture gm({}, keys, dKeys, conditionals);
+
+  EXPECT_LONGS_EQUAL(2, gm.discreteKeys().size());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
\ No newline at end of file

From 60e3321da35ec4d8536da91e528db7adbf1b4812 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 14:11:49 -0400
Subject: [PATCH 139/175] some formatting and improved printing

---
 gtsam/hybrid/GaussianMixture.cpp      | 14 ++++++--------
 gtsam/hybrid/HybridDiscreteFactor.cpp |  2 +-
 gtsam/hybrid/HybridGaussianFactor.cpp |  2 +-
 3 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp
index 3d4eb79fa..b04800d21 100644
--- a/gtsam/hybrid/GaussianMixture.cpp
+++ b/gtsam/hybrid/GaussianMixture.cpp
@@ -36,8 +36,7 @@ GaussianMixture::GaussianMixture(
       conditionals_(conditionals) {}
 
 /* *******************************************************************************/
-const GaussianMixture::Conditionals &
-GaussianMixture::conditionals() {
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
   return conditionals_;
 }
 
@@ -48,8 +47,8 @@ GaussianMixture GaussianMixture::FromConditionals(
     const std::vector<GaussianConditional::shared_ptr> &conditionalsList) {
   Conditionals dt(discreteParents, conditionalsList);
 
-  return GaussianMixture(continuousFrontals, continuousParents,
-                                    discreteParents, dt);
+  return GaussianMixture(continuousFrontals, continuousParents, discreteParents,
+                         dt);
 }
 
 /* *******************************************************************************/
@@ -66,8 +65,7 @@ GaussianMixture::Sum GaussianMixture::add(
 }
 
 /* *******************************************************************************/
-GaussianMixture::Sum
-GaussianMixture::asGaussianFactorGraphTree() const {
+GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const {
   auto lambda = [](const GaussianFactor::shared_ptr &factor) {
     GaussianFactorGraph result;
     result.push_back(factor);
@@ -106,13 +104,13 @@ bool GaussianMixture::equals(const HybridFactor &lf, double tol) const {
 
 /* *******************************************************************************/
 void GaussianMixture::print(const std::string &s,
-                                       const KeyFormatter &formatter) const {
+                            const KeyFormatter &formatter) const {
   std::cout << s;
   if (isContinuous()) std::cout << "Continuous ";
   if (isDiscrete()) std::cout << "Discrete ";
   if (isHybrid()) std::cout << "Hybrid ";
   BaseConditional::print("", formatter);
-  std::cout << "\nDiscrete Keys = ";
+  std::cout << " Discrete Keys = ";
   for (auto &dk : discreteKeys()) {
     std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), ";
   }
diff --git a/gtsam/hybrid/HybridDiscreteFactor.cpp b/gtsam/hybrid/HybridDiscreteFactor.cpp
index 2bdcdee8c..0455e1e90 100644
--- a/gtsam/hybrid/HybridDiscreteFactor.cpp
+++ b/gtsam/hybrid/HybridDiscreteFactor.cpp
@@ -47,7 +47,7 @@ bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
 void HybridDiscreteFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner_->print("inner: ", formatter);
+  inner_->print("\n", formatter);
 };
 
 }  // namespace gtsam
diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp
index 59d20fb79..4d7490e49 100644
--- a/gtsam/hybrid/HybridGaussianFactor.cpp
+++ b/gtsam/hybrid/HybridGaussianFactor.cpp
@@ -41,7 +41,7 @@ bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
 void HybridGaussianFactor::print(const std::string &s,
                                  const KeyFormatter &formatter) const {
   HybridFactor::print(s, formatter);
-  inner_->print("inner: ", formatter);
+  inner_->print("\n", formatter);
 };
 
 }  // namespace gtsam

From 8b03eb5b2d3abb3b9f3784e570b114e776f553df Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jun 2022 17:57:26 -0400
Subject: [PATCH 140/175] move += operator inside namespace

---
 gtsam/discrete/DecisionTree-inl.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h
index 99f29b8e5..ffb8d7c69 100644
--- a/gtsam/discrete/DecisionTree-inl.h
+++ b/gtsam/discrete/DecisionTree-inl.h
@@ -39,10 +39,10 @@
 #include <string>
 #include <vector>
 
-using boost::assign::operator+=;
-
 namespace gtsam {
 
+  using boost::assign::operator+=;
+
   /****************************************************************************/
   // Node
   /****************************************************************************/

From f911ccf176570cd93be5e1dea290283ec37ff3ff Mon Sep 17 00:00:00 2001
From: HViktorTsoi <cuijiahetony@live.com>
Date: Wed, 8 Jun 2022 16:30:10 +0800
Subject: [PATCH 141/175] Move Rot3::quaternion to the deprecated block

---
 gtsam/geometry/Rot3.cpp | 2 ++
 gtsam/geometry/Rot3.h   | 7 ++++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp
index 08213b065..6db5e1919 100644
--- a/gtsam/geometry/Rot3.cpp
+++ b/gtsam/geometry/Rot3.cpp
@@ -228,6 +228,7 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const {
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 Vector Rot3::quaternion() const {
   gtsam::Quaternion q = toQuaternion();
   Vector v(4);
@@ -237,6 +238,7 @@ Vector Rot3::quaternion() const {
   v(3) = q.z();
   return v;
 }
+#endif
 
 /* ************************************************************************* */
 pair<Unit3, double> Rot3::axisAngle() const {
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 18bd88b52..01ca7778c 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -515,11 +515,16 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
      */
     gtsam::Quaternion toQuaternion() const;
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
     /**
      * Converts to a generic matrix to allow for use with matlab
      * In format: w x y z
+     * @deprecated: use Rot3::toQuaternion() instead.
+     * If still using this API, remind that in the returned Vector `V`,
+     * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'.
      */
-    Vector quaternion() const;
+    Vector GTSAM_DEPRECATED quaternion() const;
+#endif
 
     /**
      * @brief Spherical Linear intERPolation between *this and other

From 27ddedfc630206a87c980c6b64f1b90dc7f26747 Mon Sep 17 00:00:00 2001
From: HViktorTsoi <cuijiahetony@live.com>
Date: Wed, 8 Jun 2022 16:42:50 +0800
Subject: [PATCH 142/175] Replace the usage of Rot3::quaternion to
 Rot3::toQuaternion in 'timeShonanAveraging.cpp'

---
 gtsam_unstable/timing/timeShonanAveraging.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp
index e932b6400..74dd04a78 100644
--- a/gtsam_unstable/timing/timeShonanAveraging.cpp
+++ b/gtsam_unstable/timing/timeShonanAveraging.cpp
@@ -79,9 +79,9 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
         myfile << "VERTEX_SE3:QUAT" << " ";
         myfile << i << " ";
         myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
-        Vector quaternion = Rot3(R).quaternion();
-        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
-               << " " << quaternion(0);
+        Quaternion quaternion = Rot3(R).toQuaternion();
+        myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z()
+               << " " << quaternion.w();
         myfile << "\n";
     }
     myfile.close();

From 5ac71d20d56c594e6ef118d01e47706a90e78c6e Mon Sep 17 00:00:00 2001
From: HViktorTsoi <cuijiahetony@live.com>
Date: Thu, 9 Jun 2022 17:56:35 +0800
Subject: [PATCH 143/175] Remove 'Rot3::quaternion()' API from 'geometry.i'

---
 gtsam/geometry/geometry.i | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 8079e2c2a..517558265 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -355,7 +355,7 @@ class Rot3 {
   double yaw() const;
   pair<gtsam::Unit3, double> axisAngle() const;
   gtsam::Quaternion toQuaternion() const;
-  Vector quaternion() const;
+  // Vector quaternion() const; // @deprecated, see https://github.com/borglab/gtsam/pull/1219
   gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
 
   // enabling serialization functionality

From f6b86fb17eab1788edc25630ab6f78508d0aaec6 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 10 Jun 2022 04:05:18 -0400
Subject: [PATCH 144/175] address review comments

---
 gtsam/hybrid/tests/testGaussianMixture.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp
index 8a91a86cc..420e22315 100644
--- a/gtsam/hybrid/tests/testGaussianMixture.cpp
+++ b/gtsam/hybrid/tests/testGaussianMixture.cpp
@@ -35,7 +35,10 @@ using symbol_shorthand::M;
 using symbol_shorthand::X;
 
 /* ************************************************************************* */
-TEST(GaussianConditional, Equals) {
+/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a
+ * specific mode i.e. P(x1 | x2, m1=1).
+ */
+TEST(GaussianMixture, Equals) {
   // create a conditional gaussian node
   Matrix S1(2, 2);
   S1(0, 0) = 1;
@@ -89,4 +92,4 @@ int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }
-/* ************************************************************************* */
\ No newline at end of file
+/* ************************************************************************* */

From c49ad326d1f34327e9d751b5e2fdbacd4f71ba38 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Fri, 17 Jun 2022 10:57:51 -0700
Subject: [PATCH 145/175] initial LOST implementation tested

---
 gtsam/geometry/tests/testTriangulation.cpp | 18 +++++++++
 gtsam/geometry/triangulation.cpp           | 46 ++++++++++++++++++++++
 gtsam/geometry/triangulation.h             | 45 +++++++++++++++++++++
 3 files changed, 109 insertions(+)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index fb66fb6a2..838a5e07f 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -96,6 +96,24 @@ TEST(triangulation, twoPoses) {
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
+TEST(triangulation, twoCamerasLOST) {
+  std::vector<PinholeCamera<Cal3_S2>> cameras = {camera1, camera2};
+  std::vector<Point2> measurements = {z1, z2};
+
+  // 1. Test simple triangulation, perfect in no noise situation
+  Point3 actual1 =  //
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+  EXPECT(assert_equal(landmark, actual1, 1e-12));
+
+  // 3. Add some noise and try again: result should be ~ (4.995,
+  // 0.499167, 1.19814)
+  measurements[0] += Point2(0.1, 0.5);
+  measurements[1] += Point2(-0.2, 0.3);
+  Point3 actual2 =  //
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4));
+}
+
 //******************************************************************************
 // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
 TEST(triangulation, twoPosesCal3DS2) {
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index 026afef24..a02da2eff 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -39,6 +39,11 @@ Vector4 triangulateHomogeneousDLT(
     const Point2& p = measurements.at(i);
 
     // build system of equations
+    // [A_1; A_2; A_3] x = [b_1; b_2; b_3]
+    // [b_3 * A_1 - b_1 * A_3] x = 0
+    // [b_3 * A_2 - b_2 * A_3] x = 0
+    // A' x = 0
+    // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3]
     A.row(row) = p.x() * projection.row(2) - projection.row(0);
     A.row(row + 1) = p.y() * projection.row(2) - projection.row(1);
   }
@@ -53,6 +58,47 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
+Vector3 triangulateLOSTHomogeneous(
+    const std::vector<Pose3>& poses,
+    const std::vector<Point3>& calibrated_measurements) {
+
+  // TODO(akshay-krishnan): make this an argument.
+  const double sigma_x = 1e-3;
+
+  size_t m = calibrated_measurements.size();
+  assert(m == poses.size());
+
+  // Construct the system matrices.
+  Matrix A = Matrix::Zero(m * 2, 3);
+  Matrix b = Matrix::Zero(m * 2, 1);
+
+  for (size_t i = 0; i < m; i++) {
+    const Pose3& wTi = poses[i];
+    // TODO(akshay-krishnan): are there better ways to select j?
+    const int j = (i + 1) % m;
+    const Pose3& wTj = poses[j];
+
+    Point3 d_ij = wTj.translation() -  wTi.translation();
+
+    Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
+    Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
+    
+    double numerator =  w_measurement_i.cross(w_measurement_j).norm();
+    double denominator = d_ij.cross(w_measurement_j).norm();
+
+    double q_i = numerator / (sigma_x * denominator);
+    Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose();
+
+    A.row(2 * i) = coefficient_mat.row(0);
+    A.row(2 * i + 1) = coefficient_mat.row(1);
+    Point2 p = coefficient_mat * wTi.translation();
+    b(2 * i) = p.x();
+    b(2 * i + 1) = p.y();
+  }
+  // Solve Ax = b, using QR decomposition
+  return A.colPivHouseholderQr().solve(b);
+}
+
 Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 401fd2d0b..3b834c615 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -62,6 +62,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
+/**
+ * @brief 
+ * 
+ * @param projection_matrices 
+ * @param measurements 
+ * @param rank_tol 
+ * @return GTSAM_EXPORT 
+ */
+GTSAM_EXPORT Vector3
+triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
+                           const std::vector<Point3>& calibrated_measurements);
+
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
  * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
@@ -382,6 +394,39 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   return point;
 }
 
+template <class CALIBRATION>
+Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& cameras,
+                             const std::vector<Point2>& measurements) {
+  const size_t num_cameras = cameras.size();
+  assert(measurements.size() == num_cameras);
+
+  if (num_cameras < 2) throw(TriangulationUnderconstrainedException());
+
+  // Convert measurements to image plane coordinates.
+  std::vector<Point3> calibrated_measurements;
+  calibrated_measurements.reserve(measurements.size());
+  for (int i = 0; i < measurements.size(); ++i) {
+    Point2 p = cameras[i].calibration().calibrate(measurements[i]);
+    calibrated_measurements.emplace_back(p.x(), p.y(), 1.0);
+  }
+
+  std::vector<Pose3> poses;
+  poses.reserve(cameras.size());
+  for (const auto& camera : cameras) poses.push_back(camera.pose());
+
+  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements);
+
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  // verify that the triangulated point lies in front of all cameras
+  for (const auto& camera : cameras) {
+    const Point3& p_local = camera.pose().transformTo(point);
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
+  }
+#endif
+
+  return point;
+}
+
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. This function is similar to the one

From 5ec6127c0b5bebe03149068f276e53870a0aa8bb Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 18 Jun 2022 11:35:46 -0700
Subject: [PATCH 146/175] add unit test, update doc

---
 gtsam/sfm/TranslationRecovery.cpp |  1 +
 tests/testTranslationRecovery.cpp | 30 ++++++++++++++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 8f1108806..28c2b2e8a 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -156,6 +156,7 @@ Values TranslationRecovery::initializeRandomly(
     insert(edge.key1());
     insert(edge.key2());
   }
+  // There may be nodes in betweenTranslations that do not have a measurement.
   for (auto edge : betweenTranslations) {
     insert(edge.key1());
     insert(edge.key2());
diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp
index 5dd319d30..15f1caa1b 100644
--- a/tests/testTranslationRecovery.cpp
+++ b/tests/testTranslationRecovery.cpp
@@ -323,6 +323,36 @@ TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
   EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
   EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
 }
+
+TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) {
+  // Checks that valid results are obtained when a between translation edge is
+  // provided with a node that does not have any other relative translations.
+  Values poses;
+  poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
+  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
+  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
+  poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
+
+  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
+      poses, {{0, 1}, {0, 3}, {1, 3}});
+
+  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
+  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
+                                   noiseModel::Constrained::All(3, 1e2));
+  // Node 4 only has this between translation prior, no relative translations.
+  betweenTranslations.emplace_back(0, 4, Point3(1, 2, 1));
+
+  TranslationRecovery algorithm;
+  auto result =
+      algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
+
+  // Check result
+  EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
+  EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
+  EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
+  EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

From 6fae6f42313552ca900b9d8af26b1f6698d889e8 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 18 Jun 2022 11:55:55 -0700
Subject: [PATCH 147/175] update docstring

---
 gtsam/sfm/TranslationRecovery.cpp | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp
index 28c2b2e8a..c7ef2e526 100644
--- a/gtsam/sfm/TranslationRecovery.cpp
+++ b/gtsam/sfm/TranslationRecovery.cpp
@@ -119,16 +119,18 @@ void TranslationRecovery::addPrior(
   graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
                                              priorNoiseModel);
 
-  // Add between factors for optional relative translations.
-  for (auto edge : betweenTranslations) {
-    graph->emplace_shared<BetweenFactor<Point3>>(
-        edge.key1(), edge.key2(), edge.measured(), edge.noiseModel());
-  }
-
   // Add a scale prior only if no other between factors were added.
   if (betweenTranslations.empty()) {
     graph->emplace_shared<PriorFactor<Point3>>(
         edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+    return;
+  }
+
+  // Add between factors for optional relative translations.
+  for (auto prior_edge : betweenTranslations) {
+    graph->emplace_shared<BetweenFactor<Point3>>(
+        prior_edge.key1(), prior_edge.key2(), prior_edge.measured(),
+        prior_edge.noiseModel());
   }
 }
 

From e18840baffd3e02fb3c05e76ee2586cf2ae83e77 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sat, 18 Jun 2022 11:56:21 -0700
Subject: [PATCH 148/175] update doc in header

---
 gtsam/sfm/TranslationRecovery.h | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h
index 6ccc39ddb..44a5ef43e 100644
--- a/gtsam/sfm/TranslationRecovery.h
+++ b/gtsam/sfm/TranslationRecovery.h
@@ -143,11 +143,15 @@ class TranslationRecovery {
    *
    * @param relativeTranslations the relative translations, in world coordinate
    * frames, vector of BinaryMeasurements of Unit3, where each key of a
-   * measurement is a point in 3D.
+   * measurement is a point in 3D. If a relative translation magnitude is zero,
+   * it is treated as a hard same-point constraint (the result of all nodes
+   * connected by a zero-magnitude edge will be the same).
    * @param scale scale for first relative translation which fixes gauge.
    * The scale is only used if betweenTranslations is empty.
    * @param betweenTranslations relative translations (with scale) between 2
-   * points in world coordinate frame known a priori.
+   * points in world coordinate frame known a priori. Unlike
+   * relativeTranslations, zero-magnitude betweenTranslations are not treated as
+   * hard constraints.
    * @param initialValues intial values for optimization. Initializes randomly
    * if not provided.
    * @return Values

From 0984fea3f506fa42675e92edbeda4ac9d4df3f5c Mon Sep 17 00:00:00 2001
From: vik748 <vikrantshah@gmail.com>
Date: Mon, 20 Jun 2022 01:18:07 -0700
Subject: [PATCH 149/175] Address matplotlib deprecation warnings for fig.gca()
 and window.set_title().

---
 python/gtsam/examples/Pose3ISAM2Example.py  |  5 ++++-
 python/gtsam/examples/VisualISAM2Example.py |  5 ++++-
 python/gtsam/utils/plot.py                  | 19 ++++++++++++++-----
 3 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py
index cb71813c5..986bd5b9c 100644
--- a/python/gtsam/examples/Pose3ISAM2Example.py
+++ b/python/gtsam/examples/Pose3ISAM2Example.py
@@ -34,7 +34,10 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa
 
     # Plot the newly updated iSAM2 inference.
     fig = plt.figure(0)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plt.cla()
 
     i = 1
diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py
index 4b480fab7..60d4fb376 100644
--- a/python/gtsam/examples/VisualISAM2Example.py
+++ b/python/gtsam/examples/VisualISAM2Example.py
@@ -33,7 +33,10 @@ def visual_ISAM2_plot(result):
     fignum = 0
 
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plt.cla()
 
     # Plot points
diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py
index a4d19f72b..880c436e8 100644
--- a/python/gtsam/utils/plot.py
+++ b/python/gtsam/utils/plot.py
@@ -333,7 +333,10 @@ def plot_point3(
 
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
     plot_point3_on_axes(axes, point, linespec, P)
 
     axes.set_xlabel(axis_labels[0])
@@ -388,7 +391,7 @@ def plot_3d_points(fignum,
 
     fig = plt.figure(fignum)
     fig.suptitle(title)
-    fig.canvas.set_window_title(title.lower())
+    fig.canvas.manager.set_window_title(title.lower())
 
 
 def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
@@ -490,7 +493,10 @@ def plot_trajectory(
         axis_labels (iterable[string]): List of axis labels to set.
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
 
     axes.set_xlabel(axis_labels[0])
     axes.set_ylabel(axis_labels[1])
@@ -522,7 +528,7 @@ def plot_trajectory(
         plot_pose3_on_axes(axes, pose, P=covariance, axis_length=scale)
 
     fig.suptitle(title)
-    fig.canvas.set_window_title(title.lower())
+    fig.canvas.manager.set_window_title(title.lower())
 
 
 def plot_incremental_trajectory(fignum: int,
@@ -545,7 +551,10 @@ def plot_incremental_trajectory(fignum: int,
             Used to create animation effect.
     """
     fig = plt.figure(fignum)
-    axes = fig.gca(projection='3d')
+    if not fig.axes:
+        axes = fig.add_subplot(projection='3d')
+    else:
+        axes = fig.axes[0]
 
     poses = gtsam.utilities.allPose3s(values)
     keys = gtsam.KeyVector(poses.keys())

From 5ea8f2529fc91b82a414a770c027eac14fc3a205 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Mon, 20 Jun 2022 23:20:29 -0700
Subject: [PATCH 150/175] make noise input, add LOST vs DLT unit test

---
 gtsam/geometry/tests/testTriangulation.cpp | 39 +++++++++++++++++++++-
 gtsam/geometry/triangulation.cpp           | 19 +++++------
 gtsam/geometry/triangulation.h             | 11 +++---
 3 files changed, 54 insertions(+), 15 deletions(-)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 838a5e07f..0b885d434 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -109,11 +109,48 @@ TEST(triangulation, twoCamerasLOST) {
   // 0.499167, 1.19814)
   measurements[0] += Point2(0.1, 0.5);
   measurements[1] += Point2(-0.2, 0.3);
+  const double measurement_sigma = 1e-3;
   Point3 actual2 =  //
-      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
+      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements, measurement_sigma);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4));
 }
 
+TEST(triangulation, twoCamerasLOSTvsDLT) {
+  // LOST has been shown to preform better when the point is much closer to one
+  // camera compared to another. This unit test checks this configuration.
+  Cal3_S2 identity_K;
+  Pose3 pose_1;
+  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
+  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
+  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+
+  Point3 gt_point(0, 0, 1);
+  Point2 x1 = camera_1.project(gt_point);
+  Point2 x2 = camera_2.project(gt_point);
+
+  Point2 x1_noisy = x1 + Point2(0.00817, 0.00977);
+  Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969);
+
+  const double measurement_sigma = 1e-2;
+  Point3 estimate_lost = triangulateLOSTPoint3<Cal3_S2>(
+      {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma);
+
+  // These values are from a MATLAB implementation.
+  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3));
+  
+  double rank_tol = 1e-9;
+
+  Pose3Vector poses = {pose_1, pose_2};
+  Point2Vector points = {x1_noisy, x2_noisy};
+  boost::shared_ptr<Cal3_S2> cal = boost::make_shared<Cal3_S2>(identity_K);
+  boost::optional<Point3> estimate_dlt =
+      triangulatePoint3<Cal3_S2>(poses, cal, points, rank_tol, false);
+
+  // The LOST estimate should have a smaller error.
+  EXPECT((gt_point - estimate_lost).norm() <=
+         (gt_point - *estimate_dlt).norm());
+}
+
 //******************************************************************************
 // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
 TEST(triangulation, twoPosesCal3DS2) {
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index a02da2eff..bec239240 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -60,11 +60,8 @@ Vector4 triangulateHomogeneousDLT(
 
 Vector3 triangulateLOSTHomogeneous(
     const std::vector<Pose3>& poses,
-    const std::vector<Point3>& calibrated_measurements) {
-
-  // TODO(akshay-krishnan): make this an argument.
-  const double sigma_x = 1e-3;
-
+    const std::vector<Point3>& calibrated_measurements, 
+    const double measurement_sigma) {
   size_t m = calibrated_measurements.size();
   assert(m == poses.size());
 
@@ -78,16 +75,18 @@ Vector3 triangulateLOSTHomogeneous(
     const int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    Point3 d_ij = wTj.translation() -  wTi.translation();
+    Point3 d_ij = wTj.translation() - wTi.translation();
 
     Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
     Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
-    
-    double numerator =  w_measurement_i.cross(w_measurement_j).norm();
+
+    double numerator = w_measurement_i.cross(w_measurement_j).norm();
     double denominator = d_ij.cross(w_measurement_j).norm();
 
-    double q_i = numerator / (sigma_x * denominator);
-    Matrix23 coefficient_mat = q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose();
+    double q_i = numerator / (measurement_sigma * denominator);
+    Matrix23 coefficient_mat =
+        q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
+        wTi.rotation().matrix().transpose();
 
     A.row(2 * i) = coefficient_mat.row(0);
     A.row(2 * i + 1) = coefficient_mat.row(1);
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 3b834c615..d31630e67 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -72,7 +72,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
  */
 GTSAM_EXPORT Vector3
 triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
-                           const std::vector<Point3>& calibrated_measurements);
+                           const std::vector<Point3>& calibrated_measurements,
+                           const double measurement_sigma);
 
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
@@ -395,8 +396,10 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 }
 
 template <class CALIBRATION>
-Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& cameras,
-                             const std::vector<Point2>& measurements) {
+Point3 triangulateLOSTPoint3(
+    const std::vector<PinholeCamera<CALIBRATION>>& cameras,
+    const std::vector<Point2>& measurements,
+    const double measurement_sigma = 1e-2) {
   const size_t num_cameras = cameras.size();
   assert(measurements.size() == num_cameras);
 
@@ -414,7 +417,7 @@ Point3 triangulateLOSTPoint3(const std::vector<PinholeCamera<CALIBRATION>>& came
   poses.reserve(cameras.size());
   for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements);
+  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras

From a1e45e0df502b8d02f3f9b8fdf6d456dfb6fc956 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 25 Jun 2022 23:34:16 -0400
Subject: [PATCH 151/175] add this-> to fix ROS issue

---
 gtsam/sam/BearingRangeFactor.h | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index da352f2e9..9874760c4 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -79,9 +79,8 @@ class BearingRangeFactor
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
-    const Vector error = unwhitenedError(
-      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
-      Hs);
+    const Vector error = this->unwhitenedError(
+        {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
     if (H1) *H1 = Hs[0];
     if (H2) *H2 = Hs[1];
     return error;

From ca4b450e7ee98a2d6344df2120148c958e6ab410 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 28 Jun 2022 00:59:20 -0700
Subject: [PATCH 152/175] experimenting with LS and TLS

---
 gtsam/geometry/triangulation.cpp | 66 +++++++++++++++++++++++++-------
 gtsam/geometry/triangulation.h   | 17 ++++++--
 2 files changed, 65 insertions(+), 18 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index bec239240..9d8325d68 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -65,6 +65,50 @@ Vector3 triangulateLOSTHomogeneous(
   size_t m = calibrated_measurements.size();
   assert(m == poses.size());
 
+  // Construct the system matrices.
+  Matrix A = Matrix::Zero(m * 2, 4);
+
+  for (size_t i = 0; i < m; i++) {
+    const Pose3& wTi = poses[i];
+    // TODO(akshay-krishnan): are there better ways to select j?
+    const int j = (i + 1) % m;
+    const Pose3& wTj = poses[j];
+
+    const Point3 d_ij = wTj.translation() - wTi.translation();
+
+    const Point3 w_measurement_i =
+        wTi.rotation().rotate(calibrated_measurements[i]);
+    const Point3 w_measurement_j =
+        wTj.rotation().rotate(calibrated_measurements[j]);
+
+    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
+                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
+    const Matrix23 coefficient_mat =
+        q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
+        wTi.rotation().matrix().transpose();
+
+    A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation();
+  }
+
+  const double rank_tol = 1e-6;
+  int rank;
+  double error;
+  Vector v;
+  boost::tie(rank, error, v) = DLT(A, rank_tol);
+
+  if (rank < 3)
+    throw(TriangulationUnderconstrainedException());
+
+  return Point3(v.head<3>() / v[3]);
+}
+
+Vector3 triangulateLOSTHomogeneousLS(
+    const std::vector<Pose3>& poses,
+    const std::vector<Point3>& calibrated_measurements, 
+    const double measurement_sigma) {
+  size_t m = calibrated_measurements.size();
+  assert(m == poses.size());
+
   // Construct the system matrices.
   Matrix A = Matrix::Zero(m * 2, 3);
   Matrix b = Matrix::Zero(m * 2, 1);
@@ -75,26 +119,20 @@ Vector3 triangulateLOSTHomogeneous(
     const int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    Point3 d_ij = wTj.translation() - wTi.translation();
+    const Point3 d_ij = wTj.translation() - wTi.translation();
 
-    Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
-    Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
+    const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
+    const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
 
-    double numerator = w_measurement_i.cross(w_measurement_j).norm();
-    double denominator = d_ij.cross(w_measurement_j).norm();
-
-    double q_i = numerator / (measurement_sigma * denominator);
-    Matrix23 coefficient_mat =
+    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
+                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
+    const Matrix23 coefficient_mat =
         q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
         wTi.rotation().matrix().transpose();
 
-    A.row(2 * i) = coefficient_mat.row(0);
-    A.row(2 * i + 1) = coefficient_mat.row(1);
-    Point2 p = coefficient_mat * wTi.translation();
-    b(2 * i) = p.x();
-    b(2 * i + 1) = p.y();
+    A.block<2, 3>(2*i, 0) = coefficient_mat;
+    b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation();
   }
-  // Solve Ax = b, using QR decomposition
   return A.colPivHouseholderQr().solve(b);
 }
 
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index d31630e67..5bea4d93c 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -75,6 +75,11 @@ triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
                            const std::vector<Point3>& calibrated_measurements,
                            const double measurement_sigma);
 
+GTSAM_EXPORT Vector3
+triangulateLOSTHomogeneousLS(const std::vector<Pose3>& poses,
+                           const std::vector<Point3>& calibrated_measurements,
+                           const double measurement_sigma);
+
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
  * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
@@ -397,9 +402,9 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
 
 template <class CALIBRATION>
 Point3 triangulateLOSTPoint3(
-    const std::vector<PinholeCamera<CALIBRATION>>& cameras,
-    const std::vector<Point2>& measurements,
-    const double measurement_sigma = 1e-2) {
+    const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
+    const Point2Vector& measurements,
+    const double measurement_sigma = 1e-2, bool use_dlt = false) {
   const size_t num_cameras = cameras.size();
   assert(measurements.size() == num_cameras);
 
@@ -417,7 +422,11 @@ Point3 triangulateLOSTPoint3(
   poses.reserve(cameras.size());
   for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point = triangulateLOSTHomogeneous(poses, calibrated_measurements, measurement_sigma);
+  Point3 point = use_dlt
+                     ? triangulateLOSTHomogeneous(
+                           poses, calibrated_measurements, measurement_sigma)
+                     : triangulateLOSTHomogeneousLS(
+                           poses, calibrated_measurements, measurement_sigma);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras

From 0d501b6de153bfc13044816480bc5752044e3bf6 Mon Sep 17 00:00:00 2001
From: yetongumich <yetong@umich.edu>
Date: Thu, 30 Jun 2022 23:38:34 -0400
Subject: [PATCH 153/175] fix testNoiseModel

---
 gtsam/linear/tests/testNoiseModel.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp
index 92774a133..f83ba7831 100644
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/gtsam/linear/tests/testNoiseModel.cpp
@@ -661,14 +661,15 @@ TEST(NoiseModel, robustNoiseDCS)
 TEST(NoiseModel, robustNoiseL2WithDeadZone)
 {
   double dead_zone_size = 1.0;
-  SharedNoiseModel robust = noiseModel::Robust::Create(
+  auto robust = noiseModel::Robust::Create(
       noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
       Unit::Create(3));
 
   for (int i = 0; i < 5; i++) {
-    Vector3 error = Vector3(i, 0, 0);
+    Vector error = Vector3(i, 0, 0);
+    robust->WhitenSystem(error);
     DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
-                  robust->loss(robust->squaredMahalanobisDistance(error)), 1e-8);
+                  robust->squaredMahalanobisDistance(error), 1e-8);
   }
 }
 

From f947b973cdc5d59214f95b80531f17c4f538b771 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 2 Jul 2022 16:02:42 -0400
Subject: [PATCH 154/175] addressed comments in PR

---
 gtsam/geometry/triangulation.h | 4 ++--
 gtsam/nonlinear/GncOptimizer.h | 6 ++++--
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index af01d3a36..a4c6b1a13 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -107,7 +107,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
     const Point2Vector& measurements, Key landmarkKey,
     const Point3& initialEstimate,
-    const SharedNoiseModel& model = nullptr) {
+    const SharedNoiseModel& model = unit2) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
@@ -117,7 +117,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     typedef PinholePose<CALIBRATION> Camera;
     Camera camera_i(pose_i, sharedCal);
     graph.emplace_shared<TriangulationFactor<Camera> > //
-        (camera_i, measurements[i], model? model : unit2, landmarkKey);
+        (camera_i, measurements[i], model, landmarkKey);
   }
   return std::make_pair(graph, values);
 }
diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index 96b03e803..e5b4718d6 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer {
                 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
           }
         }
-        if (mu_init >= 0 && mu_init < 1e-6)
+        if (mu_init >= 0 && mu_init < 1e-6){
           mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
-        // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
+          // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
+        }
+  
         return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
         // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
         // and there is no need to robustify (TLS = least squares)

From 79305c1a9d51bdb62f43993a20ad32b5b6fc8ad2 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Sat, 2 Jul 2022 17:05:14 -0400
Subject: [PATCH 155/175] fixed compile issue

---
 gtsam/geometry/triangulation.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index a4c6b1a13..486850903 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -107,11 +107,10 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
     const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
     const Point2Vector& measurements, Key landmarkKey,
     const Point3& initialEstimate,
-    const SharedNoiseModel& model = unit2) {
+    const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
   Values values;
   values.insert(landmarkKey, initialEstimate); // Initial landmark value
   NonlinearFactorGraph graph;
-  static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
   for (size_t i = 0; i < measurements.size(); i++) {
     const Pose3& pose_i = poses[i];
     typedef PinholePose<CALIBRATION> Camera;

From aab6af3368ead2be9ef0aa885de5a4777e0c79a4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 3 Jul 2022 11:09:46 -0400
Subject: [PATCH 156/175] replace boost::function with std::function

---
 gtsam/basis/Chebyshev2.h             | 4 +---
 gtsam/basis/tests/testChebyshev2.cpp | 2 +-
 gtsam/discrete/DecisionTree.h        | 2 +-
 gtsam/geometry/tests/testPoint3.cpp  | 2 --
 4 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h
index e306c93d5..4c3542d56 100644
--- a/gtsam/basis/Chebyshev2.h
+++ b/gtsam/basis/Chebyshev2.h
@@ -36,8 +36,6 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/basis/Basis.h>
 
-#include <boost/function.hpp>
-
 namespace gtsam {
 
 /**
@@ -134,7 +132,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
    * Create matrix of values at Chebyshev points given vector-valued function.
    */
   template <size_t M>
-  static Matrix matrix(boost::function<Eigen::Matrix<double, M, 1>(double)> f,
+  static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
                        size_t N, double a = -1, double b = 1) {
     Matrix Xmat(M, N);
     for (size_t j = 0; j < N; j++) {
diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp
index 9090757f4..73339e0f1 100644
--- a/gtsam/basis/tests/testChebyshev2.cpp
+++ b/gtsam/basis/tests/testChebyshev2.cpp
@@ -118,7 +118,7 @@ TEST(Chebyshev2, InterpolateVector) {
   EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
 
   // Check derivative
-  boost::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
+  std::function<Vector2(ParameterMatrix<2>)> f = boost::bind(
       &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none);
   Matrix numericalH =
       numericalDerivative11<Vector2, ParameterMatrix<2>, 2 * N>(f, X);
diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h
index 7449ae995..dede2a2f0 100644
--- a/gtsam/discrete/DecisionTree.h
+++ b/gtsam/discrete/DecisionTree.h
@@ -22,7 +22,7 @@
 #include <gtsam/base/types.h>
 #include <gtsam/discrete/Assignment.h>
 
-#include <boost/function.hpp>
+#include <boost/shared_ptr.hpp>
 #include <functional>
 #include <iostream>
 #include <map>
diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index 315391ac8..e373e1d52 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -19,8 +19,6 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Point3.h>
 
-#include <boost/function.hpp>
-
 using namespace std::placeholders;
 using namespace gtsam;
 

From b0369c48c8cab250589e28c12062c869699ca762 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 3 Jul 2022 11:10:27 -0400
Subject: [PATCH 157/175] import VectorSerialization to Point3 to allow
 serialization for downstream classes

---
 gtsam/geometry/Point3.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 835c53d7c..f47531963 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -25,6 +25,7 @@
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
+#include <gtsam/base/VectorSerialization.h>
 #include <boost/serialization/nvp.hpp>
 #include <numeric>
 

From 3595e8588c84a6e8488cd948664c765bce92bf40 Mon Sep 17 00:00:00 2001
From: yotams <yotams@mobileye.com>
Date: Mon, 4 Jul 2022 11:27:49 +0300
Subject: [PATCH 158/175] added jacobians for all pose3 methods in python
 wrappers

---
 gtsam/geometry/Pose3.cpp  |  5 +++++
 gtsam/geometry/Pose3.h    |  8 ++++++++
 gtsam/geometry/geometry.i | 33 +++++++++++++++++++++++++++++++++
 3 files changed, 46 insertions(+)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 2da51a625..f261a2302 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -489,6 +489,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
 }
 #endif
 
+/* ************************************************************************* */
+Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
+  return interpolate(*this, other, t, Hx, Hy);
+}
+
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   // Both Rot3 and Point3 have ostream definitions so we use them.
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index c36047349..e2128b28d 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -379,6 +379,14 @@ public:
     return std::make_pair(0, 2);
   }
 
+    /**
+   * @brief Spherical Linear interpolation between *this and other
+   * @param s a value between 0 and 1.5
+   * @param other final point of interpolation geodesic on manifold
+   */
+  Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
+                                             OptionalJacobian<6, 6> Hy = boost::none) const;
+
   /// Output stream operator
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 517558265..4deb5c48a 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -446,24 +446,43 @@ class Pose3 {
   // Group
   static gtsam::Pose3 identity();
   gtsam::Pose3 inverse() const;
+  gtsam::Pose3 inverse(Eigen::Ref<Eigen::MatrixXd> H) const;
   gtsam::Pose3 compose(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 compose(const gtsam::Pose3& pose,
+                       Eigen::Ref<Eigen::MatrixXd> H1,
+                       Eigen::Ref<Eigen::MatrixXd> H2) const;
   gtsam::Pose3 between(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 between(const gtsam::Pose3& pose,
+                       Eigen::Ref<Eigen::MatrixXd> H1,
+                       Eigen::Ref<Eigen::MatrixXd> H2) const;
+  gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const;
+  gtsam::Pose3 interp(double t, const gtsam::Pose3& pose,
+                           Eigen::Ref<Eigen::MatrixXd> Hx,
+                           Eigen::Ref<Eigen::MatrixXd> Hy) const;
 
   // Operator Overloads
   gtsam::Pose3 operator*(const gtsam::Pose3& pose) const;
 
   // Manifold
   gtsam::Pose3 retract(Vector v) const;
+  gtsam::Pose3 retract(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
   Vector localCoordinates(const gtsam::Pose3& pose) const;
+  Vector localCoordinates(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hxi) const;
 
   // Lie Group
   static gtsam::Pose3 Expmap(Vector v);
+  static gtsam::Pose3 Expmap(Vector v, Eigen::Ref<Eigen::MatrixXd> Hxi);
   static Vector Logmap(const gtsam::Pose3& pose);
+  static Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
   gtsam::Pose3 expmap(Vector v);
   Vector logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
+  Vector Adjoint(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
+                 Eigen::Ref<Eigen::MatrixXd> H_xib) const;
   Vector AdjointTranspose(Vector xi) const;
+  Vector AdjointTranspose(Vector xi, Eigen::Ref<Eigen::MatrixXd> H_this,
+                          Eigen::Ref<Eigen::MatrixXd> H_x) const;
   static Matrix adjointMap(Vector xi);
   static Vector adjoint(Vector xi, Vector y);
   static Matrix adjointMap_(Vector xi);
@@ -476,7 +495,11 @@ class Pose3 {
 
   // Group Action on Point3
   gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
+  gtsam::Point3 transformFrom(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
+                              Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
   gtsam::Point3 transformTo(const gtsam::Point3& point) const;
+  gtsam::Point3 transformTo(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
+                            Eigen::Ref<Eigen::MatrixXd> Hpoint) const;
 
   // Matrix versions
   Matrix transformFrom(const Matrix& points) const;
@@ -484,15 +507,25 @@ class Pose3 {
 
   // Standard Interface
   gtsam::Rot3 rotation() const;
+  gtsam::Rot3 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   gtsam::Point3 translation() const;
+  gtsam::Point3 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   double x() const;
   double y() const;
   double z() const;
   Matrix matrix() const;
   gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
+                                 Eigen::Ref<Eigen::MatrixXd> HaTb) const;
   gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const;
+  gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
+                               Eigen::Ref<Eigen::MatrixXd> HwTb) const;
   double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Hself,
+               Eigen::Ref<Eigen::MatrixXd> Hpoint);
   double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hself,
+               Eigen::Ref<Eigen::MatrixXd> Hpose);
 
   // enabling serialization functionality
   void serialize() const;

From aaeeccf8f59e26c479b39d6e8a12c78008d4e43f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 4 Jul 2022 14:55:39 -0400
Subject: [PATCH 159/175] Squashed 'wrap/' changes from 1a7dc9722..ca357ccdd

ca357ccdd Merge pull request #149 from borglab/install-package
886846724 set the GTWRAP_PATH_SEPARATOR properly for MatlabWrap
4abed7fa0 install the python package explicitly

git-subtree-dir: wrap
git-subtree-split: ca357ccdd27f0661e73ff7a1771768dc4bf8f746
---
 cmake/MatlabWrap.cmake | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/cmake/MatlabWrap.cmake b/cmake/MatlabWrap.cmake
index 3cb058102..eaffcc059 100644
--- a/cmake/MatlabWrap.cmake
+++ b/cmake/MatlabWrap.cmake
@@ -242,6 +242,13 @@ function(wrap_library_internal interfaceHeader moduleName linkLibraries extraInc
   find_package(PythonInterp ${WRAP_PYTHON_VERSION} EXACT)
   find_package(PythonLibs ${WRAP_PYTHON_VERSION} EXACT)
 
+  # Set the path separator for PYTHONPATH
+  if(UNIX)
+    set(GTWRAP_PATH_SEPARATOR ":")
+  else()
+    set(GTWRAP_PATH_SEPARATOR ";")
+  endif()
+
   add_custom_command(
     OUTPUT ${generated_cpp_file}
     DEPENDS ${interfaceHeader} ${module_library_target} ${otherLibraryTargets}

From 7348586f60b04c464fe674ee939449980ae0efc2 Mon Sep 17 00:00:00 2001
From: yotams <yotams@mobileye.com>
Date: Thu, 7 Jul 2022 14:09:10 +0300
Subject: [PATCH 160/175] 1. changed interp name to slerp 2. added python tests
 for jacobians for some pose3 apis

---
 gtsam/geometry/Pose3.cpp         |   2 +-
 gtsam/geometry/Pose3.h           |   2 +-
 gtsam/geometry/geometry.i        |   4 +-
 python/gtsam/tests/test_Pose3.py | 118 ++++++++++++++++++++++++++++++-
 4 files changed, 121 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index f261a2302..2938e90f5 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -490,7 +490,7 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
 #endif
 
 /* ************************************************************************* */
-Pose3 Pose3::interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
+Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const {
   return interpolate(*this, other, t, Hx, Hy);
 }
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index e2128b28d..33fb55250 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -384,7 +384,7 @@ public:
    * @param s a value between 0 and 1.5
    * @param other final point of interpolation geodesic on manifold
    */
-  Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
+  Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
                                              OptionalJacobian<6, 6> Hy = boost::none) const;
 
   /// Output stream operator
diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 4deb5c48a..92d9ec0a3 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -455,8 +455,8 @@ class Pose3 {
   gtsam::Pose3 between(const gtsam::Pose3& pose,
                        Eigen::Ref<Eigen::MatrixXd> H1,
                        Eigen::Ref<Eigen::MatrixXd> H2) const;
-  gtsam::Pose3 interp(double t, const gtsam::Pose3& pose) const;
-  gtsam::Pose3 interp(double t, const gtsam::Pose3& pose,
+  gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose) const;
+  gtsam::Pose3 slerp(double t, const gtsam::Pose3& pose,
                            Eigen::Ref<Eigen::MatrixXd> Hx,
                            Eigen::Ref<Eigen::MatrixXd> Hy) const;
 
diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py
index cde71de53..cdc214a2c 100644
--- a/python/gtsam/tests/test_Pose3.py
+++ b/python/gtsam/tests/test_Pose3.py
@@ -19,6 +19,62 @@ from gtsam import Point3, Pose3, Rot3, Point3Pairs
 from gtsam.utils.test_case import GtsamTestCase
 
 
+def numerical_derivative_pose(pose, method, delta=1e-5):
+    jacobian = np.zeros((6, 6))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+        pose_plus = pose.retract(xplus).__getattribute__(method)()
+        pose_minus = pose.retract(xminus).__getattribute__(method)()
+        jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
+    return jacobian
+
+
+def numerical_derivative_2_poses(pose, other_pose, method, delta=1e-5, inputs=()):
+    jacobian = np.zeros((6, 6))
+    other_jacobian = np.zeros((6, 6))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+
+        pose_plus = pose.retract(xplus).__getattribute__(method)(*inputs, other_pose)
+        pose_minus = pose.retract(xminus).__getattribute__(method)(*inputs, other_pose)
+        jacobian[:, idx] = pose_minus.localCoordinates(pose_plus) / (2 * delta)
+
+        other_pose_plus = pose.__getattribute__(method)(*inputs, other_pose.retract(xplus))
+        other_pose_minus = pose.__getattribute__(method)(*inputs, other_pose.retract(xminus))
+        other_jacobian[:, idx] = other_pose_minus.localCoordinates(other_pose_plus) / (2 * delta)
+    return jacobian, other_jacobian
+
+
+def numerical_derivative_pose_point(pose, point, method, delta=1e-5):
+    jacobian = np.zeros((3, 6))
+    point_jacobian = np.zeros((3, 3))
+    for idx in range(6):
+        xplus = np.zeros(6)
+        xplus[idx] = delta
+        xminus = np.zeros(6)
+        xminus[idx] = -delta
+
+        point_plus = pose.retract(xplus).__getattribute__(method)(point)
+        point_minus = pose.retract(xminus).__getattribute__(method)(point)
+        jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
+
+        if idx < 3:
+            xplus = np.zeros(3)
+            xplus[idx] = delta
+            xminus = np.zeros(3)
+            xminus[idx] = -delta
+            point_plus = pose.__getattribute__(method)(point + xplus)
+            point_minus = pose.__getattribute__(method)(point + xminus)
+            point_jacobian[:, idx] = (point_plus - point_minus) / (2 * delta)
+    return jacobian, point_jacobian
+
+
 class TestPose3(GtsamTestCase):
     """Test selected Pose3 methods."""
 
@@ -30,6 +86,47 @@ class TestPose3(GtsamTestCase):
         actual = T2.between(T3)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        jacobian_other = np.zeros((6, 6), order='F')
+        T2.between(T3, jacobian, jacobian_other)
+        jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(T2, T3, 'between')
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+        self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
+
+    def test_inverse(self):
+        """Test between method."""
+        pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        expected = Pose3(Rot3.Rodrigues(0, 0, math.pi/2), Point3(4, -2, 0))
+        actual = pose.inverse()
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        #test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        pose.inverse(jacobian)
+        jacobian_numerical = numerical_derivative_pose(pose, 'inverse')
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+
+    def test_slerp(self):
+        """Test slerp method."""
+        pose0 = gtsam.Pose3()
+        pose1 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        actual_alpha_0 = pose0.slerp(0, pose1)
+        self.gtsamAssertEquals(actual_alpha_0, pose0)
+        actual_alpha_1 = pose0.slerp(1, pose1)
+        self.gtsamAssertEquals(actual_alpha_1, pose1)
+        actual_alpha_half = pose0.slerp(0.5, pose1)
+        expected_alpha_half = Pose3(Rot3.Rodrigues(0, 0, -math.pi/4), Point3(0.17157288, 2.41421356, 0))
+        self.gtsamAssertEquals(actual_alpha_half, expected_alpha_half, tol=1e-6)
+
+        # test jacobians
+        jacobian = np.zeros((6, 6), order='F')
+        jacobian_other = np.zeros((6, 6), order='F')
+        pose0.slerp(0.5, pose1, jacobian, jacobian_other)
+        jacobian_numerical, jacobian_numerical_other = numerical_derivative_2_poses(pose0, pose1, 'slerp', inputs=[0.5])
+        self.gtsamAssertEquals(jacobian, jacobian_numerical)
+        self.gtsamAssertEquals(jacobian_other, jacobian_numerical_other)
+
     def test_transformTo(self):
         """Test transformTo method."""
         pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
@@ -37,6 +134,15 @@ class TestPose3(GtsamTestCase):
         expected = Point3(2, 1, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        point = Point3(3, 2, 10)
+        jacobian_pose = np.zeros((3, 6), order='F')
+        jacobian_point = np.zeros((3, 3), order='F')
+        pose.transformTo(point, jacobian_pose, jacobian_point)
+        jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformTo')
+        self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
+        self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
+
         # multi-point version
         points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T
         actual_array = pose.transformTo(points)
@@ -51,6 +157,15 @@ class TestPose3(GtsamTestCase):
         expected = Point3(3, 2, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        #test jacobians
+        point = Point3(3, 2, 10)
+        jacobian_pose = np.zeros((3, 6), order='F')
+        jacobian_point = np.zeros((3, 3), order='F')
+        pose.transformFrom(point, jacobian_pose, jacobian_point)
+        jacobian_numerical_pose, jacobian_numerical_point = numerical_derivative_pose_point(pose, point, 'transformFrom')
+        self.gtsamAssertEquals(jacobian_pose, jacobian_numerical_pose)
+        self.gtsamAssertEquals(jacobian_point, jacobian_numerical_point)
+
         # multi-point version
         points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
         actual_array = pose.transformFrom(points)
@@ -122,4 +237,5 @@ class TestPose3(GtsamTestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    # unittest.main()
+    TestPose3().test_slerp()
\ No newline at end of file

From 7c077f8536c1a06710c251d16b1760ef55be11aa Mon Sep 17 00:00:00 2001
From: yotams <yotams@mobileye.com>
Date: Thu, 7 Jul 2022 14:20:26 +0300
Subject: [PATCH 161/175] bugfix

---
 python/gtsam/tests/test_Pose3.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py
index cdc214a2c..4464e8d47 100644
--- a/python/gtsam/tests/test_Pose3.py
+++ b/python/gtsam/tests/test_Pose3.py
@@ -237,5 +237,4 @@ class TestPose3(GtsamTestCase):
 
 
 if __name__ == "__main__":
-    # unittest.main()
-    TestPose3().test_slerp()
\ No newline at end of file
+    unittest.main()
\ No newline at end of file

From f5ec070f9fc87d52076846ff394b79b001f4bd13 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 7 Jul 2022 15:09:55 -0400
Subject: [PATCH 162/175] wrap BearingRange classes and factors

---
 gtsam/geometry/geometry.i | 6 ++++++
 gtsam/sam/sam.i           | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 92d9ec0a3..f3f8a5c79 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1279,5 +1279,11 @@ class BearingRange {
 
 typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double>
     BearingRange2D;
+typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double>
+    BearingRangePose2;
+typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Point3, gtsam::Unit3, double>
+    BearingRange3D;
+typedef gtsam::BearingRange<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3, double>
+    BearingRangePose3;
 
 }  // namespace gtsam
diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i
index 90c319ede..a46a6de9e 100644
--- a/gtsam/sam/sam.i
+++ b/gtsam/sam/sam.i
@@ -108,5 +108,11 @@ typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2,
 typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2,
                                   double>
     BearingRangeFactorPose2;
+typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3, gtsam::Unit3,
+                                  double>
+    BearingRangeFactor3D;
+typedef gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Pose3, gtsam::Unit3,
+                                  double>
+    BearingRangeFactorPose3;
 
 }  // namespace gtsam

From 2ab09a580fa059b7dd5cd0750365927f81f5a5e0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 7 Jul 2022 15:24:56 -0400
Subject: [PATCH 163/175] add Python unit tests

---
 gtsam/linear/linear.i              |  2 +-
 python/gtsam/tests/test_Factors.py | 22 ++++++++++++++++++++--
 python/gtsam/tests/test_sam.py     | 28 ++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i
index 943b661d8..fdf156ff9 100644
--- a/gtsam/linear/linear.i
+++ b/gtsam/linear/linear.i
@@ -279,7 +279,6 @@ virtual class GaussianFactor {
 virtual class JacobianFactor : gtsam::GaussianFactor {
   //Constructors
   JacobianFactor();
-  JacobianFactor(const gtsam::GaussianFactor& factor);
   JacobianFactor(Vector b_in);
   JacobianFactor(size_t i1, Matrix A1, Vector b,
       const gtsam::noiseModel::Diagonal* model);
@@ -295,6 +294,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   JacobianFactor(const gtsam::GaussianFactorGraph& graph,
                  const gtsam::Ordering& ordering,
                  const gtsam::VariableSlots& p_variableSlots);
+  JacobianFactor(const gtsam::GaussianFactor& factor);
 
   //Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
diff --git a/python/gtsam/tests/test_Factors.py b/python/gtsam/tests/test_Factors.py
index 3ec8648a4..a5688eec8 100644
--- a/python/gtsam/tests/test_Factors.py
+++ b/python/gtsam/tests/test_Factors.py
@@ -11,9 +11,8 @@ Author: Varun Agrawal
 """
 import unittest
 
-import numpy as np
-
 import gtsam
+import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -21,6 +20,7 @@ class TestNonlinearEquality2Factor(GtsamTestCase):
     """
     Test various instantiations of NonlinearEquality2.
     """
+
     def test_point3(self):
         """Test for Point3 version."""
         factor = gtsam.NonlinearEquality2Point3(0, 1)
@@ -30,5 +30,23 @@ class TestNonlinearEquality2Factor(GtsamTestCase):
         np.testing.assert_allclose(error, np.zeros(3))
 
 
+class TestJacobianFactor(GtsamTestCase):
+    """Test JacobianFactor"""
+
+    def test_gaussian_factor_graph(self):
+        """Test construction from GaussianFactorGraph."""
+        gfg = gtsam.GaussianFactorGraph()
+        jf = gtsam.JacobianFactor(gfg)
+        self.assertIsInstance(jf, gtsam.JacobianFactor)
+
+        nfg = gtsam.NonlinearFactorGraph()
+        nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))
+        values = gtsam.Values()
+        values.insert(1, 0.0)
+        gfg = nfg.linearize(values)
+        jf = gtsam.JacobianFactor(gfg)
+        self.assertIsInstance(jf, gtsam.JacobianFactor)
+
+
 if __name__ == "__main__":
     unittest.main()
diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py
index e01b9c1d1..fd8da5308 100644
--- a/python/gtsam/tests/test_sam.py
+++ b/python/gtsam/tests/test_sam.py
@@ -50,6 +50,34 @@ class TestSam(GtsamTestCase):
         self.assertEqual(range_measurement, measurement.range())
         self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
 
+    def test_BearingRangeFactor3D(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactor3D.
+        """
+        bearing_measurement = gtsam.Unit3()
+        range_measurement = 10.0
+        factor = gtsam.BearingRangeFactor3D(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(3, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
+    def test_BearingRangeFactorPose3(self):
+        """
+        Test that `measured` works as expected for BearingRangeFactorPose3.
+        """
+        range_measurement = 10.0
+        bearing_measurement = gtsam.Unit3()
+        factor = gtsam.BearingRangeFactorPose3(
+            1, 2, bearing_measurement, range_measurement,
+            gtsam.noiseModel.Isotropic.Sigma(3, 1))
+        measurement = factor.measured()
+
+        self.assertEqual(range_measurement, measurement.range())
+        self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
+
 
 if __name__ == "__main__":
     unittest.main()

From 63e0446f0b04fe23e1224b19e9f819c0f9c9a249 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sun, 10 Jul 2022 16:49:15 -0700
Subject: [PATCH 164/175] update triangulation LOST API

---
 gtsam/geometry/Point3.h                    |   1 +
 gtsam/geometry/tests/testTriangulation.cpp |  63 +++---
 gtsam/geometry/triangulation.cpp           | 159 +++++++--------
 gtsam/geometry/triangulation.h             | 218 +++++++++++++--------
 4 files changed, 247 insertions(+), 194 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 835c53d7c..b1ac6195b 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -33,6 +33,7 @@ namespace gtsam {
 /// As of GTSAM 4, in order to make GTSAM more lean,
 /// it is now possible to just typedef Point3 to Vector3
 typedef Vector3 Point3;
+typedef std::vector<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
 
 // Convenience typedef
 using Point3Pair = std::pair<Point3, Point3>;
diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 0b885d434..731558327 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -96,23 +96,33 @@ TEST(triangulation, twoPoses) {
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
-TEST(triangulation, twoCamerasLOST) {
-  std::vector<PinholeCamera<Cal3_S2>> cameras = {camera1, camera2};
-  std::vector<Point2> measurements = {z1, z2};
+TEST(triangulation, twoCamerasUsingLOST) {
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  cameras.push_back(camera1);
+  cameras.push_back(camera2);
+
+  Point2Vector measurements = {z1, z2};
+  SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
+  double rank_tol = 1e-9;
 
   // 1. Test simple triangulation, perfect in no noise situation
-  Point3 actual1 =  //
-      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements);
-  EXPECT(assert_equal(landmark, actual1, 1e-12));
+  boost::optional<Point3> actual1 =  //
+      triangulatePoint3<PinholeCamera<Cal3_S2>>(
+          cameras, measurements, rank_tol,
+          /*optimize=*/false, measurementNoise,
+          /*use_lost_triangulation=*/true);
+  EXPECT(assert_equal(landmark, *actual1, 1e-12));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
   measurements[0] += Point2(0.1, 0.5);
   measurements[1] += Point2(-0.2, 0.3);
-  const double measurement_sigma = 1e-3;
-  Point3 actual2 =  //
-      triangulateLOSTPoint3<Cal3_S2>(cameras, measurements, measurement_sigma);
-  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), actual2, 1e-4));
+  boost::optional<Point3> actual2 =  //
+      triangulatePoint3<PinholeCamera<Cal3_S2>>(
+          cameras, measurements, rank_tol,
+          /*optimize=*/false, measurementNoise,
+          /*use_lost_triangulation=*/true);
+  EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual2, 1e-4));
 }
 
 TEST(triangulation, twoCamerasLOSTvsDLT) {
@@ -121,33 +131,32 @@ TEST(triangulation, twoCamerasLOSTvsDLT) {
   Cal3_S2 identity_K;
   Pose3 pose_1;
   Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
-  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
-  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  cameras.emplace_back(pose_1, identity_K);
+  cameras.emplace_back(pose_2, identity_K);
 
   Point3 gt_point(0, 0, 1);
-  Point2 x1 = camera_1.project(gt_point);
-  Point2 x2 = camera_2.project(gt_point);
+  Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977);
+  Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969);
+  Point2Vector measurements = {x1_noisy, x2_noisy};
 
-  Point2 x1_noisy = x1 + Point2(0.00817, 0.00977);
-  Point2 x2_noisy = x2 + Point2(-0.00610, 0.01969);
+  double rank_tol = 1e-9;
+  SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
 
-  const double measurement_sigma = 1e-2;
-  Point3 estimate_lost = triangulateLOSTPoint3<Cal3_S2>(
-      {camera_1, camera_2}, {x1_noisy, x2_noisy}, measurement_sigma);
+  boost::optional<Point3> estimate_lost =  //
+      triangulatePoint3<PinholeCamera<Cal3_S2>>(
+          cameras, measurements, rank_tol,
+          /*optimize=*/false, measurementNoise,
+          /*use_lost_triangulation=*/true);
 
   // These values are from a MATLAB implementation.
-  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), estimate_lost, 1e-3));
-  
-  double rank_tol = 1e-9;
+  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3));
 
-  Pose3Vector poses = {pose_1, pose_2};
-  Point2Vector points = {x1_noisy, x2_noisy};
-  boost::shared_ptr<Cal3_S2> cal = boost::make_shared<Cal3_S2>(identity_K);
   boost::optional<Point3> estimate_dlt =
-      triangulatePoint3<Cal3_S2>(poses, cal, points, rank_tol, false);
+      triangulatePoint3<Cal3_S2>(cameras, measurements, rank_tol, false);
 
   // The LOST estimate should have a smaller error.
-  EXPECT((gt_point - estimate_lost).norm() <=
+  EXPECT((gt_point - *estimate_lost).norm() <=
          (gt_point - *estimate_dlt).norm());
 }
 
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index 9d8325d68..c2c798d8b 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -24,9 +24,9 @@
 namespace gtsam {
 
 Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
-
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -52,61 +52,75 @@ Vector4 triangulateHomogeneousDLT(
   Vector v;
   boost::tie(rank, error, v) = DLT(A, rank_tol);
 
-  if (rank < 3)
-    throw(TriangulationUnderconstrainedException());
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
 
   return v;
 }
 
-Vector3 triangulateLOSTHomogeneous(
-    const std::vector<Pose3>& poses,
-    const std::vector<Point3>& calibrated_measurements, 
-    const double measurement_sigma) {
-  size_t m = calibrated_measurements.size();
-  assert(m == poses.size());
+Vector4 triangulateHomogeneousDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
+    const std::vector<Unit3>& measurements, double rank_tol) {
+  // number of cameras
+  size_t m = projection_matrices.size();
 
-  // Construct the system matrices.
+  // Allocate DLT matrix
   Matrix A = Matrix::Zero(m * 2, 4);
 
   for (size_t i = 0; i < m; i++) {
-    const Pose3& wTi = poses[i];
-    // TODO(akshay-krishnan): are there better ways to select j?
-    const int j = (i + 1) % m;
-    const Pose3& wTj = poses[j];
+    size_t row = i * 2;
+    const Matrix34& projection = projection_matrices.at(i);
+    const Point3& p =
+        measurements.at(i)
+            .point3();  // to get access to x,y,z of the bearing vector
 
-    const Point3 d_ij = wTj.translation() - wTi.translation();
-
-    const Point3 w_measurement_i =
-        wTi.rotation().rotate(calibrated_measurements[i]);
-    const Point3 w_measurement_j =
-        wTj.rotation().rotate(calibrated_measurements[j]);
-
-    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
-                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
-    const Matrix23 coefficient_mat =
-        q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
-        wTi.rotation().matrix().transpose();
-
-    A.block<2, 4>(2 * i, 0) << coefficient_mat, -coefficient_mat * wTi.translation();
+    // build system of equations
+    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
+    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
   }
-
-  const double rank_tol = 1e-6;
   int rank;
   double error;
   Vector v;
   boost::tie(rank, error, v) = DLT(A, rank_tol);
 
-  if (rank < 3)
-    throw(TriangulationUnderconstrainedException());
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
+
+  return v;
+}
+
+Point3 triangulateDLT(const std::vector<Pose3>& poses,
+                      const Point3Vector& homogenousMeasurements,
+                      double rank_tol) {
+  // number of cameras
+  size_t m = poses.size();
+
+  // Allocate DLT matrix
+  Matrix A = Matrix::Zero(m * 2, 4);
+
+  for (size_t i = 0; i < m; i++) {
+    size_t row = i * 2;
+    const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4);
+    const Point3& p = homogenousMeasurements[i];  // to get access to x,y,z of
+                                                  // the bearing vector
+
+    // build system of equations
+    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
+    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
+  }
+  int rank;
+  double error;
+  Vector v;
+  boost::tie(rank, error, v) = DLT(A, rank_tol);
+
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
 
   return Point3(v.head<3>() / v[3]);
 }
 
-Vector3 triangulateLOSTHomogeneousLS(
-    const std::vector<Pose3>& poses,
-    const std::vector<Point3>& calibrated_measurements, 
-    const double measurement_sigma) {
-  size_t m = calibrated_measurements.size();
+Point3 triangulateLOST(const std::vector<Pose3>& poses,
+                       const Point3Vector& calibratedMeasurements,
+                       const SharedIsotropic& measurementNoise) {
+  size_t m = calibratedMeasurements.size();
   assert(m == poses.size());
 
   // Construct the system matrices.
@@ -121,70 +135,43 @@ Vector3 triangulateLOSTHomogeneousLS(
 
     const Point3 d_ij = wTj.translation() - wTi.translation();
 
-    const Point3 w_measurement_i = wTi.rotation().rotate(calibrated_measurements[i]);
-    const Point3 w_measurement_j = wTj.rotation().rotate(calibrated_measurements[j]);
+    const Point3 w_measurement_i =
+        wTi.rotation().rotate(calibratedMeasurements[i]);
+    const Point3 w_measurement_j =
+        wTj.rotation().rotate(calibratedMeasurements[j]);
 
-    const double q_i = w_measurement_i.cross(w_measurement_j).norm() /
-                       (measurement_sigma * d_ij.cross(w_measurement_j).norm());
+    double q_i =
+        w_measurement_i.cross(w_measurement_j).norm() /
+        (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm());
     const Matrix23 coefficient_mat =
-        q_i * skewSymmetric(calibrated_measurements[i]).topLeftCorner(2, 3) *
+        q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
         wTi.rotation().matrix().transpose();
 
-    A.block<2, 3>(2*i, 0) = coefficient_mat;
-    b.block<2, 1>(2*i, 0) = coefficient_mat * wTi.translation();
+    A.block<2, 3>(2 * i, 0) << coefficient_mat;
+    b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation();
   }
   return A.colPivHouseholderQr().solve(b);
 }
 
-Vector4 triangulateHomogeneousDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const std::vector<Unit3>& measurements, double rank_tol) {
-
-  // number of cameras
-  size_t m = projection_matrices.size();
-
-  // Allocate DLT matrix
-  Matrix A = Matrix::Zero(m * 2, 4);
-
-  for (size_t i = 0; i < m; i++) {
-    size_t row = i * 2;
-    const Matrix34& projection = projection_matrices.at(i);
-    const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
-
-    // build system of equations
-    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
-    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
-  }
-  int rank;
-  double error;
-  Vector v;
-  boost::tie(rank, error, v) = DLT(A, rank_tol);
-
-  if (rank < 3)
-    throw(TriangulationUnderconstrainedException());
-
-  return v;
-}
-
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const Point2Vector& measurements, double rank_tol) {
-
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
-                                        rank_tol);
+  Vector4 v =
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
   // Create 3D point from homogeneous coordinates
   return Point3(v.head<3>() / v[3]);
 }
 
 Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
     const std::vector<Unit3>& measurements, double rank_tol) {
-
   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
-                                         rank_tol);
-   // Create 3D point from homogeneous coordinates
-   return Point3(v.head<3>() / v[3]);
+  Vector4 v =
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+  // Create 3D point from homogeneous coordinates
+  return Point3(v.head<3>() / v[3]);
 }
 
 ///
@@ -215,4 +202,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
   return result.at<Point3>(landmarkKey);
 }
 
-}  // \namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 5bea4d93c..9e59a92cb 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -62,24 +62,6 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
     const Point2Vector& measurements, double rank_tol = 1e-9);
 
-/**
- * @brief 
- * 
- * @param projection_matrices 
- * @param measurements 
- * @param rank_tol 
- * @return GTSAM_EXPORT 
- */
-GTSAM_EXPORT Vector3
-triangulateLOSTHomogeneous(const std::vector<Pose3>& poses,
-                           const std::vector<Point3>& calibrated_measurements,
-                           const double measurement_sigma);
-
-GTSAM_EXPORT Vector3
-triangulateLOSTHomogeneousLS(const std::vector<Pose3>& poses,
-                           const std::vector<Point3>& calibrated_measurements,
-                           const double measurement_sigma);
-
 /**
  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
  * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
@@ -112,6 +94,20 @@ GTSAM_EXPORT Point3 triangulateDLT(
     const std::vector<Unit3>& measurements,
     double rank_tol = 1e-9);
 
+/**
+ * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation)
+ * algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry
+ * and John Christian.
+ * @param poses camera poses in world frame
+ * @param calibratedMeasurements measurements in homogeneous coordinates in each
+ * camera pose
+ * @param measurementNoise isotropic noise model for the measurements
+ * @return triangulated point in world coordinates
+ */
+GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
+                                    const Point3Vector& calibratedMeasurements,
+                                    const SharedIsotropic& measurementNoise);
+
 /**
  * Create a factor graph with projection factors from poses and one calibration
  * @param poses Camera poses
@@ -320,8 +316,8 @@ template <class CAMERA>
 typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
-  const size_t num_meas = cameras.size();
-  assert(num_meas == measurements.size());
+  const size_t num_meas = measurements.size();
+  assert(num_meas == cameras.size());
   typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
   for (size_t ii = 0; ii < num_meas; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
@@ -349,6 +345,65 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
   return measurements;
 }
 
+/** Convert pixel measurements in image to homogeneous measurements in the image
+ * plane using shared camera intrinsics.
+ *
+ * @tparam CALIBRATION Calibration type to use.
+ * @param cal Calibration with which measurements were taken.
+ * @param measurements Vector of measurements to undistort.
+ * @return homogeneous measurements in image plane
+ */
+template <class CALIBRATION>
+inline Point3Vector calibrateMeasurementsShared(
+    const CALIBRATION& cal, const Point2Vector& measurements) {
+  Point3Vector calibratedMeasurements;
+  // Calibrate with cal and uncalibrate with pinhole version of cal so that
+  // measurements are undistorted.
+  std::transform(measurements.begin(), measurements.end(),
+                 std::back_inserter(calibratedMeasurements),
+                 [&cal](const Point2& measurement) {
+                   Point3 p;
+                   p << cal.calibrate(measurement), 1.0;
+                   return p;
+                 });
+  return calibratedMeasurements;
+}
+
+/** Convert pixel measurements in image to homogeneous measurements in the image
+ * plane using camera intrinsics of each measurement.
+ *
+ * @tparam CAMERA Camera type to use.
+ * @param cameras Cameras corresponding to each measurement.
+ * @param measurements Vector of measurements to undistort.
+ * @return homogeneous measurements in image plane
+ */
+template <class CAMERA>
+inline Point3Vector calibrateMeasurements(
+    const CameraSet<CAMERA>& cameras,
+    const typename CAMERA::MeasurementVector& measurements) {
+  const size_t num_meas = measurements.size();
+  assert(num_meas == cameras.size());
+  Point3Vector calibratedMeasurements(num_meas);
+  for (size_t ii = 0; ii < num_meas; ++ii) {
+    calibratedMeasurements[ii]
+        << cameras[ii].calibration().calibrate(measurements[ii]),
+        1.0;
+  }
+  return calibratedMeasurements;
+}
+
+/** Specialize for SphericalCamera to do nothing. */
+template <class CAMERA = SphericalCamera>
+inline Point3Vector calibrateMeasurements(
+    const CameraSet<SphericalCamera>& cameras,
+    const SphericalCamera::MeasurementVector& measurements) {
+  Point3Vector calibratedMeasurements(measurements.size());
+  for (size_t ii = 0; ii < measurements.size(); ++ii) {
+    calibratedMeasurements[ii] << measurements[ii].point3();
+  }
+  return calibratedMeasurements;
+}
+
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. The function checks that the
@@ -359,6 +414,7 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
  * @return Returns a Point3
  */
 template<class CALIBRATION>
@@ -366,22 +422,36 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     boost::shared_ptr<CALIBRATION> sharedCal,
     const Point2Vector& measurements, double rank_tol = 1e-9,
     bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
+    const SharedNoiseModel& model = nullptr,
+    const bool use_lost_triangulation = true) {
 
   assert(poses.size() == measurements.size());
   if (poses.size() < 2)
     throw(TriangulationUnderconstrainedException());
 
-  // construct projection matrices from poses & calibration
-  auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
-
-  // Undistort the measurements, leaving only the pinhole elements in effect.
-  auto undistortedMeasurements =
-      undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
-
   // Triangulate linearly
-  Point3 point =
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  Point3 point;
+  if(use_lost_triangulation) {
+    // Reduce input noise model to an isotropic noise model using the mean of
+    // the diagonal.
+    const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
+    SharedIsotropic measurementNoise =
+        noiseModel::Isotropic::Sigma(2, measurementSigma);
+    // calibrate the measurements to obtain homogenous coordinates in image plane.
+    auto calibratedMeasurements =
+        calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
+
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+  } else {
+    // construct projection matrices from poses & calibration
+    auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
+
+    // Undistort the measurements, leaving only the pinhole elements in effect.
+    auto undistortedMeasurements =
+        undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+
+    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  }
 
   // Then refine using non-linear optimization
   if (optimize)
@@ -400,45 +470,6 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
   return point;
 }
 
-template <class CALIBRATION>
-Point3 triangulateLOSTPoint3(
-    const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
-    const Point2Vector& measurements,
-    const double measurement_sigma = 1e-2, bool use_dlt = false) {
-  const size_t num_cameras = cameras.size();
-  assert(measurements.size() == num_cameras);
-
-  if (num_cameras < 2) throw(TriangulationUnderconstrainedException());
-
-  // Convert measurements to image plane coordinates.
-  std::vector<Point3> calibrated_measurements;
-  calibrated_measurements.reserve(measurements.size());
-  for (int i = 0; i < measurements.size(); ++i) {
-    Point2 p = cameras[i].calibration().calibrate(measurements[i]);
-    calibrated_measurements.emplace_back(p.x(), p.y(), 1.0);
-  }
-
-  std::vector<Pose3> poses;
-  poses.reserve(cameras.size());
-  for (const auto& camera : cameras) poses.push_back(camera.pose());
-
-  Point3 point = use_dlt
-                     ? triangulateLOSTHomogeneous(
-                           poses, calibrated_measurements, measurement_sigma)
-                     : triangulateLOSTHomogeneousLS(
-                           poses, calibrated_measurements, measurement_sigma);
-
-#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  // verify that the triangulated point lies in front of all cameras
-  for (const auto& camera : cameras) {
-    const Point3& p_local = camera.pose().transformTo(point);
-    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
-  }
-#endif
-
-  return point;
-}
-
 /**
  * Function to triangulate 3D landmark point from an arbitrary number
  * of poses (at least 2) using the DLT. This function is similar to the one
@@ -449,6 +480,7 @@ Point3 triangulateLOSTPoint3(
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
  * @return Returns a Point3
  */
 template<class CAMERA>
@@ -456,7 +488,8 @@ Point3 triangulatePoint3(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
     bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
+    const SharedNoiseModel& model = nullptr, 
+    const bool use_lost_triangulation=false) {
 
   size_t m = cameras.size();
   assert(measurements.size() == m);
@@ -464,19 +497,41 @@ Point3 triangulatePoint3(
   if (m < 2)
     throw(TriangulationUnderconstrainedException());
 
-  // construct projection matrices from poses & calibration
-  auto projection_matrices = projectionMatricesFromCameras(cameras);
+  // Triangulate linearly
+  Point3 point;
+  if(use_lost_triangulation) {
+    // Reduce input noise model to an isotropic noise model using the mean of
+    // the diagonal.
+    const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
+    SharedIsotropic measurementNoise =
+        noiseModel::Isotropic::Sigma(2, measurementSigma);
 
-  // Undistort the measurements, leaving only the pinhole elements in effect.
-  auto undistortedMeasurements =
-      undistortMeasurements<CAMERA>(cameras, measurements);
+    // construct poses from cameras.
+    std::vector<Pose3> poses;
+    poses.reserve(cameras.size());
+    for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point =
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    // calibrate the measurements to obtain homogenous coordinates in image
+    // plane.
+    auto calibratedMeasurements =
+        calibrateMeasurements<CAMERA>(cameras, measurements);
 
-  // The n refine using non-linear optimization
-  if (optimize)
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+  } else {
+    // construct projection matrices from poses & calibration
+    auto projection_matrices = projectionMatricesFromCameras(cameras);
+
+    // Undistort the measurements, leaving only the pinhole elements in effect.
+    auto undistortedMeasurements =
+        undistortMeasurements<CAMERA>(cameras, measurements);
+
+    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  }
+
+  // Then refine using non-linear optimization
+  if (optimize) {
     point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
+  }
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
@@ -496,9 +551,10 @@ Point3 triangulatePoint3(
     const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
     const Point2Vector& measurements, double rank_tol = 1e-9,
     bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
+    const SharedNoiseModel& model = nullptr, 
+    const bool use_lost_triangulation = false) {
   return triangulatePoint3<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, rank_tol, optimize, model);
+  (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation);
 }
 
 struct GTSAM_EXPORT TriangulationParameters {

From ed5fa923cfe04ddfed1b9e74da9d268ca8009b41 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sun, 10 Jul 2022 16:49:30 -0700
Subject: [PATCH 165/175] Example to run LOST and compare to DLT

---
 examples/TriangulationLOSTExample.cpp | 161 ++++++++++++++++++++++++++
 1 file changed, 161 insertions(+)
 create mode 100644 examples/TriangulationLOSTExample.cpp

diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp
new file mode 100644
index 000000000..a903c625a
--- /dev/null
+++ b/examples/TriangulationLOSTExample.cpp
@@ -0,0 +1,161 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 TriangulationLOSTExample.cpp
+ * @author Akshay Krishnan
+ * @brief This example runs triangulation several times using 3 different
+ * approaches: LOST, DLT, and DLT with optimization. It reports the covariance
+ * and the runtime for each approach.
+ *
+ * @date 2022-07-10
+ */
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/triangulation.h>
+
+#include <chrono>
+#include <iostream>
+#include <random>
+
+using namespace std;
+using namespace gtsam;
+
+static std::mt19937 rng(42);
+
+void PrintCovarianceStats(const Matrix& mat, const std::string& method) {
+  Matrix centered = mat.rowwise() - mat.colwise().mean();
+  Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
+  std::cout << method << " covariance: " << std::endl;
+  std::cout << cov << std::endl;
+  std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl;
+}
+
+void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
+                   const std::string& method) {
+  double nanoseconds = dur.count() / num_samples;
+  std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3
+            << std::endl;
+}
+
+void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
+                            std::vector<Pose3>* poses, Point3* point,
+                            Point2Vector* measurements) {
+  const double min_xy = -10, max_xy = 10;
+  const double min_z = -20, max_z = 0;
+  const int num_cameras = 500;
+  cameras->reserve(num_cameras);
+  poses->reserve(num_cameras);
+  measurements->reserve(num_cameras);
+  *point = Point3(0.0, 0.0, 10.0);
+
+  std::uniform_real_distribution<double> rand_xy(min_xy, max_xy);
+  std::uniform_real_distribution<double> rand_z(min_z, max_z);
+  Cal3_S2 identity_K;
+
+  for (int i = 0; i < num_cameras; ++i) {
+    Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
+    Pose3 wTi(Rot3(), wti);
+
+    poses->push_back(wTi);
+    cameras->emplace_back(wTi, identity_K);
+    measurements->push_back(cameras->back().project(*point));
+  }
+}
+
+void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
+                            std::vector<Pose3>* poses, Point3* point,
+                            Point2Vector* measurements) {
+  Pose3 pose_1;
+  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
+  Cal3_S2 identity_K;
+  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
+  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+
+  *point = Point3(0, 0, 1);
+  cameras->push_back(camera_1);
+  cameras->push_back(camera_2);
+  *poses = {pose_1, pose_2};
+  *measurements = {camera_1.project(*point), camera_2.project(*point)};
+}
+
+Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
+                                    const double measurement_sigma) {
+  std::normal_distribution<double> normal(0.0, measurement_sigma);
+
+  Point2Vector noisy_measurements;
+  noisy_measurements.reserve(measurements.size());
+  for (const auto& p : measurements) {
+    noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
+  }
+  return noisy_measurements;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  std::vector<Pose3> poses;
+  Point3 gt_point;
+  Point2Vector measurements;
+  GetLargeCamerasDataset(&cameras, &poses, &gt_point, &measurements);
+  // GetSmallCamerasDataset(&cameras, &poses, &gt_point, &measurements);
+  const double measurement_sigma = 1e-2;
+  SharedNoiseModel measurement_noise =
+      noiseModel::Isotropic::Sigma(2, measurement_sigma);
+
+  const long int num_trials = 1000;
+  Matrix dlt_errors = Matrix::Zero(num_trials, 3);
+  Matrix lost_errors = Matrix::Zero(num_trials, 3);
+  Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3);
+
+  double rank_tol = 1e-9;
+  boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
+  std::chrono::nanoseconds dlt_duration;
+  std::chrono::nanoseconds dlt_opt_duration;
+  std::chrono::nanoseconds lost_duration;
+  std::chrono::nanoseconds lost_tls_duration;
+
+  for (int i = 0; i < num_trials; i++) {
+    Point2Vector noisy_measurements =
+        AddNoiseToMeasurements(measurements, measurement_sigma);
+
+    auto lost_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_lost = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, false, measurement_noise, true);
+    lost_duration += std::chrono::high_resolution_clock::now() - lost_start;
+
+    auto dlt_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_dlt = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, false, measurement_noise, false);
+    dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start;
+
+    auto dlt_opt_start = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimate_dlt_opt = triangulatePoint3<Cal3_S2>(
+        cameras, noisy_measurements, rank_tol, true, measurement_noise, false);
+    dlt_opt_duration +=
+        std::chrono::high_resolution_clock::now() - dlt_opt_start;
+
+    lost_errors.row(i) = *estimate_lost - gt_point;
+    dlt_errors.row(i) = *estimate_dlt - gt_point;
+    dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point;
+  }
+  PrintCovarianceStats(lost_errors, "LOST");
+  PrintCovarianceStats(dlt_errors, "DLT");
+  PrintCovarianceStats(dlt_opt_errors, "DLT_OPT");
+
+  PrintDuration(lost_duration, num_trials, "LOST");
+  PrintDuration(dlt_duration, num_trials, "DLT");
+  PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT");
+}
\ No newline at end of file

From 7d9cf475794e1176d6a31f9b695bd1ade1e27301 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Sun, 10 Jul 2022 22:00:18 -0700
Subject: [PATCH 166/175] unit tests pass

---
 gtsam/geometry/triangulation.cpp | 32 +---------------
 gtsam/geometry/triangulation.h   | 64 ++++++++++++++++----------------
 2 files changed, 32 insertions(+), 64 deletions(-)

diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index c2c798d8b..c68fffb82 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -88,35 +88,6 @@ Vector4 triangulateHomogeneousDLT(
   return v;
 }
 
-Point3 triangulateDLT(const std::vector<Pose3>& poses,
-                      const Point3Vector& homogenousMeasurements,
-                      double rank_tol) {
-  // number of cameras
-  size_t m = poses.size();
-
-  // Allocate DLT matrix
-  Matrix A = Matrix::Zero(m * 2, 4);
-
-  for (size_t i = 0; i < m; i++) {
-    size_t row = i * 2;
-    const Matrix34 projection = poses[i].matrix().block(0, 0, 3, 4);
-    const Point3& p = homogenousMeasurements[i];  // to get access to x,y,z of
-                                                  // the bearing vector
-
-    // build system of equations
-    A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
-    A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
-  }
-  int rank;
-  double error;
-  Vector v;
-  boost::tie(rank, error, v) = DLT(A, rank_tol);
-
-  if (rank < 3) throw(TriangulationUnderconstrainedException());
-
-  return Point3(v.head<3>() / v[3]);
-}
-
 Point3 triangulateLOST(const std::vector<Pose3>& poses,
                        const Point3Vector& calibratedMeasurements,
                        const SharedIsotropic& measurementNoise) {
@@ -140,7 +111,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     const Point3 w_measurement_j =
         wTj.rotation().rotate(calibratedMeasurements[j]);
 
-    double q_i =
+    const double q_i =
         w_measurement_i.cross(w_measurement_j).norm() /
         (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm());
     const Matrix23 coefficient_mat =
@@ -174,7 +145,6 @@ Point3 triangulateDLT(
   return Point3(v.head<3>() / v[3]);
 }
 
-///
 /**
  * Optimize for triangulation
  * @param graph nonlinear factors for projection
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 9e59a92cb..8a709ccd0 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -414,30 +414,30 @@ inline Point3Vector calibrateMeasurements(
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of
+ * DLT
  * @return Returns a Point3
  */
-template<class CALIBRATION>
+template <class CALIBRATION>
 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
-    boost::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr,
-    const bool use_lost_triangulation = true) {
-
+                         boost::shared_ptr<CALIBRATION> sharedCal,
+                         const Point2Vector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool use_lost_triangulation = false) {
   assert(poses.size() == measurements.size());
-  if (poses.size() < 2)
-    throw(TriangulationUnderconstrainedException());
+  if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
   Point3 point;
-  if(use_lost_triangulation) {
+  if (use_lost_triangulation) {
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
     SharedIsotropic measurementNoise =
         noiseModel::Isotropic::Sigma(2, measurementSigma);
-    // calibrate the measurements to obtain homogenous coordinates in image plane.
+    // calibrate the measurements to obtain homogenous coordinates in image
+    // plane.
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
@@ -450,20 +450,20 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     auto undistortedMeasurements =
         undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
   if (optimize)
-    point = triangulateNonlinear<CALIBRATION> //
+    point = triangulateNonlinear<CALIBRATION>  //
         (poses, sharedCal, measurements, point, model);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
-  for(const Pose3& pose: poses) {
+  for (const Pose3& pose : poses) {
     const Point3& p_local = pose.transformTo(point);
-    if (p_local.z() <= 0)
-      throw(TriangulationCheiralityException());
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -480,26 +480,24 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of DLT
+ * @param use_lost_triangulation whether to use the LOST algorithm instead of
+ * DLT
  * @return Returns a Point3
  */
-template<class CAMERA>
-Point3 triangulatePoint3(
-    const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr, 
-    const bool use_lost_triangulation=false) {
-
+template <class CAMERA>
+Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
+                         const typename CAMERA::MeasurementVector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool use_lost_triangulation = false) {
   size_t m = cameras.size();
   assert(measurements.size() == m);
 
-  if (m < 2)
-    throw(TriangulationUnderconstrainedException());
+  if (m < 2) throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
   Point3 point;
-  if(use_lost_triangulation) {
+  if (use_lost_triangulation) {
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
@@ -525,7 +523,8 @@ Point3 triangulatePoint3(
     auto undistortedMeasurements =
         undistortMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
   }
 
   // Then refine using non-linear optimization
@@ -535,10 +534,9 @@ Point3 triangulatePoint3(
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
-  for(const CAMERA& camera: cameras) {
+  for (const CAMERA& camera : cameras) {
     const Point3& p_local = camera.pose().transformTo(point);
-    if (p_local.z() <= 0)
-      throw(TriangulationCheiralityException());
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
   }
 #endif
 

From 223ea468d6109273ccadbd2ec84ee0c0ed75e294 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 18:45:46 -0700
Subject: [PATCH 167/175] change global variable names in test

---
 gtsam/geometry/tests/testTriangulation.cpp | 240 ++++++++++-----------
 1 file changed, 120 insertions(+), 120 deletions(-)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 731558327..f6408fab9 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -38,24 +38,24 @@ using namespace boost::assign;
 
 // Some common constants
 
-static const boost::shared_ptr<Cal3_S2> sharedCal =  //
+static const boost::shared_ptr<Cal3_S2> kSharedCal =  //
     boost::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480);
 
 // Looking along X-axis, 1 meter above ground plane (x-y)
 static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
-static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
-PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
+static const PinholeCamera<Cal3_S2> kCamera1(kPose1, *kSharedCal);
 
 // create second camera 1 meter to the right of first camera
-static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
-PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);
+static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
+static const PinholeCamera<Cal3_S2> kCamera2(kPose2, *kSharedCal);
 
 // landmark ~5 meters infront of camera
-static const Point3 landmark(5, 0.5, 1.2);
+static const Point3 kLandmark(5, 0.5, 1.2);
 
 // 1. Project two landmarks into two cameras and triangulate
-Point2 z1 = camera1.project(landmark);
-Point2 z2 = camera2.project(landmark);
+static const Point2 kZ1 = kCamera1.project(kLandmark);
+static const Point2 kZ2 = kCamera2.project(kLandmark);
 
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
@@ -63,22 +63,22 @@ TEST(triangulation, twoPoses) {
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
-  measurements += z1, z2;
+  poses += kPose1, kPose2;
+  measurements += kZ1, kZ2;
 
   double rank_tol = 1e-9;
 
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -86,22 +86,22 @@ TEST(triangulation, twoPoses) {
   measurements.at(1) += Point2(-0.2, 0.3);
   optimize = false;
   boost::optional<Point3> actual3 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
 
   // 4. Now with optimization on
   optimize = true;
   boost::optional<Point3> actual4 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
   EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
 }
 
 TEST(triangulation, twoCamerasUsingLOST) {
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
-  cameras.push_back(camera1);
-  cameras.push_back(camera2);
+  cameras.push_back(kCamera1);
+  cameras.push_back(kCamera2);
 
-  Point2Vector measurements = {z1, z2};
+  Point2Vector measurements = {kZ1, kZ2};
   SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
   double rank_tol = 1e-9;
 
@@ -111,7 +111,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
           cameras, measurements, rank_tol,
           /*optimize=*/false, measurementNoise,
           /*use_lost_triangulation=*/true);
-  EXPECT(assert_equal(landmark, *actual1, 1e-12));
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-12));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -167,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) {
       boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
                                   -0.0003);
 
-  PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
+  PinholeCamera<Cal3DS2> camera1Distorted(kPose1, *sharedDistortedCal);
 
-  PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
+  PinholeCamera<Cal3DS2> camera2Distorted(kPose2, *sharedDistortedCal);
 
   // 0. Project two landmarks into two cameras and triangulate
-  Point2 z1Distorted = camera1Distorted.project(landmark);
-  Point2 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z1Distorted = camera1Distorted.project(kLandmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -188,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) {
   boost::optional<Point3> actual1 =  //
       triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
       triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
                                  rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -224,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) {
       boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
                                       0.0001, -0.0003);
 
-  PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
+  PinholeCamera<Calibration> camera1Distorted(kPose1, *sharedDistortedCal);
 
-  PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
+  PinholeCamera<Calibration> camera2Distorted(kPose2, *sharedDistortedCal);
 
   // 0. Project two landmarks into two cameras and triangulate
-  Point2 z1Distorted = camera1Distorted.project(landmark);
-  Point2 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z1Distorted = camera1Distorted.project(kLandmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -245,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) {
   boost::optional<Point3> actual1 =  //
       triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
       triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
                                      rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -277,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) {
 TEST(triangulation, twoPosesBundler) {
   boost::shared_ptr<Cal3Bundler> bundlerCal =  //
       boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
-  PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
-  PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
+  PinholeCamera<Cal3Bundler> camera1(kPose1, *bundlerCal);
+  PinholeCamera<Cal3Bundler> camera2(kPose2, *bundlerCal);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1, z2;
 
   bool optimize = true;
@@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) {
   boost::optional<Point3> actual =  //
       triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
                                      optimize);
-  EXPECT(assert_equal(landmark, *actual, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual, 1e-7));
 
   // Add some noise and try again
   measurements.at(0) += Point2(0.1, 0.5);
@@ -313,12 +313,12 @@ TEST(triangulation, fourPoses) {
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
-  measurements += z1, z2;
+  poses += kPose1, kPose2;
+  measurements += kZ1, kZ2;
 
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -326,37 +326,37 @@ TEST(triangulation, fourPoses) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual2, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   poses += pose3;
   measurements += z3 + Point2(0.1, -0.1);
 
   boost::optional<Point3> triangulated_3cameras =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
   boost::optional<Point3> triangulated_3cameras_opt =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-  PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
+  PinholeCamera<Cal3_S2> camera4(pose4, *kSharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
+  CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
 
   poses += pose4;
   measurements += Point2(400, 400);
 
-  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationCheiralityException);
 #endif
 }
@@ -364,33 +364,33 @@ TEST(triangulation, fourPoses) {
 //******************************************************************************
 TEST(triangulation, threePoses_robustNoiseModel) {
 
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose2, pose3;
-  measurements += z1, z2, z3;
+  poses += kPose1, kPose2, pose3;
+  measurements += kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // Add outlier
   measurements.at(0) += Point2(100, 120); // very large pixel noise!
 
   // now estimate does not match landmark
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
   // DLT is surprisingly robust, but still off (actual error is around 0.26m):
-  EXPECT( (landmark - *actual2).norm() >= 0.2);
-  EXPECT( (landmark - *actual2).norm() <= 0.5);
+  EXPECT( (kLandmark - *actual2).norm() >= 0.2);
+  EXPECT( (kLandmark - *actual2).norm() <= 0.5);
 
   // Again with nonlinear optimization
   boost::optional<Point3> actual3 =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
   // result from nonlinear (but non-robust optimization) is close to DLT and still off
   EXPECT(assert_equal(*actual2, *actual3, 0.1));
 
@@ -398,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) {
   auto model = noiseModel::Robust::Create(
         noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
   boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
-      poses, sharedCal, measurements, 1e-9, true, model);
+      poses, kSharedCal, measurements, 1e-9, true, model);
   // using the Huber loss we now have a quite small error!! nice!
-  EXPECT(assert_equal(landmark, *actual4, 0.05));
+  EXPECT(assert_equal(kLandmark, *actual4, 0.05));
 }
 
 //******************************************************************************
 TEST(triangulation, fourPoses_robustNoiseModel) {
 
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
-  measurements += z1, z1, z2, z3;
+  poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
+  measurements += kZ1, kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // Add outlier
   measurements.at(0) += Point2(100, 120); // very large pixel noise!
@@ -429,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
 
   // now estimate does not match landmark
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
   // DLT is surprisingly robust, but still off (actual error is around 0.17m):
-  EXPECT( (landmark - *actual2).norm() >= 0.1);
-  EXPECT( (landmark - *actual2).norm() <= 0.5);
+  EXPECT( (kLandmark - *actual2).norm() >= 0.1);
+  EXPECT( (kLandmark - *actual2).norm() <= 0.5);
 
   // Again with nonlinear optimization
   boost::optional<Point3> actual3 =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
   // result from nonlinear (but non-robust optimization) is close to DLT and still off
   EXPECT(assert_equal(*actual2, *actual3, 0.1));
 
@@ -444,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
   auto model = noiseModel::Robust::Create(
         noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
   boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
-      poses, sharedCal, measurements, 1e-9, true, model);
+      poses, kSharedCal, measurements, 1e-9, true, model);
   // using the Huber loss we now have a quite small error!! nice!
-  EXPECT(assert_equal(landmark, *actual4, 0.05));
+  EXPECT(assert_equal(kLandmark, *actual4, 0.05));
 }
 
 //******************************************************************************
 TEST(triangulation, fourPoses_distinct_Ks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  PinholeCamera<Cal3_S2> camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(kPose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  PinholeCamera<Cal3_S2> camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(kPose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
   Point2Vector measurements;
@@ -471,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 
   boost::optional<Point3> actual =  //
       triangulatePoint3<Cal3_S2>(cameras, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -480,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 
   boost::optional<Point3> actual2 =  //
       triangulatePoint3<Cal3_S2>(cameras, measurements);
-  EXPECT(assert_equal(landmark, *actual2, 1e-2));
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
   PinholeCamera<Cal3_S2> camera3(pose3, K3);
-  Point2 z3 = camera3.project(landmark);
+  Point2 z3 = camera3.project(kLandmark);
 
   cameras += camera3;
   measurements += z3 + Point2(0.1, -0.1);
 
   boost::optional<Point3> triangulated_3cameras =  //
       triangulatePoint3<Cal3_S2>(cameras, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
   boost::optional<Point3> triangulated_3cameras_opt =
       triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
@@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
   PinholeCamera<Cal3_S2> camera4(pose4, K4);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
+  CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
 
   cameras += camera4;
   measurements += Point2(400, 400);
@@ -519,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) {
 TEST(triangulation, fourPoses_distinct_Ks_distortion) {
   Cal3DS2 K1(1500, 1200, 0, 640, 480,  -.3, 0.1, 0.0001, -0.0003);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  PinholeCamera<Cal3DS2> camera1(pose1, K1);
+  PinholeCamera<Cal3DS2> camera1(kPose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3DS2 K2(1600, 1300, 0, 650, 440,  -.2, 0.05, 0.0002, -0.0001);
-  PinholeCamera<Cal3DS2> camera2(pose2, K2);
+  PinholeCamera<Cal3DS2> camera2(kPose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3DS2>> cameras;
   Point2Vector measurements;
@@ -537,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
 
   boost::optional<Point3> actual =  //
           triangulatePoint3<Cal3DS2>(cameras, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 }
 
 //******************************************************************************
 TEST(triangulation, outliersAndFarLandmarks) {
   Cal3_S2 K1(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  PinholeCamera<Cal3_S2> camera1(pose1, K1);
+  PinholeCamera<Cal3_S2> camera1(kPose1, K1);
 
   // create second camera 1 meter to the right of first camera
   Cal3_S2 K2(1600, 1300, 0, 650, 440);
-  PinholeCamera<Cal3_S2> camera2(pose2, K2);
+  PinholeCamera<Cal3_S2> camera2(kPose2, K2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
   Point2Vector measurements;
@@ -565,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) {
       1.0, false, landmarkDistanceThreshold);  // all default except
                                                // landmarkDistanceThreshold
   TriangulationResult actual = triangulateSafe(cameras, measurements, params);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
   EXPECT(actual.valid());
 
   landmarkDistanceThreshold = 4;  // landmark is farther than that
@@ -577,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) {
 
   // 3. Add a slightly rotated third camera above with a wrong measurement
   // (OUTLIER)
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
   Cal3_S2 K3(700, 500, 0, 640, 480);
   PinholeCamera<Cal3_S2> camera3(pose3, K3);
-  Point2 z3 = camera3.project(landmark);
+  Point2 z3 = camera3.project(kLandmark);
 
   cameras += camera3;
   measurements += z3 + Point2(10, -10);
@@ -603,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) {
 //******************************************************************************
 TEST(triangulation, twoIdenticalPoses) {
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+  PinholeCamera<Cal3_S2> camera1(kPose1, *kSharedCal);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose1;
+  poses += kPose1, kPose1;
   measurements += z1, z1;
 
-  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationUnderconstrainedException);
 }
 
@@ -629,7 +629,7 @@ TEST(triangulation, onePose) {
   poses += Pose3();
   measurements += Point2(0, 0);
 
-  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationUnderconstrainedException);
 }
 
@@ -745,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) {
   std::vector<Unit3> measurements;
 
   // Project landmark into two cameras and triangulate
-  SphericalCamera cam1(pose1);
-  SphericalCamera cam2(pose2);
-  Unit3 u1 = cam1.project(landmark);
-  Unit3 u2 = cam2.project(landmark);
+  SphericalCamera cam1(kPose1);
+  SphericalCamera cam2(kPose2);
+  Unit3 u1 = cam1.project(kLandmark);
+  Unit3 u2 = cam2.project(kLandmark);
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;
@@ -762,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) {
   // 1. Test linear triangulation via DLT
   auto projection_matrices = projectionMatricesFromCameras(cameras);
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
-  EXPECT(assert_equal(landmark, point, 1e-7));
+  EXPECT(assert_equal(kLandmark, point, 1e-7));
 
   // 2. Test nonlinear triangulation
   point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
-  EXPECT(assert_equal(landmark, point, 1e-7));
+  EXPECT(assert_equal(kLandmark, point, 1e-7));
 
   // 3. Test simple DLT, now within triangulatePoint3
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
                                          optimize);
-  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 4. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
                                          optimize);
-  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 5. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -825,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
   EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
                       1e-7));  // behind and to the right of PoseB
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;

From da78cc202a33f1eb9ba77aed8edbcb2bb7c37311 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 18:47:37 -0700
Subject: [PATCH 168/175] add author

---
 gtsam/geometry/tests/testTriangulation.cpp | 1 +
 gtsam/geometry/triangulation.cpp           | 1 +
 gtsam/geometry/triangulation.h             | 1 +
 3 files changed, 3 insertions(+)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index f6408fab9..0ce67083e 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -15,6 +15,7 @@
  * @date July 30th, 2013
  * @author Chris Beall (cbeall3)
  * @author Luca Carlone
+ * @author Akshay Krishnan
  */
 
 #include <CppUnitLite/TestHarness.h>
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index c68fffb82..4a8bb279d 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -14,6 +14,7 @@
  * @brief Functions for triangulation
  * @date July 31, 2013
  * @author Chris Beall
+ * @author Akshay Krishnan
  */
 
 #include <gtsam/geometry/triangulation.h>
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 8a709ccd0..1b0970fe2 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -15,6 +15,7 @@
  * @date July 31, 2013
  * @author Chris Beall
  * @author Luca Carlone
+ * @author Akshay Krishnan
  */
 
 #pragma once

From b52aaeab22400f9d496611f0a431466d117ad767 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 19:14:26 -0700
Subject: [PATCH 169/175] update to camelcase names in test

---
 gtsam/geometry/tests/testTriangulation.cpp | 27 +++++++++++-----------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index 0ce67083e..d9e605d55 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -129,36 +129,35 @@ TEST(triangulation, twoCamerasUsingLOST) {
 TEST(triangulation, twoCamerasLOSTvsDLT) {
   // LOST has been shown to preform better when the point is much closer to one
   // camera compared to another. This unit test checks this configuration.
-  Cal3_S2 identity_K;
-  Pose3 pose_1;
-  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
+  const Cal3_S2 identityK;
+  const Pose3 pose1;
+  const Pose3 pose2(Rot3(), Point3(5., 0., -5.));
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
-  cameras.emplace_back(pose_1, identity_K);
-  cameras.emplace_back(pose_2, identity_K);
+  cameras.emplace_back(pose1, identityK);
+  cameras.emplace_back(pose2, identityK);
 
-  Point3 gt_point(0, 0, 1);
-  Point2 x1_noisy = cameras[0].project(gt_point) + Point2(0.00817, 0.00977);
-  Point2 x2_noisy = cameras[1].project(gt_point) + Point2(-0.00610, 0.01969);
+  Point3 landmark(0, 0, 1);
+  Point2 x1_noisy = cameras[0].project(landmark) + Point2(0.00817, 0.00977);
+  Point2 x2_noisy = cameras[1].project(landmark) + Point2(-0.00610, 0.01969);
   Point2Vector measurements = {x1_noisy, x2_noisy};
 
-  double rank_tol = 1e-9;
+  const double rank_tol = 1e-9;
   SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
 
-  boost::optional<Point3> estimate_lost =  //
+  boost::optional<Point3> estimateLOST =  //
       triangulatePoint3<PinholeCamera<Cal3_S2>>(
           cameras, measurements, rank_tol,
           /*optimize=*/false, measurementNoise,
           /*use_lost_triangulation=*/true);
 
   // These values are from a MATLAB implementation.
-  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimate_lost, 1e-3));
+  EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3));
 
-  boost::optional<Point3> estimate_dlt =
+  boost::optional<Point3> estimateDLT =
       triangulatePoint3<Cal3_S2>(cameras, measurements, rank_tol, false);
 
   // The LOST estimate should have a smaller error.
-  EXPECT((gt_point - *estimate_lost).norm() <=
-         (gt_point - *estimate_dlt).norm());
+  EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm());
 }
 
 //******************************************************************************

From f347209d4ce4d36db373519d62ebc47b76c67e2b Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 19:30:41 -0700
Subject: [PATCH 170/175] more camelcase changes

---
 gtsam/geometry/tests/testTriangulation.cpp | 16 ++++++------
 gtsam/geometry/triangulation.cpp           | 19 +++++++-------
 gtsam/geometry/triangulation.h             | 30 ++++++++++------------
 3 files changed, 31 insertions(+), 34 deletions(-)

diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp
index d9e605d55..bc843ad75 100644
--- a/gtsam/geometry/tests/testTriangulation.cpp
+++ b/gtsam/geometry/tests/testTriangulation.cpp
@@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) {
 
   // 1. Test simple triangulation, perfect in no noise situation
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<PinholeCamera<Cal3_S2>>(
-          cameras, measurements, rank_tol,
-          /*optimize=*/false, measurementNoise,
-          /*use_lost_triangulation=*/true);
+      triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
+                                                /*optimize=*/false,
+                                                measurementNoise,
+                                                /*useLOST=*/true);
   EXPECT(assert_equal(kLandmark, *actual1, 1e-12));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
@@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) {
   SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
 
   boost::optional<Point3> estimateLOST =  //
-      triangulatePoint3<PinholeCamera<Cal3_S2>>(
-          cameras, measurements, rank_tol,
-          /*optimize=*/false, measurementNoise,
-          /*use_lost_triangulation=*/true);
+      triangulatePoint3<PinholeCamera<Cal3_S2>>(cameras, measurements, rank_tol,
+                                                /*optimize=*/false,
+                                                measurementNoise,
+                                                /*useLOST=*/true);
 
   // These values are from a MATLAB implementation.
   EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3));
diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp
index 4a8bb279d..acb0ddf0e 100644
--- a/gtsam/geometry/triangulation.cpp
+++ b/gtsam/geometry/triangulation.cpp
@@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
 
     const Point3 d_ij = wTj.translation() - wTi.translation();
 
-    const Point3 w_measurement_i =
-        wTi.rotation().rotate(calibratedMeasurements[i]);
-    const Point3 w_measurement_j =
-        wTj.rotation().rotate(calibratedMeasurements[j]);
+    const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
 
-    const double q_i =
-        w_measurement_i.cross(w_measurement_j).norm() /
-        (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm());
-    const Matrix23 coefficient_mat =
+    // Note: Setting q_i = 1.0 gives same results as DLT.
+    const double q_i = wZi.cross(wZj).norm() /
+                       (measurementNoise->sigma() * d_ij.cross(wZj).norm());
+
+    const Matrix23 coefficientMat =
         q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
         wTi.rotation().matrix().transpose();
 
-    A.block<2, 3>(2 * i, 0) << coefficient_mat;
-    b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation();
+    A.block<2, 3>(2 * i, 0) << coefficientMat;
+    b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
   }
   return A.colPivHouseholderQr().solve(b);
 }
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index 1b0970fe2..aa9433484 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements(
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of
- * DLT
+ * @param useLOST whether to use the LOST algorithm instead of DLT
  * @return Returns a Point3
  */
 template <class CALIBRATION>
@@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
                          const Point2Vector& measurements,
                          double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
-                         const bool use_lost_triangulation = false) {
+                         const bool useLOST = false) {
   assert(poses.size() == measurements.size());
   if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
   Point3 point;
-  if (use_lost_triangulation) {
+  if (useLOST) {
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
@@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
  * @param measurements A vector of camera measurements
  * @param rank_tol rank tolerance, default 1e-9
  * @param optimize Flag to turn on nonlinear refinement of triangulation
- * @param use_lost_triangulation whether to use the LOST algorithm instead of
+ * @param useLOST whether to use the LOST algorithm instead of
  * DLT
  * @return Returns a Point3
  */
@@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
                          const typename CAMERA::MeasurementVector& measurements,
                          double rank_tol = 1e-9, bool optimize = false,
                          const SharedNoiseModel& model = nullptr,
-                         const bool use_lost_triangulation = false) {
+                         const bool useLOST = false) {
   size_t m = cameras.size();
   assert(measurements.size() == m);
 
@@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 
   // Triangulate linearly
   Point3 point;
-  if (use_lost_triangulation) {
+  if (useLOST) {
     // Reduce input noise model to an isotropic noise model using the mean of
     // the diagonal.
     const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
@@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
 }
 
 /// Pinhole-specific version
-template<class CALIBRATION>
-Point3 triangulatePoint3(
-    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr, 
-    const bool use_lost_triangulation = false) {
-  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation);
+template <class CALIBRATION>
+Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
+                         const Point2Vector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool useLOST = false) {
+  return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
+      (cameras, measurements, rank_tol, optimize, model, useLOST);
 }
 
 struct GTSAM_EXPORT TriangulationParameters {

From 897dae4436e9d8ce5b9518c23cca7abe9e5b7f4b Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 20:03:02 -0700
Subject: [PATCH 171/175] more renames to camelcase

---
 examples/TriangulationLOSTExample.cpp | 130 +++++++++++++-------------
 gtsam/geometry/triangulation.h        |  16 ++--
 2 files changed, 72 insertions(+), 74 deletions(-)

diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp
index a903c625a..a862026e0 100644
--- a/examples/TriangulationLOSTExample.cpp
+++ b/examples/TriangulationLOSTExample.cpp
@@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
 void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
                             std::vector<Pose3>* poses, Point3* point,
                             Point2Vector* measurements) {
-  const double min_xy = -10, max_xy = 10;
-  const double min_z = -20, max_z = 0;
-  const int num_cameras = 500;
-  cameras->reserve(num_cameras);
-  poses->reserve(num_cameras);
-  measurements->reserve(num_cameras);
+  const double minXY = -10, maxXY = 10;
+  const double minZ = -20, maxZ = 0;
+  const int nrCameras = 500;
+  cameras->reserve(nrCameras);
+  poses->reserve(nrCameras);
+  measurements->reserve(nrCameras);
   *point = Point3(0.0, 0.0, 10.0);
 
-  std::uniform_real_distribution<double> rand_xy(min_xy, max_xy);
-  std::uniform_real_distribution<double> rand_z(min_z, max_z);
-  Cal3_S2 identity_K;
+  std::uniform_real_distribution<double> rand_xy(minXY, maxXY);
+  std::uniform_real_distribution<double> rand_z(minZ, maxZ);
+  Cal3_S2 identityK;
 
-  for (int i = 0; i < num_cameras; ++i) {
+  for (int i = 0; i < nrCameras; ++i) {
     Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
     Pose3 wTi(Rot3(), wti);
 
     poses->push_back(wTi);
-    cameras->emplace_back(wTi, identity_K);
+    cameras->emplace_back(wTi, identityK);
     measurements->push_back(cameras->back().project(*point));
   }
 }
@@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
 void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
                             std::vector<Pose3>* poses, Point3* point,
                             Point2Vector* measurements) {
-  Pose3 pose_1;
-  Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
-  Cal3_S2 identity_K;
-  PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
-  PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
+  Pose3 pose1;
+  Pose3 pose2(Rot3(), Point3(5., 0., -5.));
+  Cal3_S2 identityK;
+  PinholeCamera<Cal3_S2> camera1(pose1, identityK);
+  PinholeCamera<Cal3_S2> camera2(pose2, identityK);
 
   *point = Point3(0, 0, 1);
-  cameras->push_back(camera_1);
-  cameras->push_back(camera_2);
-  *poses = {pose_1, pose_2};
-  *measurements = {camera_1.project(*point), camera_2.project(*point)};
+  cameras->push_back(camera1);
+  cameras->push_back(camera2);
+  *poses = {pose1, pose2};
+  *measurements = {camera1.project(*point), camera2.project(*point)};
 }
 
 Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
-                                    const double measurement_sigma) {
-  std::normal_distribution<double> normal(0.0, measurement_sigma);
+                                    const double measurementSigma) {
+  std::normal_distribution<double> normal(0.0, measurementSigma);
 
-  Point2Vector noisy_measurements;
-  noisy_measurements.reserve(measurements.size());
+  Point2Vector noisyMeasurements;
+  noisyMeasurements.reserve(measurements.size());
   for (const auto& p : measurements) {
-    noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
+    noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
   }
-  return noisy_measurements;
+  return noisyMeasurements;
 }
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
   std::vector<Pose3> poses;
-  Point3 gt_point;
+  Point3 landmark;
   Point2Vector measurements;
-  GetLargeCamerasDataset(&cameras, &poses, &gt_point, &measurements);
-  // GetSmallCamerasDataset(&cameras, &poses, &gt_point, &measurements);
-  const double measurement_sigma = 1e-2;
-  SharedNoiseModel measurement_noise =
-      noiseModel::Isotropic::Sigma(2, measurement_sigma);
+  GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements);
+  // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements);
+  const double measurementSigma = 1e-2;
+  SharedNoiseModel measurementNoise =
+      noiseModel::Isotropic::Sigma(2, measurementSigma);
 
-  const long int num_trials = 1000;
-  Matrix dlt_errors = Matrix::Zero(num_trials, 3);
-  Matrix lost_errors = Matrix::Zero(num_trials, 3);
-  Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3);
+  const long int nrTrials = 1000;
+  Matrix errorsDLT = Matrix::Zero(nrTrials, 3);
+  Matrix errorsLOST = Matrix::Zero(nrTrials, 3);
+  Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3);
 
   double rank_tol = 1e-9;
   boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
-  std::chrono::nanoseconds dlt_duration;
-  std::chrono::nanoseconds dlt_opt_duration;
-  std::chrono::nanoseconds lost_duration;
-  std::chrono::nanoseconds lost_tls_duration;
+  std::chrono::nanoseconds durationDLT;
+  std::chrono::nanoseconds durationDLTOpt;
+  std::chrono::nanoseconds durationLOST;
 
-  for (int i = 0; i < num_trials; i++) {
-    Point2Vector noisy_measurements =
-        AddNoiseToMeasurements(measurements, measurement_sigma);
+  for (int i = 0; i < nrTrials; i++) {
+    Point2Vector noisyMeasurements =
+        AddNoiseToMeasurements(measurements, measurementSigma);
 
-    auto lost_start = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimate_lost = triangulatePoint3<Cal3_S2>(
-        cameras, noisy_measurements, rank_tol, false, measurement_noise, true);
-    lost_duration += std::chrono::high_resolution_clock::now() - lost_start;
+    auto lostStart = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
+        cameras, noisyMeasurements, rank_tol, false, measurementNoise, true);
+    durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
 
-    auto dlt_start = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimate_dlt = triangulatePoint3<Cal3_S2>(
-        cameras, noisy_measurements, rank_tol, false, measurement_noise, false);
-    dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start;
+    auto dltStart = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
+        cameras, noisyMeasurements, rank_tol, false, measurementNoise, false);
+    durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
 
-    auto dlt_opt_start = std::chrono::high_resolution_clock::now();
-    boost::optional<Point3> estimate_dlt_opt = triangulatePoint3<Cal3_S2>(
-        cameras, noisy_measurements, rank_tol, true, measurement_noise, false);
-    dlt_opt_duration +=
-        std::chrono::high_resolution_clock::now() - dlt_opt_start;
+    auto dltOptStart = std::chrono::high_resolution_clock::now();
+    boost::optional<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
+        cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
+    durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
 
-    lost_errors.row(i) = *estimate_lost - gt_point;
-    dlt_errors.row(i) = *estimate_dlt - gt_point;
-    dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point;
+    errorsLOST.row(i) = *estimateLOST - landmark;
+    errorsDLT.row(i) = *estimateDLT - landmark;
+    errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
   }
-  PrintCovarianceStats(lost_errors, "LOST");
-  PrintCovarianceStats(dlt_errors, "DLT");
-  PrintCovarianceStats(dlt_opt_errors, "DLT_OPT");
+  PrintCovarianceStats(errorsLOST, "LOST");
+  PrintCovarianceStats(errorsDLT, "DLT");
+  PrintCovarianceStats(errorsDLTOpt, "DLT_OPT");
 
-  PrintDuration(lost_duration, num_trials, "LOST");
-  PrintDuration(dlt_duration, num_trials, "DLT");
-  PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT");
-}
\ No newline at end of file
+  PrintDuration(durationLOST, nrTrials, "LOST");
+  PrintDuration(durationDLT, nrTrials, "DLT");
+  PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT");
+}
diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h
index aa9433484..f6c919b24 100644
--- a/gtsam/geometry/triangulation.h
+++ b/gtsam/geometry/triangulation.h
@@ -317,10 +317,10 @@ template <class CAMERA>
 typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
-  const size_t num_meas = measurements.size();
-  assert(num_meas == cameras.size());
-  typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
-  for (size_t ii = 0; ii < num_meas; ++ii) {
+  const size_t nrMeasurements = measurements.size();
+  assert(nrMeasurements == cameras.size());
+  typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
+  for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
     // measurements are undistorted.
     undistortedMeasurements[ii] =
@@ -382,10 +382,10 @@ template <class CAMERA>
 inline Point3Vector calibrateMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
-  const size_t num_meas = measurements.size();
-  assert(num_meas == cameras.size());
-  Point3Vector calibratedMeasurements(num_meas);
-  for (size_t ii = 0; ii < num_meas; ++ii) {
+  const size_t nrMeasurements = measurements.size();
+  assert(nrMeasurements == cameras.size());
+  Point3Vector calibratedMeasurements(nrMeasurements);
+  for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     calibratedMeasurements[ii]
         << cameras[ii].calibration().calibrate(measurements[ii]),
         1.0;

From a3ee2660e5809ef40dcd5f9a2d56f32cf7b2efa3 Mon Sep 17 00:00:00 2001
From: Akshay Krishnan <akshay.krishnan.30@gmail.com>
Date: Tue, 12 Jul 2022 22:50:58 -0700
Subject: [PATCH 172/175] LOST in python wrapper

---
 gtsam/geometry/geometry.i | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
index 517558265..597f6a169 100644
--- a/gtsam/geometry/geometry.i
+++ b/gtsam/geometry/geometry.i
@@ -1129,7 +1129,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3_S2* sharedCal,
                                    const gtsam::Point2Vector& measurements,
@@ -1151,7 +1152,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3DS2* sharedCal,
                                    const gtsam::Point2Vector& measurements,
@@ -1173,7 +1175,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3Bundler* sharedCal,
                                    const gtsam::Point2Vector& measurements,
@@ -1195,7 +1198,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3Fisheye* sharedCal,
                                    const gtsam::Point2Vector& measurements,
@@ -1217,7 +1221,8 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
                                    gtsam::Cal3Unified* sharedCal,
                                    const gtsam::Point2Vector& measurements,

From b8fb26a0b03f096214c350b57d288d71104aedd7 Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Sat, 16 Jul 2022 15:18:45 -0700
Subject: [PATCH 173/175] Clear ConcurrentMap before loading archive data

---
 gtsam/base/ConcurrentMap.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h
index 2d7cbd6db..cf4537d4c 100644
--- a/gtsam/base/ConcurrentMap.h
+++ b/gtsam/base/ConcurrentMap.h
@@ -113,6 +113,7 @@ private:
   template<class Archive>
   void load(Archive& ar, const unsigned int /*version*/)
   {
+    this->clear();
     // Load into STL container and then fill our map
     FastVector<std::pair<KEY, VALUE> > map;
     ar & BOOST_SERIALIZATION_NVP(map);

From f332d21dd7a1a3cd5ccced3cb5634fb3b1f8b470 Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Sun, 17 Jul 2022 19:00:24 -0700
Subject: [PATCH 174/175] Add unit test for ConcurrentMap serialization

---
 gtsam/base/tests/testSerializationBase.cpp | 34 ++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp
index f7aa97b31..d2f4d5f51 100644
--- a/gtsam/base/tests/testSerializationBase.cpp
+++ b/gtsam/base/tests/testSerializationBase.cpp
@@ -18,6 +18,7 @@
 
 #include <gtsam/inference/Key.h>
 
+#include <gtsam/base/ConcurrentMap.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/MatrixSerialization.h>
 #include <gtsam/base/Vector.h>
@@ -106,6 +107,39 @@ TEST (Serialization, matrix_vector) {
   EXPECT(equalityBinary<Matrix>((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()));
 }
 
+/* ************************************************************************* */
+TEST (Serialization, ConcurrentMap) {
+
+  ConcurrentMap<int, std::string> map;
+
+  map.insert(make_pair(1, "apple"));
+  map.insert(make_pair(2, "banana"));
+
+  std::string binaryPath = "saved_map.dat";
+    try {
+    std::ofstream outputStream(binaryPath);
+    boost::archive::binary_oarchive outputArchive(outputStream);
+    outputArchive << map;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  // Verify that the existing map contents are replaced by the archive data
+  ConcurrentMap<int, std::string> mapFromDisk;
+  mapFromDisk.insert(make_pair(3, "clam"));
+  EXPECT(mapFromDisk.exists(3));
+  try {
+    std::ifstream ifs(binaryPath);
+    boost::archive::binary_iarchive inputArchive(ifs);
+    inputArchive >> mapFromDisk;
+  } catch(...) {
+    EXPECT(false);
+  }
+  EXPECT(mapFromDisk.exists(1));
+  EXPECT(mapFromDisk.exists(2));
+  EXPECT(!mapFromDisk.exists(3));
+}
+
 /* ************************************************************************* */
 
 

From 07c33c46e526c63149f7fad9c8d755bf0fcaf686 Mon Sep 17 00:00:00 2001
From: Gerry Chen <gerry.chen2015@gmail.com>
Date: Wed, 20 Jul 2022 18:58:41 -0400
Subject: [PATCH 175/175] Set more swap space on linux since gcc runs out of
 memory

---
 .github/workflows/build-python.yml | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml
index 3fc2d662f..f42939bc2 100644
--- a/.github/workflows/build-python.yml
+++ b/.github/workflows/build-python.yml
@@ -112,6 +112,11 @@ jobs:
         run: |
           echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV
           echo "GTSAM Uses TBB"
+      - name: Set Swap Space
+        if: runner.os == 'Linux'
+        uses: pierotofy/set-swap-space@master
+        with:
+          swap-size-gb: 6
       - name: Build (Linux)
         if: runner.os == 'Linux'
         run: |