Merge branch 'develop' into feature/NoiseModelFactorN_test

.gitignore

@@ -18,3 +18,4 @@
 CMakeLists.txt.user*
 xcode/
 /Dockerfile
+/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb

CMakeLists.txt

@@ -10,7 +10,7 @@ endif()
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 2)
 set (GTSAM_VERSION_PATCH 0)
-set (GTSAM_PRERELEASE_VERSION "a6")
+set (GTSAM_PRERELEASE_VERSION "a7")
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 
 if (${GTSAM_VERSION_PATCH} EQUAL 0)

cmake/HandlePython.cmake

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

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

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/discrete/tests/testDiscreteFactorGraph.cpp

@@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
       "graph {\n"
       "  size=\"5,5\";\n"
       "\n"
-      "  varC[label=\"C\"];\n"
-      "  varA[label=\"A\"];\n"
-      "  varB[label=\"B\"];\n"
+      "  var0[label=\"C\"];\n"
+      "  var1[label=\"A\"];\n"
+      "  var2[label=\"B\"];\n"
       "\n"
       "  factor0[label=\"\", shape=point];\n"
-      "  varC--factor0;\n"
-      "  varA--factor0;\n"
+      "  var0--factor0;\n"
+      "  var1--factor0;\n"
       "  factor1[label=\"\", shape=point];\n"
-      "  varC--factor1;\n"
-      "  varB--factor1;\n"
+      "  var0--factor1;\n"
+      "  var2--factor1;\n"
       "}\n";
   EXPECT(actual == expected);
 }

gtsam/geometry/Point2.cpp

@@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
   return circleCircleIntersection(c1, c2, fh);
 }
 
+Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
+  const size_t n = abPointPairs.size();
+  if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
+  Point2 aSum(0, 0), bSum(0, 0);
+  for (const Point2Pair &abPair : abPointPairs) {
+    aSum += abPair.first;
+    bSum += abPair.second;
+  }
+  const double f = 1.0 / n;
+  return {aSum * f, bSum * f};
+}
+  
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
   os << p.first << " <-> " << p.second;

gtsam/geometry/Point2.h

@@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
  */
 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
+  
+/// Calculate the two means of a set of Point2 pairs
+GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
 
 /**
  * @brief Intersect 2 circles

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/Pose2.cpp

@@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
       "Pose2:Align expects 2*N matrices of equal shape.");
   }
   Point2Pairs ab_pairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     ab_pairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose2::Align(ab_pairs);

gtsam/geometry/Pose2.h

@@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional<Pose2>
 GTSAM_DEPRECATED align(const Point2Pairs& pairs);
 #endif
 
+// Convenience typedef
+using Pose2Pair = std::pair<Pose2, Pose2>;
+using Pose2Pairs = std::vector<Pose2Pair>;
+
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 

gtsam/geometry/Pose3.cpp

@@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
       "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     abPointPairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose3::Align(abPointPairs);
@@ -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/Rot2.cpp

@@ -129,6 +129,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   }
 }
 
+/* ************************************************************************* */
+Rot2 Rot2::ClosestTo(const Matrix2& M) {
+  Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const Matrix2& U = svd.matrixU();
+  const Matrix2& V = svd.matrixV();
+  const double det = (U * V.transpose()).determinant();
+  Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
+
+  double c = M_prime(0, 0);
+  double s = M_prime(1, 0);
+  return Rot2::fromCosSin(c, s);
+}
+
 /* ************************************************************************* */
 
 } // gtsam

gtsam/geometry/Rot2.h

@@ -14,6 +14,7 @@
  * @brief 2D rotation
  * @date Dec 9, 2009
  * @author Frank Dellaert
+ * @author John Lambert
  */
 
 #pragma once
@@ -209,6 +210,9 @@ namespace gtsam {
     /** return 2*2 transpose (inverse) rotation matrix   */
     Matrix2 transpose() const;
 
+    /** Find closest valid rotation matrix, given a 2x2 matrix */
+    static Rot2 ClosestTo(const Matrix2& M);
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

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
-    // c22 = a11 * cx - a12 * sx
+    // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
+    // c11 = a11 * cx - a12 * sx
     const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
     const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
     Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);

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/Similarity2.cpp

@@ -0,0 +1,242 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Similarity2.cpp
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert, Varun Agrawal
+ */
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Similarity2.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+
+namespace gtsam {
+
+using std::vector;
+
+namespace internal {
+
+/// Subtract centroids from point pairs.
+static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
+                                     const Point2Pair& centroids) {
+  Point2Pairs d_abPointPairs;
+  for (const Point2Pair& abPair : abPointPairs) {
+    Point2 da = abPair.first - centroids.first;
+    Point2 db = abPair.second - centroids.second;
+    d_abPointPairs.emplace_back(da, db);
+  }
+  return d_abPointPairs;
+}
+
+/// Form inner products x and y and calculate scale.
+static double CalculateScale(const Point2Pairs& d_abPointPairs,
+                             const Rot2& aRb) {
+  double x = 0, y = 0;
+  Point2 da, db;
+
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    std::tie(da, db) = d_abPair;
+    const Vector2 da_prime = aRb * db;
+    y += da.transpose() * da_prime;
+    x += da_prime.transpose() * da_prime;
+  }
+  const double s = y / x;
+  return s;
+}
+
+/// Form outer product H.
+static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) {
+  Matrix2 H = Z_2x2;
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    H += d_abPair.first * d_abPair.second.transpose();
+  }
+  return H;
+}
+
+/**
+ * @brief This method estimates the similarity transform from differences point
+ * pairs, given a known or estimated rotation and point centroids.
+ *
+ * @param d_abPointPairs
+ * @param aRb
+ * @param centroids
+ * @return Similarity2
+ */
+static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
+                         const Point2Pair& centroids) {
+  const double s = CalculateScale(d_abPointPairs, aRb);
+  // dividing aTb by s is required because the registration cost function
+  // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
+  const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
+  return Similarity2(aRb, aTb, s);
+}
+
+/**
+ * @brief This method estimates the similarity transform from point pairs,
+ * given a known or estimated rotation.
+ * Refer to:
+ * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+ * Chapter 3
+ *
+ * @param abPointPairs
+ * @param aRb
+ * @return Similarity2
+ */
+static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs,
+                               const Rot2& aRb) {
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
+  return internal::Align(d_abPointPairs, aRb, centroids);
+}
+}  // namespace internal
+
+Similarity2::Similarity2() : t_(0, 0), s_(1) {}
+
+Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
+
+Similarity2::Similarity2(const Rot2& R, const Point2& t, double s)
+    : R_(R), t_(t), s_(s) {}
+
+Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s)
+    : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {}
+
+Similarity2::Similarity2(const Matrix3& T)
+    : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())),
+      t_(T.topRightCorner<2, 1>()),
+      s_(1.0 / T(2, 2)) {}
+
+bool Similarity2::equals(const Similarity2& other, double tol) const {
+  return R_.equals(other.R_, tol) &&
+         traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
+         s_ > (other.s_ - tol);
+}
+
+bool Similarity2::operator==(const Similarity2& other) const {
+  return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
+}
+
+void Similarity2::print(const std::string& s) const {
+  std::cout << std::endl;
+  std::cout << s;
+  rotation().print("\nR:\n");
+  std::cout << "t: " << translation().transpose() << " s: " << scale()
+            << std::endl;
+}
+
+Similarity2 Similarity2::identity() { return Similarity2(); }
+
+Similarity2 Similarity2::operator*(const Similarity2& S) const {
+  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
+}
+
+Similarity2 Similarity2::inverse() const {
+  const Rot2 Rt = R_.inverse();
+  const Point2 sRt = Rt * (-s_ * t_);
+  return Similarity2(Rt, sRt, 1.0 / s_);
+}
+
+Point2 Similarity2::transformFrom(const Point2& p) const {
+  const Point2 q = R_ * p + t_;
+  return s_ * q;
+}
+
+Pose2 Similarity2::transformFrom(const Pose2& T) const {
+  Rot2 R = R_.compose(T.rotation());
+  Point2 t = Point2(s_ * (R_ * T.translation() + t_));
+  return Pose2(R, t);
+}
+
+Point2 Similarity2::operator*(const Point2& p) const {
+  return transformFrom(p);
+}
+
+Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) {
+  // Refer to Chapter 3 of
+  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  if (abPointPairs.size() < 2)
+    throw std::runtime_error("input should have at least 2 pairs of points");
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
+  Matrix2 H = internal::CalculateH(d_abPointPairs);
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot2 aRb = Rot2::ClosestTo(H);
+  return internal::Align(d_abPointPairs, aRb, centroids);
+}
+
+Similarity2 Similarity2::Align(const Pose2Pairs& abPosePairs) {
+  const size_t n = abPosePairs.size();
+  if (n < 2)
+    throw std::runtime_error("input should have at least 2 pairs of poses");
+
+  // calculate rotation
+  vector<Rot2> rotations;
+  Point2Pairs abPointPairs;
+  rotations.reserve(n);
+  abPointPairs.reserve(n);
+  // Below denotes the pose of the i'th object/camera/etc
+  // in frame "a" or frame "b".
+  Pose2 aTi, bTi;
+  for (const Pose2Pair& abPair : abPosePairs) {
+    std::tie(aTi, bTi) = abPair;
+    const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+    rotations.emplace_back(aRb);
+    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
+  }
+  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
+
+  return internal::AlignGivenR(abPointPairs, aRb_estimate);
+}
+
+Vector4 Similarity2::Logmap(const Similarity2& S,  //
+                            OptionalJacobian<4, 4> Hm) {
+  const Vector2 u = S.t_;
+  const Vector1 w = Rot2::Logmap(S.R_);
+  const double s = log(S.s_);
+  Vector4 result;
+  result << u, w, s;
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
+  }
+  return result;
+}
+
+Similarity2 Similarity2::Expmap(const Vector4& v,  //
+                                OptionalJacobian<4, 4> Hm) {
+  const Vector2 t = v.head<2>();
+  const Rot2 R = Rot2::Expmap(v.segment<1>(2));
+  const double s = v[3];
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
+  }
+  return Similarity2(R, t, s);
+}
+
+Matrix4 Similarity2::AdjointMap() const {
+  throw std::runtime_error("Similarity2::AdjointMap not implemented");
+}
+
+std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
+  os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
+     << p.scale() << "]\';";
+  return os;
+}
+
+Matrix3 Similarity2::matrix() const {
+  Matrix3 T;
+  T.topRows<2>() << R_.matrix(), t_;
+  T.bottomRows<1>() << 0, 0, 1.0 / s_;
+  return T;
+}
+
+}  // namespace gtsam

