diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 7d99d7c52..1e48106e5 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -89,7 +89,11 @@ namespace gtsam { template typename GenericSequentialSolver::sharedBayesNet // GenericSequentialSolver::eliminate(const Permutation& permutation, - Eliminate function, boost::optional nrToEliminate) const { + Eliminate function +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + , boost::optional nrToEliminate +#endif + ) const { // Create inverse permutation Permutation::shared_ptr permutationInverse(permutation.inverse()); @@ -103,11 +107,13 @@ namespace gtsam { // Eliminate using elimination tree provided typename EliminationTree::shared_ptr etree; +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL if (nrToEliminate) { VariableIndex structure(*factors_, *nrToEliminate); etree = EliminationTree::Create(*factors_, structure); } else - etree = EliminationTree::Create(*factors_); +#endif + etree = EliminationTree::Create(*factors_); sharedBayesNet bayesNet = etree->eliminate(function); // Undo the permutation on the original factors and on the structure. @@ -132,17 +138,36 @@ namespace gtsam { // Compute a COLAMD permutation with the marginal variables constrained to the end. // TODO in case of nrFrontals, the order of js has to be respected here ! Permutation::shared_ptr permutation( - inference::PermutationCOLAMD(*structure_, js)); + inference::PermutationCOLAMD(*structure_, js, true)); +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + // TODO Frank says: this was my attempt at eliminating exactly + // as many variables as we need. Unfortunately, in some cases + // (see testSymbolicSequentialSolver::problematicConditional) + // my trick below (passing nrToEliminate to eliminate) sometimes leads + // to a disconnected graph. // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) - size_t nrVariables = factors_->keys().size(); // TODO expensive! + size_t nrVariables = factors_->keys().size();// TODO expensive! size_t nrMarginalized = nrVariables - js.size(); size_t nrToEliminate = nrMarginalized + nrFrontals; sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); - // Get rid of conditionals on variables that we want to marginalize out for (int i = 0; i < nrMarginalized; i++) - bayesNet->pop_front(); + bayesNet->pop_front(); +#else + // Eliminate all variables + sharedBayesNet fullBayesNet = eliminate(*permutation, function); + + // Get rid of conditionals we do not need (front and back) + size_t nrMarginalized = fullBayesNet->size() - js.size(); + sharedBayesNet bayesNet(new BayesNet()); + size_t i = 1; + BOOST_FOREACH(sharedConditional c, *fullBayesNet) { + if (i > nrMarginalized && i - nrMarginalized <= nrFrontals) + bayesNet->push_back(c); + i += 1; + } +#endif return bayesNet; } @@ -161,8 +186,8 @@ namespace gtsam { sharedBayesNet bayesNet = eliminate(*permutation, function); // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalizedOut = bayesNet->size() - js.size(); - for (int i = 0; i < nrMarginalizedOut; i++) + size_t nrMarginalized = bayesNet->size() - js.size(); + for (int i = 0; i < nrMarginalized; i++) bayesNet->pop_front(); return bayesNet; diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5b4cea73c..6f0bf3b5b 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -64,6 +64,7 @@ namespace gtsam { typedef boost::shared_ptr > sharedFactorGraph; typedef typename FACTOR::ConditionalType Conditional; + typedef typename boost::shared_ptr sharedConditional; typedef typename boost::shared_ptr > sharedBayesNet; typedef std::pair, boost::shared_ptr > EliminationResult; typedef boost::function< @@ -86,11 +87,14 @@ namespace gtsam { /** * Eliminate in a different order, given a permutation - * If given a number of variables to eliminate, will only eliminate that many */ sharedBayesNet - eliminate(const Permutation& permutation, Eliminate function, - boost::optional nrToEliminate = boost::none) const; + eliminate(const Permutation& permutation, Eliminate function +#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL + , boost::optional nrToEliminate = boost::none + // If given a number of variables to eliminate, will only eliminate that many +#endif + ) const; public: