Merge branch 'develop' of https://github.com/borglab/gtsam into dsf-gtsfm-to-gtsam-port

.github/workflows/build-macos.yml

@@ -19,16 +19,16 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          macOS-10.15-xcode-11.3.1,
+          macos-11-xcode-13.4.1,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: macOS-10.15-xcode-11.3.1
+          - name: macos-11-xcode-13.4.1
-            os: macOS-10.15
+            os: macos-11
             compiler: xcode
-            version: "11.3.1"
+            version: "13.4.1"
 
     steps:
       - name: Checkout
@@ -43,7 +43,7 @@ jobs:
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
-            sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
+            sudo xcode-select -switch /Applications/Xcode.app
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi

.github/workflows/build-python.yml

@@ -22,7 +22,7 @@ jobs:
           ubuntu-18.04-gcc-5,
           ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
-          macOS-10.15-xcode-11.3.1,
+          macOS-11-xcode-13.4.1,
           ubuntu-18.04-gcc-5-tbb,
         ]
 
@@ -52,10 +52,10 @@ jobs:
             build_type: Debug
             python_version: "3"
 
-          - name: macOS-10.15-xcode-11.3.1
+          - name: macOS-11-xcode-13.4.1
-            os: macOS-10.15
+            os: macOS-11
             compiler: xcode
-            version: "11.3.1"
+            version: "13.4.1"
 
           - name: ubuntu-18.04-gcc-5-tbb
             os: ubuntu-18.04
@@ -103,7 +103,7 @@ jobs:
             echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
             echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
           else
-            sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
+            sudo xcode-select -switch /Applications/Xcode.app
             echo "CC=clang" >> $GITHUB_ENV
             echo "CXX=clang++" >> $GITHUB_ENV
           fi
@@ -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: |

examples/TriangulationLOSTExample.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 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 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(minXY, maxXY);
+  std::uniform_real_distribution<double> rand_z(minZ, maxZ);
+  Cal3_S2 identityK;
+
+  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, identityK);
+    measurements->push_back(cameras->back().project(*point));
+  }
+}
+
+void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
+                            std::vector<Pose3>* poses, Point3* point,
+                            Point2Vector* measurements) {
+  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(camera1);
+  cameras->push_back(camera2);
+  *poses = {pose1, pose2};
+  *measurements = {camera1.project(*point), camera2.project(*point)};
+}
+
+Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
+                                    const double measurementSigma) {
+  std::normal_distribution<double> normal(0.0, measurementSigma);
+
+  Point2Vector noisyMeasurements;
+  noisyMeasurements.reserve(measurements.size());
+  for (const auto& p : measurements) {
+    noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng));
+  }
+  return noisyMeasurements;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  std::vector<Pose3> poses;
+  Point3 landmark;
+  Point2Vector measurements;
+  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 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 durationDLT;
+  std::chrono::nanoseconds durationDLTOpt;
+  std::chrono::nanoseconds durationLOST;
+
+  for (int i = 0; i < nrTrials; i++) {
+    Point2Vector noisyMeasurements =
+        AddNoiseToMeasurements(measurements, measurementSigma);
+
+    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 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 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;
+
+    errorsLOST.row(i) = *estimateLOST - landmark;
+    errorsDLT.row(i) = *estimateDLT - landmark;
+    errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
+  }
+  PrintCovarianceStats(errorsLOST, "LOST");
+  PrintCovarianceStats(errorsDLT, "DLT");
+  PrintCovarianceStats(errorsDLTOpt, "DLT_OPT");
+
+  PrintDuration(durationLOST, nrTrials, "LOST");
+  PrintDuration(durationDLT, nrTrials, "DLT");
+  PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT");
+}

gtsam/CMakeLists.txt

@@ -10,6 +10,7 @@ set (gtsam_subdirs
     inference
     symbolic
     discrete
+    hybrid
     linear
     nonlinear
     sam

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);

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));
+}
+
 /* ************************************************************************* */
 
 

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++) {

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);

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 (B == 2 && i == 0) os << " [style=dashed]";
-          if (i == 0) os << " [style=dashed]";
-          if (i > 1) os << " [style=bold]";
-        }
         os << std::endl;
         branch->dot(os, labelFormatter, valueFormatter, showZero);
       }

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>
@@ -220,7 +220,7 @@ namespace gtsam {
     /// @{
 
     /// Make virtual
-    virtual ~DecisionTree() {}
+    virtual ~DecisionTree() = default;
 
     /// Check if tree is empty.
     bool empty() const { return !root_; }

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> {};

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>
 
@@ -33,6 +34,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>;

gtsam/geometry/Pose3.cpp

@@ -489,6 +489,11 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
 }
 #endif
 
+/* ************************************************************************* */
+Pose3 Pose3::slerp(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.

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 slerp(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);

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 {
@@ -292,8 +294,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
+    // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
-    // c22 = a11 * cx - a12 * sx
+    // 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);

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

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
@@ -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 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;
 
   // 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;
@@ -873,7 +906,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 +975,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);
@@ -1129,7 +1162,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 +1185,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 +1208,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 +1231,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 +1254,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,
@@ -1246,5 +1284,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

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;
 

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>
@@ -38,24 +39,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));
+static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
-PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+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));
+static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
-PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);
+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);
+static const Point2 kZ1 = kCamera1.project(kLandmark);
-Point2 z2 = camera2.project(landmark);
+static const Point2 kZ2 = kCamera2.project(kLandmark);
 
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
@@ -63,22 +64,22 @@ TEST(triangulation, twoPoses) {
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
-  measurements += z1, z2;
+  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);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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)
@@ -86,16 +87,79 @@ 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(kCamera1);
+  cameras.push_back(kCamera2);
+
+  Point2Vector measurements = {kZ1, kZ2};
+  SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
+  double rank_tol = 1e-9;
+
+  // 1. Test simple triangulation, perfect in no noise situation
+  boost::optional<Point3> actual1 =  //
+      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,
+  // 0.499167, 1.19814)
+  measurements[0] += Point2(0.1, 0.5);
+  measurements[1] += Point2(-0.2, 0.3);
+  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) {
+  // 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.
+  const Cal3_S2 identityK;
+  const Pose3 pose1;
+  const Pose3 pose2(Rot3(), Point3(5., 0., -5.));
+  CameraSet<PinholeCamera<Cal3_S2>> cameras;
+  cameras.emplace_back(pose1, identityK);
+  cameras.emplace_back(pose2, identityK);
+
+  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};
+
+  const double rank_tol = 1e-9;
+  SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2);
+
+  boost::optional<Point3> estimateLOST =  //
+      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));
+
+  boost::optional<Point3> estimateDLT =
+      triangulatePoint3<Cal3_S2>(cameras, measurements, rank_tol, false);
+
+  // The LOST estimate should have a smaller error.
+  EXPECT((landmark - *estimateLOST).norm() <= (landmark - *estimateDLT).norm());
+}
+
 //******************************************************************************
 // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
 TEST(triangulation, twoPosesCal3DS2) {
@@ -103,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 z1Distorted = camera1Distorted.project(kLandmark);
-  Point2 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -124,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)
@@ -160,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 z1Distorted = camera1Distorted.project(kLandmark);
-  Point2 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -181,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)
@@ -213,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> camera1(kPose1, *bundlerCal);
-  PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
+  PinholeCamera<Cal3Bundler> camera2(kPose2, *bundlerCal);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1, z2;
 
   bool optimize = true;
