a preliminary version of SparseQR routines that passes the unit tests but has slightly different results as the old method when optimizing large nonlinear problems.

configure.ac

@@ -54,6 +54,17 @@ AC_ARG_ENABLE([blas],
 AM_CONDITIONAL([USE_BLAS_MACOS], [test x$blas = xtrue && test x$ISMAC = xtrue])
 AM_CONDITIONAL([USE_BLAS_LINUX], [test x$blas = xtrue && test x$ISMAC = xfalse])
 
+#enabgle SparseQR for linear solving
+AC_ARG_ENABLE([spqr],
+     [  --enable-spqr    Enable SparseQR library support],
+     [case "${enableval}" in
+       yes) spqr=true ;;
+       no)  spqr=false ;;
+       *) AC_MSG_ERROR([bad value ${enableval} for --enable-spqr]) ;;
+     esac],[spqr=false])
+
+AM_CONDITIONAL([USE_SPQR], [test x$spqr = xtrue])
+
 # Checks for programs.
 AC_PROG_CXX
 AC_PROG_CC

cpp/Factorization.h

@@ -46,4 +46,36 @@ namespace gtsam {
   		return g.linearize(config);
   	}
   };
+
+#ifdef USE_SPQR
+  /**
+   * A linear system solver using factorization
+   */
+  template <class NonlinearGraph, class Config>
+  class FactorizationSPQR {
+  private:
+  	boost::shared_ptr<const Ordering> ordering_;
+
+  public:
+  	FactorizationSPQR(boost::shared_ptr<const Ordering> ordering, bool old=true)
+		: ordering_(ordering) {
+  		if (!ordering) throw std::invalid_argument("FactorizationSPQR constructor: ordering = NULL");
+  	}
+
+  	/**
+  	 * solve for the optimal displacement in the tangent space, and then solve
+  	 * the resulted linear system
+  	 */
+  	VectorConfig optimize(GaussianFactorGraph& fg) const {
+  		return fg.optimizeSPQR(*ordering_);
+  	}
+
+		/**
+		 * linearize the non-linear graph around the current config
+		 */
+  	boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const {
+  		return g.linearize(config);
+  	}
+  };
+#endif
 }

cpp/GaussianFactorGraph.cpp

@@ -323,7 +323,7 @@ VectorConfig GaussianFactorGraph::assembleConfig(const Vector& vs, const Orderin
 }
 
 /* ************************************************************************* */
-Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
+pair<Dimensions, size_t> GaussianFactorGraph::columnIndices_(const Ordering& ordering) const {
 
 	// get the dimensions for all variables
 	Dimensions variableSet = dimensions();
@@ -342,7 +342,12 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
 		j += it->second;
 	}
 
-	return result;
+	return make_pair(result, j-1);
+}
+
+/* ************************************************************************* */
+Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
+	return columnIndices_(ordering).first;
 }
 
 /* ************************************************************************* */
@@ -434,5 +439,140 @@ boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_(
 					maxIterations)));
 }
 
+#ifdef USE_SPQR
 /* ************************************************************************* */
