prune nonlinear factors in HybridNonlinearISAM
parent
b4c7d3af81
commit
649da80c91
|
@ -16,6 +16,7 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
|
||||||
|
#include <gtsam/hybrid/HybridNonlinearFactor.h>
|
||||||
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
#include <gtsam/hybrid/HybridNonlinearISAM.h>
|
||||||
#include <gtsam/inference/Ordering.h>
|
#include <gtsam/inference/Ordering.h>
|
||||||
|
|
||||||
|
@ -39,7 +40,6 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors,
|
||||||
if (newFactors.size() > 0) {
|
if (newFactors.size() > 0) {
|
||||||
// Reorder and relinearize every reorderInterval updates
|
// Reorder and relinearize every reorderInterval updates
|
||||||
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
|
if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
|
||||||
// TODO(Varun) Re-linearization doesn't take into account pruning
|
|
||||||
reorderRelinearize();
|
reorderRelinearize();
|
||||||
reorderCounter_ = 0;
|
reorderCounter_ = 0;
|
||||||
}
|
}
|
||||||
|
@ -65,8 +65,22 @@ void HybridNonlinearISAM::reorderRelinearize() {
|
||||||
// Obtain the new linearization point
|
// Obtain the new linearization point
|
||||||
const Values newLinPoint = estimate();
|
const Values newLinPoint = estimate();
|
||||||
|
|
||||||
|
auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete());
|
||||||
|
|
||||||
isam_.clear();
|
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
|
// Just recreate the whole BayesTree
|
||||||
// TODO: allow for constrained ordering here
|
// TODO: allow for constrained ordering here
|
||||||
// TODO: decouple re-linearization and reordering to avoid
|
// TODO: decouple re-linearization and reordering to avoid
|
||||||
|
|
Loading…
Reference in New Issue