delete
Chris Beall
parent
21e2be0ad6
commit
a46187ee67
197
base/DenseQR.cpp
197
base/DenseQR.cpp
|
|
@ -1,197 +0,0 @@
|
|||
/*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
/*
|
||||
* 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
|
||||
);
|
||||
}
|
||||
|
|
@ -1,227 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* DenseQRUtil.cpp
|
||||
*
|
||||
* Created on: Jul 1, 2010
|
||||
* Author: nikai
|
||||
* Description: the utility functions for DenseQR
|
||||
*/
|
||||
|
||||
#include <cstring>
|
||||
|
||||
#include <gtsam/base/timing.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>
|
||||
|
||||
using namespace std;
|
||||
namespace ublas = boost::numeric::ublas;
|
||||
|
||||
#ifdef GT_USE_LAPACK
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
long* MakeStairs(Matrix& A) {
|
||||
|
||||
const long m = A.size1();
|
||||
const long n = A.size2();
|
||||
long* Stair = new long[n];
|
||||
|
||||
// record the starting pointer of each row
|
||||
double* a[m];
|
||||
list<int> remained;
|
||||
a[0] = A.data().begin();
|
||||
for(int i=0; i<m-1; i++) {
|
||||
a[i+1] = a[i] + n;
|
||||
remained.push_back(i);
|
||||
}
|
||||
remained.push_back(m-1);
|
||||
|
||||
// reorder the rows
|
||||
int j;
|
||||
vector<int> sorted;
|
||||
list<int>::iterator itRemained;
|
||||
for(j = 0; j < n; ) {
|
||||
// remove the non-zero rows in the current column
|
||||
for(itRemained = remained.begin(); itRemained!=remained.end(); ) {
|
||||
if (*(a[*itRemained]) != 0) {
|
||||
sorted.push_back(*itRemained);
|
||||
itRemained = remained.erase(itRemained);
|
||||
} else {
|
||||
a[*itRemained]++;
|
||||
itRemained ++;
|
||||
}
|
||||
}
|
||||
|
||||
// record the stair number
|
||||
Stair[j++] = m - remained.size();
|
||||
|
||||
if(remained.empty()) break;
|
||||
}
|
||||
|
||||
// all the remained columns have maximum stair
|
||||
for (; j<n; j++)
|
||||
Stair[j] = m;
|
||||
|
||||
// copy the new row
|
||||
Matrix A_new = zeros(m,n);
|
||||
int offset[m];
|
||||
offset[0] = 0;
|
||||
for(int i=1; i<m; i++)
|
||||
offset[i] = offset[i-1] +n;
|
||||
vector<int>::const_iterator itSorted;
|
||||
double* ptr1 = A.data().begin();
|
||||
double* ptr2 = A_new.data().begin();
|
||||
int row = 0, sizeOfRow = n * sizeof(double);
|
||||
for(itSorted=sorted.begin(); itSorted!=sorted.end(); itSorted++, row++)
|
||||
memcpy(ptr2+offset[row], ptr1+offset[*itSorted], sizeOfRow);
|
||||
|
||||
A = A_new;
|
||||
|
||||
return Stair;
|
||||
}
|
||||
|
||||
void printColumnMajor(const double* a, const long m, const long n) {
|
||||
for(int i=0; i<m; i++) {
|
||||
for(int j=0; j<n; j++)
|
||||
cout << a[j*m+i] << "\t";
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void householder_denseqr(Matrix &A, long* Stair) {
|
||||
|
||||
tic("householder_denseqr");
|
||||
|
||||
long m = A.size1();
|
||||
long n = A.size2();
|
||||
|
||||
bool allocedStair = false;
|
||||
if (Stair == NULL) {
|
||||
allocedStair = true;
|
||||
Stair = new long[n];
|
||||
for(int j=0; j<n; j++)
|
||||
Stair[j] = m;
|
||||
}
|
||||
|
||||
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_denseqr: row->col");
|
||||
|
||||
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;
|
||||
char Rdead[npiv]; memset(Rdead, 0, sizeof(char)*npiv);
|
||||
double Tau[n];
|
||||
long b = min(fchunk, min(n, m));
|
||||
double W[b*(n+b)];
|
||||
double wscale = 0;
|
||||
double wssq = 0;
|
||||
|
||||
|
||||
// todo: do something with the rank
|
||||
long rank = DenseQR(m, n, npiv, tol, ntol, fchunk,
|
||||
a, Stair, Rdead, Tau, W, &wscale, &wssq);
|
||||
toc("householder_denseqr: denseqr_front");
|
||||
|
||||
|
||||
for(long j=0; j<npiv; ++j)
|
||||
if(Rdead[j]) {
|
||||
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(
|
||||
ublas::project(Acolwise, ublas::range(0, min(m,n)), ublas::range(0,n)));
|
||||
print(Matrix(ublas::triangular_adaptor<typeof(Acolsub), ublas::upper>(Acolsub)), "and the result was\n");
|
||||
cout << "The following columns are \"dead\":";
|
||||
for(long k=0; k<npiv; ++k)
|
||||
if(Rdead[k]) cout << " " << k;
|
||||
cout << endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
tic("householder_denseqr: col->row");
|
||||
long k0 = 0;
|
||||
long j0;
|
||||
int k;
|
||||
memset(A.data().begin(), 0, m*n*sizeof(double));
|
||||
for(long j=0; j<n; j++, k0+=m) {
|
||||
k = k0;
|
||||
j0 = min(j+1,m);
|
||||
for(int i=0; i<j0; i++, k++)
|
||||
A(i,j) = a[k];
|
||||
}
|
||||
|
||||
|
||||
|
||||
toc("householder_denseqr: col->row");
|
||||
|
||||
|
||||
|
||||
if(allocedStair) delete[] Stair;
|
||||
|
||||
toc("householder_denseqr");
|
||||
}
|
||||
|
||||
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_denseqr: denseqr_front");
|
||||
long npiv = min(m,n);
|
||||
double tol = -1; long ntol = -1; // no tolerance is used
|
||||
long fchunk = m < 32 ? m : 32;
|
||||
char Rdead[npiv]; memset(Rdead, 0, sizeof(char)*npiv);
|
||||
double Tau[n];
|
||||
long b = min(fchunk, min(n, m));
|
||||
double W[b*(n+b)];
|
||||
double wscale = 0;
|
||||
double wssq = 0;
|
||||
|
||||
|
||||
// todo: do something with the rank
|
||||
long rank = DenseQR(m, n, npiv, tol, ntol, fchunk,
|
||||
A.data().begin(), Stair, Rdead, Tau, W, &wscale, &wssq);
|
||||
toc("householder_denseqr: denseqr_front");
|
||||
|
||||
for(long j=0; j<npiv; ++j)
|
||||
if(Rdead[j]) {
|
||||
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(
|
||||
ublas::project(A, ublas::range(0, min(m,n)), ublas::range(0,n)));
|
||||
print(Matrix(ublas::triangular_adaptor<typeof(Acolsub), ublas::upper>(Acolsub)), "and the result was\n");
|
||||
cout << "The following columns are \"dead\":";
|
||||
for(long k=0; k<npiv; ++k)
|
||||
if(Rdead[k]) cout << " " << k;
|
||||
cout << endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
toc("householder_denseqr");
|
||||
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
#endif
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* DenseQRUtil.h
|
||||
*
|
||||
* Created on: Jul 1, 2010
|
||||
* Author: nikai
|
||||
* Description: the utility functions for DenseQR
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
#ifdef GT_USE_LAPACK
|
||||
#include <gtsam/base/DenseQR.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** 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_denseqr(Matrix &A, long* Stair = NULL);
|
||||
|
||||
/** Householder tranformation in column mafor form */
|
||||
void householder_denseqr_colmajor(boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major>& A, long *Stair);
|
||||
}
|
||||
#endif
|
||||
Loading…
Reference in New Issue