diff --git a/cpp/inference-inl.h b/cpp/inference-inl.h index 06106db30..c2fd62f96 100644 --- a/cpp/inference-inl.h +++ b/cpp/inference-inl.h @@ -85,36 +85,36 @@ namespace gtsam { } /* ************************************************************************* */ - pair marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key) { - - // todo: this does not use colamd! - - list ord; - BOOST_FOREACH(const Symbol& k, fg.keys()) { - if(k != key) - ord.push_back(k); - } - Ordering ordering(ord); - - // Now make another factor graph where we eliminate all the other variables - GaussianFactorGraph marginal(fg); - marginal.eliminate(ordering); - - GaussianFactor::shared_ptr factor; - for(size_t i=0; ikeys().size() != 1 || factor->keys().front() != key) - throw runtime_error("Didn't get the right marginal!"); - - VectorConfig mean_cfg(marginal.optimize(Ordering(key))); - Matrix A(factor->get_A(key)); - - return make_pair(mean_cfg[key], inverse(trans(A)*A)); - } +// pair marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key) { +// +// // todo: this does not use colamd! +// +// list ord; +// BOOST_FOREACH(const Symbol& k, fg.keys()) { +// if(k != key) +// ord.push_back(k); +// } +// Ordering ordering(ord); +// +// // Now make another factor graph where we eliminate all the other variables +// GaussianFactorGraph marginal(fg); +// marginal.eliminate(ordering); +// +// GaussianFactor::shared_ptr factor; +// for(size_t i=0; ikeys().size() != 1 || factor->keys().front() != key) +// throw runtime_error("Didn't get the right marginal!"); +// +// VectorConfig mean_cfg(marginal.optimize(Ordering(key))); +// Matrix A(factor->get_A(key)); +// +// return make_pair(mean_cfg[key], inverse(prod(trans(A), A))); +// } /* ************************************************************************* */ diff --git a/cpp/inference.h b/cpp/inference.h index 234322903..7100f6378 100644 --- a/cpp/inference.h +++ b/cpp/inference.h @@ -55,6 +55,6 @@ namespace gtsam { * Hacked-together function to compute a Gaussian marginal for the given variable. * todo: This is inefficient! */ - std::pair marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key); + //std::pair marginalGaussian(const GaussianFactorGraph& fg, const Symbol& key); } /// namespace gtsam