gtsam/geometry/Similarity2.h

@@ -0,0 +1,200 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   Similarity2.h
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert, Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/dllexport.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot2.h>
+
+namespace gtsam {
+
+// Forward declarations
+class Pose2;
+
+/**
+ * 2D similarity transform
+ */
+class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
+  /// @name Pose Concept
+  /// @{
+  typedef Rot2 Rotation;
+  typedef Point2 Translation;
+  /// @}
+
+ private:
+  Rot2 R_;
+  Point2 t_;
+  double s_;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  Similarity2();
+
+  /// Construct pure scaling
+  Similarity2(double s);
+
+  /// Construct from GTSAM types
+  Similarity2(const Rot2& R, const Point2& t, double s);
+
+  /// Construct from Eigen types
+  Similarity2(const Matrix2& R, const Vector2& t, double s);
+
+  /// Construct from matrix [R t; 0 s^-1]
+  Similarity2(const Matrix3& T);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Compare with tolerance
+  bool equals(const Similarity2& sim, double tol) const;
+
+  /// Exact equality
+  bool operator==(const Similarity2& other) const;
+
+  /// Print with optional string
+  void print(const std::string& s) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Return an identity transform
+  static Similarity2 identity();
+
+  /// Composition
+  Similarity2 operator*(const Similarity2& S) const;
+
+  /// Return the inverse
+  Similarity2 inverse() const;
+
+  /// @}
+  /// @name Group action on Point2
+  /// @{
+
+  /// Action on a point p is s*(R*p+t)
+  Point2 transformFrom(const Point2& p) const;
+
+  /**
+   * Action on a pose T.
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts|
+   * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim2 object.
+   * To retrieve a Pose2, we normalized the scale value into 1.
+   * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
+   * | 0      1/s | = |  0       1    |
+   *
+   * This group action satisfies the compatibility condition.
+   * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
+   */
+  Pose2 transformFrom(const Pose2& T) const;
+
+  /* syntactic sugar for transformFrom */
+  Point2 operator*(const Point2& p) const;
+
+  /**
+   *  Create Similarity2 by aligning at least two point pairs
+   */
+  static Similarity2 Align(const Point2Pairs& abPointPairs);
+
+  /**
+   * Create the Similarity2 object that aligns at least two pose pairs.
+   * Each pair is of the form (aTi, bTi).
+   * Given a list of pairs in frame a, and a list of pairs in frame b,
+   Align()
+   * will compute the best-fit Similarity2 aSb transformation to align them.
+   * First, the rotation aRb will be computed as the average (Karcher mean)
+   of
+   * many estimates aRb (from each pair). Afterwards, the scale factor will
+   be computed
+   * using the algorithm described here:
+   * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+   */
+  static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  /**
+   * Log map at the identity
+   * \f$ [t_x, t_y, \delta, \lambda] \f$
+   */
+  static Vector4 Logmap(const Similarity2& S,  //
+                        OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Exponential map at the identity
+  static Similarity2 Expmap(const Vector4& v,  //
+                            OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Chart at the origin
+  struct ChartAtOrigin {
+    static Similarity2 Retract(const Vector4& v,
+                               ChartJacobian H = boost::none) {
+      return Similarity2::Expmap(v, H);
+    }
+    static Vector4 Local(const Similarity2& other,
+                         ChartJacobian H = boost::none) {
+      return Similarity2::Logmap(other, H);
+    }
+  };
+
+  /// Project from one tangent space to another
+  Matrix4 AdjointMap() const;
+
+  using LieGroup<Similarity2, 4>::inverse;
+
+  /// @}
+  /// @name Standard interface
+  /// @{
+
+  /// Calculate 4*4 matrix group equivalent
+  Matrix3 matrix() const;
+
+  /// Return a GTSAM rotation
+  Rot2 rotation() const { return R_; }
+
+  /// Return a GTSAM translation
+  Point2 translation() const { return t_; }
+
+  /// Return the scale
+  double scale() const { return s_; }
+
+  /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
+  inline static size_t Dim() { return 4; }
+
+  /// Dimensionality of tangent space = 4 DOF
+  inline size_t dim() const { return 4; }
+
+  /// @}
+};
+
+template <>
+struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
+
+template <>
+struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
+
+}  // namespace gtsam

gtsam/geometry/Similarity3.cpp

@@ -26,7 +26,7 @@ namespace gtsam {
 
 using std::vector;
 
-namespace {
+namespace internal {
 /// Subtract centroids from point pairs.
 static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
                                     const Point3Pair &centroids) {
@@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
 static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
                                const Rot3 &aRb) {
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
   return align(d_abPointPairs, aRb, centroids);
 }
-}  // namespace
+}  // namespace internal
 
 Similarity3::Similarity3() :
     t_(0,0,0), s_(1) {
@@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
   if (abPointPairs.size() < 3)
     throw std::runtime_error("input should have at least 3 pairs of points");
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
-  Matrix3 H = calculateH(d_abPointPairs);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  Matrix3 H = internal::calculateH(d_abPointPairs);
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
   Rot3 aRb = Rot3::ClosestTo(H);
-  return align(d_abPointPairs, aRb, centroids);
+  return internal::align(d_abPointPairs, aRb, centroids);
 }
 
 Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
   }
   const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
 
-  return alignGivenR(abPointPairs, aRb_estimate);
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
 }
 
 Matrix4 Similarity3::wedge(const Vector7 &xi) {
@@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
   return os;
 }
 
-const Matrix4 Similarity3::matrix() const {
+Matrix4 Similarity3::matrix() const {
   Matrix4 T;
   T.topRows<3>() << R_.matrix(), t_;
   T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
   return T;
 }
 
-Similarity3::operator Pose3() const {
-  return Pose3(R_, s_ * t_);
-}
-
 } // namespace gtsam

gtsam/geometry/Similarity3.h

@@ -18,13 +18,12 @@
 
 #pragma once
 
-#include <gtsam/geometry/Rot3.h>
-#include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam/dllexport.h>
-
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
@@ -34,108 +33,106 @@ class Pose3;
 /**
  * 3D similarity transform
  */
-class Similarity3: public LieGroup<Similarity3, 7> {
-
+class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// @name Pose Concept
   /// @{
   typedef Rot3 Rotation;
   typedef Point3 Translation;
   /// @}
 
-private:
+ private:
   Rot3 R_;
   Point3 t_;
   double s_;
 
-public:
-
+ public:
   /// @name Constructors
   /// @{
 
   /// Default constructor
-  GTSAM_EXPORT Similarity3();
+  Similarity3();
 
   /// Construct pure scaling
-  GTSAM_EXPORT Similarity3(double s);
+  Similarity3(double s);
 
   /// Construct from GTSAM types
-  GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
+  Similarity3(const Rot3& R, const Point3& t, double s);
 
   /// Construct from Eigen types
-  GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
+  Similarity3(const Matrix3& R, const Vector3& t, double s);
 
   /// Construct from matrix [R t; 0 s^-1]
-  GTSAM_EXPORT Similarity3(const Matrix4& T);
+  Similarity3(const Matrix4& T);
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Compare with tolerance
-  GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
+  bool equals(const Similarity3& sim, double tol) const;
 
   /// Exact equality
-  GTSAM_EXPORT bool operator==(const Similarity3& other) const;
+  bool operator==(const Similarity3& other) const;
 
   /// Print with optional string
-  GTSAM_EXPORT void print(const std::string& s) const;
+  void print(const std::string& s) const;
 
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
+  friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
 
   /// @}
   /// @name Group
   /// @{
 
   /// Return an identity transform
-  GTSAM_EXPORT static Similarity3 identity();
+  static Similarity3 identity();
 
   /// Composition
-  GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
+  Similarity3 operator*(const Similarity3& S) const;
 
   /// Return the inverse
-  GTSAM_EXPORT Similarity3 inverse() const;
+  Similarity3 inverse() const;
 
   /// @}
   /// @name Group action on Point3
   /// @{
 
   /// Action on a point p is s*(R*p+t)
-  GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
-      OptionalJacobian<3, 7> H1 = boost::none, //
-      OptionalJacobian<3, 3> H2 = boost::none) const;
+  Point3 transformFrom(const Point3& p,                          //
+                       OptionalJacobian<3, 7> H1 = boost::none,  //
+                       OptionalJacobian<3, 3> H2 = boost::none) const;
 
