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

Re-factored and re-formatted, incorporated Duy's fix (don't de-reference bayesTree)

release/4.3a0
dellaert 2017-08-07 21:35:24 -07:00
parent c82fe1fde2
commit 3635380be4
1 changed files with 40 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/inference/ISAM-inst.h
View File

@ -22,43 +22,48 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE> template<class BAYESTREE>
void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
{ Cliques& orphans, const Eliminate& function) {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNetType bn; BayesNetType bn;
if (!this->empty()) { const KeySet newFactorKeys = newFactors.keys();
const KeySet newFactorKeys = newFactors.keys(); if (!this->empty()) {
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
} this->removeTop(keyVector, bn, orphans);
// Add the removed top and the new factors
FactorGraphType factors;
factors += bn;
factors += newFactors;
// Add the orphaned subtrees
for(const sharedClique& orphan: orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
// eliminate into a Bayes net
const VariableIndex varIndex(factors);
const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering =
Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
} }
/* ************************************************************************* */ // Add the removed top and the new factors
template<class BAYESTREE> FactorGraphType factors;
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function) factors += bn;
{ factors += newFactors;
Cliques orphans;
this->update_internal(newFactors, orphans, function); // Add the orphaned subtrees
} for (const sharedClique& orphan : orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
// Get an ordering where the new keys are eliminated last
const VariableIndex index(factors);
const Ordering ordering = Ordering::ColamdConstrainedLast(index,
std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
// eliminate all factors (top, added, orphans) into a new Bayes tree
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
// Re-add into Bayes tree data structures
this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
bayesTree->roots().end());
this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
}
/* ************************************************************************* */
template<class BAYESTREE>
void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
const Eliminate& function) {
Cliques orphans;
this->update_internal(newFactors, orphans, function);
}
} }
/// namespace gtsam /// namespace gtsam