12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

optional maxNrLeaves for HybridGaussianISAM

release/4.3a0
Varun Agrawal 2022-09-16 10:26:25 -04:00
parent 8b5586f3b6
commit dcad55c032
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -42,6 +42,7 @@ HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree)
void HybridGaussianISAM::updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
// Remove the contaminated part of the Bayes tree
@ -92,6 +93,10 @@ void HybridGaussianISAM::updateInternal(
HybridBayesTree::shared_ptr bayesTree =
factors.eliminateMultifrontal(elimination_ordering, function, index);
if (maxNrLeaves) {
bayesTree->prune(*maxNrLeaves);
}
// Re-add into Bayes tree data structures
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
bayesTree->roots().end());
@ -100,10 +105,11 @@ void HybridGaussianISAM::updateInternal(
/* ************************************************************************* */
void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves,
const boost::optional<Ordering>& ordering,
const HybridBayesTree::Eliminate& function) {
Cliques orphans;
this->updateInternal(newFactors, &orphans, ordering, function);
this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function);
}
} // namespace gtsam

11
gtsam/hybrid/HybridGaussianISAM.h
View File

@ -48,6 +48,7 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
void updateInternal(
const HybridGaussianFactorGraph& newFactors,
HybridBayesTree::Cliques* orphans,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
@ -57,19 +58,15 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
* @brief Perform update step with new factors.
*
* @param newFactors Factor graph of new factors to add and eliminate.
* @param maxNrLeaves The maximum number of leaves to keep after pruning.
* @param ordering Custom elimination ordering.
* @param function Elimination function.
*/
void update(const HybridGaussianFactorGraph& newFactors,
const boost::optional<size_t>& maxNrLeaves = boost::none,
const boost::optional<Ordering>& ordering = boost::none,
const HybridBayesTree::Eliminate& function =
HybridBayesTree::EliminationTraitsType::DefaultEliminate);
/**
* @brief Prune the underlying Bayes tree.
*
* @param maxNumberLeaves The max number of leaf nodes to keep.
*/
void prune(const size_t maxNumberLeaves);
};
/// traits