Merge pull request #1289 from borglab/fix/mixture-factor
commit
0909c46339
|
@ -153,12 +153,14 @@ std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>
|
|||
discreteElimination(const HybridGaussianFactorGraph &factors,
|
||||
const Ordering &frontalKeys) {
|
||||
DiscreteFactorGraph dfg;
|
||||
for (auto &fp : factors) {
|
||||
if (auto ptr = boost::dynamic_pointer_cast<HybridDiscreteFactor>(fp)) {
|
||||
dfg.push_back(ptr->inner());
|
||||
} else if (auto p =
|
||||
boost::static_pointer_cast<HybridConditional>(fp)->inner()) {
|
||||
dfg.push_back(boost::static_pointer_cast<DiscreteConditional>(p));
|
||||
|
||||
for (auto &factor : factors) {
|
||||
if (auto p = boost::dynamic_pointer_cast<HybridDiscreteFactor>(factor)) {
|
||||
dfg.push_back(p->inner());
|
||||
} else if (auto p = boost::static_pointer_cast<HybridConditional>(factor)) {
|
||||
auto discrete_conditional =
|
||||
boost::static_pointer_cast<DiscreteConditional>(p->inner());
|
||||
dfg.push_back(discrete_conditional);
|
||||
} else {
|
||||
// It is an orphan wrapper
|
||||
}
|
||||
|
@ -244,6 +246,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
|
|||
return exp(-factor->error(empty_values));
|
||||
};
|
||||
DecisionTree<Key, double> fdt(separatorFactors, factorError);
|
||||
|
||||
auto discreteFactor =
|
||||
boost::make_shared<DecisionTreeFactor>(discreteSeparator, fdt);
|
||||
|
||||
|
|
|
@ -100,11 +100,23 @@ class MixtureFactor : public HybridFactor {
|
|||
bool normalized = false)
|
||||
: Base(keys, discreteKeys), normalized_(normalized) {
|
||||
std::vector<NonlinearFactor::shared_ptr> nonlinear_factors;
|
||||
KeySet continuous_keys_set(keys.begin(), keys.end());
|
||||
KeySet factor_keys_set;
|
||||
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(
|
||||
boost::dynamic_pointer_cast<NonlinearFactor>(f));
|
||||
}
|
||||
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;
|
||||
|
|
|
@ -147,6 +147,32 @@ TEST(HybridGaussianFactorGraph, Resize) {
|
|||
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.
|
||||
*/
|
||||
|
@ -348,7 +374,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
|
|||
EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size());
|
||||
EXPECT(discreteFactor->root_->isLeaf() == false);
|
||||
|
||||
//TODO(Varun) Test emplace_discrete
|
||||
// TODO(Varun) Test emplace_discrete
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
|
|
Loading…
Reference in New Issue