Refactor elimination setup to not use C declaration style

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -344,18 +344,20 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
   // However this is also the case with iSAM2, so no pressure :)
 
   // PREPROCESS: Identify the nature of the current elimination
-  std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
-  std::set<DiscreteKey> discreteSeparatorSet;
-  std::set<DiscreteKey> discreteFrontals;
 
+  // First, identify the separator keys, i.e. all keys that are not frontal.
   KeySet separatorKeys;
-  KeySet allContinuousKeys;
-  KeySet continuousFrontals;
-  KeySet continuousSeparator;
-
-  // This initializes separatorKeys and mapFromKeyToDiscreteKey
   for (auto &&factor : factors) {
     separatorKeys.insert(factor->begin(), factor->end());
+  }
+  // remove frontals from separator
+  for (auto &k : frontalKeys) {
+    separatorKeys.erase(k);
+  }
+
+  // Build a map from keys to DiscreteKeys
+  std::unordered_map<Key, DiscreteKey> mapFromKeyToDiscreteKey;
+  for (auto &&factor : factors) {
     if (!factor->isContinuous()) {
       for (auto &k : factor->discreteKeys()) {
         mapFromKeyToDiscreteKey[k.first] = k;
@@ -363,49 +365,50 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors,
     }
   }
 
-  // remove frontals from separator
+  // Fill in discrete frontals and continuous frontals.
-  for (auto &k : frontalKeys) {
+  std::set<DiscreteKey> discreteFrontals;
-    separatorKeys.erase(k);
+  KeySet continuousFrontals;
-  }
-
-  // Fill in discrete frontals and continuous frontals for the end result
   for (auto &k : frontalKeys) {
     if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
       discreteFrontals.insert(mapFromKeyToDiscreteKey.at(k));
     } else {
       continuousFrontals.insert(k);
-      allContinuousKeys.insert(k);
     }
   }
 
-  // Fill in discrete frontals and continuous frontals for the end result
+  // Fill in discrete discrete separator keys and continuous separator keys.
+  std::set<DiscreteKey> discreteSeparatorSet;
+  KeySet continuousSeparator;
   for (auto &k : separatorKeys) {
     if (mapFromKeyToDiscreteKey.find(k) != mapFromKeyToDiscreteKey.end()) {
       discreteSeparatorSet.insert(mapFromKeyToDiscreteKey.at(k));
     } else {
       continuousSeparator.insert(k);
-      allContinuousKeys.insert(k);
     }
   }
 
+  // Check if we have any continuous keys:
+  const bool discrete_only =
+      continuousFrontals.empty() && continuousSeparator.empty();
+
   // NOTE: We should really defer the product here because of pruning
 
-  // Case 1: we are only dealing with continuous
+  if (discrete_only) {
-  if (mapFromKeyToDiscreteKey.empty() && !allContinuousKeys.empty()) {
+    // Case 1: we are only dealing with discrete
-    return continuousElimination(factors, frontalKeys);
-  }
-
-  // Case 2: we are only dealing with discrete
-  if (allContinuousKeys.empty()) {
     return discreteElimination(factors, frontalKeys);
-  }
+  } else {
-
+    // Case 2: we are only dealing with continuous
+    if (mapFromKeyToDiscreteKey.empty()) {
+      return continuousElimination(factors, frontalKeys);
+    } else {
+      // Case 3: We are now in the hybrid land!
 #ifdef HYBRID_TIMING
-  tictoc_reset_();
+      tictoc_reset_();
 #endif
-  // Case 3: We are now in the hybrid land!
+      return hybridElimination(factors, frontalKeys, continuousSeparator,
-  return hybridElimination(factors, frontalKeys, continuousSeparator,
+                               discreteSeparatorSet);
-                           discreteSeparatorSet);
+    }
+  }
 }
 
 /* ************************************************************************ */