@@ -232,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);
@@ -249,12 +313,12 @@ TEST(triangulation, fourPoses) {
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
-  measurements += z1, z2;
+  measurements += kZ1, kZ2;
 
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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)
@@ -262,37 +326,37 @@ TEST(triangulation, fourPoses) {
   measurements.at(1) += Point2(-0.2, 0.3);
 
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
-  Point2 z3 = camera3.project(landmark);
+  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);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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>(poses, sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, 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));
-  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
 }
@@ -300,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));
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose2, pose3;
+  poses += kPose1, kPose2, pose3;
-  measurements += z1, z2, z3;
+  measurements += kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  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( (kLandmark - *actual2).norm() >= 0.2);
-  EXPECT( (landmark - *actual2).norm() <= 0.5);
+  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));
 
@@ -334,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));
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
+  poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
-  measurements += z1, z1, z2, z3;
+  measurements += kZ1, kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // Add outlier
   measurements.at(0) += Point2(100, 120); // very large pixel noise!
@@ -365,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( (kLandmark - *actual2).norm() >= 0.1);
-  EXPECT( (landmark - *actual2).norm() <= 0.5);
+  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));
 
@@ -380,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 z1 = camera1.project(kLandmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
   Point2Vector measurements;
@@ -407,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)
@@ -416,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));
@@ -442,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);
@@ -455,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 z1 = camera1.project(kLandmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3DS2>> cameras;
   Point2Vector measurements;
@@ -473,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 z1 = camera1.project(kLandmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   CameraSet<PinholeCamera<Cal3_S2>> cameras;
   Point2Vector measurements;
@@ -501,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
@@ -513,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);
@@ -539,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);
 }
 
@@ -565,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);
 }
 
@@ -681,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) {
   std::vector<Unit3> measurements;
 
   // Project landmark into two cameras and triangulate
-  SphericalCamera cam1(pose1);
+  SphericalCamera cam1(kPose1);
-  SphericalCamera cam2(pose2);
+  SphericalCamera cam2(kPose2);
-  Unit3 u1 = cam1.project(landmark);
+  Unit3 u1 = cam1.project(kLandmark);
-  Unit3 u2 = cam2.project(landmark);
+  Unit3 u2 = cam2.project(kLandmark);
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;
@@ -698,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)
@@ -761,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;

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>
@@ -24,9 +25,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();
 
@@ -39,6 +40,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);
   }
@@ -47,16 +53,15 @@ Vector4 triangulateHomogeneousDLT(
   Vector v;
   boost::tie(rank, error, v) = DLT(A, rank_tol);
 
-  if (rank < 3)
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
-    throw(TriangulationUnderconstrainedException());
 
   return v;
 }
 
 Vector4 triangulateHomogeneousDLT(
-    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) {
-
   // number of cameras
   size_t m = projection_matrices.size();
 
@@ -66,7 +71,9 @@ Vector4 triangulateHomogeneousDLT(
   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
+    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);
@@ -77,34 +84,67 @@ Vector4 triangulateHomogeneousDLT(
   Vector v;
   boost::tie(rank, error, v) = DLT(A, rank_tol);
 
-  if (rank < 3)
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
-    throw(TriangulationUnderconstrainedException());
 
   return v;
 }
 
-Point3 triangulateDLT(
+Point3 triangulateLOST(const std::vector<Pose3>& poses,
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
+                       const Point3Vector& calibratedMeasurements,
-    const Point2Vector& measurements, double rank_tol) {
+                       const SharedIsotropic& measurementNoise) {
+  size_t m = calibratedMeasurements.size();
+  assert(m == poses.size());
 
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
+  // Construct the system matrices.
-                                        rank_tol);
+  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];
+
+    const Point3 d_ij = wTj.translation() - wTi.translation();
+
+    const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+
+    // 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) << coefficientMat;
+    b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
+  }
+  return A.colPivHouseholderQr().solve(b);
+}
+
+Point3 triangulateDLT(
+    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
+        projection_matrices,
+    const Point2Vector& measurements, double 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,
+  Vector4 v =
-                                         rank_tol);
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
-   // Create 3D point from homogeneous coordinates
+  // Create 3D point from homogeneous coordinates
-   return Point3(v.head<3>() / v[3]);
+  return Point3(v.head<3>() / v[3]);
 }
 
-///
 /**
  * Optimize for triangulation
  * @param graph nonlinear factors for projection
@@ -132,4 +172,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
   return result.at<Point3>(landmarkKey);
 }
 
-}  // \namespace gtsam
+}  // namespace gtsam

gtsam/geometry/triangulation.h

@@ -15,6 +15,7 @@
  * @date July 31, 2013
  * @author Chris Beall
  * @author Luca Carlone
+ * @author Akshay Krishnan
  */
 
 #pragma once
@@ -94,6 +95,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
@@ -108,17 +123,16 @@ 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 = 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;
     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);
 }
@@ -302,10 +316,10 @@ template <class CAMERA>
 typename CAMERA::MeasurementVector undistortMeasurements(
     const CameraSet<CAMERA>& cameras,
     const typename CAMERA::MeasurementVector& measurements) {
-  const size_t num_meas = cameras.size();
+  const size_t nrMeasurements = measurements.size();
-  assert(num_meas == measurements.size());
+  assert(nrMeasurements == cameras.size());
-  typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
+  typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
-  for (size_t ii = 0; ii < num_meas; ++ii) {
+  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] =
@@ -331,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 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;
+  }
+  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
@@ -341,41 +414,55 @@ 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 useLOST 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,
+                         boost::shared_ptr<CALIBRATION> sharedCal,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
+                         const Point2Vector& measurements,
-    bool optimize = false,
+                         double rank_tol = 1e-9, bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
+                         const SharedNoiseModel& model = nullptr,
-
+                         const bool useLOST = false) {
   assert(poses.size() == measurements.size());
-  if (poses.size() < 2)
+  if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
-    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 =
+  Point3 point;
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  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;
+    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)
-    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)
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
-      throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -392,41 +479,63 @@ 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 useLOST whether to use the LOST algorithm instead of
+ * DLT
  * @return Returns a Point3
  */
