Merge branch 'develop' into fix-1496

.github/workflows/build-windows.yml

@@ -99,26 +99,67 @@ jobs:
           cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib"
 
       - name: Build
+        shell: bash
         run: |
           # Since Visual Studio is a multi-generator, we need to use --config
           # https://stackoverflow.com/a/24470998/1236990
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap
 
+          # Target doesn't exist
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap
+
+      - name: Test
+        shell: bash
+        run: |
           # Run GTSAM tests
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry
+
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference
+          # Compilation error
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation
+
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam
+
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm
+          # Compile. Fail with exception
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear
-          cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic
+          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm          
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic
+
+          # Compile. Fail with exception
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear
+
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam
+
 
           # Run GTSAM_UNSTABLE tests
-          #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
+          cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable
+
+          # Compile. Fail with exception
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable
+
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable
+
+          # Compilation error
+          # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition          

CMakeLists.txt

@@ -32,6 +32,14 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
 ###############################################################################
 # Gather information, perform checks, set defaults
 
+if(MSVC)
+  set(MSVC_LINKER_FLAGS "/FORCE:MULTIPLE")
+  set(CMAKE_EXE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_MODULE_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_SHARED_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+  set(CMAKE_STATIC_LINKER_FLAGS ${MSVC_LINKER_FLAGS})
+endif()
+
 set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
 include(GtsamMakeConfigFile)
 include(GNUInstallDirs)

cmake/Config.cmake.in

@@ -21,6 +21,10 @@ else()
 find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
 endif()
 
+if(@GTSAM_USE_TBB@)
+    find_dependency(TBB 4.4 COMPONENTS tbb tbbmalloc)
+endif()
+
 if(@GTSAM_USE_SYSTEM_EIGEN@)
 find_dependency(Eigen3 REQUIRED)
 endif()

gtsam/basis/Basis.h

@@ -80,7 +80,7 @@ using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
  *
  * @ingroup basis
  */
-Matrix kroneckerProductIdentity(size_t M, const Weights& w);
+Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w);
 
 /**
  * CRTP Base class for function bases

gtsam/discrete/DecisionTreeFactor.cpp

@@ -82,6 +82,22 @@ namespace gtsam {
     ADT::print("", formatter);
   }
 
+  /* ************************************************************************ */
+  DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const {
+    // apply operand
+    ADT result = ADT::apply(op);
+    // Make a new factor
+    return DecisionTreeFactor(discreteKeys(), result);
+  }
+
+  /* ************************************************************************ */
+  DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
+    // apply operand
+    ADT result = ADT::apply(op);
+    // Make a new factor
+    return DecisionTreeFactor(discreteKeys(), result);
+  }
+
   /* ************************************************************************ */
   DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f,
                                               ADT::Binary op) const {
@@ -101,14 +117,6 @@ namespace gtsam {
     return DecisionTreeFactor(keys, result);
   }
 
-  /* ************************************************************************ */
-  DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const {
-    // apply operand
-    ADT result = ADT::apply(op);
-    // Make a new factor
-    return DecisionTreeFactor(discreteKeys(), result);
-  }
-
   /* ************************************************************************ */
   DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
       size_t nrFrontals, ADT::Binary op) const {
@@ -188,10 +196,45 @@ namespace gtsam {
 
   /* ************************************************************************ */
   std::vector<double> DecisionTreeFactor::probabilities() const {
+    // Set of all keys
+    std::set<Key> allKeys(keys().begin(), keys().end());
+
     std::vector<double> probs;
-    for (auto&& [key, value] : enumerate()) {
+
-      probs.push_back(value);
+    /* An operation that takes each leaf probability, and computes the
-    }
+     * nrAssignments by checking the difference between the keys in the factor
+     * and the keys in the assignment.
+     * The nrAssignments is then used to append
+     * the correct number of leaf probability values to the `probs` vector
+     * defined above.
+     */
+    auto op = [&](const Assignment<Key>& a, double p) {
+      // Get all the keys in the current assignment
+      std::set<Key> assignment_keys;
+      for (auto&& [k, _] : a) {
+        assignment_keys.insert(k);
+      }
+
+      // Find the keys missing in the assignment
+      std::vector<Key> diff;
+      std::set_difference(allKeys.begin(), allKeys.end(),
+                          assignment_keys.begin(), assignment_keys.end(),
+                          std::back_inserter(diff));
+
+      // Compute the total number of assignments in the (pruned) subtree
+      size_t nrAssignments = 1;
+      for (auto&& k : diff) {
+        nrAssignments *= cardinalities_.at(k);
+      }
+      // Add p `nrAssignments` times to the probs vector.
+      probs.insert(probs.end(), nrAssignments, p);
+
+      return p;
+    };
+
+    // Go through the tree
+    this->apply(op);
+
     return probs;
   }
 
@@ -305,11 +348,7 @@ namespace gtsam {
     const size_t N = maxNrAssignments;
 
     // Get the probabilities in the decision tree so we can threshold.
-    std::vector<double> probabilities;
+    std::vector<double> probabilities = this->probabilities();
-    // NOTE(Varun) this is potentially slow due to the cartesian product
-    for (auto&& [assignment, prob] : this->enumerate()) {
-      probabilities.push_back(prob);
-    }
 
     // The number of probabilities can be lower than max_leaves
     if (probabilities.size() <= N) {

gtsam/discrete/DecisionTreeFactor.h

@@ -186,6 +186,13 @@ namespace gtsam {
      * Apply unary operator (*this) "op" f
      * @param op a unary operator that operates on AlgebraicDecisionTree
      */
+    DecisionTreeFactor apply(ADT::Unary op) const;
+
+    /**
+     * Apply unary operator (*this) "op" f
+     * @param op a unary operator that operates on AlgebraicDecisionTree. Takes
+     * both the assignment and the value.
+     */
     DecisionTreeFactor apply(ADT::UnaryAssignment op) const;
 
     /**

gtsam/discrete/SignatureParser.h

@@ -22,6 +22,8 @@
 #include <string>
 #include <vector>
 
+#include <gtsam/dllexport.h>
+
 namespace gtsam {
 /**
  * @brief A simple parser that replaces the boost spirit parser.
@@ -47,7 +49,7 @@ namespace gtsam {
  *
  * Also fails if the rows are not of the same size.
  */
-struct SignatureParser {
+struct GTSAM_EXPORT SignatureParser {
   using Row = std::vector<double>;
   using Table = std::vector<Row>;
 

gtsam/discrete/TableFactor.cpp

@@ -56,9 +56,45 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys,
   sort(sorted_dkeys_.begin(), sorted_dkeys_.end());
 }
 
+/* ************************************************************************ */
+TableFactor::TableFactor(const DiscreteKeys& dkeys,
+                         const DecisionTree<Key, double>& dtree)
+    : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {}
+
+/**
+ * @brief Compute the correct ordering of the leaves in the decision tree.
+ *
+ * This is done by first taking all the values which have modulo 0 value with
+ * the cardinality of the innermost key `n`, and we go up to modulo n.
+ *
+ * @param dt The DecisionTree
+ * @return std::vector<double>
+ */
+std::vector<double> ComputeLeafOrdering(const DiscreteKeys& dkeys,
+                                        const DecisionTreeFactor& dt) {
+  std::vector<double> probs = dt.probabilities();
+  std::vector<double> ordered;
+
+  size_t n = dkeys[0].second;
+
+  for (size_t k = 0; k < n; ++k) {
+    for (size_t idx = 0; idx < probs.size(); ++idx) {
+      if (idx % n == k) {
+        ordered.push_back(probs[idx]);
+      }
+    }
+  }
+  return ordered;
+}
+
+/* ************************************************************************ */
+TableFactor::TableFactor(const DiscreteKeys& dkeys,
+                         const DecisionTreeFactor& dtf)
+    : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {}
+
 /* ************************************************************************ */
 TableFactor::TableFactor(const DiscreteConditional& c)
-    : TableFactor(c.discreteKeys(), c.probabilities()) {}
+    : TableFactor(c.discreteKeys(), c) {}
 
 /* ************************************************************************ */
 Eigen::SparseVector<double> TableFactor::Convert(

gtsam/discrete/TableFactor.h

@@ -144,6 +144,12 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
   TableFactor(const DiscreteKey& key, const std::vector<double>& row)
       : TableFactor(DiscreteKeys{key}, row) {}
 
+  /// Constructor from DecisionTreeFactor
+  TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf);
+
+  /// Constructor from DecisionTree<Key, double>/AlgebraicDecisionTree
+  TableFactor(const DiscreteKeys& keys, const DecisionTree<Key, double>& dtree);
+
   /** Construct from a DiscreteConditional type */
   explicit TableFactor(const DiscreteConditional& c);
 
@@ -180,7 +186,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
     return apply(f, Ring::mul);
   };
 
-  /// multiple with DecisionTreeFactor
+  /// multiply with DecisionTreeFactor
   DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override;
 
   static double safe_div(const double& a, const double& b);

gtsam/discrete/tests/testTableFactor.cpp

@@ -19,6 +19,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/serializationTestHelpers.h>
+#include <gtsam/discrete/DiscreteConditional.h>
 #include <gtsam/discrete/DiscreteDistribution.h>
 #include <gtsam/discrete/Signature.h>
 #include <gtsam/discrete/TableFactor.h>
@@ -131,6 +132,16 @@ TEST(TableFactor, constructors) {
   // Manually constructed via inspection and comparison to DecisionTreeFactor
   TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
   EXPECT(assert_equal(expected, f4));
+
+  // Test for 9=3x3 values.
+  DiscreteKey V(0, 3), W(1, 3);
+  DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11");
+  TableFactor f5(conditional5);
+  // GTSAM_PRINT(f5);
+  TableFactor expected_f5(
+      X & Y,
+      "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667");
+  EXPECT(assert_equal(expected_f5, f5, 1e-6));
 }
 
 /* ************************************************************************* */

gtsam/geometry/Line3.h

@@ -146,7 +146,7 @@ class GTSAM_EXPORT Line3 {
    * @param Dline -  OptionalJacobian of transformed line with respect to l
    * @return Transformed line in camera frame
    */
-  friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
+  GTSAM_EXPORT friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
                            OptionalJacobian<4, 6> Dpose,
                            OptionalJacobian<4, 4> Dline);
 };

gtsam/geometry/Pose2.h

@@ -36,7 +36,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class Pose2: public LieGroup<Pose2, 3> {
+class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
 
 public:
 
@@ -112,10 +112,10 @@ public:
   /// @{
 
   /** print with optional string */
-  GTSAM_EXPORT void print(const std::string& s = "") const;
+  void print(const std::string& s = "") const;
 
   /** assert equality up to a tolerance */
-  GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
+  bool equals(const Pose2& pose, double tol = 1e-9) const;
 
   /// @}
   /// @name Group
@@ -125,7 +125,7 @@ public:
   inline static Pose2 Identity() { return Pose2(); }
 
   /// inverse
-  GTSAM_EXPORT Pose2 inverse() const;
+  Pose2 inverse() const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
@@ -137,16 +137,16 @@ public:
   /// @{
 
   ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
-  GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
+  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
+  static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
 
   /**
    * Calculate Adjoint map
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
-  GTSAM_EXPORT Matrix3 AdjointMap() const;
+  Matrix3 AdjointMap() const;
 
   /// Apply AdjointMap to twist xi
   inline Vector3 Adjoint(const Vector3& xi) const {
@@ -156,7 +156,7 @@ public:
   /**
    * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
    */
-  GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
+  static Matrix3 adjointMap(const Vector3& v);
 
   /**
    * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
@@ -192,15 +192,15 @@ public:
   }
 
   /// Derivative of Expmap
-  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
+  static Matrix3 ExpmapDerivative(const Vector3& v);
 
   /// Derivative of Logmap
-  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
+  static Matrix3 LogmapDerivative(const Pose2& v);
 
   // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
-  struct ChartAtOrigin {
+  struct GTSAM_EXPORT ChartAtOrigin {
-	GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
+    static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
-	GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
+    static Vector3 Local(const Pose2& r, ChartJacobian H = {});
   };
 
   using LieGroup<Pose2, 3>::inverse; // version with derivative
@@ -210,7 +210,7 @@ public:
   /// @{
 
   /** Return point coordinates in pose coordinate frame */
-  GTSAM_EXPORT Point2 transformTo(const Point2& point,
+  Point2 transformTo(const Point2& point,
       OptionalJacobian<2, 3> Dpose = {},
       OptionalJacobian<2, 2> Dpoint = {}) const;
 
@@ -222,7 +222,7 @@ public:
   Matrix transformTo(const Matrix& points) const;
 
   /** Return point coordinates in global frame */
-  GTSAM_EXPORT Point2 transformFrom(const Point2& point,
+  Point2 transformFrom(const Point2& point,
       OptionalJacobian<2, 3> Dpose = {},
       OptionalJacobian<2, 2> Dpoint = {}) const;
 
@@ -273,14 +273,14 @@ public:
   }
 
   //// return transformation matrix
-  GTSAM_EXPORT Matrix3 matrix() const;
+  Matrix3 matrix() const;
 
   /**
    * Calculate bearing to a landmark
    * @param point 2D location of landmark
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  GTSAM_EXPORT Rot2 bearing(const Point2& point,
+  Rot2 bearing(const Point2& point,
                OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
 
   /**
@@ -288,7 +288,7 @@ public:
    * @param point SO(2) location of other pose
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
+  Rot2 bearing(const Pose2& pose,
                OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
 
   /**
@@ -296,7 +296,7 @@ public:
    * @param point 2D location of landmark
    * @return range (double)
    */
-  GTSAM_EXPORT double range(const Point2& point,
+  double range(const Point2& point,
       OptionalJacobian<1, 3> H1={},
       OptionalJacobian<1, 2> H2={}) const;
 
@@ -305,7 +305,7 @@ public:
    * @param point 2D location of other pose
    * @return range (double)
    */
-  GTSAM_EXPORT double range(const Pose2& point,
+  double range(const Pose2& point,
       OptionalJacobian<1, 3> H1={},
       OptionalJacobian<1, 3> H2={}) const;
 

gtsam/geometry/Pose3.h

@@ -204,7 +204,7 @@ public:
   static Matrix6 LogmapDerivative(const Pose3& xi);
 
   // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
-  struct ChartAtOrigin {
+  struct GTSAM_EXPORT ChartAtOrigin {
     static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
     static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
   };

gtsam/geometry/tests/testRot3.cpp

@@ -597,6 +597,25 @@ TEST(Rot3, quaternion) {
   EXPECT(assert_equal(expected2, actual2));
 }
 
+/* ************************************************************************* */
+TEST(Rot3, ConvertQuaternion) {
+  Eigen::Quaterniond eigenQuaternion;
+  eigenQuaternion.w() = 1.0;
+  eigenQuaternion.x() = 2.0;
+  eigenQuaternion.y() = 3.0;
+  eigenQuaternion.z() = 4.0;
+  EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9);
+
+  Rot3 R(eigenQuaternion);
+  EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9);
+  EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9);
+}
+
 /* ************************************************************************* */
 Matrix Cayley(const Matrix& A) {
   Matrix::Index n = A.cols();

gtsam/hybrid/GaussianMixture.cpp

@@ -286,8 +286,6 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) {
 
 /* *******************************************************************************/
 void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) {
-  auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys());
-  auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys());
   // Functional which loops over all assignments and create a set of
   // GaussianConditionals
   auto pruner = prunerFunc(discreteProbs);

gtsam/hybrid/HybridBayesNet.cpp

@@ -129,7 +129,6 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc(
 DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
     size_t maxNrLeaves) {
   // Get the joint distribution of only the discrete keys
-  gttic_(HybridBayesNet_PruneDiscreteConditionals);
   // The joint discrete probability.
   DiscreteConditional discreteProbs;
 
@@ -163,7 +162,6 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
   gttoc_(HybridBayesNet_PruneDiscreteConditionals);
 
   // Eliminate joint probability back into conditionals
-  gttic_(HybridBayesNet_UpdateDiscreteConditionals);
   DiscreteFactorGraph dfg{prunedDiscreteProbs};
   DiscreteBayesNet::shared_ptr dbn = dfg.eliminateSequential(discrete_frontals);
 
@@ -174,7 +172,6 @@ DecisionTreeFactor HybridBayesNet::pruneDiscreteConditionals(
     // dbn->at(i)->print();
     this->at(idx) = std::make_shared<HybridConditional>(dbn->at(i));
   }
-  gttoc_(HybridBayesNet_UpdateDiscreteConditionals);
 
   return prunedDiscreteProbs;
 }
@@ -193,7 +190,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
 
   HybridBayesNet prunedBayesNetFragment;
 
-  gttic_(HybridBayesNet_PruneMixtures);
   // Go through all the conditionals in the
   // Bayes Net and prune them as per prunedDiscreteProbs.
   for (auto &&conditional : *this) {
@@ -210,7 +206,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) {
       prunedBayesNetFragment.push_back(conditional);
     }
   }
-  gttoc_(HybridBayesNet_PruneMixtures);
 
   return prunedBayesNetFragment;
 }

gtsam/hybrid/HybridFactorGraph.h

@@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr<Factor>;
  * Hybrid Factor Graph
  * Factor graph with utilities for hybrid factors.
  */
-class HybridFactorGraph : public FactorGraph<Factor> {
+class GTSAM_EXPORT HybridFactorGraph : public FactorGraph<Factor> {
  public:
   using Base = FactorGraph<Factor>;
   using This = HybridFactorGraph;              ///< this class

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -96,7 +96,6 @@ static GaussianFactorGraphTree addGaussian(
 // TODO(dellaert): it's probably more efficient to first collect the discrete
 // keys, and then loop over all assignments to populate a vector.
 GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
-  gttic_(assembleGraphTree);
 
   GaussianFactorGraphTree result;
 
@@ -129,8 +128,6 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const {
     }
   }
 
-  gttoc_(assembleGraphTree);
-
   return result;
 }
 

gtsam/hybrid/HybridSmoother.h

@@ -24,7 +24,7 @@
 
 namespace gtsam {
 
-class HybridSmoother {
+class GTSAM_EXPORT HybridSmoother {
  private:
   HybridBayesNet hybridBayesNet_;
   HybridGaussianFactorGraph remainingFactorGraph_;

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -43,6 +43,7 @@
 #include <iostream>
 #include <iterator>
 #include <vector>
+#include <numeric>
 
 #include "Switching.h"
 #include "TinyHybridExample.h"

gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp

@@ -420,7 +420,7 @@ TEST(HybridFactorGraph, Full_Elimination) {
     DiscreteFactorGraph discrete_fg;
     // TODO(Varun) Make this a function of HybridGaussianFactorGraph?
     for (auto& factor : (*remainingFactorGraph_partial)) {
-      auto df = dynamic_pointer_cast<DecisionTreeFactor>(factor);
+      auto df = dynamic_pointer_cast<DiscreteFactor>(factor);
       assert(df);
       discrete_fg.push_back(df);
     }

gtsam/inference/Ordering.h

@@ -30,7 +30,7 @@
 
 namespace gtsam {
 
-class Ordering: public KeyVector {
+class GTSAM_EXPORT Ordering: public KeyVector {
 protected:
   typedef KeyVector Base;
 
@@ -44,8 +44,7 @@ public:
   typedef Ordering This; ///< Typedef to this class
   typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
-  /// Create an empty ordering
+  /// Create an empty ordering  
-  GTSAM_EXPORT
   Ordering() {
   }
 
@@ -99,7 +98,7 @@ public:
   }
 
   /// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
-  static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
+  static Ordering Colamd(const VariableIndex& variableIndex);
 
   /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
   /// for note on performance).  This internally builds a VariableIndex so if you already have a
@@ -124,7 +123,7 @@ public:
   /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
   /// \c constrainLast.   If \c forceOrder is false, the variables in \c constrainLast will be
   /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
-  static GTSAM_EXPORT Ordering ColamdConstrainedLast(
+  static Ordering ColamdConstrainedLast(
       const VariableIndex& variableIndex, const KeyVector& constrainLast,
       bool forceOrder = false);
 
@@ -152,7 +151,7 @@ public:
   /// KeyVector \c constrainFirst.   If \c forceOrder is false, the variables in \c
   /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
   /// reduce fill-in as well.
-  static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
+  static Ordering ColamdConstrainedFirst(
       const VariableIndex& variableIndex,
       const KeyVector& constrainFirst, bool forceOrder = false);
 
@@ -181,7 +180,7 @@ public:
   /// appear in \c groups in arbitrary order.  Any variables not present in \c groups will be
   /// assigned to group 0.  This function simply fills the \c cmember argument to CCOLAMD with the
   /// supplied indices, see the CCOLAMD documentation for more information.
-  static GTSAM_EXPORT Ordering ColamdConstrained(
+  static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
 
   /// Return a natural Ordering. Typically used by iterative solvers
@@ -195,11 +194,11 @@ public:
 
   /// METIS Formatting function
   template<class FACTOR_GRAPH>
-  static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
+  static void CSRFormat(std::vector<int>& xadj,
       std::vector<int>& adj, const FACTOR_GRAPH& graph);
 
   /// Compute an ordering determined by METIS from a VariableIndex
-  static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
+  static Ordering Metis(const MetisIndex& met);
 
   template<class FACTOR_GRAPH>
   static Ordering Metis(const FACTOR_GRAPH& graph) {
@@ -241,18 +240,16 @@ public:
   /// @name Testable
   /// @{
 
-  GTSAM_EXPORT
   void print(const std::string& str = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const;
 
-  GTSAM_EXPORT
   bool equals(const Ordering& other, double tol = 1e-9) const;
 
   /// @}
 
 private:
   /// Internal COLAMD function
-  static GTSAM_EXPORT Ordering ColamdConstrained(
+  static Ordering ColamdConstrained(
       const VariableIndex& variableIndex, std::vector<int>& cmember);
 
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

gtsam/sfm/ShonanAveraging.cpp

@@ -894,6 +894,9 @@ template <size_t d>
 std::pair<Values, double> ShonanAveraging<d>::run(const Values &initialEstimate,
                                                   size_t pMin,
                                                   size_t pMax) const {
+  if (pMin < d) {
+    throw std::runtime_error("pMin is smaller than the base dimension d");
+  }
   Values Qstar;
   Values initialSOp = LiftTo<Rot>(pMin, initialEstimate);  // lift to pMin!
   for (size_t p = pMin; p <= pMax; p++) {

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -415,6 +415,20 @@ TEST(ShonanAveraging3, PriorWeights) {
   auto result = shonan.run(initial, 3, 3);
   EXPECT_DOUBLES_EQUAL(0.0015, shonan.cost(result.first), 1e-4);
 }
+
+/* ************************************************************************* */
+// Check a small graph created using binary measurements
+TEST(ShonanAveraging3, BinaryMeasurements) {
+  std::vector<BinaryMeasurement<Rot3>> measurements;
+  auto unit3 = noiseModel::Unit::Create(3);
+  measurements.emplace_back(0, 1, Rot3::Yaw(M_PI_2), unit3);
+  measurements.emplace_back(1, 2, Rot3::Yaw(M_PI_2), unit3);
+  ShonanAveraging3 shonan(measurements);
+  Values initial = shonan.initializeRandomly();
+  auto result = shonan.run(initial, 3, 5);
+  EXPECT_DOUBLES_EQUAL(0.0, shonan.cost(result.first), 1e-4);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/linear/LPInitSolver.h

@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam_unstable/dllexport.h>
 #include <gtsam_unstable/linear/LP.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
@@ -49,7 +50,7 @@ namespace gtsam {
  * inequality constraint, we can't conclude that the problem is infeasible.
  * However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
  */
-class LPInitSolver {
+class GTSAM_UNSTABLE_EXPORT LPInitSolver {
 private:
   const LP& lp_;
 

gtsam_unstable/linear/QPSParser.h

@@ -18,12 +18,13 @@
 
 #pragma once
 
+#include <gtsam_unstable/dllexport.h>
 #include <gtsam_unstable/linear/QP.h>
 #include <fstream>
 
 namespace gtsam {
 
-class QPSParser {
+class GTSAM_UNSTABLE_EXPORT QPSParser {
 
 private:
   std::string fileName_;

python/gtsam/tests/test_ShonanAveraging.py

@@ -10,14 +10,16 @@ Author: Frank Dellaert
 """
 # pylint: disable=invalid-name, no-name-in-module, no-member
 
+import math
 import unittest
 
 import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
+from gtsam import (BetweenFactorPose2, BetweenFactorPose3,
-                   ShonanAveraging2, ShonanAveraging3,
+                   BinaryMeasurementRot3, LevenbergMarquardtParams, Pose2,
+                   Pose3, Rot2, Rot3, ShonanAveraging2, ShonanAveraging3,
                    ShonanAveragingParameters2, ShonanAveragingParameters3)
 
 DEFAULT_PARAMS = ShonanAveragingParameters3(
@@ -197,6 +199,19 @@ class TestShonanAveraging(GtsamTestCase):
         expected_thetas_deg = np.array([0.0, 90.0, 0.0])
         np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)
 
+    def test_measurements3(self):
+        """Create from Measurements."""
+        measurements = []
+        unit3 = gtsam.noiseModel.Unit.Create(3)
+        m01 = BinaryMeasurementRot3(0, 1, Rot3.Yaw(math.radians(90)), unit3)
+        m12 = BinaryMeasurementRot3(1, 2, Rot3.Yaw(math.radians(90)), unit3)
+        measurements.append(m01)
+        measurements.append(m12)
+        obj = ShonanAveraging3(measurements)
+        self.assertIsInstance(obj, ShonanAveraging3)
+        initial = obj.initializeRandomly()
+        _, cost = obj.run(initial, min_p=3, max_p=5)
+        self.assertAlmostEqual(cost, 0)
 
 if __name__ == "__main__":
     unittest.main()