From 92ddabc4ac6e2532c6b5ccc4015c4de7542aea1a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 3 Nov 2010 21:33:27 +0000
Subject: [PATCH] Moved CCOLAMD call into non-templated code to avoid
 dependency of projects on CCOLAMD includes.  Removed unused includes and
 unnecessary templates in Inference at the same time

---
 gtsam/inference/JunctionTree-inl.h   |  1 +
 gtsam/inference/inference-inl.h      | 82 ++++------------------------
 gtsam/inference/inference.cpp        | 61 ++++++++++++++++++++-
 gtsam/inference/inference.h          | 14 +++--
 gtsam/linear/GaussianFactorGraph.cpp |  1 -
 5 files changed, 81 insertions(+), 78 deletions(-)

diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h
index 0278f4f98..bfb1252e6 100644
--- a/gtsam/inference/JunctionTree-inl.h
+++ b/gtsam/inference/JunctionTree-inl.h
@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam/base/timing.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/inference/JunctionTree.h>
diff --git a/gtsam/inference/inference-inl.h b/gtsam/inference/inference-inl.h
index 727325e6d..0f4bfb356 100644
--- a/gtsam/inference/inference-inl.h
+++ b/gtsam/inference/inference-inl.h
@@ -17,98 +17,36 @@
 
 #pragma once
 
-#include <limits>
-#include <map>
-#include <stdexcept>
-
 #include <boost/foreach.hpp>
-#include <boost/format.hpp>
-#include <boost/lambda/bind.hpp>
-#include <boost/lambda/lambda.hpp>
-#include <boost/pool/pool_alloc.hpp>
 
-#include <ccolamd.h>
-
-#include <gtsam/base/timing.h>
 #include <gtsam/inference/inference.h>
-#include <gtsam/inference/FactorGraph-inl.h>
-#include <gtsam/inference/BayesNet-inl.h>
-#include <gtsam/inference/ConditionalBase.h>
 
 using namespace std;
 
 namespace gtsam {
 
 /* ************************************************************************* */
-template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
-Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) {
-  size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
-  // Convert to compressed column major format colamd wants it in (== MATLAB format!)
-  int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
-  int * A = new int[Alen]; /* colamd arg 4: row indices of A, of size Alen */
-  int * p = new int[nVars + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
-  int * cmember = new int[nVars]; /* Constraint set of A, of size n_col */
+template<typename CONSTRAINED>
+Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast) {
 
-  static const bool debug = false;
-
-  p[0] = 0;
-  int count = 0;
-  for(Index var = 0; var < variableIndex.size(); ++var) {
-    const typename VARIABLEINDEXTYPE::Factors& column(variableIndex[var]);
-    size_t lastFactorId = numeric_limits<size_t>::max();
-    BOOST_FOREACH(const size_t& factorIndex, column) {
-      if(lastFactorId != numeric_limits<size_t>::max())
-        assert(factorIndex > lastFactorId);
-      A[count++] = factorIndex; // copy sparse column
-      if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl;
-    }
-    p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
-    cmember[var] = 0;
-  }
+  vector<int> cmember(variableIndex.size(), 0);
 
   // If at least some variables are not constrained to be last, constrain the
   // ones that should be constrained.
   if(constrainLast.size() < variableIndex.size()) {
     BOOST_FOREACH(Index var, constrainLast) {
-      assert(var < nVars);
+      assert(var < variableIndex.size());
       cmember[var] = 1;
     }
   }
 
-  assert((size_t)count == variableIndex.nEntries());
+  return PermutationCOLAMD_(variableIndex, cmember);
+}
 
-  if(debug)
-    for(size_t i=0; i<nVars+1; ++i)
-      cout << "p[" << i << "] = " << p[i] << endl;
-
-  double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
-//  double knobs[CCOLAMD_KNOBS];
-//  ccolamd_set_defaults(knobs);
-//  knobs[CCOLAMD_DENSE_ROW]=-1;
-//  knobs[CCOLAMD_DENSE_COL]=-1;
-
-  int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
-
-  // call colamd, result will be in p
-  /* returns (1) if successful, (0) otherwise*/
-  int rv = ccolamd(nFactors, nVars, Alen, A, p, knobs, stats, cmember);
-  if(rv != 1)
-    throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
-  delete[] A; // delete symbolic A
-  delete[] cmember;
-
-//  ccolamd_report(stats);
-
-  // Convert elimination ordering in p to an ordering
-  Permutation::shared_ptr permutation(new Permutation(nVars));
-  for (Index j = 0; j < nVars; j++) {
-    permutation->operator[](j) = p[j];
-    if(debug) cout << "COLAMD:  " << j << "->" << p[j] << endl;
-  }
-  if(debug) cout << "COLAMD:  p[" << nVars << "] = " << p[nVars] << endl;
-  delete[] p; // delete colamd result vector
-
-  return permutation;
+/* ************************************************************************* */
+inline Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndex& variableIndex) {
+  vector<int> cmember(variableIndex.size(), 0);
+  return PermutationCOLAMD_(variableIndex, cmember);
 }
 
 } // namespace gtsam
diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp
index d9dbac9ae..a3a81e7a4 100644
--- a/gtsam/inference/inference.cpp
+++ b/gtsam/inference/inference.cpp
@@ -16,10 +16,69 @@
  */
 
 #include <gtsam/inference/inference-inl.h>
-
 #include <gtsam/inference/SymbolicFactorGraph.h>
 
+#include <boost/format.hpp>
+#include <stdexcept>
+
+#include <ccolamd.h>
+
 namespace gtsam {
 
+Permutation::shared_ptr Inference::PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember) {
+  size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
+  // Convert to compressed column major format colamd wants it in (== MATLAB format!)
+  int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
+  vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
+  vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+
+  static const bool debug = false;
+
+  p[0] = 0;
+  int count = 0;
+  for(Index var = 0; var < variableIndex.size(); ++var) {
+    const VariableIndex::Factors& column(variableIndex[var]);
+    size_t lastFactorId = numeric_limits<size_t>::max();
+    BOOST_FOREACH(const size_t& factorIndex, column) {
+      if(lastFactorId != numeric_limits<size_t>::max())
+        assert(factorIndex > lastFactorId);
+      A[count++] = factorIndex; // copy sparse column
+      if(debug) cout << "A[" << count-1 << "] = " << factorIndex << endl;
+    }
+    p[var+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
+  }
+
+  assert((size_t)count == variableIndex.nEntries());
+
+  if(debug)
+    for(size_t i=0; i<nVars+1; ++i)
+      cout << "p[" << i << "] = " << p[i] << endl;
+
+  double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
+  //  double knobs[CCOLAMD_KNOBS];
+  //  ccolamd_set_defaults(knobs);
+  //  knobs[CCOLAMD_DENSE_ROW]=-1;
+  //  knobs[CCOLAMD_DENSE_COL]=-1;
+
+  int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
+
+  // call colamd, result will be in p
+  /* returns (1) if successful, (0) otherwise*/
+  int rv = ccolamd(nFactors, nVars, Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
+  if(rv != 1)
+    throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
+
+  //  ccolamd_report(stats);
+
+  // Convert elimination ordering in p to an ordering
+  Permutation::shared_ptr permutation(new Permutation(nVars));
+  for (Index j = 0; j < nVars; j++) {
+    permutation->operator[](j) = p[j];
+    if(debug) cout << "COLAMD:  " << j << "->" << p[j] << endl;
+  }
+  if(debug) cout << "COLAMD:  p[" << nVars << "] = " << p[nVars] << endl;
+
+  return permutation;
+}
 
 }
diff --git a/gtsam/inference/inference.h b/gtsam/inference/inference.h
index f8951cbeb..b212e40b1 100644
--- a/gtsam/inference/inference.h
+++ b/gtsam/inference/inference.h
@@ -34,15 +34,21 @@ namespace gtsam {
 	  /* Static members only, private constructor */
 	  Inference() {}
 
+	  // Internal version that actually calls colamd after the constraint set is created in the right format
+    static Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, std::vector<int>& cmember);
+
 	public:
 
     /**
      * Compute a permutation (variable ordering) using colamd
      */
-    template<class VARIABLEINDEXTYPE>
-    static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); }
-    template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
-    static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast);
+    static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex);
+
+    /**
+     * Compute a permutation (variable ordering) using constrained colamd
+     */
+    template<typename CONSTRAINED>
+    static Permutation::shared_ptr PermutationCOLAMD(const VariableIndex& variableIndex, const CONSTRAINED& constrainLast);
 
 	};
 
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 547ee1155..304686b0e 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -33,7 +33,6 @@
 
 using namespace std;
 using namespace gtsam;
-using namespace boost::assign;
 
 // trick from some reading group
 #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)