-template<class CAMERA>
+template <class CAMERA>
-Point3 triangulatePoint3(
+Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
-    const CameraSet<CAMERA>& cameras,
+                         const typename CAMERA::MeasurementVector& measurements,
-    const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
+                         double rank_tol = 1e-9, bool optimize = false,
-    bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
-    const SharedNoiseModel& model = nullptr) {
+                         const bool useLOST = false) {
-
   size_t m = cameras.size();
   assert(measurements.size() == m);
 
-  if (m < 2)
+  if (m < 2) throw(TriangulationUnderconstrainedException());
-    throw(TriangulationUnderconstrainedException());
 
-  // construct projection matrices from poses & calibration
+  // Triangulate linearly
-  auto projection_matrices = projectionMatricesFromCameras(cameras);
+  Point3 point;
+  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;
+    SharedIsotropic measurementNoise =
+        noiseModel::Isotropic::Sigma(2, measurementSigma);
 
-  // Undistort the measurements, leaving only the pinhole elements in effect.
+    // construct poses from cameras.
-  auto undistortedMeasurements =
+    std::vector<Pose3> poses;
-      undistortMeasurements<CAMERA>(cameras, measurements);
+    poses.reserve(cameras.size());
+    for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point =
+    // calibrate the measurements to obtain homogenous coordinates in image
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    // plane.
+    auto calibratedMeasurements =
+        calibrateMeasurements<CAMERA>(cameras, measurements);
 
-  // The n refine using non-linear optimization
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
-  if (optimize)
+  } 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
-  for(const CAMERA& camera: cameras) {
+  for (const CAMERA& camera : cameras) {
     const Point3& p_local = camera.pose().transformTo(point);
-    if (p_local.z() <= 0)
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
-      throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -434,14 +543,14 @@ Point3 triangulatePoint3(
 }
 
 /// Pinhole-specific version
-template<class CALIBRATION>
+template <class CALIBRATION>
-Point3 triangulatePoint3(
+Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
-    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
+                         const Point2Vector& measurements,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
+                         double rank_tol = 1e-9, bool optimize = false,
-    bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
-    const SharedNoiseModel& model = nullptr) {
+                         const bool useLOST = false) {
-  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
+  return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
-  (cameras, measurements, rank_tol, optimize, model);
+      (cameras, measurements, rank_tol, optimize, model, useLOST);
 }
 
 struct GTSAM_EXPORT TriangulationParameters {

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)

gtsam/hybrid/GaussianMixture.cpp

@@ -0,0 +1,129 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   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
+ */
+
+#include <gtsam/base/utilities.h>
+#include <gtsam/discrete/DiscreteValues.h>
+#include <gtsam/hybrid/GaussianMixture.h>
+#include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+namespace gtsam {
+
+GaussianMixture::GaussianMixture(
+    const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+    const DiscreteKeys &discreteParents,
+    const GaussianMixture::Conditionals &conditionals)
+    : BaseFactor(CollectKeys(continuousFrontals, continuousParents),
+                 discreteParents),
+      BaseConditional(continuousFrontals.size()),
+      conditionals_(conditionals) {}
+
+/* *******************************************************************************/
+const GaussianMixture::Conditionals &GaussianMixture::conditionals() {
+  return conditionals_;
+}
+
+/* *******************************************************************************/
+GaussianMixture GaussianMixture::FromConditionals(
+    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::add(
+    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 tree = asGaussianFactorGraphTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
+}
+
+/* *******************************************************************************/
+GaussianMixture::Sum GaussianMixture::asGaussianFactorGraphTree() const {
+  auto lambda = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {conditionals_, lambda};
+}
+
+/* *******************************************************************************/
+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);
+}
+
+/* *******************************************************************************/
+void GaussianMixture::print(const std::string &s,
+                            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 << " Discrete Keys = ";
+  for (auto &dk : discreteKeys()) {
+    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();
+      });
+}
+}  // namespace gtsam

gtsam/hybrid/GaussianMixture.h

