Merge pull request #1289 from borglab/fix/mixture-factor

release/4.3a0
Varun Agrawal 2022-09-13 12:16:01 -04:00 committed by GitHub
commit 0909c46339
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 48 additions and 7 deletions

View File

@ -153,12 +153,14 @@ std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
discreteElimination(const HybridGaussianFactorGraph &factors, discreteElimination(const HybridGaussianFactorGraph &factors,
const Ordering &frontalKeys) { const Ordering &frontalKeys) {
DiscreteFactorGraph dfg; DiscreteFactorGraph dfg;
for (auto &fp : factors) {
if (auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp)) { for (auto &factor : factors) {
dfg.push_back(ptr->inner()); if (auto p = boost::dynamic_pointer_cast<HybridDiscreteFactor>(factor)) {
} else if (auto p = dfg.push_back(p->inner());
boost::static_pointer_cast<HybridConditional>(fp)->inner()) { } else if (auto p = boost::static_pointer_cast<HybridConditional>(factor)) {
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p)); auto discrete_conditional =
boost::static_pointer_cast<DiscreteConditional>(p->inner());
dfg.push_back(discrete_conditional);
} else { } else {
// It is an orphan wrapper // It is an orphan wrapper
} }
@ -244,6 +246,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
return exp(-factor->error(empty_values)); return exp(-factor->error(empty_values));
}; };
DecisionTree<Key, double> fdt(separatorFactors, factorError); DecisionTree<Key, double> fdt(separatorFactors, factorError);
auto discreteFactor = auto discreteFactor =
boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt); boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);

View File

@ -100,11 +100,23 @@ class MixtureFactor : public HybridFactor {
bool normalized = false) bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) { : Base(keys, discreteKeys), normalized_(normalized) {
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors; std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set;
for (auto&& f : factors) { for (auto&& f : factors) {
// Insert all factor continuous keys in the continuous keys set.
std::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.end()));
nonlinear_factors.push_back( nonlinear_factors.push_back(
boost::dynamic_pointer_cast<NonlinearFactor>(f)); boost::dynamic_pointer_cast<NonlinearFactor>(f));
} }
factors_ = Factors(discreteKeys, nonlinear_factors); factors_ = Factors(discreteKeys, nonlinear_factors);
if (continuous_keys_set != factor_keys_set) {
throw std::runtime_error(
"The specified continuous keys and the keys in the factors don't "
"match!");
}
} }
~MixtureFactor() = default; ~MixtureFactor() = default;

View File

@ -147,6 +147,32 @@ TEST(HybridGaussianFactorGraph, Resize) {
EXPECT_LONGS_EQUAL(gfg.size(), 0); EXPECT_LONGS_EQUAL(gfg.size(), 0);
} }
/***************************************************************************
* Test that the MixtureFactor reports correctly if the number of continuous
* keys provided do not match the keys in the factors.
*/
TEST(HybridGaussianFactorGraph, MixtureFactor) {
auto nonlinearFactor = boost::make_shared<BetweenFactor<double>>(
X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1));
auto discreteFactor = boost::make_shared<DecisionTreeFactor>();
auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
auto still = boost::make_shared<MotionModel>(X(0), X(1), 0.0, noise_model),
moving = boost::make_shared<MotionModel>(X(0), X(1), 1.0, noise_model);
std::vector<MotionModel::shared_ptr> components = {still, moving};
// Check for exception when number of continuous keys are under-specified.
KeyVector contKeys = {X(0)};
THROWS_EXCEPTION(boost::make_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
// Check for exception when number of continuous keys are too many.
contKeys = {X(0), X(1), X(2)};
THROWS_EXCEPTION(boost::make_shared<MixtureFactor>(
contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components));
}
/***************************************************************************** /*****************************************************************************
* Test push_back on HFG makes the correct distinction. * Test push_back on HFG makes the correct distinction.
*/ */