+cholmod_sparse* GaussianFactorGraph::cholmodSparse(const Ordering& ordering, vector<size_t>& orderedDimensions) const {
+	Dimensions colIndices;
+	size_t numCols;
+	boost::tie(colIndices, numCols) = columnIndices_(ordering);
 
+	typedef pair<int, int> RowValue; // the pair of row index and non-zero value
+	int row_start = 0, column_start = 0;
+	size_t nnz = 0;
+	vector<vector<RowValue> > ivs_vector;
+	ivs_vector.resize(numCols);
+	Vector sigmas;
+	SymbolMap<Matrix>::const_iterator jA;
+	BOOST_FOREACH(const sharedFactor& factor,factors_) {       // iterate over all the factors
+		for(jA = factor->begin(); jA!=factor->end(); jA++) {     // iterate over all matrices in the factor
+			column_start = colIndices.at(jA->first) - 1;           // find the first column index for this key
+			sigmas = factor->get_sigmas();
+			for (size_t i = 0; i < jA->second.size1(); i++)        // interate over all the non-zero entries in the submatrix
+				for (size_t j = 0; j < jA->second.size2(); j++)
+					if (jA->second(i, j) != 0.0) {
+						ivs_vector[j + column_start].push_back(make_pair(i + row_start, jA->second(i, j) / sigmas[i]));
+						nnz++;
+					}
+		}
+		row_start += factor->numberOfRows();
+	}
+
+	// assemble the CHOLMOD data structure
+	cholmod_sparse* A = new cholmod_sparse();
+	long* p = new long[numCols + 1];  // starting and ending index in A->i for each column
+	long* i = new long[nnz];          // row indices of nnz entries
+	double* x = new double[nnz];       // the values of nnz entries
+	p[0] = 0;
+	int p_index = 1;
+	int i_index = 0;
+	BOOST_FOREACH(const vector<RowValue>& ivs, ivs_vector) {
+		p[p_index] = p[p_index-1] + ivs.size();
+		BOOST_FOREACH(const RowValue& iv, ivs) {
+			i[i_index] = iv.first;
+			x[i_index++] = iv.second;
+		}
+		p_index ++;
+	}
+	A->nrow = row_start; A->ncol = numCols;	A->nzmax = nnz;
+	A->p = p; 	A->i = i; A->x = x;
+	A->stype = 0;
+	A->xtype = CHOLMOD_REAL;
+	A->dtype = CHOLMOD_DOUBLE;
+	A->itype = CHOLMOD_LONG;
+	A->packed = 1;	A->sorted = 1;
+
+	// order the column indices w.r.t. the given ordering
+	vector<size_t> orderedIndices;
+	BOOST_FOREACH(const Symbol& key, ordering)
+		orderedIndices.push_back(colIndices[key] - 1);
+	orderedIndices.push_back(numCols);
+
+	// record the dimensions for each key as the same order in the {ordering}
+	vector<size_t>::const_iterator it1 = orderedIndices.begin();
+	vector<size_t>::const_iterator it2 = orderedIndices.begin(); it2++;
+	while(it2 != orderedIndices.end())
+		orderedDimensions.push_back(*(it2++) - *(it1++));
+
+	return A;
+}
+
+
+/* ************************************************************************* */
+cholmod_dense* GaussianFactorGraph::cholmodRhs() const {
+	cholmod_dense* b = new cholmod_dense();
+	// compute the number of rows
+	b->nrow = 0;
+	BOOST_FOREACH(const sharedFactor& factor,factors_)
+		b->nrow += factor->numberOfRows();
+
+	b->ncol = 1;	b->nzmax = b->nrow;	b->d = b->nrow;
+	b->xtype = CHOLMOD_REAL;
+	b->dtype = CHOLMOD_DOUBLE;
+
+	// fill the data
+	double* x = new double[b->nrow];
+	double* x_current = x;
+  Vector::const_iterator it_b, it_sigmas, it_b_end;
+	BOOST_FOREACH(const sharedFactor& factor,factors_) {
+		it_b = factor->get_b().begin();
+		it_b_end = factor->get_b().end();
+		it_sigmas = factor->get_sigmas().begin();
+		for(; it_b != it_b_end; )
+			*(x_current++) = *(it_b++) / *(it_sigmas++);
+	}
+	b->x = x;
+
+	return b;
+}
+
+/* ************************************************************************* */
+VectorConfig GaussianFactorGraph::optimizeSPQR(const Ordering& ordering) const
+{
+	// set up the default parameters
+	cholmod_common Common, *cc ;
+	cc = &Common ;
+	cholmod_l_start(cc) ;
+
+	// get the A matrix and rhs in the compress column-format
+	vector<size_t> orderedDimensions;
+	cholmod_sparse* A = cholmodSparse(ordering, orderedDimensions);
+	cholmod_dense* b = cholmodRhs();
+
+	// QR
+	double tol = 1e-30;
+	cholmod_dense* x = SuiteSparseQR_min2norm<double> (0, tol, A, b, cc) ;
+
+	// create the update vector
+	VectorConfig config;
+	double *x_start = (double*)x->x, *x_end;
+	vector<size_t>::const_iterator itDim = orderedDimensions.begin();
+	BOOST_FOREACH(const Symbol& key, ordering) {
+		Vector v(*itDim);
+		x_end = x_start + *itDim;
+		copy(x_start, x_end, v.data().begin());
+		config.insert(key, v);
+		itDim++;
+		x_start = x_end;
+	}
+
+	// free memory
+	cholmod_l_free_sparse(&A, cc);
+	cholmod_l_free_dense (&b, cc) ;
+	cholmod_l_free_dense (&x, cc);
+	cholmod_l_finish(cc);
+
+	return config;
+}
+#endif
+
+/* ************************************************************************* */

cpp/GaussianFactorGraph.h

@@ -13,6 +13,11 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
+
+#ifdef USE_SPQR
+#include <SuiteSparseQR.hpp>
+#endif
+
 #include "FactorGraph.h"
 #include "Errors.h"
 #include "GaussianFactor.h"