-  /** 
+  /**
    * Action on a pose T.
-   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts| 
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts|
    * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim3 object.
    * To retrieve a Pose3, we normalized the scale value into 1.
    * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
    * | 0      1/s | = |  0       1    |
-   * 
-   * This group action satisfies the compatibility condition. 
+   *
+   * This group action satisfies the compatibility condition.
    * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
    */
-  GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
+  Pose3 transformFrom(const Pose3& T) const;
 
   /** syntactic sugar for transformFrom */
-  GTSAM_EXPORT Point3 operator*(const Point3& p) const;
+  Point3 operator*(const Point3& p) const;
 
   /**
    *  Create Similarity3 by aligning at least three point pairs
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
-  
+  static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+
   /**
    * Create the Similarity3 object that aligns at least two pose pairs.
    * Each pair is of the form (aTi, bTi).
    * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
    * will compute the best-fit Similarity3 aSb transformation to align them.
    * First, the rotation aRb will be computed as the average (Karcher mean) of
-   * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
-   * using the algorithm described here:
+   * many estimates aRb (from each pair). Afterwards, the scale factor will be
+   * computed using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+  static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -144,20 +141,22 @@ public:
   /** Log map at the identity
    * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
    */
-  GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
-      OptionalJacobian<7, 7> Hm = boost::none);
+  static Vector7 Logmap(const Similarity3& s,  //
+                        OptionalJacobian<7, 7> Hm = boost::none);
 
   /** Exponential map at the identity
    */
-  GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
-      OptionalJacobian<7, 7> Hm = boost::none);
+  static Similarity3 Expmap(const Vector7& v,  //
+                            OptionalJacobian<7, 7> Hm = boost::none);
 
   /// Chart at the origin
   struct ChartAtOrigin {
-    static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
+    static Similarity3 Retract(const Vector7& v,
+                               ChartJacobian H = boost::none) {
       return Similarity3::Expmap(v, H);
     }
-    static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
+    static Vector7 Local(const Similarity3& other,
+                         ChartJacobian H = boost::none) {
       return Similarity3::Logmap(other, H);
     }
   };
@@ -170,67 +169,53 @@ public:
    * @return 4*4 element of Lie algebra that can be exponentiated
    * TODO(frank): rename to Hat, make part of traits
    */
-  GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
+  static Matrix4 wedge(const Vector7& xi);
 
   /// Project from one tangent space to another
-  GTSAM_EXPORT  Matrix7 AdjointMap() const;
+  Matrix7 AdjointMap() const;
 
   /// @}
   /// @name Standard interface
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT const Matrix4 matrix() const;
+  Matrix4 matrix() const;
 
   /// Return a GTSAM rotation
-  const Rot3& rotation() const {
-    return R_;
-  }
+  Rot3 rotation() const { return R_; }
 
   /// Return a GTSAM translation
-  const Point3& translation() const {
-    return t_;
-  }
+  Point3 translation() const { return t_; }
 
   /// Return the scale
-  double scale() const {
-    return s_;
-  }
-
-  /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
-  GTSAM_EXPORT operator Pose3() const;
+  double scale() const { return s_; }
 
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
-  inline static size_t Dim() {
-    return 7;
-  }
+  inline static size_t Dim() { return 7; }
 
   /// Dimensionality of tangent space = 7 DOF
-  inline size_t dim() const {
-    return 7;
-  }
+  inline size_t dim() const { return 7; }
 
   /// @}
   /// @name Helper functions
   /// @{
 
-private:
+ private:
   /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 
   /// @}
 };
 
-template<>
+template <>
 inline Matrix wedge<Similarity3>(const Vector& xi) {
   return Similarity3::wedge(xi);
 }
 
-template<>
+template <>
 struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
 
-template<>
+template <>
 struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
 
-} // namespace gtsam
+}  // namespace gtsam

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;
@@ -547,6 +580,12 @@ class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
+  // Constructors from Pose3
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                            Eigen::Ref<Eigen::MatrixXd> H);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@@ -867,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,
@@ -890,6 +929,73 @@ class PinholeCamera {
   size_t dim() const;
   static size_t Dim();
 
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+
+  gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpose,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                                  Eigen::Ref<Eigen::MatrixXd> Dcal);
+
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
+
+#include <gtsam/geometry/PinholePose.h>
+template <CALIBRATION>
+class PinholePose {
+  // Standard Constructors and Named Constructors
+  PinholePose();
+  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);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION* K);
+
+  // Testable
+  void print(string s = "PinholePose") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
   // Transformations and measurement functions
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
@@ -915,6 +1021,35 @@ class PinholeCamera {
   void serialize() const;
 };
 
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
+#include <gtsam/geometry/Similarity2.h>
+class Similarity2 {
+  // Standard Constructors
+  Similarity2();
+  Similarity2(double s);
+  Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s);
+  Similarity2(const Matrix& R, const Vector& t, double s);
+  Similarity2(const Matrix& T);
+
+  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
+  gtsam::Pose2 transformFrom(const gtsam::Pose2& T);
+
+  static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs);
+  static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
+
+  // Standard Interface
+  bool equals(const gtsam::Similarity2& sim, double tol) const;
+  Matrix matrix() const;
+  gtsam::Rot2& rotation();
+  gtsam::Point2& translation();
+  double scale() const;
+};
+
 #include <gtsam/geometry/Similarity3.h>
 class Similarity3 {
   // Standard Constructors
@@ -931,22 +1066,13 @@ class Similarity3 {
   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
-  const gtsam::Rot3& rotation();
-  const gtsam::Point3& translation();
+  bool equals(const gtsam::Similarity3& sim, double tol) const;
+  Matrix matrix() const;
+  gtsam::Rot3& rotation();
+  gtsam::Point3& translation();
   double scale() const;
 };
 
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
-
 template <T>
 class CameraSet {
   CameraSet();
@@ -995,58 +1121,154 @@ class StereoCamera {
 };
 
 #include <gtsam/geometry/triangulation.h>
+class TriangulationResult {
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
+  Status status;
+  TriangulationResult(const gtsam::Point3& p);
+  const gtsam::Point3& get() const;
+  static TriangulationResult Degenerate();
+  static TriangulationResult Outlier();
+  static TriangulationResult FarPoint();
+  static TriangulationResult BehindCamera();
+  bool valid() const;
+  bool degenerate() const;
+  bool outlier() const;
+  bool farPoint() const;
+  bool behindCamera() const;
+};
+
+class TriangulationParameters {
+  double rankTolerance;
+  bool enableEPI;
+  double landmarkDistanceThreshold;
+  double dynamicOutlierRejectionThreshold;
+  SharedNoiseModel noiseModel;
+  TriangulationParameters(const double rankTolerance = 1.0,
+                          const bool enableEPI = false,
+                          double landmarkDistanceThreshold = -1,
+                          double dynamicOutlierRejectionThreshold = -1,
+                          const gtsam::SharedNoiseModel& noiseModel = nullptr);
+};
 
 // Templates appear not yet supported for free functions - issue raised at
 // borglab/wrap#14 to add support
+
+// Cal3_S2 versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3_S2* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3DS2* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3Bundler* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3_S2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3_S2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3DS2 versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3DS2* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3DS2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3DS2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Bundler versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Bundler* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Bundler* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Bundler& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Fisheye versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Fisheye* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Fisheye* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Fisheye& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Unified versions                                
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Unified* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
+                                const gtsam::SharedNoiseModel& model = nullptr,
+                                const bool useLOST = false);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3_S2* sharedCal,
+                                   gtsam::Cal3Unified* sharedCal,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3DS2* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3Bundler* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Unified& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>
@@ -1062,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/testSimilarity2.cpp

@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSimilarity2.cpp
+ * @brief  Unit tests for Similarity2 class
+ * @author Varun Agrawal
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Similarity2.h>
+
+#include <functional>
+
+using namespace std::placeholders;
+using namespace gtsam;
+using namespace std;
+
+GTSAM_CONCEPT_TESTABLE_INST(Similarity2)
+
+static const Point2 P(0.2, 0.7);
+static const Rot2 R = Rot2::fromAngle(0.3);
+static const double s = 4;
+
+const double degree = M_PI / 180;
+
+//******************************************************************************
+TEST(Similarity2, Concepts) {
+  BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
+}
+
+//******************************************************************************
+TEST(Similarity2, Constructors) {
+  Similarity2 sim2_Construct1;
+  Similarity2 sim2_Construct2(s);
+  Similarity2 sim2_Construct3(R, P, s);
+  Similarity2 sim2_Construct4(R.matrix(), P, s);
+}
+
+//******************************************************************************
+TEST(Similarity2, Getters) {
+  Similarity2 sim2_default;
+  EXPECT(assert_equal(Rot2(), sim2_default.rotation()));
+  EXPECT(assert_equal(Point2(0, 0), sim2_default.translation()));
+  EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/geometry/tests/testSimilarity3.cpp

@@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
   Values result;
   result = LevenbergMarquardtOptimizer(graph, initial).optimize();
   //result.print("Optimized Estimate\n");
-  Pose3 p1, p2, p3, p4, p5;
-  p1 = Pose3(result.at<Similarity3>(X(1)));
-  p2 = Pose3(result.at<Similarity3>(X(2)));
-  p3 = Pose3(result.at<Similarity3>(X(3)));
-  p4 = Pose3(result.at<Similarity3>(X(4)));
-  p5 = Pose3(result.at<Similarity3>(X(5)));
+  Similarity3 p1, p2, p3, p4, p5;
+  p1 = result.at<Similarity3>(X(1));
+  p2 = result.at<Similarity3>(X(2));
+  p3 = result.at<Similarity3>(X(3));
+  p4 = result.at<Similarity3>(X(4));
+  p5 = result.at<Similarity3>(X(5));
 
-  //p1.print("Pose1");
-  //p2.print("Pose2");
-  //p3.print("Pose3");
-  //p4.print("Pose4");
-  //p5.print("Pose5");
+  //p1.print("Similarity1");
+  //p2.print("Similarity2");
+  //p3.print("Similarity3");
+  //p4.print("Similarity4");
+  //p5.print("Similarity5");
 
   Similarity3 expected(0.7);
   EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));

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));
-PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
+static const PinholeCamera<Cal3_S2> kCamera1(kPose1, *kSharedCal);
 
 // create second camera 1 meter to the right of first camera
