diff --git a/gtsam.h b/gtsam.h index f702cc5d9..9e5fa42ce 100644 --- a/gtsam.h +++ b/gtsam.h @@ -903,6 +903,7 @@ class SymbolicFactorGraph { void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); pair eliminateFrontals(size_t nFrontals) const; + pair eliminateOne(size_t j) const; }; #include @@ -1234,6 +1235,7 @@ class GaussianFactorGraph { // Inference pair eliminateFrontals(size_t nFrontals) const; + pair eliminateOne(size_t j) const; // Building the graph void push_back(gtsam::GaussianFactor* factor); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 5be010e2c..53642dd76 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -197,14 +197,11 @@ class VariableIndex; * BayesNet. If the variables are not fully-connected, it is more efficient * to sequentially factorize multiple times. */ - std::pair::sharedConditional, FactorGraph > - eliminate( + std::pair > eliminate( const std::vector& variables, const Eliminate& eliminateFcn, boost::optional variableIndex = boost::none); - /** Eliminate a single variable, by calling - * eliminate(const Graph&, const std::vector&, const typename Graph::Eliminate&, boost::optional) - */ + /** Eliminate a single variable, by calling FactorGraph::eliminate. */ std::pair > eliminateOne( KeyType variable, const Eliminate& eliminateFcn, boost::optional variableIndex = boost::none) { diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 9b52a3d26..4a3835696 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -70,6 +70,20 @@ namespace gtsam { return FactorGraph::eliminateFrontals(nFrontals, EliminateSymbolic); } + /* ************************************************************************* */ + std::pair + SymbolicFactorGraph::eliminate(const std::vector& variables) + { + return FactorGraph::eliminate(variables, EliminateSymbolic); + } + + /* ************************************************************************* */ + std::pair + SymbolicFactorGraph::eliminateOne(Index variable) + { + return FactorGraph::eliminateOne(variable, EliminateSymbolic); + } + /* ************************************************************************* */ void SymbolicFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { diff --git a/gtsam/inference/SymbolicFactorGraph.h b/gtsam/inference/SymbolicFactorGraph.h index 5f7f7067f..5b5d19a62 100644 --- a/gtsam/inference/SymbolicFactorGraph.h +++ b/gtsam/inference/SymbolicFactorGraph.h @@ -66,6 +66,27 @@ namespace gtsam { * as the eliminate function argument. */ std::pair eliminateFrontals(size_t nFrontals) const; + + /** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + * Note that this version simply calls + * FactorGraph::eliminate with EliminateSymbolic as the eliminate + * function argument. + */ + std::pair eliminate(const std::vector& variables); + + /** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */ + std::pair eliminateOne(Index variable); /// @} /// @name Standard Interface diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index cb6bfa6eb..cb118256b 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -54,6 +54,20 @@ namespace gtsam { return FactorGraph::eliminateFrontals(nFrontals, EliminateQR); } + /* ************************************************************************* */ + std::pair + GaussianFactorGraph::eliminate(const std::vector& variables) + { + return FactorGraph::eliminate(variables, EliminateQR); + } + + /* ************************************************************************* */ + std::pair + GaussianFactorGraph::eliminateOne(Index variable) + { + return FactorGraph::eliminateOne(variable, EliminateQR); + } + /* ************************************************************************* */ void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { @@ -511,9 +525,6 @@ break; GaussianFactorGraph::EliminationResult EliminatePreferCholesky( const FactorGraph& factors, size_t nrFrontals) { - typedef JacobianFactor J; - typedef HessianFactor H; - // If any JacobianFactors have constrained noise models, we have to convert // all factors to JacobianFactors. Otherwise, we can convert all factors // to HessianFactors. This is because QR can handle constrained noise diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 623a5a7a6..e5b17e599 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -123,6 +123,27 @@ namespace gtsam { * eliminate function argument. */ std::pair eliminateFrontals(size_t nFrontals) const; + + /** Factor the factor graph into a conditional and a remaining factor graph. + * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out + * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where + * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is + * a probability density or likelihood, the factorization produces a + * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$. + * + * For efficiency, this function treats the variables to eliminate + * \c variables as fully-connected, so produces a dense (fully-connected) + * conditional on all of the variables in \c variables, instead of a sparse + * BayesNet. If the variables are not fully-connected, it is more efficient + * to sequentially factorize multiple times. + * Note that this version simply calls + * FactorGraph::eliminate with EliminateQR as the eliminate + * function argument. + */ + std::pair eliminate(const std::vector& variables); + + /** Eliminate a single variable, by calling GaussianFactorGraph::eliminate. */ + std::pair eliminateOne(Index variable); /** Permute the variables in the factors */ void permuteWithInverse(const Permutation& inversePermutation);