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

Fixed bug in BayesTree/iSAM2 clone

release/4.3a0
Richard Roberts 2012-01-15 20:31:21 +00:00
parent 500d509f81
commit 7a79edca4b
2 changed files with 31 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/inference/BayesTree-inl.h
View File

@ -152,20 +152,29 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, sharedClique parent_clique) {
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, conditional->frontals())
nodes_[key] = new_clique;
if (parent_clique != NULL) {
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
}
new_clique->assertInvariants();
addClique(new_clique, parent_clique);
return new_clique;
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, (*clique)->frontals())
nodes_[key] = clique;
if (parent_clique != NULL) {
clique->parent_ = parent_clique;
parent_clique->children_.push_back(clique);
} else {
assert(!root_);
root_ = clique;
}
clique->assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, list<sharedClique>& child_cliques) {

26
gtsam/inference/BayesTree.h
View File

@ -100,8 +100,6 @@ namespace gtsam {
/** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const;
protected:
/** Root clique */
sharedClique root_;
@ -109,12 +107,13 @@ namespace gtsam {
void removeClique(sharedClique clique);
/** add a clique (top down) */
sharedClique addClique(const sharedConditional& conditional,
sharedClique parent_clique = sharedClique());
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional,
std::list<sharedClique>& child_cliques);
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
@ -179,20 +178,19 @@ namespace gtsam {
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
void cloneTo(shared_ptr& newTree) const {
cloneTo(newTree, root());
cloneTo(newTree, root(), sharedClique());
}
private:
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree, const sharedClique& root) const {
if(root) {
sharedClique newClique = root->clone();
newTree->insert(newClique);
BOOST_FOREACH(const sharedClique& childClique, root->children()) {
cloneTo(newTree, childClique);
}
void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree->addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
public:
/**