Went back to eliminate all in conditional because of singularities :-(, added use of forceOrder flag

gtsam/inference/GenericSequentialSolver-inl.h

@@ -89,7 +89,11 @@ namespace gtsam {
   template<class FACTOR>
   typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
-      Eliminate function, boost::optional<size_t> nrToEliminate) const {
+      Eliminate function
+#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
+      , boost::optional<size_t> nrToEliminate
+#endif
+      ) const {
 
     // Create inverse permutation
     Permutation::shared_ptr permutationInverse(permutation.inverse());
@@ -103,10 +107,12 @@ namespace gtsam {
 
     // Eliminate using elimination tree provided
     typename EliminationTree<FACTOR>::shared_ptr etree;
+#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
     if (nrToEliminate) {
       VariableIndex structure(*factors_, *nrToEliminate);
       etree = EliminationTree<FACTOR>::Create(*factors_, structure);
     } else
+#endif
     etree = EliminationTree<FACTOR>::Create(*factors_);
     sharedBayesNet bayesNet = etree->eliminate(function);
 
@@ -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 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();
+#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<Conditional>());
+    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();
+    size_t nrMarginalized = bayesNet->size() - js.size();
-    for (int i = 0; i < nrMarginalizedOut; i++)
+    for (int i = 0; i < nrMarginalized; i++)
       bayesNet->pop_front();
 
     return bayesNet;

gtsam/inference/GenericSequentialSolver.h

@@ -64,6 +64,7 @@ namespace gtsam {
 
     typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
     typedef typename FACTOR::ConditionalType Conditional;
+    typedef typename boost::shared_ptr<Conditional> sharedConditional;
     typedef typename boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet;
     typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > 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,
+    eliminate(const Permutation& permutation, Eliminate function
-        boost::optional<size_t> nrToEliminate = boost::none) const;
+#ifdef ATTEMPT_AT_NOT_ELIMINATING_ALL
+        , boost::optional<size_t> nrToEliminate = boost::none
+        // If given a number of variables to eliminate, will only eliminate that many
+#endif
+        ) const;
 
   public: