diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index ac1349f25..0875ec9c3 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -82,15 +82,15 @@ namespace gtsam { push_front(conditional); } -// /* ************************************************************************* */ -// template -// FastList BayesNet::ordering() const { -// FastList ord; -// BOOST_FOREACH(sharedConditional conditional,conditionals_) -// ord.push_back(conditional->key()); -// return ord; -// } -// + /* ************************************************************************* */ + template + FastList BayesNet::ordering() const { + FastList ord; + BOOST_FOREACH(sharedConditional conditional,conditionals_) + ord.insert(ord.begin(), conditional->beginFrontals(), conditional->endFrontals()); + return ord; + } + // /* ************************************************************************* */ // template // void BayesNet::saveGraph(const std::string &s) const { diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index d43526a46..8d4c71e56 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -116,10 +116,10 @@ namespace gtsam { return conditionals_.size(); } -// /** return keys in reverse topological sort order, i.e., elimination order */ -// FastList ordering() const; -// -// /** SLOW O(n) random access to Conditional by key */ + /** return keys in reverse topological sort order, i.e., elimination order */ + FastList ordering() const; + + /** SLOW O(n) random access to Conditional by key */ sharedConditional operator[](Index key) const; /** return last node in ordering */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index fafeba94b..3b87f6370 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -42,6 +42,7 @@ namespace gtsam { typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > sharedBayesNet; + typedef CONDITIONAL ConditionalType; typedef typename CONDITIONAL::FactorType FactorType; typedef typename FactorGraph::Eliminate Eliminate; @@ -59,7 +60,7 @@ namespace gtsam { void assertInvariants() const; public: - typedef CONDITIONAL Base; + typedef CONDITIONAL ConditionalType; typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; sharedConditional conditional_; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index b356402ea..b0dab65f7 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -152,6 +152,7 @@ public: size_t dim(const_iterator variable) const { return rsd_(variable - this->begin()).cols(); } rsd_type::constBlock get_S(const_iterator variable) const { return rsd_(variable - this->begin()); } + rsd_type::constBlock get_S() const { return rsd_.range(nrFrontals(), size()); } const Vector& get_sigmas() const {return sigmas_;} protected: