diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 12de2c14f..8298e813a 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -61,6 +61,40 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, return ordering; } +/* ************************************************************************* */ +Ordering HybridSmoother::maybeComputeOrdering( + const HybridGaussianFactorGraph &updatedGraph, + const std::optional givenOrdering) { + Ordering ordering; + // If no ordering provided, then we compute one + if (!givenOrdering.has_value()) { + // Get the keys from the new factors + 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. + ordering = this->getOrdering(updatedGraph, continuousKeysToInclude); + } else { + ordering = *givenOrdering; + } + + return ordering; +} + +/* ************************************************************************* */ +void HybridSmoother::removeFixedValues( + const HybridGaussianFactorGraph &newFactors) { + for (Key key : newFactors.discreteKeySet()) { + if (fixedValues_.find(key) != fixedValues_.end()) { + fixedValues_.erase(key); + } + } +} + /* ************************************************************************* */ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, std::optional maxNrLeaves, @@ -84,22 +118,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, << std::endl; #endif - Ordering ordering; - // If no ordering provided, then we compute one - if (!given_ordering.has_value()) { - // Get the keys from the new factors - 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. - ordering = this->getOrdering(updatedGraph, continuousKeysToInclude); - } else { - ordering = *given_ordering; - } + Ordering ordering = this->maybeComputeOrdering(updatedGraph, given_ordering); #if GTSAM_HYBRID_TIMING gttic_(HybridSmootherEliminate); @@ -119,11 +138,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, #endif // Remove fixed values for discrete keys which are introduced in newFactors - for (Key key : newFactors.discreteKeySet()) { - if (fixedValues_.find(key) != fixedValues_.end()) { - fixedValues_.erase(key); - } - } + removeFixedValues(newFactors); #ifdef DEBUG_SMOOTHER // Print discrete keys in the bayesNetFragment: diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 53d058036..ef75b06b5 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -122,6 +122,14 @@ class GTSAM_EXPORT HybridSmoother { /// Optimize the hybrid Bayes Net, taking into accound fixed values. HybridValues optimize() const; + + private: + /// Helper to compute the ordering if ordering is not given. + Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph, + const std::optional givenOrdering); + + /// Remove fixed discrete values for discrete keys introduced in `newFactors`. + void removeFixedValues(const HybridGaussianFactorGraph& newFactors); }; } // namespace gtsam