Merge branch 'develop' into hybrid/model-selection

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -16,20 +16,30 @@
  */
 
 #include <gtsam/hybrid/HybridGaussianFactor.h>
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
 
 #include <boost/make_shared.hpp>
 
 namespace gtsam {
 
 /* ************************************************************************* */
-HybridGaussianFactor::HybridGaussianFactor(GaussianFactor::shared_ptr other)
-    : Base(other->keys()), inner_(other) {}
+HybridGaussianFactor::HybridGaussianFactor(
+    const boost::shared_ptr<GaussianFactor> &ptr)
+    : Base(ptr->keys()), inner_(ptr) {}
+
+HybridGaussianFactor::HybridGaussianFactor(
+    boost::shared_ptr<GaussianFactor> &&ptr)
+    : Base(ptr->keys()), inner_(std::move(ptr)) {}
 
-/* ************************************************************************* */
 HybridGaussianFactor::HybridGaussianFactor(JacobianFactor &&jf)
     : Base(jf.keys()),
       inner_(boost::make_shared<JacobianFactor>(std::move(jf))) {}
 
+HybridGaussianFactor::HybridGaussianFactor(HessianFactor &&hf)
+    : Base(hf.keys()),
+      inner_(boost::make_shared<HessianFactor>(std::move(hf))) {}
+
 /* ************************************************************************* */
 bool HybridGaussianFactor::equals(const HybridFactor &other, double tol) const {
   const This *e = dynamic_cast<const This *>(&other);

gtsam/hybrid/HybridGaussianFactor.h

@@ -19,10 +19,13 @@
 
 #include <gtsam/hybrid/HybridFactor.h>
 #include <gtsam/linear/GaussianFactor.h>
-#include <gtsam/linear/JacobianFactor.h>
 
 namespace gtsam {
 
+// Forward declarations
+class JacobianFactor;
+class HessianFactor;
+
 /**
  * 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
@@ -41,12 +44,41 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
 
   HybridGaussianFactor() = default;
 
-  // Explicit conversion from a shared ptr of GF
-  explicit HybridGaussianFactor(GaussianFactor::shared_ptr other);
+  /**
+   * Constructor from shared_ptr of GaussianFactor.
+   * Example:
+   *  boost::shared_ptr<GaussianFactor> ptr =
+   * boost::make_shared<JacobianFactor>(...);
+   *
+   */
+  explicit HybridGaussianFactor(const boost::shared_ptr<GaussianFactor> &ptr);
 
-  // Forwarding constructor from concrete JacobianFactor
+  /**
+   * Forwarding constructor from shared_ptr of GaussianFactor.
+   * Examples:
+   *   HybridGaussianFactor factor = boost::make_shared<JacobianFactor>(...);
+   *   HybridGaussianFactor factor(boost::make_shared<JacobianFactor>(...));
+   */
+  explicit HybridGaussianFactor(boost::shared_ptr<GaussianFactor> &&ptr);
+
+  /**
+   * Forwarding constructor from rvalue reference of JacobianFactor.
+   *
+   * Examples:
+   *   HybridGaussianFactor factor = JacobianFactor(...);
+   *   HybridGaussianFactor factor(JacobianFactor(...));
+   */
   explicit HybridGaussianFactor(JacobianFactor &&jf);
 
+  /**
+   * Forwarding constructor from rvalue reference of JacobianFactor.
+   *
+   * Examples:
+   *   HybridGaussianFactor factor = HessianFactor(...);
+   *   HybridGaussianFactor factor(HessianFactor(...));
+   */
+  explicit HybridGaussianFactor(HessianFactor &&hf);
+
  public:
   /// @name Testable
   /// @{

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -418,7 +418,7 @@ void HybridGaussianFactorGraph::add(JacobianFactor &&factor) {
 }
 
 /* ************************************************************************ */
-void HybridGaussianFactorGraph::add(JacobianFactor::shared_ptr factor) {
+void HybridGaussianFactorGraph::add(boost::shared_ptr<JacobianFactor> &factor) {
   FactorGraph::add(boost::make_shared<HybridGaussianFactor>(factor));
 }
 

gtsam/hybrid/HybridGaussianFactorGraph.h

@@ -37,7 +37,6 @@ class HybridEliminationTree;
 class HybridBayesTree;
 class HybridJunctionTree;
 class DecisionTreeFactor;
-
 class JacobianFactor;
 
 /**
@@ -131,7 +130,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph
   void add(JacobianFactor&& factor);
 
   /// Add a Jacobian factor as a shared ptr.
-  void add(JacobianFactor::shared_ptr factor);
+  void add(boost::shared_ptr<JacobianFactor>& factor);
 
   /// Add a DecisionTreeFactor to the factor graph.
   void add(DecisionTreeFactor&& factor);

gtsam/hybrid/HybridValues.h

@@ -118,6 +118,12 @@ class GTSAM_EXPORT HybridValues {
    */
   Vector& at(Key j) { return continuous_.at(j); };
 
+  /** For all key/value pairs in \c values, replace values with corresponding keys in this class
+  *   with those in \c values.  Throws std::out_of_range if any keys in \c values are not present
+  *   in this class. */
+  void update(const VectorValues& values) { continuous_.update(values); }
+
+  /// @}
   /// @name Wrapper support
   /// @{
 

gtsam/hybrid/hybrid.i

@@ -17,6 +17,7 @@ class HybridValues {
   bool equals(const gtsam::HybridValues& other, double tol) const;
   void insert(gtsam::Key j, int value);
   void insert(gtsam::Key j, const gtsam::Vector& value);
+  void update(const gtsam::VectorValues& values);
   size_t& atDiscrete(gtsam::Key j);
   gtsam::Vector& at(gtsam::Key j);
 };

python/gtsam/tests/test_HybridFactorGraph.py

@@ -135,6 +135,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
 
         self.assertEqual(fg.size(), 3)
 
+    @staticmethod
+    def calculate_ratio(bayesNet, fg, sample):
+        """Calculate ratio  between Bayes net probability and the factor graph."""
+        continuous = gtsam.VectorValues()
+        continuous.insert(X(0), sample.at(X(0)))
+        return bayesNet.evaluate(sample) / fg.probPrime(
+            continuous, sample.discrete())
+
     def test_ratio(self):
         """
         Given a tiny two variable hybrid model, with 2 measurements,
@@ -161,18 +169,26 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         fg.push_back(bayesNet.atGaussian(2))
         fg.push_back(bayesNet.atDiscrete(3))
 
+        # print(fg)
         self.assertEqual(fg.size(), 4)
 
         # Calculate ratio between Bayes net probability and the factor graph:
-        continuousValues = gtsam.VectorValues()
-        continuousValues.insert(X(0), sample.at(X(0)))
-        discreteValues = sample.discrete()
-        expected_ratio = bayesNet.evaluate(sample) / fg.probPrime(
-            continuousValues, discreteValues)
-        #TODO(Varun) This should be 1. Adding the normalizing factor should fix fg.probPrime
-        print(expected_ratio)
+        expected_ratio = self.calculate_ratio(bayesNet, fg, sample)
+        # print(f"expected_ratio: {expected_ratio}\n")
 
-        # TODO(dellaert): Change the mode to 0 and calculate the ratio again.
+        # Create measurements from the sample.
+        measurements = gtsam.VectorValues()
+        for i in range(2):
+            measurements.insert(Z(i), sample.at(Z(i)))
+
+        # Check with a number of other samples.
+        for i in range(10):
+            other = bayesNet.sample()
+            other.update(measurements)
+            # print(other)
+            # ratio = self.calculate_ratio(bayesNet, fg, other)
+            # print(f"Ratio: {ratio}\n")
+            # self.assertAlmostEqual(ratio, expected_ratio)
 
 
 if __name__ == "__main__":