143 lines
3.1 KiB
C++
143 lines
3.1 KiB
C++
/*
|
|
* SPQRUtil.cpp
|
|
*
|
|
* Created on: Jul 1, 2010
|
|
* Author: nikai
|
|
* Description: the utility functions for SPQR
|
|
*/
|
|
|
|
#include <map>
|
|
#include "SPQRUtil.h"
|
|
|
|
using namespace std;
|
|
|
|
#ifdef GT_USE_LAPACK
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
long* MakeStairs(Matrix& A) {
|
|
|
|
const long m = A.size1();
|
|
const long n = A.size2();
|
|
|
|
// record the starting pointer of each row
|
|
double* a[m];
|
|
a[0] = A.data().begin();
|
|
for(int i=1; i<m; i++)
|
|
a[i] = a[i-1] + n;
|
|
|
|
// go through each column from left to right
|
|
int j;
|
|
int i0, i1, i2, tmpi;
|
|
long* Stair = new long[n];
|
|
int sizeOfRow;
|
|
double tmp[n], *row1, *row2;
|
|
double tmpd;
|
|
for(j = 0; j < min(m,n); ) {
|
|
|
|
i0 = (j==0) ? 0 : Stair[j-1];
|
|
|
|
// find all the rows with leading zeros in the current column
|
|
vector<int> i_zeros, i_nonzeros, i_all;
|
|
map<int, int> i2vi;
|
|
for(int i = i0; i < m; i++) {
|
|
i_all.push_back(i);
|
|
i2vi.insert(make_pair(i, i-i0));
|
|
if (*(a[i]) == 0)
|
|
i_zeros.push_back(i);
|
|
else
|
|
i_nonzeros.push_back(i);
|
|
}
|
|
|
|
// resort the rows from i_all to i_target
|
|
vector<int>& i_target = i_nonzeros;
|
|
i_target.insert(i_nonzeros.end(), i_zeros.begin(), i_zeros.end());
|
|
sizeOfRow = (n - j) * sizeof(double);
|
|
for (int vi=0; vi<m-i0; vi++) {
|
|
i1 = i_all[vi];
|
|
i2 = i_target[vi];
|
|
if (i1 != i2) {
|
|
row1 = a[vi + i0];
|
|
tmpi = i2vi[i2];
|
|
row2 = a[i2vi[i2] + i0];
|
|
memcpy(tmp, row1, sizeOfRow);
|
|
memcpy(row1, row2, sizeOfRow);
|
|
memcpy(row2, tmp, sizeOfRow);
|
|
std::swap(i_all[vi], i_all[tmpi]);
|
|
std::swap(i2vi[i1], i2vi[i2]);
|
|
}
|
|
}
|
|
|
|
// record the stair number
|
|
Stair[j++] = m - i_zeros.size();
|
|
if (i_zeros.empty()) break;
|
|
|
|
// right shift the pointers
|
|
for(int i=0; i<m; i++) a[i]++;
|
|
}
|
|
|
|
// all the remained columns have maximum stair
|
|
for (; j<n; j++)
|
|
Stair[j] = m;
|
|
|
|
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_spqr(Matrix &A, long* Stair) {
|
|
long m = A.size1();
|
|
long n = A.size2();
|
|
|
|
if (Stair == NULL) {
|
|
Stair = new long[n];
|
|
for(int j=0; j<n; j++)
|
|
Stair[j] = m;
|
|
}
|
|
|
|
// convert from row major to column major
|
|
double a[m*n]; int k = 0;
|
|
for(int j=0; j<n; j++)
|
|
for(int i=0; i<m; i++, k++)
|
|
a[k] = A(i,j);
|
|
|
|
long npiv = min(m,n);
|
|
double tol = -1; long ntol = -1; // no tolerance is used
|
|
long fchunk = m < 32 ? m : 32;
|
|
char Rdead[npiv];
|
|
double Tau[n];
|
|
long b = min(fchunk, min(n, m));
|
|
double W[b*(n+b)];
|
|
double wscale = 0;
|
|
double wssq = 0;
|
|
|
|
cholmod_common cc;
|
|
cholmod_l_start(&cc);
|
|
|
|
long rank = spqr_front<double>(m, n, npiv, tol, ntol, fchunk,
|
|
a, Stair, Rdead, Tau, W, &wscale, &wssq, &cc);
|
|
|
|
long k0 = 0;
|
|
long j0;
|
|
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];
|
|
}
|
|
|
|
delete []Stair;
|
|
cholmod_l_finish(&cc);
|
|
}
|
|
|
|
} // namespace gtsam
|
|
#endif
|