@@ -0,0 +1,145 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GaussianMixture.h
+ * @brief  A hybrid conditional in the Conditional Linear Gaussian scheme
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @date   Mar 12, 2022
+ */
+
+#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>
+
+namespace gtsam {
+
+/**
+ * @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 selection of discrete variables corresponding to a subset
+ * of the Gaussian variables and Z is parent of this node
+ *
+ * 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
+    : public HybridFactor,
+      public Conditional<HybridFactor, GaussianMixture> {
+ public:
+  using This = GaussianMixture;
+  using shared_ptr = boost::shared_ptr<GaussianMixture>;
+  using BaseFactor = HybridFactor;
+  using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
+
+  /// 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_;
+
+  /**
+   * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs.
+   */
+  Sum asGaussianFactorGraphTree() const;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Defaut constructor, mainly for serialization.
+  GaussianMixture() = default;
+
+  /**
+   * @brief Construct a new GaussianMixture 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. 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.
+   */
+  GaussianMixture(const KeyVector &continuousFrontals,
+                  const KeyVector &continuousParents,
+                  const DiscreteKeys &discreteParents,
+                  const Conditionals &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 FromConditionals(
+      const KeyVector &continuousFrontals, const KeyVector &continuousParents,
+      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
+  /// @{
+
+  /// Test equality with base HybridFactor
+  bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
+
+  /* print utility */
+  void print(
+      const std::string &s = "GaussianMixture\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  /// @}
+
+  /// Getter for the underlying Conditionals DecisionTree
+  const Conditionals &conditionals();
+
+  /**
+   * @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 add(const Sum &sum) const;
+};
+
+// traits
+template <>
+struct traits<GaussianMixture> : public Testable<GaussianMixture> {};
+
+}  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.cpp

@@ -0,0 +1,97 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  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/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) {}
+
+/* *******************************************************************************/
+bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const {
+  const This *e = dynamic_cast<const This *>(&lf);
+  return e != nullptr && Base::equals(*e, tol);
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor GaussianMixtureFactor::FromFactors(
+    const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys,
+    const std::vector<GaussianFactor::shared_ptr> &factors) {
+  Factors dt(discreteKeys, factors);
+
+  return GaussianMixtureFactor(continuousKeys, discreteKeys, dt);
+}
+
+/* *******************************************************************************/
+void GaussianMixtureFactor::print(const std::string &s,
+                                  const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  std::cout << "]{\n";
+  factors_.print(
+      "", [&](Key k) { return formatter(k); },
+      [&](const GaussianFactor::shared_ptr &gf) -> std::string {
+        RedirectCout rd;
+        std::cout << ":\n";
+        if (gf)
+          gf->print("", formatter);
+        else
+          return {"nullptr"};
+        return rd.str();
+      });
+  std::cout << "}" << std::endl;
+}
+
+/* *******************************************************************************/
+const GaussianMixtureFactor::Factors &GaussianMixtureFactor::factors() {
+  return factors_;
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor::Sum GaussianMixtureFactor::add(
+    const GaussianMixtureFactor::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 tree = asGaussianFactorGraphTree();
+  return sum.empty() ? tree : sum.apply(tree, add);
+}
+
+/* *******************************************************************************/
+GaussianMixtureFactor::Sum GaussianMixtureFactor::asGaussianFactorGraphTree()
+    const {
+  auto wrap = [](const GaussianFactor::shared_ptr &factor) {
+    GaussianFactorGraph result;
+    result.push_back(factor);
+    return result;
+  };
+  return {factors_, wrap};
+}
+}  // namespace gtsam

gtsam/hybrid/GaussianMixtureFactor.h

@@ -0,0 +1,140 @@
+/* ----------------------------------------------------------------------------
+
+ * 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.h
+ * @brief  A set of GaussianFactors, indexed by a set of discrete keys.
+ * @author Fan Jiang
+ * @author Varun Agrawal
+ * @author Frank Dellaert
+ * @date   Mar 12, 2022
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DecisionTree.h>
+#include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
+
+namespace gtsam {
+
+class GaussianFactorGraph;
+
+using GaussianFactorVector = std::vector<gtsam::GaussianFactor::shared_ptr>;
+
+/**
+ * @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.
+ *
+ */
+class GTSAM_EXPORT 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:
+  /// Decision tree of Gaussian factors indexed by discrete keys.
+  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);
+
+  /**
+   * @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);
+
+  /// @}
+  /// @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();
+
+  /**
+   * @brief Combine the Gaussian Factor Graphs in `sum` and `this` while
+   * maintaining the original tree structure.
+   *
+   * @param sum Decision Tree of Gaussian Factor Graphs indexed by the
+   * variables.
+   * @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
+template <>
+struct traits<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
+};
+
+}  // namespace gtsam

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>

gtsam/hybrid/HybridBayesNet.h

@@ -0,0 +1,41 @@
+/* ----------------------------------------------------------------------------
+ * 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>
+
+namespace gtsam {
+
+/**
+ * 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:
+  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() = default;
+};
+
+}  // namespace gtsam

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
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
+ */
+
+#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>
+
+namespace gtsam {
+
+// Instantiate base class
+template class BayesTreeCliqueBase<HybridBayesTreeClique,
+                                   HybridGaussianFactorGraph>;
+template class BayesTree<HybridBayesTreeClique>;
+
+/* ************************************************************************* */
+bool HybridBayesTree::equals(const This& other, double tol) const {
+  return Base::equals(other, tol);
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridBayesTree.h

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ * @date Mar 11, 2022
+ * @author  Fan Jiang
+ */
+
+#pragma once
+
+#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>
+
+#include <string>
+
+namespace gtsam {
+
+// Forward declarations
+class HybridConditional;
+class VectorValues;
+
+/* ************************************************************************* */
+/** A clique in a HybridBayesTree
+ * which is a HybridConditional internally.
+ */
+class GTSAM_EXPORT HybridBayesTreeClique
+    : public BayesTreeCliqueBase<HybridBayesTreeClique,
+                                 HybridGaussianFactorGraph> {
+ public:
+  typedef HybridBayesTreeClique This;
+  typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
+      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;
+
+  /// @}
+};
+
+/**
+ * @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<
+                boost::is_same<CLIQUE, HybridBayesTreeClique>::value> >
+    : public CLIQUE::ConditionalType {
+ public:
+  typedef CLIQUE CliqueType;
+  typedef typename CLIQUE::ConditionalType Base;
+
+  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());
+  }
+
+  /// print utility
+  void print(
+      const std::string& s = "",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+    clique->print(s + "stored clique", formatter);
+  }
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridConditional.cpp

@@ -0,0 +1,108 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/hybrid/HybridFactor.h>
+#include <gtsam/inference/Conditional-inst.h>
+#include <gtsam/inference/Key.h>
+
+namespace gtsam {
+
+/* ************************************************************************ */
+HybridConditional::HybridConditional(const KeyVector &continuousFrontals,
+                                     const DiscreteKeys &discreteFrontals,
+                                     const KeyVector &continuousParents,
+                                     const DiscreteKeys &discreteParents)
+    : HybridConditional(
+          CollectKeys(
+              {continuousFrontals.begin(), continuousFrontals.end()},
+              KeyVector{continuousParents.begin(), continuousParents.end()}),
+          CollectDiscreteKeys(
+              {discreteFrontals.begin(), discreteFrontals.end()},
+              {discreteParents.begin(), discreteParents.end()}),
+          continuousFrontals.size() + discreteFrontals.size()) {}
+
+/* ************************************************************************ */
+HybridConditional::HybridConditional(
+    boost::shared_ptr<GaussianConditional> continuousConditional)
+    : 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 {
+  std::cout << s;
+
+  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++;
+    }
+  }
+}
+
+/* ************************************************************************ */
+bool HybridConditional::equals(const HybridFactor &other, double tol) const {
+  const This *e = dynamic_cast<const This *>(&other);
+  return e != nullptr && BaseFactor::equals(*e, tol);
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridConditional.h

@@ -0,0 +1,177 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/discrete/DiscreteConditional.h>
+#include <gtsam/hybrid/GaussianMixture.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>
+
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <stdexcept>
+#include <string>
+#include <typeinfo>
+#include <vector>
+
+namespace gtsam {
+
+class HybridGaussianFactorGraph;
+
+/**
+ * Hybrid Conditional Density
+ *
+ * As a type-erased variant of:
+ * - 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 Eduaado Madrid's CppCon
+ * talk (https://www.youtube.com/watch?v=s082Qmd_nHs).
+ */
+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
+
+ protected:
+  // Type-erased pointer to the inner type
+  boost::shared_ptr<Factor> inner_;
+
+ public:
+  /// @name Standard Constructors
+  /// @{
+
+  /// 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<GaussianMixture> gaussianMixture);
+
+  /**
+   * @brief Return HybridConditional as a GaussianMixture
+   *
+   * @return GaussianMixture::shared_ptr
+   */
+  GaussianMixture::shared_ptr asMixture() {
+    if (!isHybrid()) throw std::invalid_argument("Not a mixture");
+    return boost::static_pointer_cast<GaussianMixture>(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_);
+  }
+
+  /// @}
+  /// @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;
+
+  /// @}
+
+  /// Get the type-erased pointer to the inner type
+  boost::shared_ptr<Factor> inner() { return inner_; }
+
+};  // DiscreteConditional
+
+// traits
+template <>
+struct traits<HybridConditional> : public Testable<DiscreteConditional> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridDiscreteFactor.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 HybridDiscreteFactor.cpp
+ *  @brief Wrapper for a discrete factor
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#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()),
+      inner_(other) {}
+
+/* ************************************************************************ */
+HybridDiscreteFactor::HybridDiscreteFactor(DecisionTreeFactor &&dtf)
+    : Base(dtf.discreteKeys()),
+      inner_(boost::make_shared<DecisionTreeFactor>(std::move(dtf))) {}
+
+/* ************************************************************************ */
+bool HybridDiscreteFactor::equals(const HybridFactor &lf, double tol) const {
+  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);
+}
+
+/* ************************************************************************ */
+void HybridDiscreteFactor::print(const std::string &s,
+                                 const KeyFormatter &formatter) const {
+  HybridFactor::print(s, formatter);
+  inner_->print("\n", formatter);
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridDiscreteFactor.h

@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ *  @author Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/discrete/DecisionTreeFactor.h>
+#include <gtsam/discrete/DiscreteFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+
+namespace gtsam {
+
+/**
+ * A HybridDiscreteFactor is a thin container for DiscreteFactor, which allows
+ * us to hide the implementation of DiscreteFactor and thus avoid diamond
+ * inheritance.
+ */
+class GTSAM_EXPORT HybridDiscreteFactor : public HybridFactor {
+ private:
+  DiscreteFactor::shared_ptr inner_;
+
+ public:
+  using Base = HybridFactor;
+  using This = HybridDiscreteFactor;
+  using shared_ptr = boost::shared_ptr<This>;
+
+  /// @name Constructors
+  /// @{
+
+  // Implicit conversion from a shared ptr of DF
+  HybridDiscreteFactor(DiscreteFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete DecisionTreeFactor
+  HybridDiscreteFactor(DecisionTreeFactor &&dtf);
+
+  /// @}
+  /// @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_; }
+};
+
+// traits
+template <>
+struct traits<HybridDiscreteFactor> : public Testable<HybridDiscreteFactor> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridEliminationTree.cpp

@@ -0,0 +1,42 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/hybrid/HybridEliminationTree.h>
+#include <gtsam/inference/EliminationTree-inst.h>
+
+namespace gtsam {
+
+// Instantiate base class
+template class EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>;
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridGaussianFactorGraph& factorGraph,
+    const VariableIndex& structure, const Ordering& order)
+    : Base(factorGraph, structure, order) {}
+
+/* ************************************************************************* */
+HybridEliminationTree::HybridEliminationTree(
+    const HybridGaussianFactorGraph& factorGraph, const Ordering& order)
+    : Base(factorGraph, order) {}
+
+/* ************************************************************************* */
+bool HybridEliminationTree::equals(const This& other, double tol) const {
+  return Base::equals(other, tol);
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridEliminationTree.h

@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/HybridGaussianFactorGraph.h>
+#include <gtsam/inference/EliminationTree.h>
+
+namespace gtsam {
+
+/**
+ * Elimination Tree type for Hybrid
+ */
+class GTSAM_EXPORT HybridEliminationTree
+    : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
+ private:
+  friend class ::EliminationTreeTester;
+
+ public:
+  typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
+      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.
+   * @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 HybridGaussianFactorGraph& 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 HybridGaussianFactorGraph& factorGraph,
+                        const Ordering& order);
+
+  /// @}
+
+  /** Test whether the tree is equal to another */
+  bool equals(const This& other, double tol = 1e-9) const;
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridFactor.cpp

@@ -0,0 +1,104 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+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;
+}
+
+/* ************************************************************************ */
+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(const KeyVector &keys)
+    : Base(keys),
+      isContinuous_(true),
+      nrContinuous_(keys.size()),
+      continuousKeys_(keys) {}
+
+/* ************************************************************************ */
+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()),
+      discreteKeys_(discreteKeys),
+      continuousKeys_(continuousKeys) {}
+
+/* ************************************************************************ */
+HybridFactor::HybridFactor(const DiscreteKeys &discreteKeys)
+    : Base(CollectKeys({}, discreteKeys)),
+      isDiscrete_(true),
+      discreteKeys_(discreteKeys),
+      continuousKeys_({}) {}
+
+/* ************************************************************************ */
+bool HybridFactor::equals(const HybridFactor &lf, double tol) const {
+  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_;
+}
+
+/* ************************************************************************ */
+void HybridFactor::print(const std::string &s,
+                         const KeyFormatter &formatter) const {
+  std::cout << s;
+  if (isContinuous_) std::cout << "Continuous ";
+  if (isDiscrete_) std::cout << "Discrete ";
+  if (isHybrid_) std::cout << "Hybrid ";
+  for (size_t c=0; c<continuousKeys_.size(); c++) {
+    std::cout << 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

gtsam/hybrid/HybridFactor.h

@@ -0,0 +1,136 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/base/Testable.h>
+#include <gtsam/discrete/DiscreteKey.h>
+#include <gtsam/inference/Factor.h>
+#include <gtsam/nonlinear/Values.h>
+
+#include <cstddef>
+#include <string>
+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
+ *
+ * Examples:
+ *  - HybridGaussianFactor
+ *  - HybridDiscreteFactor
+ *  - GaussianMixtureFactor
+ *  - GaussianMixture
+ */
+class GTSAM_EXPORT HybridFactor : public Factor {
+ private:
+  bool isDiscrete_ = false;
+  bool isContinuous_ = false;
+  bool isHybrid_ = false;
+
+  size_t nrContinuous_ = 0;
+
+ protected:
+  DiscreteKeys discreteKeys_;
+
+  /// Record continuous keys for book-keeping
+  KeyVector continuousKeys_;
+
+ 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
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** 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);
+
+  /// Virtual destructor
+  virtual ~HybridFactor() = default;
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// equals
+  virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;
+
+  /// print
+  void print(
+      const std::string &s = "HybridFactor\n",
+      const KeyFormatter &formatter = DefaultKeyFormatter) const override;
+
+  /// @}
+  /// @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
+
+// traits
+template <>
+struct traits<HybridFactor> : public Testable<HybridFactor> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -0,0 +1,47 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+
+#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 &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);
+  inner_->print("\n", formatter);
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.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 HybridGaussianFactor.h
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+namespace gtsam {
+
+/**
+ * A HybridGaussianFactor is a layer over GaussianFactor so that we do not have
+ * a diamond inheritance i.e. an extra factor type that inherits from both
+ * HybridFactor and GaussianFactor.
+ */
+class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
+ private:
+  GaussianFactor::shared_ptr inner_;
+
+ public:
+  using Base = HybridFactor;
+  using This = HybridGaussianFactor;
+  using shared_ptr = boost::shared_ptr<This>;
+
+  // Explicit conversion from a shared ptr of GF
+  explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
+
+  // Forwarding constructor from concrete JacobianFactor
+  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_; }
+};
+
+// traits
+template <>
+struct traits<HybridGaussianFactor> : public Testable<HybridGaussianFactor> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -0,0 +1,369 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   HybridGaussianFactorGraph.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/DiscreteEliminationTree.h>
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/discrete/DiscreteJunctionTree.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>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.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/GaussianEliminationTree.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+#include <algorithm>
+#include <cstddef>
+#include <iostream>
+#include <iterator>
+#include <memory>
+#include <stdexcept>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+
+template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
+
+/* ************************************************************************ */
+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 = GaussianMixtureFactor::Sum(result);
+
+  } else {
+    auto add = [&factor](const Y &graph) {
+      auto result = graph;
+      result.push_back(factor);
+      return result;
+    };
+    sum = sum.apply(add);
+  }
+  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,
+                      const Ordering &frontalKeys) {
+  GaussianFactorGraph gfg;
+  for (auto &fp : factors) {
+    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 {
+      // It is an orphan wrapped conditional
+    }
+  }
+
+  auto result = EliminatePreferCholesky(gfg, frontalKeys);
+  return {boost::make_shared<HybridConditional>(result.first),
+          boost::make_shared<HybridGaussianFactor>(result.second)};
+}
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
+discreteElimination(const HybridGaussianFactorGraph &factors,
+                    const Ordering &frontalKeys) {
+  DiscreteFactorGraph dfg;
+  for (auto &fp : factors) {
+    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 {
+      // It is an orphan wrapper
+    }
+  }
+
+  auto result = EliminateDiscrete(dfg, frontalKeys);
+
+  return {boost::make_shared<HybridConditional>(result.first),
+          boost::make_shared<HybridDiscreteFactor>(result.second)};
+}
+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
+hybridElimination(const HybridGaussianFactorGraph &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
+  GaussianMixtureFactor::Sum sum = sumFrontals(factors);
+
+  using EliminationPair = GaussianFactorGraph::EliminationResult;
+
+  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()) {
+      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;
+  };
+
+  // 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;
+
+  // 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
+  // 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 HybridGaussianFactorGraph &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
+  // 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 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 carefully consider 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 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 :)
+
+  // PREPROCESS: Identify the nature of the current elimination
+  std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
+  std::set<DiscreteKey> discreteSeparatorSet;
+  std::set<DiscreteKey> discreteFrontals;
+
+  KeySet separatorKeys;
+  KeySet allContinuousKeys;
+  KeySet continuousFrontals;
+  KeySet continuousSeparator;
+
+  // This initializes separatorKeys and mapFromKeyToDiscreteKey
+  for (auto &&factor : factors) {
+    separatorKeys.insert(factor->begin(), factor->end());
+    if (!factor->isContinuous()) {
+      for (auto &k : factor->discreteKeys()) {
+        mapFromKeyToDiscreteKey[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 (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
+      discreteFrontals.insert(mapFromKeyToDiscreteKey.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 (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
+      discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
+    } else {
+      continuousSeparator.insert(k);
+      allContinuousKeys.insert(k);
+    }
+  }
+
+  // NOTE: We should really defer the product here because of pruning
+
+  // Case 1: we are only dealing with continuous
+  if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
+    return continuousElimination(factors, frontalKeys);
+  }
+
+  // Case 2: we are only dealing with discrete
+  if (allContinuousKeys.empty()) {
+    return discreteElimination(factors, frontalKeys);
+  }
+
+  // Case 3: We are now in the hybrid land!
+  return hybridElimination(factors, frontalKeys, continuousSeparator,
+                           discreteSeparatorSet);
+}
+
+/* ************************************************************************ */
+void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
+  FactorGraph::add(boost::make_shared<HybridGaussianFactor>(std::move(factor)));
+}
+
+/* ************************************************************************ */
+void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
+  FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
+}
+
+/* ************************************************************************ */
+void HybridGaussianFactorGraph::add(DecisionTreeFactor &&factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(std::move(factor)));
+}
+
+/* ************************************************************************ */
+void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) {
+  FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -0,0 +1,118 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   HybridGaussianFactorGraph.h
+ * @brief  Linearized 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 HybridGaussianFactorGraph;
+class HybridConditional;
+class HybridBayesNet;
+class HybridEliminationTree;
+class HybridBayesTree;
+class HybridJunctionTree;
+class DecisionTreeFactor;
+
+class JacobianFactor;
+
+/** Main elimination function for HybridGaussianFactorGraph */
+GTSAM_EXPORT
+std::pair<boost::shared_ptr<HybridConditional>, HybridFactor::shared_ptr>
+EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
+
+/* ************************************************************************* */
+template <>
+struct EliminationTraits<HybridGaussianFactorGraph> {
+  typedef HybridFactor FactorType;  ///< Type of factors in factor graph
+  typedef HybridGaussianFactorGraph
+      FactorGraphType;  ///< Type of the factor graph (e.g.
+                        ///< 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
+  /// 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);
+  }
+};
+
+/**
+ * Gaussian Hybrid Factor Graph
+ * -----------------------
+ * This is the linearized version of a hybrid factor graph.
+ * Everything inside needs to be hybrid factor or hybrid conditional.
+ */
+class GTSAM_EXPORT HybridGaussianFactorGraph
+    : public FactorGraph<HybridFactor>,
+      public EliminateableFactorGraph<HybridGaussianFactorGraph> {
+ public:
+  using Base = FactorGraph<HybridFactor>;
+  using This = HybridGaussianFactorGraph;  ///< 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
+
+  /// @name Constructors
+  /// @{
+
+  HybridGaussianFactorGraph() = default;
+
+  /**
+   * Implicit copy/downcast constructor to override explicit template container
+   * constructor. In BayesTree this is used for:
+   * `cachedSeparatorMarginal_.reset(*separatorMarginal)`
+   * */
+  template <class DERIVEDFACTOR>
+  HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
+      : Base(graph) {}
+
+  /// @}
+
+  using FactorGraph::add;
+
+  /// 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);
+};
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianISAM.cpp

@@ -0,0 +1,101 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridGaussianISAM.h
+ * @date March 31, 2022
+ * @author Fan Jiang
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridGaussianISAM.h>
+#include <gtsam/inference/ISAM-inst.h>
+#include <gtsam/inference/Key.h>
+
+#include <iterator>
+
+namespace gtsam {
+
+// Instantiate base class
+// template class ISAM<HybridBayesTree>;
+
+/* ************************************************************************* */
+HybridGaussianISAM::HybridGaussianISAM() {}
+
+/* ************************************************************************* */
+HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
+    : Base(bayesTree) {}
+
+/* ************************************************************************* */
+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();
+  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);
+
+  // 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 HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
+                                const HybridBayesTree::Eliminate& function) {
+  Cliques orphans;
+  this->updateInternal(newFactors, &orphans, function);
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridGaussianISAM.h

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 HybridGaussianISAM.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/HybridGaussianFactorGraph.h>
+#include <gtsam/inference/ISAM.h>
+
+namespace gtsam {
+
+class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
+ public:
+  typedef ISAM<HybridBayesTree> Base;
+  typedef HybridGaussianISAM This;
+  typedef boost::shared_ptr<This> shared_ptr;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** Create an empty Bayes Tree */
+  HybridGaussianISAM();
+
+  /** Copy constructor */
+  HybridGaussianISAM(const HybridBayesTree& bayesTree);
+
+  /// @}
+
+ private:
+  /// Internal method that performs the ISAM update.
+  void updateInternal(
+      const HybridGaussianFactorGraph& 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 HybridGaussianFactorGraph& newFactors,
+              const HybridBayesTree::Eliminate& function =
+                  HybridBayesTree::EliminationTraitsType::DefaultEliminate);
+};
+
+/// traits
+template <>
+struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
+
+}  // namespace gtsam

gtsam/hybrid/HybridJunctionTree.cpp

@@ -0,0 +1,173 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 11, 2022
+ * @author Fan Jiang
+ */
+
+#include <gtsam/hybrid/HybridEliminationTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
+#include <gtsam/hybrid/HybridJunctionTree.h>
+#include <gtsam/inference/JunctionTree-inst.h>
+#include <gtsam/inference/Key.h>
+
+#include <unordered_map>
+
+namespace gtsam {
+
+// Instantiate base classes
+template class EliminatableClusterTree<HybridBayesTree,
+                                       HybridGaussianFactorGraph>;
+template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
+
+struct HybridConstructorTraversalData {
+  typedef
+      typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
+          Node;
+  typedef
+      typename JunctionTree<HybridBayesTree,
+                            HybridGaussianFactorGraph>::sharedNode sharedNode;
+
+  HybridConstructorTraversalData* const parentData;
+  sharedNode junctionTreeNode;
+  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 data =
+        HybridConstructorTraversalData(&parentData);
+    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()) {
+        data.discreteKeys.insert(k.first);
+      }
+    }
+
+    return data;
+  }
+
+  // Post-order visitor function
+  static void ConstructorTraversalVisitorPostAlg2(
+      const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
+      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
+    // 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() +
+                            data.childSymbolicFactors.size());
+    // Add ETree node factors
+    symbolicFactors += ETreeNode->factors;
+    // Add symbolic factors passed up from children
+    symbolicFactors += data.childSymbolicFactors;
+
+    Ordering keyAsOrdering;
+    keyAsOrdering.push_back(ETreeNode->key);
+    SymbolicConditional::shared_ptr conditional;
+    SymbolicFactor::shared_ptr separatorFactor;
+    boost::tie(conditional, separatorFactor) =
+        internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
+
+    // Store symbolic elimination results in the parent
+    data.parentData->childSymbolicConditionals.push_back(conditional);
+    data.parentData->childSymbolicFactors.push_back(separatorFactor);
+    data.parentData->discreteKeys.merge(data.discreteKeys);
+
+    sharedNode node = data.junctionTreeNode;
+    const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
+        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 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> nrChildrenFrontals = node->nrFrontalsOfChildren();
+    std::vector<bool> merge(nrChildren, false);
+    size_t nrFrontals = 1;
+    for (size_t i = 0; i < nrChildren; i++) {
+      // Check if we should merge the i^th child
+      if (nrParents + nrFrontals == childConditionals[i]->nrParents()) {
+        const bool myType =
+            data.discreteKeys.exists(conditional->frontals()[0]);
+        const bool theirType =
+            data.discreteKeys.exists(childConditionals[i]->frontals()[0]);
+
+        if (myType == theirType) {
+          // Increment number of frontal variables
+          nrFrontals += nrChildrenFrontals[i];
+          merge[i] = true;
+        }
+      }
+    }
+
+    // now really merge
+    node->mergeChildren(merge);
+  }
+};
+
+/* ************************************************************************* */
+HybridJunctionTree::HybridJunctionTree(
+    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 HybridConstructorTraversalData Data;
+  Data rootData(0);
+  rootData.junctionTreeNode =
+      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.junctionTreeNode);
+
+  // Transfer remaining factors from elimination tree
+  Base::remainingFactors_ = eliminationTree.remainingFactors();
+}
+
+}  // namespace gtsam

