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

prune nonlinear factors in HybridNonlinearISAM

release/4.3a0
Varun Agrawal 2024-10-29 10:03:23 -04:00
parent b4c7d3af81
commit 649da80c91
1 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/hybrid/HybridNonlinearISAM.cpp
View File

@ -16,6 +16,7 @@
*/
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/inference/Ordering.h>
@ -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<HybridNonlinearFactor>(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