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

Merge branch 'hybrid-error-scalars' into hybrid-enum

release/4.3a0
Varun Agrawal 2024-09-17 14:54:43 -04:00
parent 340fae9f11 c1ebdb200c
commit ccebd38146
5 changed files with 39 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -55,6 +55,15 @@ HybridGaussianConditional::conditionals() const {
return conditionals_;
}
/* *******************************************************************************/
HybridGaussianConditional::HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionals)
: HybridGaussianConditional(continuousFrontals, continuousParents,
discreteParents,
Conditionals(discreteParents, conditionals)) {}
/* *******************************************************************************/
// TODO(dellaert): This is copy/paste: HybridGaussianConditional should be
// derived from HybridGaussianFactor, no?

16
gtsam/hybrid/HybridGaussianConditional.h
View File

@ -106,6 +106,20 @@ class GTSAM_EXPORT HybridGaussianConditional
const DiscreteKeys &discreteParents,
const Conditionals &conditionals);
/**
* @brief Make a Gaussian Mixture from a vector of Gaussian conditionals.
* The DecisionTree-based constructor is preferred over this one.
*
* @param continuousFrontals The continuous frontal variables
* @param continuousParents The continuous parent variables
* @param discreteParents Discrete parents variables
* @param conditionals Vector of conditionals
*/
HybridGaussianConditional(
const KeyVector &continuousFrontals, const KeyVector &continuousParents,
const DiscreteKeys &discreteParents,
const std::vector<GaussianConditional::shared_ptr> &conditionals);
/// @}
/// @name Testable
/// @{
@ -247,7 +261,7 @@ class GTSAM_EXPORT HybridGaussianConditional
#endif
};
/// Return the DiscreteKeys vector as a set.
/// Return the DiscreteKey vector as a set.
std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
// traits

12
gtsam/hybrid/hybrid.i
View File

@ -76,7 +76,7 @@ virtual class HybridConditional {
class HybridGaussianFactor : gtsam::HybridFactor {
HybridGaussianFactor(
const gtsam::KeyVector& continuousKeys,
const gtsam::DiscreteKeys& discreteKeys,
const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::GaussianFactor::shared_ptr, double>>&
factorsList);
@ -91,8 +91,12 @@ class HybridGaussianConditional : gtsam::HybridFactor {
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>&
conditionalsList);
const gtsam::HybridGaussianConditional::Conditionals& conditionals);
HybridGaussianConditional(
const gtsam::KeyVector& continuousFrontals,
const gtsam::KeyVector& continuousParents,
const gtsam::DiscreteKeys& discreteParents,
const std::vector<gtsam::GaussianConditional::shared_ptr>& conditionals);
gtsam::HybridGaussianFactor* likelihood(
const gtsam::VectorValues& frontals) const;
@ -248,7 +252,7 @@ class HybridNonlinearFactor : gtsam::HybridFactor {
bool normalized = false);
HybridNonlinearFactor(
const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys,
const gtsam::KeyVector& keys, const gtsam::DiscreteKey& discreteKey,
const std::vector<std::pair<gtsam::NonlinearFactor*, double>>& factors,
bool normalized = false);

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

@ -20,7 +20,7 @@ import gtsam
from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
HybridBayesNet, HybridGaussianConditional,
HybridGaussianFactor, HybridGaussianFactorGraph,
HybridValues, JacobianFactor, Ordering, noiseModel)
HybridValues, JacobianFactor, noiseModel)
DEBUG_MARGINALS = False
@ -31,13 +31,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_create(self):
"""Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3)
dk = DiscreteKeys()
dk.push_back((C(0), 2))
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)])
gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1)
@ -58,13 +56,11 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_optimize(self):
"""Test construction of hybrid factor graph."""
model = noiseModel.Unit.Create(3)
dk = DiscreteKeys()
dk.push_back((C(0), 2))
jf1 = JacobianFactor(X(0), np.eye(3), np.zeros((3, 1)), model)
jf2 = JacobianFactor(X(0), np.eye(3), np.ones((3, 1)), model)
gmf = HybridGaussianFactor([X(0)], dk, [(jf1, 0), (jf2, 0)])
gmf = HybridGaussianFactor([X(0)], (C(0), 2), [(jf1, 0), (jf2, 0)])
hfg = HybridGaussianFactorGraph()
hfg.push_back(jf1)
@ -96,8 +92,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
# Create Gaussian mixture Z(0) = X(0) + noise for each measurement.
I_1x1 = np.eye(1)
keys = DiscreteKeys()
keys.push_back(mode)
for i in range(num_measurements):
conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
I_1x1,
@ -107,8 +101,10 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
I_1x1,
X(0), [0],
sigma=3)
discreteParents = DiscreteKeys()
discreteParents.push_back(mode)
bayesNet.push_back(
HybridGaussianConditional([Z(i)], [X(0)], keys,
HybridGaussianConditional([Z(i)], [X(0)], discreteParents,
[conditional0, conditional1]))
# Create prior on X(0).

4
python/gtsam/tests/test_HybridNonlinearFactorGraph.py
View File

@ -27,8 +27,6 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
def test_nonlinear_hybrid(self):
nlfg = gtsam.HybridNonlinearFactorGraph()
dk = gtsam.DiscreteKeys()
dk.push_back((10, 2))
nlfg.push_back(
BetweenFactorPoint3(1, 2, Point3(1, 2, 3),
noiseModel.Diagonal.Variances([1, 1, 1])))
@ -40,7 +38,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
noiseModel.Unit.Create(3)), 0.0),
(PriorFactorPoint3(1, Point3(1, 2, 1),
noiseModel.Unit.Create(3)), 0.0)]
nlfg.push_back(gtsam.HybridNonlinearFactor([1], dk, factors))
nlfg.push_back(gtsam.HybridNonlinearFactor([1], (10, 2), factors))
nlfg.push_back(gtsam.DecisionTreeFactor((10, 2), "1 3"))
values = gtsam.Values()
values.insert_point3(1, Point3(0, 0, 0))