GURLS_EXPORT float* pinv(const float* A, int rows, int cols, int& res_rows, int& res_cols, float* RCOND)
{
int M = rows;
int N = cols;
float* a = new float[rows*cols];
copy<float>(a, A, rows*cols);
int LDA = M;
int LDB = std::max(M, N);
int NRHS = LDB;
// float* b = eye(LDB).getData()
const int b_size = LDB*NRHS;
float *b = new float[LDB*NRHS];
set<float>(b, 0.f, b_size);
set<float>(b, 1.f, std::min(LDB, NRHS), NRHS+1);
float* S = new float[std::min(M,N)];
// float condnum = 0.f; // The condition number of A in the 2-norm = S(1)/S(min(m,n)).
float rcond = (RCOND == NULL)? (std::max(rows, cols)*FLT_EPSILON): *RCOND;
// if (RCOND < 0)
// RCOND = 0.f;
int RANK = -1; // std::min(M,N);
int LWORK = -1; //2 * (3*LDB + std::max( 2*std::min(M,N), LDB));
float* WORK = new float[1];
/*
subroutine SGELSS ( INTEGER M,
INTEGER N,
INTEGER NRHS,
REAL,dimension( lda, * ) A,
INTEGER LDA,
REAL,dimension( ldb, * ) B,
INTEGER LDB,
REAL,dimension( * ) S,
REAL RCOND,
INTEGER RANK,
REAL,dimension( * ) WORK,
INTEGER LWORK,
INTEGER INFO
)
*/
/*
INFO:
= 0: successful exit
< 0: if INFO = -i, the i-th argument had an illegal value.
> 0: the algorithm for computing the SVD failed to converge;
if INFO = i, i off-diagonal elements of an intermediate
bidiagonal form did not converge to zero.
*/
int INFO;
/* Query and allocate the optimal workspace */
int res = sgelss_( &M, &N, &NRHS, a, &LDA, b, &LDB, S, &rcond, &RANK, WORK, &LWORK, &INFO);
LWORK = static_cast<int>(WORK[0]);
delete [] WORK;
WORK = new float[LWORK];
res = sgelss_( &M, &N, &NRHS, a, &LDA, b, &LDB, S, &rcond, &RANK, WORK, &LWORK, &INFO);
// TODO: check INFO on exit
//condnum = S[0]/(S[std::min(M, N)]-1);
if(INFO != 0)
{
std::stringstream str;
str << "Pinv failed, error code " << INFO << ";" << std::endl;
throw gException(str.str());
}
delete [] S;
delete [] WORK;
delete [] a;
res_rows = LDB;
res_cols = NRHS;
return b;
}
GURLS_EXPORT int gelss( int *m, int *n, int* nrhs, float *a, int *lda, float* b, int *ldb, float *s, float *rcond, int *rank, float *work, int *lwork, int *info)
{
return sgelss_( m, n, nrhs, a, lda, b, ldb, s, rcond, rank, work, lwork, info);
}
GURLS_EXPORT void pinv(const gMat2D<float>& A, gMat2D<float>& Ainv, float RCOND){
/*
subroutine SGELSS ( INTEGER M,
INTEGER N,
INTEGER NRHS,
REAL,dimension( lda, * ) A,
INTEGER LDA,
REAL,dimension( ldb, * ) B,
INTEGER LDB,
REAL,dimension( * ) S,
REAL RCOND,
INTEGER RANK,
REAL,dimension( * ) WORK,
INTEGER LWORK,
INTEGER INFO
)
*/
int M = A.rows();
int N = A.cols();
// The following step is required because we are currently storing
// the matrices using a column-major order while LAPACK's
// routines require row-major ordering
float* a = new float[M*N];
const float* ptr_A = A.getData();
float* ptr_a = a;
for (int j = 0; j < N ; j++){
for (int i = 0; i < M ; i++){
*ptr_a++ = *(ptr_A+i*N+j);
}
}
int LDA = M;
int LDB = std::max(M, N);
int NRHS = LDB;
float *b = new float[LDB*NRHS], *b_ptr = b;
for (int i = 0; i < LDB*NRHS; i++){
*b_ptr++=0.f;
}
b_ptr = b;
for (int i = 0; i < std::min(LDB, NRHS); i++, b_ptr+=(NRHS+1)){
*b_ptr = 1.f;
}
float* S = new float[std::min(M,N)];
float condnum = 0.f; // The condition number of A in the 2-norm = S(1)/S(min(m,n)).
if (RCOND < 0){
RCOND = 0.f;
}
int RANK = -1; // std::min(M,N);
int LWORK = -1; //2 * (3*LDB + std::max( 2*std::min(M,N), LDB));
float* WORK = new float[1];
/*
INFO:
= 0: successful exit
< 0: if INFO = -i, the i-th argument had an illegal value.
> 0: the algorithm for computing the SVD failed to converge;
if INFO = i, i off-diagonal elements of an intermediate
bidiagonal form did not converge to zero.
*/
int INFO;
/* Query and allocate the optimal workspace */
/*int res = */sgelss_( &M, &N, &NRHS, a, &LDA, b, &LDB, S, &RCOND, &RANK, WORK, &LWORK, &INFO);
LWORK = static_cast<int>(WORK[0]);
delete [] WORK;
WORK = new float[LWORK];
/*res = */sgelss_( &M, &N, &NRHS, a, &LDA, b, &LDB, S, &RCOND, &RANK, WORK, &LWORK, &INFO);
// TODO: check INFO on exit
condnum = S[0]/(S[std::min(M, N)]-1);
// gMat2D<float> *tmp = new gMat2D<float>(b, LDB, LDB, false);
float *ainv = new float[N*M];
float* ptr_b = ainv;
float* ptr_B = b;
for (int i = 0; i < N ; i++){
for (int j = 0; j < M ; j++){
*(ptr_b+i*M+j) = *(ptr_B+j*NRHS+i);
}
}
Ainv = * new gMat2D<float>(ainv, N, M, true);
// gMat2D<float> *tmp = new gMat2D<float>(b, LDB, NRHS, false);
// gMat2D<float> *tmp1 = new gMat2D<float>(NRHS, LDB);
// tmp->transpose(*tmp1);
// Ainv = * new gMat2D<float>(tmp1->getData(), N, M, true);
// std::cout << "A = " << std::endl << A << std::endl;
// std::cout << "pinv(A) = " << std::endl << Ainv << std::endl;
delete [] S;
delete [] WORK;
delete [] a;
// delete tmp, tmp1;
delete [] b;
}
