diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index cfe5c4b597..b0bd171311 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -50,7 +50,7 @@ #include #include -#define GTSAM_HYBRID_WITH_TABLEFACTOR 0 +#define GTSAM_HYBRID_WITH_TABLEFACTOR 1 namespace gtsam { diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index b66b63ea67..51fd1f1dbc 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -26,7 +26,7 @@ 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(); @@ -34,7 +34,7 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, 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.