diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 503afaa72..37a556096 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { - // TODO(Varun) Re-linearization doesn't take into account pruning reorderRelinearize(); reorderCounter_ = 0; } @@ -65,8 +65,22 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); + auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + isam_.clear(); + // Prune nonlinear factors based on discrete conditional probabilities + HybridNonlinearFactorGraph pruned_factors; + for (auto&& factor : factors_) { + if (auto nf = std::dynamic_pointer_cast(factor)) { + pruned_factors.push_back(nf->prune(discreteProbs)); + } else { + pruned_factors.push_back(factor); + } + } + factors_ = pruned_factors; + factors_.print("OG factors"); + // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple re-linearization and reordering to avoid