@@ -205,10 +210,11 @@ namespace gtsam {
     VectorConfig assembleConfig(const Vector& v, const Ordering& ordering) const;
 
     /**
-     * get the starting column indices for all variables
+     * get the 1-based starting column indices for all variables
      * @param ordering of variables needed for matrix column order
      * @return The set of all variable/index pairs
      */
+    std::pair<Dimensions, size_t> columnIndices_(const Ordering& ordering) const;
     Dimensions columnIndices(const Ordering& ordering) const;
 
     /**
@@ -258,6 +264,24 @@ namespace gtsam {
 		boost::shared_ptr<VectorConfig> conjugateGradientDescent_(
 				const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3,
 				size_t maxIterations = 0) const;
-  };
 
+#ifdef USE_SPQR
+		/**
+		 * Convert to CHOLMOD's compressed-column form, refer to CHOLMOD's user guide for details.
+		 * The returned pointer needs to be free by calling cholmod_l_free_sparse
+		 */
+		cholmod_sparse* cholmodSparse(const Ordering& ordering, std::vector<std::size_t>& orderedDimensions) const;
+
+		/**
+		 * Convert the RHS to CHOLMOD's column-major matrix format
+		 * The returned pointer needs to be free by calling cholmod_l_free_sparse
+		 */
+		cholmod_dense* cholmodRhs() const;
+
+    /**
+     * optimizing using SparseQR package
+     */
+    VectorConfig optimizeSPQR(const Ordering& ordering) const;
+#endif
+  };
 } // namespace gtsam

cpp/Makefile.am

@@ -298,7 +298,13 @@ endif
 include_HEADERS = $(headers)
 
 AM_CXXFLAGS += -I.. 
-AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization)
+AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization) 
+
+# adding SparseQR support
+if USE_SPQR
+AM_CXXFLAGS += -I$(HOME)/include/SuiteSparse -DUSE_SPQR
+AM_LDFLAGS += -L$(HOME)/lib/SuiteSparse -lcholmod -lsqpr
+endif
 
 # adding cblas implementation - split into a default linux version using the 
 # autotools script, and a mac version that is hardcoded 

cpp/NonlinearOptimizer-inl.h

@@ -26,16 +26,21 @@ namespace gtsam {
 			int verbosity) {
 		// check if diverges
 		double absoluteDecrease = currentError - newError;
-		if (verbosity >= 2)
+		if (verbosity >= 2) {
-			cout << "absoluteDecrease: " << absoluteDecrease << endl;
+			if (absoluteDecrease < absoluteErrorTreshold)
-//		if (absoluteDecrease < 0)
+				cout << "absoluteDecrease: " << absoluteDecrease << " < " << absoluteErrorTreshold << endl;
-//			throw overflow_error(
+			else
-//					"NonlinearFactorGraph::optimize: error increased, diverges.");
+				cout << "absoluteDecrease: " << absoluteDecrease << " >= " << absoluteErrorTreshold << endl;
+		}
 
 		// calculate relative error decrease and update currentError
 		double relativeDecrease = absoluteDecrease / currentError;
-		if (verbosity >= 2)
+		if (verbosity >= 2) {
-			cout << "relativeDecrease: " << relativeDecrease << endl;
+			if (relativeDecrease < relativeErrorTreshold)
+				cout << "relativeDecrease: " << relativeDecrease << " < " << relativeErrorTreshold << endl;
+			else
+				cout << "relativeDecrease: " << relativeDecrease << " >= " << relativeErrorTreshold << endl;
+		}
 		bool converged = (relativeDecrease < relativeErrorTreshold)
 				|| (absoluteDecrease < absoluteErrorTreshold);
 		if (verbosity >= 1 && converged)

cpp/testGaussianFactorGraph.cpp

@@ -413,6 +413,91 @@ TEST( GaussianFactorGraph, sparse )
 		10.,10., -10.,-10., 10., 10.,   5.,5.,-5.,-5.,   5., 5.,-5.,-5.), ijs);
 }
 