gtsam/hybrid/HybridJunctionTree.h

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.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, HybridGaussianFactorGraph> {
+ public:
+  typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
+      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);
+};
+
+}  // namespace gtsam

gtsam/hybrid/hybrid.i

@@ -0,0 +1,145 @@
+//*************************************************************************
+// 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/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* inner();
+};
+
+#include <gtsam/hybrid/GaussianMixtureFactor.h>
+class GaussianMixtureFactor : gtsam::HybridFactor {
+  static GaussianMixtureFactor FromFactors(
+      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 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 = "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/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::HybridGaussianFactorGraph& 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::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::HybridGaussianFactorGraph*>
+  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::HybridGaussianFactorGraph*>
+  eliminatePartialMultifrontal(const gtsam::Ordering& ordering);
+
+  string dot(
+      const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
+      const gtsam::DotWriter& writer = gtsam::DotWriter()) const;
+};
+
+}  // namespace gtsam

gtsam/hybrid/tests/CMakeLists.txt

@@ -0,0 +1 @@
+gtsamAddTestsGlob(hybrid "test*.cpp" "" "gtsam")

gtsam/hybrid/tests/Switching.h

@@ -0,0 +1,86 @@
+/* ----------------------------------------------------------------------------
+
+ * 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>
+#include <gtsam/hybrid/HybridGaussianFactorGraph.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 HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
+    size_t n, std::function<Key(int)> keyFunc = X,
+    std::function<Key(int)> dKeyFunc = C) {
+  HybridGaussianFactorGraph 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::FromFactors(
+        {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<HybridGaussianFactorGraph>(std::move(hfg));
+}
+
+inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
+    std::vector<Key> &input) {
+  KeyVector 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

gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp

@@ -0,0 +1,593 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testHybridGaussianFactorGraph.cpp
+ *  @date Mar 11, 2022
+ *  @author Fan Jiang
+ */
+
+#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>
+#include <gtsam/hybrid/HybridBayesTree.h>
+#include <gtsam/hybrid/HybridConditional.h>
+#include <gtsam/hybrid/HybridDiscreteFactor.h>
+#include <gtsam/hybrid/HybridFactor.h>
+#include <gtsam/hybrid/HybridGaussianFactor.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>
+#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 "Switching.h"
+
+using namespace boost::assign;
+
+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;
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, creation) {
+  HybridConditional test;
+
+  HybridGaussianFactorGraph 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)));
+  GTSAM_PRINT(clgc);
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, eliminate) {
+  HybridGaussianFactorGraph hfg;
+
+  hfg.add(HybridGaussianFactor(JacobianFactor(0, I_3x3, Z_3x1)));
+
+  auto result = hfg.eliminatePartialSequential(KeyVector{0});
+
+  EXPECT_LONGS_EQUAL(result.first->size(), 1);
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, eliminateMultifrontal) {
+  HybridGaussianFactorGraph hfg;
+
+  DiscreteKey c(C(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
+
+  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);
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) {
+  HybridGaussianFactorGraph 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(HybridGaussianFactorGraph, eliminateFullSequentialSimple) {
+  HybridGaussianFactorGraph 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));
+  // 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(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(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) {
+  HybridGaussianFactorGraph 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));
+  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())}));
+
+  // hfg.add(GaussianMixtureFactor({X(0)}, {c1}, dt));
+  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")));
+
+  auto result = hfg.eliminateMultifrontal(
+      Ordering::ColamdConstrainedLast(hfg, {C(1), C(2)}));
+
+  GTSAM_PRINT(*result);
+  GTSAM_PRINT(*result->marginalFactor(C(2)));
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) {
+  HybridGaussianFactorGraph hfg;
+
+  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));
+
+  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)}, {c}, dt));
+  hfg.add(HybridDiscreteFactor(DecisionTreeFactor(c, {2, 8})));
+  //  hfg.add(HybridDiscreteFactor(DecisionTreeFactor({{C(1), 2}, {C(2), 2}}, "1
+  //  2 3 4")));
+
+  auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {C(1)});
+
+  HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full);
+
+  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(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));
+
+  {
+    // 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::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())}));
+
+    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(GaussianMixtureFactor({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(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(GaussianMixtureFactor({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;
+  HybridGaussianFactorGraph::shared_ptr remaining;
+  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.
+  */
+}
+
+/* ************************************************************************* */
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridGaussianFactorGraph, Switching) {
+  auto N = 12;
+  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);
+
+  // 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;
+  HybridGaussianFactorGraph::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);
+  }
+  /*
+  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.
+  */
+  hbt->marginalFactor(C(11))->print("HBT: ");
+}
+
+/* ************************************************************************* */
+// TODO(fan): make a graph like Varun's paper one
+TEST(HybridGaussianFactorGraph, 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;
+  HybridGaussianFactorGraph::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 = HybridGaussianISAM(*hbt);
+
+  {
+    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);
+    std::cout << isam.dot();
+    isam.marginalFactor(C(11))->print();
+  }
+}
+
+/* ************************************************************************* */
+TEST(HybridGaussianFactorGraph, 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;
+    HybridGaussianFactorGraph::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";
+    }
+  }
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/hybrid/tests/testGaussianMixture.cpp

@@ -0,0 +1,95 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+/* ************************************************************************* */
+/* 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;
+  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);
+}
+/* ************************************************************************* */

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);
+}
+/* ************************************************************************* */

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,33 @@ namespace gtsam {
   }; // BayesTree
 
   /* ************************************************************************* */
