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

borglab · Sep 13, 2022 · 0909c46 · 0909c46
2 parents 3e25e7d + 8b5b42b
commit 0909c46
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 7 deletions.
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -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);
 

diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h
@@ -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;

diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
@@ -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
 }
 
 /****************************************************************************