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

incremental nonlinear working

release/4.3a0
Michael Kaess 2010-01-20 01:52:40 +00:00
parent b860f06614
commit ecde508318
3 changed files with 21 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cpp/BayesTree-inl.h
View File

@ -262,6 +262,20 @@ namespace gtsam {
equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),check_pair<Conditional>); equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),check_pair<Conditional>);
} }
/* ************************************************************************* */
template<class Conditional>
Symbol BayesTree<Conditional>::findParentClique(const list<Symbol>& parents, const list<Symbol>& ordering) const {
Symbol parent;
for (list<Symbol>::const_iterator it = ordering.begin(); it!=ordering.end(); it++) {
list<Symbol>::const_iterator pit = find(parents.begin(), parents.end(), *it);
if (pit!=parents.end()) {
parent = *pit;
break;
}
}
return parent;
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional> template<class Conditional>
void BayesTree<Conditional>::insert(const sharedConditional& conditional, const list<Symbol>* ordering) void BayesTree<Conditional>::insert(const sharedConditional& conditional, const list<Symbol>* ordering)
@ -281,13 +295,7 @@ namespace gtsam {
if (!ordering) { if (!ordering) {
parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated) parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated)
} else { } else {
for (list<Symbol>::const_iterator it = ordering->begin(); it!=ordering->end(); it++) { parent = findParentClique(parents, *ordering);
list<Symbol>::iterator pit = find(parents.begin(), parents.end(), *it);
if (pit!=parents.end()) {
parent = *pit;
break;
}
}
} }
sharedClique parent_clique = (*this)[parent]; sharedClique parent_clique = (*this)[parent];

3
cpp/BayesTree.h
View File

@ -128,6 +128,9 @@ namespace gtsam {
/** check equality */ /** check equality */
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const; bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
/** find parent clique of a conditional, given an ordering */
Symbol findParentClique(const std::list<Symbol>& parents, const std::list<Symbol>& ordering) const;
/** insert a new conditional */ /** insert a new conditional */
void insert(const sharedConditional& conditional, const std::list<Symbol>* ordering = NULL); void insert(const sharedConditional& conditional, const std::list<Symbol>* ordering = NULL);

5
cpp/ISAM2-inl.h
View File

@ -126,7 +126,7 @@ namespace gtsam {
FactorGraph<GaussianFactor> affectedFactors; FactorGraph<GaussianFactor> affectedFactors;
list<Symbol> newFactorsKeys = newFactors.keys(); list<Symbol> newFactorsKeys = newFactors.keys();
#if 0 #if 1
// relinearize all keys that are in newFactors, and already exist (not new variables!) // relinearize all keys that are in newFactors, and already exist (not new variables!)
list<Symbol> keysToRelinearize; list<Symbol> keysToRelinearize;
@ -237,7 +237,8 @@ namespace gtsam {
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
Symbol key = orphan->separator_.front(); // Symbol key = orphan->separator_.front();
Symbol key = findParentClique(orphan->separator_, ordering);
sharedClique parent = (*this)[key]; sharedClique parent = (*this)[key];
parent->children_ += orphan; parent->children_ += orphan;