Merge branch 'develop' into hybrid/model-selection
						commit
						1beeef840b
					
				|  | @ -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); | ||||
|  |  | |||
|  | @ -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
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -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)); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
| }; | ||||
|  |  | |||
|  | @ -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__": | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue