Play with different elimination schemes

gtsam/hybrid/HybridGaussianFactorGraph.cpp

@@ -50,7 +50,7 @@
 #include <utility>
 #include <vector>
 
-#define GTSAM_HYBRID_WITH_TABLEFACTOR 0
+#define GTSAM_HYBRID_WITH_TABLEFACTOR 1
 
 namespace gtsam {
 

gtsam/hybrid/HybridSmoother.cpp

@@ -26,15 +26,15 @@ namespace gtsam {
 
 /* ************************************************************************* */
 Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
-                                     const KeySet &continuousKeys) {
+                                     const KeySet &lastKeysToEliminate) {
   // Get all the discrete keys from the factors
   KeySet allDiscrete = factors.discreteKeySet();
 
   // Create KeyVector with continuous keys followed by discrete keys.
   KeyVector lastKeys;
 
   // Insert continuous keys first.
-  for (auto &k : continuousKeys) {
+  for (auto &k : lastKeysToEliminate) {
     if (!allDiscrete.exists(k)) {
       lastKeys.push_back(k);
     }
@@ -44,11 +44,10 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
   std::copy(allDiscrete.begin(), allDiscrete.end(),
             std::back_inserter(lastKeys));
 
-  const VariableIndex index(factors);
-
   // Get an ordering where the new keys are eliminated last
   Ordering ordering = Ordering::ColamdConstrainedLast(
-      index, KeyVector(lastKeys.begin(), lastKeys.end()), true);
+      factors, KeyVector(lastKeys.begin(), lastKeys.end()), true);
+
   return ordering;
 }
 
@@ -79,7 +78,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
   // If no ordering provided, then we compute one
   if (!given_ordering.has_value()) {
     // Get the keys from the new factors
-    const KeySet continuousKeysToInclude;// = newFactors.keys();
+    KeySet continuousKeysToInclude;  // Scheme 1: empty, 15sec/2000, 64sec/3000 (69s without TF)
+    // continuousKeysToInclude = newFactors.keys();  // Scheme 2: all, 8sec/2000, 160sec/3000
+    // continuousKeysToInclude = updatedGraph.keys();  // Scheme 3: all, stopped after 80sec/2000
 
     // Since updatedGraph now has all the connected conditionals,
     // we can get the correct ordering.