diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 3e06d98ae..3f429198e 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -24,17 +24,14 @@ namespace gtsam { /* ************************************************************************* */ -Ordering HybridSmoother::getOrdering( - const HybridGaussianFactorGraph &newFactors) { - HybridGaussianFactorGraph factors(hybridBayesNet()); - factors.push_back(newFactors); - +Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors, + const KeySet &newFactorKeys) { // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeySet(); // Create KeyVector with continuous keys followed by discrete keys. KeyVector newKeysDiscreteLast; - const KeySet newFactorKeys = newFactors.keys(); + // Insert continuous keys first. for (auto &k : newFactorKeys) { if (!allDiscrete.exists(k)) { @@ -56,22 +53,29 @@ Ordering HybridSmoother::getOrdering( } /* ************************************************************************* */ -void HybridSmoother::update(HybridGaussianFactorGraph graph, +void HybridSmoother::update(const HybridGaussianFactorGraph &graph, std::optional maxNrLeaves, const std::optional given_ordering) { + HybridGaussianFactorGraph updatedGraph; + // Add the necessary conditionals from the previous timestep(s). + std::tie(updatedGraph, hybridBayesNet_) = + addConditionals(graph, hybridBayesNet_); + Ordering ordering; // If no ordering provided, then we compute one if (!given_ordering.has_value()) { - ordering = this->getOrdering(graph); + // Get the keys from the new factors + const KeySet newFactorKeys = graph.keys(); + + // Since updatedGraph now has all the connected conditionals, + // we can get the correct ordering. + ordering = this->getOrdering(updatedGraph, newFactorKeys); } else { ordering = *given_ordering; } - // Add the necessary conditionals from the previous timestep(s). - std::tie(graph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_); - // Eliminate. - HybridBayesNet bayesNetFragment = *graph.eliminateSequential(ordering); + HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); /// Prune if (maxNrLeaves) { diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 4669b1d8f..f941385bd 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -49,11 +49,35 @@ class GTSAM_EXPORT HybridSmoother { * @param given_ordering The (optional) ordering for elimination, only * continuous variables are allowed */ - void update(HybridGaussianFactorGraph graph, + void update(const HybridGaussianFactorGraph& graph, std::optional maxNrLeaves = {}, const std::optional given_ordering = {}); - Ordering getOrdering(const HybridGaussianFactorGraph& newFactors); + /** + * @brief Get an elimination ordering which eliminates continuous and then + * discrete. + * + * Expects `newFactors` to already have the necessary conditionals connected + * to the + * + * @param factors + * @return Ordering + */ + + /** + * @brief Get an elimination ordering which eliminates continuous + * and then discrete. + * + * Expects `factors` to already have the necessary conditionals + * which were connected to the variables in the newly added factors. + * Those variables should be in `newFactorKeys`. + * + * @param factors All the new factors and connected conditionals. + * @param newFactorKeys The keys/variables in the newly added factors. + * @return Ordering + */ + Ordering getOrdering(const HybridGaussianFactorGraph& factors, + const KeySet& newFactorKeys); /** * @brief Add conditionals from previous timestep as part of liquefication.