diff --git a/gtsam/hybrid/HybridISAM.cpp b/gtsam/hybrid/HybridISAM.cpp index f743b92f1..057f784ad 100644 --- a/gtsam/hybrid/HybridISAM.cpp +++ b/gtsam/hybrid/HybridISAM.cpp @@ -36,6 +36,7 @@ HybridISAM::HybridISAM() {} /* ************************************************************************* */ HybridISAM::HybridISAM(const HybridBayesTree& bayesTree) : Base(bayesTree) {} +/* ************************************************************************* */ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function) { @@ -79,8 +80,6 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), true); - ordering.print("ORD"); - // eliminate all factors (top, added, orphans) into a new Bayes tree auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); @@ -90,6 +89,7 @@ void HybridISAM::updateInternal(const HybridFactorGraph& newFactors, this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); } +/* ************************************************************************* */ void HybridISAM::update(const HybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function) { Cliques orphans; diff --git a/gtsam/hybrid/HybridISAM.h b/gtsam/hybrid/HybridISAM.h index 7f9e77b8b..0738f46c8 100644 --- a/gtsam/hybrid/HybridISAM.h +++ b/gtsam/hybrid/HybridISAM.h @@ -43,11 +43,20 @@ class GTSAM_EXPORT HybridISAM : public ISAM { /// @} + private: + /// Internal method that performs the ISAM update. void updateInternal( const HybridFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); + public: + /** + * @brief Perform update step with new factors. + * + * @param newFactors Factor graph of new factors to add and eliminate. + * @param function Elimination function. + */ void update(const HybridFactorGraph& newFactors, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate);