diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 8ba80c07c..b0581fe73 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -78,7 +78,7 @@ namespace gtsam { { node_ptr new_clique(new Node(conditional)); nodes_.insert(make_pair(conditional->key(), new_clique)); - if (parent_clique!=NULL) { + if (parent_clique!=NULL) { new_clique->parent_ = parent_clique; parent_clique->children_.push_back(new_clique); } @@ -102,10 +102,7 @@ namespace gtsam { // otherwise, find the parent clique string parent = parents.front(); - typename Nodes::const_iterator it = nodes_.find(parent); - if (it == nodes_.end()) throw(invalid_argument( - "BayesTree::insert('"+key+"'): parent '" + parent + "' not yet inserted")); - node_ptr parent_clique = it->second; + node_ptr parent_clique = (*this)[parent]; // if the parents and parent clique have the same size, add to parent clique if (parent_clique->size() == parents.size()) { @@ -130,13 +127,8 @@ namespace gtsam { template boost::shared_ptr BayesTree::marginal(const string& key) const { - // find the clique to which key belongs - typename Nodes::const_iterator it = nodes_.find(key); - if (it == nodes_.end()) throw(invalid_argument( - "BayesTree::marginal('"+key+"'): key not found")); - // get clique containing key, and remove all factors below key - node_ptr clique = it->second; + node_ptr clique = (*this)[key]; Ordering ordering = clique->ordering(); FactorGraph graph(*clique); while(ordering.front()!=key) { diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index eeed835f0..bf97f556e 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -93,6 +93,15 @@ namespace gtsam { /** return root clique */ boost::shared_ptr > root() const {return root_;} + /** find the clique to which key belongs */ + node_ptr operator[](const std::string& key) const { + typename Nodes::const_iterator it = nodes_.find(key); + if (it == nodes_.end()) + throw(std::invalid_argument("BayesTree::operator['"+ key + "'): key not found")); + node_ptr clique = it->second; + return clique; + } + /** return marginal on any variable */ template boost::shared_ptr marginal(const std::string& key) const;