diff --git a/Makefile.am b/Makefile.am index 34d3dc6fb..51fb6f1a3 100644 --- a/Makefile.am +++ b/Makefile.am @@ -17,10 +17,6 @@ SUBDIRS = CppUnitLite base geometry inference linear nonlinear slam . tests wrap SUBLIBS = base/libbase.la geometry/libgeometry.la inference/libinference.la \ linear/liblinear.la nonlinear/libnonlinear.la slam/libslam.la -if USE_LAPACK -SUBLIBS += -L$(DenseQRLib) -lDenseQR -endif - # TODO: UFconfig, CCOLAMD, and LDL automake magic without adding or touching any file # in those directories as to not invalidate the LGPL license # See some possibilities in diff --git a/base/DenseQR.cpp b/base/DenseQR.cpp new file mode 100644 index 000000000..a5882b28c --- /dev/null +++ b/base/DenseQR.cpp @@ -0,0 +1,197 @@ +/* + * DenseQR.cpp + * + * Created on: Oct 19, 2010 + * Author: nikai + * Description: Dense QR, inspired by Tim Davis's dense solver + */ + +#include +#include +#include + +#include "DenseQR.h" + +// all the lapack functions we need here +extern "C" { +void dlarft_ (char *direct, char *storev, int *n, int *k, double *V, int *ldv, double *Tau, double *T, int *ldt) ; +void dlarfb_ (char *side, char *trans, char *direct, char *storev, int *m, int *n, int *k, double *V, int *ldv, double *T, int *ldt, double *C, int *ldc, double *Work, int *ldwork) ; +void dlarfg_ (int *n, double *alpha, double *X, int *incx, double *tau) ; +void dlarf_ (char *side, int *m, int *n, double *V, int *incv, double *tau, double *C, int *ldc, double *Work) ; +} + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + /** + * LARF applies a real elementary reflector H to a real m by n matrix + * C, from either the left or the right. H is represented in the form + */ + void dlarf_wrap(long m, long n, long ldc, double *V, double tau, double *C, double *W) + { + static char left = 'L' ; + double vsave ; + if (m <= 0 || n <= 0) return ; + vsave = V [0] ; // temporarily restore unit diagonal of V + V [0] = 1 ; + int m_ = m, n_ = n, ldc_ = ldc, one = 1 ; + dlarf_ (&left, &m_, &n_, V, &one, &tau, C, &ldc_, W) ; + V [0] = vsave ; // restore V [0] + } + + /* ************************************************************************* */ + void dlarftb_wrap(long m, long n, long k, long ldc, long ldv, double *V, double *Tau, double *C, double *W) + { + static char direct = 'F'; + static char storev = 'C'; + static char side = 'L'; + static char trans = 'T'; + if (m <= 0 || n <= 0 || k <= 0) return ; + + double *T, *Work ; + T = W ; // triangular k-by-k matrix for block reflector + Work = W + k*k ; // workspace of size n*k or m*k for larfb + + // construct and apply the k-by-k upper triangular matrix T + // larft and larfb are always used "Forward" and "Columnwise" + assert (m >= k) ; + int m_ = m, n_ = n, k_ = k, ldv_ = ldv, ldc_ = ldc; + dlarft_(&direct, &storev, &m_, &k_, V, &ldv_, Tau, T, &k_) ; + // Left, Transpose, Forward, Columwise: + dlarfb_(&side, &trans, &direct, &storev, &m_, &n_, &k_, V, &ldv_, + T, &k_, C, &ldc_, Work, &n_); + + } + + /* ************************************************************************* */ + long DenseQR(long m, long n, long npiv, double tol, long ntol, long fchunk, + double *F, long *Stair, char *Rdead, double *Tau, double *W, double *wscale, double *wssq) { + double tau, wk, *V; + long k, t, g, g1, nv, k1, k2, i, t0, vzeros, mleft, nleft, vsize, minchunk, rank ; + + assert (F != NULL) ; + assert (Stair != NULL) ; + assert (Rdead != NULL) ; + assert (Tau != NULL) ; + assert (W != NULL) ; + assert (m >= 0 && n >= 0) ; + + npiv = max (0l, npiv) ; // npiv must be between 0 and n + npiv = min (n, npiv) ; + g1 = 0 ; // row index of first queued-up Householder + k1 = 0 ; // pending Householders are in F (g1:t, k1:k2-1) + k2 = 0 ; + V = F ; // Householder vectors start here + g = 0 ; // number of good Householders found + nv = 0 ; // number of Householder reflections queued up + vzeros = 0 ; // number of explicit zeros in queued-up H's + t = 0 ; // staircase of current column + fchunk = max (fchunk, 1l) ; + minchunk = max (4l, fchunk/4l) ; + rank = min (m,npiv) ; + ntol = min (ntol, npiv) ; + + for (k = 0; k < n; k++) { + t0 = t; // t0 = staircase of column k-1 + t = Stair[k]; // t = staircase of this column k + + if (g >= m) { + for (; k < npiv; k++) { + Rdead[k] = 1; + Stair[k] = 0; + Tau[k] = 0; + } + for (; k < n; k++) { + Stair[k] = m; + Tau[k] = 0; + } + assert (nv == 0); + return (rank); + } + + t = max(g + 1, t); + Stair[k] = t; + + vzeros += nv * (t - t0); + if (nv >= minchunk) { + vsize = (nv * (nv + 1)) / 2 + nv * (t - g1 - nv); + if (vzeros > max(16l, vsize / 2)) { + dlarftb_wrap(t0 - g1, n - k2, nv, m, m, V, // F (g1:t-1, k1:k1+nv-1) + &Tau[k1], &F[g1+k2*m], W); + nv = 0; + vzeros = 0; + } + } + + // find a Householder reflection that reduces column k + int n_ = t - g, one = 1; + double *X = &F[g+k*m]; + dlarfg_(&n_, X, X + 1, &one, &tau); + + // check to see if the kth column is OK + if (k < ntol && (wk = fabs(F[g+k*m])) <= tol) { + if (wk != 0) { + if ((*wscale) == 0) { + (*wssq) = 1; + } + if ((*wscale) < wk) { + double rr = (*wscale) / wk; + (*wssq) = 1 + (*wssq) * rr * rr; + (*wscale) = wk; + } else { + double rr = wk / (*wscale); + (*wssq) += rr * rr; + } + } + + // zero out F (g:m-1,k) and flag it as dead + for (i = g; i < m; i++) + F[i+k*m] = 0; + Stair[k] = 0; + Tau[k] = 0; + Rdead[k] = 1; + + // apply pending block of Householder reflections + if (nv > 0) { + dlarftb_wrap(t0 - g1, n - k2, nv, m, m, V, &Tau[k1], &F[g1+k2*m], W); + nv = 0; // clear queued-up Householder reflections + vzeros = 0; + } + } else { + // one more good pivot column found + Tau[k] = tau; + if (nv == 0) { + g1 = g; + k1 = k; + k2 = min(n, k + fchunk); + V = &F[g1+k1*m]; + + // check for switch to unblocked code + mleft = m - g1; // number of rows left + nleft = n - k1; // number of columns left + if (mleft * (nleft - (fchunk + 4)) < 5000 || mleft <= fchunk / 2 + || fchunk <= 1) + k2 = n; + } + nv++; // one more pending update; V is F (g1:t-1, k1:k1+nv-1) + + // apply the kth Householder reflection to the current panel + dlarf_wrap(t - g, k2 - k - 1, m, &F[g+k*m], tau, &F[g+(k+1)*m], W); + + g++; // one more pivot found + + if (k == k2 - 1 || g == m) { + dlarftb_wrap(t - g1, n - k2, nv, m, m, V, &Tau[k1], &F[g1+(k2*m)], W); + nv = 0; // clear queued-up Householder reflections + vzeros = 0; + } + } + + if (k == npiv - 1) rank = g; + } + + return rank; + } +} diff --git a/base/DenseQR.h b/base/DenseQR.h new file mode 100644 index 000000000..6294cd73b --- /dev/null +++ b/base/DenseQR.h @@ -0,0 +1,38 @@ +/* + * DenseQR.h + * + * Created on: Oct 19, 2010 + * Author: nikai + * Description: Dense QR, inspired by Tim Davis's dense solver + */ + +#pragma once + +namespace gtsam { + long DenseQR( + long m, // F is m-by-n with leading dimension m + long n, + long npiv, // number of pivot columns + double tol, // a column is flagged as dead if its norm is <= tol + long ntol, // apply tol only to first ntol pivot columns + long fchunk, // block size for compact WY Householder reflections, + // treated as 1 if fchunk <= 1 + + // input/output + double *F, // frontal matrix F of size m-by-n + long *Stair, // size n, entries F (Stair[k]:m-1, k) are all zero, + // and remain zero on output. + char *Rdead, // size npiv; all zero on input. If k is dead, + // Rdead [k] is set to 1 + + // output, not defined on input + double *Tau, // size n, Householder coefficients + + // workspace, undefined on input and output + double *W, // size b*(n+b), where b = min (fchunk,n,m) + + // input/output + double *wscale, + double *wssq + ); +} diff --git a/base/SPQRUtil.cpp b/base/DenseQRUtil.cpp similarity index 81% rename from base/SPQRUtil.cpp rename to base/DenseQRUtil.cpp index 430ba2823..7e35228c0 100644 --- a/base/SPQRUtil.cpp +++ b/base/DenseQRUtil.cpp @@ -10,15 +10,15 @@ * -------------------------------------------------------------------------- */ /* - * SPQRUtil.cpp + * DenseQRUtil.cpp * * Created on: Jul 1, 2010 * Author: nikai - * Description: the utility functions for SPQR + * Description: the utility functions for DenseQR */ #include -#include +#include #include #include #include @@ -99,9 +99,9 @@ namespace gtsam { } /* ************************************************************************* */ - void householder_spqr(Matrix &A, long* Stair) { + void householder_denseqr(Matrix &A, long* Stair) { - tic("householder_spqr"); + tic("householder_denseqr"); long m = A.size1(); long n = A.size2(); @@ -114,13 +114,13 @@ namespace gtsam { Stair[j] = m; } - tic("householder_spqr: row->col"); + tic("householder_denseqr: row->col"); // convert from row major to column major ublas::matrix Acolwise(A); double *a = Acolwise.data().begin(); - toc("householder_spqr: row->col"); + toc("householder_denseqr: row->col"); - tic("householder_spqr: spqr_front"); + tic("householder_denseqr: denseqr_front"); long npiv = min(m,n); double tol = -1; long ntol = -1; // no tolerance is used long fchunk = m < 32 ? m : 32; @@ -131,18 +131,18 @@ namespace gtsam { double wscale = 0; double wssq = 0; - cholmod_common cc; - cholmod_l_start(&cc); +// cholmod_common cc; +// cholmod_l_start(&cc); // todo: do something with the rank - long rank = spqr_front(m, n, npiv, tol, ntol, fchunk, - a, Stair, Rdead, Tau, W, &wscale, &wssq, &cc); - toc("householder_spqr: spqr_front"); + long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, + a, Stair, Rdead, Tau, W, &wscale, &wssq); + toc("householder_denseqr: denseqr_front"); //#ifndef NDEBUG for(long j=0; j > Acolsub( @@ -156,7 +156,7 @@ namespace gtsam { } //#endif - tic("householder_spqr: col->row"); + tic("householder_denseqr: col->row"); long k0 = 0; long j0; int k; @@ -173,24 +173,24 @@ namespace gtsam { // ublas::matrix_range Asub(ublas::project(A, ublas::range(0, min(m,n)), ublas::range(0,n))); // ublas::noalias(Asub) = ublas::triangular_adaptor(Acolsub); - toc("householder_spqr: col->row"); + toc("householder_denseqr: col->row"); - cholmod_l_finish(&cc); +// cholmod_l_finish(&cc); if(allocedStair) delete[] Stair; - toc("householder_spqr"); + toc("householder_denseqr"); } - void householder_spqr_colmajor(ublas::matrix& A, long *Stair) { - tic("householder_spqr"); + void householder_denseqr_colmajor(ublas::matrix& A, long *Stair) { + tic("householder_denseqr"); long m = A.size1(); long n = A.size2(); assert(Stair != NULL); - tic("householder_spqr: spqr_front"); + tic("householder_denseqr: denseqr_front"); long npiv = min(m,n); double tol = -1; long ntol = -1; // no tolerance is used long fchunk = m < 32 ? m : 32; @@ -201,18 +201,18 @@ namespace gtsam { double wscale = 0; double wssq = 0; - cholmod_common cc; - cholmod_l_start(&cc); +// cholmod_common cc; +// cholmod_l_start(&cc); // todo: do something with the rank - long rank = spqr_front(m, n, npiv, tol, ntol, fchunk, - A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq, &cc); - toc("householder_spqr: spqr_front"); + long rank = DenseQR(m, n, npiv, tol, ntol, fchunk, + A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq); + toc("householder_denseqr: denseqr_front"); //#ifndef NDEBUG for(long j=0; j > Acolsub( @@ -226,9 +226,9 @@ namespace gtsam { } //#endif - cholmod_l_finish(&cc); +// cholmod_l_finish(&cc); - toc("householder_spqr"); + toc("householder_denseqr"); } diff --git a/base/SPQRUtil.h b/base/DenseQRUtil.h similarity index 60% rename from base/SPQRUtil.h rename to base/DenseQRUtil.h index e7efc84ad..43ea60e02 100644 --- a/base/SPQRUtil.h +++ b/base/DenseQRUtil.h @@ -10,11 +10,11 @@ * -------------------------------------------------------------------------- */ /* - * SPQRUtil.h + * DenseQRUtil.h * * Created on: Jul 1, 2010 * Author: nikai - * Description: the utility functions for SPQR + * Description: the utility functions for DenseQR */ #pragma once @@ -22,16 +22,16 @@ #include #ifdef GT_USE_LAPACK -#include +#include namespace gtsam { - /** make stairs and speed up householder_spqr. Stair is defined as the row index of where zero entries start in each column */ + /** make stairs and speed up householder_denseqr. Stair is defined as the row index of where zero entries start in each column */ long* MakeStairs(Matrix &A); /** Householder tranformation, zeros below diagonal */ - void householder_spqr(Matrix &A, long* Stair = NULL); + void householder_denseqr(Matrix &A, long* Stair = NULL); - void householder_spqr_colmajor(boost::numeric::ublas::matrix& A, long *Stair); + void householder_denseqr_colmajor(boost::numeric::ublas::matrix& A, long *Stair); } #endif diff --git a/base/Makefile.am b/base/Makefile.am index 74fa59dfd..277f44488 100644 --- a/base/Makefile.am +++ b/base/Makefile.am @@ -18,8 +18,8 @@ sources += Vector.cpp svdcmp.cpp Matrix.cpp check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix if USE_LAPACK -sources += SPQRUtil.cpp -check_PROGRAMS += tests/testSPQRUtil +sources += DenseQR.cpp DenseQRUtil.cpp +check_PROGRAMS += tests/testDenseQRUtil endif # Testing @@ -50,7 +50,7 @@ base_HEADERS = $(headers) noinst_LTLIBRARIES = libbase.la libbase_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(DenseQRInc) -I$(top_srcdir)/.. +AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(top_srcdir)/.. AM_LDFLAGS = $(BOOST_LDFLAGS) if USE_BLAS @@ -76,7 +76,6 @@ endif if USE_LAPACK AM_CPPFLAGS += -DGT_USE_LAPACK -AM_LDFLAGS += -L$(DenseQRLib) -lDenseQR endif if USE_LAPACK_LINUX diff --git a/base/tests/testSPQRUtil.cpp b/base/tests/testDenseQRUtil.cpp similarity index 98% rename from base/tests/testSPQRUtil.cpp rename to base/tests/testDenseQRUtil.cpp index d26dbb4cf..a767aa498 100644 --- a/base/tests/testSPQRUtil.cpp +++ b/base/tests/testDenseQRUtil.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include using namespace std; using namespace gtsam; @@ -77,7 +77,7 @@ TEST(SPQRUtil, MakeStair2) } /* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_spqr) +TEST(SPQRUtil, houseHolder_denseqr) { double data[] = { -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, @@ -91,12 +91,12 @@ TEST(SPQRUtil, houseHolder_spqr) 0, 0, 0, 4.4721, 0, -4.4721, 0.894 }; Matrix expected1 = Matrix_(4, 7, data1); Matrix A1 = Matrix_(4, 7, data); - householder_spqr(A1); + householder_denseqr(A1); CHECK(assert_equal(expected1, A1, 1e-3)); } /* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_spqr2) +TEST(SPQRUtil, houseHolder_denseqr2) { double data[] = { -5, 0, 5, 0, 0, 0, -1, 00,-5, 0, 5, 0, 0, 1.5, @@ -111,13 +111,13 @@ TEST(SPQRUtil, houseHolder_spqr2) Matrix expected1 = Matrix_(4, 7, data1); Matrix A1 = Matrix_(4, 7, data); long* Stair = MakeStairs(A1); - householder_spqr(A1, Stair); + householder_denseqr(A1, Stair); delete[] Stair; CHECK(assert_equal(expected1, A1, 1e-3)); } /* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_spqr3) +TEST(SPQRUtil, houseHolder_denseqr3) { double data[] = { 1, 1, 9, 1, 0, 5}; @@ -127,11 +127,11 @@ TEST(SPQRUtil, houseHolder_spqr3) 0, -1/sqrt(2), -4/sqrt(2)}; Matrix expected1 = Matrix_(2, 3, data1); Matrix A1 = Matrix_(2, 3, data); - householder_spqr(A1); + householder_denseqr(A1); CHECK(assert_equal(expected1, A1, 1e-3)); } /* ************************************************************************* */ -TEST(SPQRUtil, houseHolder_spqr4) +TEST(SPQRUtil, houseHolder_denseqr4) { Matrix A = Matrix_(15, 34, -5.48351, 23.2337, -45.2073, 6.33455,-0.342553,-0.897005, 7.91126, 3.20237, -2.49219, -2.44189,-0.977376, -1.61127, -3.68421,-1.28059, -2.83303, 2.45949, 0.218835, -0.71239,-0.169314,-0.131355, 2.04233, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.0782689, @@ -185,13 +185,13 @@ TEST(SPQRUtil, houseHolder_spqr4) 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.86199, 8.53755, -19.6873, 20.8838, 6.09985, -12.3691, -28.4341, 7.05672, 3.02888, 0.594889,-0.214789); Matrix actualR = A; - householder_spqr(actualR); + householder_denseqr(actualR); long* Stair = MakeStairs(A); CHECK(assert_equal(expectedA, A)); Matrix actualRstair = A; - householder_spqr(actualRstair, Stair); + householder_denseqr(actualRstair, Stair); delete[] Stair; CHECK(assert_equal(expectedR, actualR, 1e-3)); diff --git a/configure.ac b/configure.ac index 402edbc53..82da5887c 100644 --- a/configure.ac +++ b/configure.ac @@ -169,21 +169,4 @@ AC_ARG_WITH([ccolamd-lib], [CCOLAMDLib=${HOME}/lib]) AC_SUBST([CCOLAMDLib]) -# ask for DenseQR library include directory -AC_ARG_WITH([denseqr-inc], - [AS_HELP_STRING([--with-denseqr-inc], - [specify the DenseQR library include directory (defaults to /HOME/include/DenseQR)])], - [DenseQRInc=$withval], - [DenseQRInc=${HOME}/include/DenseQR]) -AC_SUBST([DenseQRInc]) - -# ask for DenseQR library lib directory -AC_ARG_WITH([denseqr-lib], - [AS_HELP_STRING([--with-denseqr-lib], - [specify the DenseQR library lib directory (defaults to /HOME/lib)])], - [DenseQRLib=$withval], - [DenseQRLib=${HOME}/lib]) -AC_SUBST([DenseQRLib]) - - AC_OUTPUT diff --git a/linear/Makefile.am b/linear/Makefile.am index 59030014b..da26fd224 100644 --- a/linear/Makefile.am +++ b/linear/Makefile.am @@ -51,7 +51,7 @@ lineardir = $(pkgincludedir)/linear linear_HEADERS = $(headers) noinst_LTLIBRARIES = liblinear.la liblinear_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(DenseQRInc) -I$(top_srcdir)/.. +AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(top_srcdir)/.. AM_LDFLAGS = $(BOOST_LDFLAGS) AM_CXXFLAGS = @@ -78,6 +78,5 @@ endif if USE_LAPACK AM_CXXFLAGS += -DGT_USE_LAPACK -AM_LDFLAGS += -L$(DenseQRLib) -lDenseQR endif diff --git a/linear/NoiseModel.cpp b/linear/NoiseModel.cpp index b480dc39b..a72819c76 100644 --- a/linear/NoiseModel.cpp +++ b/linear/NoiseModel.cpp @@ -31,7 +31,7 @@ #include #include -#include +#include namespace ublas = boost::numeric::ublas; typedef ublas::matrix_column column; @@ -133,9 +133,9 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firstZero // Perform in-place Householder #ifdef GT_USE_LAPACK if(firstZeroRows) - householder_spqr(Ab, &(*firstZeroRows)[0]); + householder_denseqr(Ab, &(*firstZeroRows)[0]); else - householder_spqr(Ab); + householder_denseqr(Ab); #else householder(Ab, maxRank); #endif @@ -156,7 +156,7 @@ SharedDiagonal Gaussian::QRColumnWise(ublas::matrix // Perform in-place Householder #ifdef GT_USE_LAPACK - householder_spqr_colmajor(Ab, &firstZeroRows[0]); + householder_denseqr_colmajor(Ab, &firstZeroRows[0]); #else householder(Ab, maxRank); #endif