Test on ratio

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);
-
-  // Forwarding constructor from concrete JacobianFactor
+  /**
+   * 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 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

@@ -394,7 +394,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

@@ -36,7 +36,6 @@ class HybridEliminationTree;
 class HybridBayesTree;
 class HybridJunctionTree;
 class DecisionTreeFactor;
-
 class JacobianFactor;
 
 /**
@@ -130,7 +129,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

@@ -16,6 +16,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

@@ -11,30 +11,20 @@
 # pylint: disable=invalid-name, no-name-in-module, no-member
 
 import unittest
-import math
 
 import numpy as np
 from gtsam.symbol_shorthand import C, M, X, Z
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import (
-    DecisionTreeFactor,
-    DiscreteConditional,
-    DiscreteKeys,
-    GaussianConditional,
-    GaussianMixture,
-    GaussianMixtureFactor,
-    HybridGaussianFactorGraph,
-    JacobianFactor,
-    Ordering,
-    noiseModel,
-)
+from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
+                   GaussianMixture, GaussianMixtureFactor,
+                   HybridGaussianFactorGraph, JacobianFactor, Ordering,
+                   noiseModel)
 
 
 class TestHybridGaussianFactorGraph(GtsamTestCase):
     """Unit tests for HybridGaussianFactorGraph."""
-
     def test_create(self):
         """Test construction of hybrid factor graph."""
         model = noiseModel.Unit.Create(3)
@@ -52,8 +42,8 @@ def test_create(self):
         hfg.push_back(gmf)
 
         hbn = hfg.eliminateSequential(
-            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
-        )
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
+                hfg, [C(0)]))
 
         self.assertEqual(hbn.size(), 2)
 
@@ -84,8 +74,8 @@ def test_optimize(self):
         hfg.push_back(dtf)
 
         hbn = hfg.eliminateSequential(
-            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)])
-        )
+            Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(
+                hfg, [C(0)]))
 
         hv = hbn.optimize()
         self.assertEqual(hv.atDiscrete(C(0)), 1)
@@ -105,15 +95,16 @@ def tiny(num_measurements: int = 1):
         keys = DiscreteKeys()
         keys.push_back(mode)
         for i in range(num_measurements):
-            conditional0 = GaussianConditional.FromMeanAndStddev(
-                Z(i), I, X(0), [0], sigma=0.5
-            )
-            conditional1 = GaussianConditional.FromMeanAndStddev(
-                Z(i), I, X(0), [0], sigma=3
-            )
-            bayesNet.emplaceMixture(
-                [Z(i)], [X(0)], keys, [conditional0, conditional1]
-            )
+            conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
+                                                                 I,
+                                                                 X(0), [0],
+                                                                 sigma=0.5)
+            conditional1 = GaussianConditional.FromMeanAndStddev(Z(i),
+                                                                 I,
+                                                                 X(0), [0],
+                                                                 sigma=3)
+            bayesNet.emplaceMixture([Z(i)], [X(0)], keys,
+                                    [conditional0, conditional1])
 
         # Create prior on X(0).
         prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0)
@@ -142,6 +133,14 @@ def test_tiny(self):
 
         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_tiny2(self):
         """Test a tiny two variable hybrid model, with 2 measurements."""
         # Create the Bayes net and sample from it.
@@ -160,17 +159,26 @@ def test_tiny2(self):
         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
-        )
-        print(expected_ratio)
-
-        # TODO(dellaert): Change the mode to 0 and calculate the ratio again.
+
+        # Calculate ratio between Bayes net probability and the factor graph:
+        expected_ratio = self.calculate_ratio(bayesNet, fg, sample)
+        # print(f"expected_ratio: {expected_ratio}\n")
+
+        # 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__":