-static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
-PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);
+static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
+static const PinholeCamera<Cal3_S2> kCamera2(kPose2, *kSharedCal);
 
 // landmark ~5 meters infront of camera
-static const Point3 landmark(5, 0.5, 1.2);
+static const Point3 kLandmark(5, 0.5, 1.2);
 
 // 1. Project two landmarks into two cameras and triangulate
-Point2 z1 = camera1.project(landmark);
-Point2 z2 = camera2.project(landmark);
+static const Point2 kZ1 = kCamera1.project(kLandmark);
+static const Point2 kZ2 = kCamera2.project(kLandmark);
 
 //******************************************************************************
 // Simple test with a well-behaved two camera situation
@@ -63,22 +64,22 @@ TEST(triangulation, twoPoses) {
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
-  measurements += z1, z2;
+  poses += kPose1, kPose2;
+  measurements += kZ1, kZ2;
 
   double rank_tol = 1e-9;
 
   // 1. Test simple DLT, perfect in no noise situation
   bool optimize = false;
   boost::optional<Point3> actual1 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual1, 1e-7));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
 
   // 2. test with optimization on, same answer
   optimize = true;
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
-  EXPECT(assert_equal(landmark, *actual2, 1e-7));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
 
   // 3. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -86,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 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z1Distorted = camera1Distorted.project(kLandmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -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 z2Distorted = camera2Distorted.project(landmark);
+  Point2 z1Distorted = camera1Distorted.project(kLandmark);
+  Point2 z2Distorted = camera2Distorted.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1Distorted, z2Distorted;
 
   double rank_tol = 1e-9;
@@ -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> camera2(pose2, *bundlerCal);
+  PinholeCamera<Cal3Bundler> camera1(kPose1, *bundlerCal);
+  PinholeCamera<Cal3Bundler> camera2(kPose2, *bundlerCal);
 
   // 1. Project two landmarks into two cameras and triangulate
-  Point2 z1 = camera1.project(landmark);
-  Point2 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  Point2 z2 = camera2.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += z1, z2;
 
   bool optimize = true;
@@ -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;
-  measurements += z1, z2;
+  poses += kPose1, kPose2;
+  measurements += kZ1, kZ2;
 
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // 2. Add some noise and try again: result should be ~ (4.995,
   // 0.499167, 1.19814)
@@ -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);
-  EXPECT(assert_equal(landmark, *actual2, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
 
   // 3. Add a slightly rotated third camera above, again with measurement noise
-  Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   poses += pose3;
   measurements += z3 + Point2(0.1, -0.1);
 
   boost::optional<Point3> triangulated_3cameras =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
 
   // Again with nonlinear optimization
   boost::optional<Point3> triangulated_3cameras_opt =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
-  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
+  EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
 
   // 4. Test failure: Add a 4th camera facing the wrong way
   Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
-  PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal);
+  PinholeCamera<Cal3_S2> camera4(pose4, *kSharedCal);
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-  CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
+  CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
 
   poses += pose4;
   measurements += Point2(400, 400);
 
-  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
+  CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
                   TriangulationCheiralityException);
 #endif
 }
@@ -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));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose2, pose3;
-  measurements += z1, z2, z3;
+  poses += kPose1, kPose2, pose3;
+  measurements += kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // Add outlier
   measurements.at(0) += Point2(100, 120); // very large pixel noise!
 
   // now estimate does not match landmark
   boost::optional<Point3> actual2 =  //
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
   // DLT is surprisingly robust, but still off (actual error is around 0.26m):
-  EXPECT( (landmark - *actual2).norm() >= 0.2);
-  EXPECT( (landmark - *actual2).norm() <= 0.5);
+  EXPECT( (kLandmark - *actual2).norm() >= 0.2);
+  EXPECT( (kLandmark - *actual2).norm() <= 0.5);
 
   // Again with nonlinear optimization
   boost::optional<Point3> actual3 =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
   // result from nonlinear (but non-robust optimization) is close to DLT and still off
   EXPECT(assert_equal(*actual2, *actual3, 0.1));
 
@@ -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));
-  PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal);
-  Point2 z3 = camera3.project(landmark);
+  Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
+  PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
+  Point2 z3 = camera3.project(kLandmark);
 
   vector<Pose3> poses;
   Point2Vector measurements;
-  poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1
-  measurements += z1, z1, z2, z3;
+  poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
+  measurements += kZ1, kZ1, kZ2, z3;
 
   // noise free, so should give exactly the landmark
   boost::optional<Point3> actual =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
-  EXPECT(assert_equal(landmark, *actual, 1e-2));
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
+  EXPECT(assert_equal(kLandmark, *actual, 1e-2));
 
   // Add outlier
   measurements.at(0) += Point2(100, 120); // very large pixel noise!
@@ -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( (landmark - *actual2).norm() <= 0.5);
+  EXPECT( (kLandmark - *actual2).norm() >= 0.1);
+  EXPECT( (kLandmark - *actual2).norm() <= 0.5);
 
   // Again with nonlinear optimization
   boost::optional<Point3> actual3 =
-      triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
+      triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
   // result from nonlinear (but non-robust optimization) is close to DLT and still off
   EXPECT(assert_equal(*actual2, *actual3, 0.1));
 
@@ -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 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  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 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  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 z2 = camera2.project(landmark);
+  Point2 z1 = camera1.project(kLandmark);
+  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 cam2(pose2);
-  Unit3 u1 = cam1.project(landmark);
-  Unit3 u2 = cam2.project(landmark);
+  SphericalCamera cam1(kPose1);
+  SphericalCamera cam2(kPose2);
+  Unit3 u1 = cam1.project(kLandmark);
+  Unit3 u2 = cam2.project(kLandmark);
 
-  poses += pose1, pose2;
+  poses += kPose1, kPose2;
   measurements += u1, u2;
 
   CameraSet<SphericalCamera> cameras;
@@ -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)
-    throw(TriangulationUnderconstrainedException());
+  if (rank < 3) 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)
-    throw(TriangulationUnderconstrainedException());
+  if (rank < 3) throw(TriangulationUnderconstrainedException());
 
   return v;
 }
 