+#ifdef USE_SPQR
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, cholmodSparse )
+{
+	// create a small linear factor graph
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+
+	Ordering ord; ord += "x2","l1","x1";
+	vector<size_t> dimensions;
+	cholmod_sparse* A = fg.cholmodSparse(ord, dimensions);
+
+	CHECK(A->ncol == 6);
+	CHECK(A->nrow == 8);
+	CHECK(A->nzmax == 14);
+	CHECK(A->stype == 0);
+	CHECK(A->xtype == CHOLMOD_REAL);
+	CHECK(A->dtype == CHOLMOD_DOUBLE);
+	CHECK(A->itype == CHOLMOD_LONG);
+	CHECK(A->packed == 1);
+	CHECK(A->sorted == 1);
+
+	long p[7] = {0, 2, 4, 6, 8, 11, 14};
+	for(long index=0; index<7; index++)
+		CHECK(p[index] == *((long*)A->p + index));
+
+	long i[14] = {2, 6, 3, 7, 4, 6, 5, 7, 0, 2, 4, 1, 3, 5};
+	for(long index=0; index<14; index++)
+		CHECK(i[index] == *((long*)A->i + index));
+
+	double x[14] = {10, -5, 10, -5, 5, 5, 5, 5, 10, -10, -5, 10, -10, -5};
+	for(int index=0; index<14; index++)
+		CHECK(x[index] == *((double*)A->x + index));
+
+	cholmod_common Common, *cc ;
+	cc = &Common ;
+	cholmod_l_start (cc) ;
+	cholmod_l_free_sparse(&A, cc);
+	cholmod_l_finish(cc) ;
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, cholmodRhs )
+{
+	// create a small linear factor graph
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+
+	cholmod_dense* b = fg.cholmodRhs();
+
+	CHECK(b->ncol == 1);
+	CHECK(b->nrow == 8);
+	CHECK(b->nzmax == 8);
+	CHECK(b->d == 8);
+	CHECK(b->xtype == CHOLMOD_REAL);
+	CHECK(b->dtype == CHOLMOD_DOUBLE);
+
+	double x[8] = {-1., -1., 2., -1., 0., 1., -1., 1.5};
+	for(int index=0; index<8; index++)
+		CHECK(x[index] == *((double*)b->x + index));
+
+	cholmod_common Common, *cc;
+	cc = &Common;
+	cholmod_l_start (cc);
+	cholmod_l_free_dense(&b, cc);
+	cholmod_l_finish(cc);
+}
+
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, optimizeSPQR )
+{
+	// create a graph
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+
+	// create an ordering
+	Ordering ord; ord += "x2","l1","x1";
+
+	// optimize the graph
+	VectorConfig actual = fg.optimizeSPQR(ord);
+
+	// verify
+	VectorConfig expected = createCorrectDelta();
+
+  CHECK(assert_equal(expected,actual));
+}
+#endif
+
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
 {
@@ -461,7 +546,7 @@ TEST( GaussianFactorGraph, optimize )
 	GaussianFactorGraph fg = createGaussianFactorGraph();
 
 	// create an ordering
-	Ordering ord = fg.getOrdering();
+	Ordering ord; ord += "x2","l1","x1";
 
 	// optimize the graph
 	VectorConfig actual = fg.optimize(ord);

cpp/testNonlinearOptimizer.cpp

@@ -201,6 +201,35 @@ TEST( NonlinearOptimizer, Factorization )
 	CHECK(assert_equal(expected, *optimized.config(), 1e-5));
 }
 
+/* ************************************************************************* */
+#ifdef USE_SPQR
+TEST( NonlinearOptimizer, FactorizationSPQR )
+{
+	typedef NonlinearOptimizer<Pose2Graph, Pose2Config, GaussianFactorGraph, FactorizationSPQR<Pose2Graph, Pose2Config> > Optimizer;
+
+	boost::shared_ptr<Pose2Config> config(new Pose2Config);
+	config->insert(1, Pose2(0.,0.,0.));
+	config->insert(2, Pose2(1.5,0.,0.));
+
+	boost::shared_ptr<Pose2Graph> graph(new Pose2Graph);
+	graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10));
+	graph->addPrior(2, Pose2(1.5,0.,0.), Isotropic::Sigma(3, 1e-10));
+
+	boost::shared_ptr<Ordering> ordering(new Ordering);
+	ordering->push_back(Pose2Config::Key(1));
+	ordering->push_back(Pose2Config::Key(2));
+	Optimizer::shared_solver solver(new FactorizationSPQR<Pose2Graph, Pose2Config>(ordering));
+
+	Optimizer optimizer(graph, config, solver);
+	Optimizer optimized = optimizer.iterateLM();
+
+	Pose2Config expected;
+	expected.insert(1, Pose2(0.,0.,0.));
+	expected.insert(2, Pose2(1.5,0.,0.));
+	CHECK(assert_equal(expected, *optimized.config(), 1e-5));
+}
+#endif
+
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, SubgraphSolver )
 {