-  template<class CLIQUE>
+  template <class CLIQUE, typename = void>
-  class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType
+  class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType {
-  {
+   public:
-  public:
     typedef CLIQUE CliqueType;
     typedef typename CLIQUE::ConditionalType Base;
 
     boost::shared_ptr<CliqueType> clique;
 
-    BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique) :
+    /**
-      clique(clique)
+     * @brief Construct a new Bayes Tree Orphan Wrapper object
-    {
+     *
-      // Store parent keys in our base type factor so that eliminating those parent keys will pull
+     * This object stores parent keys in our base type factor so that
-      // this subtree into the elimination.
+     * eliminating those parent keys will pull this subtree into the
-      this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
+     * elimination.
+     *
+     * @param clique Orphan clique to add for further consideration in
+     * elimination.
+     */
+    BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
+        : clique(clique) {
+      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);
     }
   };

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> >

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
    */

gtsam/inference/JunctionTree.h

@@ -70,7 +70,7 @@ namespace gtsam {
 
     /// @}
 
-  private:
+  protected:
 
     // Private default constructor (used in static construction methods)
     JunctionTree() {}

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;

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 @{

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/HybridGaussianFactorGraph.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::HybridGaussianFactorGraph}>
   static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
 
   template <
       FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
-                      gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
+                      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::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::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::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::SymbolicFactorGraph, gtsam::GaussianFactorGraph, gtsam::HybridGaussianFactorGraph}>
   static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
                                 const FACTOR_GRAPH& graph);
 

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) {

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 */

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__

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 

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;

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 =

gtsam/linear/tests/testNoiseModel.cpp

@@ -661,12 +661,13 @@ 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->squaredMahalanobisDistance(error), 1e-8);
   }

