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