-Point3 triangulateDLT(
-    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
-    const Point2Vector& measurements, double rank_tol) {
+Point3 triangulateLOST(const std::vector<Pose3>& poses,
+                       const Point3Vector& calibratedMeasurements,
+                       const SharedIsotropic& measurementNoise) {
+  size_t m = calibratedMeasurements.size();
+  assert(m == poses.size());
 
-  Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
-                                        rank_tol);
+  // Construct the system matrices.
+  Matrix A = Matrix::Zero(m * 2, 3);
+  Matrix b = Matrix::Zero(m * 2, 1);
+
+  for (size_t i = 0; i < m; i++) {
+    const Pose3& wTi = poses[i];
+    // TODO(akshay-krishnan): are there better ways to select j?
+    const int j = (i + 1) % m;
+    const Pose3& wTj = poses[j];
+
+    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,
-                                         rank_tol);
-   // Create 3D point from homogeneous coordinates
-   return Point3(v.head<3>() / v[3]);
+  Vector4 v =
+      triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
+  // Create 3D point from homogeneous coordinates
+  return Point3(v.head<3>() / v[3]);
 }
 
-///
 /**
  * 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
@@ -23,6 +24,7 @@
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -93,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
@@ -107,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);
 }
@@ -301,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();
-  assert(num_meas == measurements.size());
-  typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
-  for (size_t ii = 0; ii < num_meas; ++ii) {
+  const size_t nrMeasurements = measurements.size();
+  assert(nrMeasurements == cameras.size());
+  typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
+  for (size_t ii = 0; ii < nrMeasurements; ++ii) {
     // Calibrate with cal and uncalibrate with pinhole version of cal so that
     // measurements are undistorted.
     undistortedMeasurements[ii] =
@@ -330,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
@@ -340,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,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
-
+                         boost::shared_ptr<CALIBRATION> sharedCal,
+                         const Point2Vector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool useLOST = false) {
   assert(poses.size() == measurements.size());
-  if (poses.size() < 2)
-    throw(TriangulationUnderconstrainedException());
-
-  // construct projection matrices from poses & calibration
-  auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
-
-  // Undistort the measurements, leaving only the pinhole elements in effect.
-  auto undistortedMeasurements =
-      undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
+  if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
 
   // Triangulate linearly
-  Point3 point =
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  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);
+    // 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)
-      throw(TriangulationCheiralityException());
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -391,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>
-Point3 triangulatePoint3(
-    const CameraSet<CAMERA>& cameras,
-    const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
-
+template <class CAMERA>
+Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
+                         const typename CAMERA::MeasurementVector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool useLOST = false) {
   size_t m = cameras.size();
   assert(measurements.size() == m);
 
-  if (m < 2)
-    throw(TriangulationUnderconstrainedException());
+  if (m < 2) throw(TriangulationUnderconstrainedException());
 
-  // construct projection matrices from poses & calibration
-  auto projection_matrices = projectionMatricesFromCameras(cameras);
+  // Triangulate linearly
+  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.
-  auto undistortedMeasurements =
-      undistortMeasurements<CAMERA>(cameras, measurements);
+    // construct poses from cameras.
+    std::vector<Pose3> poses;
+    poses.reserve(cameras.size());
+    for (const auto& camera : cameras) poses.push_back(camera.pose());
 
-  Point3 point =
-      triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+    // calibrate the measurements to obtain homogenous coordinates in image
+    // plane.
+    auto calibratedMeasurements =
+        calibrateMeasurements<CAMERA>(cameras, measurements);
 
-  // The n refine using non-linear optimization
-  if (optimize)
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+  } else {
+    // construct projection matrices from poses & calibration
+    auto projection_matrices = projectionMatricesFromCameras(cameras);
+
+    // Undistort the measurements, leaving only the pinhole elements in effect.
+    auto undistortedMeasurements =
+        undistortMeasurements<CAMERA>(cameras, measurements);
+
+    point =
+        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
+  }
+
+  // Then refine using non-linear optimization
+  if (optimize) {
     point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
+  }
 
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
   // verify that the triangulated point lies in front of all cameras
-  for(const CAMERA& camera: cameras) {
+  for (const CAMERA& camera : cameras) {
     const Point3& p_local = camera.pose().transformTo(point);
-    if (p_local.z() <= 0)
-      throw(TriangulationCheiralityException());
+    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
   }
 #endif
 
@@ -433,14 +543,14 @@ Point3 triangulatePoint3(
 }
 
 /// Pinhole-specific version
-template<class CALIBRATION>
-Point3 triangulatePoint3(
-    const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
-    const Point2Vector& measurements, double rank_tol = 1e-9,
-    bool optimize = false,
-    const SharedNoiseModel& model = nullptr) {
-  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
-  (cameras, measurements, rank_tol, optimize, model);
+template <class CALIBRATION>
+Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
+                         const Point2Vector& measurements,
+                         double rank_tol = 1e-9, bool optimize = false,
+                         const SharedNoiseModel& model = nullptr,
+                         const bool useLOST = false) {
+  return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
+      (cameras, measurements, rank_tol, optimize, model, useLOST);
 }
 
 struct GTSAM_EXPORT TriangulationParameters {
@@ -510,18 +620,18 @@ private:
 };
 
 /**
- * TriangulationResult is an optional point, along with the reasons why it is invalid.
+ * TriangulationResult is an optional point, along with the reasons why it is
+ * invalid.
  */
-class TriangulationResult: public boost::optional<Point3> {
-  enum Status {
-    VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
-  };
-  Status status_;
-  TriangulationResult(Status s) :
-      status_(s) {
-  }
-public:
+class TriangulationResult : public boost::optional<Point3> {
+ public:
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
+  Status status;
 
+ private:
+  TriangulationResult(Status s) : status(s) {}
+
+ public:
   /**
    * Default constructor, only for serialization
    */
@@ -530,54 +640,38 @@ public:
   /**
    * Constructor
    */
-  TriangulationResult(const Point3& p) :
-      status_(VALID) {
-    reset(p);
-  }
+  TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
   static TriangulationResult Degenerate() {
     return TriangulationResult(DEGENERATE);
   }
-  static TriangulationResult Outlier() {
-    return TriangulationResult(OUTLIER);
-  }
+  static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
   static TriangulationResult FarPoint() {
     return TriangulationResult(FAR_POINT);
   }
   static TriangulationResult BehindCamera() {
     return TriangulationResult(BEHIND_CAMERA);
   }
-  bool valid() const {
-    return status_ == VALID;
-  }
-  bool degenerate() const {
-    return status_ == DEGENERATE;
-  }
-  bool outlier() const {
-    return status_ == OUTLIER;
-  }
-  bool farPoint() const {
-    return status_ == FAR_POINT;
-  }
-  bool behindCamera() const {
-    return status_ == BEHIND_CAMERA;
-  }
+  bool valid() const { return status == VALID; }
+  bool degenerate() const { return status == DEGENERATE; }
+  bool outlier() const { return status == OUTLIER; }
+  bool farPoint() const { return status == FAR_POINT; }
+  bool behindCamera() const { return status == BEHIND_CAMERA; }
   // stream to output
-  friend std::ostream &operator<<(std::ostream &os,
-      const TriangulationResult& result) {
+  friend std::ostream& operator<<(std::ostream& os,
+                                  const TriangulationResult& result) {
     if (result)
       os << "point = " << *result << std::endl;
     else
-      os << "no point, status = " << result.status_ << std::endl;
+      os << "no point, status = " << result.status << std::endl;
     return os;
   }
 
-private:
-
+ private:
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(status_);
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int version) {
+    ar& BOOST_SERIALIZATION_NVP(status);
   }
 };
 
@@ -644,6 +738,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;

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/BayesNet-inst.h

@@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
     auto frontals = conditional->frontals();
     const Key me = frontals.front();
     auto parents = conditional->parents();
-    for (const Key& p : parents)
-      os << "  var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n";
+    for (const Key& p : parents) {
+      os << "  var" << p << "->var" << me << "\n";
+    }
   }
 
   os << "}";

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

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

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/LossFunctions.h