gtsam/nonlinear/GncOptimizer.h

@@ -207,9 +207,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;
     }
@@ -218,12 +220,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);
 
@@ -255,10 +261,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;
   }
 
@@ -293,6 +301,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){
+          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)
@@ -340,8 +353,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)
+    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
-      std::cout << "checkCostConvergence = true " << std::endl;
+      std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
+                << ", curr. cost = " << cost << ")" << std::endl;
+    }
     return costConverged;
   }
 
@@ -436,18 +451,16 @@ 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
-            if (u2_k >= upperbound) {
+            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;
-            } 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;
             }
           }
         }

gtsam/nonlinear/GncParams.h

@@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams {
   enum Verbosity {
     SILENT = 0,
     SUMMARY,
+    MU,
+    WEIGHTS,
     VALUES
   };
 

gtsam/sam/BearingRangeFactor.h

@@ -79,9 +79,8 @@ class BearingRangeFactor
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
-    const Vector error = unwhitenedError(
+    const Vector error = this->unwhitenedError(
-      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+        {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
-      Hs);
     if (H1) *H1 = Hs[0];
     if (H2) *H2 = Hs[1];
     return error;

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

gtsam/sfm/TranslationRecovery.cpp

@@ -119,21 +119,24 @@ 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());
   }
 }
 
 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 +158,20 @@ 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());
