12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1357 from borglab/hybrid/store_constant

release/4.3a0
Varun Agrawal 2022-12-30 06:34:06 -05:00 committed by GitHub
parent 6466b03b5c 10079f6341
commit 16da553496
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 102 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/hybrid/HybridGaussianFactor.cpp
View File

@ -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);

40
gtsam/hybrid/HybridGaussianFactor.h
View File

@ -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
/// @{

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -417,7 +417,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));
}

3
gtsam/hybrid/HybridGaussianFactorGraph.h
View File

@ -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);

6
gtsam/hybrid/HybridValues.h
View File

@ -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
/// @{

1
gtsam/hybrid/hybrid.i
View File

@ -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);
};

80
python/gtsam/tests/test_HybridFactorGraph.py
View File

@ -11,30 +11,20 @@ Author: Fan Jiang
# 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 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
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 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
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 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
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 @@ 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_tiny2(self):
"""Test a tiny two variable hybrid model, with 2 measurements."""
# Create the Bayes net and sample from it.
@ -160,17 +159,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
)
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__":