12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

DenseQR relaunched in gtsam now.

release/4.3a0
Kai Ni 2010-10-20 04:21:58 +00:00
parent 7d4f1ad268
commit e83950373e
10 changed files with 288 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Makefile.am
View File

@ -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

197
base/DenseQR.cpp Normal file
View File

@ -0,0 +1,197 @@
/*
* DenseQR.cpp
*
* Created on: Oct 19, 2010
* Author: nikai
* Description: Dense QR, inspired by Tim Davis's dense solver
*/
#include <cassert>
#include <math.h>
#include <algorithm>
#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;
}
}

38
base/DenseQR.h Normal file
View File

@ -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
);
}

58
base/SPQRUtil.cpp → base/DenseQRUtil.cpp
View File

@ -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 <gtsam/base/timing.h>
#include <gtsam/base/SPQRUtil.h>
#include <gtsam/base/DenseQRUtil.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
@ -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<double, ublas::column_major> 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<double>(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<npiv; ++j)
if(Rdead[j]) {
cout << "In householder_spqr, aborting because some columns were found to be\n"
cout << "In householder_denseqr, aborting because some columns were found to be\n"
"numerically linearly-dependent and we cannot handle this case yet." << endl;
print(A, "The matrix being factored was\n");
ublas::matrix_range<ublas::matrix<double,ublas::column_major> > 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<Matrix> Asub(ublas::project(A, ublas::range(0, min(m,n)), ublas::range(0,n)));
// ublas::noalias(Asub) = ublas::triangular_adaptor<typeof(Acolsub), ublas::upper>(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<double, ublas::column_major>& A, long *Stair) {
tic("householder_spqr");
void householder_denseqr_colmajor(ublas::matrix<double, ublas::column_major>& 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<double>(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<npiv; ++j)
if(Rdead[j]) {
cout << "In householder_spqr, aborting because some columns were found to be\n"
cout << "In householder_denseqr, aborting because some columns were found to be\n"
"numerically linearly-dependent and we cannot handle this case yet." << endl;
print(A, "The matrix being factored was\n");
ublas::matrix_range<ublas::matrix<double,ublas::column_major> > Acolsub(
@ -226,9 +226,9 @@ namespace gtsam {
}
//#endif
cholmod_l_finish(&cc);
// cholmod_l_finish(&cc);
toc("householder_spqr");
toc("householder_denseqr");
}

12
base/SPQRUtil.h → base/DenseQRUtil.h
View File

@ -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 <gtsam/base/Matrix.h>
#ifdef GT_USE_LAPACK
#include <spqr_subset.hpp>
#include <gtsam/base/DenseQR.h>
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<double, boost::numeric::ublas::column_major>& A, long *Stair);
void householder_denseqr_colmajor(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& A, long *Stair);
}
#endif

7
base/Makefile.am
View File

@ -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

20
base/tests/testSPQRUtil.cpp → base/tests/testDenseQRUtil.cpp
View File

@ -17,7 +17,7 @@
#include <iostream>
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/SPQRUtil.h>
#include <gtsam/base/DenseQRUtil.h>
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));

17
configure.ac
View File

@ -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

3
linear/Makefile.am
View File

@ -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

8
linear/NoiseModel.cpp
View File

@ -31,7 +31,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/base/SPQRUtil.h>
#include <gtsam/base/DenseQRUtil.h>
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix_column<Matrix> column;
@ -133,9 +133,9 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> 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<double, ublas::column_major>
// 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