@@ -54,23 +54,31 @@ namespace noiseModel {
 // clang-format on
 namespace mEstimator {
 
-//---------------------------------------------------------------------------------------
-
+/**
+ * Pure virtual class for all robust error function classes.
+ *
+ * It provides the machinery for block vs scalar reweighting strategies, in
+ * addition to defining the interface of derived classes.
+ */
 class GTSAM_EXPORT Base {
  public:
+  /** the rows can be weighted independently according to the error
+   * or uniformly with the norm of the right hand side */
   enum ReweightScheme { Scalar, Block };
   typedef boost::shared_ptr<Base> shared_ptr;
 
  protected:
-  /** the rows can be weighted independently according to the error
-   * or uniformly with the norm of the right hand side */
+  /// Strategy for reweighting \sa ReweightScheme
   ReweightScheme reweight_;
 
  public:
   Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
   virtual ~Base() {}
 
-  /*
+  /// Returns the reweight scheme, as explained in ReweightScheme
+  ReweightScheme reweightScheme() const { return reweight_; }
+
+  /**
    * This method is responsible for returning the total penalty for a given
    * amount of error. For example, this method is responsible for implementing
    * the quadratic function for an L2 penalty, the absolute value function for
@@ -80,16 +88,20 @@ class GTSAM_EXPORT Base {
    * error vector, then it prevents implementations of asymmeric loss
    * functions. It would be better for this function to accept the vector and
    * internally call the norm if necessary.
+   *
+   * This returns \rho(x) in \ref mEstimator
    */
-  virtual double loss(double distance) const { return 0; };
+  virtual double loss(double distance) const { return 0; }
 
-  /*
+  /**
    * This method is responsible for returning the weight function for a given
    * amount of error. The weight function is related to the analytic derivative
    * of the loss function. See
    *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
    * for details. This method is required when optimizing cost functions with
    * robust penalties using iteratively re-weighted least squares.
+   *
+   * This returns w(x) in \ref mEstimator
    */
   virtual double weight(double distance) const = 0;
 
@@ -124,7 +136,15 @@ class GTSAM_EXPORT Base {
   }
 };
 
-/// Null class should behave as Gaussian
+/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or
+ *  plain least-squares (non-robust).
+ *
+ *  This model has no additional parameters.
+ *
+ * - Loss       \rho(x)          = 0.5 x²
+ * - Derivative \phi(x)          = x
+ * - Weight     w(x) = \phi(x)/x = 1
+ */
 class GTSAM_EXPORT Null : public Base {
  public:
   typedef boost::shared_ptr<Null> shared_ptr;
@@ -146,7 +166,14 @@ class GTSAM_EXPORT Null : public Base {
   }
 };
 
-/// Fair implements the "Fair" robust error model (Zhang97ivc)
+/** Implementation of the "Fair" robust error model (Zhang97ivc)
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (|x|/c - log(1+|x|/c))
+ * - Derivative \phi(x) = x/(1+|x|/c)
+ * - Weight     w(x) = \phi(x)/x = 1/(1+|x|/c)
+ */
 class GTSAM_EXPORT Fair : public Base {
  protected:
   double c_;
@@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double c, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base {
   }
 };
 
-/// Huber implements the "Huber" robust error model (Zhang97ivc)
+/** The "Huber" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x)          = 0.5 x²  if |x|<k, 0.5 k² + k|x-k|  otherwise
+ * - Derivative \phi(x)          = x       if |x|<k, k sgn(x)         otherwise
+ * - Weight     w(x) = \phi(x)/x = 1       if |x|<k, k/|x|            otherwise
+ */
 class GTSAM_EXPORT Huber : public Base {
  protected:
   double k_;
@@ -185,6 +220,7 @@ class GTSAM_EXPORT Huber : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -196,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
   }
 };
 
-/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
-/// by:
-///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
-///   Information Technology, Karlsruhe, Germany.
-///   oberlaender@fzi.de
-/// Thanks Jan!
+/** Implementation of the "Cauchy" robust error model (Lee2013IROS).
+ * Contributed by:
+ *  Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
+ *  Information Technology, Karlsruhe, Germany.
+ *  oberlaender@fzi.de
+ *  Thanks Jan!
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0.5 k² log(1+x²/k²)
+ * - Derivative \phi(x) = (k²x)/(x²+k²)
+ * - Weight     w(x) = \phi(x)/x = k²/(x²+k²)
+ */
 class GTSAM_EXPORT Cauchy : public Base {
  protected:
   double k_, ksquared_;
@@ -215,6 +258,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -227,7 +271,14 @@ class GTSAM_EXPORT Cauchy : public Base {
   }
 };
 
-/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
+/** Implementation of the "Tukey" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (1 - (1-x²/c²)³)/6  if |x|<c,  c²/6   otherwise
+ * - Derivative \phi(x) = x(1-x²/c²)² if |x|<c,  0   otherwise
+ * - Weight     w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c,  0   otherwise
+ */
 class GTSAM_EXPORT Tukey : public Base {
  protected:
   double c_, csquared_;
@@ -241,6 +292,7 @@ class GTSAM_EXPORT Tukey : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -252,7 +304,14 @@ class GTSAM_EXPORT Tukey : public Base {
   }
 };
 
-/// Welsch implements the "Welsch" robust error model (Zhang97ivc)
+/** Implementation of the "Welsch" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
+ * - Derivative \phi(x) = x exp(-x²/c²)
+ * - Weight     w(x) = \phi(x)/x = exp(-x²/c²)
+ */
 class GTSAM_EXPORT Welsch : public Base {
  protected:
   double c_, csquared_;
@@ -266,6 +325,7 @@ class GTSAM_EXPORT Welsch : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -278,12 +338,16 @@ class GTSAM_EXPORT Welsch : public Base {
   }
 };
 
-/// GemanMcClure implements the "Geman-McClure" robust error model
-/// (Zhang97ivc).
-///
-/// Note that Geman-McClure weight function uses the parameter c == 1.0,
-/// but here it's allowed to use different values, so we actually have
-/// the generalized Geman-McClure from (Agarwal15phd).
+/** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
+ *
+ * Note that Geman-McClure weight function uses the parameter c == 1.0,
+ * but here it's allowed to use different values, so we actually have
+ * the generalized Geman-McClure from (Agarwal15phd).
+ *
+ * - Loss       \rho(x) = 0.5 (c²x²)/(c²+x²)
+ * - Derivative \phi(x) = xc⁴/(c²+x²)²
+ * - Weight     w(x) = \phi(x)/x = c⁴/(c²+x²)²
+ */
 class GTSAM_EXPORT GemanMcClure : public Base {
  public:
   typedef boost::shared_ptr<GemanMcClure> shared_ptr;
@@ -295,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -309,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   }
 };
 
-/// DCS implements the Dynamic Covariance Scaling robust error model
-/// from the paper Robust Map Optimization (Agarwal13icra).
-///
-/// Under the special condition of the parameter c == 1.0 and not
-/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+/** DCS implements the Dynamic Covariance Scaling robust error model
+ *  from the paper Robust Map Optimization (Agarwal13icra).
+ *
+ *  Under the special condition of the parameter c == 1.0 and not
+ *  forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+ *
+ *  This model has a scalar parameter "c" (with "units" of squared error).
+ *
+ * - Loss       \rho(x) = (c²x² + cx⁴)/(x²+c)²   (for any "x")
+ * - Derivative \phi(x) = 2c²x/(x²+c)²
+ * - Weight     w(x) = \phi(x)/x = 2c²/(x²+c)²  if x²>c,   1  otherwise
+ */
 class GTSAM_EXPORT DCS : public Base {
  public:
   typedef boost::shared_ptr<DCS> shared_ptr;
@@ -325,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -339,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
   }
 };
 
-/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
-/// width 2*k, centered at the origin. The resulting penalty within the dead
-/// zone is always zero, and grows quadratically outside the dead zone. In this
-/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
-/// robust to outliers. This penalty can be used to create barrier functions in
-/// a general way.
+/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
+ *  width 2*k, centered at the origin. The resulting penalty within the dead
+ *  zone is always zero, and grows quadratically outside the dead zone. In this
+ *  sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
+ *  robust to outliers. This penalty can be used to create barrier functions in
+ *  a general way.
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0 if |x|<k,    0.5(k-|x|)² otherwise
+ * - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k,  (k+x) if x<-k
+ * - Weight     w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k,  (k+x)/x if x<-k
+ */
 class GTSAM_EXPORT L2WithDeadZone : public Base {
  protected:
   double k_;
@@ -358,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */

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/SubgraphBuilder.cpp

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

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 =
@@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
   DummyPreconditionerParameters();
 };
 
+virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
+  BlockJacobiPreconditionerParameters();
+};
+
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();

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/navigation/AHRSFactor.h

@@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
   /**
    * Add a single Gyroscope measurement to the preintegration.
-   * @param measureOmedga Measured angular velocity (in body frame)
+   * Measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegratedRotationParams` (if provided).
+   *
+   * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param deltaT Time step
    */
   void integrateMeasurement(const Vector3& measuredOmega, double deltaT);

gtsam/navigation/CombinedImuFactor.h

@@ -208,8 +208,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the
-   * sensor)
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between two consecutive IMU measurements
    */

gtsam/navigation/ImuFactor.h

@@ -121,7 +121,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between this and the last IMU measurement
    */

gtsam/navigation/tests/testGPSFactor.cpp

@@ -27,7 +27,6 @@
 #include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
+  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
       std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
@@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,NavState>(
+  Matrix expectedH = numericalDerivative11<Vector, NavState>(
       std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative

gtsam/navigation/tests/testMagFactor.cpp

@@ -26,7 +26,6 @@
 
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) {
   Matrix H;
   Point3 expected(22735.5, 314.502, 44202.5);
   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
-  EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
+  EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
       (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
 }
 
@@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) {
 
   // MagFactor
   MagFactor f(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
-  EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
+  EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
+  EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
       (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
 
-// MagFactor1
+  // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
+  EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
+  EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
       (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
 
-// MagFactor2
+  // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
-  EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
+  EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
       (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
       H1, 1e-7));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
       (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
       H2, 1e-7));
 
-// MagFactor2
+  // MagFactor3
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //

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)
-      std::cout << "checkCostConvergence = true " << std::endl;
+    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
+      std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
+                << ", curr. cost = " << cost << ")" << std::endl;
+    }
     return costConverged;
   }
 
