diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp new file mode 100644 index 000000000..585ca5309 --- /dev/null +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridSmoother.cpp + * @brief An incremental smoother for hybrid factor graphs + * @author Varun Agrawal + * @date October 2022 + */ + +#include + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void HybridSmoother::update(HybridGaussianFactorGraph graph, + const Ordering &ordering, + boost::optional maxNrLeaves) { + // Add the necessary conditionals from the previous timestep(s). + std::tie(graph, hybridBayesNet_) = + addConditionals(graph, hybridBayesNet_, ordering); + + // Eliminate. + auto bayesNetFragment = graph.eliminateSequential(ordering); + + /// Prune + if (maxNrLeaves) { + // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in + // all the conditionals with the same keys in bayesNetFragment. + HybridBayesNet prunedBayesNetFragment = + bayesNetFragment->prune(*maxNrLeaves); + // Set the bayes net fragment to the pruned version + bayesNetFragment = + boost::make_shared(prunedBayesNetFragment); + } + + // Add the partial bayes net to the posterior bayes net. + hybridBayesNet_.push_back(*bayesNetFragment); + + tictoc_print_(); +} + +/* ************************************************************************* */ +std::pair +HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, + const HybridBayesNet &originalHybridBayesNet, + const Ordering &ordering) const { + HybridGaussianFactorGraph graph(originalGraph); + HybridBayesNet hybridBayesNet(originalHybridBayesNet); + + // If we are not at the first iteration, means we have conditionals to add. + if (!hybridBayesNet.empty()) { + // We add all relevant conditional mixtures on the last continuous variable + // in the previous `hybridBayesNet` to the graph + + // Conditionals to remove from the bayes net + // since the conditional will be updated. + std::vector conditionals_to_erase; + + // New conditionals to add to the graph + gtsam::HybridBayesNet newConditionals; + + // NOTE(Varun) Using a for-range loop doesn't work since some of the + // conditionals are invalid pointers + for (size_t i = 0; i < hybridBayesNet.size(); i++) { + auto conditional = hybridBayesNet.at(i); + + for (auto &key : conditional->frontals()) { + if (std::find(ordering.begin(), ordering.end(), key) != + ordering.end()) { + newConditionals.push_back(conditional); + conditionals_to_erase.push_back(conditional); + + break; + } + } + } + // Remove conditionals at the end so we don't affect the order in the + // original bayes net. + for (auto &&conditional : conditionals_to_erase) { + auto it = find(hybridBayesNet.begin(), hybridBayesNet.end(), conditional); + hybridBayesNet.erase(it); + } + + graph.push_back(newConditionals); + // newConditionals.print("\n\n\nNew Conditionals to add back"); + } + return {graph, hybridBayesNet}; +} + +/* ************************************************************************* */ +GaussianMixture::shared_ptr HybridSmoother::gaussianMixture( + size_t index) const { + return boost::dynamic_pointer_cast( + hybridBayesNet_.at(index)); +} + +/* ************************************************************************* */ +const HybridBayesNet &HybridSmoother::hybridBayesNet() const { + return hybridBayesNet_; +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h new file mode 100644 index 000000000..7e90f9425 --- /dev/null +++ b/gtsam/hybrid/HybridSmoother.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridSmoother.h + * @brief An incremental smoother for hybrid factor graphs + * @author Varun Agrawal + * @date October 2022 + */ + +#include +#include +#include + +namespace gtsam { + +class HybridSmoother { + private: + HybridBayesNet hybridBayesNet_; + HybridGaussianFactorGraph remainingFactorGraph_; + + public: + /** + * Given new factors, perform an incremental update. + * The relevant densities in the `hybridBayesNet` will be added to the input + * graph (fragment), and then eliminated according to the `ordering` + * presented. The remaining factor graph contains Gaussian mixture factors + * that are not connected to the variables in the ordering, or a single + * discrete factor on all discrete keys, plus all discrete factors in the + * original graph. + * + * \note If maxComponents is given, we look at the discrete factor resulting + * from this elimination, and prune it and the Gaussian components + * corresponding to the pruned choices. + * + * @param graph The new factors, should be linear only + * @param ordering The ordering for elimination, only continuous vars are + * allowed + * @param maxNrLeaves The maximum number of leaves in the new discrete factor, + * if applicable + */ + void update(HybridGaussianFactorGraph graph, const Ordering& ordering, + boost::optional maxNrLeaves = boost::none); + + /** + * @brief Add conditionals from previous timestep as part of liquefication. + * + * @param graph The new factor graph for the current time step. + * @param hybridBayesNet The hybrid bayes net containing all conditionals so + * far. + * @param ordering The elimination ordering. + * @return std::pair + */ + std::pair addConditionals( + const HybridGaussianFactorGraph& graph, + const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const; + + /// Get the Gaussian Mixture from the Bayes Net posterior at `index`. + GaussianMixture::shared_ptr gaussianMixture(size_t index) const; + + /// Return the Bayes Net posterior. + const HybridBayesNet& hybridBayesNet() const; +}; + +}; // namespace gtsam