+  }
   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,
+  return initializeRandomly(relativeTranslations, betweenTranslations,
-                            initialValues);
+                            &kRandomNumberGenerator, initialValues);
 }
 
 Values TranslationRecovery::run(
@@ -187,8 +196,8 @@ Values TranslationRecovery::run(
            &graph);
 
   // Uses initial values from params if provided.
-  Values initial =
+  Values initial = initializeRandomly(
-      initializeRandomly(nonzeroRelativeTranslations, initialValues);
+      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.

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;
 
   /**
@@ -137,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

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

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();
+        Quaternion quaternion = Rot3(R).toQuaternion();
-        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
+        myfile << quaternion.x() << " " << quaternion.y() << " " << quaternion.z()
-               << " " << quaternion(0);
+               << " " << quaternion.w();
         myfile << "\n";
     }
     myfile.close();

python/CMakeLists.txt

@@ -69,6 +69,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)
@@ -105,7 +106,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
 # Hack to get python test and util files copied every time they are modified
 file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
 foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
-        configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
+        get_filename_component(test_file_name ${test_file} NAME)
+        configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY)
 endforeach()
 file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
 foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})

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

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

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

python/gtsam/preamble/discrete.h

@@ -13,4 +13,3 @@
 
 #include <pybind11/stl.h>
 
-PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys);

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++.
+ */
+
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::GaussianFactor::shared_ptr>);

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>

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> >();

python/gtsam/specializations/inference.h

@@ -11,3 +11,4 @@
  * and saves one copy operation.
  */
 
+py::bind_map<std::map<char, double>>(m_, "__MapCharDouble");

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

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()

python/gtsam/tests/test_HybridFactorGraph.py

@@ -0,0 +1,60 @@
+"""
+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 C, X
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestHybridGaussianFactorGraph(GtsamTestCase):
+    """Unit tests for HybridGaussianFactorGraph."""
+
+    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))
+
+        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.FromFactors([X(0)], dk, [jf1, jf2])
+
+        hfg = gtsam.HybridGaussianFactorGraph()
+        hfg.add(jf1)
+        hfg.add(jf2)
+        hfg.push_back(gmf)
+
+        hbn = hfg.eliminateSequential(
+            gtsam.Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
+                hfg, [C(0)]))
+
+        # print("hbn = ", hbn)
+        self.assertEqual(hbn.size(), 2)
+
+        mixture = hbn.at(0).inner()
+        self.assertIsInstance(mixture, gtsam.GaussianMixture)
+        self.assertEqual(len(mixture.keys()), 2)
+
+        discrete_conditional = hbn.at(hbn.size() - 1).inner()
+        self.assertIsInstance(discrete_conditional, gtsam.DiscreteConditional)
+
+
+if __name__ == "__main__":
+    unittest.main()

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,4 @@ class TestPose3(GtsamTestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    unittest.main()

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()

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())

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);
+}

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;

wrap/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}