@@ -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/nonlinear/Marginals.h

@@ -121,7 +121,7 @@ public:
 
   /** Optimize the bayes tree */
   VectorValues optimize() const;
-            
+
 protected:
   
   /** Compute the Bayes Tree as a helper function to the constructor */

gtsam/nonlinear/PriorFactor.h

@@ -94,7 +94,6 @@ namespace gtsam {
     Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
-      // TODO(ASL) Add Jacobians.
       return -traits<T>::Local(x, prior_);
     }
 

gtsam/nonlinear/nonlinear.i

@@ -226,6 +226,10 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -269,6 +273,10 @@ class Values {
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -310,6 +318,10 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, Vector vector);
@@ -351,6 +363,10 @@ class Values {
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                 gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,
@@ -468,6 +484,9 @@ virtual class NonlinearOptimizerParams {
   bool isSequential() const;
   bool isCholmod() const;
   bool isIterative() const;
+
+  // This only applies to python since matlab does not have lambda machinery.
+  gtsam::NonlinearOptimizerParams::IterationHook iterationHook;
 };
 
 bool checkConvergence(double relativeErrorTreshold,

gtsam/sam/BearingRangeFactor.h

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

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/SfmData.h

@@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
   size_t numberCameras() const { return cameras.size(); }
 
   /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const { return tracks[idx]; }
+  const SfmTrack& track(size_t idx) const { return tracks[idx]; }
 
   /// The camera pose at frame index `idx`
-  SfmCamera camera(size_t idx) const { return cameras[idx]; }
+  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
+
+  /// Getters
+  const std::vector<SfmCamera>& cameraList() const { return cameras; }
+  const std::vector<SfmTrack>& trackList() const { return tracks; }
 
   /**
    * @brief Create projection factors using keys i and P(j)

gtsam/sfm/TranslationRecovery.cpp

@@ -21,13 +21,16 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/TranslationFactor.h>
 #include <gtsam/sfm/TranslationRecovery.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/expressions.h>
 
 #include <set>
 #include <utility>
@@ -38,16 +41,13 @@ using namespace std;
 // In Wrappers we have no access to this so have a default ready.
 static std::mt19937 kRandomNumberGenerator(42);
 
-TranslationRecovery::TranslationRecovery(
-    const TranslationRecovery::TranslationEdges &relativeTranslations,
-    const LevenbergMarquardtParams &lmParams)
-    : params_(lmParams) {
-  // Some relative translations may be zero. We treat nodes that have a zero
-  // relativeTranslation as a single node.
-
-  // A DSFMap is used to find sets of nodes that have a zero relative
-  // translation. Add the nodes in each edge to the DSFMap, and merge nodes that
-  // are connected by a zero relative translation.
+// Some relative translations may be zero. We treat nodes that have a zero
+// relativeTranslation as a single node.
+// A DSFMap is used to find sets of nodes that have a zero relative
+// translation. Add the nodes in each edge to the DSFMap, and merge nodes that
+// are connected by a zero relative translation.
+DSFMap<Key> getSameTranslationDSFMap(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) {
   DSFMap<Key> sameTranslationDSF;
   for (const auto &edge : relativeTranslations) {
     Key key1 = sameTranslationDSF.find(edge.key1());
@@ -56,94 +56,161 @@ TranslationRecovery::TranslationRecovery(
       sameTranslationDSF.merge(key1, key2);
     }
   }
-  // Use only those edges for which two keys have a distinct root in the DSFMap.
-  for (const auto &edge : relativeTranslations) {
-    Key key1 = sameTranslationDSF.find(edge.key1());
-    Key key2 = sameTranslationDSF.find(edge.key2());
-    if (key1 == key2) continue;
-    relativeTranslations_.emplace_back(key1, key2, edge.measured(),
-                                       edge.noiseModel());
-  }
-  // Store the DSF map for post-processing results.
-  sameTranslationNodes_ = sameTranslationDSF.sets();
+  return sameTranslationDSF;
 }
 
-NonlinearFactorGraph TranslationRecovery::buildGraph() const {
+// Removes zero-translation edges from measurements, and combines the nodes in
+// these edges into a single node.
+template <typename T>
+std::vector<BinaryMeasurement<T>> removeSameTranslationNodes(
+    const std::vector<BinaryMeasurement<T>> &edges,
+    const DSFMap<Key> &sameTranslationDSFMap) {
+  std::vector<BinaryMeasurement<T>> newEdges;
+  for (const auto &edge : edges) {
+    Key key1 = sameTranslationDSFMap.find(edge.key1());
+    Key key2 = sameTranslationDSFMap.find(edge.key2());
+    if (key1 == key2) continue;
+    newEdges.emplace_back(key1, key2, edge.measured(), edge.noiseModel());
+  }
+  return newEdges;
+}
+
+// Adds nodes that were not optimized for because they were connected
+// to another node with a zero-translation edge in the input.
+Values addSameTranslationNodes(const Values &result,
+                               const DSFMap<Key> &sameTranslationDSFMap) {
+  Values final_result = result;
+  // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
+  // from a key that was optimized to keys that were not optimized. Iterate over
+  // map and add results for keys not optimized.
+  for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
+    Key optimizedKey = optimizedAndDuplicateKeys.first;
+    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
+    // Add the result for the duplicate key if it does not already exist.
+    for (const Key duplicateKey : duplicateKeys) {
+      if (final_result.exists(duplicateKey)) continue;
+      final_result.insert<Point3>(duplicateKey,
+                                  final_result.at<Point3>(optimizedKey));
+    }
+  }
+  return final_result;
+}
+
+NonlinearFactorGraph TranslationRecovery::buildGraph(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const {
   NonlinearFactorGraph graph;
 
-  // Add all relative translation edges
-  for (auto edge : relativeTranslations_) {
+  // Add translation factors for input translation directions.
+  for (auto edge : relativeTranslations) {
     graph.emplace_shared<TranslationFactor>(edge.key1(), edge.key2(),
                                             edge.measured(), edge.noiseModel());
   }
-
   return graph;
 }
 
 void TranslationRecovery::addPrior(
-    const double scale, NonlinearFactorGraph *graph,
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const double scale,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    NonlinearFactorGraph *graph,
     const SharedNoiseModel &priorNoiseModel) const {
-  auto edge = relativeTranslations_.begin();
-  if (edge == relativeTranslations_.end()) return;
-  graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
-                                              priorNoiseModel);
-  graph->emplace_shared<PriorFactor<Point3> >(
-      edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
+  auto edge = relativeTranslations.begin();
+  if (edge == relativeTranslations.end()) return;
+  graph->emplace_shared<PriorFactor<Point3>>(edge->key1(), Point3(0, 0, 0),
+                                             priorNoiseModel);
+
+  // Add a scale prior only if no other between factors were added.
+  if (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(std::mt19937 *rng) const {
+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
   // initializes if not.
   Values initial;
   auto insert = [&](Key j) {
-    if (!initial.exists(j)) {
+    if (initial.exists(j)) return;
+    if (initialValues.exists(j)) {
+      initial.insert<Point3>(j, initialValues.at<Point3>(j));
+    } else {
       initial.insert<Point3>(
           j, Point3(randomVal(*rng), randomVal(*rng), randomVal(*rng)));
     }
+    // Assumes all nodes connected by zero-edges have the same initialization.
   };
 
   // Loop over measurements and add a random translation
-  for (auto edge : relativeTranslations_) {
+  for (auto edge : relativeTranslations) {
     insert(edge.key1());
     insert(edge.key2());
   }
-
-  // If there are no valid edges, but zero-distance edges exist, initialize one
-  // of the nodes in a connected component of zero-distance edges.
-  if (initial.empty() && !sameTranslationNodes_.empty()) {
-    for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-      Key optimizedKey = optimizedAndDuplicateKeys.first;
-      initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
-    }
+  // 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 {
-  return initializeRandomly(&kRandomNumberGenerator);
+Values TranslationRecovery::initializeRandomly(
+    const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const Values &initialValues) const {
+  return initializeRandomly(relativeTranslations, betweenTranslations,
+                            &kRandomNumberGenerator, initialValues);
 }
 
-Values TranslationRecovery::run(const double scale) const {
-  auto graph = buildGraph();
-  addPrior(scale, &graph);
-  const Values initial = initializeRandomly();
-  LevenbergMarquardtOptimizer lm(graph, initial, params_);
-  Values result = lm.optimize();
+Values TranslationRecovery::run(
+    const TranslationEdges &relativeTranslations, const double scale,
+    const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+    const Values &initialValues) const {
+  // Find edges that have a zero-translation, and recompute relativeTranslations
+  // and betweenTranslations by retaining only one node for every zero-edge.
+  DSFMap<Key> sameTranslationDSFMap =
+      getSameTranslationDSFMap(relativeTranslations);
+  const TranslationEdges nonzeroRelativeTranslations =
+      removeSameTranslationNodes(relativeTranslations, sameTranslationDSFMap);
+  const std::vector<BinaryMeasurement<Point3>> nonzeroBetweenTranslations =
+      removeSameTranslationNodes(betweenTranslations, sameTranslationDSFMap);
 
-  // Nodes that were not optimized are stored in sameTranslationNodes_ as a map
-  // from a key that was optimized to keys that were not optimized. Iterate over
-  // map and add results for keys not optimized.
-  for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
-    Key optimizedKey = optimizedAndDuplicateKeys.first;
-    std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
-    // Add the result for the duplicate key if it does not already exist.
-    for (const Key duplicateKey : duplicateKeys) {
-      if (result.exists(duplicateKey)) continue;
-      result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
+  // Create graph of translation factors.
+  NonlinearFactorGraph graph = buildGraph(nonzeroRelativeTranslations);
+
+  // Add global frame prior and scale (either from betweenTranslations or
+  // scale).
+  addPrior(nonzeroRelativeTranslations, scale, nonzeroBetweenTranslations,
+           &graph);
+
+  // Uses initial values from params if provided.
+  Values initial = initializeRandomly(
+      nonzeroRelativeTranslations, 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.
+  if (initial.empty() && !sameTranslationDSFMap.sets().empty()) {
+    for (const auto &optimizedAndDuplicateKeys : sameTranslationDSFMap.sets()) {
+      Key optimizedKey = optimizedAndDuplicateKeys.first;
+      initial.insert<Point3>(optimizedKey, Point3(0, 0, 0));
     }
   }
-  return result;
+
+  LevenbergMarquardtOptimizer lm(graph, initial, lmParams_);
+  Values result = lm.optimize();
+  return addSameTranslationNodes(result, sameTranslationDSFMap);
 }
 
 TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements(

gtsam/sfm/TranslationRecovery.h

@@ -11,7 +11,7 @@
 
 /**
  * @file TranslationRecovery.h
- * @author Frank Dellaert
+ * @author Frank Dellaert, Akshay Krishnan
  * @date March 2020
  * @brief Recovering translations in an epipolar graph when rotations are given.
  */
@@ -57,68 +57,109 @@ class TranslationRecovery {
   // Translation directions between camera pairs.
   TranslationEdges relativeTranslations_;
 
-  // Parameters used by the LM Optimizer.
-  LevenbergMarquardtParams params_;
-
-  // Map from a key in the graph to a set of keys that share the same
-  // translation.
-  std::map<Key, std::set<Key>> sameTranslationNodes_;
+  // Parameters.
+  LevenbergMarquardtParams lmParams_;
 
  public:
   /**
    * @brief Construct a new Translation Recovery object
    *
-   * @param relativeTranslations the relative translations, in world coordinate
-   * frames, vector of BinaryMeasurements of Unit3, where each key of a
-   * measurement is a point in 3D.
-   * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
-   * used to modify the parameters for the LM optimizer. By default, uses the
-   * default LM parameters.
+   * @param lmParams parameters for optimization.
    */
-  TranslationRecovery(
-      const TranslationEdges &relativeTranslations,
-      const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
+  TranslationRecovery(const LevenbergMarquardtParams &lmParams)
+      : lmParams_(lmParams) {}
+
+  /**
+   * @brief Default constructor.
+   */
+  TranslationRecovery() = default;
 
   /**
    * @brief Build the factor graph to do the optimization.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @return NonlinearFactorGraph
    */
-  NonlinearFactorGraph buildGraph() const;
+  NonlinearFactorGraph buildGraph(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
 
   /**
-   * @brief Add priors on ednpoints of first measurement edge.
+   * @brief Add 3 factors to the graph:
+   *    - A prior on the first point to lie at (0, 0, 0)
+   *    - If betweenTranslations is non-empty, between factors provided by it.
+   *    - If betweenTranslations is empty, a prior on scale of the first
+   * relativeTranslations edge.
    *
+   * @param relativeTranslations unit translation directions between
+   * translations to be estimated
    * @param scale scale for first relative translation which fixes gauge.
    * @param graph factor graph to which prior is added.
+   * @param betweenTranslations relative translations (with scale) between 2
+   * points in world coordinate frame known a priori.
    * @param priorNoiseModel the noise model to use with the prior.
    */
-  void addPrior(const double scale, NonlinearFactorGraph *graph,
-                const SharedNoiseModel &priorNoiseModel =
-                    noiseModel::Isotropic::Sigma(3, 0.01)) const;
+  void addPrior(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const double scale,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      NonlinearFactorGraph *graph,
+      const SharedNoiseModel &priorNoiseModel =
+          noiseModel::Isotropic::Sigma(3, 0.01)) const;
 
   /**
    * @brief Create random initial translations.
    *
+   * @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(std::mt19937 *rng) const;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      std::mt19937 *rng, const Values &initialValues = Values()) const;
 
   /**
    * @brief Version of initializeRandomly with a fixed seed.
    *
+   * @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;
+  Values initializeRandomly(
+      const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Build and optimize factor graph.
    *
+   * @param relativeTranslations the relative translations, in world coordinate
+   * frames, vector of BinaryMeasurements of Unit3, where each key of a
+   * measurement is a point in 3D. 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. Unlike
+   * relativeTranslations, zero-magnitude betweenTranslations are not treated as
+   * hard constraints.
+   * @param initialValues intial values for optimization. Initializes randomly
+   * if not provided.
    * @return Values
    */
-  Values run(const double scale = 1.0) const;
+  Values run(
+      const TranslationEdges &relativeTranslations, const double scale = 1.0,
+      const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
+      const Values &initialValues = Values()) const;
 
   /**
    * @brief Simulate translation direction measurements

gtsam/sfm/sfm.i

@@ -4,11 +4,15 @@
 
 namespace gtsam {
 
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
 class SfmTrack {
   SfmTrack();
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
+  
+  Point3 p;
 
   double r;
   double g;
@@ -34,12 +38,15 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
+  std::vector<gtsam::SfmTrack>& trackList() const;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
+
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
   size_t numberCameras() const;
-  gtsam::SfmTrack track(size_t idx) const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::SfmTrack& track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
 
   gtsam::NonlinearFactorGraph generalSfmFactors(
       const gtsam::SharedNoiseModel& model =
@@ -83,6 +90,7 @@ class BinaryMeasurement {
 
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
+typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
 
 class BinaryMeasurementsUnit3 {
   BinaryMeasurementsUnit3();
@@ -91,6 +99,20 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsPoint3 {
+  BinaryMeasurementsPoint3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Point3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
+};
+
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in
@@ -142,8 +164,8 @@ class ShonanAveraging2 {
   ShonanAveraging2(string g2oFile);
   ShonanAveraging2(string g2oFile,
                    const gtsam::ShonanAveragingParameters2& parameters);
-  ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
-                   const gtsam::ShonanAveragingParameters2 &parameters);
+  ShonanAveraging2(const gtsam::BetweenFactorPose2s& factors,
+                   const gtsam::ShonanAveragingParameters2& parameters);
 
   // Query properties
   size_t nrUnknowns() const;
@@ -184,6 +206,10 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
+  ShonanAveraging3(
+      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
+      const gtsam::ShonanAveragingParameters3& parameters =
+          gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);
@@ -252,15 +278,36 @@ class MFAS {
 };
 
 #include <gtsam/sfm/TranslationRecovery.h>
+
 class TranslationRecovery {
-  TranslationRecovery(
+  TranslationRecovery(const gtsam::LevenbergMarquardtParams& lmParams);
+  TranslationRecovery();  // default params.
+  void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                const double scale,
+                const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                gtsam::NonlinearFactorGraph @graph,
+                const gtsam::SharedNoiseModel& priorNoiseModel) const;
+  void addPrior(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                const double scale,
+                const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                gtsam::NonlinearFactorGraph @graph) const;
+  gtsam::NonlinearFactorGraph buildGraph(
+      const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
+  gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                    const double scale,
+                    const gtsam::BinaryMeasurementsPoint3& betweenTranslations,
+                    const gtsam::Values& initialValues) const;
+  // default random initial values
+  gtsam::Values run(
       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
-      const gtsam::LevenbergMarquardtParams& lmParams);
-  TranslationRecovery(
-      const gtsam::BinaryMeasurementsUnit3&
-          relativeTranslations);  // default LevenbergMarquardtParams
-  gtsam::Values run(const double scale) const;
-  gtsam::Values run() const;  // default scale = 1.0
+      const double scale,
+      const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
+  // default empty betweenTranslations
+  gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+                    const double scale) const;
+  // default scale = 1.0, empty betweenTranslations
+  gtsam::Values run(
+      const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const;
 };
 
 }  // namespace gtsam