Fixed major bug: splitting off a subgraph preconditioner only worked if keys were numbered 0...n-1, because we used DSFVector to implement Kruskal. Now it'll be a bit slower but will work for any keys. Note this only affected two constructors.

gtsam/linear/SubgraphSolver.cpp

@@ -22,7 +22,9 @@
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/base/DSFVector.h>
+#include <gtsam_unstable/base/DSFMap.h>
+
+#include <iostream>
 
 using namespace std;
 
@@ -120,45 +122,44 @@ void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
 }
 
 /**************************************************************************************************/
+// Run Kruskal algorithm to create a spanning tree of factor "edges".
+// Edges are not weighted, and will only work if factors are binary.
+// Unary factors are ignored for this purpose and added to tree graph.
 boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
-SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
+SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 
-  const VariableIndex index(jfg);
+  // Create disjoint set forest data structure for Kruskal algorithm
-  const size_t n = index.size();
+  DSFMap<Key> dsf;
-  DSFVector D(n);
 
-  GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
+  // Allocate two output graphs
-  GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
+  auto tree = boost::make_shared<GaussianFactorGraph>();
+  auto constraints = boost::make_shared<GaussianFactorGraph>();
 
-  size_t t = 0;
+  // Loop over all "edges"
-  for ( const GaussianFactor::shared_ptr &gf: jfg ) {
+  for ( const auto &factor: factorGraph ) {
 
-    if (gf->keys().size() > 2) {
+    // Fail on > binary factors
+    const auto& keys = factor->keys();
+    if (keys.size() > 2) {
       throw runtime_error(
           "SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
     }
 
-    bool augment = false;
+    // check whether this factor should be augmented to the "tree" graph
-
+    if (keys.size() == 1)
-    /* check whether this factor should be augmented to the "tree" graph */
+      tree->push_back(factor);
-    if (gf->keys().size() == 1)
+    else if (dsf.find(keys[0]) != dsf.find(keys[1])) {
-      augment = true;
+      // We merge two trees joined by this edge if they are still disjoint
-    else {
+      tree->push_back(factor);
-      const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u),
+      // Record this fact in DSF
-          v_root = D.find(v);
+      dsf.merge(keys[0], keys[1]);
-      if (u_root != v_root) {
+    } else {
-        t++;
+      // This factor would create a loop, so we add it to non-tree edges...
-        augment = true;
+      constraints->push_back(factor);
-        D.merge(u_root, v_root);
     }
   }
-    if (augment)
-      At->push_back(gf);
-    else
-      Ac->push_back(gf);
-  }
 
-  return boost::tie(At, Ac);
+  return boost::tie(tree, constraints);
 }
 
 /****************************************************************************/

gtsam/linear/SubgraphSolver.h

@@ -75,7 +75,11 @@ protected:
 
 public:
 
-  /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
+  /**
+   * Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
+   * Will throw exception if there are ternary factors or higher arity, as we use Kruskal's
+   * algorithm to split the graph, treating binary factors as edges.
+   */
   SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
       const Ordering& ordering);