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