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

undo changes

release/4.3a0
Varun Agrawal 2022-09-17 08:05:07 -04:00
parent 9ef5c184ec
commit 12db5dd947
1 changed files with 3 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -62,9 +62,9 @@ void HybridGaussianISAM::updateInternal(
for (const sharedClique& orphan : *orphans) for (const sharedClique& orphan : *orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Node> >(orphan); factors += boost::make_shared<BayesTreeOrphanWrapper<Node> >(orphan);
// Get all the discrete keys from the new factors // Get all the discrete keys from the factors
KeySet allDiscrete; KeySet allDiscrete;
for (auto& factor : newFactors) { for (auto& factor : factors) {
for (auto& k : factor->discreteKeys()) { for (auto& k : factor->discreteKeys()) {
allDiscrete.insert(k.first); allDiscrete.insert(k.first);
} }
@ -83,7 +83,7 @@ void HybridGaussianISAM::updateInternal(
std::back_inserter(newKeysDiscreteLast)); std::back_inserter(newKeysDiscreteLast));
// Get an ordering where the new keys are eliminated last // Get an ordering where the new keys are eliminated last
const VariableIndex index(newFactors); const VariableIndex index(factors);
Ordering elimination_ordering; Ordering elimination_ordering;
if (ordering) { if (ordering) {
@ -95,14 +95,10 @@ void HybridGaussianISAM::updateInternal(
true); true);
} }
GTSAM_PRINT(elimination_ordering);
std::cout << "\n\n\n\neliminateMultifrontal" << std::endl;
GTSAM_PRINT(factors);
// eliminate all factors (top, added, orphans) into a new Bayes tree // eliminate all factors (top, added, orphans) into a new Bayes tree
HybridBayesTree::shared_ptr bayesTree = HybridBayesTree::shared_ptr bayesTree =
factors.eliminateMultifrontal(elimination_ordering, function, index); factors.eliminateMultifrontal(elimination_ordering, function, index);
std::cout << "optionally prune" << std::endl;
if (maxNrLeaves) { if (maxNrLeaves) {
bayesTree->prune(*maxNrLeaves); bayesTree->prune(*maxNrLeaves);
} }