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

Fixed confusing variable naming, e.g. Index key -> Index j

release/4.3a0
Frank Dellaert 2012-09-08 04:21:21 +00:00
parent c67966718a
commit 3e8544880a
2 changed files with 50 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -167,8 +167,8 @@ namespace gtsam {
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) { void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, (*clique)->frontals()) BOOST_FOREACH(Index j, (*clique)->frontals())
nodes_[key] = clique; nodes_[j] = clique;
if (parent_clique != NULL) { if (parent_clique != NULL) {
clique->parent_ = parent_clique; clique->parent_ = parent_clique;
parent_clique->children_.push_back(clique); parent_clique->children_.push_back(clique);
@ -185,8 +185,8 @@ namespace gtsam {
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) { const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index key, conditional->frontals()) BOOST_FOREACH(Index j, conditional->frontals())
nodes_[key] = new_clique; nodes_[j] = new_clique;
new_clique->children_ = child_cliques; new_clique->children_ = child_cliques;
BOOST_FOREACH(sharedClique& child, child_cliques) BOOST_FOREACH(sharedClique& child, child_cliques)
child->parent_ = new_clique; child->parent_ = new_clique;
@ -210,13 +210,13 @@ namespace gtsam {
#endif #endif
if(debug) conditional->print("Adding conditional "); if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique "); if(debug) clique->print("To clique ");
Index key = conditional->lastFrontalKey(); Index j = conditional->lastFrontalKey();
bayesTree.nodes_.resize(std::max(key+1, bayesTree.nodes_.size())); bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size()));
bayesTree.nodes_[key] = clique; bayesTree.nodes_[j] = clique;
FastVector<Index> newKeys((*clique)->size() + 1); FastVector<Index> newIndices((*clique)->size() + 1);
newKeys[0] = key; newIndices[0] = j;
std::copy((*clique)->begin(), (*clique)->end(), newKeys.begin()+1); std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1);
clique->conditional_ = CONDITIONAL::FromKeys(newKeys, (*clique)->nrFrontals() + 1); clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is "); if(debug) clique->print("Expanded clique is ");
clique->assertInvariants(); clique->assertInvariants();
} }
@ -234,8 +234,8 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_) BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename Clique::weak_ptr(); child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Index key, (*clique->conditional())) { BOOST_FOREACH(Index j, (*clique->conditional())) {
nodes_[key].reset(); nodes_[j].reset();
} }
} }
@ -379,7 +379,7 @@ namespace gtsam {
{ {
static const bool debug = false; static const bool debug = false;
// get key and parents // get indices and parents
const typename CONDITIONAL::Parents& parents = conditional->parents(); const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional "); if(debug) conditional->print("Adding conditional ");
@ -402,7 +402,7 @@ namespace gtsam {
if ((*parent_clique)->size() == size_t(parents.size())) { if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) std::cout << "Adding to parent clique" << std::endl; if(debug) std::cout << "Adding to parent clique" << std::endl;
#ifndef NDEBUG #ifndef NDEBUG
// Debug check that the parent keys of the new conditional match the keys // Debug check that the parent indices of the new conditional match the indices
// currently in the clique. // currently in the clique.
// list<Index>::const_iterator parent = parents.begin(); // list<Index>::const_iterator parent = parents.begin();
// typename Clique::const_iterator cond = parent_clique->begin(); // typename Clique::const_iterator cond = parent_clique->begin();
@ -441,7 +441,7 @@ namespace gtsam {
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) { void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node // Add each frontal variable of this root node
BOOST_FOREACH(const Index& key, subtree->conditional()->frontals()) { nodes_[key] = subtree; } BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child // Fill index for each child
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique; typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children_) { BOOST_FOREACH(const sharedClique& child, subtree->children_) {
@ -480,26 +480,26 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor( typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor(
Index key, Eliminate function) const { Index j, Eliminate function) const {
// get clique containing key // get clique containing Index j
sharedClique clique = (*this)[key]; sharedClique clique = (*this)[j];
// calculate or retrieve its marginal // calculate or retrieve its marginal
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function); FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
return GenericSequentialSolver<FactorType>(cliqueMarginal).marginalFactor( return GenericSequentialSolver<FactorType>(cliqueMarginal).marginalFactor(
key, function); j, function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet( typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet(
Index key, Eliminate function) const { Index j, Eliminate function) const {
// calculate marginal as a factor graph // calculate marginal as a factor graph
FactorGraph<FactorType> fg; FactorGraph<FactorType> fg;
fg.push_back(this->marginalFactor(key,function)); fg.push_back(this->marginalFactor(j,function));
// eliminate factor graph marginal to a Bayes net // eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<FactorType>(fg).eliminate(function); return GenericSequentialSolver<FactorType>(fg).eliminate(function);
@ -510,28 +510,28 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
BayesTree<CONDITIONAL,CLIQUE>::joint(Index key1, Index key2, Eliminate function) const { BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
// get clique C1 and C2 // get clique C1 and C2
sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
// calculate joint // calculate joint
FactorGraph<FactorType> p_C1C2(C1->joint(C2, root_, function)); FactorGraph<FactorType> p_C1C2(C1->joint(C2, root_, function));
// eliminate remaining factor graph to get requested joint // eliminate remaining factor graph to get requested joint
std::vector<Index> key12(2); key12[0] = key1; key12[1] = key2; std::vector<Index> j12(2); j12[0] = j1; j12[1] = j2;
GenericSequentialSolver<FactorType> solver(p_C1C2); GenericSequentialSolver<FactorType> solver(p_C1C2);
return solver.jointFactorGraph(key12,function); return solver.jointFactorGraph(j12,function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet( typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet(
Index key1, Index key2, Eliminate function) const { Index j1, Index j2, Eliminate function) const {
// eliminate factor graph marginal to a Bayes net // eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<FactorType> ( return GenericSequentialSolver<FactorType> (
*this->joint(key1, key2, function)).eliminate(function); *this->joint(j1, j2, function)).eliminate(function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -574,11 +574,11 @@ namespace gtsam {
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) { BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Index& key, keys) { BOOST_FOREACH(const Index& j, keys) {
// get the clique // get the clique
if(key < nodes_.size()) { if(j < nodes_.size()) {
const sharedClique& clique(nodes_[key]); const sharedClique& clique(nodes_[j]);
if(clique) { if(clique) {
// remove path from clique to root // remove path from clique to root
this->removePath(clique, bn, orphans); this->removePath(clique, bn, orphans);

34
gtsam/inference/BayesTree.h
View File

@ -91,9 +91,17 @@ namespace gtsam {
CliqueStats getStats() const; CliqueStats getStats() const;
}; };
/** Map from keys to Clique */ /** Map from indices to Clique */
typedef std::deque<sharedClique> Nodes; typedef std::deque<sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root clique */
sharedClique root_;
public: public:
/// @name Standard Constructors /// @name Standard Constructors
@ -160,29 +168,29 @@ namespace gtsam {
/** return root clique */ /** return root clique */
const sharedClique& root() const { return root_; } const sharedClique& root() const { return root_; }
/** find the clique to which key belongs */ /** find the clique that contains the variable with Index j */
sharedClique operator[](Index key) const { sharedClique operator[](Index j) const {
return nodes_.at(key); return nodes_.at(j);
} }
/** Gather data on all cliques */ /** Gather data on all cliques */
CliqueData getCliqueData() const; CliqueData getCliqueData() const;
/** return marginal on any variable */ /** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index key, Eliminate function) const; typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
/** /**
* return marginal on any variable, as a Bayes Net * return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net * NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function * This is more expensive than the above function
*/ */
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key, Eliminate function) const; typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
/** return joint on two variables */ /** return joint on two variables */
typename FactorGraph<FactorType>::shared_ptr joint(Index key1, Index key2, Eliminate function) const; typename FactorGraph<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
/** return joint on two variables as a BayesNet */ /** return joint on two variables as a BayesNet */
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1, Index key2, Eliminate function) const; typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
/** /**
* Read only with side effects * Read only with side effects
@ -211,11 +219,11 @@ namespace gtsam {
void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans); void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
/** /**
* Given a list of keys, turn "contaminated" part of the tree back into a factor graph. * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments. * Factors and orphans are added to the in/out arguments.
*/ */
template<class CONTAINER> template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans); void removeTop(const CONTAINER& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
/** /**
* Hang a new subtree off of the existing tree. This finds the appropriate * Hang a new subtree off of the existing tree. This finds the appropriate
@ -243,9 +251,6 @@ namespace gtsam {
protected: protected:
/** Map from keys to Clique */
Nodes nodes_;
/** private helper method for saving the Tree to a text file in GraphViz format */ /** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
int parentnum = 0) const; int parentnum = 0) const;
@ -253,9 +258,6 @@ namespace gtsam {
/** Gather data on a single clique */ /** Gather data on a single clique */
void getCliqueData(CliqueData& stats, sharedClique clique) const; void getCliqueData(CliqueData& stats, sharedClique clique) const;
/** Root clique */
sharedClique root_;
/** remove a clique: warning, can result in a forest */ /** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique); void removeClique(sharedClique clique);