Merge pull request #32 from varunagrawal/fix/bug-conditional

gtsam/hybrid/IncrementalHybrid.cpp

@@ -34,37 +34,42 @@ void IncrementalHybrid::update(GaussianHybridFactorGraph graph,
     // in the previous `hybridBayesNet` to the graph
     std::unordered_set<Key> allVars(ordering.begin(), ordering.end());
 
+    // Conditionals to remove from the bayes net
+    // since the conditional will be updated.
+    std::vector<AbstractConditional::shared_ptr> conditionals_to_erase;
+
     // TODO(Varun) Using a for-range loop doesn't work since some of the
     // conditionals are invalid pointers
     for (size_t i = 0; i < hybridBayesNet_.size(); i++) {
       auto conditional = hybridBayesNet_.at(i);
-      // Flag indicating if a conditional will be updated due to factors in
-      // `graph`
-      bool marked_for_update = false;
 
       for (auto &key : conditional->frontals()) {
         if (allVars.find(key) != allVars.end()) {
           if (auto gf =
                   boost::dynamic_pointer_cast<GaussianMixture>(conditional)) {
             graph.push_back(gf);
-            marked_for_update = true;
+
+            conditionals_to_erase.push_back(conditional);
+
           } else if (auto df = boost::dynamic_pointer_cast<DiscreteConditional>(
                          conditional)) {
             graph.push_back(df);
-            marked_for_update = true;
-          }
 
-          // If a conditional is due to be updated, we remove if from the
-          // previous bayes net.
-          if (marked_for_update) {
-            auto it = find(hybridBayesNet_.begin(), hybridBayesNet_.end(),
-                           conditional);
-            hybridBayesNet_.erase(it);
+            conditionals_to_erase.push_back(conditional);
           }
+
           break;
         }
       }
     }
+
+    // Remove conditionals at the end so we don't affect the order in the
+    // original bayes net.
+    for (auto &&conditional : conditionals_to_erase) {
+      auto it =
+          find(hybridBayesNet_.begin(), hybridBayesNet_.end(), conditional);
+      hybridBayesNet_.erase(it);
+    }
   }
 
   // Eliminate partially.